How do I transfer the pose data in the kitti raw data packet to the pose data in the kitti odometry data packet?

[sunmk2006]

How do I transfer the pose data in the kitti raw data packet to the pose data in the kitti odometry data packet?if the conversion is correct, the output should be consistent with the ground truth in the kitti odometry data set. Formula is as follows： T_cam0_to_world = kitti.oxts.T_w_imu.dot(inv(kitti.calib.T_cam0_imu)) But the result is not ideal. Can you point out my mistakes and correct them， or can you provide a tool or some code to convert the original pose data into the form of ground truth in the kitti odometry data set ?

[leeclemnet]

The world frame is defined differently in the raw vs. odometry ground truth poses. In the odometry poses, the world frame is defined such that it coincides with the pose of the camera in the first image of the sequence. This is why the first ground truth pose in the odometry sequences is always the identity transformation. In the raw sequences, the ground truth poses are the poses of the OXTS sensor, not the camera, and pykitti retains the East-North-Up coordinate system for the orientation.

So to get from raw ground truth poses to odometry ground truth poses, you need to first convert the IMU poses to camera poses (which you already did), but then also transform all the camera poses so that they are relative to the first camera pose.

Here's some pseudocode that may be helpful. I tacked on _odo or _raw to indicate where each pose came from, and [0] refers to the first image/pose.

# Convert raw IMU poses to raw cam0 poses
T_w_cam0_raw = T_w_imu_raw * inv(T_cam0_imu) 

# Convert raw cam0 poses to odometry (relative) cam0 poses
T_w_cam0_odo =  inv(T_w_cam0_raw[0]) * T_w_cam0_raw

I tried this and was able to recover odometry poses from raw poses, give or take some rounding error.