动态环境下激光SLAM算法专题介绍[一]

1、ICRA-2021，“ERASOR: Egocentric RAtio of pSeudo Occupancy-based dynamic object Removal for Static 3D Point Cloud Map Building”

ERASOR 算法框架如下：
首先，通过常规的激光SLAM手段获得了原始点云地图后，回放数据以query scan的位姿为中心从地图中截取对应的submap，再将两者按同样的方式划分成一个个 bin；
随后，为两者的每一个 bin 计算占据状态描述子（点云高度差）；
接下来，找出 submap 中那些描述子之比<0.2 的 bin，作为潜在的动态 bin，也就是 query scan 中bin 的高度差远小于submap中 bin 的高度差；
最后，在每一个潜在动态 bin 中用主成分分析法拟合地平面，将地平面以上的点作为动态点滤除；

总结：ERASOR 的效果依赖较为严格的假设，即所有的动态物体必须位于地平面上，高度区间必须为 -1~3米之间。这一假设在标准的开阔公路路面上能够满足，但在其它不规则的环境下有很大的失效风险。此外，计算占据状态描述子（R-POD）时仅取了一个高度差，方式过于简单，可能容易受噪声/误差干扰，如果换成分布描述可能更准确。并且每个bin内的地面位置是独立估计的，如果参考附近bin的信息，应该会更准确，并且可以(部分)解决假设过于严格的问题。
论文：https://arxiv.org/abs/2103.04316 代码：https://github.com/LimHyungTae/ERASOR-------离线思路

2、RAL-2018，“The peopleremover – removing dynamic objects from 3D point cloud data by traversing a voxel occupancy grid ”

该算法本质上是一种ray casting-based method，基本思想是一个栅格先被激光点击中（hit），而后又被其它激光点的光路穿过（miss, or intersect, or see-through），那么这个栅格是动态栅格。但是当入射角比较大时（下图），红色点的光线穿过绿色点所在栅格，导致绿色点被误杀，常见于地面/墙面。因此，基于这种情况提出peopleremover 算法，其流程如下：

首先，构建一个全局栅格地图（global occupancy grid），并将SLAM中的每一帧点云（scan）按照其SLAM位姿投影到栅格地图中（The peopleremover 操作的对象是栅格，最终输出的也是“哪些栅格被标记为动态”）。
随后，对一帧激光扫描的处理：1、先将激光点按距离原点的远近排序；2、选距离最近的点，在光路上距离该点“一个体素对角线长度”的位置定一个球心，以“一个体素对角线长度”为半径做球，球体后方被遮挡的区域，称之为shadow area，所有被 shadow 的点将作为该点的neighbors，借助neighbors计算该点处的法向量，安全面”垂直于法向量且距离该点 “一个体素对角线长度”，再遍历该点光路上安全面内的所有栅格；3、随后对该点的shadow点利用同样的安全面遍历栅格即可；4、将第2、3步中被处理过的点标记为“已处理”；5、从剩下未处理的点中选出最近的点，回到第2步；所有的点都“已处理”，结束。

论文：https://robotik.informatik.uni-wuerzburg.de/telematics/download/icra2018.pdf 代码：https://sourceforge.net/p/slam6d/code/HEAD/tree/trunk/-------离线思路

3、IROS2020，“Remove, then Revert”

该算法本质上是一种visibility-based，基本思想是在某一个视线方向上，只能看到离你最近的物体，该物体后方的物体则被遮挡，是不可见的。如果环境中全部都是静态物体的话，是不会出现这种遮挡的，如果发生了，那我们就倾向于认为，在这个视线方向上更近处的点（脸前的公交车）是动态点。算法流程如下：
首先，将所有的scan划分为一个个batch；
随后，遍历每个batch内部：1、先随机挑选一个scan在自身视角下转换为深度图（range image），再将batch内其余scan对应的submap转换到挑选scan坐标系下（统一视点），再将转换坐标后的submap也转换为深度图（同一个方向上是可能有多个点的，但这里只取最近的那个点的深度作为中相应像素的深度值），直接将二者做差，如果submap中某一个像素的深度小于scan中对应像素的深度，那就意味着地图中的相应点遮挡了 query scan中的点，地图中的这个点就是动态点——此时以最高的图像分辨率做Remove；2、随机轮流按照第1步骤遍历batch内的所有scan标记动静点，最终投票得到当前batch内的全部静态和动态地图(此时静态地图为准确的，而动态地图包含大量被误杀的静态点)；3、由于SLAM位姿误差或点云畸变引起的角度投影误差，使得高分辨率容易将静态边界误杀而低分辨率错杀情况明显减少。所以我们为了避免大量的错杀使得动态地图包含大量的静态点，我们用更低分辨率的深度图把“曾经被误识别为动态点”的点，重新恢复到静态地图中。因此，我们增加角分辨率即降低图像分辨率对batch内的点云重复第1、2步骤将动态地图中的点恢复到静态地图中。——此时以较低的图像分辨率做Revert；4、重复第3步骤，直至所有的设定分辨率执行完成，进而获得该batch范围内的最终静态地图和动态地图；
接下来，将所有batch中的静态地图和动态地图分别组合便可完成动静地图的划分；
论文：http://ras.papercept.net/images/temp/IROS/files/0855.pdf 代码：https://github.com/irapkaist/removert-------离线思路

