【视觉SLAM十四讲】三维空间刚体运动

本文为视觉 SLAM 学习总结。第三讲讲解的是观测方程中的 x x x 是什么。

本讲内容概要

  • 三维空间的刚体运动的描述方式:旋转矩阵、变换矩阵、四元数和欧拉角
  • Eigen 库的矩阵、几何模块的使用方法

旋转矩阵

点和向量,坐标系

定义坐标系后,向量可由 R 3 R^3 R3 坐标表示:
a ⃗ = [ e 1 ⃗ , e 2 ⃗ , e 3 ⃗ ] [ a 1 a 2 a 3 ] = a 1 e 1 ⃗ + a 2 e 2 ⃗ + a 3 e 3 ⃗ \vec{a}=[\vec{e_1},\vec{e_2},\vec{e_3}] \left[ \begin{matrix} a_1 \\ a_2 \\ a_3 \\ \end{matrix} \right]=a_1\vec{e_1}+a_2\vec{e_2}+a_3\vec{e_3} a =[e1 ,e2 ,e3 ]a1a2a3=a1e1 +a2e2 +a3e3
直接用坐标进行向量间的运算。

我们对外积进行介绍,这个概念在之后的学习中会经常使用,我们可以将向量叉乘写为矩阵点乘的形式:

【视觉SLAM十四讲】三维空间刚体运动_第1张图片

其中 a^ 表示将向量转换为矩阵的形式,为反对称矩阵,也可称为反对称符号。

坐标系间的欧式变换

SLAM 中有两个坐标系,一个是世界坐标系,通常以地面参考,另一个为机器人坐标系,会随着机器人的运动而运动。

那么坐标系之间是如何变化的?如何计算同一个向量在不同坐标系下的坐标?

【视觉SLAM十四讲】三维空间刚体运动_第2张图片

直观来看,我们需要用旋转+平移来描述刚体运动。

  • 坐标系原点的平移
  • 三个轴的旋转。

同一向量在不同坐标系下可以被描述为以下两种不同的形式:

【视觉SLAM十四讲】三维空间刚体运动_第3张图片

左乘 [ e 1 T e 2 T e 3 T ] \left[ \begin{matrix} e_1^T \\ e_2^T \\ e_3^T \\ \end{matrix} \right] e1Te2Te3T,得:

【视觉SLAM十四讲】三维空间刚体运动_第4张图片

将中间的矩阵定义为 R R R,称为旋转矩阵。其性质有:正交矩阵且行列式=1;相反,满足这些性质的矩阵也可以称为旋转矩阵,也称为特殊正交群,定义如下:
S O ( n ) = { R ∈ R n × n ∣ R R T = I , d e t ( R ) = 1 } SO(n)=\{R∈R^{n×n}|RR^T=I,det(R)=1\} SO(n)={RRn×nRRT=I,det(R)=1}
n = 3 n=3 n=3 时为 S O ( 3 ) SO(3) SO(3) 的旋转矩阵。

旋转矩阵描述了两个坐标的变换关系。 a 1 = R 12 a 2 , a 2 = R 21 a 1 a_1=R_{12}a_2, a_2=R_{21}a_1 a1=R12a2,a2=R21a1,于是:
R 21 = R 12 − 1 = R 12 T R_{21}=R_{12}^{-1}=R_{12}^T R21=R121=R12T
进一步,三个坐标系(多次旋转)亦有:
a 3 = R 32 a 2 = R 32 R 21 a 1 = R 31 a 1 a_3=R_{32}a_2=R_{32}R_{21}a_1=R_{31}a_1 a3=R32a2=R32R21a1=R31a1
这么多矩阵连乘看起来很复杂?我们可以用一个技巧来记忆:观察下标,我们可以将下标相连的变量进行组合,如: R 32 R 21 = R 31 R_{32}R_{21}=R_{31} R32R21=R31

