【SLAM】姓李名群

说在前面

水水水

名群的

• 群是一种集合加上一种运算的代数结构。
• 性质：
  1. 封闭性: $\forall a_1,a_2\in A,a_1\cdot a_2\in A$
     这里的 · 表示一种运算符，并不是点乘
  2. 结合律: $\forall a_1,a_2,a_3\in A,(a_1\cdot a_2)\cdot a_3=a_1\cdot (a_2\cdot a_3)$
  3. 幺元: $\exists a_0\in A,s.t.\forall a\in A,a_0\cdot a=a\cdot a_0=a$
     举个栗子，在实数加法中，$a_0=0$
  4. 逆: $\forall a\in A,\exists a^{-1}\in A,s.t.a\cdot a^{-1}=a_0$

姓李的

• 李群是指具有连续（光滑）性质的群。

李群代交数学

• 引入

  对于旋转矩阵R，满足$R{R}^T=I$，将R看为时间的函数R(t)，则：
  $$R(t)\cdot R(t{)}^T=I$$
  两边同时求导：
  $$\begin{gathered} R(t{)}^{\prime }\cdot R(t{)}^T+R(t)\cdot (R(t{)}^T{)}^{\prime }=0 \\ \Downarrow R=(R^T{)}^T \\ R(t{)}^{\prime }\cdot R(t{)}^T+(R(t{)}^T\cdot R(t{)}^{\prime }{)}^T=0 \\ \Downarrow get \\ R(t{)}^{\prime }\cdot R(t{)}^T=-(R(t{)}^{\prime }\cdot R(t{)}^T{)}^T \end{gathered}$$
  因此，$R(t{)}^{\prime }\cdot R(t{)}^T$是一个反对称矩阵。
  (主对角线为0，其余反对称
  $$S=\left[\begin{array}{ccc}{x}_{11}& x_{12}& x_{13}\\ x_{21}& x_{22}& x_{23}\\ x_{31}& x_{32}& x_{33}\end{array}\right]=\left[\begin{array}{ccc}-x_{11}& -x_{21}& -x_{31}\\ -x_{12}& -x_{22}& -x_{32}\\ -x_{13}& -x_{23}& -x_{33}\end{array}\right]=-S^T$$)
  之前有提到^表示一个三维向量的反对称矩阵化过程，即${\vec{a}}^{˄}=A$(A为反对称矩阵)，现在我们定义$A^{˅}=\vec{a}$，那么：
  $$\begin{gathered} R(t{)}^{\prime }\cdot R(t{)}^T=\varphi (t{)}^{˄} \\ \Downarrow right\cdot R(t) \\ R(t{)}^{\prime }=\varphi (t{)}^{˄}\cdot R(t)=\left[\begin{array}{ccc}0& -{\varphi }_3& {\varphi }_2\\ {\varphi }_3& 0& -{\varphi }_1\\ -{\varphi }_2& {\varphi }_1& 0\end{array}\right]\cdot R(t) \end{gathered}$$
  在 $t_0$ 附近，设 $\varphi$ 保持为常数 $\varphi (t_0)={\varphi }_0$。那么有
  $$R(t{)}^{\prime }=\varphi (t_0{)}^{\wedge }R(t)={\varphi }_0^{\wedge }R(t)$$
  上式是一个关于 R 的微分方程，而且我们知道初始值 $R(0)=I$，解之，得：
  $$R(t)=exp({\varphi }_0^{\wedge }t)$$

• 定义

  由集合+数域+二元运算组成，且满足：

  1. 封闭性 $\forall X,Y\in V,[X,Y]\in V$
     这个好理解，就像群一样
  2. 双线性 $\forall X,Y,Z\in V,a,b\in F$, 有：
     $[aX+bY,Z]=a[X,Z]+b[Y,Z],[Z,aX+bY]=a[Z,X]+b[Z,Y]$
     这个也好理解，就像分解律一样
  3. 自反性$\neg \forall X\in V,[X,X]=0$
  4. 雅可比等价 $\forall X,Y,Z\in V,[X,[Y,Z]]+[Z,[Y,X]]+[Y,[Z,X]]=0$
     这个怎么理解，，，

