作者：
Deep蓝同学
编审：Los
论文链接：https://arxiv.org/abs/2309.10896
开源链接：https://github.com/luigifreda/plvs
导读：
这篇文章介绍了PLVS系统——一个结合了稀疏SLAM（同步定位与建图）、体素建图和3D无监督增量分割的实时系统。PLVS支持RGB-D和立体相机，并可选择性地配备IMUs。该系统旨在在小型机器人平台上运行，并生成一致的周围环境的稠密地图。
本文介绍了PLVS：一个实时系统，它集成了稀疏SLAM，体素建图和3D非监督增量分割。PLVS的含义是Points(点)、Lines(线段)、Volumetric mapping(体素建图)和Segmentation(分割)。它支持RGB-D和立体相机，这些相机可以选择性地配备IMU。SLAM模块基于关键帧，使用稀疏点和线段作为特征进行提取和跟踪。体素建图模块与SLAM前端同时运行，通过将关键帧的点云反投影并融合来生成环境的3D重建。
我们支持并集成了不同的体素建图方法。我们使用一种新的重投影误差来对线段进行束缚调整，该误差利用可用的深度信息来稳定线段端点的位置估计。我们在PLVS框架中实现并集成了一种基于几何的增量分割方法，用于RGB-D相机。我们在一些公开可用的数据集上展示了PLVS框架的定性和定量评估结果。附录中详细介绍了我们采用的立体线段三角测量方法，并给出了我们用于线段误差项的Jacobian的推导。该软件以开源形式提供。
PLVS 实质上也可以称作一个开源框架。它拥有如下特征：
1. 利用关键点和关键线进行相机跟踪和稀疏建图；集成了不同的体素建图方法，特别是八叉树、八叉树地图和带TSDF的空间哈希体素。
2. 支持RGB-D和立体相机，这些相机可以选择性地配备IMU设备。
3. 在框架中实现了一种新的重投影误差，用于线段。该误差利用可用的深度信息来稳定线段端点的位置估计。而且该误差是通用的，可用于RGB-D和立体相机系统。
4. 具有RGB-D相机的体素建图的增量分割方法，该方法基于几何和非监督。目前，它仅适用于RGB-D相机，并利用SLAM提取的线段信息。
5. PLVS整体均在CPU上运行，可选择将关键点提取移至GPU以减少计算量。
▲图1. 使用1cm分辨率的八叉树模型获得的3D重建的三个方面 (a) 线段 (b) 点云 (c) 分割后的点云。为简单起见，我们不显示点特征。
▲图2. 图1中的3D重建的细节 (a) 线段 (b) 法线 (c) 点云 (d) 分割后的点云。
在PLVS中，稀疏SLAM系统的架构借鉴了ORB-SLAM架构。它在图像上提取关键线段，并与关键点一起用于3D线段和3D点的跟踪、建图、束缚调整。
接下来，我们将介绍关键线段和3D线段的管理、重投影误差以及我们采用的表示方法的更多细节。在本文中，我们采用以下符号：
设为图像域，通用第个RGB-D图像由校正后的彩色图像和注册的深度图像组成。深度图像对每个像素返回与图像平面最近的物体点拦截的光线与相机中心之间的正距离。
与对应的相机变换矩阵记为，将一个点从世界坐标系变换到相机坐标系。有
其中和。对于3D点，我们使用上标和表示在相机和世界坐标系中的点坐标。
给定向量，定义向量。此外，定义运算符为，设为固定的相机标定矩阵。3D点在图像上的投影为：
其中定义了焦距，是主点。
给定深度图像和像素，该像素在相机中的3D反投影点为：
其中。
对于RGB-D图像，我们定义(稍微滥用符号)投影建图和反投影建图如下：
在图像平面上，我们考虑以下形式的2D线方程:
其中是单位法向量，是考虑线段到原点的符号距离。给定两个点，我们计算和(或)。或者，我们可以计算齐次表示，等价的线方程为。
给定协方差矩阵 ，两个向量之间的马氏距离为。我们用表示单位矩阵。
3.1 点的重投影误差
在本节中，我们描述了在PLVS中使用的不同点重投影误差。
考虑通用特征点，具有个观测值及其欧几里德表示。在这方面，点观测是在特定帧(也可称为关键帧）中收集的，如果其对应的深度是有限存在的，则被视为立体；否则为单目。
单目点观测的2D重投影误差定义为：
其中协方差矩阵为，是点检测噪声方差特征点在高斯图像金字塔的不同尺度上提取。噪声方差取决于检测到的金字塔层级。是Huber稳健代价函数。
在双目相机的情况下，我们假设图像未失真且校正过。使用双目相机，立体点观测为，其中前两个坐标是在左图像中的像素测量值，而是在右图像中的行测量值。在这种情况下，经典的立体重投影误差为：
其中是立体基线，协方差矩阵为，且。
这里，我们从视差估计深度为。
对于RGB-D相机，不存在。但是，它可以通过使用“虚拟”基线（一个常数参数）并计算为来模拟，其中是测量的点深度。在这种情况下，立体重投影误差定义为：
其中协方差矩阵为：
且
是式(8)范数论点的雅可比矩阵，是轴向深度噪声模型。对于RGB-D相机，我们考虑并测试了使用协方差矩阵(9)和更简单的对角协方差矩阵，后者适用于立体支架。
原则上，没有考虑到立体重投影误差(7)和轴向深度不确定性与测量深度之间的实际关系的第一个和第三个分量之间的相关性。但是请注意，标定过程可能会引入建模错误，不能保证一定会带来更好的结果。
作为升级的替代方案，我们还考虑了以下专门适用于RGB-D相机的立体重投影误差：
并伴随着一个更简单的对角协方差矩阵：
在我们的测试中，我们观察到简单的立体协方差可以获得更稳定和准确的结果。
▲图3. 示意图显示了如何将同一3D线段的三个线段观测值反投影到3D线段l上的不同元素的类Cl上。
3.2 线段表示
稀疏建图中3D线段由一对3D端点表示。在图像中检测（遵循2D线段拟合阶段）包括一个支持区域，它是一个由两个端点表示的2D紧凑连接区域。匹配的图像端点对也称为的第个线段观测。
由于噪声、可能的遮挡和其他因素，3D线段可能无法被相机完全观测到，端点在图像之间通常不一致。此外，3D线段可以分割成多个线段（属于3D中的同一条线）。实际上，反投影的所有观测值会产生一类3D线段，其中一般情况下，对有（见图3）。在此，为简单起见，我们省略了提取/匹配时构建的隐式建图，它将的第个观测与第个帧/关键帧相关联。
我们使用以下要素来表示3D线段：
1.在通用图像中检测并成功匹配的2D图像端点;
2.与相机的坐标框相关的3D反投影端点;
3.在建图中表示线段的3D建图端点。
为简单起见，我们不明确地表示在提取/匹配时构建的隐含建图，它将的第个观测与第个关键帧相关联。
如果和存在且有限，则线段观测被定义为立体;否则为单目。仅当为立体时，才能计算出和。3D反投影点的可用性允许引入一种新的误差类型，该误差在捆绑调整阶段考虑可用的深度信息以更好地估计线段端点的位置。
一般来说，RGB-D相机提供和的深度。在使用两台相机或两张不同相机图像之间三角化线段的情况下，可以使用附录中描述的方法以闭式解决方案估计深度。
3.3 线段检测与匹配
对于每个处理的图像，首先计算高斯图像金字塔。然后，使用EDlines方法在图像金字塔的每个级别上提取线段。这是一种线性时间算法，还包括错误检测控制。我们选择EDlines方法是因为其可重复性、精度和效率。
在图像金字塔的每个级别上，我们使用一个特定的噪声方差模型来建模端点检测过程中的不确定性。我们用表示图像端点上每个坐标的噪声方差。
为了实现帧间线段匹配，使用线带描述符（LBD）方法为每个提取的线段关联一个二进制描述符。
我们使用平铺技术来加速线段匹配。具体来说，在提取时，我们为每个线段计算其法线方向和符号距离。我们将2D参数流形分割成平铺。因此，对于每个图像，我们将提取的线段分组到其对应的平铺中，并存储结果划分在中。
在匹配时，我们首先将3D线段投影到目标图像上; 然后，我们计算其投影线表示; 接下来，我们将参数位于同一平铺中的的线段识别为在中的候选匹配项。最后，根据描述符之间的汉明距离对找到的最近邻进行评分。
为了拒绝可能的错误匹配，如果:
(i)其与第二近邻的汉明距离比低于一定阈值（通常为0.8）；
(ii)其线际距离低于一定阈值，则验证找到的最近邻。
▲图4. 普通3D线段，它在图像中的观测值以及建图端点及其在相机中的投影e_2Dpq(p_i,q_i)所描绘的垂直点线距离。另一方面，3D反投影误差在式(26)中组合了和与3D线的3D垂直点线距离以及之间的3D端点距离。
3.4 线段的重投影与反投影误差
为了将线段纳入捆绑调整（BA），我们需要定义适当的线段重投影误差，并计算其相对于相机姿态参数和3D线段参数的微分。
考虑通用线段，具有个观测值及其在稀疏建图中的3D表示。在BA中，每个观测与关键帧中的匹配，关键帧的相机变换为。
给定观测，其方程参数遵循式 (5)。在图像平面上，我们将3D点的相机投影与通过点和的2D线的距离计算为：
我们定义2D线间距离向量为：
其协方差矩阵为：
其中是式(13)相对于两个图像端点坐标的雅可比矩阵。在此，为简单起见，我们假设对角。
给定单目线段观测，其2D重投影误差定义为：
其中是稳健的Huber代价函数。不过在实际操作中，我们允许用户可选地选择柯西代价函数。
值得注意的是，式(17)组合了图像平面上的点线距离，如图4所示。在我们的实验中，我们观察到仅当BA中包括至少有个观测的单目线段时，才能获得更好的结果。
现在，进入3D，我们定义以下3D距离：
其中式(18)表示地图点与3D线之间的3D垂直距离【通过使用式(20)变换到相机帧】，式(19)表示与其相关图像点的反投影之间的距离。
与上面类似，我们定义3D距离向量：
其中标量权重用于赋予不同的重要性。通常，我们优先最小化距离（即）。
的协方差矩阵设置为：
其中是式(18)相对于两个反投影端点和坐标的雅可比矩阵，是式(3)相对于的雅可比矩阵。
值得注意的是：
1.为简单起见，我们规定对角；
2.表示3D反投影点的(诱导)噪声协方差矩阵。
对于立体线观测，我们使用以下3D反投影误差：
它组合了3D线际距离和3D端点端点距离，如图4所示。
为了正确计算距离（这些距离为中的组合元素），我们需要正确关联与以及与。这是通过在初始化BA时计算两个候选配对和之间的距离来实现的，然后实际确定最近的一对点。
在实践中，许多试验确认，如果不使用3D反投影误差，则3D线段端点的位置通常沿线方向漂移，或者线方向估计趋于降级。在大多数情况下，由于这些在后续帧中无法匹配和跟踪，因此线段剔除会在几帧后从中删除许多不稳定的线段。图5显示了在包括和不包括反投影误差的BA中对同一环境构建的两个地图。
▲图5. 为同一环境构建的两个3D线段地图。左侧：在不使用反投影误差的情况下构建的地图。右侧：通过在BA中包括反投影误差获得的地图。
3.5 稀疏建图捆绑调整
稀疏SLAM的后端使用完整的捆绑调整（BA）来计算稀疏建图中的模型参数的最优估计。这些是关键帧的姿态，3D特征点的位置以及线段的3D端点位置。另一方面，局部BA仅优化与当前参考关键帧可视的一组关键帧相关的点、线段和姿态的模型参数。
在BA中，估计的最优性是由所最小化的重投影误差隐式定义的。在适当的数学公式中，BA等价于MAP或MLE估计，这取决于是否存在先验。
完整BA最小化的目标函数是:
其中是关键帧集，和分别是在第个关键帧中被观测到的点和线段的子集，上标表示单目或立体观测。优化使用Levenberg-Marquardt方法执行，其中增广正规方程:
被迭代求解以获得建图参数向量的更新。这里，误差向量组合了式(27)中包含的所有重投影和反投影误差，组合了所有对应的协方差矩阵。
SLAM模块生成的每个新关键帧都作为VOMA的输入。首先，VOMA将每个新关键帧的depthmap反投影到表示在相机坐标框中的点云。为此，我们使用图像像素上的建图。
与此平行，在反投影时对每个RGB-D图像增量且稳健地计算法线。特别是，在当前反投影深度图像的每个3D点上，我们通过在4邻域上进行中心差分对4个获得的法线进行区域加权平均。来自不同RGB-D图像的法线在我们的自定义八叉树地图中进一步集成和平均。
不同的方法可用于融合计算的关键帧点云。特别是，八叉树、八叉树地图和带TSDF的空间哈希体素。我们在以下子部分中简要介绍它们。
4.1 基于八叉树的地图
在这种情况下，我们使用八叉树数据结构有效地表示3D体素化地图。提供两种不同的基于八叉树的体积建图。
1)我们的自定义基于八叉树的模型：每个体素表示平均所有集成点(坐标、颜色和法线)的质心。可以启用投影关联和空间锯齿以应对环境中发生的动态变化；
2)fastfusion方法：这是一个非常高效的多尺度八叉树，在多个级别存储符号距离函数(SDF)。
4.2 八叉树地图模型
在这种知名方法中，使用3D占用网格对3D空间体积建模。使用适当的传感器模型和光线投射操作以概率方式实现新的点云集成。属于同一体素的点的颜色被平均。
4.3 带TSDF的空间哈希体素
使用带TSDF的空间哈希数据结构来表示3D空间体积。一般来说，TSDF在许多观测下可以平滑传感器噪声。在这种情况下，从构建的TSDF中使用Marching Cubes方法提取网格模型，以表示周围的3D表面。提供两种不同基于TSDF的方法。
我们调整和改进了原始开源代码的一些部分，以达到更好的实时性能。
该方法提供了高效的实现，最初是为了从TSDF中增量构建ESDF（用于规划目的）而提出的。
4.4 实现细节
在VOMA中，一个FIFO队列收集来自SLAM模块的所有新关键帧。每次处理并集成个关键帧到体素建图模型中，其中是一个由用户定义的整数参数。随后执行体素模型更新。后者包括可能的网格模型更新(这取决于所采用的体积方法，例如通过使用增量Marching Cubes)，以及提取全局点云地图(例如由可视化器使用或提供给其他目的)。
一般来说，网格模型的更新和从TSDF提取零水平面（例如，使用Marching Cubes）需要比单个点云集成更多的时间。原因是网格模型更新需要考虑来自体素邻域关系的更多信息。
为了在稀疏和体素建图之间保持一致性，每当稀疏SLAM建图或其基础姿态图进行全局调整时（例如在检测到闭环时），体素建图会被重建（或调整）。
4.5 增量分割
我们的增量分割方法仅适用于RGB-D相机和激活我们的自定义基于八叉树的VOMA时。对于原始方法，在深度图像分割阶段，我们还集成了SLAM生成的可用线段信息，以更好地定义段边界。
我们已成功使用不同传感器（Asus Xtion Pro、ZED立体摄像头、Intel Realsense等）实时测试了PLVS框架，以证明其通用性。图6显示了一个配备Realsense R200的tracked robot构建的体素建图(自定义基于八叉树的方法)。图8则显示了我们使用手持Asus Xtion Pro实时获得的一个实验室的3D重建和分割。
▲图6. 配备Realsense R200的tracked robot以及构建的基于八叉树模型的体素地图。
▲图7. 增量分割细节：(a)PLVS；(b)inSeg。
▲图8. 在大型办公环境中进行的分割
▲表I：TUM RGB-D数据集中评估的静态场景的ATE RMSE。在PLVS系统中，我们在所有序列上使用了相同的参数。帧丢失列报告了缺失帧的百分比（这些帧从未被原始捕获设备记录），这些数据是根据ElasticFusion论文提供的。
▲表II：ICL-NUIM数据集中评估的合成序列的表面重建精度的比较。报告的值是每个点与真实3D模型中最近表面之间的平均距离（以米为单位）。比较的值是从ElasticFusion论文中提取的。
PLVS I框架的获得的评估结果表明，PLVS I的定位和3D重建精度与SLAM文献中众所周知的系统相当。值得注意的是，我们在所有TUM RGB-D序列上使用了相同的PLVS参数集(每个数据集不fine-tuning)。
我们提出了PLVS，一个模块化和通用的系统，它展示了特征基础SLAM系统的跟踪“敏捷性”，并且能够仅依靠CPU生成稠密体素建图。支持不同的体素建图方法：八叉树、八叉树地图和带有TSDF和网格的空间哈希体素。系统的不同功能按部门组织，用户可以以多种方式启用/禁用和配置。这允许根据需要精细地在建图准确性/分辨率与CPU负载之间进行权衡，并将框架功能适配到手头的系统。
PLVS I框架的评估结果表明，PLVS I的定位和3D重建精度与SLAM文献中众所周知的系统相当。值得注意的是，我们在所有评估序列上使用了相同的PLVS参数集（每个数据集不进行微调）。
PLVS是一个活跃的项目。自我们进行第一次评估以来，该系统在许多方面得到了发展。目前PLVS已有两种不同的版本，并随着时间的推移收集了许多新的优化迭代。第一个版本PLVS I基于ORB-SLAM2，而最新版本PLVS II基于ORB-SLAM3并支持配备IMU的相机系统。我们正在对PLVS I和PLVS II系统进行新一轮评估，相关结果也会尽快公布。
深蓝学院作者团队强势招募中！期待你的加入
* 点击在看收藏本文 *
点击“阅读原文”直通论文
继续阅读
阅读原文