What is the graph optimization for in `SubMapping::insert_frame()`'s `enable_imu` branch?

[VRichardJP]

I fail to understand what the graph optimization is doing in this branch:

https://github.com/koide3/glim/blob/42bf6770ed3924bd33bb010279303978cd4f2b8f/src/glim/mapping/sub_mapping.cpp#L117-L161

graph and values are both local variables, and optimization output (values) does not seem to be used before the if branch is left. In other words, the lines 127 to 153 are self-contained and seem to have no effect.

Do I miss something?

[VRichardJP]

Basically, it feels like these lines should use the smoothened values values->at<gtsam::Pose3>(X(i)) instead of the preintegrated IMU poses imu_poses[i]:

https://github.com/koide3/glim/blob/42bf6770ed3924bd33bb010279303978cd4f2b8f/src/glim/mapping/sub_mapping.cpp#L156-L160

[koide3]

Ah, you are correct. It intended to smooth the trajectory with the estimation results of odometry estimation (i.e., fusing IMU forward prediction and point cloud backward correction in a loose way), but it wrongly used imu_poses instead of values as you pointed out... I'll fix it soon.