视觉SLAM十四讲---03三维空间刚体运动02
3、旋转向量和欧拉角
（1）旋转向量
问题的引入：
①旋转矩阵R 3x3 用9个量来表示3个自由度， 变换矩阵T 4x4 用16个量来表示6个自由度，表达方式过于冗余。
②旋转矩阵和变换矩阵自身带有约束，使得估计或优化较难。
使用一个三维向量，其方向同旋转轴一致，而长度等于旋转角，即用旋转向量来描述一次旋转。
使用一个旋转向量和一个平移向量来表达一次变换，即用一个六维向量来描述变换。
- 旋转向量到旋转矩阵的变换
假设有一个旋转轴为n ， 角度为θ的旋转，对应的旋转向量为θn 。
由罗德里格斯公式（Rodrigues’s Formula) 可以得到旋转矩阵为：
- 旋转矩阵到旋转向量的变换
对应转角θ，有：
证明如下：
由于“旋转轴经过旋转之后不变”，可有 Rn = n 。故，转轴n 是矩阵R 特征值1对应的特征向量。
（3）欧拉角
-
“偏航-俯仰-滚转”（yaw-pitch-roll）即ZYX转角分解
- 绕物体的Z轴旋转，得到偏航角yaw
- 绕旋转之后的Y轴旋转，得到俯仰角pitch
- 绕旋转之后的X轴旋转，得到滚转角roll
用[r,p,y]T来描述任意旋转。
-
万向锁问题
在俯仰角为±90°时，第一次旋转与第三次旋转使用同一个轴，使得系统失去了一个自由度。
使用三个实数来表达三维旋转都为遇到这样的问题，故欧拉角不适合插值和迭代，也很少在SLAM中直接使用欧拉角。
4、四元数
（1）四元数定义
为了解决三维向量奇异性（万向锁）的问题 ，Hamilton找到一种扩展的复数四元数 。
- 一个四元数q 定义如下：
q = q0 + q1i + q2j + q3k
其中i，j，k为四元数的三个虚部，且满足
i2 = j2 = k2 = -1
ij = k, ji = -k
jk = i, kj = -i
ki = j, ik = -j
q = [s, v], s = q0 ∈ R, v = [q1, q2, q3]T ∈ R3
也可以简化为用一个标量和一个向量来表达四元数。
- 使用单位四元数来描述三维空间中的旋转
假设某个旋转是绕单位向量n = [nx, ny, nz]T进行了角度为θ的旋转，那么这个旋转的四元数形式为：
q = [cos(θ/2),nxsin(θ/2), nysin(θ/2), nzsin(θ/2)]T
（令θ=θ+2π可以得到相同的旋转，但此时四元数为-q ，故任意的旋转都可以由两个互为相反数的四元数表示 ）
则可以计算出对应旋转轴与夹角：
θ = 2arccos q0
[nx, ny, nz]T = [q1, q2, q3]T/sin(θ/2)
（2）四元数的运算
设q a = [sa, v a] = sa + xai + yaj + zak，q b = [sb, v b] = sb + xbi + ybj + zbk，则有：
- 加减法
q a ± q b = [sa±sb, v a±v b]
- 乘法
q aq b = [sasb - v aTv b, sav b + sbv a + v a x v b]
- 共轭
q a* = [sa, -v a] = sa - xai - yaj - zak
q q = q q = [s2 + v Tv** , 0]
即，实部为模长的平方。
- 模长
||q a|| = (sa2+xa2+ya2+za2)1/2
||q aq b|| = ||q a|| ||q b||
- 逆
q -1 = q * / ||q ||2
qq -1 = q -1q = 1
(q aq b)-1 = q b-1q a-1
- 数乘与点乘
kq = [ks, kv]
q a · q b = sasb + xaxbi + yaybj + zazbk
（3）用四元数表示旋转
假设p = [x,y,z]T ∈ R3，以及旋转n ，θ，旋转后得到p ’。
使用矩阵描述为p ’ = Rp ，而用四元数描述如下：
p = [0,x,y,z] = [0, v]
q = [cos(θ/2), n sin(θ/2)]
则，p ’ = qpq -1
计算结果为纯虚四元数，虚部的三个分量即表示旋转后3D点的坐标。
（4）四元数到旋转矩阵的转换
设四元数q = q0 + q1i + q2j + q3k，对应旋转矩阵R 为：
设矩阵R = {mij}，i，j∈[1,2,3]，对应的四元数q 为：
5、Eigen几何模块
CMakeLists.txt
cmake_minimum_required(VERSION 2.6)
project(usegeometry)
set(CMAKE_BUILD_TYPE "Debug")
include_directories("/usr/include/eigen3")
add_executable(usegeometry main.cpp)
install(TARGETS usegeometry RUNTIME DESTINATION bin)
useGeometry.cpp
#include <iostream>
#include <cmath>
using namespace std;
#include <Eigen/Core>
#include <Eigen/Geometry>
int main(int argc, char **argv) {
//使用Matrix3d或Matrix3f声明3D旋转矩阵
//Identity()表示单位矩阵
Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
//旋转向量使用AngleAxis，它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）
Eigen::AngleAxisd rotation_vector ( M_PI/4, Eigen::Vector3d(0,0,1));//沿Z轴旋转45°
cout .precision(3);
cout << "rotation_matrix =\n" << rotation_vector.matrix() << endl << endl; //用matrix()转换成矩阵
//也可以直赋值
rotation_matrix = rotation_vector.toRotationMatrix();
cout << rotation_matrix << endl << endl;
//向量旋转的两种方法
//（1）用旋转向量
Eigen::Vector3d v (1,0,0);
Eigen::Vector3d v_rotated = rotation_vector * v;
cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl << endl;
//（2）或者用旋转矩阵
v_rotated = rotation_matrix * v;
cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl << endl;
//欧拉角：可以将旋转矩阵直接转换为欧拉角
Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles(2,1,0);//ZYX顺序
cout << "yaw pitch roll = " << euler_angles.transpose() << endl << endl;
//欧式变换矩阵使用Eigen::Isometry
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();//虽然称为3d，实际上是4x4的矩阵
T.rotate(rotation_vector);//按照rotation_vector进行旋转
T.pretranslate(Eigen::Vector3d(1,3,4));//平移向量为（1,3,4）
cout << "Transform matrix = \n" << T.matrix() << endl << endl;
//用变换矩阵进行坐标变换
Eigen::Vector3d v_transformed = T * v; //相当于R*v*t
cout << "v tranformed = " << v_transformed.transpose() << endl << endl;
//四元数
//可以直接把AngleAxis赋值给四元数，反之亦然
Eigen::Quaterniond q = Eigen::Quaterniond(rotation_vector);
cout << "quaternion = \n" << q.coeffs() << endl << endl;//注意coeffs的顺序为（x,y,z,w) w为实部，前三者为虚部
//也可以把旋转矩阵赋值给它
q = Eigen::Quaterniond(rotation_matrix);
cout << "quaternion = \n" << q.coeffs() << endl << endl;
//使用四元数旋转一个向量，使用重载的乘法即可
v_rotated = q*v;//数学上是qvq^-1
cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl << endl;
//四元数赋值给旋转矩阵
rotation_matrix = q.matrix();
cout <<"rotation_matrix from quaternion = \n" << rotation_matrix << endl << endl;
return 0;
}