【SLAM】4李群&李代数

李群: 表征旋转矩阵R或变换矩阵RT的集合
李代数: 描述旋转矩阵R或变换矩阵RT的导数关系
作用是解决矩阵求导问题, 解决微小扰动等问题

4.1.基础

• 旋转矩阵++=特殊正交群$SO(3)$ $\mathrm{SO}(3)=\left\{\mathbf{R}\in {\mathbb{R}}^{3\times 3}\mid \mathbf{R}{\mathbf{R}}^{\mathrm{T}}=\mathbf{I},\det (\mathbf{R})=1\right\}$
• 变换矩阵++=特殊欧式群$SE(3)$ S E ( 3 ) = { T = [ R t 0 T 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 } \mathrm{SE}(3)=\left\{\boldsymbol{T}=\left[
  Rt0T1" role="presentation">R0Tt1$$\begin{array}{cc}\mathit{R}& \mathit{t}\\ {\mathbf{0}}^{\mathrm{T}}& 1\end{array}$$
  \right] \in \mathbb{R}^{4 \times 4} \mid \boldsymbol{R} \in \mathrm{SO}(3), \boldsymbol{t} \in \mathbb{R}^{3}\right\} SE(3)={T=[R0T​t1​]∈R4×4∣R∈SO(3),t∈R3}
• 对加法不封闭,对乘法封闭${\mathbf{R}}_1+{\mathbf{R}}_2\notin \mathrm{SO}(3),{\mathbf{T}}_1+{\mathbf{T}}_2\notin \mathrm{SE}(3)$      ~~~~      ${\mathbf{R}}_1{\mathbf{R}}_2\in \mathrm{SO}(3),{\mathbf{T}}_1{\mathbf{T}}_2\in \mathrm{SE}(3)$

群

• 群: 一种集合+一种运算的代数结构 集合A 运算· 群G=(A,·)
  • 封闭性 $\forall a_1,a_2\in A,a_1\cdot a_2\in A$
  • 结合律 $\forall a_1,a_2,a_3\in A,\left(a_1\cdot a_2\right)\cdot a_3=a_1\cdot \left(a_2\cdot a_3\right)$
  • 幺元 $\exists a_0\in A,\text{ s.t. }\forall a\in A,a_0\cdot a=a\cdot a_0=a$
  • 逆 $\forall a\in A,\exists a^{-1}\in A,\text{ s.t. }a\cdot a^{-1}=a_0$
  • 常见
    • 整数加法$(Z,+)$      ~~~~      不含0有理数乘法$(Q\setminus 0,\cdot )$
    • 一般线性群GL(n) nxn可逆矩阵, 对矩阵乘法成群
    • 特殊正交群SO(n) 旋转矩阵群 SO(2) SO(3)
    • 特殊欧氏群SE(n) 欧氏变换 SE(2) SE(3)
  • 李群: 连续(光滑)性质的群 SO(n) SE(n) 能连续运动

李代数的引出

• 李代数

  • 任意旋转矩阵R满足 $R{R}^T=I$
  • 反对称换算 a ∧ = A = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] , A ∨ = a \boldsymbol{a}^{\wedge}=\boldsymbol{A}=\left[
    0−a3a2a30−a1−a2a10" role="presentation" style="position: relative;">0a3−a2−a30a1a2−a10$$\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}$$
    \right], \quad \boldsymbol{A}^{\vee}=\boldsymbol{a} a∧=A=⎣ ⎡​0a3​−a2​​−a3​0a1​​a2​−a1​0​⎦ ⎤​,A∨=a
  • 等式两边求导得反对称矩阵$\dot{\mathbf{R}}(t)\mathbf{R}(t{)}^{\mathrm{T}}=-{\left(\dot{\mathbf{R}}(t)\mathbf{R}(t{)}^{\mathrm{T}}\right)}^{\mathrm{T}}=\varphi (t{)}^{\wedge }$
  • 每对旋转矩阵求一次导, 左乘一个${\varphi }^{\wedge }(t)$矩阵, $\dot{R}(t)={\varphi }_0^{\wedge }R(t)$
    • 解得$\text{R}(t)=\exp ({\varphi }_0^{\wedge }t)$ 说明 $t=0$附近, 旋转矩阵可由$\exp ({\varphi }_0\stackrel{ˆ}{}t)$计算
  1. 给定某时刻$R$, 求得一个$\varphi$, 描述了$R$在局部的导数关系: $\varphi$正是对应到$SO(3)$上的李代数$\mathfrak{so}(3)$
  2. 给定向量$\varphi$计算矩阵指数$\exp ({\varphi }^{\wedge })$; 给定$R$计算$\varphi$: 指数/对数映射

李代数

