RLOAM/RO-LOAM

LOAM框架

LOAM框架包含三个步骤：

• Scan registration：从原始激光扫描点数据中提取点特征。点特征是角点或者面点。

• odometry estimation：在特征提取之后，特征点传递到里程计模块，通过特征匹配和优化步骤计算相对坐标变换即当前帧激光扫描数据相对前一帧激光扫描数据的位姿。里程计模块输出一个6-DoF位姿估计，作为建图模块的初始位姿估计。

• mapping：
  建图模块的第一步是地图准备，将地图的特征点排序并存储在一个立方体结构中，用于查找当前扫描点的特征与地图特征的对应关系。
  $$J(q,t)=\sum _{c^{\mathcal{S}}\in C^{\mathcal{S}}}\rho (\Vert f^{\mathcal{S}}(c^{\mathcal{S}},q,t){\Vert }^2)+\sum _{c^{\mathcal{C}}\in C^{\mathcal{C}}}\rho (\Vert f^{\mathcal{C}}(c^{\mathcal{C}},q,t){\Vert }^2)$$

  $C^{\mathcal{C}}=\{c_1^{\mathcal{C}},c_2^{\mathcal{C}},\cdots ,c_n^{\mathcal{C}}\}$是角点特征的特征点对应关系，$C^{\mathcal{S}}=\{c_1^{\mathcal{S}},c_2^{\mathcal{S}},\cdots ,c_m^{\mathcal{S}}\}$是面特征的特征点对应关系，特征点对应关系的格式为$c=\{p_1,p_2\}$，其中$p_1$是当前帧雷达扫描的点，$p_2$是地图的点。$q,t$是雷达坐标系转换到地图坐标系的四元数和平移向量。通过优化得到最优坐标变换，再次迭代执行特征点匹配和位姿优化。在两次迭代之后，使用最终的最优坐标变换将当前帧雷达扫描点的特征点插入到地图中。随后，更新后的地图可用于估计下一帧激光扫描的6-DoF位姿。

R-LOAM

Motivation动机

利用环境中的参考物体等先验知识来改善定位精度，减少漂移，提升地图构建质量，但之前的大多数工作都是利用二维CAD平面图等二维先验信息，即使使用三维模型，对3D先验信息的处理也很粗糙，例如对3D CAD建筑图采样得到初始点云地图进行定位，没有用3D先验信息优化建图的算法。该算法率先提出利用3D参考对象进行轨迹和地图优化，并提出了一种使用具体使用3D三角网格模型的方法。

Challenge挑战

1. 该算法假设已知参考对象的3D模型及其在全局坐标系的位置，而且该算法对于参考模型的模型精度和位置精度要求很高，建模误差和模型位置误差都会导致算法性能的下降，甚至造成反向提升的效果。

2. 该算法只针对激光雷达，应用范围受限，也不可避免的继承了激光SLAM的固有缺陷。

能否扩展到视觉传感器?

能否不知道参考对象的全局位姿,只知道参考对象的几何信息?

能否利用除几何信息外的先验信息?例如,桌子的四个腿成矩形排列,桌子上面有个碗。

先验信息能否来自AI检测或分割算法?

能否利用更高级、更抽象的先验信息帮助定位建图?

主要贡献

拓展了主流激光定位LOAM算法框架，在后端优化中不只利用激光特征点信息，还利用从参考物体的3D mesh模型中提取的网格特征，网格特征是在参考对象mesh模型表面上采样的虚拟点（mesh模型表面距离当前激光扫描点最近的点）。point-to-mesh匹配特征的欧式距离平方和作为新的代价函数加入LOAM的常规地图优化步骤。

使用了Axis-Aligned-Bounding-Box-Tree(AABB Tree)数据结构来加速网格特征匹配，能够处理由数十万三角网格组成的3D mesh模型。不止利用已经提前构建好的地图来做全局定位，还考虑与参考物体的相对位置减少长期漂移。

技术方案

首先假设参考对象的模型及其在全局坐标系下的位姿是已知的，模型以3D三角网格的形式给出。

