Open fls1995 opened 2 years ago
https://github.com/gisbi-kim/FAST_LIO_SLAM/blob/10eab538db70678f41aaa24247d16831ca921374/SC-PGO/src/laserPosegraphOptimization.cpp#L472-L477 In _loop_kf_idx local coordinate system, PoseFrom is the origin of _curr_kf_idx, and PoseTo is the origin of _loop_kf_idx. So the return should be _curr_kf_idx.between(_loop_kf_idx). But in line 689, the edge is prev_node_idx.between(curr_node_idx). Maybe the direction of loop constraint is reserve? https://github.com/gisbi-kim/FAST_LIO_SLAM/blob/10eab538db70678f41aaa24247d16831ca921374/SC-PGO/src/laserPosegraphOptimization.cpp#L689 In another project, I find similar code as expect. https://github.com/irapkaist/SC-LeGO-LOAM/blob/705367eceaef797d9b4bda1c5bb91c9a0ec771f7/SC-LeGO-LOAM/LeGO-LOAM/src/mapOptmization.cpp#L1081-L1088
Another probelm, I argee in calculating _curr_kf_idx cloud using _loop_kf_idx as root_idx. But in calculating _loop_kf_idx nearby cloud, we shound use current index as root_idx just like the code below. https://github.com/irapkaist/SC-LeGO-LOAM/blob/705367eceaef797d9b4bda1c5bb91c9a0ec771f7/SC-LeGO-LOAM/LeGO-LOAM/src/mapOptmization.cpp#L942-L947
https://github.com/gisbi-kim/FAST_LIO_SLAM/blob/10eab538db70678f41aaa24247d16831ca921374/SC-PGO/src/laserPosegraphOptimization.cpp#L472-L477 In _loop_kf_idx local coordinate system, PoseFrom is the origin of _curr_kf_idx, and PoseTo is the origin of _loop_kf_idx. So the return should be _curr_kf_idx.between(_loop_kf_idx). But in line 689, the edge is prev_node_idx.between(curr_node_idx). Maybe the direction of loop constraint is reserve? https://github.com/gisbi-kim/FAST_LIO_SLAM/blob/10eab538db70678f41aaa24247d16831ca921374/SC-PGO/src/laserPosegraphOptimization.cpp#L689 In another project, I find similar code as expect. https://github.com/irapkaist/SC-LeGO-LOAM/blob/705367eceaef797d9b4bda1c5bb91c9a0ec771f7/SC-LeGO-LOAM/LeGO-LOAM/src/mapOptmization.cpp#L1081-L1088
Another probelm, I argee in calculating _curr_kf_idx cloud using _loop_kf_idx as root_idx. But in calculating _loop_kf_idx nearby cloud, we shound use current index as root_idx just like the code below. https://github.com/irapkaist/SC-LeGO-LOAM/blob/705367eceaef797d9b4bda1c5bb91c9a0ec771f7/SC-LeGO-LOAM/LeGO-LOAM/src/mapOptmization.cpp#L942-L947