知乎同款
由KITTI数据集Odometry模块的devkit_odometry\devkit\readme.txt所述，KITII数据集提供里程计的真值是在左相机表坐标系下的，并没有提供激光雷达坐标系下的真值。因此，求得激光雷达坐标系下的真值，对我们使用KITTI数据集进行点云处理任务时，将会有极大的方便。
readme.txt提供了激光雷达坐标系下点云到图像坐标系的转换关系：
x = P i ∗ T r ∗ x x=P_i * T r * x x=Pi∗Tr∗x
其中， P i P_i Pi指投影矩阵， T r T r Tr将一个点从雷达坐标系变换到相机坐标系。
根据calib.txt， T r T r Tr为
T r = [ 4.276802385584 e − 04 − 9.999672484946 e − 01 − 8.084491683471 e − 03 − 1.198459927713 e − 02 − 7.210626507497 e − 03 8.081198471645 e − 03 − 9.999413164504 e − 01 − 5.403984729748 e − 02 9.999738645903 e − 01 4.859485810390 e − 04 − 7.206933692422 e − 03 − 2.921968648686 e − 01 0 0 0 1 ] T r = \left[\begin{array}{cccc} 4.276802385584e-04 & -9.999672484946e-01 & -8.084491683471e-03 & -1.198459927713e-02 \\ -7.210626507497e-03 & 8.081198471645e-03 & -9.999413164504e-01 & -5.403984729748e-02 \\ 9.999738645903e-01 & 4.859485810390e-04 & -7.206933692422e-03 & -2.921968648686e-01 \\ 0 & 0 & 0 & 1 \end{array}\right] Tr=⎣⎢⎢⎡4.276802385584e−04−7.210626507497e−039.999738645903e−010−9.999672484946e−018.081198471645e−034.859485810390e−040−8.084491683471e−03−9.999413164504e−01−7.206933692422e−030−1.198459927713e−02−5.403984729748e−02−2.921968648686e−011⎦⎥⎥⎤
设相机坐标系中有两点， p ˉ 1 \bar{p}_1 pˉ1和 p ˉ 2 \bar{p}_2 pˉ2，其在激光雷达坐标系下对应为 p 1 p_1 p1和 p 2 p_2 p2,则有如下关系：
p 1 = T r ∗ p ˉ 1 (1) p_1 = T r * \bar{p}_1 \tag{1} p1=Tr∗pˉ1(1)
p 2 = T r ∗ p ˉ 2 (2) p_2 = T r * \bar{p}_2 \tag{2} p2=Tr∗pˉ2(2)
设 R 2 1 R_2^1 R21表示相机坐标系下两点的位姿变换关系， R ˉ 2 1 \bar{R}_2^1 Rˉ21表示激光雷达坐标系下两点的位姿变换关系：
p 1 = R 2 1 ∗ p 2 (3) p_1 = R_2^1 * p_2 \tag{3} p1=R21∗p2(3)
p ˉ 1 = R ˉ 2 1 ∗ p ˉ 2 (4) \bar{p}_1 = \bar{R}_2^1 * \bar{p}_2 \tag{4} pˉ1=Rˉ21∗pˉ2(4)
将公式1,2代入公式3，可得
T r ∗ p ˉ 1 = R 2 1 ∗ T r ∗ p ˉ 2 (5) T r * \bar{p}_1 = R_2^1 * T r * \bar{p}_2 \tag{5} Tr∗pˉ1=R21∗Tr∗pˉ2(5)
即
p ˉ 1 = T r − 1 ∗ R 2 1 ∗ T r ∗ p ˉ 2 (6) \bar{p}_1 = T r^{-1} * R_2^1 * T r * \bar{p}_2 \tag{6} pˉ1=Tr−1∗R21∗Tr∗pˉ2(6)
对照公式4,6，得
R ˉ 2 1 = T r − 1 ∗ R 2 1 ∗ T r (7) \bar{R}_2^1 = T r^{-1} * R_2^1 * T r \tag{7} Rˉ21=Tr−1∗R21∗Tr(7)
公式7即我们要求的激光坐标系下的位姿变换与相机坐标系下的位姿变换的转换关系，即里程计真值的变换关系。
