经典文献阅读之--Removert(SLAM动态障碍物滤除)

最新推荐文章于 2025-04-12 20:35:14 发布

敢敢のwings

最新推荐文章于 2025-04-12 20:35:14 发布

阅读量1.1w

点赞数 2

分类专栏： slam 文章标签：人工智能

原文链接：https://zhuanlan.zhihu.com/p/491271775

版权

slam 专栏收录该内容

207 篇文章

订阅专栏

论文原题目：2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) Oct 25-29, 2020, Las Vegas, NV, USA (Virtual)
Giseop Kim, Ayoung Kim, Remove, then Revert: Static Point cloud Map Construction using Multiresolution Range Images

1. 引言

在单次激光SLAM的过程中，动态障碍物（如：移动的车辆、行人等）会或多或少地劣化SLAM中的两个方面：（1）激光点云的配准和（2）点云地图的构建。首先第一个方面，我们取极端的情况：当一帧激光数据中的大部分点云都是从动态障碍物上返回的数据，那么该帧点云则没有能力在reference scan（或者reference map）中找到correspondent points，点云的配准就会失效。但一般的情况下，每帧激光数据受动态障碍物影响的点云是“有限的”、难以“兴风做浪”的（因为大多数的点云可以找到它们对应的correspondence，少数的异常量不会产生太大的干扰），而且若是采用点云特征进行匹配的话，动态障碍物在预处理阶段也会被剔除。那么，另一个方面，动态障碍物对点云地图的构建的影响在于会在最终生成的地图中产生“鬼影”，而若简单使用在第一种情况中提到的特征点云构建地图的话，会错误地滤除掉很多实际需要的物体信息，具体的影响可见以下图示：

动态障碍物对点云地图的构建的影响

本文提及的论文主要工作是为了解决抑或着缓解上述提到的第二个方面问题，目的是在承受了动态障碍物的干扰下，SLAM过程结束后生成的点云地图是一个近乎全由静态点云构成的地图（也就是Static Point Cloud Map Construction）。而其提出的方法论不同于基于栅格地图的概率更新消除策略和通过学习的方法找出动态点进行删除，该篇论文如其题目所说，使用的一种基于多重分辨率的深度图像来判断滤除动态点云----以构建一张Static Point Cloud Map。

2. 论文原理解析

2.1 论文概要与问题背景

Giseop Kim和他的团队提到，他们的工作是展示了一种创新的算法：命名为 REMOVERT ，在动态变化的城市环境中来构建静态点云地图（Static Point Cloud Map）。一般的动态点云（dynamic point）滤除都需要通过对比判断机制：用一张带有noise（dynamic points）的map和输入点云（query）进行比对来完成，而在这种对比判断机制中存在一个问题：当query或者noise map中的全局位姿的计算不准确时，会导致判断结果出错，如将static points判断成了dynamic points（False Negative：FN），或将dynamic points当成了static points（False Positive：FP）。

为了解决这个问题，Giseop Kim和他的团队提出了使用多重分辨率的深度图像来判断滤除动态点云并赋予能恢复被错误当成dynamic points的static points（也就是避免了False Negative：FN的情况）。REMOVERT 算法的大致流程是会首先保守地保留确定的静态点（也就是会有很多FN的情况），再通过一次次的迭代将query-to-map的搜索窗口扩大来恢复被误杀掉的静态点，其中query-to-map的搜索窗口隐式补偿了激光雷达运动或配准错误。一言以蔽之，正如算法的名称一样：在第一阶段先疯狂地Remove掉所有可能的动态点，就算杀错了也不管较粗，到了第二阶段，通过一次次地放大query-to-map的搜索窗口来恢复在第一阶段中被过度杀掉的静态点，最终达到“当了婊子还要立牌坊”的目的（Static Point cloud Map Construction）。

还有一个需要提及的方面是，文体的 REMOVERT 算法是一个离线处理的方法，也就是SLAM过程是为每一帧的点云数据赋予全局的位姿，当SLAM结束后将过程中的点云按照其对应的全局位姿全部拼接起来，REMOVERT 则是应用在离线点云的构建拼接的阶段。

