经典算法详解 | BALM激光点云的BA优化问题

最新推荐文章于 2024-09-14 15:47:50 发布

自动驾驶之心

最新推荐文章于 2024-09-14 15:47:50 发布

阅读量2.5k

点赞数

文章标签：算法

原文链接：https://mp.weixin.qq.com/s?__biz=Mzg2NzUxNTU1OA==&mid=2247545715&idx=2&sn=cfeb3dc691c95ee3a7eb54109c2be0cc&chksm=ceb81ebaf9cf97ac166fbe746a32b4775973c33de86d162229d00c87d18eea92d80693b729c9&scene=126&sessionid=0

版权

点击下方卡片，关注“自动驾驶之心”公众号

ADAS巨卷干货，即可获取

今天自动驾驶之心很荣幸邀请到Nicky来分享BALM激光点云的BA优化问题，如果您有相关工作需要分享，请在文末联系我们！

作者 | Nicky

编辑 | 自动驾驶之心

>>点击进入→自动驾驶之心【SLAM】技术交流群

论文：https://ieeexplore.ieee.org/document/9366383?denied=
代码：https://github.com/hku-mars/BALM/tree/master/BALM-old

Bundle Adjustment是一种用于同时估计三维结构和传感器运动运动的优化算法。在视觉SLAM，三维重建等应用中，它被广泛用于优化相机位姿和地图的精度。在视觉slam中，BA通过最小化特征之间的重投影误差，有效得优化相机位姿和地图的一致性，从而提高SLAM系统的精度和鲁棒性。然而来到激光雷达这边情况就很不一样：激光雷达点云虽然拥有深度信息，但是比起图像比较稀疏，单帧的出点量和图像中的像素数量根本不在一个量级，导致特征点稀疏，特征匹配和对应难做。那么有没有办法解决激光SLAM中的BA优化问题，让它可以在后端优化、雷达外参优化等应用上发挥作用呢？MARS实验室2021年6月发表在RAL的论文《Bundle Adjustment for LiDAR Mapping》[1]给出了一个切实可行的方案。下面简述一下这套方案，并对源码中的一些关键操作对应的函数入口进行提示，觉得讲不清楚的看源码都能理解。

RMK: 其实BALM的开源代码[6]结合了一些其它建图框架的demo，一上来没有那么容易理解。可以优先去看MLCC[8]中的pose_refine.cpp，其实干的是一回事，代码可读性更高一些。但是本文中提示的函数入口还是以BALM-old的命名为准

2. 激光建图中的Bundle Adjustment问题定义

假设由帧雷达点云扫描出雷达坐标系下的点表示为, 表示这一帧点云中的第个点，表示雷达坐标系。同时假设我们有对应时刻世界坐标系下的组雷达姿态，这个可以通过多种方法获得，不论是跑个LO，还是借助于惯导等其他传感器。假设机器人运动低速，不考虑点云运动畸变的情况，通过这组位姿位姿将点云转换回世界坐标系，得到世界坐标系下的点

由这些点集堆叠成一张地图。定义这张地图的一致性指标(mapping consistency indicator) 描述地图接近实际物理状态的程度。激光雷达中的BA优化目的和视觉SLAM中一样，都是希望找到一组最优的位姿，使得使用这组位姿堆叠的地图具有最佳的建图一致性，即

3. 自适应体素特征提取与consistency indicator的递归构建

为了高效解决激光点云的特征稀疏以及难以匹配的问题，BALM中借助八叉树的数据结构实现了自适应体素化的特征提取操作：首先对这个局部地图进行体素化操作：将构建的地图分割成一个个预设体积的体素，其中记录了这个点云来自第几帧，也就能找到它对应的雷达位姿，然后对每个体素分别计算这个体素内所有点的协方差矩阵。其中代表一个体素内的点数量。记为协方差矩阵中第大的特征值，那么就可以提取线面特征：

面特征：如果与的比值超越某一阈值，那么就标记为一个面特征体素。面特征的法向量为对应的特征向量
线特征：如果与的比值小于某一阈值，那么就标记为一个线特征体素。线特征的方向向量为对应的特征向量

如果满足上述任一条件，那么这个体素就会被标记为一个线或者面特征体素。如果不满足平面体素的条件，那么就将这个体素分成八个小体素，然后对这八个小体素分别判断是不是满足上述条件，不断递归直到八叉树的层数达到预先设定的最大值。这样就可以实现对局部地图快速且通用的特征提取，不需要使用检测、分割的方法，也不关心雷达型号(机械式还是固态)，具有非常强的通用性。在开源代码中，堆叠了局部地图之后，使用cut_voxel()的函数对局部地图进行体素化并建八叉树，然后通过OCTO_TREE::recut()实现对局部地图的自适应体素化特征提取，然后对不同体素中的点云给不同的着色进行相应的可视化操作。论文中的效果如下图所示

在对地图进行了自适应体素分割之后，特征的对应关系就很好做了：对于任一被标记为平面特征的体素，可以认为同一个体素内的所有点构成对应关系。从而可以构建点面误差或者点线误差：

