slam学习笔记
slam学习笔记之三维空间刚体运动
最近买了高教授的十四讲，打算过一遍，前两章内容，主要是环境的配置，我觉得有问题的是：vscode 远程debug C++ 代码
概念：
三维空间的刚体运动描述方式：旋转矩阵，变换矩阵，四元数，欧拉角
旋转矩阵（Rotation matrix）是在乘以一个向量的时候有改变向量的方向但不改变大小的效果并保持了手性的矩阵
变换矩阵是数学线性代数中的一个概念。在线性代数中，线性变换能够用矩阵表示。如果T是一个把Rn映射到Rm的线性变换，且x是一个具有n个元素的列向量 ，那么我们把m×n的矩阵A，称为T的变换矩阵
四元数是简单的超复数。 复数是由实数加上虚数单位 i 组成，其中i²= -1。 相似地，四元数都是由实数加上三个虚数单位 i、j和k 组成，而且它们有如下的关系： i² = j² = k² = -1， iº = jº = kº = 1 , 每个四元数都是 1、i、j 和 k 的线性组合，即是四元数一般可表示为a + bi+ cj + dk，其中a、b、c 、d是实数
欧拉角用来确定定点转动刚体位置的3个一组独立角参量，由章动角θ、旋进角（即进动角）ψ和自转角φ组成，为欧拉首先提出而得名
常用公式：
内积 ： a ∙ b = ∑ i = 1 3 a i ∗ b i = ∣ a ∣ ∣ b ∣ cos ⟨ a , b ⟩ a \bullet b = \sum_{i=1}^{3} a_{i} * b_{i}= |a||b|\cos \langle a , b \rangle a∙b=i=1∑3ai∗bi=∣a∣∣b∣cos⟨a,b⟩
外积：
a × b = ∣ i j k a 1 a 2 a 3 b 1 b 2 b 3 ∣ = ∣ a 2 b 3 − a 3 b 2 a 3 b 1 − a 1 b 3 a 3 b 2 − a 2 b 1 ∣ = ∣ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ∣ b ≜ a t b a \times b = \left |\begin{array}{cccc} i &j & k \\ a_1 &a_2 & a_3 \\ b_1 & b_2 &b_3 \\ \end{array}\right|=\left |\begin{array}{cccc} a_2b_3-a_3b_2\\ a_3b_1-a_1b_3 \\ a_3b_2-a_2b_1 \\ \end{array}\right|=\left |\begin{array}{cccc} 0 & -a_3 & a_2 \\ a_3 &0 & -a_1 \\ -a_2 & a_1 &0 \\ \end{array}\right|b\triangleq a^tb a×b=∣∣∣∣∣∣ia1b1ja2b2ka3b3∣∣∣∣∣∣=∣∣∣∣∣∣a2b3−a3b2a3b1−a1b3a3b2−a2b1∣∣∣∣∣∣=∣∣∣∣∣∣0a3−a2−a30a1a2−a10∣∣∣∣∣∣b≜atb
a × b a \times b a×b： 用右手法则，向量 a 指向 b 大拇指指向的方向 ，几何：向量 a 指向 b 组成平行四方形的面积
注：行列式本身表示的就是体积, 上面 a^t 是行列式 ∣ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ∣ \left |\begin{array}{cccc} 0 & -a_3 & a_2 \\ a_3 &0 & -a_1 \\ -a_2 & a_1 &0 \\ \end{array}\right| ∣∣∣∣∣∣0a3−a2−a30a1a2−a10∣∣∣∣∣∣ 的简写，是一个反对称矩阵（ a T a^T aT = − a -a −a）
坐标系直接怎么变化？
slam 中有多个坐标系，因为存在一个固定不变的世界坐标系，存在机器人运动过程中自己感知到的坐标系，那具体的变化就是 T = 旋 转 + 平 移 T = 旋转 + 平移 T=旋转+平移
字丑，忍忍，，，
平移（平移向量）比较好理解
旋转呢？如何理解？
设坐标系 ( e 1 , e 2 , e 3 ) (e_1, e_2,e_3) (e1,e2,e3) , 发生一次旋转，变为 ( e 1 ˊ , e 2 ˊ , e 3 ˊ ) (\acute{e_1},\acute{e_2},\acute{e_3} ) (e1ˊ,e2ˊ,e3ˊ), 对于一组固定的向量 a a a (向量不随坐标系旋转)，它的坐标系如何变化？
坐标关系：
[ e 1 , e 2 , e 3 ] [ a 1 a 2 a 3 ] = [ e 1 ˊ , e 2 ˊ , e 3 ˊ ] [ a 1 ˊ a 2 ˊ a 3 ˊ ] \begin{aligned} \begin{bmatrix} e_{1} , e_{2}, e_{3} \end{bmatrix} \end{aligned}\begin{aligned} \begin{bmatrix} a_{1} \\ a_{2}\\ a_{3} \end{bmatrix} \end{aligned}=\begin{aligned} \begin{bmatrix} \acute{e_1} , \acute{e_2}, \acute{e_3} \end{bmatrix} \end{aligned}\begin{aligned} \begin{bmatrix} \acute{a_1} \\ \acute{a_2}\\ \acute{a_3} \end{bmatrix} \end{aligned} [e1,e2,e3]⎣⎡a1a2a3⎦⎤=[e1ˊ,e2ˊ,e3ˊ]⎣⎡a1ˊa2ˊa3ˊ⎦⎤
这个等式相等，证明，两边同事乘以 [ e 1 T e 2 T e 3 T ] \begin{bmatrix} e_1^T \\ e_2^T\\ e_3^T \end{bmatrix} ⎣⎡e1Te2Te3T⎦⎤
[ a 1 a 2 a 3 ] = [ e 1 T e 1 ˊ e 1 T e 2 ˊ e 1 T e 3 ˊ e 2 T e 1 ˊ e 2 T e 2 ˊ e 2 T e 3 ˊ e 3 T e 1 ˊ e 3 T e 2 ˊ e 3 T e 3 ˊ ] [ a 1 ˊ a 2 ˊ a 3 ˊ ] ≜ R a ˊ \begin{aligned} \begin{bmatrix} a_{1} \\ a_{2}\\ a_{3} \end{bmatrix} \end{aligned}=\begin{aligned} \begin{bmatrix} e_1^T\acute{e_1} & e_1^T\acute{e_2} & e_1^T\acute{e_3}\\ e_2^T\acute{e_1}&e_2^T\acute{e_2}&e_2^T\acute{e_3}\\ e_3^T\acute{e_1}&e_3^T\acute{e_2}&e_3^T\acute{e_3} \end{bmatrix} \end{aligned}\begin{aligned} \begin{bmatrix} \acute{a_1} \\ \acute{a_2}\\ \acute{a_3} \end{bmatrix} \end{aligned}\triangleq R \acute{a} ⎣⎡a1a2a3⎦⎤=⎣⎡e1Te1ˊe2Te1ˊe3Te1ˊe1Te2ˊe2Te2ˊe3Te2ˊe1Te3ˊe2Te3ˊe3Te3ˊ⎦⎤⎣⎡a1ˊa2ˊa3ˊ⎦⎤≜Raˊ
注： R R R 为旋转矩阵： R R R 也是正交矩阵（ R − 1 = R T R^{-1}=R^T R−1=RT）,且行列式为1
特殊正交群：Special Orthogonal Group 满足： S O ( n ) = { R ∈ R n × n ∣ R R T = I , d e t ( R ) = 1 } SO(n)=\left\{R \in R^{n \times n} | RR^T = I , det(R) = 1 \right\} SO(n)={R∈Rn×n∣RRT=I,det(R)=1}
因此：旋转矩阵描述了两个坐标系的关系
如： a 1 = R 12 a 2 a_1 = R_{12}a_2 a1=R12a2 反之， a 2 = R 21 a 1 a_2 = R_{21}a_1 a2=R21a1
于是： R 21 = R 12 − 1 = R 12 T R_{21} = R_{12}^{-1} = R_{12}^T R21=R12−1=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
加上平移：
a ˊ = R a + t \acute{a} = Ra + t aˊ=Ra+t 这样两个刚体运动可以有 R R R ， t t t 完全描述
轴角旋转矩阵：罗德里格斯公式
R = cos θ I + ( 1 − cos θ ) n n T + sin θ n R = \cos\theta I + (1-\cos\theta)nn^T + \sin\theta n R=cosθI+(1−cosθ)nnT+sinθn
旋转矩阵转轴角
角度： θ = arccos ( ( t r ( R ) − 1 ) / 2 ) \theta=\arccos((tr(R) - 1)/2 ) θ=arccos((tr(R)−1)/2)
轴： R n = n Rn=n Rn=n