LT-mapper系统的原理和实现应用分析

　　1、 前言

　　不得不承认的一件事情是，在SLAM的这个技术领域内各位大佬们的内卷可以说是越来越严重了，每一次在拜读了行业内顶尖的论文或者是观看了行业大佬们发出来的技术demo视频后，我本人就会陷入无限的自我怀疑当中……

　　精度、鲁棒性、效率等等问题大家好像在各自的数据集中都实现了solved，而我却还在我“目前的数据集”挣扎（文字狗头）。摒弃掉妄自菲薄的念头，针对之前的文章中各位大佬在life-long问题中提出的勘误点：

　　1） Life-long问题中需要克服的问题在于如何应对视角变化，天气变化，光照变化条件以及动态障碍物环境下，确保定位和建模的精度。

　　2） Life-long实际是包含了两个维度的问题：long term和large scale。若是基于图优化的激光匹配算法，需要考虑怎么保证约束的准确性，当环境发生变化之后，匹配算法非常容易产生歧义，需要避免感知混淆的问题。根据上述的问题点，我们可以发现在life-long问题解决的关键流程中都会出现环境变换检测的功能，这个功能既需要包括针对短期的高动态障碍物的检测，也需要包括针对长期的缓慢环境变化的检测。而针对这个关键的环境变换检测功能，学术界也提出了很多种方案，本片文章则将简单分析一下韩国KAIST Department of Civil and Environmental Engineering 在2021年发表的“LT-mapper： A Modular Framework for LiDAR-based Lifelong Mapping“论文。

　　2、 LT-mapper系统总概

　　图：LT-Mapper系统的主要流程框图由LT-Mapper系统的主要流程框图，我们可以发现其系统主要由LT-SLAM， LT-removert， and LT-map三个模块组成，各个模块各司其职又互相独立共同保证整个life long框架的正常运行。

　　（1） LT-SLAM， 在LT-SLAM模块中，Kim团队利用multi-session SLAM，基于激光雷达的global localizer实现回环检测，以此内部联合优化多个session SLAM的轨迹。在这个模块中，query map被注册添加到现有的central map（target map）。

　　（2） LT-removert， 该模块用于处理在query map和central map（target map）对齐时的”模糊性”问题，一方面用以去除map中的噪声，一方面用以去除map中的高动态物体点云（High Dynamic Points）

　　（3） LT-map，将query map注册添加到现有的central map并移除HD点后，通过在query map和central map间进行set difference operation的操作来检测环境上的变化。此时检测变化的time scale就会大于之前LT-removert模块检测高动态物体点云的time scale了，即是探测不同session静态地图的变化，其阶段检测出来的动态点云称为低动态点云（LD）。低动态点云（LD）随后将其进一步分为“新出现“点（positive difference （PD））和”消失“点（negative difference （ND））两类。

　　3、 LT-SLAM方法

　　该模块本质上就是multi-session SLAM中多SLAM Trajectories的拼接对齐问题，论文中使用的匹配方案是ICP或者SCAN-CONTEXT（SC-Loops也是Kim团队研究和发表的一种回环检测方法。论文可详见链接：Scan Context： Egocentric Spatial Descriptor for Place Recognition within 3D Point Cloud Map）来计算过两个点云“关键帧”之间的6自由度的变换关系。在通过回环检测计算出frame之间的约束关系后，在pose-graph中引入相关的对象和约束进行multiple sessions的优化计算以实现轨迹间的对齐。值得说明的是，论文中提出了anchor node（锚点）的概念，该anchor node用以替代SLAM中的key node（frame），因为LT-SLAM中会处理许多不同时期的SLAM trajectories，若是以传统意义上的关键帧作为一个处理单元的话，则计算量和复杂度太大了。

　　因此，anchor node（锚点）则代表了若干个key node（frame）形成的map的原点，在SLAM轨迹的优化过程中也将优化的处理单元改为了anchor node（类似cartographer中submap的概念）。而系统中使用anchor node，一方面减少了在回环检测后pose-graph优化调整的复杂度，另一方面也减少了系统地图环境变化和更新的复杂度。

　　4、 LT-REMOVERT方法

　　回到对环境中动态物体点云的处理和分类，文章将所有的动态点分类成了两类：HD（高变化的动态障碍物）和LD（缓慢变化的场景点）。根据以上的分类，再将整个环境变化的检测功能分成两个sessions：

