search

utiasSTARS / pykitti

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

Calculation of transforms #70

Closed RaviBeagle closed 1 year ago

RaviBeagle commented 1 year ago

Hello,

I am trying to run a project that uses pykitti and here I have the code from odometry.py:

        # Compute the rectified extrinsics from cam0 to camN
        T1 = np.eye(4)
        T1[0, 3] = P_rect_10[0, 3] / P_rect_10[0, 0]
        T2 = np.eye(4)
        T2[0, 3] = P_rect_20[0, 3] / P_rect_20[0, 0]
        T3 = np.eye(4)
        T3[0, 3] = P_rect_30[0, 3] / P_rect_30[0, 0]

        print(T1,T2,T3)

         # Compute the velodyne to rectified camera coordinate transforms
        data['T_cam0_velo'] = np.reshape(filedata['Tr'], (3, 4))
        data['T_cam0_velo'] = np.vstack([data['T_cam0_velo'], [0, 0, 0, 1]])
        data['T_cam1_velo'] = T1.dot(data['T_cam0_velo'])
        data['T_cam2_velo'] = T2.dot(data['T_cam0_velo'])
        data['T_cam3_velo'] = T3.dot(data['T_cam0_velo'])

        print("T_cam0_velo:\n",data['T_cam0_velo'])
        print("T_cam1_velo:\n",data['T_cam1_velo'])
        print("T_cam2_velo:\n",data['T_cam2_velo'])
        print("T_cam3_velo:\n",data['T_cam3_velo'])

Now I see T1,T2 and T3 printed as 

T1:
[[ 1.    0.    0.   -0.54]
 [ 0.    1.    0.    0.  ]
 [ 0.    0.    1.    0.  ]
 [ 0.    0.    0.    1.  ]]
T2:
 [[1.   0.   0.   0.06]
 [0.   1.   0.   0.  ]
 [0.   0.   1.   0.  ]
 [0.   0.   0.   1.  ]] 
T3:
[[ 1.    0.    0.   -0.48]
 [ 0.    1.    0.    0.  ]
 [ 0.    0.    1.    0.  ]
 [ 0.    0.    0.    1.  ]]

The velodyne to camX transforms are:

T_cam0_velo:
 [[ 1.          0.          0.          0.27000001]
 [ 0.          1.          0.          0.        ]
 [ 0.          0.          1.         -0.08000004]
 [ 0.          0.          0.          1.        ]]
T_cam1_velo:
 [[ 1.          0.          0.         -0.26999999]
 [ 0.          1.          0.          0.        ]
 [ 0.          0.          1.         -0.08000004]
 [ 0.          0.          0.          1.        ]]
T_cam2_velo:
 [[ 1.          0.          0.          0.33000001]
 [ 0.          1.          0.          0.        ]
 [ 0.          0.          1.         -0.08000004]
 [ 0.          0.          0.          1.        ]]
T_cam3_velo:
 [[ 1.          0.          0.         -0.20999999]
 [ 0.          1.          0.          0.        ]
 [ 0.          0.          1.         -0.08000004]
 [ 0.          0.          0.          1.        ]]

If I try to print the transforms using my code I get the values as which seems right according to the sensor setup I have done in my virtual environment of CARLA

Tr_velo2cam0: 
[[ 1.          0.          0.          0.27000001]
 [ 0.          1.          0.          0.        ]
 [ 0.          0.          1.         -0.08000004]
 [ 0.          0.          0.          1.        ]]
Tr_velo2cam1: 
[[ 1.          0.          0.          0.27000001]
 [ 0.          1.          0.         -0.54000002]
 [ 0.          0.          1.         -0.08000004]
 [ 0.          0.          0.          1.        ]]
Tr_velo2cam2: 
[[ 1.          0.          0.          0.27000001]
 [ 0.          1.          0.          0.06      ]
 [ 0.          0.          1.         -0.08000004]
 [ 0.          0.          0.          1.        ]]
Tr_velo2cam3: 
[[ 1.          0.          0.          0.27000001]
 [ 0.          1.          0.         -0.47999999]
 [ 0.          0.          1.         -0.08000004]
 [ 0.          0.          0.          1.        ]]

I am not sure why this is T1[0, 3] = P_rect_10[0, 3] / P_rect_10[0, 0] and not T1[ 1 , 3] = P_rect_10[0, 3] / P_rect_10[0, 0]

RaviBeagle commented 1 year ago

This is not an bug. Sorry, my bad understanding!