相关工作

Adjustment是一种用于同时估计三维结构和传感器运动运动的优化算法。

在视觉SLAM三维重建等应用中它被广泛用于优化相机位姿和地图的精度。

在视觉slam中BA通过最小化特征之间的重投影误差有效得优化相机位姿和地图的一致性从而提高SLAM系统的精度和鲁棒性。

然而来到激光雷达这边情况就很不一样激光雷达点云虽然拥有深度信息但是比起图像比较稀疏单帧的出点量和图像中的像素数量根本不在一个量级导致特征点稀疏特征匹配对应难做。

那么有没有办法解决激光SLAM中的BA优化问题让它可以在后端优化、雷达外参优化等应用上发挥作用呢MARS实验室2021年6月发表在RAL的论文《Bundle

Adjustment

在本文中我们将激光BA建模为将特征点与其匹配的边缘或平面之间的距离最小化。

与视觉SLAM以及激光SLAM中先前的平面调整方法不同的是特征点可以通过解析方法求解并从BA中移除得到的BA只依赖于扫描位姿。

这大大减少了优化的规模并允许使用大规模稠密的平面和边缘特征。

为了加速优化过程我们以闭合形式导出了代价函数的解析导数包括二阶导数。

此外我们提出了一种新颖的自适应体素化方法以高效地搜索特征对应关系。

所提出的建模方法已应用于LOAM后端进行地图优化。

结果表明尽管作为后端局部BA可以非常高效地求解甚至在实时优化20个点云扫描时也能达到10Hz的速度。

局部BA还显著降低了LOAM的漂移。

相关工作

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

其中代表一个体素内的点数量。

记为协方差矩阵中第大的特征值那么就可以提取线面特征

面特征如果与的比值超越某一阈值那么就标记为一个面特征体素。

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

线特征的方向向量为对应的特征向量

如果满足上述任一条件那么这个体素就会被标记为一个线或者面特征体素。

如果不满足平面体素的条件那么就将这个体素分成八个小体素然后对这八个小体素分别判断是不是满足上述条件不断递归直到八叉树的层数达到预先设定的最大值。

这样就可以实现对局部地图快速且通用的特征提取不需要使用检测、分割的方法也不关心雷达型号(机械式还是固态)具有非常强的通用性。

在开源代码中堆叠了局部地图之后使用cut_voxel()的函数对局部地图进行体素化并建八叉树然后通过OCTO_TREE::recut()实现对局部地图的自适应体素化特征提取然后对不同体素中的点云给不同的着色进行相应的可视化操作。

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

从而可以构建点面误差或者点线误差

点面对于每一个平面体素点面误差计算表达式如下其中是平面体素中的任意一个点。

理论上随便选特征体素中的某个点作为参照不影响损失函数的计算的结果毕竟它们都应该在同一个面上。

点线对于每一个线特体素点线误差计算表达式如下同理是线特征体素中的任意一个点。

理论上随便选特征体素中的某个点作为参照不影响损失函数的计算的结果毕竟它们都应该在同一个面上。

给定一组稀疏特征点这些特征点来自于个扫描但都对应于同一个特征平面或边缘见图3。

假设第

个特征点来自于第个扫描其中将个扫描的姿态表示为其中。

然后全局坐标系中的特征点为

如前所述激光雷达BA问题是指联合确定个扫描的姿态和全局三维点云地图。

现在三维地图是一个单一的特征边缘或平面那么BA问题就简化为联合确定单一特征的姿态和位置该特征由特征上的点和单位向量是平面的法向量或边缘的方向表示。

对于平面特征直接BA公式是最小化每个平面特征点到平面上的距离的平方和

与平面特征类似边缘特征的直接BA公式是最小化每个边缘特征点到边缘的平方距离的总和

注意在公式(2)和(4)中最优点

并不唯一因为该点可以在平面内或沿边缘自由移动。

然而这对最终的优化代价函数没有影响。

此外公式(2)和(4)暗示了在BA之前可以解析地获得最优的特征平面或边缘参数而最终的BA问题只依赖于姿态T。

这与我们的直觉相符即一旦扫描姿态确定3D点云地图因此平面或边缘特征也就确定了。

此外对姿态T的优化转化为最小化公式(3)中矩阵A的特征值。

即BA导致最小化

在T上其中

为了使成本函数(5)的优化更加高效我们对位姿T进行了解析地计算了一阶和二阶的闭合导数。

由于链式法则我们首先对点向量p进行了导数的计算。

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

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

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

其中是同一个体素中来自不同帧的点云坐标。

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

定理2对于一组点

和在3中定义的协方差矩阵。

假设具有对应于特征向量的特征值。

此外当时则

二阶近似

利用前面章节中的一阶和二阶导数我们可以通过其二阶近似来近似成本函数5如下所示

其中J(p)是Jacobian矩阵其i元素在6中H(p)是Hessian矩阵其i行j列元素在7中。

请注意点向量p还依赖于扫描姿势T如1所示。

使用在[38]中定义的操作将姿势扰动到其切平面我们有

将12代入8得到

