Why Does IMU integeration Degrade Pose Accuracy?

[Amin Abouee]

Hello everyone,

I’m currently working with GTSAM to perform loosely coupled fusion of IMU and camera data. To test the algorithm, I started with the EuRoC dataset, specifically the v1_01_easy dataset, as it provides both extrinsic and intrinsic parameters. The VO result was estimated using a direct method and saved into a text file in TUM format. So we just load the VO and raw IMU data to fuse them using both Levenberg-Marquardt (LM) and iSAM2 for optimization.

However, I’m encountering an issue: after fusion, the optimized poses perform worse than the initial visual odometry estimates. The following plots illustrate the results with and without fusion.

Could you please help me understand why the results diverge when fusing the IMU? Is there an additional parameter or step needed to ensure the results are in metric scale?

The code is available here: https://github.dev/amin-abouee/test_gtsam/main.cpp 

Thank you for your insights!

[Terry]

This composition looks off. It's presumably `imu_t_cam * world_t_cam * cam_t_imu`

// Transform VO measurements to the IMU frame.

gtsam::Pose3 current_vo_in_imu = T_imu_cam_ * iter_odom->pose * T_imu_cam_.inverse();

Should it not be:

gtsam::Pose3 current_vo_in_imu = iter_odom->pose * T_imu_cam_.inverse();