论文 REMOVERT 算法使用SemanticKITTI作为基本真理的KITTI数据集上，结果能表明其方法在对比判断的定性上优于不明确区域中的人工标记数据（SemanticKITTI）。

2.2 方法论：问题定义

Setup：

在离线拼接点云地图过程中，使用的一组原始LiDAR扫描数据（原始数据就会包括静态点和动态点）来构建，其中的目标是删除原始地图中动态点。在此过程中主要考虑两个不同的坐标：全局统一地图坐标系 $M$ 和本地传感器的局部坐标系 $Q$ 。

Notations：

$P^{Q}$ 表示在局部坐标系下的query scan， $P^{M}$ 表示在全局坐标系下的map，对于 $P_{Q}^{k}$ 的位姿通过 $T_{Q}^{k}$ 表达，而 $T_{Q}^{k}$ 是经由SLAM系统计算出的包含了一定误差的结果。每一帧 $P^{Q}$ 可视作一个集合 ${ {P^{Q}} \}$ ，集合中的每个元素较粗即为每帧中的点云，表达为 $P_{k}^{Q}$ ，算法则通过将 $P_{k}^{Q}$ 与 $P^{M}$ 对比来删除map中的dynamic points。

在比较过程中，我们将目标地图 $P^{M}$ 一分为两个互斥子集：静态点云组成的静态点云地图 $P^{SM}$ 和动态点云组成的动态点云地图 $P^{DM}$

$P^{M}=P{SM}+P^{DM} $
$P^{SM} \cap P^{DM}=\oslash$

初始对比判断利用保守的分割估计 $P^{SM}$ 和 $P^{DM}$ 的方法会不可避免地包含状态预测错误。本文将点云的status分为static和dynamic两种， static status 认定为 positive § 和 dynamic status 认定为 negative (N).

$P^{SM}=TP\cup FP$
$P^{DM}=TN\cup FN$

其中 TP, FP, TN, 和 FN 分别表示 true positive(被正确判定为static的点), false positive(被错误判定为static的点), true negative(被正确判定为dynamic的点), and false negative(被错误判定为static的点)的点云集合。那么以上，这个Static Point cloud Map Construction的问题可以重新被定义为：减少FP和FN 点云集合中元素的个数。

2.3 方法论：解决思路

按照这个思路，我们如果假设所有的点只有static和dynamic两种真实状态，那么 FN 肯定是属于 TP 的子集，则我们的目标就可以转变为：

严格限制 $P^{SM}$ 的生成，即“宁可杀错不可错过”，以减少 FP 集合的元素个数；除了 $P^{SM}$ ，余下的点云就是 $P^{DM}$ ，再从一次次地从的 $P^{DM}$ 中检测出 FN 的点云，并将FN加入到 $P^{SM}$ 。REMOVERT算法就是重复执行此过程并增强预测的 $P^{SM}$ ，使其收敛得更接近true的 $P^{SM}$ 。简而言之，这种迭代static map增强策略是“ REMOVERT ”背后的核心。

具体的算法pipeline见下图：

图：论文Remove, then Revert: Static Point cloud Map Construction using Multiresolution Range Images中系统outline

上图中的 $S (k)$ 和 $D (k)$ 分别表示在第k次迭代中主要由static points和dynamic points组成的点云“地图”，而图中框住 $S (k)$ 和 $D (k)$ 的方框大小则表达成点云“地图”的大小（size），由上图的pipeline可以看到在第一次remove的结果中dynamic point cloud的点云地图大小是其最大的时候，而对应着的static point cloud则是其最小的时候；随后进入第二个大阶段：迭代revert，每次都从dynamic point cloud的点云地图中找出一定大小static points并加入到之前的static point cloud中，经过整个阶段的revert，static point cloud会越来越大，对应的dynamic point cloud则越来越小。而最终的输出点云地图就是pipeline最终累加所得Static point cloud。

