Lidar Mapping Method

知也无涯12345

已于 2023-11-21 17:19:20 修改

阅读量278

点赞数

分类专栏：激光SLAM slam 文章标签： ubuntu 算法

于 2023-11-02 19:53:03 首次发布

本文链接：https://blog.csdn.net/m0_37604894/article/details/134187681

版权

slam 同时被 2 个专栏收录

8 篇文章 4 订阅

订阅专栏

激光SLAM

7 篇文章 6 订阅

订阅专栏

一、背景

通过Lidar建立点云地图，方案介绍。

二、BALM[1]

1、Local BA原理

BALM是基于keyframe晃动窗口的 local Bundle Adjustment (BA)方案。先介绍核心的Local BA窗口的位姿：
在这里插入图片描述
一组稀疏特征点 $p_{f_i}$ (i = 1, · · · ,N),来自M个scan，但对应于相同的feature（如相同的平面、边缘）.假设第i个特征点来自第 $s_i$ 个scan，其中 $s_i$ ∈ {1, · · · ,M}, M个scan的位姿为：
世界坐标系的点的位置为：
在这里插入图片描述
lidar BA 指的是联合优化M scan的位姿和全局3D 点云地图.这里3D点云地图变为面和边缘特征，以平面为例，这里残差为每一个平面上的特征点 $p_i$ 到对应组成的平面的距离的平方和：

其中 $\lambda_k(A)$ 表示矩阵A第k大的特征值， $u_k$ 是对应的特征向量, ¯p 和A是
在这里插入图片描述
和面类似，线的话是最小化如下cost：

在（2）和（4）中，最优点q∗并不唯一，因为该点可以在平面内（或沿边缘）自由移动。然而，这对待优化的cost函数没有影响。此外，（2）和（4）意味着在BA之前可以分析得到最优特征（平面或边缘）参数，因此BA问题只依赖于姿态T。即一旦知道scan姿态，就可以确定3D点云地图（因此平面或边缘特征）。此外，注意到，对姿态T的优化归结为最小化矩阵A在（3）中的特征值。经过推导，最终的残差就是优化特征点云形成的协方差特征值:
在这里插入图片描述
对于面特征就是最小特征值，线特征就是两个大特征值之和。其中 p = [pT 1 · · · pT N]T 是对应于同一特征的所有特征点的向量。
用LM求解的画需要得到一阶雅克比J和二阶Hessian H

$\textbf{J}$ 和 $\textbf{H}$ 可以用链式求导，先求 $\lambda_k$ 对点 $\textbf{p}_i$ 求导，得到 $\textbf{J}_i$
在这里插入图片描述
二阶导 $\textbf{H}_{ij}$

此时二阶近似为：

然后求 $\textbf{p}_i$ 对位姿 $\textbf{T}_j$ 求导

最后便可得到 $\lambda_k$ 在 $\textbf{T}$ 添加扰动后的二阶展开，得到 $\textbf{J}$ 和 $\textbf{H}$ ，这里 $\textbf{J}_i$ 是j的第i列， $\textbf{H}_{ij}$ 是H的i行j列元素。
在这里插入图片描述
注意，在计算 $\textbf{J}$ 和 $\textbf{H}$ 过程中， $\textbf{J}$ 的计算为O(N)， $\textbf{H}$ 的计算为O(N^2)，其中N为每个特征对应的平均点云数量.

2、算法整体框架

其地图维护和VoxelMap介绍的基本相同。即自适应体素化方法：假设不同scan的粗略初始姿态是可用的（例如，来自LOAM里程计），我们反复地将3D空间从默认大小（例如，1m）体素化：如果当前体素中的所有特征点（来自所有scan）都位于一个平面或边缘上（例如，通过检查点协方差矩阵（3）的特征值），则将当前体素及其包含的特征点保留在地图中；否则，当前体素分解为八个八分体（即一个八叉树），并继续检查每个八分体，直到达到最小大小（例如，0.125m）。所提出的自适应体素方法生成一个体素地图，其中不同的体素可能有适应环境的不同大小。对于每个体素，它对应于一个特征，对应为（13）中的一个cost项。在下图中可以看到一个示例体素地图。
在这里插入图片描述

算法整体框架如下图所示，上面Local BA主要用于Map Refinement过程中。
在这里插入图片描述
在向体素地图推送一定数量的新scans后，将触发Map Refinement。Map Refinement在激光雷达姿态的滑动窗口上执行local BA。任何包含滑动窗口内的点（即， $P_{sw}$ ）的体素都用于构造cost function，每个体素对应一个残差维度lambda，如（2）或（4）。然后，通过（13）中的LM迭代求解。这样就可以优化滑动窗口所有lidar的位姿，优化后，使用更新后的位姿来更新所有涉及的体素的中心点和法向量。
一旦滑动窗口满了，来自最老scan的点将合并到地图点 $P_{fix}$ 。点协方差矩阵（3）的一个好属性是可以增量更新，允许将滑动窗口外的所有点通过几个矩阵和向量来表示，因此地图点不需要保留，只需要保留对应的特征和协方差即可。
注意，这里合并的点 $P_{fix}$ 将在体素地图中保留，用于odometry和map-refinement(每个体素的观测也会包含 $P_{fix}$ 的点（实际用的是一些cov和向量），只不过和状态无关，直接变成测量了)。

