SLAM十四讲-(3)三维空间刚体运动

描述刚体在三维空间的运动：一次旋转加一次平移。

旋转矩阵

坐标系间的欧式变换

对于刚体运动，可以表示为一次旋转加一次平移运动。因此，可以定义一个物体的刚体运动表示为：

[ a 1 ′ a 2 ′ ] = [ e 1 e 2 e 3 e 4 ] [ a 1 a 2 ] = R [ a 1 a 2 ] \begin{bmatrix}a1'\\a2'\end{bmatrix}=\begin{bmatrix}e1&e2\\e3&e4\end{bmatrix}\begin{bmatrix}a1\\a2\end{bmatrix}=R\begin{bmatrix}a1\\a2\end{bmatrix} [a1′a2′​]=[e1e3​e2e4​][a1a2​]=R[a1a2​]

其中的R称为旋转矩阵。旋转矩阵是行列式为1的正交矩阵。

旋转加上平移表示为：

a’=Ra+t

变换矩阵和齐次坐标

通过在三维向量的末尾添加1，变为齐次坐标。

[ a ′ 1 ] = [ R t 0 1 ] [ a 1 ] = T [ a 1 ] \begin{bmatrix}a'\\1\end{bmatrix}=\begin{bmatrix}R&t\\0&1\end{bmatrix}\begin{bmatrix}a\\1\end{bmatrix}=T\begin{bmatrix}a\\1\end{bmatrix} [a′1​]=[R0​t1​][a1​]=T[a1​]

T叫做变换矩阵。

Eigen实践

Eigen中所有向量和矩阵都是Eigen::Matrix,它是一个模板类，例如，声明一个2x3的矩阵:

Matrix<float,2,3> matrix_23
           

Vector3d 实质上是 Eigen::Matrix<double, 3, 1>，即三维向量

求解matrix_NN * x = v_Nd 方程有三种方法：

1.求矩阵的逆

Matrix<double, MATRIX_SIZE, 1> x = matrix_NN.inverse() * v_Nd;
           

2.用矩阵分解来求，例如QR分解

x = matrix_NN.colPivHouseholderQr().solve(v_Nd);

3.对于正定矩阵，还可以用cholesky分解来解方程

x = matrix_NN.ldlt().solve(v_Nd);
           

欧拉角

欧拉角是围绕着三个轴的旋转来组成的旋转变换。但是会存在万向锁问题，当旋转有两个轴重合后，就会丢失一个自由度。

四元数

参考复数形式，四元数q有一个实部和三个虚部，表达式：

q=q0+q1i+q2j+q3k

其中i，j，k是四元数的三个虚部，相互垂直的单位向量。

四元数旋转公式为：

p’ = qpq^(-1)

实例

3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f：

Matrix3d rotation_matrix = Matrix3d::Identity();
           

旋转向量使用 AngleAxis, 它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）

AngleAxisd rotation_vector(M_PI / 4, Vector3d(0, 0, 1));     //沿 Z 轴旋转 45 度
           

// 欧拉角: 可以将旋转矩阵直接转换成欧拉角

Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0); // ZYX顺序，即yaw-pitch-roll顺序
           

可以直接把AngleAxis,旋转矩阵赋值给四元数，反之亦然

Quaterniond q = Quaterniond(rotation_vector); q = Quaterniond(rotation_matrix);