那么流程介绍完了，具体的实现细节是什么呢？甚至如何进行动态点和静态点的对比判断呢？先我们带着这些问题继续往下分析。

2.4 方法论：基于深度图的地图对比判断策略

本小节的主题在于：怎么实现地图对比判断？正如本子章节的标题，是基于深度图信息来完成地图的对比判断，
Kim他们的方法本质上还是一个Visibility-based approach，通过visibility（可见性）来区分出dynamic points而不是基于栅格的ray casting方法。对于 $P^{Q}$ 或者 $P^{M}$ ，可以将其360度的环境展开转化成一张平面的深度图像，而其转化后的深度图像本质就是一个二维的矩阵，矩阵的columns会对应水平方向angle range的0度～360度，矩阵的rows会对应竖直方向上angle range（其最大最小值应与激光雷达的FoV匹配），矩阵每个元素值则对应着具体水平角度和竖直角度方向上的点云range信息。

首先为了进行对比，需要先将点云地图转化成range image。投影大尺寸地图 $P^{M} （ P^{M} > P^{Q} ）$ 到一个fixed-size range image，其变换关系是query scan中的第 $k$ 帧frame到Map frame的坐标变换。需要注意的是，每次地图的对比判断都是将global map变换到query scan所在的local frame坐标系下。 至于为什么？大家可以带着这个问题接着往下看。在投影过程中，range image的分辨率物理意义是水平方向和竖直方向上的角度分辨率（e.g，如果水平视角360度，竖直方向60度，分辨率为1度“即单个像素表示 1 度水平FoV和垂直FoV”，那么range image的大小就是360x60。），而如此将点云转化到range image就会有视场限制，在range image中的每个像素值都是map point cloud中在某个query scan frame角度下的"可见地图点云"，可见地图点云 $P_{k}^{M}$ 等效为 range image $I_{k}^{M} = \left( I_{k}^{M} \right) \in R^{m\times n}$ 。

$\div horizontalAngleResolution$
$\div verticalAngleResolution$
$I_{k,ij}^{M}=min_{p\in P_{ij}^{M}} r(p)$
$P_{k}^{M}=\left\{ P_{k,ij}^{M} | P_{k,ij}^{M}=argmin_{p\in P_{ij}^{M} }r(p) \right\}$

其中r(·)表示在某个query scan frame角度下的一系列点云 $p\in R^{3} , I_{k,ij}^{M}$ 表示range image中索引为 $[i ， j]$ 的像素值， $P_{ij}^{M}$ 是 $P^{M}$ 的子集表示的是落在了对应着 $[i ， j]$ 索引的球面坐标系（elevation and azimuth angle在像素角度块的范围内）的所有点集合，我们可以在这里利用图示进行一下说明，见下图：

图：百度百科球面坐标系图示

图中所示即为球面坐标系的表达，坐标系要求三个数值，其中两个是角度（elevation and azimuth angle），第三个是距离（range），见上图左边的坐标系。而将点云转换成range image的过程实际就是将点云原生的迪卡尔坐标系（ x,y,z ）转换成对应的球面坐标系（ $\theta，\gamma，r$ ），而球面坐标系的（ $\theta，\gamma，r$ ）又对应到上文中的range image中的（row index（horizontal angle），col index（vertical angle），像素值（range））。 $P_{ij}^{M}$ 的物理意义我们再结合上图的右边坐标系：（ $d\theta，d\gamma$ ）对应range image中（row angle resolution，col angle resolution），那么右边 $u$ 坐标系中展示的那一小块球面就对应的是range image中的像素块了，而该像素块的数值是唯一的，球面块的range却有多个，那怎么对应呢？这是再根据公式： $I_{k,ij}^{M}=min_{p\in P_{ij}^{M}} r(p)$ ，该像素块的数值就是整个球面块中的minimum range。

