SLAM算法总结1

前言

读完高翔的视觉SLAM十四讲的总结前七讲

旋转矩阵，旋转向量，四元数

旋转矩阵 R：任何物体的运动都可以用一个3乘3的矩阵乘法来描述，需要明白旋转矩阵本身是个正交矩阵且行列式为1就好了。

参考：https://www.matongxue.com/madocs/228/
这其中对特征值和特征向量以及矩阵乘法的意义有很直观的表现。

旋转向量：用旋转角度和旋转轴四个变量描述旋转。
欧拉角：以XYZ(roll pitch yaw)为轴旋转以及旋转的先后顺序表示物体旋转。
四元数 q：用四个数字表示一次旋转，且旋转之间可以有运算法则。
欧式变换矩阵 T：一个4乘4的矩阵，同时表达了旋转和平移。构成如下：
$$\left[\begin{array}{c}b\\ 1\end{array}\right]=\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]\left[\begin{array}{c}a\\ 1\end{array}\right]---\hat{b}=T\hat{a}$$
注：a,b都是3v向量，多加1维就是a,b的齐次坐标，t是一个3v向量表达平移，T存在是为了方便连续的旋转变换，比如c=T1* T2*a。

linux中通过Eigen几何模块可以直接完成以上变量关系之间的转换，Eigen本身是一个矩阵库。总之只需要知道旋转矩阵，旋转向量，四元数，欧式变换矩阵以及欧拉角之间都是可以互相转换，以及所代表的意思。

Eigen实现创建（片段）：

//旋转向量初始化
AngleAxisd rotation_vector(M_PI / 4, Vector3d(0, 0, 1));     //沿 Z 轴旋转 45 度
//欧拉角初始化
Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0); // ZYX顺序，即roll pitch yaw顺序
//欧式变换矩阵初始化
  Isometry3d T = Isometry3d::Identity();               // 虽然称为3d，实质上是4＊4的矩阵
  T.rotate(rotation_vector);                                     // 按照rotation_vector进行旋转
  T.pretranslate(Vector3d(1, 3, 4));                     // 把平移向量设成(1,3,4)
//由旋转向量初始化四元数
 Quaterniond q = Quaterniond(rotation_vector);

李群李代数

总的来说李群李代数就是对旋转矩阵和欧式变换矩阵做了一个总结。

李群：就是对特殊矩阵做了总结，将满足某种性质和某种运算规则的所有矩阵归结为一个总体叫做群
以下为旋转矩阵和欧式变换矩阵的李群的表达。

S O ( 3 ) = { R ∈ R 3 × 3 ∣ R R T = I , d e t ( R ) = 1 } SO(3)=\lbrace R\in R^{3 \times 3 }| RR^T = I,det(R)=1 \rbrace SO(3)={ R∈R3×3∣RRT=I,det(R)=1}
S E ( 3 ) = { T = [ R t 0 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 } SE(3)=\lbrace T =\left[ \begin{matrix} R &t \\ 0 &1 \end{matrix} \right] \in R^{4 \times 4 }| R \in SO(3),t\in R^3 \rbrace SE(3)={ T=[R0​t1​]∈R4×4∣R∈SO(3),t∈R3}

李代数：通过李群和李代数的关系，可以用来表述李群的导数。
以下为旋转矩阵和欧式变换矩阵的李代数表达

s o ( 3 ) = { ϕ ∈ R 3 , Φ = ϕ ^ ∈ R 3 × 3 } so(3)=\lbrace \phi \in R^3, \Phi = \hat{\phi} \in R^{3 \times 3} \rbrace so(3)={ ϕ∈R3,Φ=ϕ^​∈R3×3}
s e ( 3 ) = { ξ = [ ρ ϕ ] ∈ R 6 , ρ ∈ R 3 ， ϕ ∈ s o ( 3 ) , ξ ^ = [ ϕ ^ ρ 0 0 ] ∈ R 4 × 4 } se(3)=\lbrace \xi = \left[ \begin{matrix} \rho \\ \phi \end{matrix} \right]\in R^6,\rho\in R^3，\phi\in so(3),\hat{\xi}= \left[ \begin{matrix} \hat{\phi}&\rho \\ 0&0 \end{matrix} \right] \in R^{4 \times 4}\rbrace se(3)={ ξ=[ρϕ​]∈R6,ρ∈R3，ϕ∈so(3),ξ^​=[ϕ