点面：对于每一个平面体素，点面误差计算表达式如下：
其中是平面体素中的任意一个点。理论上随便选特征体素中的某个点作为参照不影响损失函数的计算的结果，毕竟它们都应该在同一个面上。
点线：对于每一个线特体素，点线误差计算表达式如下：
同理是线特征体素中的任意一个点。理论上随便选特征体素中的某个点作为参照不影响损失函数的计算的结果，毕竟它们都应该在同一个面上。

上图给出了点线和点面特征的直观解释，熟悉激光SLAM的对这俩式子应该都不陌生。就这样，在将局部地图使用八叉树存储之后，就可以通过递归的方式对整张地图构建优化目标函数。局部地图中有多少特征体素，优化问题就应该产生多少个residual block，这样整个优化问题可以通过八叉树递归的方式被创建。优化问题的递归创建主要看OCTO_TREE::traversal_opt()和LM_SLWD_VOXEL::push_voxel()这两个函数就明白了。

理论上介绍到这里我们就可以写出一个实现一个算法的demo，然后把剩下的优化丢给第三方优化库去做。然而下面作者提出，对于线面特征，这个损失函数可以被进一步简化成一个形式，并相应得可以获得解析的雅克比和黑塞矩阵用于高效和稳定的进行优化。

4. 损失函数与二阶近似的解析形式

论文中指出，对于线面特征都可以将损失函数简化为同一个形式，即最小化体素中点云的方差矩阵的特征值，并针对这个形式计算了解析的雅克比和黑塞矩阵以便于对损失函数进行二阶近似，进而给出解析的优化步表达式。

4.1. 损失函数的简化

对于上述的线面特征，取整个体素内点云的质心和方差，，那么点面误差和点线误差就可以有如下的简化：

点面误差：将特征平面上的参考点取成质心，法向量取成最大的特征值对应的特征向量时，最优化的位姿对应于最小化方差矩阵的最大特征值的问题
点线误差：将线特征上的参考点取成质心，法向量取成最小的特征值对应的特征向量时，最优化的位姿对应于最小化方差矩阵的特征值的问题

也就是因为这个特性，无论是线特征还是面特征，我们可以将优化问题简化为一个关于雷达位姿的“最小化体素中方差矩阵的特征值”问题

其中是同一个体素中来自不同帧的点云坐标。区别于视觉SLAM中BA优化问题中每一轮的特征会随着位姿的变化而变化，在激光SLAM的一轮BA优化中，一旦位姿被确定，地图被构建，线面特征提取的结果也都是一样的，损失函数的只和待优化的位姿有关，特征点都已经被解析得计算出来，不再显式得参与计算。

4.2. 简化损失函数的二阶近似

为了使用这个损失函数构建非线性优化问题，并且使用非线性二阶优化方法，我们需要对损失函数进行二阶近似。文中的推导使用了链式法则，首先求损失函数关于特征点坐标的近似：

其中是雅克比矩阵，表示的是黑塞矩阵

损失函数关于局部地图特征点坐标的解析雅克比：对应原论文中的Theorem 1：对于一组三维点以及使用它们求得的协方差矩阵，那么矩阵对各个点坐标的导数为
其中表示对应的特征向量。因此确定了中的每一个元素
损失函数关于局部地图特征点坐标的解析Hessian矩阵：对应原文中的Theorem 2：对于一组三维点以及对应计算出的协方差矩阵。如果这个协方差矩阵没有重数大于1的特征值，那么就有
其中
原论文中对Theorem 1和2都给出了证明，感兴趣的可以自己去看。

RMK: 如果非常不幸，这个体素中点云坐标算出的方差矩阵有重数大于1的特征值，文中说直接把对应的体素扔掉，不参与计算就好了

由于特征点不是我们优化的目标，位姿才是。而待优化的位姿又是决定了局部地图中的特征点坐标，因此需要通过链式法则，再求地图特征点坐标对雷达位姿的导数：采用惯常做法，对位姿进行一次扰动

得到特征点坐标坐标关于待优化的位姿的导数

那么最后关于特征值关于雷达位姿的损失函数就表示为

其中，这个矩阵的每个元素表示为

这样我们就可以用Levenberg-Marquardt等二阶优化方法计算位姿，每一个优化步求解如下的线性方程

其中是LM优化的阻尼系数。相关的计算在代码的LM_SLWD_VOXEL::acc_t_evaluate() 这里同时给出了residual，jacobian和hessian的计算方式。如果对于计算还有什么问题，可以参见代码，应该是写的比较清楚了。

5. 实验-将BALM结合进LOAM

由于激光SLAM中的运动估计通常是基于IMU和里程计，因此会存在累计误差。因此，应用BA优化可以有效地纠正这些累计误差，提高SLAM系统的精度和鲁棒性。原文中把LOAM的后端换成了BALM，对odometry进行后端优化。同时通过共享内存的方式将自适应体素化特征提取的结果用于前端的registration来进行加速。整套SLAM的架构如下图所示

在整个论文的Experiment中体现出了这个算法如下几个特性：