• 一个集合V, 一个数域F, 一个二元运算[,]组成, 满足以下性质则(V,F,[,])为一个李代数
• 性质
  • 封闭性 $\forall \mathbf{X},\mathbf{Y}\in \mathbb{V},[\mathbf{X},\mathbf{Y}]\in \mathbb{V}$
  • 双线性 $\forall \mathbf{X},\mathbf{Y},\mathbf{Z}\in \mathbb{V},a,b\in \mathbb{F}$ 有 { [ a X + b Y , Z ] = a [ X , Z ] + b [ Y , Z ] [ Z , a X + b Y ] = a [ Z , X ] + b [ Z , Y ] \left\{
    [aX+bY,Z]=a[X,Z]+b[Y,Z][Z,aX+bY]=a[Z,X]+b[Z,Y]" role="presentation" style="position: relative;">[aX+bY,Z]=a[X,Z]+b[Y,Z][Z,aX+bY]=a[Z,X]+b[Z,Y]$$\begin{array}{c}[a\mathit{X}+b\mathit{Y},\mathit{Z}]=a[\mathit{X},\mathit{Z}]+b[\mathit{Y},\mathit{Z}]\\ [\mathit{Z},a\mathit{X}+b\mathit{Y}]=a[\mathit{Z},\mathit{X}]+b[\mathit{Z},\mathit{Y}]\end{array}$$
    \right. {[aX+bY,Z]=a[X,Z]+b[Y,Z][Z,aX+bY]=a[Z,X]+b[Z,Y]​
  • 自反性 $\forall \mathbf{X}\in \mathbb{V},[\mathbf{X},\mathbf{X}]=0$
  • 雅可比等价 $\forall \mathbf{X},\mathbf{Y},\mathbf{Z}\in \mathbb{V},[\mathbf{X},[\mathbf{Y},\mathbf{Z}]]+[\mathbf{Z},[\mathbf{X},\mathbf{Y}]]+[\mathbf{Y},[\mathbf{Z},\mathbf{X}]]=0$
• 李括号[,]
  • 表达了两个元素的差异
  • 三维向量$R^3$定义的叉积是一种李括号, $g=(R^3,R,\mathrm{x})$构成一个李代数

李代数$\mathfrak{so}(3)$

• SO(3)对应的李代数是定义于$R^3$上的向量, 记作$\varphi$, 每个$\varphi$可生成一个反对称阵 $\Phi ={\varphi }^{\wedge }$
  • 李括号 $[{\varphi }_1,{\varphi }_2]=({\Phi }_1{\Phi }_2-{\Phi }_2{\Phi }_1{)}^{\vee }$
  • s o ( 3 ) = { ϕ ∈ R 3 , Φ = ϕ ∧ ∈ R 3 × 3 } \mathfrak{so}(3)=\{\phi\in R^3,\Phi=\phi^{\wedge}\in R^{3\times3}\} so(3)={ϕ∈R3,Φ=ϕ∧∈R3×3}
    • 由三维向量组成的集合, 每个向量对应一个反对称阵, 以表达旋转矩阵的导数
    • 与$SO(3)$的关系 $R=\exp ({\varphi }^{\wedge })$

李代数$\mathfrak{se}(3)$

• 李代数 s e ( 3 ) = { ξ = [ ρ ϕ ] ∈ R 6 , ρ ∈ R 3 , ϕ ∈ s o ( 3 ) , ξ ∧ = [ ϕ ∧ ρ 0 T 0 ] ∈ R 4 × 4 } \mathfrak{s e}(3)=\left\{\boldsymbol{\xi}=\left[
  ρϕ" role="presentation" style="position: relative;">ρϕ$$\begin{array}{l}\mathit{\rho }\\ \varphi \end{array}$$
  \right] \in \mathbb{R}^{6}, \boldsymbol{\rho} \in \mathbb{R}^{3}, \phi \in \mathfrak{s o}(3), \boldsymbol{\xi}^{\wedge}=\left[
  ϕ∧ρ0T0" role="presentation" style="position: relative;">ϕ∧0Tρ0$$\begin{array}{cc}{\varphi }^{\wedge }& \rho \\ {\mathbf{0}}^{\mathrm{T}}& 0\end{array}$$
  \right] \in \mathbb{R}^{4 \times 4}\right\} se(3)={ξ=[ρϕ​]∈R6,ρ∈R3,ϕ∈so(3),ξ∧=[ϕ∧0T​ρ0​]∈R4×4}
  • 六维向量$\xi$, 平移$\rho$, 旋转$\varphi$, 向量->矩阵${}^{\wedge }$, 矩阵->向量${}^{\vee }$
  • 括号 $[{\xi }_1,{\xi }_2]=({\xi }_1^{\wedge }{\xi }_2^{\wedge }-{\xi }_2^{\wedge }{\xi }_1^{\wedge }{)}^{\vee }$

4.2.指数与对数映射

4.2.1.$SO(3)$指数映射

• 对$\mathfrak{so}(3)$的任意元素 $\varphi$, 定义指数映射 $\exp(\phi^{\wedge)=\sum_{n=0}}{\infty}\frac{1}{n!}(\phi^\wedge)n $
  • 一顿推导得 $\exp (\theta a^{\wedge })=\cos \theta I+(1-\cos \theta )a{a}^T+\sin \theta a^{\wedge }$ (罗德里格斯公式-从旋转向量到旋转矩阵的转换)
  • 表明$\mathfrak{so}(3)$是旋转向量组成的空间
• 对数映射$\varphi =\ln (R{)}^{\vee }=({\sum }_{n=0}^{\infty }\frac{(-1{)}^n}{n+1}(R-I{)}^{n+1}{)}^{\vee }$
• 指数映射是满射
  • 每个$SO(3)$的元素可以找到一个$\mathfrak{so}(3)$元素对应
  • 可能存在多个$\mathfrak{so}(3)$元素对应同一个$SO(3)$
    • 对旋转角$\theta$, 多转$360^{\circ }$和没转一样
  • 指数映射即罗德里格斯公式
  • 旋转矩阵的导数可由旋转向量指定, 指导如何在旋转矩阵中进行微积分运算

4.2.2.$SE(3)$的指数映射

• exp ⁡ ( ξ ∧ ) = [ ∑ n = 0 ∞ 1 n ! ( ϕ ∧ ) n ∑ n = 0 ∞ 1 ( n + 1 ) ! ( ϕ ∧ ) n ρ 0 T ] ≜ [ R J ρ 0 T 1 ] = T .
  exp⁡(ξ∧)=[∑n=0∞1n!(ϕ∧)n∑n=0∞1(n+1)!(ϕ∧)nρ0T]≜[RJρ0T1]=T." role="presentation">exp(ξ∧)=[∑∞n=01n!(ϕ∧)n0T∑∞n=01(n+1)!(ϕ∧)nρ]≜[R0TJρ1]=T.$$\begin{array}{rl}\exp \left({\mathit{\xi }}^{\wedge }\right)& =\left[\begin{array}{cc}\sum _{n=0}^{\mathrm{\infty }}\frac{1}{n!}{\left({\mathit{\varphi }}^{\wedge }\right)}^n& \sum _{n=0}^{\mathrm{\infty }}\frac{1}{(n+1)!}{\left({\mathit{\varphi }}^{\wedge }\right)}^n\mathit{\rho }\\ {\mathbf{0}}^{\mathrm{T}}\end{array}\right]\\ & \triangleq \left[\begin{array}{cc}\mathit{R}& \mathit{J}\mathit{\rho }\\ {\mathbf{0}}^{\mathrm{T}}& 1\end{array}\right]=\mathit{T}.\end{array}$$
  exp(ξ∧)​=[∑n=0∞​n!1​(ϕ∧)n0T​∑n=0∞​(n+1)!1​(ϕ∧)nρ]≜[R0T​Jρ1​]=T.​
  • 左上角的R是$SO(3)$的元素, 与$\mathfrak{se}(3)$的旋转部分$\varphi$对应
  • 右上角$\mathbf{J}=\frac{\sin \theta }{\theta }\mathbf{I}+\left(1-\frac{\sin \theta }{\theta }\right)\mathbf{a}{\mathbf{a}}^{\mathrm{T}}+\frac{1-\cos \theta }{\theta }{\mathbf{a}}^{\wedge }$
    • 平移部分经指数映射后, 发生了一次以$J$为系数矩阵的线性变换
    • 左上角的$R$计算旋转向量, 右上角的$t$满足$t=J\rho$

4.3.李代数求导与扰动模型

4.3.1.BCH公式&近似形式

• $\ln (\exp (\mathbf{A})\exp (\mathbf{B}))=\mathbf{A}+\mathbf{B}+\frac{1}{2}[\mathbf{A},\mathbf{B}]+\frac{1}{12}[\mathbf{A},[\mathbf{A},\mathbf{B}]]-\frac{1}{12}[\mathbf{B},[\mathbf{A},\mathbf{B}]]+\cdots$
  • 两个矩阵指数积, 会产生一些由李括号组成的余项
• $SO(3)$
  • 旋转$R$-李代数$\varphi$, 左乘一个微小旋转$\Delta R$-李代数$\Delta \varphi$, 则李群上得$\Delta R\cdot R$, 李代数$J_l^{-1}(\varphi )\Delta \varphi +\varphi$
    • $\exp \left(\Delta {\varphi }^{\wedge }\right)\exp \left({\varphi }^{\wedge }\right)=\exp \left({\left(\varphi +J_l^{-1}(\varphi )\Delta \varphi \right)}^{\wedge }\right)$
  • 李代数加法, $\varphi +\Delta \varphi$, 近似为李群上带左右雅可比的乘法
    • $\exp ((\varphi +\Delta \varphi {)}^{\wedge })=\exp ({\varphi }^{\wedge })\exp ((J_r\Delta \varphi {)}^{\wedge })$
  • 此为之后做微积分的理论基础
• $SE(3)$
  • exp ⁡ ( Δ ξ ∧ ) exp ⁡ ( ξ ∧ ) ≈ exp ⁡ ( ( J l − 1 Δ ξ + ξ ) ∧ ) exp ⁡ ( ξ ∧ ) exp ⁡ ( Δ ξ ∧ ) ≈ exp ⁡ ( ( J r − 1 Δ ξ + ξ ) ∧ )
    exp⁡(Δξ∧)exp⁡(ξ∧)≈exp⁡((Jl−1Δξ+ξ)∧)exp⁡(ξ∧)exp⁡(Δξ∧)≈exp⁡((Jr−1Δξ+ξ)∧)" role="presentation" style="position: relative;">exp(Δξ∧)exp(ξ∧)≈exp((J−1lΔξ+ξ)∧)exp(ξ∧)exp(Δξ∧)≈exp((J−1rΔξ+ξ)∧)$$\begin{array}{l}\exp \left(\mathrm{\Delta }{\mathit{\xi }}^{\wedge }\right)\exp \left({\mathit{\xi }}^{\wedge }\right)\approx \exp \left({\left({\mathcal{J}}_l^{-1}\mathrm{\Delta }\mathit{\xi }+\mathit{\xi }\right)}^{\wedge }\right)\\ \exp \left({\mathit{\xi }}^{\wedge }\right)\exp \left(\mathrm{\Delta }{\mathit{\xi }}^{\wedge }\right)\approx \exp \left({\left({\mathcal{J}}_r^{-1}\mathrm{\Delta }\mathit{\xi }+\mathit{\xi }\right)}^{\wedge }\right)\end{array}$$
    exp(Δξ∧)exp(ξ∧)≈exp((Jl−1​Δξ+ξ)∧)exp(ξ∧)exp(Δξ∧)≈exp((Jr−1​Δξ+ξ)∧)​

4.3.2.李代数求导

• 小萝卜位姿T, 观察到世界坐标位于p的点 $z=Tp+w$
  • 观测与实际数据误差 $e=z-Tp$
  • 寻找一个最优T使整体误差最小化 ${\min }_{T}J(T)={\sum }_{i=1}^N||z_i-T{p}_i|{|}_2^2$
• 构建与位姿有关函数, 讨论该函数关于位姿的导数, 调整当前估计值
  • 李代数表示姿态, 据李代数加法对李代数求导
  • 对李群左乘/右乘微小扰动, 对扰动求导称左扰动/右扰动模型

4.3.4.扰动模型(左乘)

• 对$R$进行一次扰动$\Delta R$(李代数$\phi$), 对$\phi$求导
  • ∂ ( R p ) ∂ φ = lim ⁡ φ → 0 exp ⁡ ( φ ∧ ) exp ⁡ ( ϕ ∧ ) p − exp ⁡ ( ϕ ∧ ) p φ = lim ⁡ φ → 0 ( I + φ ∧ ) exp ⁡ ( ϕ ∧ ) p − exp ⁡ ( ϕ ∧ ) p φ = lim ⁡ φ → 0 φ ∧ R p φ = lim ⁡ φ → 0 − ( R p ) ∧ φ φ = − ( R p ) ∧
    ∂(Rp)∂φ=limφ→0exp⁡(φ∧)exp⁡(ϕ∧)p−exp⁡(ϕ∧)pφ=limφ→0(I+φ∧)exp⁡(ϕ∧)p−exp⁡(ϕ∧)pφ=limφ→0φ∧Rpφ=limφ→0−(Rp)∧φφ=−(Rp)∧" role="presentation" style="position: relative;">∂(Rp)∂φ=limφ→0exp(φ∧)exp(ϕ∧)p−exp(ϕ∧)pφ=limφ→0(I+φ∧)exp(ϕ∧)p−exp(ϕ∧)pφ=limφ→0φ∧Rpφ=limφ→0−(Rp)∧φφ=−(Rp)∧$$\begin{array}{rl}\frac{\mathrm{\partial }(\mathit{R}\mathit{p})}{\mathrm{\partial }\mathit{\phi }}& =\underset{\mathit{\phi }\to 0}{lim}\frac{\exp \left({\mathit{\phi }}^{\wedge }\right)\exp \left({\mathit{\varphi }}^{\wedge }\right)\mathit{p}-\exp \left({\mathit{\varphi }}^{\wedge }\right)\mathit{p}}{\mathit{\phi }}\\ & =\underset{\mathit{\phi }\to 0}{lim}\frac{\left(\mathit{I}+{\mathit{\phi }}^{\wedge }\right)\exp \left({\mathit{\varphi }}^{\wedge }\right)\mathit{p}-\exp \left({\mathit{\varphi }}^{\wedge }\right)\mathit{p}}{\mathit{\phi }}\\ & =\underset{\mathit{\phi }\to 0}{lim}\frac{{\mathit{\phi }}^{\wedge }\mathit{R}\mathit{p}}{\mathit{\phi }}=\underset{\mathit{\phi }\to \mathbf{0}}{lim}\frac{-(\mathit{R}\mathit{p}{)}^{\wedge }\mathit{\phi }}{\mathit{\phi }}=-(\mathit{R}\mathit{p}{)}^{\wedge }\end{array}$$
    ∂φ∂(Rp)​​=φ→0lim​φexp(φ∧)exp(ϕ∧)p−exp(ϕ∧)p​=φ→0lim​φ(I+φ∧)exp(ϕ∧)p−exp(ϕ∧)p​=φ→0lim​φφ∧Rp​=φ→0lim​φ−(Rp)∧φ​=−(Rp)∧​

4.3.5.$SE(3)$李代数求导

• ∂ ( T p ) ∂ δ ξ = lim ⁡ δ ξ → 0 exp ⁡ ( δ ξ ∧ ) exp ⁡ ( ξ ∧ ) p − exp ⁡ ( ξ ∧ ) p δ ξ = lim ⁡ δ ξ → 0 ( I + δ ξ ∧ ) exp ⁡ ( ξ ∧ ) p − exp ⁡ ( ξ ∧ ) p δ ξ = lim ⁡ δ ξ → 0 δ ξ ∧ exp ⁡ ( ξ ∧ ) p δ ξ = lim ⁡ δ ξ → 0 [ δ ϕ ∧ δ ρ 0 T 0 ] [ R p + t 1 ] δ ξ = lim ⁡ δ ξ → 0 [ δ ϕ ∧ ( R p + t ) + δ ρ 0 T ] [ δ ρ , δ ϕ ] T = [ I − ( R p + t ) ∧ 0 T 0 T ] =  def  ( T p ) ⊙ . \frac{\partial(\boldsymbol{T} \boldsymbol{p})}{\partial \delta \boldsymbol{\xi}}\\
  \begin{array}{l}=\lim _{\delta \boldsymbol{\xi} \rightarrow 0} \frac{\exp \left(\delta \boldsymbol{\xi}^{\wedge}\right) \exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{p}-\exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{p}}{\delta \boldsymbol{\xi}}\\=\lim _{\delta \boldsymbol{\xi} \rightarrow \mathbf{0}} \frac{\left(\boldsymbol{I}+\delta \boldsymbol{\xi}^{\wedge}\right) \exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{p}-\exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{p}}{\delta \boldsymbol{\xi}} \\=\lim _{\delta \boldsymbol{\xi} \rightarrow 0} \frac{\delta \boldsymbol{\xi}^{\wedge} \exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{p}}{\delta \boldsymbol{\xi}} \\=\lim _{\delta \boldsymbol{\xi} \rightarrow 0} \frac{\left[\begin{array}{cc}\delta \boldsymbol{\phi}^{\wedge} & \delta \boldsymbol{\rho} \\\mathbf{0}^{\mathrm{T}} & 0\end{array}\right]\left[\begin{array}{c}\boldsymbol{R} \boldsymbol{p}+\boldsymbol{t} \\1\end{array}\right]}{\delta \boldsymbol{\xi}} \\=\lim _{\delta \boldsymbol{\xi} \rightarrow 0} \frac{\left[\begin{array}{c}\delta \boldsymbol{\phi}^{\wedge}(\boldsymbol{R} \boldsymbol{p}+\boldsymbol{t})+\delta \boldsymbol{\rho} \\\mathbf{0}^{\mathrm{T}}\end{array}\right]}{[\delta \boldsymbol{\rho}, \delta \boldsymbol{\phi}]^{\mathrm{T}}}=\left[\begin{array}{cc}\boldsymbol{I} & -(\boldsymbol{R} \boldsymbol{p}+\boldsymbol{t})^{\wedge} \\\mathbf{0}^{\mathrm{T}} & \mathbf{0}^{\mathrm{T}}\end{array}" role="presentation" style="position: relative;">\begin{array}{l}=\lim _{\delta \boldsymbol{\xi} \rightarrow 0} \frac{\exp \left(\delta \boldsymbol{\xi}^{\wedge}\right) \exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{p}-\exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{p}}{\delta \boldsymbol{\xi}}\\=\lim _{\delta \boldsymbol{\xi} \rightarrow \mathbf{0}} \frac{\left(\boldsymbol{I}+\delta \boldsymbol{\xi}^{\wedge}\right) \exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{p}-\exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{p}}{\delta \boldsymbol{\xi}} \\=\lim _{\delta \boldsymbol{\xi} \rightarrow 0} \frac{\delta \boldsymbol{\xi}^{\wedge} \exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{p}}{\delta \boldsymbol{\xi}} \\=\lim _{\delta \boldsymbol{\xi} \rightarrow 0} \frac{\left[\begin{array}{cc}\delta \boldsymbol{\phi}^{\wedge} & \delta \boldsymbol{\rho} \\\mathbf{0}^{\mathrm{T}} & 0\end{array}\right]\left[\begin{array}{c}\boldsymbol{R} \boldsymbol{p}+\boldsymbol{t} \\1\end{array}\right]}{\delta \boldsymbol{\xi}} \\=\lim _{\delta \boldsymbol{\xi} \rightarrow 0} \frac{\left[\begin{array}{c}\delta \boldsymbol{\phi}^{\wedge}(\boldsymbol{R} \boldsymbol{p}+\boldsymbol{t})+\delta \boldsymbol{\rho} \\\mathbf{0}^{\mathrm{T}}\end{array}\right]}{[\delta \boldsymbol{\rho}, \delta \boldsymbol{\phi}]^{\mathrm{T}}}=\left[\begin{array}{cc}\boldsymbol{I} & -(\boldsymbol{R} \boldsymbol{p}+\boldsymbol{t})^{\wedge} \\\mathbf{0}^{\mathrm{T}} & \mathbf{0}^{\mathrm{T}}\end{array}$$\begin{array}{l}=\lim _{\delta \boldsymbol{\xi} \rightarrow 0} \frac{\exp \left(\delta \boldsymbol{\xi}^{\wedge}\right) \exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{p}-\exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{p}}{\delta \boldsymbol{\xi}}\\=\lim _{\delta \boldsymbol{\xi} \rightarrow \mathbf{0}} \frac{\left(\boldsymbol{I}+\delta \boldsymbol{\xi}^{\wedge}\right) \exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{p}-\exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{p}}{\delta \boldsymbol{\xi}} \\=\lim _{\delta \boldsymbol{\xi} \rightarrow 0} \frac{\delta \boldsymbol{\xi}^{\wedge} \exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{p}}{\delta \boldsymbol{\xi}} \\=\lim _{\delta \boldsymbol{\xi} \rightarrow 0} \frac{\left[\begin{array}{cc}\delta \boldsymbol{\phi}^{\wedge} & \delta \boldsymbol{\rho} \\\mathbf{0}^{\mathrm{T}} & 0\end{array}\right]\left[\begin{array}{c}\boldsymbol{R} \boldsymbol{p}+\boldsymbol{t} \\1\end{array}\right]}{\delta \boldsymbol{\xi}} \\=\lim _{\delta \boldsymbol{\xi} \rightarrow 0} \frac{\left[\begin{array}{c}\delta \boldsymbol{\phi}^{\wedge}(\boldsymbol{R} \boldsymbol{p}+\boldsymbol{t})+\delta \boldsymbol{\rho} \\\mathbf{0}^{\mathrm{T}}\end{array}\right]}{[\delta \boldsymbol{\rho}, \delta \boldsymbol{\phi}]^{\mathrm{T}}}=\left[\begin{array}{cc}\boldsymbol{I} & -(\boldsymbol{R} \boldsymbol{p}+\boldsymbol{t})^{\wedge} \\\mathbf{0}^{\mathrm{T}} & \mathbf{0}^{\mathrm{T}}\end{array}$$
  \right] \stackrel{\text { def }}{=}(\boldsymbol{T} \boldsymbol{p})^{\odot} .\end{array} ∂δξ∂(Tp)​=limδξ→0​δξexp(δξ∧)exp(ξ∧)p−exp(ξ∧)p​=limδξ→0​δξ(I+δξ∧)exp(ξ∧)p−exp(ξ∧)p​=limδξ→0​δξδξ∧exp(ξ∧)p​=limδξ→0​δξ[δϕ∧0T​δρ0​][Rp+t1​]​=limδξ→0​[δρ,δϕ]T[δϕ∧(Rp+t)+δρ0T​]​=[I0T​−(Rp+t)∧0T​]= def (Tp)⊙.​
  • 矩阵求导 d [ a b ] d [ x y ] = ( d [ a , b ] T d [ x y ] ) T = [ d a d x d b d x d a d y d b d y ] T = [ d a d x d a d y d b d x d b d y ] \frac{\mathrm{d}\left[
    ab" role="presentation" style="position: relative;">ab$$\begin{array}{l}\mathit{a}\\ \mathit{b}\end{array}$$
    \right]}{\mathrm{d}\left[
    xy" role="presentation" style="position: relative;">xy$$\begin{array}{l}\mathit{x}\\ \mathit{y}\end{array}$$
    \right]}=\left(\frac{\mathrm{d}[\boldsymbol{a}, \boldsymbol{b}]^{\mathrm{T}}}{\mathrm{d}\left[
    xy" role="presentation" style="position: relative;">xy$$\begin{array}{l}\mathit{x}\\ \mathit{y}\end{array}$$
    \right]}\right)^{\mathrm{T}}=\left[
    dadxdbdxdadydbdy" role="presentation" style="position: relative;">dadxdadydbdxdbdy$$\begin{array}{ll}\frac{\mathrm{d}\mathit{a}}{\mathrm{d}\mathit{x}}& \frac{\mathrm{d}\mathit{b}}{\mathrm{d}\mathit{x}}\\ \frac{\mathrm{d}\mathit{a}}{\mathrm{d}\mathit{y}}& \frac{\mathrm{d}\mathit{b}}{\mathrm{d}\mathit{y}}\end{array}$$
    \right]^{\mathrm{T}}=\left[
    dadxdadydbdxdbdy" role="presentation" style="position: relative;">dadxdbdxdadydbdy$$\begin{array}{cc}\frac{\mathrm{d}\mathit{a}}{\mathrm{d}\mathit{x}}& \frac{\mathrm{d}\mathit{a}}{\mathrm{d}\mathit{y}}\\ \frac{\mathrm{d}\mathit{b}}{\mathrm{d}\mathit{x}}& \frac{\mathrm{d}\mathit{b}}{\mathrm{d}\mathit{y}}\end{array}$$
    \right] d[xy​]d[ab​]​=⎝ ⎛​d[xy​]d[a,b]T​⎠ ⎞​T=[dxda​dyda​​dxdb​dydb​​]T=[dxda​dxdb​​dyda​dydb​​]

4.5.相似变换

• 单目视觉下存在尺度不确定性
  • 若用$SE(3)$表示位姿, 由于尺度不确定性与尺度漂移, 整个SLAM过程的尺度会发生变化
  • 显式地表达尺度因子, 对空间点p, 相机坐标系下经过一个相似变换, 而非欧氏变换
    • p ′ = [ s R t 0 T 1 ] p = s R p + t \boldsymbol{p}^{\prime}=\left[
      sRt0T1" role="presentation" style="position: relative;">sR0Tt1$$\begin{array}{ll}s\mathit{R}& \mathit{t}\\ {\mathbf{0}}^{\mathrm{T}}& 1\end{array}$$
      \right] \boldsymbol{p}=s \boldsymbol{R} \boldsymbol{p}+\boldsymbol{t} p′=[sR0T​t1​]p=sRp+t
      • 尺度s同时作用在p的三个坐标上, 对其进行缩放
    • 相似变换群 Sim ⁡ ( 3 ) = { S = [ s R t 0 T 1 ] ∈ R 4 × 4 } \operatorname{Sim}(3)=\left\{\boldsymbol{S}=\left[
      sRt0T1" role="presentation" style="position: relative;">sR0Tt1$$\begin{array}{ll}s\mathit{R}& \mathit{t}\\ {\mathbf{0}}^{\mathrm{T}}& 1\end{array}$$
      \right] \in \mathbb{R}^{4 \times 4}\right\} Sim(3)={S=[sR0T​t1​]∈R4×4}
    • 李代数

实现

main.cpp

#include 
#include 
#include 
#include 
#include "sophus/se3.hpp"
#include 
#include 
using namespace std;
using namespace Eigen;
typedef vector> TrajectoryType; 
typedef vector> PosesType;
void Change_SO3();
void Change_SE3();
void DrawTrajectory(vector> poses);
void test_play(string TrajectFile);
void test_play2(string TrajectFile1, string TrajectFile2);
TrajectoryType ReadTrajectory(const string &path);
string estimated_file = "../estimated.txt";   //  在build文件的上一级寻找
string groundtruth_file = "../groundtruth.txt";   //  在build文件的上一级寻找
int main(int argc, char **argv)
{
    // Change_SO3();
    // Change_SE3();
    // test_play(estimated_file);
    test_play2(groundtruth_file,estimated_file);

    TrajectoryType ExtimateData = ReadTrajectory(estimated_file);
    TrajectoryType GroundData = ReadTrajectory(groundtruth_file);
    assert(!ExtimateData.empty() && !GroundData.empty());
    assert(ExtimateData.size() == GroundData.size());
    double rmse = 0;
    for(size_t i=0;i反对称阵
    Matrix3d so3hat = Sophus::SO3d::hat(so3);
    cout << "\nso3hat:\n" << so3hat << endl;
    // vee反对称阵->向量
    Vector3d so3vee = Sophus::SO3d::vee(so3hat).transpose();
    cout << "\nso3vee:\n" << so3vee << endl;

    // 增量扰动模型更新
    Vector3d update_so3(1e-4,0,0);
    Sophus::SO3d SO3_updated = Sophus::SO3d::exp(update_so3) * SO3_R;
    cout << "\nSO(3) updated:\n" << SO3_updated.matrix() << endl;
}
void Change_SE3()
{
    // 沿y轴旋转90度的矩阵
    Matrix3d R = AngleAxisd(M_PI_2, Vector3d(0,1,0)).toRotationMatrix();
    Quaterniond q(R);
    Vector3d t(1,0,0);  //  沿x轴平移1
    Sophus::SE3d SE3_Rt(R,t);   //  从Rt构造SE(3)
    cout << "\nSE3 from R,t:\n" << SE3_Rt.matrix() << endl;
    Sophus::SE3d SE3_qt(q,t);   //  从qt构造SE(3)
    cout << "\nSE3 from q,t:\n" << SE3_qt.matrix() << endl;

    typedef Eigen::Matrix Vector6d; //  李代数是一个六维向量
    typedef Eigen::Matrix Matrix6d; //  李代数是一个六维向量
    Vector6d se3 = SE3_Rt.log();    //  取李代数
    cout << "\nse3:\n" << se3.transpose() << endl;
    Matrix6d se3hat = Sophus::SE3d::hat(se3);
    cout << "\nse3 hat\n" << se3hat << endl;
    Vector6d se3vee = Sophus::SE3d::vee(se3hat).transpose();
    cout << "\nse3 vee\n" << se3vee << endl;

    // 增量扰动模型更新
    Vector6d update_se3;
    update_se3.setZero();
    update_se3(0,0) = 1e-4d;
    Sophus::SE3d SE3_updated = Sophus::SE3d::exp(update_se3) * SE3_Rt;
    cout << "\nSE3 update\n" << SE3_updated.matrix() << endl;
}

void DrawTrajectory(PosesType poses)
{
    pangolin::CreateWindowAndBind("Viewer",1024,768);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
    pangolin::OpenGlRenderState s_cam(
        pangolin::ProjectionMatrix(1024,768,500,500,512,389,0.1,1000),
        pangolin::ModelViewLookAt(0,-0.1,-1.8,0,0,0,0.0,-1.0,0.0)
    );
    pangolin::View &d_cam = pangolin::CreateDisplay()
        .SetBounds(0.0,1.0,0.0,1.0,-1024.0f/768.0f)
        .SetHandler(new pangolin::Handler3D(s_cam));
    while (pangolin::ShouldQuit() == false)
    {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
        d_cam.Activate(s_cam);
        glClearColor(1.0f,1.0f,1.0f,1.0f);
        glLineWidth(2);
        for(size_t i=0; i> poses;
    ifstream fin(TrajectFile);  //  读取坐标文件
    if(!fin){
        cout << "fail to open";
    }
    while(!fin.eof())   //  循环读取所有数据v
    {
        double time,tx,ty,tz,qx,qy,qz,qw;
        fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
        Isometry3d Twr(Quaterniond(qw,qx,qy,qz));   //  +四元数
        Twr.pretranslate(Vector3d(tx,ty,tz));   //  +T坐标
        poses.push_back(Twr);   //  放到后边
    }
    cout << "read" << poses.size() << "poses" << endl;
    DrawTrajectory(poses);
}

void DrawTrajectory2(PosesType poses1, PosesType poses2)
{
    pangolin::CreateWindowAndBind("Viewer",1024,768);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
    pangolin::OpenGlRenderState s_cam(
        pangolin::ProjectionMatrix(1024,768,500,500,512,389,0.1,1000),
        pangolin::ModelViewLookAt(0,-0.1,-1.8,0,0,0,0.0,-1.0,0.0)
    );
    pangolin::View &d_cam = pangolin::CreateDisplay()
        .SetBounds(0.0,1.0,0.0,1.0,-1024.0f/768.0f)
        .SetHandler(new pangolin::Handler3D(s_cam));
    while (pangolin::ShouldQuit() == false)
    {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
        d_cam.Activate(s_cam);
        glClearColor(1.0f,1.0f,1.0f,1.0f);
        glLineWidth(2);

        for(size_t i=0; i> poses1;
    ifstream fin1(TrajectFile1);  //  读取坐标文件
    if(!fin1){
        cout << "fail to open";
    }
    while(!fin1.eof())   //  循环读取所有数据v
    {
        double time,tx,ty,tz,qx,qy,qz,qw;
        fin1 >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
        Isometry3d Twr(Quaterniond(qw,qx,qy,qz));   //  +四元数
        Twr.pretranslate(Vector3d(tx,ty,tz));   //  +T坐标
        poses1.push_back(Twr);   //  放到后边
    }
    cout << "read" << poses1.size() << "poses" << endl;

    vector> poses2;
    ifstream fin2(TrajectFile2);  //  读取坐标文件
    if(!fin2){
        cout << "fail to open";
    }
    while(!fin2.eof())   //  循环读取所有数据v
    {
        double time,tx,ty,tz,qx,qy,qz,qw;
        fin2 >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
        Isometry3d Twr(Quaterniond(qw,qx,qy,qz));   //  +四元数
        Twr.pretranslate(Vector3d(tx,ty,tz));   //  +T坐标
        poses2.push_back(Twr);   //  放到后边
    }
    cout << "read" << poses2.size() << "poses" << endl;

    DrawTrajectory2(poses1, poses2);
}
TrajectoryType ReadTrajectory(const string &path)
{
    TrajectoryType trajectory;
    ifstream fin(path);  //  读取坐标文件
    if(!fin){
        cout << "fail to open";
    }
    while(!fin.eof())   //  循环读取所有数据v
    {
        double time,tx,ty,tz,qx,qy,qz,qw;
        fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
        Sophus::SE3d p1(Eigen::Quaterniond(qx,qy,qz,qw),Eigen::Vector3d(tx,ty,tz));
        trajectory.push_back(p1);   //  放到后边
    }
    return trajectory;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262

CMakeLists.txt

cmake_minimum_required(VERSION 3.0)
project(test_lie)
find_package(Sophus REQUIRED)
include_directories("/usr/include/eigen3")
add_executable(test_lie main.cpp)
target_link_libraries(test_lie Sophus::Sophus)

find_package(Pangolin REQUIRED)
include_directories(${Pangolin_LIBRARIES})
target_link_libraries(test_lie ${Pangolin_LIBRARIES})
1
2
3
4
5
6
7
8
9
10