" $P_{ij}^{M}$ 是 $P^{M}$ **的子集表示的是落在了对应着 $[i ， j]$ 索引的球面坐标系（elevation and azimuth angle在像素角度块的范围内）的所有点集合"😗*上述加粗的句子就是回答上面问题的关键，每次对global map的变换实际就是将其原点转移到了query scan frame的位姿，所有点云的visibility都将从query scan的frame进行判断（公式： $I_{k,ij}^{M}=min_{p\in P_{ij}^{M}} r(p)$ 表达了visibility的物理意义），也只有转移到了query scan frame下才有办法基于range image来区分dynamic and static points。

同理，我们再用同样的方法转化 $P_{k}^{Q}$ 和获取对应的query scan的range image $I_{k}^{Q}$ 。然后，最终的地图点云可见性则可以通过矩阵的减法获取差值range $image I_{k}^{Diff}$ 。

$I_{k}^{Diff} = I_{k}^{Q} - I_{k}^{M}$

也就是通过该对比判断环节，我们会生成三张range image：第一张是用原始地图点云转化获得的，第二张是用query scan的点云转换获得的，最后一张是用第二张减去第一张获得的。这里我们使用论文中的图例进行说明～

图：论文Remove, then Revert: query scan，map的多分辨率range images和其对应的差值range images

上图的左栏：高分辨率范围图像（像素为0.4 ◦）。右栏：低分辨率范围图像（1 ◦ 表示像素）。从上到下，每行分别代表 $I_{k}^{Q}、 I_{k}^{M}$ 和 $I_{k}^{Diff}$ 。色图表达为像素的3D点相对于第k个传感器帧frame 原点（视原点）的的距离，蓝色更近，红色更远。因此，红色像素底部范围图像（即 $I_{k}^{Diff}$ ）表示查询扫描（query scans）和地图（map）之间的距离差异。因此，地图点应将该像素标记为dynamic（如左列中的白框）。使用高分辨率范围图像（左）会容易误删除物体的边界或地面处的一些不明确点（左列中的橙色框），而这些点都是static points，也就是使用高分辨range image容易产生FN的情况。而相反，在低分辨range image的步骤中，又可以逐步恢复上述被错误分类的dynamic points（实际上是static）。以上就是在后续remove，then revert依赖的原理。

2.5 方法论：Removert: remove, then revert static points

基于2.4中基于深度图的地图对比判断策略，我们就有办法进行Remove，then revert的操作了。我们再一次参考上一张图示，高分辨率范围图像（左）虽然会容易误删除物体的边界或地面一些静态点，但其会保证动态点都逃不过这次的过滤，这里就是上文提到的“宁可sha错不可放过”的意思。而我们在remove的结果中对首次获取的dynamic point cloud进行进一步处理，通过一步步的降低range image的分辨率，把我们误杀的静态点从dynamic point cloud中一次次地修复还原出来，并入static point cloud中，以此来达成我们静态点云地图的生成。

具体的流程阶段的结果可以参考下图，origin point cloud map（左边第一张）由于收到骑自行车人的影响在道路上留有“鬼影”，将origin point cloud map基于高分辨率range image进行对比，会得到中间所示的static point cloud，此时的static point cloud道路上的点云在第一次remove的过程中别当成了动态点误杀了，所以会有“白色”的空白部分，再经过revert过程从dynamic point cloud找出static points修补到static point cloud中，最终修复出来的结果就如最右边的图所示了。

以上就是整个pipeline流程的原理和结果图示了，论文中还提供了伪代码，我们可以再对伪代码进一步分析，如下图所示：绿色框的目的是origin point cloud map的初始化，因为是offline的处理，所以我们认为scans和其对应的poses都是已知的，黄色框的batch remove就是利用最高分辨率range image进行remove操作（至于batch的含义，本文想留下一些空间给读者自行阅读论文原文去理解～），红色框就是revert的实现了。总的来说，思路都能清晰，实现的路线也很明确。

