蓝色 紫色 红色
一、前言
??本章简要介绍 刚体在三维世界中 运动描述:旋转矩阵、旋转向量、欧拉角、四元数等。
二、SLAM数学表达问题
用 数学语言 描述:小萝卜携带某种东西 传感器 在未知的环境中 运动
相机：在 某些时刻 采集数据 ? 连续运动 => 离散时刻
对于小萝卜： t = 1 , . . . , K t = 1,...,K t=1,...,K 小萝卜在每一刻的位置： x 1 , . . . , x K x_1,...,x_K x1,...,xK ??=> 萝卜的轨迹
对于地图：设置许多路标 (Landmark），传感器每时每刻都会测量一些路标点，得到它们的观测数据。
N 个路标点： y 1 , . . . , y N y_1,...,y_N y1,...,yN
运动：从 k = 1时刻 到 k时刻，小萝卜的 位置x 是如何变化的。
机器人会携带一个 测量自身运动 的传感器（eg.码盘或惯性传感器），这个传感器可以测量有关运动的读数，但不一定直接是 位置之差，还可能是 加速度、角速度 等信息。抽象成数学模型：(运动方程）
u k u_k uk 是运动传感器的读数（）， w k w_k wk 是噪声。
观测：假设小萝卜在 k时刻，于 x k x_k xk 处探测到了某一个路标 y j y_j yj，如何用数学语言描述。
观测方程： 描述当小萝卜在 x k x_k xk 位置上看到某个路标点 y j y_j yj ，产生了一个观测数据 z k , j z_{k,j} zk,j 。 v k , j v_{k,j} vk,j 是观测里的噪声。
参数化 Parameterization
位姿：位置+姿态
如果小萝卜在 平面 中运动，那么它的位姿由 两个位置 和 一个转角 来描述，即 x k = [ x , y , θ ] k T x_k=[x,y,θ]^T_k xk=[x,y,θ]kT 。同时，运动传感器能够测量到小萝卜在每两个时间 间隔位置和转角的变化量 u k = [ Δ x , Δ y , Δ θ ] k T u_k=[Δx,Δy,Δθ]^T_k uk=[Δx,Δy,Δθ]kT，则运动方程可具体化为：
上述是简单的 线性关系 。但并不是所有的传感器都直接能测量出 位移和角度变化 ，所以会有更复杂的运动方程，需要进行动力学分析。
关于观测方程，比方说小萝卜携带着一个 二维激光传感器。我们知道激光传感器观测一个2D 路标点时，能够测到两个量：路标点与小萝卜本体之间的 距离r 和 夹角ϕ 。我们记 路标点 为 y = [ p x , p y ] T y=[p_x,p_y]^T y=[px,py]T（为保持简洁，省略了下标），观测数据 为 z = [ r , ϕ ] T z=[r,ϕ]^T z=[r,ϕ]T ，那么观测方程就具体化为：
视觉SLAM的问题表述
视觉SLAM：传感器是 相机。
观测方程就是 “对路标点拍摄后，得到了图像中的像素” 的过程。针对不同的传感器，这两个方程有不同的参数化形式。
如果我们保持通用性，把它们取成通用的抽象形式。则SLAM 过程可总结为两个基本方程： 这两个方程描述了最基本的SLAM 问题：当我们知道运动测量的读数u，以及传感器的读数z 时，如何求解定位问题（估计 位置x ）和建图问题（估计 路标y ）？
SLAM问题 建模成 状态估计问题 ［通过带有噪声的测量数据，估计内部的、隐藏着的 状态变量]
三、线性代数相关知识
1. 点和向量、坐标系
刚体：内部任意两点间距离保持不变，不易发生形变的物体。
刚体变换：由旋转和平移组成
存在疑惑：刚体配准和非刚体配准区别？
在西电的一篇硕士论文第五章里看到这样的表述：
“非刚性物体的图像配准比刚体物体的复杂，因为非刚性物体图像配准中的变换必须考虑 非线性形变 ，这种形变可能是全局的也可能是局部的，也可能二者都有。”
三维空间由3个轴组成，所以空间点的位置可由3个坐标 [x,y,z] 指定。 注意区别 坐标 和 向量
向量只有在指定坐标系之后才可以谈论该向量在 此坐标系下的坐标，即找到 若干个实数 对应这个向量。
当指定一个坐标系，即给定一个线性空间的基 ( e 1 , e 2 , e 3 ) (e_1,e_2,e_3) (e1,e2,e3)，向量 在这组基下的坐标：
a , b ∈ R 3 a,b ∈ R^3 a,b∈R3
- 向量内积
描述向量间的投影关系： - 向量外积
可表示向量的旋转：两个向量张成的四边形的有向面积，大小为 |a||b|sin<a,b>，方向垂直于这两个向量构成的平面 把 写成反对称矩阵(Skew-symmetric)，变成
，可以由旋转向量 来描述， 即是 ×，其大小由 和 的夹角大小决定。
2. 坐标系间的欧氏变换
坐标系之间的变换关系：旋转和平移
惯性坐标系 / 世界坐标系 => 固定不动 x W , y W , z W x_W,y_W,z_W xW,yW,zW
移动坐标系 => 相机或机器人 x C , y C , z C x_C,y_C,z_C xC,yC,zC
相机视野中某个向量 p \mathbf p p ，它的坐标为 p c \mathbf p_\mathbf c pc ，而从世界坐标系下看，它的坐标 p w \mathbf p_\mathbf w pw 。这两个坐标之间的转换过程如下：
- 该点在相机坐标系下的 坐标值
- 根据 相机位姿 把坐标转换到世界坐标系中
- 该坐标变换由 矩阵 T 来描述
四、旋转的表达
欧式变换：同一个向量在各个坐标系下的长度和夹角都不会发生变化 平移和旋转
1. 旋转矩阵
设某个单位正交基 ( e 1 , e 2 , e 3 ) (e_1 , e_2 , e_3 ) (e1,e2,e3) 经过一次旋转，变成了 ( e 1 ′ , e 2 ′ , e 3 ′ ) (e_1' , e_2' , e_3') (e1′,e2′,e3′) 。那么，对于同一个向量 （该向量并没有随着坐标系的旋转而发生运动），它在两个坐标系下的坐标为 [ a 1 , a 2 , a 3 ] T [a_1 , a_2 , a_3 ]^T [a1,a2,a3]T 和 [ a 1 ′ , a 2 ′ , a 3 ′ ] T [a_1' , a_2' , a_3' ]^T [a1′,a2′,a3′]T 。
为了描述两个坐标之间的关系，上式左右同时左乘 [ e 1 T e 2 T e 3 T ] \begin{bmatrix} e_1^T\\ e_2^T\\ e_3^T \end{bmatrix} ⎣⎡e1Te2Te3T⎦⎤ :
旋转矩阵 R：描述旋转本身 由两组基之间的 内积 组成，刻画了旋转前后同一个向量的坐标变换关系。只要旋转是一样的，那么这个矩阵也是一样的。
性质：旋转矩阵 ⇔ 行列式为1的正交矩阵
正交矩阵： A T = A − 1 A^T=A^{-1} AT=A−1
定义：设A是一个n 阶方阵，如果有 A A T = I AA^T=I AAT=I ，则A是正交矩阵。
特殊正交群（Special Orthogonal Group） S O ( n ) SO(n) SO(n)
旋转矩阵的集合定义：
S O ( 3 ) SO(3) SO(3) 表示三维空间的旋转。
相反的旋转
由于 旋转矩阵R 是正交矩阵，它的 逆 R − 1 R^{-1} R−1 / 转置 R T R^T RT 描述了一个相反的旋转。
完整的欧式空间坐标变换
考虑世界坐标系中的向量 ，经过一次旋转（用 描述）和一次平移 后，得到了 ，那么把旋转和平移合到一起，有： 但是这样的变换关系不是一个 线性关系 ，多次变换之后会过于复杂。
引入 齐次坐标和变换矩阵T 重写式 (3.8)：
两次变换的累加 变为：
特殊欧氏群（Special Euclidean Group） S E ( n ) SE(n) SE(n)
与 S O ( 3 ) SO(3) SO(3) 一样，求解 该矩阵的逆 表示一个反向的变换：
2. 旋转向量
变换矩阵 T T T D o F ( T ) = 6 DoF(T)=6 DoF(T)=6 (1+1)*3