Poses from interpolate_ins_poses using RTK has broken sections.

[maximechoulika]

Hi,
I have found broken sections using rtk.csv file with interpolate_ins_poses function.

I used this code snippet to generate the plots:

ins_path = os.path.join(dataset_dir, "images", dataset, "gps/rtk.csv")
timestamp_path = os.path.join(dataset_dir, "images", dataset, f"mono_{view}.timestamps")
    
pose_timestamps = np.loadtxt(timestamp_path)[:, 0].tolist()

origin_timestamp = pose_timestamps[0]
    
print('Interpolating poses...')
insext = np.loadtxt(os.path.join('robotcar_helper/extrinsics', f'mono_{view}.txt')).tolist()
mtx = transform.build_se3_transform(insext)

T_I_Cx = pose.fromMatrix(np.asarray(mtx)).inverse()
T_Cx_C = pose.yRotationDeg(90) * pose.zRotationDeg(90)
T_I_C = T_I_Cx * T_Cx_C

T_W_I0 = pose.xRotationDeg(180)

T_I0_I = interpolate_poses.interpolate_ins_poses(
    ins_path, pose_timestamps[1:], origin_timestamp, use_rtk=True)

T_I0_I = [pose.fromMatrix(np.asarray(i)) for i in T_I0_I]

T_W_Cs = [T_W_I0 * i * T_I_C for i in T_I0_I]
t_W_Cs = np.array([T_W_C.t.ravel() for T_W_C in T_W_Cs])

plt.scatter(t_W_Cs[:, 1], t_W_Cs[:, 0], marker='.', s=4)

plt.axes().set_aspect('equal')
plt.grid()
plt.show()

The traversal used in this example is 2014-11-28-12-07-13.

I have also direclty plot the poses using northing/easting data provided in the rtk.csv file an I have observed similar issues at similar sections of the traversal.

[echo-gui]

Hi, I try to generate groundtruth trajectories using the rtk files, may I ask what is the pose in pose.fromMatrix()?