多传感器融合定位五-点云地图构建及定位
多传感器融合定位五-点云地图构建及定位
- 1. 回环检测
- 1.1 基于Scan Context
- 1.2 基于直方图
- 2. 后端优化
- 2.1 后端优化基本原理
- 2.2 李群、李代数基本知识
- 2.3 李群、李代数基本知识
- 2.3.1 S O ( 3 ) \mathrm{SO}(3) SO(3) 对应的 B C H \mathrm{BCH} BCH 公式
- 2.3.2 S E ( 3 ) \mathrm{SE}(3) SE(3) 对应的 B C H \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 回环检测与修正
Reference:
- 深蓝学院-多传感器融合
- 多传感器融合定位理论基础
文章跳转：
- 多传感器融合定位一-3D激光里程计其一：ICP
- 多传感器融合定位二-3D激光里程计其二：NDT
- 多传感器融合定位三-3D激光里程计其三：点云畸变补偿
- 多传感器融合定位四-3D激光里程计其四：点云线面特征提取
- 多传感器融合定位五-点云地图构建及定位
- 多传感器融合定位六-惯性导航原理及误差分析
- 多传感器融合定位七-惯性导航解算及误差分析其一
- 多传感器融合定位八-惯性导航解算及误差分析其二
- 多传感器融合定位九-基于滤波的融合方法Ⅰ其一
- 多传感器融合定位十-基于滤波的融合方法Ⅰ其二
- 多传感器融合定位十一-基于滤波的融合方法Ⅱ
- 多传感器融合定位十二-基于图优化的建图方法其一
- 多传感器融合定位十三-基于图优化的建图方法其二
- 多传感器融合定位十四-基于图优化的定位方法
- 多传感器融合定位十五-多传感器时空标定(综述)
前面做出了一个比较好的里程计，但是在实际项目使用的时候，都是基于地图的。原因如下：
- 基于地图有先验的信息，地图的质量是经过检查了的，它没有太多的不可控在里面。而里程计有很多的干扰因素在里面，会导致里程计的不稳定。
- 地图更多是一个全局优化得到的结果，误差相比于里程计会更小。
建图流程如下：
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 后端优化基本原理
目的：利用 回环检测结果 和 惯导先验位姿 修正里程计误差(即开环里程计提供的相对位姿)，得到一个高质量的地图。回环在此处提供的是两帧之间的相对位姿。
观测：
- 连续两帧的相对位姿观测；
- 回环匹配得到的相对位姿观测；
- 组合导航提供的先验位姿观测。
使用方式：
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 + 1 2 [ A , B ] + 1 12 [ A , [ A , B ] ] − 1 12 [ 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 = T 1 ⋅ T 2 T=T_1\cdot T_2 T=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∧) 这两项相乘能得到什么呢，或者说，这个 T T T 所对应的李代数，是什么呢？
BCH公式，就是解决的这个问题。这里公式中使用 A A A 和 B B B，是因为这两下既可能是在so(3)下的表达，又可能是在 s e ( 3 ) se(3) se(3)下的表达。这里使用 ln ( exp ( A ) exp ( B ) ) \ln (\exp (A) \exp (B)) ln(exp(A)exp(B))，就是求它对应的李代数。可以看出来，公式不是只有 A + B A+B A+B，而是多了后面的几项。
S O ( 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 S E ( 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 S O ( 3 ) \mathrm{SO}(3) SO(3) 对应的 B C H \mathrm{BCH} BCH 公式
(小量可以想像成，第 k k k帧和第 k + 1 k+1 k+1帧的相对旋转，而另一个即为坐标系下当前角度)
ln ( exp ( ϕ 1 ∧ ) exp ( ϕ 2 ∧ ) ) ∨ ≈ { J l ( ϕ 2 ) − 1 ϕ 1 + ϕ 2 , 当 ϕ 1 为小量 J r ( ϕ 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 为小量 其中左乘雅可比为：(左乘意味着在左边乘上一个小旋转)
J l = sin θ θ I + ( 1 − sin θ θ ) a a T + 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∧即：
J l − 1 = θ 2 cot θ 2 I + ( 1 − θ 2 cot θ 2 ) a a T − θ 2 a ∧ 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∧右乘雅可比仅需要在左乘雅可比的基础上对自变量取负号，即：
J r ( ϕ ) = J l ( − ϕ ) J_r(\boldsymbol{\phi})=J_l(-\boldsymbol{\phi}) Jr(ϕ)=Jl(−ϕ)
2.3.2 S E ( 3 ) \mathrm{SE}(3) SE(3) 对应的 B C H \mathrm{BCH} BCH 公式
ln ( exp ( ξ 1 ∧ ) exp ( ξ 2 ∧ ) ) ∨ ≈ { J ℓ ( ξ 2 ) − 1 ξ 1 + ξ 2 ，当 ξ 1 为小量 J r ( ξ 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的推导过程。( J l J_l Jl更复杂)
J r ( ξ ) − 1 ≈ I + 1 2 [ ϕ ∧ ρ ∧ 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 非常小，则左右雅可比均可以直接约等于单位阵，即 J r ( ξ ) − 1 ≈ I \mathcal{J}_r(\boldsymbol{\xi})^{-1} \approx I Jr(ξ)−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 伴随矩阵
伴随矩阵的目的是，假设我们有式子 exp ( p ∧ ) R \exp \left(p^{\wedge}\right) R exp(p∧)R，想对 p p p 求偏导，但是 p p p 在公式左边是很难求偏导的，转到右边之后，偏导就比较容易求了。
S O ( 3 ) \mathrm{SO}(3) SO(3) 上的伴随性质：
exp ( p ∧ ) R = R exp ( ( R − 1 p ) ∧ ) \exp \left(p^{\wedge}\right) R=R \exp \left(\left(R^{-1} p\right)^{\wedge}\right) exp(p∧)R=Rexp((R−1p)∧) S E ( 3 ) S E(3) SE(3) 上的伴随性质：
exp ( ξ ∧ ) T = T exp ( ( 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 ) = [ R t ∧ R 0 R ] \operatorname{Ad}(T)=\left[\begin{array}{cc} R & t^{\wedge} R \\ 0 & R \end{array}\right] Ad(T)=[R0t∧RR]
2.3 基于回环的位姿修正
位姿图优化是把所有的观测和状态放在一起优化，残差项是前面所讲的残差项的总和。在实际使用中，各残差会被分配一个权重，也就是信息矩阵，它相当于对残差进行加权。考虑信息矩阵后，总的残差项可以表示为： F ( x ) = ∑ ⟨ i , j ⟩ ∈ C e i j T Ω i j e i j ⏟ F i j \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⟩∈CFij eijTΩijeij
此时优化问题可以表示为： x ∗ = argmin x F ( x ) \mathbf{x}^*=\underset{\mathbf{x}}{\operatorname{argmin}} \mathbf{F}(\mathbf{x}) x∗=xargminF(x)
第 i \mathrm{i} i 帧和第 j \mathrm{j} j 帧之间的相对位姿，在李群SE(3)上可以表示为： T i j = T i − 1 T j \boldsymbol{T}_{i j}=\boldsymbol{T}_i^{-1} \boldsymbol{T}_j Tij=Ti−1Tj
也可以在李代数上表示为：
ξ i j = ln ( T i − 1 T j ) ∨ = 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−1Tj)∨=ln(exp((−ξi)∧)exp(ξj∧))∨若位姿没有误差，则上面两个式子是精确相等的，但当位姿有误差存在时，便可以使用等式的左右两端计算残差项。(也就是，第 i \mathrm{i} i 帧和第 j \mathrm{j} j 帧之间的相对位姿，要添加一个什么样的转换才能到 T i j T_{ij} Tij，要是相等，这里就是一个单位阵 I I I)
e i j = ln ( T i j − 1 T i − 1 T j ) ∨ = ln ( exp ( ( − ξ i j ) ∧ ) 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−1Ti−1Tj)∨=ln(exp((−ξij)∧)exp((−ξi)∧)exp(ξj∧))∨位姿图优化的思想是通过调整状态量(即位姿)，使残差项的值最小化，这就需要用残差项对位姿求雅可比，才能使用高斯牛顿方法进行优化。(需要对矩阵求导了)
求雅可比的方式是对位姿添加扰动，此时残差表示为：
e ^ i j = ln ( T i j − 1 T i − 1 exp ( ( − δ ξ i ) ∧ ) exp ( δ ξ j ∧ ) T j ) ∨ \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−1Ti−1exp((−δξi)∧)exp(δξj∧)Tj)∨这里的扰动都是左扰动， T j \boldsymbol{T}_j Tj 的扰动是 exp ( δ ξ j ∧ ) \exp \left(\delta \boldsymbol{\xi}_j^{\wedge}\right) exp(δξj∧)，而 T i \boldsymbol{T}_i Ti的在右边是因为这里是求的它的逆。
进一步对前面的式子进行化简：
e ^ i j = ln ( T i j − 1 T i − 1 exp ( ( − δ ξ i ) ∧ ) exp ( δ ξ j ∧ ) T j ) ∨ = ( 1 ) ln ( T i j − 1 T i − 1 exp ( ( − δ ξ i ) ∧ ) T j exp ( ( Ad ( T j − 1 ) δ ξ j ) ∧ ) ∨ = ( 2 ) ln ( T i j − 1 T i − 1 T j exp ( ( − Ad ( T j − 1 ) δ ξ i ) ∧ ) exp ( ( Ad ( T j − 1 ) δ ξ j ) ∧ ) ∨ ≈ ( 3 ) ln ( T i j − 1 T i − 1 T j exp ( ( − Ad ( T j − 1 ) δ ξ i ) ∧ + ( Ad ( T j − 1 ) δ ξ j ) ∧ ) ∨ = ( 4 ) ln ( exp ( e i j ∧ ) exp ( ( − Ad ( T j − 1 ) δ ξ i ) ∧ + ( Ad ( T j − 1 ) δ ξ j ) ∧ ) ∨ ≈ ( 5 ) e i j − J r − 1 ( e i j ) Ad ( T j − 1 ) δ ξ i + J r − 1 ( e i j ) Ad ( T j − 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−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首先要将两个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) B C H \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) 残差的定义，即 e i j = ln ( T i j − 1 T i − 1 T j ) ∨ \boldsymbol{e}_{i j} =\ln \left(\boldsymbol{T}_{i j}^{-1} \boldsymbol{T}_i^{-1} \boldsymbol{T}_j\right)^{\vee} eij=ln(Tij−1Ti−1Tj)∨
(5) BCH 公式
上面的式子表明，残差关于 T i T_i Ti 的雅可比为：
A i j = ∂ e i j ∂ δ ξ i = − J r − 1 ( e i j ) Ad ( T j − 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)残差关于 T j T_j Tj 的雅可比为：
B i j = ∂ e i j ∂ δ ξ j = J r − 1 ( e i j ) Ad ( T j − 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)其中：
J r − 1 ( e i j ) ≈ I + 1 2 [ ϕ 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∧]现在有了雅克比了，剩下的就是高斯牛顿的流程了。
按照高斯牛顿法的流程，需要对残差进行一阶泰勒展开， 即求雅可比：
e i j ( x i + Δ x i , x j + Δ x j ) = e i j ( x + Δ x ) ≈ e i j + J i j Δ 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其中 J i j \mathbf{J}_{i j} Jij 即为前面推导的残差关于位姿的雅可比组成的矩阵：
J i j = ( 0 ⋯ 0 A i j ⏟ node i 0 ⋯ 0 B i j ⏟ node j 0 ⋯ 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 i Aij0⋯0node j Bij0⋯0)对于每一个残差块，便有：
F i j ( x + Δ x ) = e i j ( x + Δ x ) T Ω i j e i j ( x + Δ x ) ≈ ( e i j + J i j Δ x ) T Ω i j ( e i j + J i j Δ x ) = e i j T Ω i j e i j ⏟ c i j + 2 e i j T Ω i j J i j ⏟ b i j T Δ x + Δ x T J i j T Ω i j J i j ⏟ H i j Δ x = c i j + 2 b i j T Δ x + Δ x T H i j Δ 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Ωijeij(x+Δx)≈(eij+JijΔx)TΩij(eij+JijΔx)=cij eijTΩijeij+2bijT eijTΩijJijΔx+ΔxTHij JijTΩijJijΔx=cij+2bijTΔx+ΔxTHijΔx其中：
H i j = ( ⋱ A i j T Ω i j A i j ⋯ A i j T Ω i j B i j ⋮ ⋱ ⋮ B i j T Ω i j A i j ⋯ B i j T Ω i j B i j ⋱ ) b i j = ( ⋮ A i j T Ω i j e i j ⋮ B i j T Ω i j e i j ⋮ ) \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 BijBijTΩij Bij bij= ⋮AijTΩijeij⋮BijTΩijeij⋮ 此后便可以使用高斯牛顿法进行优化。
2.4 基于先验观测的位姿修正
先验观测是一元边，它不像前面所述的帧间观测连接两个位姿状态，而是只连接一个位姿状态量，它直接给出的就是该状态量的观测值，因此它对应的残差就是观测值与状态量之间的差异，即：
e i = ln ( Z i − 1 T i ) ∨ = ln ( exp ( ( − ξ z i ) ∧ ) 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−1Ti)∨=ln(exp((−ξzi)∧)exp(ξi∧))∨对残差添加扰动，可得：
e ^ i = ln ( Z i − 1 exp ( δ ξ i ∧ ) T i ) ∨ \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−1exp(δξi∧)Ti)∨利用伴随性质和 B C H \mathrm{BCH} BCH 公式进行化简，可得：
e ^ i = ln ( Z i − 1 T i exp ( ( Ad ( T i − 1 ) δ ξ i ) ∧ ) ) ∨ = ln ( exp ( e i ∧ ) exp ( ( Ad ( T i − 1 ) δ ξ i ) ∧ ) ) ∨ ≈ e i + J r − 1 ( e i ) Ad ( T i − 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−1Tiexp((Ad(Ti−1)δξi)∧))∨=ln(exp(ei∧)exp((Ad(Ti−1)δξi)∧))∨≈ei+Jr−1(ei)Ad(Ti−1)δξi因此残差关于 T i T_i Ti 的雅可比为：
∂ e i ∂ δ ξ i = J r − 1 ( e i ) Ad ( T i − 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)其中：
J r − 1 ( e i ) ≈ I + 1 2 [ ϕ 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 主要特点
- 对地面做了分割，减小了特征搜索范围(环境中找线特征，地面找面特征----提高效率)；
- 提取特征之前做了聚类，提高了特征质量(很多曲率大的地方都是杂点，甚至树叶上的点都当成线特征来使用了)，这里删除掉了环境中比较杂乱的点，即不形成足够数量的点；
- 以帧为单位进行优化，使得全局地图可以多次调整，而不像LOAM那样不可修改；
- 增加了回环修正。
5.2 特征提取
- 根据线与线之间的夹角，以及点的曲率，筛选出地面点。所有用于匹配的平面点仅使用地面点。
- 在非地面点中，使用
广度优先搜索(BFS)做聚类，聚类中点的数量大于 30 30 30，才用来筛选线特征。 - 筛选线特征方法，与LOAM中相同。
5.3 位姿解算
LeGO-LOAM 相较于之前的 LOAM 有个很重要的区别，它把6dof拆成了两个3dof。从理论上讲，面特征既然都提自于地面，那么它确实只能约束两个水平角和 z z z 向的距离；线特征也一样，它不在地面只分配在周围，而且大多数线特征都是竖直向上的，那它也确实只能约束航向角以及 x x x、 y y y 的距离。这个理论上，从6dof拆成3dof和周围环境结合起来分析，它在精度上是没有损失的（或者说损失较少？），但是效率上会大幅提升。
- 使用地面面特征优化高度和水平角；
- 使用线特征优化水平位移和航向角。
注意：实际代码中，只有odom环节(scan-to-scan)使用了该模式，在mapping环节(scan-to-map)仍使用的是六自由度模型(低频，可以接受运算量大一些？)。
5.4 回环检测与修正
- 回环检测使用的初始位姿，采用前端里程计位姿
- 匹配采用 ICP 方法
- 优化库采用 gtsam
多传感器融合定位五-点云地图构建及定位相关推荐
- 多传感器融合定位 第四章 点云地图构建及基于点云地图定位
多传感器融合定位 第四章 点云地图构建及基于点云地图定位 代码下载 https://github.com/kahowang/sensor-fusion-for-localization-and-map ...
- 基于激光雷达的里程计及3D点云地图中的定位方法
本文转载自公众号@点云PCL,基于激光雷达的里程计及3D点云地图中的定位方法 :https://mp.weixin.qq.com/s/laA1YAPBCpqlzdGi0yb2cQ 论文:LOL: Li ...
- 深度学习传感器融合技术在自动驾驶汽车感知与定位中的应用研究进展
论文研读--Deep Learning Sensor Fusion for Autonomous Vehicle Perception and Localization: A Review 深度学习传 ...
- 利用自己的数据包实现点云地图的NDT定位
本文旨在帮助读者利用自己的数据包完成3D点云地图下的定位,公开数据包NDT:自动驾驶系统进阶与项目实战(六)基于NDT的自动驾驶高精度定位和ROS项目实战,本文只讲应用. 废话不多说,直接开始! 一. ...
- ORB-SLAM2系统的实时点云地图构建
ORB-SLAM2系统的实时点云地图构建 这篇博客 点云地图构建的流程 代码介绍 点云地图构建类对象 小调整 获取关键帧 点云地图构建与叠加 在地图中设置当前相机位置 点云地图到Octomap的转换 ...
- 基于激光雷达构建高精度点云地图及其感知定位
目前,前期大多数自动驾驶公司采用高精度惯导(imu+gnss+rtk)来实现高精度定位,但存在应用场景缺陷,无法应用隧道.高楼.林阴路等非开阔场景.所以感知定位方式应运而生,感知定位方式大多分为两种: ...
- ORB-SLAM稠密点云地图构建（黑白+彩色）+ pcd文件以八叉树形式表示
pcl1.8.1 VTK 7.1.1 版本一定要对好,如果安装了不符的版本如我之前安的pcl1.1.3和VTK8.2 一定要卸载干净不然会一直报错 ,不同版本的pcl和vtk是无法共存的,并且光把包删 ...
- 苹果系统地图定位服务器,iOS 系统地图实现及定位
1:添加库CoreLocation.framework,MApKit.framework; 2:@property (nonatomic, strong) CLLocationManager *loc ...
- 多传感器融合定位三-3D激光里程计其三：点云畸变补偿
多传感器融合定位三-3D激光里程计其三:点云畸变补偿 1. 产生原因 2. 补偿方法 Reference: 深蓝学院-多传感器融合 多传感器融合定位理论基础 文章跳转: 多传感器融合定位一-3D激光里 ...
最新文章
- 【视觉SLAM14讲】ch3课后题答案
- 算法工程师的落地能力具体指的是什么？
- 六月份收集到比较有营养价值的AI方面的文章
- 服务器Jmail配置问题
- NET130署名错误一事，改正也着实迅速
- python3 循环写入一对多键值对_为什么Python 3.6以后字典有序并且效率更高？
- 【设计模式】五、单例模式（独一无二的对象）
- 计算机分php,计算机按照处理数据的形态分类，可以分为什么？
- scp选择二进制_二进制传输与文本传输区别
- 华为机试HJ83：二维数组操作
- 经典机器学习系列(四)【神经网络详解】
- 实验-网络攻击（字典攻击）
- 渗透测试之信息收集（下篇）
- 三极管的输入输出的特性曲线
- oracle数据库处于recover,oracle数据库recover和restore的区别
- 武汉大学计算机系就业方向如何,武汉大学有什么王牌专业？它们的就业在哪些方向？...
- 第三方登入时昵称出现emoji表情致mysql插入失败
- Unity 灯光及光照烘焙
- 微信支付（小程序微信支付）
- 前端面试官怎么问的问题都是原理呢？