utiasSTARS / pykitti

Python tools for working with KITTI data.
MIT License
1.14k stars 239 forks source link

Error between camera relative poses for odometry Vs raw labels #63

Open brucemuller opened 3 years ago

brucemuller commented 3 years ago

@leeclemnet Hi there. I'm using these codes to compute camera relative poses within sequence 00 for both the odometry and raw labels respectively:

odometry

   # Transformation from world coordinates to camera 2 (colour) in frame i
   T_i = torch.matmul(  self.T_cam0tocam2     ,   torch.inverse(torch.FloatTensor(self.dataset.poses[frame_cami])) )

    # Extract rotation and translation from world to camera frame i
    R_i = torch.FloatTensor(T_i)[0:3,0:3]
    t_i = torch.FloatTensor(T_i)[0:3,3].unsqueeze(-1)

    # Transformation from world coordinates to camera 2 (colour) in frame i
    T_j = torch.matmul(  self.T_cam0tocam2     ,   torch.inverse(torch.FloatTensor(self.dataset.poses[frame_camj])) )

    # Extract rotation and translation from world to camera frame i
    R_j = torch.FloatTensor(T_j)[0:3,0:3]
    t_j = torch.FloatTensor(T_j)[0:3,3].unsqueeze(-1)

    # Relative pose: transformation from frame i to j coordinate systems
    R_itoj = torch.matmul(R_j,tp(R_i,-2,-1))
    t_itoj = t_j - torch.matmul(R_itoj,t_i)

raw

    # Transformation from world coordinates to camera 2 (colour) in frame i
    T_i = torch.matmul(torch.FloatTensor(self.dataset.calib.T_cam2_imu),torch.inverse(torch.FloatTensor(self.dataset.oxts[frame_cami].T_w_imu)))

    # Extract rotation and translation from world to camera frame i
    R_i = torch.FloatTensor(T_i)[0:3,0:3]
    t_i = torch.FloatTensor(T_i)[0:3,3].unsqueeze(-1)

    # Transformation from world coordinates to camera 2 (colour) in frame j
    T_j = torch.matmul(torch.FloatTensor(self.dataset.calib.T_cam2_imu),torch.inverse(torch.FloatTensor(self.dataset.oxts[frame_camj].T_w_imu)))

    # Extract rotation and translation from world to camera frame i
    R_j = torch.FloatTensor(T_j)[0:3,0:3]
    t_j = torch.FloatTensor(T_j)[0:3,3].unsqueeze(-1)

    # Relative pose: transformation from frame i to j coordinate systems
    R_itoj = torch.matmul(R_j,tp(R_i,-2,-1))
    t_itoj = t_j - torch.matmul(R_itoj,t_i)

There seems to be error between these methods. For example the first X relative poses the error looks like this where the x-axis corresponds to each pair (i.e. frames 0-->1, 0-->2 ... 0-->10, 1-->2, 1-->3, ... , 1 -- > 11 , 2-->3,... etc)

raw vs odo error

Any idea why this might be? Did the odometry dataset do further processing on the GPS/IMU raw measurements? Thanks!