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

刚体运动中变换矩阵的逆

数学公式

刚体运动中的变换矩阵为
$$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();
  }