maybe the direction of loop constraint is reserve

[fls1995]

FAST_LIO_SLAM/SC-PGO/src/laserPosegraphOptimization.cpp

Lines 472 to 477 in 10eab53

	correctionLidarFrame = icp.getFinalTransformation();
	pcl::getTranslationAndEulerAngles (correctionLidarFrame, x, y, z, roll, pitch, yaw);
	gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z));
	gtsam::Pose3 poseTo = Pose3(Rot3::RzRyRx(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
	return poseFrom.between(poseTo);

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?

FAST_LIO_SLAM/SC-PGO/src/laserPosegraphOptimization.cpp

Line 689 in 10eab53

gtSAMgraph.add(gtsam::BetweenFactor<gtsam::Pose3>(prev_node_idx, curr_node_idx, relative_pose, robustLoopNoise));

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