SLAM总结（二）-数学基础之位姿表示

1.位姿的不同表达方式：位姿的表达方式是表达2个坐标系之间相对关系的数学方法，由旋转和位移组成。SLAM中一般有每帧传感器帧坐标系在世界坐标（世界坐标系的重要作用是提供一个统一的参考，坐标系的原点及姿态如何并不是特别重要，根据实际选定即可，如SLAM中原点一般以初始化成功的地方，可能在空间的任何地方）下的位姿Twc（待求状态变量）、两传感器帧之间的相对位姿。这里统一使用右手坐标系，表达方式如下：
1)变换矩阵，可以表示旋转和平移
旋转（姿态）R用3x3的正交矩阵(旋转矩阵)表示，具有3个自由度。位移t用3x1的列向量表示。为了简化计算通常将两者组合成（欧式）变换矩阵T，最后一列为4x1的齐次坐标，如下式所示。

常见坐标表示T_WC,T_c^w(从左往右，从上到下)都表示从（from）C坐标系到（to）W坐标系的变换，以W作为参考坐标系，a、b、c分别是C的x、y、z轴在W中的向量，t是C的坐标原点在W的位置。对于相似变换S_WC中，sR为w坐标系下的非单位正交向量，t为c坐标系原点在w坐标系的坐标，尺度与w坐标系（参考坐标系）一样。
注：正交矩阵的性质
a)行向量、列向量的模都为1。
b)行向量、列向量两两相互正交，即点积（内积）为0
c)矩阵的转置等于矩阵的逆，即U^TU=U^-1U=I
d)正交矩阵相乘结果仍是正交矩阵，构成群
e)保范性：正交矩阵乘个向量，即(Ux)^T(Ux) = x^TU^TUx = x^Tx
f)正交矩阵的特征值只能是1或-1
2)四元数：只能表示旋转，q=q_Rb表示R绕旋转轴u旋转θ角与b系重合
单位四元数q表示旋转，其形式为q=[q0,q1,q2,q3]^T。q1,q2,q3为虚部，q0为实部。其约束是四元数的模为1。那么q的共轭(虚部取负)和逆相等。
纯需四元数[0,q1,q2,q3]^T可以用于表示点，虚部对应点坐标3个元素
3)旋转向量（轴角，Axis-Angle）：只能表示旋转
旋转向量θn表示旋转，其为3x1列向量，其方向与旋转轴（n，单位向量）一致，而长度等于旋转角（θ用弧度表示）。
4)欧拉角：只能表示旋转
欧拉角表示旋转，可以用3x1列向量[r,p,y]T表示。旋转被分解成先后沿坐标系3个不同轴旋转的转角,注意每次都是绕上一次旋转之后的坐标系的轴旋转，常见的旋转顺序（VINS-Mono使用）是z轴-旋转后y轴-旋转后x轴，偏航-俯仰-横滚（yaw-pitch-roll）。想象一架平飞的飞机，z轴由下往上，x轴从机尾向前，根据右手系可得到y轴，这样正好与偏好、俯仰和横滚的中文含义对上。欧拉角一个重大缺点是万向锁问题，在俯仰是正负90度时，第三次旋转的x轴与第一次旋转的z轴在物理空间上重合，那么第三次少转几度，第一次多转相同度数和原来结果是一样的（用手比划一下就知道了），因此第三次或第一次只需转一个即可，这样由3个自由度变成了2个自由度，成为奇异性问题。3维旋转是3维流形，3个自由度表达旋转时都可能存在奇异性问题。欧拉角往往只用于人机交互，或者IMU的姿态解算。
5)李代数：可以表示旋转和平移
群是一种集合加上一种运算的代数结构。其满足一定条件。李群是指具有连续（光滑）性质的群。旋转矩阵R和矩阵乘法、欧式变换矩阵T和矩阵乘法都属于李群。分别称为特殊正交群SO(3)、特殊欧式群SE(3)。
李代数由一个集合、一个数域和一个二元运算（李括号）组成。三维向量∅或对应反对称矩阵∅^∩、实数域和[∅1,∅2]=(∅1^∩ ∅2^∩-∅2^∩ ∅1^∩)^∪一起构成一个李代数，称为so(3)。6维向量ε=[ρ,∅]^T或其对应反对称矩阵ε^∩、实数域和[ε1,ε2]=(ε1^∩ ε2^∩-ε2^∩ ε1^∩)^∪一起构成一个李代数,称为se(3)。
其中, ρ是三维向量，与平移t线性相关（t=J ρ）；∩表示将向量转换成反对称矩阵，∪则是将矩阵变成向量。叉乘可表示为： c=a×b=a^∩ b表示如下（口诀：对角为0，右上321，2为正，元素转置后取反）