再加上平移: a ′ = R a + t a^{'}=Ra+t a=Ra+t。两个坐标系的刚体运动可完全由 R , t R,t R,t 描述。

变换矩阵与期齐次坐标

但我们连续进行坐标变换时,叠加形式过于复杂:
c = R 2 ( R 1 a + t 1 ) + t 2 c=R_2(R_1a+t_1)+t_2 c=R2(R1a+t1)+t2
我们需要引入变换矩阵的概念,改变形式:

【视觉SLAM十四讲】三维空间刚体运动_第5张图片

可以将旋转+平移的计算放在一个矩阵中。 T T T 称为变换矩阵. a ˜ a˜ a˜ 表示 a a a 的齐次坐标,则多次变换可写成:
b ˜ = T 1 a ˜ , c ˜ = T 2 b ˜ ⇒ c ˜ = T 2 T 1 a ˜ b˜=T_1a˜,\quad c˜=T_2b˜ \quad \Rightarrow c˜=T_2T_1a˜ b˜=T1a˜,c˜=T2b˜c˜=T2T1a˜
用 4 个数描述三维向量的做法称为齐次坐标,这里引入齐次坐标是为了让矩阵运算能够符合规则——4×4 的矩阵与 3×1 的矩阵无法相乘,需给 3×1 的矩阵增加一维。

变换矩阵称为特殊欧式群,定义如下:

【视觉SLAM十四讲】三维空间刚体运动_第6张图片

可定义反向变换矩阵为:

【视觉SLAM十四讲】三维空间刚体运动_第7张图片

在 SLAM 中,通常定义世界坐标系 T W T_W TW 与机器人坐标系 T R T_R TR。若一个点的世界坐标为 p W p_W pW,机器人坐标系下为 p R p_R pR,那么满足关系:
p R = T R W p W p_R=T_{RW}p_W pR=TRWpW
反之亦然。在实际编程中,可使用 T R W T_{RW} TRW T W R T_{WR} TWR 来描述机器人的位姿。

当我们给 T R W T_{RW} TRW 乘零向量的齐次坐标后,得到一个平移向量,这个向量是世界坐标系原点在机器人坐标系中的位置;如果是 T W R T_{WR} TWR 则为机器坐标系在世界坐标系下的位置,即为机器人的运动轨迹

实践部分:EIGEN

EIGEN 效率较高,我们通常用其描述 C++ 中的一些矩阵运算。

我们可以用下面的命令安装 EIGEN:

sudo apt-get install libeigen3-dev

可以输入 ls /usr/include/eigen3 找到 EIGEN 的安装位置。EIGEN 是一个全是头文件的库,没有库文件,因此无需链接库 target_link_libraries(),仅仅把头文件目录加入就可以了。下面为 CMakeLists.txt 的内容:

cmake_minimum_required( VERSION 2.8 )
project( useEigen )

set( CMAKE_BUILD_TYPE "Release" )
set( CMAKE_CXX_FLAGS "-O3" )

# 添加Eigen头文件,一般会先进行搜索,然后再添加。
include_directories( "/usr/include/eigen3" )

# in osx and brew install
# include_directories( /usr/local/Cellar/eigen/3.3.3/include/eigen3 )

add_executable( eigenMatrix eigenMatrix.cpp )

其中 Eigen/Core 提供了一些核心的矩阵运算。

我们可以看到 Matrix 中的模板参数如下,有 6 个参数组成的类,较为复杂。

// _Scalar为类型,_Rows、_Rows分别为行列,固定不变时可以指定大小,后面有默认值
template<typename _Scalar, int _Rows, int _Rows, int _Options, int _MaxRows, int _MaxCols>

若提前已知矩阵大小,可对矩阵进行加速,动态类型较慢。EIGEN 中还有一些内置的小矩阵,如 Matrix3f 为 3×3 的浮点数矩阵,转到其定义可以发现其实就是一个 typedef。如 Vector3d 其实是一个 1×3 的矩阵。

// 矩阵定义后赋初值
Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero();
// 如果不确定矩阵大小,可以使用动态大小的矩阵。转到Dynamic定义:const int Dynamic = -1;
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > matrix_dynamic;
// 更简单的动态大小的矩阵,X表示不知道维度
Eigen::MatrixXd matrix_x;

下面对 Eigen 阵的操作:

// 流输入符,输入数据(初始化)。类中重载了 <<
matrix_23 << 1, 2, 3, 4, 5, 6;
// 直接cout输出
cout << matrix_23 << endl;
// 用()访问矩阵中的元素,重载了()。
// 对于向量如 Vector3d v_3d,可以写中括号运算符v_3d[0]当作数组。
for (int i=0; i<2; i++) {
	for (int j=0; j<3; j++)
		cout<<matrix_23(i,j)<<"\t";
	cout<<endl;
}

// 矩阵和向量相乘(实际上仍是矩阵和矩阵),重载了乘号
// 但是在Eigen里你不能混合两种不同类型的矩阵,像这样是错的:double不能乘float。
// 此时会报错一长串,很难找到问题。
// 如:Eigen::Matrix result_wrong_type = matrix_23 * v_3d;
// 应该显式转换(c++中有隐式类型提升)
Eigen::Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d;

// 同样不能搞错矩阵的维度
// Eigen::Matrix result_wrong_dimension = matrix_23.cast() * v_3d;

下面演示一些矩阵的运算:

// 四则运算就不演示了,直接用+-*/即可。
matrix_33 = Eigen::Matrix3d::Random();      // 随机数矩阵
cout << matrix_33 << endl << endl;
cout << matrix_33.transpose() << endl;      // 转置
cout << matrix_33.sum() << endl;            // 各元素和
cout << matrix_33.trace() << endl;          // 迹,即对角线元素和
cout << 10*matrix_33 << endl;               // 数乘
cout << matrix_33.inverse() << endl;        // 逆,矩阵规模很大时耗时
cout << matrix_33.determinant() << endl;    // 行列式

下面演示如何求解特征值,特征值的概念见《线性代数》:

// 实对称矩阵可以保证对角化成功
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver ( matrix_33.transpose() * matrix_33 );
cout << "Eigen values = \n" << eigen_solver.eigenvalues() << endl;
cout << "Eigen vectors = \n" << eigen_solver.eigenvectors() << endl;

下面演示解方程:

// 我们求解 matrix_NN * x = v_Nd 这个方程
// N的大小在前边的宏里定义,它由随机数生成
// 直接求逆自然是最直接的,但是求逆运算量大
Eigen::Matrix< double, MATRIX_SIZE, MATRIX_SIZE > matrix_NN;
matrix_NN = Eigen::MatrixXd::Random( MATRIX_SIZE, MATRIX_SIZE );
Eigen::Matrix< double, MATRIX_SIZE,  1> v_Nd;
v_Nd = Eigen::MatrixXd::Random( MATRIX_SIZE,1 );

clock_t time_stt = clock(); // 计时
// 直接求逆
Eigen::Matrix<double,MATRIX_SIZE,1> x = matrix_NN.inverse()*v_Nd;
cout <<"time use in normal inverse is " << 1000* (clock() - time_stt) / (double)CLOCKS_PER_SEC << "ms"<< endl;

// 通常用矩阵分解来求,例如QR分解,速度会快很多
time_stt = clock();
x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
cout <<"time use in Qr decomposition is " <<1000*  (clock() - time_stt)/ (double)CLOCKS_PER_SEC <<"ms" << endl;

关于矩阵分解的相关知识见《矩阵论》。

旋转向量与欧拉角

旋转向量

一个三维的旋转最少可以用三个数进行描述,但我们的旋转矩阵使用了 3×3=9 个数字描述,浪费存储空间,不够紧凑。并且旋转矩阵还有一些约束,不能当作普通的矩阵进行优化。因此我们考虑用其他方式进行描述。

我们任意一次旋转可以分解为绕着一个轴 w w w 转过了一个角度。

【视觉SLAM十四讲】三维空间刚体运动_第8张图片

方向为旋转轴方向 n ⃗ \vec{n} n 、长度为转过的角度 θ \theta θ 的向量,被称为角轴或旋转向量。角轴只有三个量,且没有约束。角轴也就是 CH4 中要介绍的李代数。
w ⃗ = θ n ⃗ \vec{w}=\theta \vec{n} w =θn
角轴与旋转矩阵之间可以相互转换。角轴转旋转矩阵使用罗德里格斯公式(直接给出结果):

在这里插入图片描述

如果我们知道了 R R R,也可以转换为角轴:

  • 角度: θ = a r c c o s ( t r ( R ) − 1 2 ) \theta=arccos(\frac{tr(R)-1}{2}) θ=arccos(2tr(R)1)
  • 轴: R n = n Rn=n Rn=n。轴 n ⃗ \vec{n} n 在旋转后不动,因此 n ⃗ \vec{n} n 对应为矩阵 R R R 特征值为 1 的特征向量,解出方程即可。

欧拉角

旋转矩阵和旋转向量不是很直观,欧拉角是一种直观的表示方法,方便人观察,常用于人机交互,程序中很少用来描述旋转。

欧拉角将一次旋转分界为三次不同轴上的转动,可以求出绕每个轴转动了多少度。因描述转动的轴的顺序可以不同,且分为转动前的轴(定轴)和转动后的轴(动轴),欧拉角有多种定义方式,常见的为 yaw-pitch-roll(偏航-俯仰-滚转)角。

【视觉SLAM十四讲】三维空间刚体运动_第9张图片

  1. 绕 Z 轴旋转,得到偏航角 yaw;
  2. 绕旋转之后的 Y 轴旋转,得到俯仰角 pitch;
  3. 绕旋转之后的 Z 轴旋转,得到滚转角 roll。

万向锁问题:

下图中第三次旋转和第一次旋转其实是绕着同一个轴进行旋转,使得系统少了一个自由度——存在奇异性问题。

【视觉SLAM十四讲】三维空间刚体运动_第10张图片

因万向锁的问题,欧拉角很少在 SLAM 中使用,仅与人进行交互。

四元数

(单位圆上)复数可以表达二维平面的旋转。四元数是一种扩展的复数,有 3 个虚部,可以描述三维空间的旋转:
q ⃗ = q 0 + q 1 i + q 2 j + q 3 k \vec{q}=q_0+q_1i+q_2j+q_3k q =q0+q1i+q2j+q3k
四元数由一个实部和一个虚部向量组成, q ⃗ = [ s ⃗ , v ⃗ ] \vec{q}=[\vec{s},\vec{v}] q =[s ,v ] s = q 0 ∈ R , v ⃗ = [ q 1 , q 2 , q 3 ] T ∈ R 3 s=q_0∈R,\vec{v}=[q_1,q_2,q_3]^T∈R^3 s=q0R,v =[q1,q2,q3]TR3

虚部之间的关系:

【视觉SLAM十四讲】三维空间刚体运动_第11张图片

记忆技巧:自己和自己运算像复数,自己和别人运算像叉乘。

四元数的运算性质

  • 加减法:实部和虚部分别相加
  • 乘法:与复数相同
  • 共轭 q ∗ q^* q:虚部取反
  • 模长:各部分平方开根号
  • 逆: q − 1 = q ∗ / ∣ ∣ q ∣ ∣ 2 q^{-1}=q^*/||q||^2 q1=q/q2
  • 点乘: q a ⃗ ⋅ q b ⃗ = s a s b + x a x b i ⃗ + y a y b j ⃗ + z a z b k ⃗ \vec{q_a}·\vec{q_b}=s_as_b+x_ax_b\vec{i}+y_ay_b\vec{j}+z_az_b\vec{k} qa qb =sasb+xaxbi +yaybj +zazbk

四元数与角轴的关系

角轴到四元数:
q ⃗ = [ c o s θ 2 , n x s i n θ 2 , n y s i n θ 2 , n z s i n θ 2 ] T \vec{q}=[cos\frac{\theta}{2},n_xsin\frac{\theta}{2},n_ysin\frac{\theta}{2},n_zsin\frac{\theta}{2}]^T q =[cos2θ,nxsin2θ,nysin2θ,nzsin2θ]T
四元数到角轴:

【视觉SLAM十四讲】三维空间刚体运动_第12张图片

四元数亦可转换为旋转矩阵、欧拉角。

四元数表示旋转

设点 p p p 经过一次以 q q q 表示的旋转后,得到了 p ′ p' p,它们的关系:

  1. p p p 的坐标用四元数表示(虚四元数): p = [ 0 , x , y , z ] = [ 0 , v ⃗ ] p=[0,x,y,z]=[0,\vec{v}] p=[0,x,y,z]=[0,v ]
  2. 旋转之后的关系: p ′ = q p q − 1 p'=qpq^{-1} p=qpq1,可以验证 p ′ p' p 也是四元数。几何意义是先把四元数转到四维空间中,然后再转回来。

四元数紧凑、无奇异性。

以上这些方法中,最常用的是矩阵和四元数。当把轨迹存到文件中时通常使用四元数,矩阵太麻烦。

实践部分:EIGEN 几何模块

下面程序演示 Eigen 几何模块的使用方法。Eigen/Geometry 模块提供了各种旋转和平移的表示。

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

// 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f
// 这里为单位矩阵,表示没有旋转
Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();

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

// 给定沿哪个轴转多少角度,这里沿 Z 轴旋转 45 度
Eigen::AngleAxisd rotation_vector ( M_PI/4, Eigen::Vector3d ( 0,0,1 ) );
cout .precision(3);
// 调用成员函数matrix,将角轴转换为矩阵
cout<<"rotation matrix =\n"<<rotation_vector.matrix() <<endl;
// 也可以直接赋值,使用toRotationMatrix
rotation_matrix = rotation_vector.toRotationMatrix();

可以进行坐标变换:

// 用 AngleAxis 可以进行坐标变换
Eigen::Vector3d v ( 1,0,0 );
Eigen::Vector3d v_rotated = rotation_vector * v; // 重载了*,旋转v
cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;
// 或者用旋转矩阵,打印结果相同
v_rotated = rotation_matrix * v;
cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;

得到结果相同:

在这里插入图片描述

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

Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles ( 2,1,0 ); // ZYX顺序,即roll pitch yaw顺序
cout<<"yaw pitch roll = "<<euler_angles.transpose()<<endl;

欧氏变换矩阵使用 Eigen::Isometry:

// 旋转为0,平移也为0的标准变换矩阵
Eigen::Isometry3d T=Eigen::Isometry3d::Identity(); // 虽然称为3d,实质上是4*4的矩阵
T.rotate ( rotation_vector ); // 按照rotation_vector进行旋转,将旋转放到变换矩阵中
T.pretranslate ( Eigen::Vector3d ( 1,3,4 ) ); // 把平移向量设成(1,3,4)
cout << "Transform matrix = \n" << T.matrix() <<endl;

用变换矩阵进行坐标变换:

Eigen::Vector3d v_transformed = T*v; // 相当于R*v+t,重载了*自动转换为齐次坐标
cout<<"v tranformed = "<<v_transformed.transpose()<<endl;

四元数:

// 可以直接把AngleAxis赋值给四元数,反之亦然。
Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector );
// 请注意coeffs的顺序是(x,y,z,w),w为实部,前三者为虚部!!!
cout<<"quaternion = \n"<<q.coeffs() <<endl;
// 也可以把旋转矩阵赋给它
q = Eigen::Quaterniond ( rotation_matrix );
cout<<"quaternion = \n"<<q.coeffs() <<endl;

//Eigen::Matrix3d qx = q.toRotationMatrix();// 四元数转换为矩阵

// 使用四元数旋转一个向量,使用重载的乘法。
v_rotated = q*v; // 先将向量转换为四元数,数学上是qvq^{-1}
cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;

以上仅演示了基本的做法。更深入的操作可以查看 EIGEN 库文档。

你可能感兴趣的:(#,视觉SLAM)