【泡泡点云时空】BALM：基于集束调整的雷达建图
泡泡点云时空，带你精读点云领域顶级会议文章
标题：BALM: Bundle Adjustment for Lidar Mapping
作者：Zheng Liu and Fu Zhang
来源：ICRA 2021
编译：廖邦彦
审核：lionheart
这是泡泡点云时空推送的第217篇文章，欢迎个人转发朋友圈；其他机构或自媒体如需转载，后台留言申请授权
摘要
对于包含关键帧的滑窗进行局部BA（集束调整）被广泛的应用于视觉SLAM，并且已经证明BA可以有效的降低累计飘移。但是在激光SLAM中，BA受限于难以对稀疏的特征点（例如边沿以及平面）进行精确匹配，从而很少被使用。在这篇文章中，我们将激光BA建模成最小化特征点到对应匹配的边缘或者平面特征的距离。不像视觉SLAM（以及雷达SLAM中的先验平面优化方法）需要特征点与位姿共同优化确定，我们表明特征点可以通过解析方法求解并且从BA中去除，剩下的BA只取决于关键帧的位姿。这个方法显著的降低了优化的尺度并且使得大规模的稠密边缘以及平面特征能够被应用于优化中。为了加速整体优化，我们闭式推导了误差函数导数的解析式，一直到二阶。同时，我们提出了一个新颖的自适应体素化方法来高效的搜索对应的特征。提出的方法被嵌入到LOAM后端来实现地图优化。结果表明，即使在后端，局部BA也能够高效的求解，即使在实时环境也能以10Hz运行同时优化20帧点云。局部BA同时也能在一定程度上降低LOAM漂移。我们的BA优化以及相关LOAM实现已经开源。
主要贡献
我们在稀疏雷达特征点上建立BA优化模型，包括边缘以及平面，通过直接最小化特征点到边缘以及平面的距离。和视觉BA同时优化特征位置以及相机位姿不同的是，我们表明特征（边缘以及平面）变量在雷达BA中能够得到解析的闭式解，最终使得BA优化只需要处理每帧的位姿。将特征变量从BA里面去除显著的降低了优化的维度并且因此使得大规模的稠密特征能够被优化。
为了满足高效的BA求解，我们推导了对应帧位姿的代价函数梯度以及海塞矩阵的解析解。
我们提出了一个自适应的体素化方法来高效的搜索匹配特征。
我们将提出的雷达BA融合进了LOAM的后端来实现地图优化，图二展示了它能够在旋转式激光雷达以及小视场角激光雷达（Livox Horizon）上相较于目前存在的LOAM实现有更加高效的性能。
结果显示局部BA能够有效的降低漂移。虽然它被设计为后端系统，局部BA任然运行的非常快，当优化一个大小为20帧的滑窗时，它能够几乎以10Hz实时运行。BA求解公式，优化库以及LOAM的具体实现都已经在社区开源。
方法概述
BA FORMULATION AND DERIVATIVES
Direct BA formulation
定义稀疏特征点群
属于不同的M帧，并且都对应着统一特征。
我们定义第i个特征点来自于
帧，并且定义M帧的位姿集合
所以在世界坐标系下的特征点定义为
图三展示了稀疏特征点到平面以及边缘的距离定义
对于每个平面特征，有以下公式
其中
表示的是依据矩阵A的特征值从大到小第k个
是对应的特征向量，其它定义如下
对于每个边缘特征，也有以下公式
其中
表示的是矩阵A的迹。从上式我们可以看到，最优点q*对于之后的代价函数并没有其它作用。此外，公式2以及4表明平面或者边缘的最佳特征变量可以被解析的得到在执行BA之前，以及剩下的BA问题只取决于位姿T。最后的优化问题就变为最小化矩阵A的特征值。
The Derivatives
定理一
矩阵A对于的特征值对特质点有如下公式
定理二
对于不同的特征值
有如下公式
Second order approximation
有了上文的第一二阶导数，我们就可以对公式5进行二阶近似
下面导出位姿扰动
对特征点的导数
定义位姿扰动对位姿的影响
最后将公式12导入公式8 得到
使用LM方法迭代优化
ADAPTIVE VOXELIZATION
假设我们获得了不同帧的大概初始位姿（例如，通过LOAM里程计），我们不断的将3D空间依据默认尺寸（例如，1m）进行栅格化。如果发现在当前栅格中的所有特征点都与同一个特征相对应（通过检查协方差矩阵的特征值），我们就将其与包含的特征点保存在内存中，如果不是这样，当前的栅格就会分裂成八叉树，并且对每个子栅格做同样检查一直到栅格到了最小的尺寸（例如，0.125m）。最后可以得到一个有不同栅格尺寸的地图，并且每个栅格都对应着一个特征，因此也只有一个损失函数。图四是一个示例。
a）不同的颜色对应着不同的栅格。在图片右下角的是对于树枝平面点的聚焦图像，包含了3个尺寸为0.125米的栅格
b）实际环境图片
c）所有的树节点都在哈希表中建立了索引
自适应栅格拥有以下的优点
1）有现存的数据结构天然兼容，例如八叉树，因此可以促进它的实施与效率。
2）它通常比直接在特征点上构造一个完整的Kd-树更有效，因为当包含的特征点位于同一平面或边缘上时，可能会更早终止继续构建。这种优点在一个拥有大尺度平面以及长边缘特征时更加明显。
3）具有自适应体素的地图将降低在激光雷达里程计中搜索对应特征点的时间。只需要搜索特征点被包含或接近的对应栅格，而不需要更耗时的搜索的最近邻点。在我们的实际实现中，我们使用了两个栅格地图，分别存储平面以及边缘特征。为了降低树的层数，我们使用哈希键值来存储。
备注一
如果栅格地图包含点过多，我们将同一特征上的同一帧的特征点进行平均。这样既能够减少计算量也能够保证不会降低建图的一致性。
备注二
因为在定理二中要求对于λk具有代数多重性的体素，我们只需跳过它。
备注三
该方法自然地扩展到非平面特征（例如曲面)，通过在更精细的构造体素映射，并在检查所包含的点是否位于同一平面上时允许更大的方差。
备注四
我们设置了两个条件来停止递归细分：一个是树的最大深度，另一个是体素中的最小点数。
LOAM WITH LOCAL BA
当构造体素映射时，我们计算体素中平面（或边缘)的中心点和法线(或方向）向量。然后，对于新来的一帧中的一个特征点，我们通过计算体素中的点与平面或边缘特征之间的距离来搜索最近的体素（由它的中心点表示）。当新的一帧加入到栅格地图中后，地图优化被触发，对一个滑窗中的全部雷达位姿进行局部BA，优化过后的位姿进一步更新所有涉及的栅格的中心点以及法向量。当一个滑窗满了，属于旧的帧的特征点就会被融合到Pfix中去，特征点点协方差矩阵的一个很好的性质是存在递归形式，允许滑动窗口外的所有特征点组成一些紧凑的矩阵和向量中，而不用保存原始数据点。合并后的Pfix将保留在体素图中，用于里程计和地图优化。
实验结果
户外行走数据
a）使用BALM构建全局地图
b）使用LOAM构建地图
c）d）展示在起点/终点分别使用BALM和LOAM的侧视图
分别使用Livox Horizon雷达的BALM和LOAM的漂移误差比较
室内建图以及里程计结果
a）整体场景
b）d）展示了LOAM的结果
c）e）展示了BALM的结果
展示了户外数据集的结果
使用velodyne-16线雷达得到的漂移误差以及与LOAM运行时间建立栅格数目比较。
Abstract
A local Bundle Adjustment (BA) on a sliding window of keyframes has been widely used in visual SLAM and proved to be very effective in lowering the drift. But in lidar SLAM, BA method is hardly used because the sparse feature points (e.g., edge and plane) make the exact point matching impossible. In this paper, we formulate the lidar BA as minimizing the distance from a feature point to its matched edge or plane. Unlike the visual SLAM (and prior plane adjustment method in lidarSLAM) where the feature has to be co-determined along with the pose, we show that the feature can be analytically solved and removed from the BA, the resultant BA is only dependent on the scan poses. This greatly reduces the optimization scale and allows large-scale dense plane and edge features to be used. To speedup the optimization, we derive the analytical derivatives of the cost function, up to second order, in closed form. Moreover, we propose a novel adaptive voxelization method to search feature correspondence efficiently. The proposed formulations are incorporated into a LOAM back-end for map refinement. Results show that, although as a back-end, the local BA can be solved very efficiently, even in real-time at 10Hz when optimizing 20 scans of point-cloud. The local BA also considerably lowers the LOAM drift. Our implementation of the BA optimization and LOAM are open-sourced to benefit the community.
如果你对本文感兴趣，想要下载完整文章进行阅读，可以关注【泡泡机器人SLAM】公众号。
欢迎来到泡泡论坛，这里有大牛为你解答关于SLAM的任何疑惑。
有想问的问题，或者想刷帖回答问题，泡泡论坛欢迎你！
泡泡网站：www.paopaorobot.org
泡泡论坛：http://paopaorobot.org/bbs/