AABB-Tree 每个叶节点都包含网格的一个面。

1) 点云分离 Scan-Isolation

1. 使用odometry模块的输出作为initial pose将原始点云数据从雷达坐标系转换到地图坐标系。

2. 分离点云

   从原始点云数据中分离出属于参考对象的雷达扫描点。具体方法为只保留一个比参考对象3D模型大一点的3D bounding box之内的点云。理想情况下，只有属于参考对象的雷达扫描点会被保留下来。
   需要权衡裁剪点云的方法，如果裁剪条件太严格，则必须要很准确的雷达初始位姿估计，否则可能剔除很多参考对象的扫描点，导致优化结果效果不好。而如果裁剪条件太宽松，则可能保留很多不属于参考对象的扫描点，导致反向优化使结果更差。

2) 网格特征提取 Mesh Feature Extraction(包含scan-to-model Alignment)

3D triangular mesh 模型是由一个个三角形面组成，每一个三角面由其顶点隐式定义。

1. 在AABB-Tree的帮助下，可以高效计算出分离的点云与模型三角网格的对应关系。网格特征是三角网格平面上距离对应扫描点最近的虚拟点。
2. 如果没有AABB-Tree，最简单的寻找扫描点与mesh对应关系的算法是蛮力算法(遍历)，计算量非常大；而遍历AABB-Tree非常高效，因为只有在bounding box内的点才会被考虑。
3. 一但为每个扫描点都找到了mesh面上的虚拟点，point-to-mesh对应关系就找到了。

3) 联合优化
$$f^{\mathcal{M}}(c^{\mathcal{M}},q,t)=(q{p}_1{q}^{-1}+t)-p_2,\mathrm{with}{p}_1,p_2\in c^{\mathcal{M}}$$

$$J(q,t)=\frac{1}{\stackrel{ˉ}{\overline{C^{\mathcal{S}}}}}\sum _{c^{\mathcal{S}}\in C^{\mathcal{S}}}\rho (\Vert f^{\mathcal{S}}(c^{\mathcal{S}},q,t){\Vert }^2)+\frac{1}{\stackrel{ˉ}{\overline{C^{\mathcal{C}}}}}\sum _{c^{\mathcal{C}}\in C^{\mathcal{C}}}\rho (\Vert f^{\mathcal{C}}(c^{\mathcal{C}},q,t){\Vert }^2)+\frac{\lambda }{\stackrel{ˉ}{\overline{C^{\mathcal{M}}}}}\sum _{c^{\mathcal{M}}\in C^{\mathcal{M}}}\rho (\Vert f^{\mathcal{M}}(c^{\mathcal{M}},q,t){\Vert }^2)$$

$\stackrel{ˉ}{\overline{C^{\mathcal{S}}}},\stackrel{ˉ}{\overline{C^{\mathcal{C}}}},\stackrel{ˉ}{\overline{C^{\mathcal{M}}}}$是对应关系集合的元素数；$\lambda$是加权乘子，调整point-to-mesh残差的影响。

就像 LOAM 一样，在得出最终优化的 6-DoF 姿势之前，我们至少运行了两次对应估计和联合优化迭代。直观地说，联合优化问题最小化了扫描点和地图点之间的残差，同时也最小化了孤立扫描点对网格表面的残差。这有效地将扫描与网格表面对齐，在当前环境中提供绝对参考并减少长期漂移，同时还考虑了构建的地图。地图构建过程与LOAM相同。

和SOTA的对比结果

与LOAM算法的对比，在三个场景中 APE中位值分别降低了超过92%，69%，94%。

而随着迭代次数的增加，R-LOAM的提升更明显。

网格特征是对点特征的重要补充。在某些数据集上，由于点特征很稀疏，LOAM可能无法运行成功，而由于R-LOAM利用了3D参考模型补充了mesh feature可以成功跑通。

指标：

• RPE(relative pose error)相对位姿误差

• APE(Absolute Pose Error)绝对位姿误差
  APE 首先将真实值和估计值进行对齐，然后计算每个真实值和估计值之间的偏差得到，适合评估整条轨迹的全局一致性。

