【KITTI】Kitti数据集 Tr（旋转平移）矩阵求逆-Toy模板网

这篇具有很好参考价值的文章主要介绍了【KITTI】Kitti数据集 Tr（旋转平移）矩阵求逆。希望对大家有所帮助。如果存在错误或未考虑完全的地方，请大家不吝赐教，您也可以点击"举报违法"按钮提交疑问。

本节详细介绍KITTI从图像坐标系到激光雷达坐标系的变换原理、过程和代码。

Kitti数据集的Tr_velo_to_cam矩阵是将激光雷达点云坐标变换到图像坐标系。Tr是一个3x4的矩阵，直接左乘激光雷达坐标即可得到图像坐标系中的坐标。

Tr可以认为是由旋转矩阵R和平移矩阵T组成，即表示为增广矩阵R|T。旋转矩阵R的维度是3x3，T的维度为1x3。将图像坐标系中的坐标变回激光雷达坐标系中需要用到Tr的逆矩阵。Tr逆矩阵求解方法主要有分步求解和直接求解两种。

1 分步求解

Tr矩阵的变换过程相当于：（1）用R对坐标系进行变换，将原坐标变换到新的坐标系下，R的列向量就是新的坐标系的坐标轴向量；（2）用T对变换后的坐标进行平移，平移尺度由变换后的坐标系决定。

Tr的逆矩阵相当于反向操作：（1）用R的逆矩阵进行反向变换；（2）对反向变换的坐标进行平移，平移尺度由变换后的坐标系决定，此时变换后的坐标系相当于最原始的坐标系。

因此，Tr的逆矩阵可以表示为：

根据上述分析可知，Tr的逆矩阵先通过R的逆矩阵求得反变换坐标系，然后用R^{-1}T来求得平移尺度，加上一个负号进行反向平移即可。

注意到：用于变换前后坐标系是正交的，那么R一定也是一个正交矩阵。正交矩阵的逆矩阵为其转置矩阵。因此第一种求逆方法如下：

def inverse_rigid_trans(Tr):

    ''' Inverse a rigid body transform matrix (3x4 as [R|t])

        [R'|-R't; 0|1]

    '''

    inv_Tr = np.zeros_like(Tr)  # 3x4

    inv_Tr[0:3, 0:3] = np.transpose(Tr[0:3, 0:3])

    inv_Tr[0:3, 3] = np.dot(-np.transpose(Tr[0:3, 0:3]), Tr[0:3, 3])

    return inv_Tr

2 直接求解

直接求解是指直接对矩阵进行求逆运算，但是这种运算一般对应方阵。因此，在进行求逆运算时，需要把Tr矩阵增加一行[0, 0, 0, 1]，进而变成4x4方阵。方法如下所示：

Tr = np.concatenate((Tr, np.array([0, 0, 0, 1]).reshape(1, 4)))
inv_Tr = np.linalg.inv()

3 结果对比

假设输入为：

array([[ 7.533745e-03, -9.999714e-01, -6.166020e-04, -4.069766e-03],

       [ 1.480249e-02,  7.280733e-04, -9.998902e-01, -7.631618e-02],

       [ 9.998621e-01,  7.523790e-03,  1.480755e-02, -2.717806e-01]],

      dtype=float32)

分步求解的结果为：

array([[ 7.5337449e-03,  1.4802490e-02,  9.9986207e-01,  2.7290344e-01],

       [-9.9997139e-01,  7.2807330e-04,  7.5237900e-03, -1.9692658e-03],

       [-6.1660202e-04, -9.9989021e-01,  1.4807550e-02, -7.2285905e-02]],

      dtype=float32)

直接求解的结果为：

array([[ 7.53374482e-03,  1.48024872e-02,  9.99862035e-01,

         2.72903444e-01],

       [-9.99971472e-01,  7.28073268e-04,  7.52379041e-03,

        -1.96926581e-03],

       [-6.16602039e-04, -9.99890136e-01,  1.48075518e-02,

        -7.22858974e-02],

       [ 0.00000000e+00,  0.00000000e+00,  0.00000000e+00,

         1.00000000e+00]])

通过对比可以看到，直接求解的结果的前三行与分步求解的结果一致。