最后我们使用Levenberg-MarquardtLM[39]方法通过反复将其近似为二阶近似13来最小化成本。

在每次迭代中解决方案从中解决

自适应体素化

在第三节中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树相比它通常更高效因为当包含的特征点位于同一平面或边缘上时可以提前终止。

当环境具有大平面或长边缘时这种优势将更加明显具有自适应体素的地图将减少在激光雷达里程计中搜索特征对应关系所需的时间。

只需要搜索特征点所在或附近的体素而不是需要更多穷举搜索的最近点。

将BALM结合进LOAM

由于激光SLAM中的运动估计通常是基于IMU和里程计因此会存在累计误差。

因此应用BA优化可以有效地纠正这些累计误差提高SLAM系统的精度和鲁棒性。

原文中把LOAM的后端换成了BALM对odometry进行后端优化。

同时通过共享内存的方式将自适应体素化特征提取的结果用于前端的registration来进行加速。

我们将提出的BA公式和其优化方法纳入到LOAM框架中。

系统概述如图5所示。

它由三个并行线程组成特征提取、里程计和地图优化。

一旦接收到新扫描的特征点里程计通过将新的特征点与现有地图进行配准来估计激光雷达的姿态。

与现有方法不同的是我们利用自适应体素地图来加速匹配过程。

具体而言在构建体素地图时我们计算体素中平面或边缘的中心点和法向量或方向向量。

然后对于新扫描的特征点我们通过计算该点与体素中的平面或边缘特征之间的距离来搜索最近的体素由其中心点表示。

使用里程计可以将新的扫描点大致注册到全局坐标系并将其推送到体素地图中对于新扫描中的每个点搜索其所在的体素并将该点添加到相应八叉树的叶节点中。

如果在现有地图中找不到与新扫描中的点对应的体素则创建一个新的八叉树将其根索引到哈希表中并将该点添加到根节点中。

在将新扫描的所有特征点分布到现有八叉树的叶节点或新创建八叉树的根节点后我们按照构造方式更新体素地图如果节点叶节点或节点中的点不能构成一个单一的特征平面或边缘则将该节点分割为八个并检查每个节点。

在将一定数量的新扫描推送到体素地图后会触发地图优化。

地图优化在激光雷达姿态的滑动窗口上执行局部BA。

滑动窗口内包含的任何包含点的体素即Psw将用于构建成本项如2或4。

然后地图优化反复最小化由所有相关体素组成的总成本的二阶近似13。

这将优化滑动窗口内的所有激光雷达姿态。

更新后的姿态然后用于更新所有相关体素的中心点和法向量。

一旦滑动窗口装满来自较早扫描的点将合并到地图点中。

点协方差矩阵3的一个良好特性是存在递归形式[40]允许将滑动窗口外的所有点汇总为几个紧凑的矩阵和向量而无需保存原始点参见图5的下部。

合并后的点将保留在体素地图中用于里程计和地图优化。

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

精度高手持场景下使用Livox

Horizon绕HKU校园一圈回到终点发现用BA作为后端的LOAM平移量飘了31cm而纯LOAM平移量飘了6.23m。

同时跑了一组室内数据可以发现偏移量非常小可见用它来做后端优化还是有比较高的精度的。

不挑lidar原论文在Livox

Mid40和VLP-16上都跑了实验无论雷达多少FOV扫出来点云什么样都可以跑而且漂移量都远小于LOAM。

使用VLP16采一圈回到原点之后LOAM平移量飘了56.8cm(0.27%)而BALM飘了28cm(0.13%)运行速度在结合了BALM的LOAM框架中主要得益于不需要建KD树去搜索五个最近邻结合了BALM的LOAM表现出了较好的实时性。

当然我理解这个运行速度只是使用了自适应体素特征以及前后端线程共享内存的操作取代了原本建KD树搜索最近邻带来的增益实际上基于非线性优化的后端难免还是要比基于滤波器的后端实时性要差一些的。

算法应用场景解析

适应多种雷达不管你是机械式还是固态不管是16线还是128线得益于自适应体素化的特征提取方法全都可以在一个框架下完成特征提取与对应场景要求更高如果连线面特征都提取不出来场景给的约束可能会不够充分实时性略差相对基于滤波的后端要差一些。

比如一不当心某个特征体素内点云太多了Hessian矩阵就会有很高的维数求解非线性优化步的耗时就会很长

因此除了作为SLAM的后端这个算法一个很适合的应用是搞激光雷达外参标定不管是雷达到车体opencalib[5]的雷达到车体自标定[9]有借鉴这个思想还是多激光联合标定[3]都已经有行之有效的论文和开源代码。

吃场景和缺乏实时性对于传感器标定的业务来说是不值得一提的弱点一般标定不会跟智驾同时运行可以有充分的算力而且可以对场景提出要求。

此外这个算法还有新的一代BALM2

[2][7]其最大的亮点是引入了PointCluster的概念加快了计算速度并且能计算当前位姿的uncertainty还围绕这个概念设计了一套完备的理论。

但是其实BALM1对于离线SLAM应用来说已经效果非常不错了。