[SLAM] 数学基础

总是碰到很多数学问题，有些东西碰到都得查一遍，干脆整理到一块凑成一篇文章
以下内容主要是一个手册的形式，碰到不会的内容可以进行一个速查，具体推导部分一般省略，关注细节的朋友可以再去进行针对性搜索

1. 点乘叉乘

①点乘（内积）：$a\cdot b=|a||b|cos\theta =a^{T}b={\sum }_{i=0}^N{a}_i\ast b_i$
表示一个向量到另一个向量的投影乘上被投影向量的模，是个数值；
其±由$cos\theta$决定，表示了两个向量是否同向；
②叉乘（外积）：
$a\times b=\left[\begin{array}{ccc}i& j& k\\ a_1& a_2& a_3\\ b_1& b_2& b_3\end{array}\right]=\left[\begin{array}{c}{a}_2{b}_3-a_3{b}_2\\ a_3{b}_1-a_1{b}_3\\ a_1{b}_2-a_2{b}_1\end{array}\right]=\left[\begin{array}{ccc}0& a_3& -a_2\\ -a_3& 0& a_1\\ a_2& -a_1& b_3\end{array}\right]b=a\stackrel{ˆ}{}b$
叉积只在3维下有效，其结果为一个向量，方向符合a到b的右手定则表示，叉积的这种表示方向的性质还常常被用于图形学中（如计算点是否在图形内）；
此处的$a\stackrel{ˆ}{}$表示a构成的反对称矩阵，这在SLAM中非常常用，是叉乘的另一种表现形式；

2. 坐标系的欧式变换

在SLAM中，常说在那个点在哪个坐标系下的表示，又常坐标系之间的位姿变换；
对于一个固定点p，其在W和C两个坐标系下的意义应该是相同的，有等式：
$$\left[\begin{array}{ccc}{e}_1& e_2& e_3\end{array}\right]\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]=\left[\begin{array}{ccc}{e}_1^{\prime }& e_2^{\prime }& e_3^{\prime }\end{array}\right]\left[\begin{array}{c}{a}_1^{\prime }\\ a_2^{\prime }\\ a_3^{\prime }\end{array}\right]$$这里的$\left[\begin{array}{ccc}{e}_1& e_2& e_3\end{array}\right]$和$\left[\begin{array}{ccc}{e}_1^{\prime }& e_2^{\prime }& e_3^{\prime }\end{array}\right]$表示两个坐标系下的基，而$\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]$和$\left[\begin{array}{c}{a}_1^{\prime }\\ a_2^{\prime }\\ a_3^{\prime }\end{array}\right]$则表示p点在这两组基下的坐标；
此处两边同时乘上W坐标系基的逆有：
$$\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]=\left[\begin{array}{ccc}{e}_1^T{e}_1^{\prime }& e_1^T{e}_2^{\prime }& e_1^T{e}_3^{\prime }\\ e_2^T{e}_1^{\prime }& e_2^T{e}_2^{\prime }& e_2^T{e}_3^{\prime }\\ e_3^T{e}_1^{\prime }& e_3^T{e}_2^{\prime }& e_3^T{e}_3^{\prime }\end{array}\right]\left[\begin{array}{c}{a}_1^{\prime }\\ a_2^{\prime }\\ a_3^{\prime }\end{array}\right]=R{a}^{\prime }$$$$\left[\begin{array}{ccc}{e}_1^{\prime }& e_2^{\prime }& e_3^{\prime }\end{array}\right]=\left[\begin{array}{ccc}{e}_1& e_2& e_3\end{array}\right]\left[\begin{array}{ccc}{e}_1^T{e}_1^{\prime }& e_1^T{e}_2^{\prime }& e_1^T{e}_3^{\prime }\\ e_2^T{e}_1^{\prime }& e_2^T{e}_2^{\prime }& e_2^T{e}_3^{\prime }\\ e_3^T{e}_1^{\prime }& e_3^T{e}_2^{\prime }& e_3^T{e}_3^{\prime }\end{array}\right]$$这里的R就是常说的旋转矩阵，重点来了！R具有的两重意思：
①表示坐标$a^{\prime }$到$a$的变换，也就是P点在C坐标系转换到W坐标系，在代码中这个矩阵往往以$R_{WC}$这个名称出现，可以理解为C系下坐标到W系坐标下变换的旋转矩阵；
②表示基$e$到$e^{\prime }$的变换，和坐标刚好相反，这其实是初学矩阵论是就碰到的一个有趣的点，可以简单的理解为C上坐标到W上坐标的变换和C坐标到W坐标的变换刚好是反向的；
定义n维旋转矩阵R组成的特殊正交群SO(n)如下：
S O ( n ) = { R ∈ R n × n ∣ R R T = I , d e t ( R ) = 1 } SO(n) = \{R\in\mathbb{R}^{n\times n}| RR^T=I, det(R)=1\} SO(n)={R∈Rn×n∣RRT=I,det(R)=1}将3维下旋转矩阵和平移组合之后的变换矩阵T组成的特殊欧式群SE(3)定义如下：
S E ( 3 ) = { T = [ R t 0 T 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 } SE(3) = \{T=[Rt0T1]\in\mathbb{R}^{4\times 4}| R\in SO(3), t\in \mathbb{R}^3\} SE(3)={T=[R0T​t1​]∈R4×4∣R∈SO(3),t∈R3}$$T^{-1}=\left[\begin{array}{cc}{R}^T& -R^{T}t\\ 0^T& 1\end{array}\right]$$

3. 各种坐标系

①ECEF坐标系
地心地固坐标系（Earth-Centered,Earth-Fixed），是一种以地心为原点的笛卡尔坐标系，z 轴与地轴平行指向 北极点，x 轴指向本初子午线与赤道的交点，y 轴垂直于xoz平面构成右手坐标系。ecef坐标系的坐标值一般很大，因此是不太方便进行空间计算的
②ENU坐标系
站心地平直角坐标系（East North Up），是定义在地表正切平面的局部坐标系，以一个站心点为坐标原点，当把坐标系定义为X轴指东、Y轴指北，Z轴指天，就是ENU（东北天）站心坐标系，ENU坐标系与ECEF坐标系的转换关系如下图所示

ENU转换到ECEF坐标的变换矩阵，由先平移再旋转两部分组成
$$M=T\cdot R=\left[\begin{array}{cccc}1& 0& 0& X_p\\ 0& 1& 1& T_p\\ 0& 0& 1& Z_p\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}-sinL& -sinBcosL& cosBcosL& 0\\ cosL& -sinBsinL& cosBsinL& 0\\ 0& cosB& sinB& 0\\ 0& 0& 0& 1\end{array}\right]$$上式中${\left[\begin{array}{ccc}{X}_p& T_p& Z_p\end{array}\right]}^T$为站心P坐标，L和B为P所在经纬度，具体推导过程不做详细解释；