　　1. 首先删除HD而不删除LD点，在这个过程中文章是使用了REMOVERT算法作为HD的删除引擎，具体的算法原理和流程可以详见本人的另一篇文章SLAM动态障碍物滤除 | IROS 2020 REMOVERT：Remove， then Revert的论文解析与实现结果。

　　2. 在完成对HD点的删除后，进入LD的检测阶段，该阶段中会先将query map和target map的坐标系对齐并且移除两者中的HD后，为target map构造了一个kd-tree并判断query map中的点云在半径r米的范围内是否有足够多的target map点云，如果没有，则该query map中的点为LD。然后再对LD点进行ND和PD的划分。其中PD（Positive Difference）指的是在query map中新增加上的点云变化，ND（Negative Difference）则是指原先存在而现在query map中消失了的点云，所以根据上述的定义我们相应需要去除的点云即为ND，需要添加的点云为PD。

　　而Kim在LD的基础上进行ND和PD的区分，主要是为了解决一种点云“误杀”的问题：Occlusions（遮挡），试想一种情况：target map中有一个静态物体O并在完整构建了其点云“形状”，而在query map中实际的环境是该静态物体O前突然多了一堵墙遮挡住了传感器关于物体O的识别。

　　那么在query map中该物体O点云会被先识别为LD，但进一步的分析是该物体O的点云是应当保留而非去除的，Kim称此类被遮挡住的点云为weak ND，并且weak ND是不会被去除以避免“误杀”的情况。对于这一步关于“Weak ND Preservation”的问题，Kim团队再次使用了Removert，但进行了修改：不同于原来的Removert只删除视点原点近处的地图点，而修改版本的Removert则关注于删除原始ND map中的远处的点，并然后将它们还原到static map中。

　　说完这么多，我们参照论文中的实例进行一下说明，下图A、B分别为同一街道不同时期的地图（MulRan dataset DCC 01和DCC 02），其关键的不同之处在于马路边上是否存在墙面。而且在有墙面的地图后面是不存在点云的（因为遮挡的原因）。

　　图A：MulRan dataset DCC 01（有墙面）

　　图B：MulRan dataset DCC 02（无墙面）

　　具体步骤的结果可详见下图，其中标记为蓝色的点云为PD，标记为红色的点云为ND，灰色的点云为weak ND。情况（1）：选择A为target map，B为query map，左列的环境检测结果图片是普通的kd-tree取半径集合差操作的获取的naïve ND和PD点，右栏则显示了在检测处LD后进一步PD和ND区分的结果。在Case 1中，红色ND点大部分被删除，而添加了整个蓝色PD点（相对于A，B中新增了许多点云，所以为PD）。

　　图：情况1下的环境检测结果情况（2）：选择B为target map，A为query map，在map A中的部分地面点被遮挡，并在左栏中错误地标记为ND。在右栏中，只有强ND（红色）点被移除，弱ND点被还原（即灰色标记的地面点）。由此可见，Kim团结针对LD点云的进一步PD、ND 和weak ND区分是可以有效避免遮挡引起的误杀问题的。

　　图：情况2下的环境检测结果

　　5、 LT-MAP方法

　　该部分的主要任务就是地图的更新（map update）和地图的长期管理（Long-term Map Management）。

　　针对地图的更新（map update），论文中使用了一个公式进行了表述，该公式就是LT-map部分最核心的逻辑：

　　

　　其中为去除了HD关键帧点云，函数ND（*）和PD（*）表示返回在关键帧中类型为ND 和 PD的点云，一言以蔽之，给到central map和query map，去除HD和ND的点云并保证PD点云保留。

　　针对地图的长期管理（Long-term Map Management），在论文中的进一步测试说明了LT-mapper在更新world representation时分为两种方法，如下图所示。第一，LT-mapper可以高效地通过只发送LD类型的环境变化点云（而不是整个关键帧点云地图），以更改到一个central server来维护一个live map。第二个表述为meta map（区别于live map）， LT-mapper在更新扩展时没有考虑添加weak PD类型的点云，即不考虑短期静止或周期性的变化的点云。

　　

　　图：地图的长期管理的两种形式

　　6、 LT-MAPPER关键代码实现方法

　　第一， 先上LT-Mapper的GitHub开源代码链接：https://github.com/gisbi-kim/lt-mapper该开源代码整体流程和函数的设计都比较清晰，较为晦涩的地方可能也就是LT-REMOVERT模块了，本节也将根据系统框图的顺序从LT-SLAM， LT-removert， and LT-map各个模块进行关键部分代码的展示。

