Unable to align GT and GPS Data

[skwelker22]

I wanted to try and align the ground truth data (provided by KITTI in the camera frame) to the gps data from the oxts (provided in the IMU frame).

For whatever reason, when I rotate the gps data into the camera frame (see code and plot below), there exists a rotational misalignment which I can't figure out the root cause of. Below is a minimalist example of me doing the conversions.

NOTE: due to whatever error Ive made, I use the x/y cordinates of the gps data (unexpected) and x/z cordinates of the camera data (expected) to plot the translation.

CODE:

import pykitti
import math
import numpy as np
import matplotlib.pyplot as plt

###################
##Set Data Paths
#odometry path
base_path = '.\pykitti\data\odom_gray'
sequence = '00' #sequence 00 is common
#raw data path
raw_base = '.\pykitti\data\2011_10_03_drive_0027_sync'
raw_date = '2011_10_03'
raw_drive = '0027'
####################

##############
##Load Data
#Load gray + color + velodyne + poses
data = pykitti.odometry(base_path, sequence)
rawData = pykitti.raw(raw_base, raw_date, raw_drive)
################

##############
##Extract imu->camera pose matrix
lenRaw = len(rawData)
T_cam0_imu = rawData.calib.T_cam0_imu

#Access ground truth poses
poses_gt = np.array(data.poses) # list of 4x4 matrices

#Convert OXTS data from IMU/GPS frame to camera frame (camera->world , should be equivalent to the gt data)
#plot all poses (top down)
t_gps_in_cam0 = np.zeros((lenRaw,3))
for i in range(lenRaw):
T_w_imu = rawData.oxts[i].T_w_imu
T_w_cam0 = T_w_imu @ np.linalg.inv(T_cam0_imu)
t_gps_in_cam0[i,:] = T_w_cam0[:3,3]

#Plot results w gps + ground truth
plt.plot(poses_gt[:,0,3], poses_gt[:,2,3], label="GT Full") #x vs z
plt.plot(t_gps_in_cam0[:,0], t_gps_in_cam0[:,1], 'm', marker='o', label="GPS")
plt.xlabel("X [m]")
plt.ylabel("Z [m]")
plt.title("KITTI Camera Trajectory (top-down view)")
plt.axis("equal")
plt.legend()
plt.show()