多传感器融合定位（3-点云地图构建及基于地图定位）3-实现ScanContext 回环检测

参考博客
从零开始做自动驾驶定位(十一): 闭环修正
 自动驾驶系统进阶与项目实战（四）自动驾驶高精度地图构建中的三维场景识别和闭环检测
 自动驾驶系统进阶与项目实战（五）使用SC-LEGO-LOAM进行较大规模点云地图构建和闭环优化

源码：
这里使用scan context 部分源码来自 github sc-lego-loam
整体代码框架来自任乾 github tag11 闭环修正代码

ps：因为个人小白一枚，此处只是发表自己的见解，和学习思路，望指正

代码思路

整体框架

我们先来回顾一下整体的框架
参考博客从零开始做自动驾驶定位(九): 建图系统结构优化
在这里插入图片描述
本次，要完成的模块为闭环检测模块

在这里插入图片描述
其中闭环检测模块又包含两部分：分别是闭环帧检测与点云的匹配
所对应的代码块如下:

    //闭环检测模块  （任乾）
    if (!DetectNearestKeyFrame(key_frame_index))
        return false;
        
	//点云配准 
    if (!CloudRegistration(key_frame_index))
        return false;

通过调用 ScanContext，将当前帧点云与历史帧点云进行回环检测，得出 key_frame_index(检测出的回环帧) 和 yaw diff (位姿偏差角) ；进而把检测到的回环帧传给 CloudRegistration 点云配准模块

代码编写

移植 sc-lego-loam 的 ScanContext相关源代码和库文件

在这里插入图片描述
添加相关源文件 scan_context/Scancontext.cpp

添加头文件

注意CMakeLists.txt添加

本机OpenCV路径添加

#本机OpenCV路径查看
locate   OpenCVConfig.cmake

在这里插入图片描述

主要调用SC中的两个函数

scManager.detectLoopClosureID()；回环帧检测函数，没有检测到返回 -1 ，检测到回环帧返回对应帧的索引

scManager.makeAndSaveScancontextAndKeys(*raw_cloud_ptr); 实时保存当前帧点云，用于回环匹配

编写函数

SaveCurrentFrame(int pcd_idx)
用于保存当前帧点云，仿照点云配准的方法，从磁盘实时获取当前帧点云的pcd存储在 raw_cloud_ptr 指针中，并调用 scManager.makeAndSaveScancontextAndKeys 进行当前帧的保存

DetectScanContext(int& key_frame_index,int pcd_idx)
检测回环帧，if true 输出回环帧的索引和偏差位姿； else flase 返回 -1

void LoopClosing::SaveCurrentFrame(int pcd_idx){
    
       
        // 读取并存储当前点云
    std::string file_path = key_frames_path_ + "/key_frame_" + std::to_string(pcd_idx) + ".pcd";
    CloudData::CLOUD_PTR raw_cloud_ptr(new CloudData::CLOUD());
     pcl::io::loadPCDFile(file_path, *raw_cloud_ptr);     //   raw_cloud_ptr  存储当前帧点云的point
    scManager.makeAndSaveScancontextAndKeys(*raw_cloud_ptr);     
}

// scan context  回环检测模块
int LoopClosing::DetectScanContext(int& key_frame_index,int pcd_idx) {
    
            
    SaveCurrentFrame(pcd_idx);

    auto detectResult = scManager.detectLoopClosureID();     //  进行回环帧的检测  first: nn index, second: yaw diff

    key_frame_index = detectResult.first;    // 获取回环帧的索引
    yawDiffRad = detectResult.second; // not use for v1 (because pcl icp withi initial somthing wrong...)
    
    return   key_frame_index;    //没有检测到返回 -1 ，检测到回环帧返回   对应帧 的索引 
}

调用函数

取代任乾老师的 DetectNearestKeyFrame 回环检测部分，eg： ScanContext 所做的工作为粗定位，检测到回环帧后，把回环帧输出给下一个点云配准模块CloudRegistration （精修定位）才完成整个回环定位修正！！！

bool LoopClosing::Update(const KeyFrame key_frame, const KeyFrame key_gnss) {
    
    
    has_new_loop_pose_ = false;

    all_key_frames_.push_back(key_frame);
    all_key_gnss_.push_back(key_gnss);

    int key_frame_index = 0;

   //  scan  context   回环检测 
    if (DetectScanContext(key_frame_index,key_frame.index)  == -1)       //  key_frame_index  闭环检测帧 ；   key_frame.index  当前帧数
        return false;

    //任乾  闭环检测模块
    // if (!DetectNearestKeyFrame(key_frame_index))
    //     return false;
    
    if (!CloudRegistration(key_frame_index))
        return false;

    has_new_loop_pose_ = true;
        return true;
}

错误调试

在调试过程中，出现读取越界的原因，原因是在点云匹配模块部分，采取的配准策略时 Scan2Map方法，构建map需要取回环帧附近的帧融合，倘若 index为0 ，在构建map时就会出现附近帧不存在的情况，进而导致指向越界的错误发生。
在这里插入图片描述

修改部分：在 JointMap 添加

//越界保护
  if (i < 0 || i >= all_key_frames_.size( ) )    continue;

在这里插入图片描述

最终效果

使用任乾的 DetectNearestKeyFrame 回环检测

在这里插入图片描述

rosservice call /optimize_map

生成 optimized.txt
在这里插入图片描述
进行evo评估

evo_ape kitti ground_truth.txt optimized.txt -r full --plot --plot_mode xyz

在这里插入图片描述

使用 DetectScanContext 回环检测

在这里插入图片描述

通过数据对比，可看出和任乾老师的回环检测模块差别不大