　　6.1 LT-SLAM

　　该部分和其他SLAM的回环检测和因子图优化的过程实现并没有本质上的区别，仅仅是该模块是直接加载Single session SLAM的轨迹和因子图信息。在multiple sessions间进行回环检测的操作，并在计算出约束后添加到已有的因子图中完成优化匹配以实现multipleSessions SLAM 轨迹信息的对齐拼接。

1、 void LTslam::run( void )

2、 {3、     initOptimizer();4、     initNoiseConstants();5、 6、     loadAllSessions();7、     addAllSessionsToGraph();8、 9、     optimizeMultisesseionGraph(true); // optimize the graph with existing edges10、     writeAllSessionsTrajectories(std::string("bfr_intersession_loops"));11、 12、     detectInterSessionSCloops(); // detectInterSessionRSloops was internally done while sc detection13、     addSCloops();14、     optimizeMultisesseionGraph(true); // optimize the graph with existing edges + SC loop edges15、 16、     bool toOpt = addRSloops(); // using the optimized estimates (rough alignment using SC)17、     optimizeMultisesseionGraph(toOpt); // optimize the graph with existing edges + SC loop edges + RS loop edges18、 19、     writeAllSessionsTrajectories(std::string("aft_intersession_loops"));20、 } 

　　6.2 LT-removert

　　整个Removerter：：run（void）的流程分为了（1）初始化加载地图和预处理，（2）高动态点云HD的去除，（3）低动态点云LD的检测，（4）LT-map。代码详见如下：

 1、 void Removerter::run(void)2、 {3、     // # Step 0: Preparations4、     loadSessionInfo();5、  6、     parseKeyframes();7、     loadKeyframes();8、     precleaningKeyframes(2.5); // optional. remove points within near radius from the lidar9、  10、     makeGlobalMap();11、 12、     // # Step 1: HD noise removal13、     removeHighDynamicPoints();14、     parseStaticScansViaProjection();15、 16、     // # Step 2: LD change detection17、     detectLowDynamicPoints();18、 19、     // # Step 3: LT-map20、     updateCurrentMap();21、     parseUpdatedStaticScansViaProjection();22、     parseLDScansViaProjection(); // TODO23、     updateScansScanwise(); // == eq(4) in the paper24、     saveAllTypeOfScans(); // TODO25、 26、 } // Removerter::run  其中对于（2）部分，该部分的代码可以详见另一篇文章：SLAM动态障碍物滤除 | IROS 2020 REMOVERT：Remove, then Revert的论文解析与实现结果，该文章中有详细的注释和说明。针对（3）中低动态点云LD的检测，代码实现如下：  1、 void Removerter::detectLowDynamicPoints(void)2、 {3、 // 先对class Session的两个实例：central_sess_ & query_sess_进行基于KnnDiff的LD检测。4、 central_sess_.extractLowDynPointsViaKnnDiff(query_sess_.map_global_curr_static_);5、 query_sess_.extractLowDynPointsViaKnnDiff(central_sess_.map_global_curr_static_);6、 7、     // strong ND8、     central_sess_.constructGlobalNDMap();9、     filterStrongND(central_sess_, query_sess_); // filtering central_sess_.scans_knn_diff_10、     central_sess_.removeWeakNDMapPointsHavingStrongNDInNear(); // propagation"11、 12、     // strong PD13、     query_sess_.constructGlobalPDMap();14、     filterStrongPD(query_sess_, central_sess_); // filtering central_sess_.scans_knn_diff_15、     query_sess_.revertStrongPDMapPointsHavingWeakPDInNear(); // propagation"16、 17、 //1. 保存PD map(在query sess实例中)到central sess实例18、 //2. 地图已经在全局(central sess)坐标系下，但由于后续会重新投影这个地图保存到central coord，所以先在此使用query coord。19、     *central_sess_.map_global_pd_ = *query_sess_.map_global_pd_;20、     *central_sess_.map_global_pd_orig_ = *query_sess_.map_global_pd_orig_;21、     *central_sess_.map_global_pd_strong_ = *query_sess_.map_global_pd_strong_; // Removerter::run22、 }

　　上述代码中，extractLowDynPointsViaKnnDiff（_target_map）是使用了pcl库中KdTreeFLANN的结构进行LowDynamicPoints的检测并储存。constructGlobalNDMap（）函数是基于Session类中的坐标和对于点云数据进行点云拼接生成point cloud map，filterStrongND（）和removeWeakNDMapPointsHavingStrongNDInNear（）与函数名的定义符合，就是为了将strong ND独立的返回出来以便在LT-map中使用。

　　6.3 LT-map

　　在整个代码工程中，LT-map和LT-removert都放在了`ltremovert`的文件目录下，和论文中的说明对应：LT-map中在session间环境变化的检测方法是基于LT-removert进行的修改。removert是用以进行single session中map的高动态点云的清洗，LT-map则是实现sessions间“静态”环境物体的变化检测。在6.2中的Removerter：：run（void）的流程中，我们可以看到在完成了地图加载初始化、高动态点云HD的去除和低动态点云LD的检测后，就进入了LT-map的地图更新处理了。

1、 // # Step 3: LT-map2、     updateCurrentMap();3、     parseUpdatedStaticScansViaProjection();4、     updateScansScanwise(); // == eq(4) in the paper

updateCurrentMap()函数的功能就是将query sess map和central sess map对齐整合到一起，并保证去除了strong ND points和保留weak ND points以及添加strong PD points到整合对齐后的地图中。

 1、 void Removerter::updateCurrentMap(void)2、 {3、     pcl::PointCloud::Ptr map_global_updated(new pcl::PointCloud());4、 // 1. 将query sess map和central sess map对齐整合到一起并在整合的时候去除了strong ND points5、     auto map_global_union_queryside = mergeScansWithinGlobalCoordUtil(query_sess_.scans_knn_coexist_, query_sess_.keyframe_poses_, query_sess_.kSE3MatExtrinsicLiDARtoPoseBase);6、     octreeDownsampling(map_global_union_queryside, map_global_union_queryside, 0.05);7、 8、     auto map_global_union_centralside = mergeScansWithinGlobalCoordUtil(central_sess_.scans_knn_coexist_, central_sess_.keyframe_poses_, central_sess_.kSE3MatExtrinsicLiDARtoPoseBase);9、     octreeDownsampling(map_global_union_centralside, map_global_union_centralside, 0.05);10、 11、     *map_global_updated = *map_global_union_queryside; // init12、     *map_global_updated += *map_global_union_centralside; // init13、     14、     // 2. 保留weak ND points 15、     *map_global_updated += *central_sess_.map_global_nd_weak_; // append16、 17、     // 3-1: 添加strong PD points 18、     pcl::PointCloud::Ptr map_global_updated_strong_(new pcl::PointCloud());19、     *map_global_updated_strong_ = *map_global_updated; // ==     pcl::copyPointCloud(*map_global_updated, *map_global_updated_strong_);20、     *map_global_updated_strong_ += *central_sess_.map_global_pd_strong_;21、     octreeDownsampling(map_global_updated_strong_, map_global_updated_strong_, 0.05);22、 23、     // 3-2: 添加PD points 24、     *map_global_updated += *central_sess_.map_global_pd_orig_; // append25、     octreeDownsampling(map_global_updated, map_global_updated, 0.05);26、 27、     // 4: 写操作28、     *central_sess_.map_global_updated_ = *map_global_updated;29、     pcl::savePCDFileBinary(save_pcd_directory_ + "updated_map.pcd", *central_sess_.map_global_updated_);30、 31、     *central_sess_.map_global_updated_strong_ = *map_global_updated_strong_;32、     pcl::savePCDFileBinary(save_pcd_directory_ + "updated_map_strong.pcd", *central_sess_.map_global_updated_strong_);33、 34、 } // updateCurrentMap

　　updateScansScanwise（）函数则是在central sess map上进行论文中公式（4）的更新，具体的操作也是在class Session上实现的。

　　7、 总结

　　简单的来说，Kim团队发布的论文《LT-mapper： A Modular Framework for LiDAR-based Lifelong Mapping》贡献领域在于三点：

　　（1）地图的管理策略map management，

　　（2）一种解决highlow dynamic change detection（类型HD和LD点云的区分）

　　（3）LD类型点云的PDND点云管理和处理策略（positive egative change management）的方法

　　由此，我们可以发现LT-mapper 的侧重点在于Life-long SLAM的建图阶段以及地图管理阶段，其研究解决的即是life-long问题中都会出现环境变换检测的功能。再者，由于LT-Mapper系统框架在设计时各个模块的功能封装性较好，其为代码的移植和工程化提供了便利性，也为目前手头上的研究项目的系统扩展和优化提供了很好的思路和方向。

　　审核编辑：郭婷