• RMSE(Root Mean Square Error)均方根误差

• STD(Standard Deviation)标准差

RE(Rotational Error)旋转误差

RO-LOAM

Motivation动机

R-LOAM没有对于扫描-模型误匹配的处理机制，一旦使用误匹配的扫描-模型误差项优化，会极大影响定位和建图的效果。R-LOAM中的利用先验信息的优化与LOAM中的map optimization紧耦合，这限制了算法在线运行下的地图优化迭代次数。

Challenge挑战

1. 与R-LOAM类似,RO-LOAM依赖于参考对象的准确全局位姿估计和几何形状,全局位姿的误差会严重影响算法性能,模型与参考对象的实际几何形状之间的偏差也会增加扫描点-模型匹配阶段的误差。
2. 该算法只针对激光雷达类距离传感器,应用场景受限。

主要贡献

1. 提出了一种利用3D参考模型建图的扩展算法,可以用于任意其他3D LiDAR-SLAM的插件扩展，以实现基于参考对象的轨迹和地图优化。
2. 提出了一种基于EKF的判断扫描点与参考模型的配准程度的评估方法,只有在高度准确的scan-to-model匹配时才会触发TMO(trajectory and map optimization)轨迹和地图优化。提高了算法对于误匹配的鲁棒性。
3. 作为3DLiDAR-SLAM的松耦合扩展，可以与SLAM算法异步执行。与R-LOAM相比，该算法能够卸载到边缘云以完全并行的方式在线运行，降低机器人端侧的负载。

技术方案

1. 扫描点云分离 Scan-Isolation

   点云分离阶段，与R-LOAM相同，目的是保留只属于参考对象的扫描点。

2. 扫描点到模型的匹配 Scan-to-Model Alignment

   扫描点-模型匹配阶段的目的是精细化LOAM的输出map-optimized poses。这是通过匹配分离后的点云与参考模型来实现的，然而，扫描点与参考模型的匹配质量对于优化效果影响很大，错误的匹配会增大位姿误差。为此，RO-LOAM改进了R-LOAM的匹配方式，使用一系列连续的时序数据而不是只是某一时刻的数据去评估点云-模型匹配的准确性。

   在接收到L次分离雷达扫描点后，如果最近的M+1次分离扫描点云都超过50个点，则执行分离扫描点与模型的配准操作。之后的分离扫描点云和对应的校正位姿会被缓存直到这个条件被满足。M+1帧从时间t-M到t采集的被分离的扫描点${\mathcal{P}}^{D_{t-M:t}}$与模型的匹配可以多线程执行。参数L控制TMO的频率；参数M控制了下一阶段候选者评估的序列长度，M过小将导致EKF的输入序列过短，达不到收敛，M过大更可能导致由于状态转移函数的偏差而收敛到错误值，这两种情况下，执行TMO的次数都会减少。

   在匹配之前，会统一执行一次降采样将分离点云的数量限制在500以限制计算复杂度。使用距离阈值来去除外点提高鲁棒性。匹配方法为ICP迭代最近点，执行100次匹配估计和优化迭代以收敛到极小的位姿误差。

   该模块的输出是与模型配准后的位姿${}^W{\tilde{T}}_{D_{t-M:t}}$ 。

3. 候选者评估 Candidate Evaluation

   接收上一模块的${}^W{\tilde{T}}_{D_{t-M:t}}$ 输出，并评估扫描-模型匹配的精度是否达标，如果达标则将${}^W{\tilde{T}}_{D_t}$输出到下一模型。

   ${}^W{\tilde{T}}_{D_{t-M:t-1}}$作为观测输入$z_K$，对于每个观测，EKF执行一次预测步和一次校正步，最终通过一步预测得到预测状态$x_{K|K-1}$作为最后一个模型配准位姿${}^W{\tilde{T}}_{D_t}$ 的运动先验${}^W{\hat{T}}_{D_t}$。

   如果${}^W{\hat{T}}_{D_t}$与${}^W{\tilde{T}}_{D_t}$的平移误差和旋转误差均小于阈值，则说明扫描与模型配准的精度足够高，将${}^W{\tilde{T}}_{D_t}$输出到下一个模块优化轨迹。