三、BALM2.0[2]

这也是一个LocalBA方案，相对BALM性能更优，核心创新点是引入了点簇Point Cluster来避免BALM中计算J,H都需要用到平面上的点坐标，导致将计算复杂度从 $O(NM_fM_p+N^2 M_f M_p^2+M_p^3)$ 降低到 $O(M_fM_p+M_f M_p^2+M_p^3)$ ，其中N为每个特征对应的平均点云数量， $M_f$ 为特征数量， $M_p$ 为位姿数量.

1、点簇的引入及核心创新点

点簇的定义如下,其实就是很多点的集合，通过一个矩阵表示出来，这样不需要保存每个原始点云的信息，只需要保存其“坐标矩阵”即可。
在这里插入图片描述
点集具有如下特性：

点簇可以投影（从local到global）和合并
点簇的协方差 $\textbf{A}$ 可以通过其坐标矩阵 $\textbf{C}$ 得到

这样，我们BALM的优化问题就可以转化为如下形式（完全等价），同样的BA问题，但与点无关，之和其“坐标矩阵”有关，同样也是关于位姿 $\textbf{T}$ 的一个优化问题。
点簇具有gauge freedom（即所有待优化的位姿做同一个齐次变换T，优化问题不变）

因此对于第i个特征平面形成的点簇（由多个位姿上观测到的 $C_{ij}$ 组成），优化问题便化为：

注：论文[3]和这篇论文计算复杂度相同，但推导更加简单，不过这篇论文开源了，[3]没开源，可以对比一下加深理解。

四、global BA[4]

大规模的BA不能直接使用BALM2.0去优化，算力和内存要求太高，而且难以收敛，因此使用了分层次的滑窗BA来解决这个问题.

1、原理介绍

这篇文章提出两种策略，一部分是自下而上的过程（Bottom-Up Hierarchical BA）纠正点云地图的局部一致性，另一部分是自上而下的过程（Top-Down Processes），综合BA得到的所有位姿约束得到最小二乘解。
图中总层数l=3，步长s=3，滑窗大小w=6。
在这里插入图片描述

自下而上Bottom-Up Hierarchical BA: 第一层包含了所有待优化的LiDAR帧和对应位姿，形成一个大小为6的滑窗，步长为3，因此滑窗之间有重叠，滑窗内采用论文BALM中的方法进行BA，利用BA后的位姿将滑窗内点云投影到第一帧（比如 $x_0^1, x_1^1,x_2^1,x_3^1,x_4^1,x_5^1$ 的点云都投影到第 $x_0^1$ ,得到 $F_0^2$ ）。保存此时计算的Hession矩阵，后面Top-Down Processes会用到。

这样依次往上，重复第一层的操作，直到最上面一层，然后对最上面一层的所有关键帧进行一个全局BA。每一层Local BA的时间复杂度为 $O(M_fM_p+M_f M_p^2+M_p^3)$ ，虽然越往上滑窗内的平面 $M_f$ 越多，仅为一次相关，但位姿数量是不变的（都是6），所以整体的算力要求在可控范围内。
自上而下Top-Down Pose Graph Optimization：这里进行pose graph优化，第i层 $x^i_j$ 和 $x^i_{j+1}$ 之间的cost为：

其中 $\Omega$ 是协方差矩阵，这里通过前面保存Hession矩阵的inverse得到。这里只进行一次pose graph优化，这次优化会综合各层BA得到的所有位姿约束和Hessian矩阵，对第一层的所有关键帧进行位姿图优化，如果收敛则输出真值位姿，否则继续循环进行以上过程.
考虑到 $x_j^{i+1}$ 和 $x^i_{s,j}$ 节点是相同的（不同层的约束中存在重复节点），因此将上述优化边转换为

即将所有层的约束都转换为对第一层keyframe的约束。即优化问题变为：

综上，整体算法的pipeline如下图所示：
在这里插入图片描述

[1] Liu Z, Zhang F. Balm: Bundle adjustment for lidar mapping[J]. IEEE Robotics and Automation Letters, 2021, 6(2): 3184-3191.
[2] Liu Z, Liu X, Zhang F. Efficient and Consistent Bundle Adjustment on Lidar Point Clouds[J]. arXiv preprint arXiv:2209.08854, 2022.
[3] Zhou L, Koppel D, Ju H, et al. An efficient planar bundle adjustment algorithm[C]//2020 IEEE International Symposium on Mixed and Augmented Reality (ISMAR). IEEE, 2020: 136-145.
[4] Large-Scale LiDAR Consistent Mapping using Hierarchical LiDAR Bundle Adjustment