视觉SLAM14精讲——三维空间刚体运动1.1

三维空间刚体运动

特殊正交群与特殊欧式群

接下来我们继续讨论特殊正交群。一次变换的表示如下：
$$a_{(3,1)}^{\prime }=R_{(3,3)}{a}_{(3,1)}+t_{(3,1)}$$
公式右下角标明了矩阵的形状。需要注意的是，所有有关位置的变量($a^{\prime },a,t$等)均为列向量，三行一列。很多人初始化这些变量的时候容易将其初始化为行向量。整个变换的过程看起来会像下式所示：
$$\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{13}\\ r_{31}& r_{32}& r_{33}\end{array}\right]\cdot \left[\begin{array}{c}x\\ y\\ z\end{array}\right]+\left[\begin{array}{c}{t}_x\\ t_y\\ t_z\end{array}\right]=\left[\begin{array}{c}{x}^{\prime }\\ y^{\prime }\\ z^{\prime }\end{array}\right]$$
然后讨论一下各个编程语言环境下的实现。Python非常简单直接:

import numpy as np
a = np.array([[1],[1],[1]])
t = np.array([[1],[1],[1]])
R = np.eye(3)

a_ = R.dot(a)+t
# a_ = R@a+t
print(a_)
#output
#[[2.]
# [2.]
# [2.]]

这里简单用单位矩阵做了一下演示，坐标进行了移动。需要要注意两点。

• 一个是前面提到的，列向量的初始化形式不是a = np.array([1,1,1])而是a = np.array([[1],[1],[1]])，或者可以先初始化行向量，然后转置一下。
• 其次是要注意乘法的形式。这里需要的是矩阵乘法。*乘法在目前使用的python版本中是数位乘法，应该采用矩阵乘法，则使用@乘法调用.dot()函数。

C++代码主要在于复杂的初始化过程

//OPENCV
cv::Mat a = (cv::Mat_<float>(3,1)<<1, 1, 1);
cv::Mat t = (cv::Mat_<float>(3,1)<<1, 1, 1);
cv::Mat R = (cv::Mat_<float>(3,3)<<1,0,0, 0,1,0, 0,0,1);

cv::Mat a_ = R * a + t;
cout<<a_<<endl;

//Eigen
Eigen::Vector3d a(0,0,1);
Eigen::Vector3d t(0,0,1);
// Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
Eigen::Matrix3d R = {
    1, 0, 0,
    0, 1, 0,
    0, 0, 1
};

Eigen::Vector3d a_ = R*a + t;
cout<<a_<<endl;

特殊欧式群

特殊欧式群的引入其实就是为了将旋转和平移都整合在一个表达当中。如下：
$$Transform=T=\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]$$
这里最后一行无论如何变化均是[0 0 0 1]， 是为了满足齐次的需要。一定要将大小T区分开，大T是Tranform变换，既包含旋转也包含平移; 小t是平移矩阵translation，仅涉及到坐标的变动。T之间也是能够通过乘法进行变换的，变换的过程直接涵盖了旋转和平移两个部分。但是相应的，坐标也需要变成齐次的形式：

$$\left[\begin{array}{cccc}1& 0& 0|& 1\\ 0& 1& 0|& 1\\ 0& 0& 1|& 1\\ \overline{0}& \overline{0}& \overline{0}& \overline{1}\end{array}\right]\cdot \left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]=\left[\begin{array}{c}{x}^{\prime }\\ y^{\prime }\\ z^{\prime }\\ 1\end{array}\right]$$
这个乘法的过程会让右侧的乘数先乘以旋转矩阵的分量，而齐次状态下的t分量不变，结果仍旧是先乘以旋转，再加平移矩阵。同样的书中给出了变换T逆的形式，同上一章一样，仍然建议使用数学性质进行求逆。普通的求逆函数会消耗过多的计算资源，的到的值反而是近似值。

对于特殊欧式群的应用，其实很多项目代码中多数情况下会使用旋转矩阵和平移矩阵。使用特殊欧式群的场景多是一些传感器外参的表达，例如VINS和ORB SLAM的配置文件中，就有表示双目和imu之间空间关系的变换矩阵T。求逆的表达同样重要，一些厂商给出的位姿刚好与我们所需求的相反，则需要调用数学公式求得逆向的外参，而不是重新进行标定。