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);

上一篇:SALM入门教程:SLAM for Dummies


下一篇:视觉SLAM十四讲(高翔第二版)