目录
1 摘要
Bundle Adjustment是一种用于同时估计三维结构和传感器运动运动的优化算法。在视觉SLAM，三维重建等应用中，它被广泛用于优化相机位姿和地图的精度。在视觉slam中，BA通过最小化特征之间的重投影误差，有效得优化相机位姿和地图的一致性，从而提高SLAM系统的精度和鲁棒性。
然而来到激光雷达这边情况就很不一样：激光雷达点云虽然拥有深度信息，但是比起图像比较稀疏，单帧的出点量和图像中的像素数量根本不在一个量级，导致特征点稀疏，特征匹配对应难做。那么有没有办法解决激光SLAM中的BA优化问题，让它可以在后端优化、雷达外参优化等应用上发挥作用呢？MARS实验室2021年6月发表在RAL的论文《Bundle Adjustment for LiDAR Mapping》给出了一个切实可行的方案。
在本文中，我们将激光BA建模为将特征点与其匹配的边缘或平面之间的距离最小化。与视觉SLAM（以及激光SLAM中先前的平面调整方法）不同的是，特征点可以通过解析方法求解并从BA中移除，得到的BA只依赖于扫描位姿。这大大减少了优化的规模，并允许使用大规模稠密的平面和边缘特征。为了加速优化过程，我们以闭合形式导出了代价函数的解析导数，包括二阶导数。此外，我们提出了一种新颖的自适应体素化方法，以高效地搜索特征对应关系。所提出的建模方法已应用于LOAM后端进行地图优化。结果表明，尽管作为后端，局部BA可以非常高效地求解，甚至在实时优化20个点云扫描时也能达到10Hz的速度。局部BA还显著降低了LOAM的漂移。
2 相关工作
3 BA公式和导数
A. 直接BA公式
为了高效解决激光点云的特征稀疏以及难以匹配的问题，BALM中借助八叉树的数据结构实现了自适应体素化的特征提取操作：首先对这个局部地图进行体素化操作：将构建的地图分割成一个个预设体积的体素，其中记录了这个点云来自第几帧，也就能找到它对应的雷达位姿，然后对每个体素分别计算这个体素内所有点的协方差矩阵。其中代表一个体素内的点数量。记为协方差矩阵中第大的特征值，那么就可以提取线面特征：
- 面特征：如果与的比值超越某一阈值，那么就标记为一个面特征体素。面特征的法向量为对应的特征向量
- 线特征：如果与的比值小于某一阈值，那么就标记为一个线特征体素。线特征的方向向量为对应的特征向量
如果满足上述任一条件，那么这个体素就会被标记为一个线或者面特征体素。如果不满足平面体素的条件，那么就将这个体素分成八个小体素，然后对这八个小体素分别判断是不是满足上述条件，不断递归直到八叉树的层数达到预先设定的最大值。这样就可以实现对局部地图快速且通用的特征提取，不需要使用检测、分割的方法，也不关心雷达型号(机械式还是固态)，具有非常强的通用性。在开源代码中，堆叠了局部地图之后，使用cut_voxel()的函数对局部地图进行体素化并建八叉树，然后通过OCTO_TREE::recut()实现对局部地图的自适应体素化特征提取，然后对不同体素中的点云给不同的着色进行相应的可视化操作。
在对地图进行了自适应体素分割之后，特征的对应关系就很好做了：对于任一被标记为平面特征的体素，可以认为同一个体素内的所有点构成对应关系。从而可以构建点面误差或者点线误差：
- 点面：对于每一个平面体素，点面误差计算表达式如下：其中是平面体素中的任意一个点。理论上随便选特征体素中的某个点作为参照不影响损失函数的计算的结果，毕竟它们都应该在同一个面上。
- 点线：对于每一个线特体素，点线误差计算表达式如下：同理是线特征体素中的任意一个点。理论上随便选特征体素中的某个点作为参照不影响损失函数的计算的结果，毕竟它们都应该在同一个面上。
给定一组稀疏特征点
，这些特征点来自于
个扫描，但都对应于同一个特征（平面或边缘）（见图3）。假设第 i 个特征点来自于第
个扫描，其中
，将个扫描的姿态表示为
，其中
。然后，全局坐标系中的特征点为：
如前所述，激光雷达BA问题是指联合确定个扫描的姿态和全局三维点云地图。现在，三维地图是一个单一的特征（边缘或平面），那么BA问题就简化为联合确定单一特征的姿态
和位置，该特征由特征上的点
和单位向量
（是平面的法向量或边缘的方向）表示。对于平面特征，直接BA公式是最小化每个平面特征点
到平面上的距离的平方和：
其中
表示矩阵的
第k个最大特征值，
是对应的特征向量，
和
分别表示：
与平面特征类似，边缘特征的直接BA公式是最小化每个边缘特征点到边缘的平方距离的总和：
其中 
表示
的迹。
注意，在公式(2)和(4)中，最优点 
并不唯一，因为该点可以在平面内（或沿边缘）自由移动。然而，这对最终的优化代价函数没有影响。此外，公式(2)和(4)暗示了在BA之前可以解析地获得最优的特征（平面或边缘）参数，而最终的BA问题只依赖于姿态T。这与我们的直觉相符，即一旦扫描姿态确定，3D点云地图（因此平面或边缘特征）也就确定了。此外，对姿态T的优化转化为最小化公式(3)中矩阵A的特征值。即，BA导致最小化
在T上，其中 
是与相同特征对应的所有特征点的向量。 为了使成本函数(5)的优化更加高效，我们对位姿T进行了解析地计算了一阶和二阶的闭合导数。由于链式法则，我们首先对点向量p进行了导数的计算。
对于上述的线面特征，取整个体素内点云的质心和方差，那么点面误差和点线误差就可以有如下的简化：
- 点面误差：将特征平面上的参考点取成质心，法向量取成最大的特征值对应的特征向量时，最优化的位姿对应于最小化方差矩阵的最大特征值的问题
- 点线误差：将线特征上的参考点取成质心，法向量取成最小的特征值对应的特征向量时，最优化的位姿对应于最小化方差矩阵的特征值的问题
也就是因为这个特性，无论是线特征还是面特征，我们可以将优化问题简化为一个关于雷达位姿的“最小化体素中方差矩阵的特征值”问题
其中是同一个体素中来自不同帧的点云坐标。区别于视觉SLAM中BA优化问题中每一轮的特征会随着位姿的变化而变化，在激光SLAM的一轮BA优化中，一旦位姿被确定，地图被构建，线面特征提取的结果也都是一样的，损失函数的只和待优化的位姿有关，特征点都已经被解析得计算出来，不再显式得参与计算。
B. 导数
定理1：对于一组点
和在（3）中定义的协方差矩阵
。假设 具有对应于特征向量
的特征值
，则
其中
是N个点的平均值，如（3）中所示。
定理2：对于一组点 
和在（3）中定义的协方差矩阵
。假设具有对应于特征向量
的特征值
。此外，当
时
，则
C. 二阶近似
利用前面章节中的一阶和二阶导数，我们可以通过其二阶近似来近似成本函数（5），如下所示：
其中J(p)是Jacobian矩阵，其i元素在（6）中，H(p)是Hessian矩阵，其i行，j列元素在（7）中。
请注意，点向量p还依赖于扫描姿势T，如（1）所示。使用在[38]中定义的操作将姿势
扰动到其切平面
，我们有：
和
将（12）代入（8）得到
最后，我们使用Levenberg-Marquardt（LM）[39]方法通过反复将其近似为二阶近似（13）来最小化成本
。在每次迭代中，解决方案从中解决
其中 
是根据LM方法确定的步长。
4 自适应体素化
在第三节中，BA公式需要找到所有与同一特征（边缘或平面）对应的特征点。为此，我们提出了一种新颖的自适应体素化方法：假设存在不同扫描的粗略初始姿态（例如来自LOAM里程计），我们重复从默认尺寸（例如1米）开始对3D空间进行体素化：如果当前体素中的所有特征点（来自所有扫描）都位于平面或边缘上（例如通过检查点协方差矩阵的特征值(3)），则将当前体素与其中的特征点保存在内存中；否则，当前体素分解为八个八分体，并继续检查每个八分体，直到达到最小尺寸（例如0.125米）。所提出的自适应体素化生成一个体素地图，其中不同的体素可以根据环境进行不同的尺寸调整。对于每个体素，它对应于一个特征，因此对应于(13)中的一个成本项。图4(a)展示了一个示例体素地图。
自适应体素化具有许多优点：1）它与现有的数据结构（如八叉树）自然兼容，因此可以大大简化其实现和效率；2）与构建特征点的完整Kd树[10]相比，它通常更高效，因为当包含的特征点位于同一平面或边缘上时，可以提前终止。当环境具有大型平面或长边缘时，这种优势将更加明显；3）具有自适应体素的地图将减少在激光雷达里程计中搜索特征对应关系的时间。只需要搜索特征点所在或附近的体素，而不是需要更详尽搜索的最近点[10]。
在我们的实现中，我们构建了两个体素地图，一个用于边缘特征，一个用于平面特征。由于其构造方式，体素地图自然适合八叉树结构。为了减少八叉树的深度，我们使用一组由哈希表索引的八叉树（见图4(c)）。每个八叉树对应空间中默认体素尺寸（例如1米）的一个非空立方体。不同的八叉树可能具有不同的深度，取决于空间中该立方体的几何形状。每个叶节点（即体素）保存与同一特征（例如平面或边缘）对应的所有特征点。
备注1：如果一个体素包含太多的点，(8)中的Hessian矩阵将具有非常高的维度，在这种情况下，我们可以对同一扫描的点进行平均。平均后的点数量较少，并且位于由原始特征点确定的同一平面上。这样可以节省大量计算而不会降低地图的一致性。
备注2：定理2中计算的Hessian矩阵要求当i≠k时，λi≠λk。对于具有超过一个代数重数的λk的体素，我们简单地跳过它。
备注3：尽管我们一直在提到边缘特征和平面特征，但该方法自然地可以扩展到非平面特征（例如曲面），方法是在更细的层次上构建体素地图，并允许在检查包含点是否位
自适应体素化具有许多优点：
- 它与现有的数据结构（如八叉树）自然兼容，因此可以极大地简化其实现和效率；
- 与构建完整的特征点Kd树相比，它通常更高效，因为当包含的特征点位于同一平面或边缘上时，可以提前终止。当环境具有大平面或长边缘时，这种优势将更加明显；
- 具有自适应体素的地图将减少在激光雷达里程计中搜索特征对应关系所需的时间。只需要搜索特征点所在或附近的体素，而不是需要更多穷举搜索的最近点。
5 将BALM结合进LOAM
由于激光SLAM中的运动估计通常是基于IMU和里程计，因此会存在累计误差。因此，应用BA优化可以有效地纠正这些累计误差，提高SLAM系统的精度和鲁棒性。原文中把LOAM的后端换成了BALM，对odometry进行后端优化。同时通过共享内存的方式将自适应体素化特征提取的结果用于前端的registration来进行加速。
我们将提出的BA公式和其优化方法纳入到LOAM框架中。系统概述如图5所示。它由三个并行线程组成：特征提取、里程计和地图优化。
一旦接收到新扫描的特征点，里程计通过将新的特征点与现有地图进行配准来估计激光雷达的姿态。与现有方法不同的是，我们利用自适应体素地图来加速匹配过程。具体而言，在构建体素地图时，我们计算体素中平面（或边缘）的中心点和法向量（或方向向量）。然后对于新扫描的特征点，我们通过计算该点与体素中的平面或边缘特征之间的距离来搜索最近的体素（由其中心点表示）。
使用里程计，可以将新的扫描点大致注册到全局坐标系，并将其推送到体素地图中：对于新扫描中的每个点，搜索其所在的体素，并将该点添加到相应八叉树的叶节点中。如果在现有地图中找不到与新扫描中的点对应的体素，则创建一个新的八叉树，将其根索引到哈希表中，并将该点添加到根节点中。在将新扫描的所有特征点分布到现有八叉树的叶节点或新创建八叉树的根节点后，我们按照构造方式更新体素地图：如果节点（叶节点或节点）中的点不能构成一个单一的特征（平面或边缘），则将该节点分割为八个并检查每个节点。
在将一定数量的新扫描推送到体素地图后，会触发地图优化。地图优化在激光雷达姿态的滑动窗口上执行局部BA。滑动窗口内包含的任何包含点的体素（即Psw）将用于构建成本项，如（2）或（4）。然后，地图优化反复最小化由所有相关体素组成的总成本的二阶近似（13）。这将优化滑动窗口内的所有激光雷达姿态。更新后的姿态然后用于更新所有相关体素的中心点和法向量。
一旦滑动窗口装满，来自较早扫描的点将合并到地图点
中。点协方差矩阵（3）的一个良好特性是存在递归形式[40]，允许将滑动窗口外的所有点汇总为几个紧凑的矩阵和向量，而无需保存原始点（参见图5的下部）。合并后的点将保留在体素地图中，用于里程计和地图优化。
6 实验
在整个论文的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树搜索最近邻带来的增益，实际上基于非线性优化的后端难免还是要比基于滤波器的后端实时性要差一些的。
7 算法应用场景解析
由实际应用看下来这个算法存在以下特性：
- 适应多种雷达：不管你是机械式还是固态，不管是16线还是128线，得益于自适应体素化的特征提取方法，全都可以在一个框架下完成特征提取与对应
- 场景要求更高：如果连线面特征都提取不出来，场景给的约束可能会不够充分
- 实时性略差：相对基于滤波的后端要差一些。比如一不当心某个特征体素内点云太多了，Hessian矩阵就会有很高的维数，求解非线性优化步的耗时就会很长
因此除了作为SLAM的后端，这个算法一个很适合的应用是搞激光雷达外参标定：不管是雷达到车体（opencalib[5]的雷达到车体自标定[9]有借鉴这个思想），还是多激光联合标定[3]，都已经有行之有效的论文和开源代码。吃场景和缺乏实时性对于传感器标定的业务来说，是不值得一提的弱点：一般标定不会跟智驾同时运行，可以有充分的算力，而且可以对场景提出要求。此外这个算法还有新的一代BALM2 [2][7]，其最大的亮点是引入了PointCluster的概念，加快了计算速度，并且能计算当前位姿的uncertainty，还围绕这个概念设计了一套完备的理论。但是其实BALM1对于离线SLAM应用来说已经效果非常不错了。