点云地图构建及定位

• 1. 回环检测
• • 1.1 基于Scan Context
  • 1.2 基于直方图
• 2. 后端优化
• • 2.1 后端优化基本原理
  • 2.2 李群、李代数基本知识
  • 2.3 李群、李代数基本知识
  • • 2.3.1 SO(3)\mathrm{SO}(3)SO(3) 对应的 BCH\mathrm{BCH}BCH 公式
    • 2.3.2 SE(3)\mathrm{SE}(3)SE(3) 对应的 BCH\mathrm{BCH}BCH 公式
  • 2.4 伴随矩阵
  • 2.3 基于回环的位姿修正
  • 2.4 基于先验观测的位姿修正
• 3. 点云地图建立
• • 3.1 整体流程
• 4. 基于地图的定位
• • 4.1 整体流程
  • 4.2 位姿初始化
  • • 4.2.1 整体流程
    • 4.2.2 位姿初始化
• 5. LeGO-LOAM介绍
• • 5.1 主要特点
  • 5.2 特征提取
  • 5.3 位姿解算
  • 5.4 回环检测与修正

前面做出了一个比较好的里程计，但是在实际项目使用的时候，都是基于地图的。原因如下：

1. 基于地图有先验的信息，地图的质量是经过检查了的，它没有太多的不可控在里面。而里程计有很多的干扰因素在里面，会导致里程计的不稳定。
2. 地图更多是一个全局优化得到的结果，误差相比于里程计会更小。

建图流程如下：

1. 回环检测

• 目的：消除累计误差，并提高地图一致性。
• 方法：从历史帧中找出相似帧（即物理世界中相近位置且有充足共视区域的帧），并给出二者的相对位姿。(注意，不需要完全重合)
• 用法：在后端优化中，作为一个相对位姿约束的边，加入图优化系统中。

目前比较可靠的方法还是ICP和NDT两个比较基础的方法（小范围可以使用里程计确定是不是在之前帧周边，大范围就需要用到GPS了）。

1.1 基于Scan Context

论文：Scan Context: Egocentric Spatial Descriptor for Place Recognition within 3D Point Cloud Map

基本思想为，将一圈点云分成很多的扇形，每个扇形在纵向上再切割成小段。三维点云展开，就成了二维数组。下次再走到这里的时候，根据二维数组的匹配相似性，就能判断是不是同一区域了。
缺点：旋转可以计算，但是平移计算不准。

应用案例： LeGO-LOAM + scan context 闭环检测

1.2 基于直方图

论文：[A fast, complete, point cloud based loop closure for LiDAR odometry and mapping]
应用案例：loam_livox

将点云按三维网格划分，每个网格求特征值，会有三个特征值。如果将每帧点云跟之前帧匹配计算相似性，可以大致判断出来是不是走到了同一位置。

2. 后端优化

2.1 后端优化基本原理

目的：利用 回环检测结果 和 惯导先验位姿 修正里程计误差，得到一个高质量的地图。回环在此处提供的是两帧之间的相对位姿。
观测：

1. 连续两帧的相对位姿观测；
2. 回环匹配得到的相对位姿观测；
3. 组合导航提供的先验位姿观测。

使用方式：
a. 1)和2)的观测构成了基于回环的位姿修正(LeGO-LOAM)；
b. 1)和3)的观测构成了基于先验观测的位姿修正；
c. 1) 2) 3) 也可以同时使用。

2.2 李群、李代数基本知识

常用转换：

• R=exp⁡(ϕ∧)R=\exp \left(\boldsymbol{\phi}^{\wedge}\right)R=exp(ϕ∧)
  R−1=exp⁡((−ϕ)∧)R^{-1}=\exp \left((-\boldsymbol{\phi})^{\wedge}\right)R−1=exp((−ϕ)∧)
  ϕ=ln⁡(exp⁡(ϕ∧))∨=ln⁡(R)∨\boldsymbol{\phi}=\ln \left(\exp \left(\boldsymbol{\phi}^{\wedge}\right)\right)^{\vee}=\ln (R)^{\vee}ϕ=ln(exp(ϕ∧))∨=ln(R)∨
• T=exp⁡(ξ∧)T=\exp \left(\boldsymbol{\xi}^{\wedge}\right)T=exp(ξ∧)
  T−1=exp⁡((−ξ)∧)T^{-1}=\exp \left((-\boldsymbol{\xi})^{\wedge}\right)T−1=exp((−ξ)∧)
  ξ=ln⁡(exp⁡(ξ∧))∨=ln⁡(T)∨\boldsymbol{\xi}=\ln \left(\exp \left(\boldsymbol{\xi}^{\wedge}\right)\right)^{\vee}=\ln (T)^{\vee}ξ=ln(exp(ξ∧))∨=ln(T)∨

2.3 李群、李代数基本知识