4、IROS-2021，“RF-LIO: Removal-First Tightly-coupled Lidar Inertial dometry in High Dynamic Environments”

该算法提出基于自适应的多分辨率Range Image的动态点移除算法，基本思想是使用紧耦合的激光雷达惯导里程计，先去除移动物体，然后将激光雷达扫描与子图相匹配，构建基于优先移除的面向高动态场景的LIO。算法流程如下：
首先，IMU预积分模块用于推断系统的运动并生成IMU观测，再特征提取模块对点云的运动失真进行补偿并通过粗糙度来提取边缘和平面特征；
随后，1、初始姿态是由IMU测距得到的；2、然后用上一时刻IMU预积分和扫描匹配之间的位姿误差传递预测当前时刻的IMU预积分误差，进而得到初始分辨率；3、RF-LIO使用这个初始分辨率，分别从当前激光雷达扫描和相应的子图中构建Range Image图像，通过比较它们的可见度，删除子图的主要移动点；4、RF-LIO将激光雷达扫描与去除动态的子图相匹配，并判断扫描匹配是否收敛；5、如果是收敛的，在图形优化后，最终的精细分辨率被用来去除当前关键帧中剩余的移动点。否则，根据里程计误差将产生一个新的分辨率，并重复第3、4和5步骤。

论文：https://pan.baidu.com/s/1GdwaNrH80mgem4xbZKdZ-Q#list/path=%2F 提取码：384o-------在线思路

5、ICRA-2021，“DynamicFilter: an Online Dynamic Objects Removal Framework for Highly Dynamic Environments”

这篇论文的方法包括两部分：scan-to-map前端+map-to-map后端。算法流程如下：
首先，框架将连续 scans 及其 poses 作为输入，前端借助Romve的方法循环对子图和scan的range images进行对比用于动态对象移除。去除后，可以得到原始静态子图和动态子图 。请注意，在大入射角和遮挡的情况下，上述Romve方法存在缺陷，保留了稀疏的静态点。然后，使用采用了 Revert提出了基于地图的恢复方法来从稀疏特征动态子图中恢复更密集的静态点(其中对于动态地图中的每个动态点，首先在静态地图中搜索它的最近点。然后，我们使用主成分分析来计算最近点集的特征向量。类似地，对于这组最近点和动态点本身的并集也可以计算另一个特征向量。最后，通过比较这两个特征向量之间的法向距离，可以得到加入动态点后的分布变化。如果变化足够小，我们将动态点还原为静态）-------由于连续帧的范围有限，前端生成的静态子图可能不尽如人意。
随后，引入 map-to-map 后端进一步优化结果。后端的核心思想是计算查询姿态附近的多个静态子图中计算每个体素中的占用概率。当光线穿过体素时体素被标记为 free，体素被击中记为occ。为了近似射线追踪过程、提高算法速度，首先将距离维度离散为多个深度通道，考虑大入射角情况下，同一表面可能探测到不同深度。这些问题会导致在每个像素中占据多个不同深度的情况。因此，先依次计算多个静态子图中每个点的入射角并将大于阈值距离最小的点标记为伪占据点，可以得到不同静态子图各自的 visibility 边界，进而统计每个单元格落在可见性边界的次数并计算出占用概率，以加速光线追踪过程。

论文：https://sites.google.com/view/dynamicfilter/-------在线思路