刚体运动中变换矩阵的性质小结（持续更新）

zyuzhi

October 30, 2018

4766 views

No comments

2168 words

数学编程

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

## 刚体运动中变换矩阵的逆

### 数学公式

刚体运动中的变换矩阵为
$$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()][1]函数代码。

以下是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库的矩阵模块进行刚体运动变换矩阵求逆的话，就需要自己单独处理了。

- 等距变换
```cpp
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;
}
```
- 仿射变换
```cpp
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;
}
```
- 投影变换
```cpp
template<typename Scalar, int Dim>
Eigen::Matrix<Scalar, Dim, Dim> inverse(const Eigen::Matrix<Scalar, Dim, Dim> &trans) {
    return trans.inverse();
}
```

[1]: https://github.com/eigenteam/eigen-git-mirror/blob/599e4685c7fd8ec221b64b6b09aaec344c83a897/Eigen/src/Geometry/Transform.h#L1202

Last modification：January 9, 2020