maybe the direction of loop constraint is reserve
Opened this issue · 0 comments
fls1995 commented
FAST_LIO_SLAM/SC-PGO/src/laserPosegraphOptimization.cpp
Lines 472 to 477 in 10eab53
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?
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