在这里记录一下刚体运动中变换矩阵的性质。

刚体运动中变换矩阵的逆

数学公式

刚体运动中的变换矩阵为
$$Trans=\begin{bmatrix}R & T \\ 0 & 1\end{bmatrix},$$
其中$R_{3\times3}$为旋转矩阵,$T_{3\times1}$为平移矩阵。在刚体运动中,$R$是纯旋转,旋转矩阵是正交矩阵,所以$R$满足如下性质
$$R^{-1}=R^T,$$
又因为分块矩阵求逆的性质
$$M=\begin{bmatrix}A & B \\ 0 & D\end{bmatrix} ,则 \\ M^{-1}=\begin{bmatrix}A{-1} & -A^{-1}\cdot B\cdot D^{-1} \\ 0 & D^{-1}\end{bmatrix},$$
所以
$$\begin{matrix}Trans^{-1} &=& \begin{bmatrix}R^{-1} & -R^{-1}\cdot T \\ 0 & 1\end{bmatrix} \\ &=& \begin{bmatrix}R^T & -R^T\cdot T \\ 0 & 1\end{bmatrix} \end{matrix}$$

Eigen3代码

Eigen::Transform部分

Eigen3库的代码中Eigen::Transform相关的都已经对刚体运动的变换矩阵做了简化处理,而不是直接求逆,直接求逆效率地下且精度损失严重。相关代码可以参考Transform::inverse()函数代码。

以下是Eigen3库的处理原理:

  1. 投影变换Projective:整个$4\times4$矩阵直接求逆
    $$Trans^{-1}$$
  2. 仿射变换Affine:左上角旋转部分$3\times3$矩阵求逆,再带入分块矩阵求逆
    $$Trans^{-1} = \begin{bmatrix}R^{-1} & -R^{-1}\cdot T \\ 0 & 1\end{bmatrix}$$
  3. 等距变换Isometry:左上角$3\times3$矩阵是纯旋转,按照转置分块矩阵求逆方法运算
    $$Trans^{-1} = \begin{bmatrix}R^T & -R^T\cdot T \\ 0 & 1\end{bmatrix}$$

使用时直接transform.inverse()即可,Eigen3库已经帮你做好所有事情了

Eigen::Matrix部分

如果直接使用Eigen3库的矩阵模块进行刚体运动变换矩阵求逆的话,就需要自己单独处理了。

  • 等距变换

    template<typename Scalar, int Dim>
    Eigen::Matrix<Scalar, Dim, Dim> inverse(const Eigen::Matrix<Scalar, Dim, Dim> &trans) {
    using Matrix = Eigen::Matrix<Scalar, Dim, Dim>;
    Matrix res = Matrix::Identity();
    res.template topLeftCorner<Dim-1, Dim-1>()
        = trans.template topLeftCorner<Dim-1, Dim-1>().transpose();
    res.template topRightCorner<Dim-1, 1>()
        = - res.template topLeftCorner<Dim-1, Dim-1>()
            * trans.template topRightCorner<Dim-1, 1>();
    return res;
    }
  • 仿射变换

    template<typename Scalar, int Dim>
    Eigen::Matrix<Scalar, Dim, Dim> inverse(const Eigen::Matrix<Scalar, Dim, Dim> &trans) {
    using Matrix = Eigen::Matrix<Scalar, Dim, Dim>;
    Matrix res = Matrix::Identity();
    res.template topLeftCorner<Dim-1, Dim-1>()
        = trans.template topLeftCorner<Dim-1, Dim-1>().inverse();
    res.template topRightCorner<Dim-1, 1>()
        = - res.template topLeftCorner<Dim-1, Dim-1>()
            * trans.template topRightCorner<Dim-1, 1>();
    return res;
    }
  • 投影变换

    template<typename Scalar, int Dim>
    Eigen::Matrix<Scalar, Dim, Dim> inverse(const Eigen::Matrix<Scalar, Dim, Dim> &trans) {
      return trans.inverse();
    }
Last modification:January 9, 2020
If you think my article is useful to you, please feel free to appreciate