• so(3)

  • 定义
    s o ( 3 ) = { ϕ ∈ R 3 , Φ = ϕ ∧ ∈ R 3 × 3 } so(3) =\{\phi ∈R^3,\Phi = ϕ^∧ ∈R^{3×3}\} so(3)={ϕ∈R3,Φ=ϕ∧∈R3×3}
    其中：
    $$\Phi ={\varphi }^{\wedge }=\left[\begin{array}{ccc}0& -{\varphi }_3& {\varphi }_2\\ {\varphi }_3& 0& -{\varphi }_1\\ -{\varphi }_2& {\varphi }_1& 0\end{array}\right]$$
    二元运算为：
    $$[{\varphi }_1,{\varphi }_2]=({\Phi }_1{\Phi }_2-{\Phi }_2{\Phi }_1{)}^{\vee }$$
    指数映射为：
    $$R=exp({\varphi }^{\wedge })$$
  • 映射
    在收敛的情况下进行泰勒展开：
    $$exp({\varphi }^{\wedge })=\sum _{n=0}^{\infty }\frac{1}{n!}({\varphi }^{\wedge }{)}^n$$
    由于$\varphi$为一个三维向量，可以用$\varphi =\theta \vec{a}$($\theta$为大小，$\vec{a}$为单位向量)
    (
    下面俩的证明可以令:
    $$a=\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]$$
    比较容易证明
    $$a^{\wedge }{a}^{\wedge }=a{a}^T-I$$
    $$a^{\wedge }{a}^{\wedge }{a}^{\wedge }=-a^{\wedge }$$
    )
    那么(矩阵的零次方为单位阵)：
    $$\begin{array}{rl}exp({\varphi }^{\wedge })=exp(\theta a^{\wedge })& =\sum _{n=0}^{\infty }\frac{1}{n!}(\theta a^{\wedge }{)}^n\\ & =I+\theta a^{\wedge }+\frac{1}{2!}{\theta }^2{a}^{\wedge }{a}^{\wedge }+\frac{1}{3!}{\theta }^3{a}^{\wedge }{a}^{\wedge }{a}^{\wedge }+\frac{1}{4!}{\theta }^4(a^{\wedge }{)}^4+...\\ & =a{a}^T-a^{\wedge }{a}^{\wedge }+\theta a^{\wedge }+\frac{1}{2!}{\theta }^2{a}^{\wedge }{a}^{\wedge }-\frac{1}{3!}{\theta }^3{a}^{\wedge }-\frac{1}{4!}{\theta }^4(a^{\wedge }{)}^2+...\\ & =a{a}^T+(\theta -\frac{1}{3!}{\theta }^3+\frac{1}{5!}{\theta }^5-...)a^{\wedge }-(1-\frac{1}{2!}{\theta }^2+\frac{1}{4!}{\theta }^4-...)a^{\wedge }{a}^{\wedge }\\ & =a^{\wedge }{a}^{\wedge }+I+sin\theta a^{\wedge }-cos\theta a^{\wedge }{a}^{\wedge }\\ & =(1-cos\theta )a^{\wedge }{a}^{\wedge }+I+sin\theta a^{\wedge }\\ & =cos\theta I+(1-cos\theta )a{a}^T+sin\theta a^{\wedge }\end{array}$$
    $$\begin{gathered} sin\theta =\theta -\frac{1}{3!}{\theta }^3+\frac{1}{5!}{\theta }^5-... \\ cos\theta =1-\frac{1}{2!}{\theta }^2+\frac{1}{4!}{\theta }^4-... \end{gathered}$$
    即：
    $$exp({\varphi }^{\wedge })=cos\theta I+(1-cos\theta )a{a}^T+sin\theta a^{\wedge }$$
    与罗德里格斯公式一样，这样我们就将旋转向量空间映射到指数空间。

• se(3)

  • 定义
    $$se(3)=\left\{\xi =\left[\begin{array}{c}\rho \\ \varphi \end{array}\right]\in R^6,\rho \in R^3,\varphi \in so(3),{\xi }^{\vee }=\left[\begin{array}{cc}{\varphi }^{\wedge }& \rho \\ 0^T& 0\end{array}\right]\right\}$$
    二元运算为：
    $$[{\xi }_1,{\xi }_2]=({\xi }_1^{\wedge }{\xi }_2^{\wedge }-{\xi }_2^{\wedge }{\xi }_1^{\wedge }{)}^{\vee }$$
  • 映射
    $$exp({\xi }^{\wedge })=\left[\begin{array}{cc}{\sum }_{n=0}^{\infty }\frac{1}{n!}(\theta a^{\wedge }{)}^n& {\sum }_{n=0}^{\infty }\frac{1}{(n+1)!}(\theta a^{\wedge }{)}^n\rho \\ 0^T& 1\end{array}\right]=\left[\begin{array}{cc}R& J\rho \\ 0^T& 1\end{array}\right]$$
    其中：
    $$J=\frac{sin\theta }{\theta }I+(1-\frac{sin\theta }{\theta })a{a}^T+1-\frac{cos\theta }{\theta }{a}^{\wedge }$$

• sim(3)-单目

  • 定义
    $$Sim(3)=\left\{S=\left[\begin{array}{cc}sR& t\\ 0^T& 1\end{array}\right]\in R^{4\times 4}\right\}$$
    $$sim(3)=\left\{\zeta =\left[\begin{array}{c}\rho \\ \varphi \\ \sigma \end{array}\right]\in R^7,{\zeta }^{\wedge }=\left[\begin{array}{cc}\sigma I+{\varphi }^{\wedge }& \rho \\ 0^T& 0\end{array}\right]\right\}$$
  • 映射
    $$exp({\zeta }^{\wedge })=\left[\begin{array}{cc}{e}^{\sigma }exp({\varphi }^{\wedge })& J_s\rho \\ 0^T& 1\end{array}\right]$$
    其中：
    $$J_s=\frac{e^{\sigma }-1}{\sigma }I+\frac{\sigma e^{\sigma }sin\theta +(1-e^{\sigma }cos\theta )\theta }{{\sigma }^2+{\theta }^2}{a}^{\wedge }+(\frac{e^{\sigma }-1}{\sigma }-\frac{(e^{\sigma }cos\theta -1)\sigma +(e^{\sigma }sin\theta )\theta }{{\sigma }^2+{\theta }^2})a^{\wedge }{a}^{\wedge }$$
    对应关系：
    $$s=e^{\sigma },R=exp({\varphi }^{\wedge }),t=J_s\rho$$