以上就是论文中的方法论了，Kim和其团队在论文中展示的算法performance我就不做分析和说明了，毕竟是发了顶会，结果肯定不会差，无非就是想说明提出的 removert 的算法有多好了。但好不好，他们说了不算，还是得我们在自己的数据集上复现了才行。接下来，就是根据Kim团队开源的代码进行实现和测试验证了（开源代码的链接在文章开头）。

3. 论文代码实现与魔改

3.1 开源代码核心节选解析

Kim及其团队开源的代码的整体框架还是很简单易懂的，一共有四个文件

Removerter.cpp(h): 论文中算法的核心实现类的定义和函数方法实现；
RosParamSever.cpp(h)：与ROS的interface和参数的定义；
removert_main.cpp；
utility.cpp(h)：功能函数的定义与实现；

所以我们可以直接查看 Removerter.cpp中的核心函数：

void Removerter::run( void )
{
    // 加载scans和poses，为点云地图的初始化准备
    parseValidScanInfo();
    readValidScans();

    // 点云地图的初始化，该步骤生成的map就是origin point cloud，对应伪代码中的绿色框。 
    makeGlobalMap();

    // map-side removals
    // Remove的部分，对应伪代码中的黄色框。但我感觉和轮中不同的是此处的remove也进行了多分辨率rangeimage的滤除，可能是为了保证remove的能力吧。
    for(float _rm_res: remove_resolution_list_) {
        removeOnce( _rm_res );
    } 

    // if you want to every iteration's map data, place below two lines to inside of the above for loop 
    saveCurrentStaticAndDynamicPointCloudGlobal();              // if you want to save within the global points uncomment this line
    saveCurrentStaticAndDynamicPointCloudLocal(base_node_idx_); // w.r.t specific node's coord. 0 means w.r.t the start node, as an Identity.

    // TODO
    // map-side reverts
    // if you want to remove as much as possible, you can use omit this steps
    // 开源代码中的revert函数方法没有给出具体实现，留有TODO。但完全可以根据论文的思路自行补全。
    for(float _rv_res: revert_resolution_list_) {
        revertOnce( _rv_res );
    }

    //下面这两行是通过scan进行的remove，其实大体上与map-side一致，两者选其一就行了。
    // scan-side removals
    //scansideRemovalForEachScanAndSaveThem();

}

上述代码块就是 removert 的pipline实现代码，scans和poses的加载函数和点云地图初始化的部分就不在此处展开，本质就是pcd点云文件的加载和pcl点云的一些变换拼接。剩下的关键部分就是remove_resolution_list_ 和 revert_resolution_list_定义，removeOnce的实现。

# @ Range image resolution
# the below is actually magnifier ratio (i.e., 5 means x5 resolution, the x1 means 1 deg x 1 deg per pixel)
# - recommend to use the first removing resolution's magnifier ratio should meet the seonsor vertical fov / number of rays 
#     - e.g., HDL 64E of KITTI dataset -> appx 25 deg / 64 ray ~ 0.4 deg per pixel -> the magnifier ratio = 1/0.4 = 2.5
#     - e.g., Ouster OS1-64 of MulRan dataset -> appx 45 deg / 64 ray ~ 0.7 deg per pixel -> the magnifier ratio = 1/0.7 = 1.4
# - recommend to use the first reverting resolution's magnifier ratio should lied in 1.0 to 1.5
  
remove_resolution_list: [2.5, 2.0, 1.5] # for HDL 64E of KITTI dataset 
# remove_resolution_list: [1.4, 1.1] # for Ouster OS1-64 of MulRan dataset

revert_resolution_list: [1.0, 0.9, 0.8, 0.7] # TODO

注意的是，使用的3D激光雷达都是64线的，其实也能理解，线数越多在竖直方向上的支持的最高分辨率就越高，生成高分辨率range image时就不容易失真。