在推导里永远无法避免使用 BCH 公式：
ln⁡(exp⁡(A)exp⁡(B))=A+B+12[A,B]+112[A,[A,B]]−112[B,[A,B]]+⋯\ln (\exp (A) \exp (B))=A+B+\frac{1}{2}[A, B]+\frac{1}{12}[A,[A, B]]-\frac{1}{12}[B,[A, B]]+\cdots ln(exp(A)exp(B))=A+B+21​[A,B]+121​[A,[A,B]]−121​[B,[A,B]]+⋯这个公式用来解决一个问题：
已知 T=T1⋅T2T=T_1\cdot T_2T=T1​⋅T2​，又知道 T=exp⁡(ξ∧)T=\exp \left(\boldsymbol{\xi}^{\wedge}\right)T=exp(ξ∧)，那么 exp⁡(ξ1∧)exp⁡(ξ2∧)\exp \left(\boldsymbol{\xi_1}^{\wedge}\right)\exp \left(\boldsymbol{\xi_2}^{\wedge}\right)exp(ξ1​∧)exp(ξ2​∧) 这两项相乘能得到什么呢，或者说，这个 TTT 所对应的李代数，是什么呢？

BCH公式，就是解决的这个问题。这里公式中使用AAA和BBB，是因为这两下既可能是在so(3)下的表达，又可能是在se(3)se(3)se(3)下的表达。这里使用 ln⁡(exp⁡(A)exp⁡(B))\ln (\exp (A) \exp (B))ln(exp(A)exp(B))，就是求它对应的李代数。可以看出来，公式不是只有A+BA+BA+B，而是多了后面的几项。

• SO(3)\mathrm{SO}(3)SO(3) 下的李括号定义为：
  [ϕ1,ϕ2]=(Φ1Φ2−Φ2Φ1)∨\left[\boldsymbol{\phi}_1, \boldsymbol{\phi}_2\right]=\left(\Phi_1 \Phi_2-\Phi_2 \Phi_1\right)^{\vee} [ϕ1​,ϕ2​]=(Φ1​Φ2​−Φ2​Φ1​)∨其中，
  Φ=ϕ∧=[0−ϕ(3)ϕ(2)ϕ(3)0−ϕ(1)−ϕ(2)ϕ(1)0]\Phi=\phi^{\wedge}=\left[\begin{array}{ccc} 0 & -\phi(3) & \phi(2) \\ \phi(3) & 0 & -\phi(1) \\ -\phi(2) & \phi(1) & 0 \end{array}\right] Φ=ϕ∧=​0ϕ(3)−ϕ(2)​−ϕ(3)0ϕ(1)​ϕ(2)−ϕ(1)0​​

• SE(3)\mathrm{SE}(3)SE(3) 下的李括号定义为：
  [ξ1,ξ2]=(ξ1∧ξ2∧−ξ2∧ξ1∧)∨\left[\boldsymbol{\xi}_1, \boldsymbol{\xi}_2\right]=\left(\boldsymbol{\xi}_1^{\wedge} \boldsymbol{\xi}_2^{\wedge}-\boldsymbol{\xi}_2^{\wedge} \boldsymbol{\xi}_1^{\wedge}\right)^{\vee} [ξ1​,ξ2​]=(ξ1∧​ξ2∧​−ξ2∧​ξ1∧​)∨

但是很显然，这个BCH公式太复杂了，那么我们有没有简化的方式来解决它呢？这里根据 so(3) 和 se(3)，给出了两个简化的表达。

2.3.1 SO(3)\mathrm{SO}(3)SO(3) 对应的 BCH\mathrm{BCH}BCH 公式