4. 位姿图优化 Pose Graph Optimization (PGO)

   PGO校正自上次TMO以来的轨迹，使用校正后的位姿可以用来更新地图。

   在接收到模型配准位姿${}^W{\tilde{T}}_{D_t}$之后，从LiDAR-SLAM算法的mapping module接收的map-optimized poses ${}^W{T}_{D_{t-X:t}}$构建位姿图，前一个TMO在时间$t-X$执行，当前TMO在时间$t$执行。节点由边连接$\epsilon =\{e_{0,1},e_{1,2},\cdots \}$，每一条边$e_{i,j}=⟨{\Omega }_{i,j},q_{i,j},t_{i,j}⟩$代表一个连接节点$n_i$和$n_j$的图约束，$q_{i,j}$和$t_{i,j}$描述两个节点之间的相对位姿，由${}^{D_i}{T}_{D_j}{=}^W{T}_{D_i}^{-1}{}^W{T}_{D_j}$得到，${\Omega }_{i,j}$是$6\times 6$的信息矩阵，描述相对位姿的可信程度。
   $$\begin{gathered} J({\hat{q}}_{i,j},{\hat{t}}_{i,j})=\sum _{e\in \epsilon }\rho (\Vert f(e_{i,j},{\hat{q}}_{i,j},{\hat{t}}_{i,j}){\Vert }^2) \\ f(e_{i,j},{\hat{q}}_{i,j},{\hat{t}}_{i,j})={\hat{\Omega }}_{i,j}\left[\begin{array}{c}\Delta t_{i,j}\\ 2\Delta q_{i,j}\end{array}\right] \\ \Delta t_{i,j}=t_{i,j}-{\hat{t}}_{i,j} \\ \Delta q_{i,j}=q_{i,j}{\hat{q}}_{i,j}^{\ast } \end{gathered}$$
   其中${\hat{q}}_{i,j},{\hat{t}}_{i,j}$是迭代优化的节点变量，${\hat{\Omega }}_{i,j}$ 是信息矩阵${\Omega }_{i,j}$经过Cholesky 分解的下三角矩阵。

   对于每个相邻节点的相对位姿，取单位信息矩阵$\Omega =I$，在优化过程中每条相对位姿的边是平等的。最后加入上一个TMO位姿与当前TMO位姿的相对位姿边，约束${}^{D_{t-X}}{T}_{D_t}{=}^W{\bar{T}}_{D_{t-X}}^{-1}{}^W{\tilde{T}}_{D_t}$，并将位姿图的第一个节点${}^W{T}_{D_{t-X}}$，即上一次TMO优化的位姿设置为常数，这样优化器就不会再优化它。设置这条边的信息矩阵为很大的值${\Omega }_{t-X,t}=4000\ast I$。由于第一个节点是常数，并且对于当前TMO位姿设置了一个很大的信息矩阵，所以只有两个TMO pose之间的位姿节点被优化，输出结果为${}^W{\bar{T}}_{D_{t-X:t}}$，${}^W{\bar{T}}_{D_{t-X}}{=}^W{\tilde{T}}_{D_{t-X}},and{}^W{\bar{T}}_{D_t}{=}^W{\tilde{T}}_{D_t}$.

5. 地图校正 Map Correction

   有了上一阶段的输出PG-optimized poses${}^W{\bar{T}}_{D_{t-X:t}}$，就可以通过重投影之前的雷达扫描点用来校正地图。

   地图校正模块是TMO扩展算法与LiDAR-SLAM算法直接的接口，是RO-SLAM中唯一与LiDAR-SLAM算法紧耦合的模块，因为它高度依赖于建图模块的地图结构。

和SOTA的对比结果

在LOAM和R-LOAM上激活RO扩展程序可以显著减少 LOAM 和 R-LOAM 的 APE，而在车载计算机上几乎没有额外的计算成本。