void Removerter::removeOnce( float _res_alpha )
{
    // filter spec (i.e., a shape of the range image) 
    curr_res_alpha_ = _res_alpha;

    std::pair<int, int> rimg_shape = resetRimgSize(kFOV, _res_alpha);
    float deg_per_pixel = 1.0 / _res_alpha;
    ROS_INFO_STREAM("\033[1;32m Removing starts with resolution: x" << _res_alpha << " (" << deg_per_pixel << " deg/pixel)\033[0m");   
    ROS_INFO_STREAM("\033[1;32m -- The range image size is: [" << rimg_shape.first << ", " << rimg_shape.second << "].\033[0m");   
    ROS_INFO_STREAM("\033[1;32m -- The number of map points: " << map_global_curr_->points.size() << "\033[0m");  
    ROS_INFO_STREAM("\033[1;32m -- ... starts cleaning ... " << "\033[0m");  

    // map-side removal: remove and get dynamic (will be removed) points' index set
    std::vector<int> dynamic_point_indexes = calcDescrepancyAndParseDynamicPointIdxForEachScan( rimg_shape );
    ROS_INFO_STREAM("\033[1;32m -- The number of dynamic points: " << dynamic_point_indexes.size() << "\033[0m"); 
    parseDynamicMapPointcloudUsingPtIdx(dynamic_point_indexes);  

    // static_point_indexes == complemently indexing dynamic_point_indexes
    std::vector<int> static_point_indexes = getGlobalMapStaticIdxFromDynamicIdx(dynamic_point_indexes); 
    ROS_INFO_STREAM("\033[1;32m -- The number of static points: " << static_point_indexes.size() << "\033[0m");  
    parseStaticMapPointcloudUsingPtIdx(static_point_indexes);

    // Update the current map and reset the tree
    map_global_curr_->clear();
    *map_global_curr_ = *map_global_curr_static_;

    // if(kUseSubsetMapCloud) // NOT recommend to use for under 5 million points map input
    //     kdtree_map_global_curr_->setInputCloud(map_global_curr_); 

} // removeOnce

对于revertOnce函数方法的具体实现，我们参考removeOnce的实现，其实只要把输入改成removeOnce输出的最终dynamic point cloud（而不再是和removeOnce一样的origin map），其revertOnce输出则是每次从dynamic point cloud修复出来的static point cloud即可。

3.2 开源代码结果展示

我在补充完开源的代码后，使用了velodyne 16线激光雷达进行了测试，结果如下：

图test1:办公区域的origin map，可以明显看到“鬼影”

图test1:办公区域的static map，仅仅把“鬼影”的一半给消除了

图test2:办公区域的origin map，可以明显看到“鬼影”

3.3 结果分析与魔改思路

其实通过上面简单的展示，我们发现很多的动态点并不能被有效地滤除，test2的结果是仅仅让动态点变稀疏，而test1确是莫名其妙的只滤除了一半，这是怎么回事呢？

其实通过test1的“奇怪”现象，结合实际的操作，我有以下的图示：

上图红色三角代表动态障碍物，红色线箭头表示动态障碍物的运动方向，蓝色弧线代表一帧帧的scan数据，蓝色线箭头则代表query scan的访问方向（并且scan的标号也表示remove时顺序访问的次序）。

我们通过跑代码发现，当是情况一的时候：动态物和scan访问同方向时，动态点云仅仅变稀疏（或者基本消除不掉），而是情况二的时候：动态物和scan访问对向时，动态点云就能消除一半。反正不能完全全部消除（摊手）。

后来发现，造成这样结果的原因就是因为每次query scan的选取都是按照slam过程中的时间进行的顺序遍历，在情况1时：query scan和map的range image基本都是一样的，所以没办法找到动态点。情况2就是只能滤除序号0～4的动态点，因为query scan和map的range image存在diff，而5～9的动态点其range image就和map的就再次没有区别了。综上，其实就是顺序遍历query scan会导致算法进入到corner situation。

那么，找到了问题的关键，进行优化调整就行了：把顺序遍历改成随机遍历，并把一次顺序遍历改成多次随机遍历，以尽量保证remove和revert时的情况和上图情形2的0～4一致。（这部分还可以参考论文中的Batch Remove的策略，本质也是解决上述的corner situation）