Why does odometry_estimation_cpu.cpp add both a BetweenFactor and PriorFactor to the graph?

[VRichardJP]

In odometry_estimation_cpu.cpp:

glim/src/glim/odometry/odometry_estimation_cpu.cpp

Lines 165 to 167 in 42bf677

	// TODO: Extract a relative pose covariance from a frame-to-model matching result?
	factors.emplace_shared<gtsam::BetweenFactor<gtsam::Pose3>>(X(last), X(current), gtsam::Pose3(T_last_current.matrix()), gtsam::noiseModel::Isotropic::Precision(6, 1e3));
	factors.emplace_shared<gtsam::PriorFactor<gtsam::Pose3>>(X(current), gtsam::Pose3(T_target_imu.matrix()), gtsam::noiseModel::Isotropic::Precision(6, 1e3));

There are 2 factors added to the graph:

• a BetweenFactor factor, which constraints the motion between between 2 scans
• a PriorFactor factor, which constraints the absolute position of the last scan in the current target point cloud

Since both factor are tightly coupled, I have the feeling there are redundant. Are they?

[koide3]

Yeah, the current form is a bit redundant. I first tried to use only BetweenFactor to smoothly fuse LiDAR ego-motion estimation and IMU prediction, but it resulted in an unstable strange behavior due to inconsistent local point cloud map and estimated pose history. Then, I added PriorFactor to force the estimate to be consistent with the local map, and it worked somehow. Maybe it's fine to remove BetweenFactor in the current implementation.

BTW, I don't really like this kind of scan-to-model matching because it has problems in IMU fusion like this, and thus I take a different approach based on keyframe matching in the GPU-based odometry estimation.