[SLAM] 数学基础
总是碰到很多数学问题，有些东西碰到都得查一遍，干脆整理到一块凑成一篇文章
以下内容主要是一个手册的形式，碰到不会的内容可以进行一个速查，具体推导部分一般省略，关注细节的朋友可以再去进行针对性搜索
1. 点乘叉乘
①点乘（内积）： a ⋅ b = ∣ a ∣ ∣ b ∣ c o s θ = a T b = ∑ i = 0 N a i ∗ b i a\cdot b = |a||b|cos\theta=a^Tb=\sum_{i=0}^Na_i*b_i a⋅b=∣a∣∣b∣cosθ=aTb=∑i=0Nai∗bi
表示一个向量到另一个向量的投影乘上被投影向量的模，是个数值；
其±由 c o s θ cos\theta cosθ决定，表示了两个向量是否同向；
②叉乘（外积）：
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 1 b 2 − a 2 b 1 ] = [ 0 a 3 − a 2 − a 3 0 a 1 a 2 − a 1 b 3 ] b = a ˆ b a\times b=\begin{bmatrix} i & j & k \\ a_1 & a_2 & a_3 \\ b_1 & b_2 & b_3 \end{bmatrix}=\begin{bmatrix} a_2b_3-a_3b_2 \\ a_3b_1-a_1b_3 \\ a_1b_2-a_2b_1 \end{bmatrix} = \begin{bmatrix} 0 & a_3 & -a_2 \\ -a_3 & 0 & a_1 \\ a_2 & -a_1 & b_3 \end{bmatrix}b=a\^{\ }b a×b=⎣ ⎡ia1b1ja2b2ka3b3⎦ ⎤=⎣ ⎡a2b3−a3b2a3b1−a1b3a1b2−a2b1⎦ ⎤=⎣ ⎡0−a3a2a30−a1−a2a1b3⎦ ⎤b=a ˆb
叉积只在3维下有效，其结果为一个向量，方向符合a到b的右手定则表示，叉积的这种表示方向的性质还常常被用于图形学中（如计算点是否在图形内）；
此处的 a ˆ a\^{\ } a ˆ表示a构成的反对称矩阵，这在SLAM中非常常用，是叉乘的另一种表现形式；
![[SLAM] 数学基础_第1张图片](http://img.e-com-net.com/image/info8/06b6a242bd344bc2a9d476b9628106bb.jpg)
2. 坐标系的欧式变换
![[SLAM] 数学基础_第2张图片](http://img.e-com-net.com/image/info8/cb07f80ea03b48c8a0aaaa9dfb9da4fd.jpg)
在SLAM中，常说在那个点在哪个坐标系下的表示，又常坐标系之间的位姿变换；
对于一个固定点p，其在W和C两个坐标系下的意义应该是相同的，有等式：
[ 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{bmatrix} e_1 & e_2 & e_3\end{bmatrix}\begin{bmatrix} a_1 \\ a_2 \\ a_3\end{bmatrix} = \begin{bmatrix} e_1' & e_2' & e_3'\end{bmatrix}\begin{bmatrix} a_1' \\ a_2' \\ a_3'\end{bmatrix} [e1e2e3]⎣ ⎡a1a2a3⎦ ⎤=[e1′e2′e3′]⎣ ⎡a1′a2′a3′⎦ ⎤这里的 [ e 1 e 2 e 3 ] \begin{bmatrix} e_1 & e_2 & e_3\end{bmatrix} [e1e2e3]和 [ e 1 ′ e 2 ′ e 3 ′ ] \begin{bmatrix} e_1' & e_2' & e_3'\end{bmatrix} [e1′e2′e3′]表示两个坐标系下的基，而 [ a 1 a 2 a 3 ] \begin{bmatrix} a_1 \\ a_2 \\ a_3\end{bmatrix} ⎣ ⎡a1a2a3⎦ ⎤和 [ a 1 ′ a 2 ′ a 3 ′ ] \begin{bmatrix} a_1' \\ a_2' \\ a_3'\end{bmatrix} ⎣ ⎡a1′a2′a3′⎦ ⎤则表示p点在这两组基下的坐标；
此处两边同时乘上W坐标系基的逆有：
[ 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{bmatrix} a_1 \\ a_2 \\ a_3\end{bmatrix} = \begin{bmatrix} e_1^Te_1' & e_1^Te_2' & e_1^Te_3' \\ e_2^Te_1' & e_2^Te_2' & e_2^Te_3' \\ e_3^Te_1' & e_3^Te_2' & e_3^Te_3'\end{bmatrix}\begin{bmatrix} a_1' \\ a_2' \\ a_3'\end{bmatrix}=Ra' \\ ⎣ ⎡a1a2a3⎦ ⎤=⎣ ⎡e1Te1′e2Te1′e3Te1′e1Te2′e2Te2′e3Te2′e1Te3′e2Te3′e3Te3′⎦ ⎤⎣ ⎡a1′a2′a3′⎦ ⎤=Ra′ [ e 1 ′ e 2 ′ e 3 ′ ] = [ e 1 e 2 e 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 ′ ] \begin{bmatrix} e_1' & e_2' & e_3'\end{bmatrix} = \begin{bmatrix} e_1 & e_2 & e_3\end{bmatrix}\begin{bmatrix} e_1^Te_1' & e_1^Te_2' & e_1^Te_3' \\ e_2^Te_1' & e_2^Te_2' & e_2^Te_3' \\ e_3^Te_1' & e_3^Te_2' & e_3^Te_3'\end{bmatrix} [e1′e2′e3′]=[e1e2e3]⎣ ⎡e1Te1′e2Te1′e3Te1′e1Te2′e2Te2′e3Te2′e1Te3′e2Te3′e3Te3′⎦ ⎤这里的R就是常说的旋转矩阵，重点来了！R具有的两重意思：
①表示坐标 a ′ a' a′到 a a a的变换，也就是P点在C坐标系转换到W坐标系，在代码中这个矩阵往往以 R W C R_{WC} RWC这个名称出现，可以理解为C系下坐标到W系坐标下变换的旋转矩阵；
②表示基 e e e到 e ′ e' e′的变换，和坐标刚好相反，这其实是初学矩阵论是就碰到的一个有趣的点，可以简单的理解为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=\begin{bmatrix} R & t \\ 0^T & 1 \end{bmatrix}\in\mathbb{R}^{4\times 4}| R\in SO(3), t\in \mathbb{R}^3\} SE(3)={T=[R0Tt1]∈R4×4∣R∈SO(3),t∈R3} T − 1 = [ R T − R T t 0 T 1 ] T^{-1} = \begin{bmatrix} R^T & -R^Tt \\ 0^T & 1 \end{bmatrix} T−1=[RT0T−RTt1]
3. 各种坐标系
①ECEF坐标系
地心地固坐标系（Earth-Centered,Earth-Fixed），是一种以地心为原点的笛卡尔坐标系，z 轴与地轴平行指向 北极点，x 轴指向本初子午线与赤道的交点，y 轴垂直于xoz平面构成右手坐标系。ecef坐标系的坐标值一般很大，因此是不太方便进行空间计算的
②ENU坐标系
站心地平直角坐标系（East North Up），是定义在地表正切平面的局部坐标系，以一个站心点为坐标原点，当把坐标系定义为X轴指东、Y轴指北，Z轴指天，就是ENU（东北天）站心坐标系，ENU坐标系与ECEF坐标系的转换关系如下图所示
![[SLAM] 数学基础_第3张图片](http://img.e-com-net.com/image/info8/c2f2a415c38a42a0a7c5bb6b330707c0.jpg)
ENU转换到ECEF坐标的变换矩阵，由先平移再旋转两部分组成
M = T ⋅ R = [ 1 0 0 X p 0 1 1 T p 0 0 1 Z p 0 0 0 1 ] [ − s i n L − s i n B c o s L c o s B c o s L 0 c o s L − s i n B s i n L c o s B s i n L 0 0 c o s B s i n B 0 0 0 0 1 ] M = T\cdot R=\begin{bmatrix} 1 & 0 & 0 & X_p \\ 0 & 1 & 1 & T_p \\ 0 & 0 & 1 & Z_p \\ 0 & 0 & 0 & 1\end{bmatrix}\begin{bmatrix} -sinL & -sinBcosL & cosBcosL & 0 \\ cosL & -sinBsinL & cosBsinL & 0 \\ 0 & cosB & sinB & 0 \\ 0 & 0 & 0 & 1\end{bmatrix} M=T⋅R=⎣ ⎡100001000110XpTpZp1⎦ ⎤⎣ ⎡−sinLcosL00−sinBcosL−sinBsinLcosB0cosBcosLcosBsinLsinB00001⎦ ⎤上式中 [ X p T p Z p ] T \begin{bmatrix} X_p & T_p & Z_p\end{bmatrix}^T [XpTpZp]T为站心P坐标，L和B为P所在经纬度，具体推导过程不做详细解释；
4. 各个旋转表示之间的变换
①角轴到旋转矩阵，罗德里格斯公式：
R = c o s θ I + ( 1 − c o s θ ) n n T + s i n θ n ˆ R = cos\theta I + (1-cos\theta)nn^T + sin\theta n\ \^{} R=cosθI+(1−cosθ)nnT+sinθn ˆ②旋转矩阵到角轴：
t r ( R ) = c o s θ t r ( I ) + ( 1 − c o s θ ) t r ( n n T ) + s i n θ t r ( n ˆ ) = 3 c o s θ + ( 1 − c o s θ ) = 1 + 2 ∗ c o s θ \begin{aligned} & tr(R) = cos\theta tr(I) + (1-cos\theta)tr(nn^T) + sin\theta tr(n\ \^{}\ ) \\ & =3cos\theta + (1-cos\theta) \\&=1+2*cos\theta\\ \end{aligned} \\ tr(R)=cosθtr(I)+(1−cosθ)tr(nnT)+sinθtr(n ˆ )=3cosθ+(1−cosθ)=1+2∗cosθ那么角轴的的角度可以通过旋转矩阵的迹，方向可以通过旋转矩阵特征值为1的特征向量求取（因为轴绕着轴旋转==不旋转）
θ = a r c c o s t r ( R ) − 1 2 R n = n \theta = arccos\frac{tr(R)-1}{2} \\ Rn = n θ=arccos2tr(R)−1Rn=n③角轴转四元数
绕单位向量 [ n x , n y , n z ] [n_x, n_y, n_z] [nx,ny,nz]旋转角度 θ \theta θ：
q = [ c o s θ 2 , n x s i n θ 2 , n y s i n θ 2 , n z θ 2 ] q = [cos\frac{\theta}{2}, n_xsin\frac{\theta}{2}, n_ysin\frac{\theta}{2}, n_z\frac{\theta}{2}] q=[cos2θ,nxsin2θ,nysin2θ,nz2θ]④四元数到角轴
θ = 2 ∗ a r c c o s ( q 0 ) \theta = 2*arccos(q_0) \\ θ=2∗arccos(q0) [ n x , n y , n z ] = [ q 1 , q 2 , q 3 ] / s i n θ 2 [n_x, n_y, n_z] = [q_1, q_2, q_3]/sin\frac{\theta}{2} [nx,ny,nz]=[q1,q2,q3]/sin2θ⑤旋转矩阵到四元数
旋转矩阵 R = m i j , i ∈ [ 1 , 3 ] , j ∈ [ 1 , 3 ] R={m_{ij}}, i\in[1, 3], j\in[1,3] R=mij,i∈[1,3],j∈[1,3]
q 0 = t r ( R ) + 1 2 , q 1 = m 23 − m 32 4 q 0 , q 2 = m 31 − m 13 4 q 0 , q 3 = m 12 − m 21 4 q 0 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} q0=2tr(R)+1,q1=4q0m23−m32,q2=4q0m31−m13,q3=4q0m12−m21⑥四元数到旋转矩阵
四元数 q = q 0 + q 1 i + q 2 j + q 3 k = [ s , v ] T q = q_0 + q_1i + q_2j + q_3k=[s, v]^T q=q0+q1i+q2j+q3k=[s,v]T
R = v v T + s 2 I + 2 s v ˆ + ( v ˆ ) 2 = [ 1 − 2 q 2 2 − 2 q 3 2 2 q 1 q 2 + 2 q 0 q 3 2 q 1 q 2 − 2 q 0 q 2 2 q 1 q 2 − 2 q 0 q 3 1 − 2 q 1 2 − 2 q 3 2 2 q 2 q 3 + 2 q 0 q 1 2 q 1 q 2 + 2 q 0 q 2 2 q 2 q 3 − 2 q 0 q 1 1 − 2 q 1 2 − 2 q 2 2 ] R = vv^T + s^2I + 2sv\ \^{} + (v\ \^{}\ )^2 \\ = \begin{bmatrix} 1-2q_2^2-2q_3^2 & 2q_1q_2+2q_0q_3 & 2q_1q_2-2q_0q_2 \\ 2q_1q_2-2q_0q_3 & 1-2q_1^2-2q_3^2 & 2q_2q_3 + 2q_0q_1 \\ 2q_1q_2+2q_0q_2 & 2q_2q_3-2q_0q_1 & 1-2q_1^2-2q_2^2\end{bmatrix} R=vvT+s2I+2sv ˆ+(v ˆ )2=⎣ ⎡1−2q22−2q322q1q2−2q0q32q1q2+2q0q22q1q2+2q0q31−2q12−2q322q2q3−2q0q12q1q2−2q0q22q2q3+2q0q11−2q12−2q22⎦ ⎤
5. 四元数
四元数运算转换成矩阵运算
定义两个四元数下的符号+和 ⊕ \oplus ⊕有：
q + = [ s − v T v s I + v ˆ ] , q ⊕ = [ s − v T v s I − v ˆ ] q^+ = \begin{bmatrix} s & -v^T \\ v & sI+v\ \^{}\ \end{bmatrix}, q^{\oplus} = \begin{bmatrix} s & -v^T \\ v & sI-v\ \^{}\ \end{bmatrix} q+=[sv−vTsI+v ˆ ],q⊕=[sv−vTsI−v ˆ ]对于这两个运算符有
q 1 q 2 = q 1 + q 2 = q 2 ⊕ q 1 q_1q_2 = q_1^+q_2 = q_2^{\oplus}q_1 q1q2=q1+q2=q2⊕q1
6.误差指标
①RPE： relative pose error 相对位姿误差
计算时间间隔为 Δ t \Delta t Δt的两帧之间的位姿变化的误差（即这两帧之间的计算值和真值的误差），对于时刻t有相对位姿误差 RPE为：
E i : = ( Q i − 1 Q i + Δ t ) − 1 ( P i − 1 P i + Δ t ) E_i:=(Q_{i}^{-1}Q_{i+\Delta t})^{-1}(P_{i}^{-1}P_{i+\Delta t}) Ei:=(Qi−1Qi+Δt)−1(Pi−1Pi+Δt)上式中左边的部分表示估计的位姿变化值，右边的部分表示位姿变换真值；
对于一个n帧的轨迹，取时间间隔 Δ \Delta Δ，就有 m = n − Δ m=n-\Delta m=n−Δ个RPE值，用RMSE表示整个轨迹的RPE为：
R M S E ( E 1 : n , Δ ) : = ( 1 m ∑ i = 1 m ∣ ∣ t r a n s ( E i ) ∣ ∣ 2 ) 1 2 RMSE(E_{1:n}, \Delta) := (\frac{1}{m}\sum_{i=1}^{m}||trans(E_i)||^2)^{\frac{1}{2}} RMSE(E1:n,Δ):=(m1i=1∑m∣∣trans(Ei)∣∣2)21上式中trans表示旋转矩阵中平移部分；
②ATE： absolute trajectory error 绝对轨迹误差
估计位姿和真实位姿的直接差值，可以非常直观地反应算法精度和轨迹全局一致性，对于时刻t的ATE为：
F i − 1 : = Q i − 1 S P i F_i^{-1}:=Q_i^{-1}SP_i Fi−1:=Qi−1SPi上式中 S ∈ S i m ( 3 ) S\in Sim(3) S∈Sim(3)表示单目下估计的位姿到真实位姿的相似变换矩阵;
用RMSE表示整个轨迹的ATE为：
R M S E ( E 1 : n , Δ ) : = ( 1 n ∑ i = 1 n ∣ ∣ t r a n s ( F i ) ∣ ∣ 2 ) 1 2 RMSE(E_{1:n}, \Delta) := (\frac{1}{n}\sum_{i=1}^{n}||trans(F_i)||^2)^{\frac{1}{2}} RMSE(E1:n,Δ):=(n1i=1∑n∣∣trans(Fi)∣∣2)21