4. 各个旋转表示之间的变换

①角轴到旋转矩阵，罗德里格斯公式：
$$R=cos\theta I+(1-cos\theta )n{n}^T+sin\theta n\stackrel{ˆ}{}$$②旋转矩阵到角轴：
$$\begin{array}{rl}& tr(R)=cos\theta tr(I)+(1-cos\theta )tr(n{n}^T)+sin\theta tr(n\stackrel{ˆ}{})\\ & =3cos\theta +(1-cos\theta )\\ & =1+2\ast cos\theta \end{array}$$那么角轴的的角度可以通过旋转矩阵的迹，方向可以通过旋转矩阵特征值为1的特征向量求取（因为轴绕着轴旋转==不旋转）
$$\begin{gathered} \theta =arccos\frac{tr(R)-1}{2} \\ Rn=n \end{gathered}$$③角轴转四元数
绕单位向量$[n_x,n_y,n_z]$旋转角度$\theta$：
$$q=[cos\frac{\theta }{2},n_{x}sin\frac{\theta }{2},n_{y}sin\frac{\theta }{2},n_z\frac{\theta }{2}]$$④四元数到角轴
$$\theta =2\ast arccos(q_0)$$$$[n_x,n_y,n_z]=[q_1,q_2,q_3]/sin\frac{\theta }{2}$$⑤旋转矩阵到四元数
旋转矩阵$R=m_{ij},i\in [1,3],j\in [1,3]$
$$q_0=\frac{\sqrt{tr(R)+1}}{2},q1=\frac{m_{23}-m_{32}}{4q_0},q2=\frac{m_{31}-m_{13}}{4q_0},q3=\frac{m_{12}-m_{21}}{4q_0}$$⑥四元数到旋转矩阵
四元数$q=q_0+q_{1}i+q_{2}j+q_{3}k=[s,v{]}^T$
$$\begin{gathered} R=v{v}^T+s^{2}I+2sv\stackrel{ˆ}{}+(v\stackrel{ˆ}{}{)}^2 \\ =\left[\begin{array}{ccc}1-2q_2^2-2q_3^2& 2q_1{q}_2+2q_0{q}_3& 2q_1{q}_2-2q_0{q}_2\\ 2q_1{q}_2-2q_0{q}_3& 1-2q_1^2-2q_3^2& 2q_2{q}_3+2q_0{q}_1\\ 2q_1{q}_2+2q_0{q}_2& 2q_2{q}_3-2q_0{q}_1& 1-2q_1^2-2q_2^2\end{array}\right] \end{gathered}$$

5. 四元数

四元数运算转换成矩阵运算
定义两个四元数下的符号+和$\oplus$有：
$$q^{+}=\left[\begin{array}{cc}s& -v^T\\ v& sI+v\stackrel{ˆ}{}\end{array}\right],q^{\oplus }=\left[\begin{array}{cc}s& -v^T\\ v& sI-v\stackrel{ˆ}{}\end{array}\right]$$对于这两个运算符有
$$q_1{q}_2=q_1^{+}{q}_2=q_2^{\oplus }{q}_1$$

6.误差指标

①RPE： relative pose error 相对位姿误差
计算时间间隔为$\Delta t$的两帧之间的位姿变化的误差（即这两帧之间的计算值和真值的误差），对于时刻t有相对位姿误差 RPE为：
$$E_i:=(Q_i^{-1}{Q}_{i+\Delta t}{)}^{-1}(P_i^{-1}{P}_{i+\Delta t})$$上式中左边的部分表示估计的位姿变化值，右边的部分表示位姿变换真值；
对于一个n帧的轨迹，取时间间隔$\Delta$，就有$m=n-\Delta$个RPE值，用RMSE表示整个轨迹的RPE为：
$$RMSE(E_{1:n},\Delta ):=(\frac{1}{m}\sum _{i=1}^m||trans(E_i)|{|}^2{)}^{\frac{1}{2}}$$上式中trans表示旋转矩阵中平移部分；
②ATE： absolute trajectory error 绝对轨迹误差
估计位姿和真实位姿的直接差值，可以非常直观地反应算法精度和轨迹全局一致性，对于时刻t的ATE为：
$$F_i^{-1}:=Q_i^{-1}S{P}_i$$上式中$S\in Sim(3)$表示单目下估计的位姿到真实位姿的相似变换矩阵;
用RMSE表示整个轨迹的ATE为：
$$RMSE(E_{1:n},\Delta ):=(\frac{1}{n}\sum _{i=1}^n||trans(F_i)|{|}^2{)}^{\frac{1}{2}}$$