精度高：手持场景下使用Livox Horizon绕HKU校园一圈回到终点，发现用BA作为后端的LOAM平移量飘了31cm，而纯LOAM平移量飘了6.23m。同时跑了一组室内数据，可以发现偏移量非常小，可见用它来做后端优化还是有比较高的精度的。

不挑lidar：原论文在Livox Horizon, Livox Mid40和VLP-16上都跑了实验，无论雷达多少FOV，扫出来点云什么样，都可以跑，而且漂移量都远小于LOAM。使用VLP16采一圈回到原点之后LOAM平移量飘了56.8cm(0.27%)，而BALM飘了28cm(0.13%)
运行速度：在结合了BALM的LOAM框架中，主要得益于不需要建KD树去搜索五个最近邻，结合了BALM的LOAM表现出了较好的实时性。当然我理解这个运行速度只是使用了自适应体素特征，以及前后端线程共享内存的操作取代了原本建KD树搜索最近邻带来的增益，实际上基于非线性优化的后端难免还是要比基于滤波器的后端实时性要差一些的。

6. 算法应用场景解析

由实际应用看下来这个算法存在以下特性：

不挑雷达：不管你是机械式还是固态，不管是16线还是128线，得益于自适应体素化的特征提取方法，全都可以在一个框架下完成特征提取与对应
更吃场景：如果连线面特征都提取不出来，场景给的约束可能会不够充分
实时性略差：相对基于滤波的后端要差一些。比如一不当心某个特征体素内点云太多了，Hessian矩阵就会有很高的维数，求解非线性优化步的耗时就会很长
八叉树以及一系列递归操作会增加程序debug的难度，开发的时候容易翻车

因此除了作为SLAM的后端，这个算法一个很适合的应用是搞激光雷达外参标定：不管是雷达到车体（opencalib[5]的雷达到车体自标定[9]有借鉴这个思想），还是多激光联合标定[3]，都已经有行之有效的论文和开源代码。吃场景和缺乏实时性对于传感器标定的业务来说，是不值得一提的弱点：一般标定不会跟智驾同时运行，可以有充分的算力，而且可以对场景提出要求。此外这个算法还有新的一代BALM2 [2][7]，其最大的亮点是引入了PointCluster的概念，加快了计算速度，并且能计算当前位姿的uncertainty，还围绕这个概念设计了一套完备的理论。但是其实BALM1对于离线SLAM应用来说已经效果非常不错了。感兴趣的后续可以再出一期进行解读。

7. 参考文献与代码仓库

[1] Z. Liu and F. Zhang, "BALM: Bundle Adjustment for Lidar Mapping," in IEEE Robotics and Automation Letters, vol. 6, no. 2, pp. 3184-3191, April 2021, doi: 10.1109/LRA.2021.3062815.

[2] Liu, Z., Liu, X., & Zhang, F. (2022). Efficient and Consistent Bundle Adjustment on Lidar Point Clouds. arXiv preprint arXiv:2209.08854.

[3] Liu, X., Yuan, C., & Zhang, F. (2022). Targetless extrinsic calibration of multiple small FoV LiDARs and cameras using adaptive voxelization. IEEE Transactions on Instrumentation and Measurement, 71, 1-12.

[4] Yan, G., Pi, J., Wang, C., Cai, X., & Li, Y. (2022). An Extrinsic Calibration Method of a 3D-LiDAR and a Pose Sensor for Autonomous Driving. arXiv preprint arXiv:2209.07694.

[5] Yan, G., Liu, Z., Wang, C., Shi, C., Wei, P., Cai, X., ... & Li, Y. (2022). Opencalib: A multi-sensor calibration toolbox for autonomous driving. Software Impacts, 14, 100393.

[6] BALM/BALM-old at master · hku-mars/BALM · GitHub

[7] GitHub - hku-mars/BALM: An efficient and consistent bundle adjustment for lidar mapping

[8] GitHub - hku-mars/mlcc: Fast and Accurate Extrinsic Calibration for Multiple LiDARs and Cameras

[9] SensorsCalibration/lidar2imu/auto_calib at master · PJLab-ADG/SensorsCalibration · GitHub

（一）视频课程来了！

自动驾驶之心为大家汇集了毫米波雷达视觉融合、高精地图、BEV感知、传感器标定、传感器部署、自动驾驶协同感知、语义分割、自动驾驶仿真、L4感知、决策规划、轨迹预测等多个方向学习视频，欢迎大家自取（扫码进入学习）

（扫码学习最新视频）

视频官网：www.zdjszx.com

（二）国内首个自动驾驶学习社区

近1000人的交流社区，和20+自动驾驶技术栈学习路线，想要了解更多自动驾驶感知（分类、检测、分割、关键点、车道线、3D目标检测、Occpuancy、多传感器融合、目标跟踪、光流估计、轨迹预测）、自动驾驶定位建图（SLAM、高精地图）、自动驾驶规划控制、领域技术方案、AI模型部署落地实战、行业动态、岗位发布，欢迎扫描下方二维码，加入自动驾驶之心知识星球，这是一个真正有干货的地方，与领域大佬交流入门、学习、工作、跳槽上的各类难题，日常分享论文+代码+视频，期待交流！