exp（∅^∩）表示旋转，即SO(n)；exp（ε^∩）表示平移和旋转，即SE(n)。旋转角度在正负180度内时，李群和李代数元素是一一对应的。李代数的指数映射是李群，反之，李群的对数映射是李代数。SO(3)实际由旋转矩阵R构成，SE(3)与欧式变换矩阵T对应，so(3)和旋转向量θn对应。
注：李代数是向量，李代数的指数映射才能表示旋转矩阵或欧式变换矩阵
SLAM中常见用法：
1)不同表达式之间的转换，如优化时一般用李代数，位姿表示和计算用欧式变换矩阵，优化前后需在两者之间进行转换
2)变换的传递，如：T₁₃=T₁₂T₂₃；q₁₃=q₁₂q₂₃。注意四元数乘法和向量点乘不同
3)转换点或向量的参考坐标系：p_w=T_WCp_c，v_w=R_WCv_c；v_w=q_wcv_cq_wc^-1（三维向量需表示成纯虚四元数，虚部对应向量3个元素，结果也会是纯虚四元数）；注意向量计算时需要将所有向量转换到同一个坐标系
4)已知当前机体坐标系2绕前一机体坐标系1的某已知向量旋转已知角度，如IMU可以得到绕坐标系1每个坐标轴旋转的角速度，角速度乘时间便得到角度，因为转动角度很小，可以分解为欧拉角计算；编码器可以得到绕坐标系1垂直于地面的轴旋转已知角度，可以用任何一种方式方便的得到R₁₂。

2.位姿的不同表达式之间的转换，常见的给定条件是绕某已知轴旋转已知角度，已知轴一般指参考坐标系1中的轴n，角度是指从坐标系1转到坐标系2的角度θ（视线方向与轴方向相反，逆时针为正），得到结果是R₁₂，q₁₂。
1)R-q
a)R->q：R对应的四元数q并不是唯一的，下面列举其中一种。当q0接近于0时，q₁、q₂、q₃的分母很小，可以考虑其他表示形式
q₀=sqrt[tr(R.)+1]/2, q₁=(c₂-b₃)/(4q0), q2=(a₃-c₁)/(4q0), q3=(b₁-a₂)/(4q₀)
sqrt—根号；tr-矩阵的迹，对角线上元素相加
b)q->R

2)R-θn
a)R->θn
θ=arccos([tr(R.)-1]/2)
但是，这种方法在数值上并不太准确，而且在θ=pi时找不到轴线，故用arctan较好，见《多视图几何》P584
n是R特征值1对应的特征向量Rn=1n->(R-I)n=0，因n不为0向量，等价于齐次方程有非0解，故r(R-I)<.3,通过高斯消元法可以求得n
b)θn->R
R=cosθI+(1-cosθ)nnT+sinθn^∩
特例：绕坐标系1坐标轴旋转θ得到坐标系2，可得旋转矩阵R12如下（口诀：绕哪个坐标系哪个坐标系为参考坐标系，绕哪个轴哪个轴不变，正对角为cos，斜对角为正负sin，绕y上为正）:

3）R-欧拉角
a)R->欧拉角：待补充
b)欧拉角->R：通过2)-b)特例可以得到每次旋转后的相对位姿，则有：R=R₁₂R₂₃R₃₄

4)θn-李代数so(3)：为了6）将该条提前
a)θn->李代数so(3)
∅=θn
b)李代数so(3)->θn
θ=|∅|，|∅|表示∅的模长
n=∅/θ

5)R/T-李代数so(3)/ se(3)
李代数so(3)/ se(3)中的3维向量∅可以按4)-b)转换成轴角θn
a)R->李代数so(3)
先用2)-a)将R转换成轴角θn，得∅=θn
b)李代数so(3)->R
R=exp(∅^∩),根据exp(A)、sin(x)、cos(x)泰勒展开公式可得2）-b)一样的结果：R=cosθI+(1-cosθ)nn^T+sinθn^∩

c)T->李代数se(3)
先求3维旋转部分∅，用a)将T得R转化成∅即可
再由平移t得到ρ：ρ=J^-1) t
J^-1=0.5θ cot⁡(0.5θ)I+[1-0.5θ cot⁡(0.5θ) ]nn^T-0.5θn^∩
d)李代数se(3)->T
T=exp(ε^∩)
R可以使用b）公式由ε中∅得到
t=J ρ
J=(sinθ/θ)I+(1-sinθ/θ)nn^T+[(1-cosθ)/θ]n^∩

6)q-θn
a)q->θn
θ=2arccosq0
[n_x,n_y,n_z]^T=[q₁,q₂,q₃]^T/sin(θ/2)
b)θn->q
q=[cos(θ/2), sin(θ/2)n_x, sin(θ/2)n_y, sin(θ/2)n_z]^T

7)q-欧拉角
a)q->欧拉角：待补充
b)欧拉角->θn：通过5)-b)可以得到每次旋转后的相对位姿，则有：q=q₁₂q₂₃q₃₄

8)q-李代数so(3)
a)q->李代数so(3)
先按4）-a）将q变换成θn，则∅=θn
b)李代数so(3)->q
先按4）-a）将∅变换成θn，再按6）-b）求q

9)θn-欧拉角
a)θn ->欧拉角：待补充
b)欧拉角->θn：通过3)-b) 或6)-b)可以得到每次旋转R或q，再将R或q转成θn

10)欧拉角-李代数so(3)
a)欧拉角->李代数so(3)
先按9）-b）将欧拉角变换成θn，则∅=θn
b)李代数so(3)->欧拉角：待补充