(小量可以想像成，第kkk帧和第k+1k+1k+1帧的相对旋转，而另一个即为坐标系下当前角度)
ln⁡(exp⁡(ϕ1∧)exp⁡(ϕ2∧))∨≈{Jl(ϕ2)−1ϕ1+ϕ2, 当 ϕ1为小量 Jr(ϕ1)−1ϕ2+ϕ1, 当 ϕ2为小量 \ln \left(\exp \left(\boldsymbol{\phi}_1^{\wedge}\right) \exp \left(\phi_2^{\wedge}\right)\right)^{\vee} \approx\left\{\begin{array}{l} J_l\left(\boldsymbol{\phi}_2\right)^{-1} \boldsymbol{\phi}_1+\boldsymbol{\phi}_2 \text {, 当 } \boldsymbol{\phi}_1 \text { 为小量 } \\ J_r\left(\boldsymbol{\phi}_1\right)^{-1} \boldsymbol{\phi}_2+\boldsymbol{\phi}_1 \text {, 当 } \boldsymbol{\phi}_2 \text { 为小量 } \end{array}\right. ln(exp(ϕ1∧​)exp(ϕ2∧​))∨≈{Jl​(ϕ2​)−1ϕ1​+ϕ2​, 当 ϕ1​ 为小量 Jr​(ϕ1​)−1ϕ2​+ϕ1​, 当 ϕ2​ 为小量 ​其中左乘雅可比为：(左乘意味着在左边乘上一个小旋转)
Jl=sin⁡θθI+(1−sin⁡θθ)aaT+1−cos⁡θθa∧J_l=\frac{\sin \theta}{\theta} I+\left(1-\frac{\sin \theta}{\theta}\right) a a^{\mathrm{T}}+\frac{1-\cos \theta}{\theta} a^{\wedge} Jl​=θsinθ​I+(1−θsinθ​)aaT+θ1−cosθ​a∧即：
Jl−1=θ2cot⁡θ2I+(1−θ2cot⁡θ2)aaT−θ2a∧J_l^{-1}=\frac{\theta}{2} \cot \frac{\theta}{2} I+\left(1-\frac{\theta}{2} \cot \frac{\theta}{2}\right) a a^T-\frac{\theta}{2} a^{\wedge} Jl−1​=2θ​cot2θ​I+(1−2θ​cot2θ​)aaT−2θ​a∧右乘雅可比仅需要在左乘雅可比的基础上对自变量取负号，即：
Jr(ϕ)=Jl(−ϕ)J_r(\boldsymbol{\phi})=J_l(-\boldsymbol{\phi}) Jr​(ϕ)=Jl​(−ϕ)

2.3.2 SE(3)\mathrm{SE}(3)SE(3) 对应的 BCH\mathrm{BCH}BCH 公式

ln⁡(exp⁡(ξ1∧)exp⁡(ξ2∧))∨≈{Jℓ(ξ2)−1ξ1+ξ2，当 ξ1为小量 Jr(ξ1)−1ξ2+ξ1，当 ξ2为小量 \ln \left(\exp \left(\boldsymbol{\xi}_1^{\wedge}\right) \exp \left(\boldsymbol{\xi}_2^{\wedge}\right)\right)^{\vee} \approx\left\{\begin{array}{l} \mathcal{J}_{\ell}\left(\boldsymbol{\xi}_2\right)^{-1} \boldsymbol{\xi}_1+\boldsymbol{\xi}_2 \text { ，当 } \boldsymbol{\xi}_1 \text { 为小量 } \\ \mathcal{J}_r\left(\boldsymbol{\xi}_1\right)^{-1} \boldsymbol{\xi}_2+\boldsymbol{\xi}_1 \text { ，当 } \boldsymbol{\xi}_2 \text { 为小量 } \end{array}\right. ln(exp(ξ1∧​)exp(ξ2∧​))∨≈{Jℓ​(ξ2​)−1ξ1​+ξ2​ ，当 ξ1​ 为小量 Jr​(ξ1​)−1ξ2​+ξ1​ ，当 ξ2​ 为小量 ​由于SE(3)上的雅可比形式过于复杂，此处直接给出本章所用到的近似形式如下。详细内容可参考《机器人中的状态估计》 中公式7.83的推导过程。(JlJ_lJl​更复杂)
Jr(ξ)−1≈I+12[ϕ∧ρ∧0ϕ∧]\mathcal{J}_r(\boldsymbol{\xi})^{-1} \approx I+\frac{1}{2}\left[\begin{array}{cc} \boldsymbol{\phi}^{\wedge} & \boldsymbol{\rho}^{\wedge} \\ 0 & \boldsymbol{\phi}^{\wedge} \end{array}\right] Jr​(ξ)−1≈I+21​[ϕ∧0​ρ∧ϕ∧​]若 ξ1\boldsymbol{\xi}_1ξ1​ 和 ξ2\boldsymbol{\xi}_2ξ2​ 非常小，则左右雅可比均可以直接约等于单位阵，即Jr(ξ)−1≈I\mathcal{J}_r(\boldsymbol{\xi})^{-1} \approx IJr​(ξ)−1≈I，此时有
exp⁡(ξ1∧)exp⁡(ξ2∧)∨≈exp⁡(ξ1∧+ξ1∧)∨\exp \left(\boldsymbol{\xi}_1^{\wedge}\right) \exp \left(\boldsymbol{\xi}_2^{\wedge}\right)^{\vee} \approx \exp \left(\boldsymbol{\xi}_1^{\wedge}+\boldsymbol{\xi}_1^{\wedge}\right)^{\vee} exp(ξ1∧​)exp(ξ2∧​)∨≈exp(ξ1∧​+ξ1∧​)∨

2.4 伴随矩阵

SO(3)\mathrm{SO}(3)SO(3) 上的伴随性质：
exp⁡(p∧)R=Rexp⁡((R−1p)∧)\exp \left(p^{\wedge}\right) R=R \exp \left(\left(R^{-1} p\right)^{\wedge}\right) exp(p∧)R=Rexp((R−1p)∧)SE(3)S E(3)SE(3) 上的伴随性质：
exp⁡(ξ∧)T=Texp⁡((Ad⁡(T−1)ξ)∧)\exp \left(\xi^{\wedge}\right) T=T \exp \left(\left(\operatorname{Ad}\left(T^{-1}\right) \xi\right)^{\wedge}\right) exp(ξ∧)T=Texp((Ad(T−1)ξ)∧)其中伴随矩阵的定义如下：
Ad⁡(T)=[Rt∧R0R]\operatorname{Ad}(T)=\left[\begin{array}{cc} R & t^{\wedge} R \\ 0 & R \end{array}\right] Ad(T)=[R0​t∧RR​]

伴随矩阵的目的是，假设我们有exp⁡(p∧)R\exp \left(p^{\wedge}\right) Rexp(p∧)R，想对ppp求偏导，但是ppp在左边是很难求偏导的，转到右边之后，偏导就比较容易求了。

2.3 基于回环的位姿修正

位姿图优化是把所有的观测和状态放在一起优化，残差项是前面所讲的残差项的总和。在实际使用中，各残差会被分配一个权重，也就是信息矩阵，它相当于对残差进行加权。考虑信息矩阵后，总的残差项可以表示为：F(x)=∑⟨i,j⟩∈CeijTΩijeij⏟Fij\mathbf{F}(\mathbf{x})=\sum_{\langle i, j\rangle \in \mathcal{C}} \underbrace{\mathbf{e}_{i j}^T \boldsymbol{\Omega}_{i j} \mathbf{e}_{i j}}_{\mathbf{F}_{i j}}F(x)=∑⟨i,j⟩∈C​Fij​eijT​Ωij​eij​​​

此时优化问题可以表示为：x∗=argmin⁡xF(x)\mathbf{x}^*=\underset{\mathbf{x}}{\operatorname{argmin}} \mathbf{F}(\mathbf{x})x∗=xargmin​F(x)

第 i\mathrm{i}i 帧和第 j\mathrm{j}j 帧之间的相对位姿，在李群SE(3)上可以表示为：Tij=Ti−1Tj\boldsymbol{T}_{i j}=\boldsymbol{T}_i^{-1} \boldsymbol{T}_jTij​=Ti−1​Tj​
也可以在李代数上表示为：
ξij=ln⁡(Ti−1Tj)∨=ln⁡(exp⁡((−ξi)∧)exp⁡(ξj∧))∨\begin{aligned} \boldsymbol{\xi}_{i j} & =\ln \left(\boldsymbol{T}_i^{-1} \boldsymbol{T}_j\right)^{\vee} \\ & =\ln \left(\exp \left(\left(-\boldsymbol{\xi}_i\right)^{\wedge}\right) \exp \left(\boldsymbol{\xi}_j^{\wedge}\right)\right)^{\vee} \end{aligned} ξij​​=ln(Ti−1​Tj​)∨=ln(exp((−ξi​)∧)exp(ξj∧​))∨​若位姿没有误差，则上面两个式子是精确相等的，但当位姿有误差存在时，便可以使用等式的左右两端计算残差项。(也就是，第 i\mathrm{i}i 帧和第 j\mathrm{j}j 帧之间的相对位姿，要添加一个什么样的转换才能到TijT_{ij}Tij​，要是相等，这里就是一个单位阵III)
eij=ln⁡(Tij−1Ti−1Tj)∨=ln⁡(exp⁡((−ξij)∧)exp⁡((−ξi)∧)exp⁡(ξj∧))∨\begin{aligned} \boldsymbol{e}_{i j} & =\ln \left(\boldsymbol{T}_{i j}^{-1} \boldsymbol{T}_i^{-1} \boldsymbol{T}_j\right)^{\vee} \\ & =\ln \left(\exp \left(\left(-\boldsymbol{\xi}_{i j}\right)^{\wedge}\right) \exp \left(\left(-\boldsymbol{\xi}_i\right)^{\wedge}\right) \exp \left(\boldsymbol{\xi}_j^{\wedge}\right)\right)^{\vee} \end{aligned} eij​​=ln(Tij−1​Ti−1​Tj​)∨=ln(exp((−ξij​)∧)exp((−ξi​)∧)exp(ξj∧​))∨​位姿图优化的思想是通过调整状态量(即位姿)，使残差项的值最小化，这就需要用残差项对位姿求雅可比，才能使用高斯牛顿方法进行优化。(需要对矩阵求导了)

求雅可比的方式是对位姿添加扰动，此时残差表示为：
e^ij=ln⁡(Tij−1Ti−1exp⁡((−δξi)∧)exp⁡(δξj∧)Tj)∨\hat{\boldsymbol{e}}_{i j}=\ln \left(\boldsymbol{T}_{i j}^{-1} \boldsymbol{T}_i^{-1} \exp \left(\left(-\boldsymbol{\delta} \boldsymbol{\xi}_i\right)^{\wedge}\right) \exp \left(\delta \boldsymbol{\xi}_j^{\wedge}\right) \boldsymbol{T}_j\right)^{\vee} e^ij​=ln(Tij−1​Ti−1​exp((−δξi​)∧)exp(δξj∧​)Tj​)∨这里的扰动都是左扰动，Tj\boldsymbol{T}_jTj​ 的扰动是 exp⁡(δξj∧)\exp \left(\delta \boldsymbol{\xi}_j^{\wedge}\right)exp(δξj∧​)，而Ti\boldsymbol{T}_iTi​的在右边是因为这里是求的它的逆。

进一步对前面的式子进行化简：
e^ij=ln⁡(Tij−1Ti−1exp⁡((−δξi)∧)exp⁡(δξj∧)Tj)∨=(1)ln⁡(Tij−1Ti−1exp⁡((−δξi)∧)Tjexp⁡((Ad⁡(Tj−1)δξj)∧)∨=(2)ln⁡(Tij−1Ti−1Tjexp⁡((−Ad⁡(Tj−1)δξi)∧)exp⁡((Ad⁡(Tj−1)δξj)∧)∨≈(3)ln⁡(Tij−1Ti−1Tjexp⁡((−Ad⁡(Tj−1)δξi)∧+(Ad⁡(Tj−1)δξj)∧)∨=(4)ln⁡(exp⁡(eij∧)exp⁡((−Ad⁡(Tj−1)δξi)∧+(Ad⁡(Tj−1)δξj)∧)∨≈(5)eij−Jr−1(eij)Ad⁡(Tj−1)δξi+Jr−1(eij)Ad⁡(Tj−1)δξj\begin{aligned} \hat{e}_{i j}&=\ln \left(\boldsymbol{T}_{i j}^{-1} \boldsymbol{T}_i^{-1} \exp \left(\left(-\delta \boldsymbol{\xi}_i\right)^{\wedge}\right) \exp \left(\delta \boldsymbol{\xi}_j^{\wedge}\right) \boldsymbol{T}_j\right)^{\vee} \\ &\stackrel{(1)}{=} \ln \left(\boldsymbol{T}_{i j}^{-1} \boldsymbol{T}_i^{-1} \exp \left(\left(-\delta \boldsymbol{\xi}_i\right)^{\wedge}\right) \boldsymbol{T}_j \exp \left(\left(\operatorname{Ad}\left(\boldsymbol{T}_j^{-1}\right) \delta \boldsymbol{\xi}_j\right)^{\wedge}\right)^{\vee}\right. \\ &\stackrel{(2)}{=} \ln \left(\boldsymbol{T}_{i j}^{-1} \boldsymbol{T}_i^{-1} \boldsymbol{T}_j \exp \left(\left(-\operatorname{Ad}\left(\boldsymbol{T}_j^{-1}\right) \delta \boldsymbol{\xi}_i\right)^{\wedge}\right) \exp \left(\left(\operatorname{Ad}\left(\boldsymbol{T}_j^{-1}\right) \boldsymbol{\delta} \boldsymbol{\xi}_j\right)^{\wedge}\right)^{\vee}\right. \\ &\stackrel{(3)}{\approx} \ln \left(\boldsymbol{T}_{i j}^{-1} \boldsymbol{T}_i^{-1} \boldsymbol{T}_j \exp \left(\left(-\operatorname{Ad}\left(\boldsymbol{T}_j^{-1}\right) \delta \boldsymbol{\xi}_i\right)^{\wedge}+\left(\operatorname{Ad}\left(\boldsymbol{T}_j^{-1}\right) \delta \boldsymbol{\xi}_j\right)^{\wedge}\right)^{\vee}\right. \\ &\stackrel{(4)}{=} \ln \left(\exp \left(\boldsymbol{e}_{i j}^{\wedge}\right) \exp \left(\left(-\operatorname{Ad}\left(\boldsymbol{T}_j^{-1}\right) \delta \boldsymbol{\xi}_i\right)^{\wedge}+\left(\operatorname{Ad}\left(\boldsymbol{T}_j^{-1}\right) \delta \boldsymbol{\xi}_j\right)^{\wedge}\right)^{\vee}\right. \\ &\stackrel{(5)}{\approx} \boldsymbol{e}_{i j}-\mathcal{J}_r^{-1}\left(e_{i j}\right) \operatorname{Ad}\left(\boldsymbol{T}_j^{-1}\right) \delta \boldsymbol{\xi}_i+\mathcal{J}_r^{-1}\left(e_{i j}\right) \operatorname{Ad}\left(\boldsymbol{T}_j^{-1}\right) \boldsymbol{\delta} \boldsymbol{\xi}_j \end{aligned} e^ij​​=ln(Tij−1​Ti−1​exp((−δξi​)∧)exp(δξj∧​)Tj​)∨=(1)ln(Tij−1​Ti−1​exp((−δξi​)∧)Tj​exp((Ad(Tj−1​)δξj​)∧)∨=(2)ln(Tij−1​Ti−1​Tj​exp((−Ad(Tj−1​)δξi​)∧)exp((Ad(Tj−1​)δξj​)∧)∨≈(3)ln(Tij−1​Ti−1​Tj​exp((−Ad(Tj−1​)δξi​)∧+(Ad(Tj−1​)δξj​)∧)∨=(4)ln(exp(eij∧​)exp((−Ad(Tj−1​)δξi​)∧+(Ad(Tj−1​)δξj​)∧)∨≈(5)eij​−Jr−1​(eij​)Ad(Tj−1​)δξi​+Jr−1​(eij​)Ad(Tj−1​)δξj​​

首先要将两个exp放到最右边去：
(1) 三维变换下的伴随性质，先将 exp⁡(δξj∧)\exp \left(\delta \boldsymbol{\xi}_j^{\wedge}\right)exp(δξj∧​) 移到右边去；
(2) 三维变换下的伴随性质，这次是将 exp⁡(δξi∧)\exp \left(\delta \boldsymbol{\xi}_i^{\wedge}\right)exp(δξi∧​) 移到右边去；
(3) BCH\mathrm{BCH}BCH 公式，且是两个李代数都比较小情况下的近似。就是两个李代数对应都非常小的时候，可以使用exp⁡(ξ1∧)exp⁡(ξ2∧)∨≈exp⁡(ξ1∧+ξ1∧)∨\exp \left(\boldsymbol{\xi}_1^{\wedge}\right) \exp \left(\boldsymbol{\xi}_2^{\wedge}\right)^{\vee} \approx \exp \left(\boldsymbol{\xi}_1^{\wedge}+\boldsymbol{\xi}_1^{\wedge}\right)^{\vee}exp(ξ1∧​)exp(ξ2∧​)∨≈exp(ξ1∧​+ξ1∧​)∨的形式。
(4) 残差的定义，即 eij=ln⁡(Tij−1Ti−1Tj)∨\boldsymbol{e}_{i j} =\ln \left(\boldsymbol{T}_{i j}^{-1} \boldsymbol{T}_i^{-1} \boldsymbol{T}_j\right)^{\vee}eij​=ln(Tij−1​Ti−1​Tj​)∨
(5) BCH 公式

上面的式子表明，残差关于 TiT_iTi​ 的雅可比为：
Aij=∂eij∂δξi=−Jr−1(eij)Ad⁡(Tj−1)A_{i j}=\frac{\partial \boldsymbol{e}_{i j}}{\partial \boldsymbol{\delta} \boldsymbol{\xi}_i}=-\mathcal{J}_r^{-1}\left(\boldsymbol{e}_{i j}\right) \operatorname{Ad}\left(\boldsymbol{T}_j^{-1}\right) Aij​=∂δξi​∂eij​​=−Jr−1​(eij​)Ad(Tj−1​)残差关于 TjT_jTj​ 的雅可比为：
Bij=∂eij∂δξj=Jr−1(eij)Ad⁡(Tj−1)B_{i j}=\frac{\partial \boldsymbol{e}_{i j}}{\partial \boldsymbol{\delta} \boldsymbol{\xi}_j}=\mathcal{J}_r^{-1}\left(\boldsymbol{e}_{i j}\right) \operatorname{Ad}\left(\boldsymbol{T}_j^{-1}\right) Bij​=∂δξj​∂eij​​=Jr−1​(eij​)Ad(Tj−1​)其中：
Jr−1(eij)≈I+12[ϕe∧ρe∧0ϕe∧]\mathcal{J}_r^{-1}\left(\boldsymbol{e}_{i j}\right) \approx I+\frac{1}{2}\left[\begin{array}{cc} \boldsymbol{\phi}_e^{\wedge} & \boldsymbol{\rho}_e^{\wedge} \\ 0 & \boldsymbol{\phi}_e^{\wedge} \end{array}\right] Jr−1​(eij​)≈I+21​[ϕe∧​0​ρe∧​ϕe∧​​]

现在有了雅克比了，剩下的就是高斯牛顿的流程了。
按照高斯牛顿法的流程，需要对残差进行一阶泰勒展开， 即求雅可比：
eij(xi+Δxi,xj+Δxj)=eij(x+Δx)≈eij+JijΔx\begin{array}{l} \mathbf{e}_{i j}\left(\mathbf{x}_i+\Delta \mathbf{x}_i, \mathbf{x}_j+\Delta \mathbf{x}_j\right) \\ =\mathbf{e}_{i j}(\mathbf{x}+\Delta \mathbf{x}) \\ \approx \mathbf{e}_{i j}+\mathbf{J}_{i j} \Delta \mathbf{x} \end{array} eij​(xi​+Δxi​,xj​+Δxj​)=eij​(x+Δx)≈eij​+Jij​Δx​
其中 Jij\mathbf{J}_{i j}Jij​ 即为前面推导的残差关于位姿的雅可比组成的矩阵：
Jij=(0⋯0Aij⏟node i0⋯0Bij⏟node j0⋯0)\mathbf{J}_{i j}=(0 \cdots 0 \underbrace{\mathbf{A}_{i j}}_{\text {node } i} \mathbf{0} \cdots 0 \underbrace{\mathbf{B}_{i j}}_{\text {node } j} \mathbf{0} \cdots 0) Jij​=(0⋯0node iAij​​​0⋯0node jBij​​​0⋯0)对于每一个残差块，便有：
Fij(x+Δx)=eij(x+Δx)TΩijeij(x+Δx)≈(eij+JijΔx)TΩij(eij+JijΔx)=eijTΩijeij⏟cij+2eijTΩijJij⏟bijTΔx+ΔxTJijTΩijJij⏟HijΔx=cij+2bijTΔx+ΔxTHijΔx\begin{array}{l} \mathbf{F}_{i j}(\mathbf{x}+\Delta \mathbf{x}) \\ =\mathbf{e}_{i j}(\mathbf{x}+\Delta \mathbf{x})^T \Omega_{i j} \mathbf{e}_{i j}(\mathbf{x}+\Delta \mathbf{x}) \\ \approx\left(\mathbf{e}_{i j}+\mathbf{J}_{i j} \Delta \mathbf{x}\right)^T \Omega_{i j}\left(\mathbf{e}_{i j}+\mathbf{J}_{i j} \Delta \mathbf{x}\right) \\ =\underbrace{\mathbf{e}_{i j}^T \Omega_{i j} \mathbf{e}_{i j}}_{c_{i j}}+2 \underbrace{\mathbf{e}_{i j}^T \Omega_{i j} \mathbf{J}_{i j}}_{\mathbf{b}_{i j}^T} \Delta \mathbf{x}+\Delta \mathbf{x}^T \underbrace{\mathbf{J}_{i j}^T \Omega_{i j} \mathbf{J}_{i j}}_{\mathbf{H}_{i j}} \Delta \mathbf{x} \\ =c_{i j}+2 \mathbf{b}_{i j}^T \Delta \mathbf{x}+\Delta \mathbf{x}^T \mathbf{H}_{i j} \Delta \mathbf{x} \\ \end{array} Fij​(x+Δx)=eij​(x+Δx)TΩij​eij​(x+Δx)≈(eij​+Jij​Δx)TΩij​(eij​+Jij​Δx)=cij​eijT​Ωij​eij​​​+2bijT​eijT​Ωij​Jij​​​Δx+ΔxTHij​JijT​Ωij​Jij​​​Δx=cij​+2bijT​Δx+ΔxTHij​Δx​其中：
Hij=(⋱AijTΩijAij⋯AijTΩijBij⋮⋱⋮BijTΩijAij⋯BijTΩijBij⋱)bij=(⋮AijTΩijeij⋮BijTΩijeij⋮)\mathbf{H}_{i j}=\left(\begin{array}{cccc} \ddots & & & \\ & \mathbf{A}_{i j}^T \Omega_{i j} \mathrm{~A}_{i j} & \cdots & \mathbf{A}_{i j}^T \Omega_{i j} \mathrm{~B}_{i j} \\ \vdots & \ddots & \vdots & \\ & \mathrm{B}_{i j}^T \Omega_{i j} \mathrm{~A}_{i j} & \cdots & \mathrm{B}_{i j}^T \Omega_{i j} \mathrm{~B}_{i j} \\ \ddots & & & \end{array}\right) \quad \mathrm{b}_{i j}=\left(\begin{array}{c} \vdots \\ \mathrm{A}_{i j}^T \Omega_{i j} \mathrm{e}_{i j} \\ \vdots \\ \mathrm{B}_{i j}^T \Omega_{i j} \mathrm{e}_{i j} \\ \vdots \end{array}\right) Hij​=​⋱⋮⋱​AijT​Ωij​ Aij​⋱BijT​Ωij​ Aij​​⋯⋮⋯​AijT​Ωij​ Bij​BijT​Ωij​ Bij​​​bij​=​⋮AijT​Ωij​eij​⋮BijT​Ωij​eij​⋮​​
此后便可以使用高斯牛顿法进行优化。

2.4 基于先验观测的位姿修正

先验观测是一元边，它不像前面所述的帧间观测连接两个 位姿状态，而是只连接一个位姿状态量，它直接给出的就 是该状态量的观测值，因此它对应的残差就是观测值与状 态量之间的差异，即
ei=ln⁡(Zi−1Ti)∨=ln⁡(exp⁡((−ξzi)∧)exp⁡(ξi∧))∨\begin{aligned} \boldsymbol{e}_i & =\ln \left(\boldsymbol{Z}_i^{-1} \boldsymbol{T}_i\right)^{\vee} \\ & =\ln \left(\exp \left(\left(-\boldsymbol{\xi}_{z i}\right)^{\wedge}\right) \exp \left(\boldsymbol{\xi}_i^{\wedge}\right)\right)^{\vee} \end{aligned} ei​​=ln(Zi−1​Ti​)∨=ln(exp((−ξzi​)∧)exp(ξi∧​))∨​
对残差添加扰动，可得
e^i=ln⁡(Zi−1exp⁡(δξi∧)Ti)∨\hat{\boldsymbol{e}}_i=\ln \left(\boldsymbol{Z}_i^{-1} \exp \left(\delta \boldsymbol{\xi}_i^{\wedge}\right) \boldsymbol{T}_i\right)^{\vee} e^i​=ln(Zi−1​exp(δξi∧​)Ti​)∨
利用伴随性质和 BCH\mathrm{BCH}BCH 公式进行化简，可得
e^i=ln⁡(Zi−1Tiexp⁡((Ad⁡(Ti−1)δξi)∧))∨=ln⁡(exp⁡(ei∧)exp⁡((Ad⁡(Ti−1)δξi)∧))∨≈ei+Jr−1(ei)Ad⁡(Ti−1)δξi\begin{aligned} \hat{\boldsymbol{e}}_i & =\ln \left(\boldsymbol{Z}_i^{-1} \boldsymbol{T}_i \exp \left(\left(\operatorname{Ad}\left(\boldsymbol{T}_i^{-1}\right) \delta \boldsymbol{\xi}_i\right)^{\wedge}\right)\right)^{\vee} \\ & =\ln \left(\exp \left(\boldsymbol{e}_i^{\wedge}\right) \exp \left(\left(\operatorname{Ad}\left(\boldsymbol{T}_i^{-1}\right) \delta \boldsymbol{\xi}_i\right)^{\wedge}\right)\right)^{\vee} \\ & \approx e_i+\mathcal{J}_r^{-1}\left(\boldsymbol{e}_i\right) \operatorname{Ad}\left(\boldsymbol{T}_i^{-1}\right) \delta \boldsymbol{\xi}_i \end{aligned} e^i​​=ln(Zi−1​Ti​exp((Ad(Ti−1​)δξi​)∧))∨=ln(exp(ei∧​)exp((Ad(Ti−1​)δξi​)∧))∨≈ei​+Jr−1​(ei​)Ad(Ti−1​)δξi​​因此残差关于 TiT_iTi​ 的雅可比为
∂ei∂δξi=Jr−1(ei)Ad⁡(Ti−1)\frac{\partial \boldsymbol{e}_i}{\partial \boldsymbol{\delta} \boldsymbol{\xi}_i}=\mathcal{J}_r^{-1}\left(\boldsymbol{e}_i\right) \operatorname{Ad}\left(\boldsymbol{T}_i^{-1}\right) ∂δξi​∂ei​​=Jr−1​(ei​)Ad(Ti−1​)其中：
Jr−1(ei)≈I+12[ϕe∧ρe∧0ϕe∧]\mathcal{J}_r^{-1}\left(e_i\right) \approx I+\frac{1}{2}\left[\begin{array}{cc} \phi_e^{\wedge} & \boldsymbol{\rho}_e^{\wedge} \\ 0 & \phi_e^{\wedge} \end{array}\right] Jr−1​(ei​)≈I+21​[ϕe∧​0​ρe∧​ϕe∧​​]后面的推导过程，便与相对位姿做观测的推导过程完全一致。

此外，部分场合提供的观测只有位置，没有姿态，比如只 有RTK，而没有组合导航，这里的残差便只剩下位置误差。 相应的雅可比公式，可自行推导。

3. 点云地图建立

3.1 整体流程

建图流程设计的核心原则是准确、高效地把里程计相对位姿、回环相对位姿、惯导先验位姿进行融合。

轨迹对齐：比如有个GPS轨迹和一个激光里程计，两个轨迹没有在一起，是因为初始的激光里程计是以单位阵为原点的，它与GPS之间没有产生过任何相对位姿联系。解决这个问题的方法是，把整个轨迹做调整，把激光里程计的轨迹旋转到GPS轨迹附近。
添加先验约束：如果没有检测到回环，需要添加GPS的先验位姿

4. 基于地图的定位

4.1 整体流程

在地图匹配中，鲁棒性和运行速度更加重要，因此实际使用中，基于NDT的匹配使用更广泛。

获取初始信息：假如有一个地图，这个地图很大，有可能在任何位置，需要有一个东西告诉你你在地图的哪个大致位置，如有GPS告诉我在一个位置，这时就需要把这附近一块区域的地图索引出来，索引出来后将当前雷达的点云和地图做匹配。为什么叫索引呢？因为不索引的话，就需要和整张地图做匹配了，这时不现实的，所以需要和局部地图做匹配，因此就加载一小块局部地图。

位姿初始化：在正式匹配之前，需要做一个初始化。第一次匹配前，需要多这么一个步骤。后面再做匹配时，就不需要这样做了，因为有了上一帧的位姿做参考。

更新局部地图：在地图上肯定是要沿着轨迹走的，上次加载的局部地图在一个位置，走到后面时，跟前面的局部地图就匹配不上了，因此需要拿另一块局部地图来做匹配。

4.2 位姿初始化

4.2.1 整体流程

由于NDT匹配需要较准确的初始位姿，因此在定位之前需要初始化环节，给出载体的初始位姿。
按照难度由低到高，常见的初始化需求有这样几种：

1. 已知位姿的初始化
   2）位置已知而姿态位置的初始化
   3）位置和姿态均未知的初始化

4.2.2 位姿初始化

由于NDT匹配需要较准确的初始位姿，因此在定位之前需要初始化环节，给出载体的初始位姿。

按照难度由低到高，常见的初始化需求有这样几种：
1）已知位姿的初始化
2）位置已知而姿态位置的初始化
3）位置和姿态均未知的初始化(非常有难度)

5. LeGO-LOAM介绍

论文：LeGO-LOAM: Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain

5.1 主要特点

1. 对地面做了分割，减小了特征搜索范围(环境中找线特征，地面找面特征)；
2. 提取特征之前做了聚类，提高了特征质量(很多曲率大的地方都是杂点，甚至树叶上的点都当成线特征来使用了)，这里删除掉了环境中比较杂乱的点，即不形成足够数量的点；
3. 以帧为单位进行优化，使得全局地图可以多次调整，而不像LOAM那样不可修改；
4. 增加了回环修正。

5.2 特征提取

1. 根据线与线之间的夹角，以及点的曲率，筛选出地面点。所有用于匹配的平面点仅使用地面点。
2. 在非地面点中，使用广度优先搜索(BFS)做聚类，聚类中点的数量大于30，才用来筛选线特征。
3. 筛选线特征方法，与LOAM中相同。

5.3 位姿解算

LeGO-LOAM 相较于之前的 LOAM 有个很重要的区别，它把6dof拆成了两个3dof。从理论上讲，面特征既然都提自于地面，那么它确实只能约束两个水平角和z向的距离；线特征也一样，它不在地面只分配在周围，而且大多数线特征都是竖直向上的，那它也确实只能约束航向角以及x、y的距离。这个理论上，从6dof拆成3dof和周围环境结合起来分析，它在精度上是没有损失的（或者说损失较少？），但是效率上会大幅提升。

1. 使用地面面特征优化高度和水平角；
2. 使用线特征优化水平位移和航向角。

注意：实际代码中，只有odom环节(scan-to-scan)使用了该模式，在mapping环节(scan-to-map)仍使用的是六自由度模型。

5.4 回环检测与修正

1. 回环检测使用的初始位姿，采用前端里程计位姿
2. 匹配采用 ICP 方法
3. 优化库采用 gtsam