《多传感器融合定位》 点云地图构建及基于地图定位

任乾大佬知乎专栏汇总链接： https://zhuanlan.zhihu.com/p/113616755.

一、闭环修正及精度评价

提供的工程框架中已经给出了闭环的流程和实现,但是是基于ICP的,这是在已知粗略位姿情况下的实现方式。在未知粗略位姿情况下,闭环检测的实现难度会加大。要求使用前面讲过的scan context,实现此功能。并和已有的图优化功能完成对接,实现修正。并最终给出修正前后的轨迹精度对比。

1.ICP方法闭环（原有框架代码）

（1）回环代码分析

回环代码的实现比较简单，主要是通过先验信息对关键帧进行最近邻匹配，得到回环的相似帧，再将这两组关键帧进行NDT匹配，得到相对位姿，再送到后端进行优化。
Update函数：

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;
    //使用gnss（先验信息） 寻找与当前帧最相似的回环检测
    if (!DetectNearestKeyFrame(key_frame_index))
        return false;
    //回环匹配
    if (!CloudRegistration(key_frame_index))
        return false;

    has_new_loop_pose_ = true;
    return true;
}

DetectNearestKeyFrame（）利用先验gnss信息寻找相似帧

bool LoopClosing::DetectNearestKeyFrame(int& key_frame_index) {
    static int skip_cnt = 0;
    static int skip_num = loop_step_;
    //并不是每一次关键帧进来都做回环检测 skip_num个关键帧才做一次
    if (++skip_cnt < skip_num)
        return false;
    //设置一个关键帧路程差  
    //两帧回环至少需要差diff_num_个gnss帧
    if ((int)all_key_gnss_.size() < diff_num_ + 1)
        return false;

    int key_num = (int)all_key_gnss_.size();
    float min_distance = 1000000.0;
    float distance = 0.0;

    KeyFrame history_key_frame;
    KeyFrame current_key_frame = all_key_gnss_.back();

    key_frame_index = -1;
    for (int i = 0; i < key_num - 1; ++i) {
        //# 过于小的闭环没有意义
        //所以当前帧的相似帧只在diff_num_帧之前找
        if (key_num - i < diff_num_)
            break;
        //当前关键帧与历史帧（先验信息） 的比较  
        history_key_frame = all_key_gnss_.at(i);
        distance = fabs(current_key_frame.pose(0,3) - history_key_frame.pose(0,3)) + 
                   fabs(current_key_frame.pose(1,3) - history_key_frame.pose(1,3)) + 
                   fabs(current_key_frame.pose(2,3) - history_key_frame.pose(2,3));
        if (distance < min_distance) {
            min_distance = distance;
            key_frame_index = i;
        }
    }
    //放弃初始的几帧
    if (key_frame_index < extend_frame_num_)
        return false;

    skip_cnt = 0;
    //防止回环过于频繁
    //成功检测一次闭环后之后路径可能有较长一段仍然是重复的，因为是重复路段，这段闭环检测基本都能成功，
    //但不希望同一路段增加太多闭环约束，对其进行稀释
    //根据当前最小距离计算重新等待时间
    skip_num = (int)min_distance;
    //detect_area_检测区域，只有两帧距离小于这个值，才做闭环匹配
    if (min_distance > detect_area_) {
        skip_num = std::max((int)(min_distance / 2.0), loop_step_);
        return false;
    } else {
        skip_num = loop_step_;
        return true;
    }
}

CloudRegistration（）NDT匹配

bool LoopClosing::CloudRegistration(int key_frame_index) {
    // 生成地图
    CloudData::CLOUD_PTR map_cloud_ptr(new CloudData::CLOUD());
    Eigen::Matrix4f map_pose = Eigen::Matrix4f::Identity();
    JointMap(key_frame_index, map_cloud_ptr, map_pose);

    // 生成当前scan
    CloudData::CLOUD_PTR scan_cloud_ptr(new CloudData::CLOUD());
    Eigen::Matrix4f scan_pose = Eigen::Matrix4f::Identity();
    JointScan(scan_cloud_ptr, scan_pose);

    // 匹配
    Eigen::Matrix4f result_pose = Eigen::Matrix4f::Identity();
    Registration(map_cloud_ptr, scan_cloud_ptr, scan_pose, result_pose);

    // 计算相对位姿
    current_loop_pose_.pose = map_pose.inverse() * result_pose;

    // 判断是否有效GetFitnessScore（）返回值越小越好
    if (registration_ptr_->GetFitnessScore() > fitness_score_limit_)
        return false;
    
    static int loop_close_cnt = 0;
    loop_close_cnt ++;

    std::cout << "检测到闭环 "<<  loop_close_cnt
              << ": 帧" << current_loop_pose_.index0 
              << "------>" << "帧" << current_loop_pose_.index1 << std::endl
              << "fitness score: " << registration_ptr_->GetFitnessScore() 
              << std::endl << std::endl;

    // std::cout << "相对位姿 x y z roll pitch yaw:";
    // PrintInfo::PrintPose("", current_loop_pose_.pose);
    return true;
}

（2）启动launch文件（运行）

roslaunch lidar_localization mapping.launch 
//受限于电脑性能，bag包的播放频率是正常频率的0.5倍（根据自己实际情况调整）
rosbag play kitti_2011_10_03_drive_0027_synced.bag -r 0.5

启动后的参数信息：

rivz实时显示（第二张图为bag播放完进行手动优化后的结果）：


手动优化（将最后未达到优化数量的位姿进行优化，可选项）：

rosservice call /optimize_map

保存地图：

rosservice call /save_map

（3）评估

与rviz中显示的三条线对应，在trajectory中保存了三个文件：
橙色线–gnss位姿–ground_truth.txt
红色线–里程计位姿–laser_odom.txt
绿色线–优化后的位姿–optimized.txt
将ground_truth.txt，laser_odom.txt和ground_truth.txt，optimized.txt做两次整体评估：
ground_truth.txt，laser_odom.txt的比较：

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

ground_truth.txt，optimized.txt的比较：

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

结论：
显然，优化后的位姿与gnss的先验信息比较接近（当然要接近～毕竟我们用它来优化我们的里程计位姿 啊，回环的寻找相似帧也用的gnss信息），其实从riz也能看出来，在优化后橙色线和绿色线基本重合.
注意：其实这样的分析并不够严谨，因为优化的组成是所有关键帧顶点，与gnss的先验信息单边，再与回环检测的双边所构成的全局优化，这里的结论也应该是对全局优化的效果体现。

2.scan context方法实现回环

（1）SC-LeGO-LOAM中SC实现分析

github链接: SC-LeGO-LOAM.
scan context论文链接: Scan Context: Egocentric Spatial Descriptor for Place Recognition within 3D Point Cloud Map.

我现在也没看过这个框架的完整源码，scan context方法实现主要在Scancontext.cpp中。将点云转换为sc矩阵的接口函数为makeAndSaveScancontextAndKeys（），由saveKeyFramesAndFactor（）函数调用实现.而回环的检测则单独拉了一个1hz的线程loopClosureThread，看起来和LeGO-LOAM很相似，线程一直运行performLoopClosure（）函数，接着，performLoopClosure（）函数里有detectLoopClosure（）函数.不同的是LeGO-LOAM使用的是ICP匹配回环，然而SC-LeGO-LOAM在detectLoopClosure（）里面调用了detectLoopClosureID()实现了scan context方法的回环.
SC矩阵生成的接口函数makeAndSaveScancontextAndKeys（）

void SCManager::makeAndSaveScancontextAndKeys( pcl::PointCloud<SCPointType> & _scan_down )
{
    //生成scancontext  
    Eigen::MatrixXd sc = makeScancontext(_scan_down); // v1 
    //得到每个环ring的最大高度平均值  --->  sc矩阵每行数据的平均值  20*1的矩阵
    Eigen::MatrixXd ringkey = makeRingkeyFromScancontext( sc );
     // 每个轴向sector的最大高度平均值   ------>sc矩阵每列数据的平均值
     //目的 降维匹配 减少计算量
    Eigen::MatrixXd sectorkey = makeSectorkeyFromScancontext( sc );
    //将eigen的矩阵转换为vector
    //列的平移对应位姿的旋转  保证匹配的旋转不变性  
    std::vector<float> polarcontext_invkey_vec = eig2stdvec( ringkey );
    //保存数据
    polarcontexts_.push_back( sc ); 
    polarcontext_invkeys_.push_back( ringkey );
    polarcontext_vkeys_.push_back( sectorkey );
    polarcontext_invkeys_mat_.push_back( polarcontext_invkey_vec );

    // cout <

} // SCManager::makeAndSaveScancontextAndKeys

由一帧点云得到sc矩阵makeScancontext（）

MatrixXd SCManager::makeScancontext( pcl::PointCloud<SCPointType> & _scan_down )
{
    TicToc t_making_desc;
    //激光点数
    int num_pts_scan_down = _scan_down.points.size();

    // main
    const int NO_POINT = -1000;
    //构造Scancontext矩阵：环数PC_NUM_RING（20）*扇形数PC_NUM_SECTOR（60） 
    //初值-1000*1 ones（）函数：设置全一矩阵 
    MatrixXd desc = NO_POINT * MatrixXd::Ones(PC_NUM_RING, PC_NUM_SECTOR);

    SCPointType pt;
    float azim_angle, azim_range; // wihtin 2d plane
    int ring_idx, sctor_idx;
    //遍历激光点
    for (int pt_idx = 0; pt_idx < num_pts_scan_down; pt_idx++)
    {
        pt.x = _scan_down.points[pt_idx].x; 
        pt.y = _scan_down.points[pt_idx].y;
        //添加雷达高度 对所有激光点都操作 使其高度大于0 （可能跟数据集有关）
        pt.z = _scan_down.points[pt_idx].z + LIDAR_HEIGHT; // naive adding is ok (all points should be > 0).

        // xyz to ring, sector
        //距离
        azim_range = sqrt(pt.x * pt.x + pt.y * pt.y);
        //角度
        azim_angle = xy2theta(pt.x, pt.y);

        // if range is out of roi, pass
        //不考虑太远处的激光点
        if( azim_range > PC_MAX_RADIUS )
            continue;
        //看这个激光点落在矩阵的那个位置
        ring_idx = std::max( std::min( PC_NUM_RING, int(ceil( (azim_range / PC_MAX_RADIUS) * PC_NUM_RING )) ), 1 );
        sctor_idx = std::max( std::min( PC_NUM_SECTOR, int(ceil( (azim_angle / 360.0) * PC_NUM_SECTOR )) ), 1 );

        // taking maximum z 
        //矩阵内储存该划分的三维空间里所有点的Z的最大值
        if ( desc(ring_idx-1, sctor_idx-1) < pt.z ) // -1 means cpp starts from 0
            desc(ring_idx-1, sctor_idx-1) = pt.z; // update for taking maximum value at that bin
    }

    // reset no points to zero (for cosine dist later)
    //遍历矩阵，找到所有划分空间里没有激光点的位置 赋值0
    for ( int row_idx = 0; row_idx < desc.rows(); row_idx++ )
        for ( int col_idx = 0; col_idx < desc.cols(); col_idx++ )
            if( desc(row_idx, col_idx) == NO_POINT )
                desc(row_idx, col_idx) = 0;

    t_making_desc.toc("PolarContext making");

    return desc;
} // SCManager::makeScancontext

计算两个SC矩阵的列偏移量和距离distanceBtnScanContext（）

std::pair<double, int> SCManager::distanceBtnScanContext( MatrixXd &_sc1, MatrixXd &_sc2 )
{
    // 1. fast align using variant key (not in original IROS18)
    //计算两帧的列向量均值  1*60
    MatrixXd vkey_sc1 = makeSectorkeyFromScancontext( _sc1 );
    MatrixXd vkey_sc2 = makeSectorkeyFromScancontext( _sc2 );
    //计算列向量的偏移
    int argmin_vkey_shift = fastAlignUsingVkey( vkey_sc1, vkey_sc2 );
    //取上面得到的初始偏移值argmin_vkey_shift 左右各1/2 SC的列数*SEARCH_RATIO（0.1 ）作为搜索空间
    const int SEARCH_RADIUS = round( 0.5 * SEARCH_RATIO * _sc1.cols() ); // a half of search range 
    std::vector<int> shift_idx_search_space { argmin_vkey_shift };
    for ( int ii = 1; ii < SEARCH_RADIUS + 1; ii++ )
    {
        shift_idx_search_space.push_back( (argmin_vkey_shift + ii + _sc1.cols()) % _sc1.cols() );
        shift_idx_search_space.push_back( (argmin_vkey_shift - ii + _sc1.cols()) % _sc1.cols() );
    }
    //将偏移量搜索空间排序
    std::sort(shift_idx_search_space.begin(), shift_idx_search_space.end());

    // 2. fast columnwise diff 
    int argmin_shift = 0;
    double min_sc_dist = 10000000;
    for ( int num_shift: shift_idx_search_space )
    {
        //将一个SC按偏移量右移
        MatrixXd sc2_shifted = circshift(_sc2, num_shift);
        //计算两个SC的距离
        double cur_sc_dist = distDirectSC( _sc1, sc2_shifted );
        //找到距离最小的偏移量 以及最小的偏移距离
        if( cur_sc_dist < min_sc_dist )
        {
            argmin_shift = num_shift;
            min_sc_dist = cur_sc_dist;
        }
    }

    return make_pair(min_sc_dist, argmin_shift);

} // distanceBtnScanContext

//计算两个sc矩阵的列偏移量的实验函数fastAlignUsingVkey（）

int SCManager::fastAlignUsingVkey( MatrixXd & _vkey1, MatrixXd & _vkey2)
{
    int argmin_vkey_shift = 0;
    double min_veky_diff_norm = 10000000;
    for ( int shift_idx = 0; shift_idx < _vkey1.cols(); shift_idx++ )
    {
        //将_vkey2矩阵的循环右移shift_idx位
        MatrixXd vkey2_shifted = circshift(_vkey2, shift_idx);
        //两矩阵之间相减  如果相似的话，差值应该比较小
        MatrixXd vkey_diff = _vkey1 - vkey2_shifted;

        double cur_diff_norm = vkey_diff.norm();
        //得到两帧的最小偏移
        if( cur_diff_norm < min_veky_diff_norm )
        {
            argmin_vkey_shift = shift_idx;
            min_veky_diff_norm = cur_diff_norm;
        }
    }

    return argmin_vkey_shift;

} // fastAlignUsingVkey

（2）SC-LeGO-LOAM的移植与实现

源文件的移植
将SC-LeGO-LOAM中涉及到的4个头文件（如图）复制到include/lidar_localization/mapping/loop_closing文件夹下

将Scancontext.cpp同样放到src的loop_closing下面
实现接口
修改复制文件所包含的头文件路径，然后在LoopClosing类中定义一个SCManager类的指针和回环实现的接口函数DetectNearestKeyFrameByScanContext（）：

bool DetectNearestKeyFrameByScanContext(int& key_frame_index);
std::shared_ptr<SCManager> scmanager_ptr_;
bool use_scancontext_=false;//是否使用ssc方法进行闭环检测

修改loop_closing.cpp中的回环检测函数

    if (use_scancontext_)
    {
        if (!DetectNearestKeyFrameByScanContext(key_frame_index))
            return false;
    }
    else
    {
        if (!DetectNearestKeyFrame(key_frame_index))
            return false;
    }

DetectNearestKeyFrameByScanContext（）的具体实现如下：

bool LoopClosing::DetectNearestKeyFrameByScanContext(int& key_frame_index)
{
    //将当前帧点云存储成SC矩阵
    pcl::PointCloud<SCPointType>  current_point_cloud;
    std::string file_path = key_frames_path_ + "/key_frame_" +std::to_string(all_key_frames_.back().index) + ".pcd";
    pcl::io::loadPCDFile(file_path, current_point_cloud);
    scmanager_ptr_->makeAndSaveScancontextAndKeys(current_point_cloud);


    static int skip_cnt = 0;
    static int skip_num = loop_step_;
    //并不是每一次关键帧进来都做回环检测 skip_num个关键帧才做一次
    if (++skip_cnt < skip_num)
       return false;

    //回环检测
    auto loopResult=scmanager_ptr_->detectLoopClosureID();

    if (loopResult.first==-1)
    {
        return false;
    }
    key_frame_index =loopResult.first;
    return true;
}

在进行闭环的特征匹配时也要由于点云位姿的获取改变了，其对应的函数JointMap（）和JointScan也要做对应的修改：

bool LoopClosing::JointMap(int key_frame_index, CloudData::CLOUD_PTR& map_cloud_ptr, Eigen::Matrix4f& map_pose) {

    //最相似帧的激光点云的索引
    current_loop_pose_.index0 = all_key_frames_.at(key_frame_index).index;
    //最相似帧的先验位姿

    // 合成地图
    //将相似帧附近的关键帧合成局部地图
    for (int i = key_frame_index - extend_frame_num_; i < key_frame_index + extend_frame_num_; ++i) {
        // 原框架bug：key_frame_index=0时 i<0 数组越界报错
        //就是很奇怪，这个bug不应该存在啊 loop_step_不是有初值吗 怎么会key_frame_index=0呢？
        //添加越界保护！
        if (i<0 || i>=all_key_frames_.size())
        {
            continue;
        }
        std::string file_path = key_frames_path_ + "/key_frame_" + std::to_string(all_key_frames_.at(i).index) + ".pcd";
        
        CloudData::CLOUD_PTR cloud_ptr(new CloudData::CLOUD());
        pcl::io::loadPCDFile(file_path, *cloud_ptr);
          //最相似帧的先验位姿
        if(use_scancontext_)
        {
            map_pose=all_key_frames_.at(i).pose;
            pcl::transformPointCloud(*cloud_ptr, *cloud_ptr, map_pose);
        }
        else
        {
            //激光点云与gnss先验信息的变换矩阵
            map_pose = all_key_gnss_.at(key_frame_index).pose;
            Eigen::Matrix4f pose_to_gnss = map_pose * all_key_frames_.at(key_frame_index).pose.inverse();
             //将激光点云转换到gnss的统一坐标下
            Eigen::Matrix4f cloud_pose = pose_to_gnss * all_key_frames_.at(i).pose;
            pcl::transformPointCloud(*cloud_ptr, *cloud_ptr, cloud_pose);
        }
       
        *map_cloud_ptr += *cloud_ptr;
    }
    map_filter_ptr_->Filter(map_cloud_ptr, map_cloud_ptr);
    return true;
}
bool LoopClosing::JointScan(int key_frame_index,CloudData::CLOUD_PTR& scan_cloud_ptr, Eigen::Matrix4f& scan_pose) {
    if (use_scancontext_)
    {
        scan_pose=all_key_frames_.at(key_frame_index).pose;
    }
    else
    {
        scan_pose = all_key_gnss_.back().pose;
    }
    
    current_loop_pose_.index1 = all_key_frames_.back().index;
    current_loop_pose_.time = all_key_frames_.back().time;

    std::string file_path = key_frames_path_ + "/key_frame_" + std::to_string(all_key_frames_.back().index) + ".pcd";
    pcl::io::loadPCDFile(file_path, *scan_cloud_ptr);
    scan_filter_ptr_->Filter(scan_cloud_ptr, scan_cloud_ptr);
    return true;
}

（3）ScanContext方法的评估

未加回环的后端优化

use_loop_close: false

运行过程的优化信息：

ground_truth.txt，optimized.txt的比较：

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

迷惑行为：未加回环的后端优化居然效果要比加了（第一种方法）回环的效果要好～
使用SC回环的后端优化

use_loop_close: true

ground_truth.txt，optimized.txt的比较：

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

说实话这个误差让我有点懵了～～～～，有没有好心人看看我有那里不对吗

二、位姿初始化

NDT匹配需要较准确的初始位姿,因此在定位之前需要初始化环节,给出载体的初始位姿。
按照难度由低到高,常见的初始化需求有这样几种:
1 已知位姿的初始化
2位置已知而姿态位置的初始化
3位置和姿态均未知的初始化

提供的工程框架中已经给出了位姿初始化功能,但是是在起始位置的,并且是基于已知粗略位姿的。要求实现地图中任意位置的位姿初始化,可以从三种难度等级中任选一种,难度越高,得分越高。

1.以第一帧为（0,0,0）初始化的问题

最主要的问题是当进行已知地图的定位时，若定位的起始点不是建图的起点时，无法实现正常的定位功能.
下图为先播放bag包，再启动定位程序的定位效果：

2.已知位姿的初始化

播放bag包，并订阅得到建图时原始的GNSS数据

rostopic echo /kitti/oxts/gps/fix

根据信息修改gnss初始化函数：

void GNSSData::InitOriginPosition() {

    longitude = 8.39200;
    latitude = 48.9832;
    altitude = 116.793;

    geo_converter.Reset(latitude, longitude, altitude);

    origin_longitude = longitude;
    origin_latitude = latitude;
    origin_altitude = altitude;

    origin_position_inited = true;
}

未完待续～～～～～～～（2021.05.13)
任意点启动定位程序的定位效果：
分别在100s，200s，300s处进行初始化，然后定位效果如下：

rosbag play KITTI/kitti_2011_10_03_drive_0027_synced.bag -s 100

rosbag play KITTI/kitti_2011_10_03_drive_0027_synced.bag -s 200

rosbag play KITTI/kitti_2011_10_03_drive_0027_synced.bag -s 300

三、运行代码遇到的问题

1.内存空间不足的问题

导致代码在后端优化的时候直接内存爆炸，我先试着清理了以下缓存：

sudo sh -c 'echo 1 > /proc/sys/vm/drop_caches'
sudo sh -c 'echo 2 > /proc/sys/vm/drop_caches'
sudo sh -c 'echo 3 > /proc/sys/vm/drop_caches'

发现效果不大，在运行代码的时候，我一直使用top指令（打开终端，输入top）监视着内存和cpu，因为缓存是在代码运行时产生的.于是使用free -m 查询内存,发现没有swap区没有分配～
参考链接： 如何在Ubuntu 16.04上增加Swap分区
手动分配swap区：
1.查看电脑的硬盘信息

df -h  

2.在你选择的地方创建一个20G（一般是你系统内存的1.5倍以上）的swap区，从"/dev/zero"文件读出内容并存到你的文件中，并设这

sudo dd if=/dev/zero of=在你电脑选择一个空间较大的位置/swapfile bs=1M count=20k
sudo mkswap 在你电脑选择一个空间较大的位置/swapfile

3.开启或关闭swap区

sudo swapon swapfile文件位置
sudo swapoff swapfile文件位置

4.开机设置
编辑 /etc/fstab ，加入如下一行：

swapfile文件位置     none    swap    defaults      0       0

2.数组下表溢出问题

回环检测的拼接地图JointMap（）函数

bool LoopClosing::JointMap(int key_frame_index, CloudData::CLOUD_PTR& map_cloud_ptr, Eigen::Matrix4f& map_pose) {
    //最相似帧的先验位姿
    map_pose = all_key_gnss_.at(key_frame_index).pose;
    //最相似帧的激光点云的索引
    current_loop_pose_.index0 = all_key_frames_.at(key_frame_index).index;
    
    // 合成地图
    //激光点云与gnss先验信息的变换矩阵
    Eigen::Matrix4f pose_to_gnss = map_pose * all_key_frames_.at(key_frame_index).pose.inverse();
    //将相似帧附近的关键帧合成局部地图
    for (int i = key_frame_index - extend_frame_num_; i < key_frame_index + extend_frame_num_; ++i) {
        // 原框架bug：key_frame_index=0时 i<0 数组越界报错
        //就是很奇怪，这个bug不应该存在啊 loop_step_不是有初值吗 怎么会key_frame_index=0呢？
        //试着添加下面的if判断，添加越界保护，错误消失
        if (i<0 || i>=all_key_frames_.size())
        {
            continue;
        }
        std::string file_path = key_frames_path_ + "/key_frame_" + std::to_string(all_key_frames_.at(i).index) + ".pcd";
        
        CloudData::CLOUD_PTR cloud_ptr(new CloudData::CLOUD());
        pcl::io::loadPCDFile(file_path, *cloud_ptr);
        //将激光点云转换到gnss的统一坐标下
        Eigen::Matrix4f cloud_pose = pose_to_gnss * all_key_frames_.at(i).pose;
        pcl::transformPointCloud(*cloud_ptr, *cloud_ptr, cloud_pose);

        *map_cloud_ptr += *cloud_ptr;
    }
    map_filter_ptr_->Filter(map_cloud_ptr, map_cloud_ptr);
    return true;
}

3.ScanContext移植问题

这个问题困扰了我两到三天，我甚至一度想要放弃移植了，主要是这个问题没有显示具体的错误，而是一个提示recipe for target “xxx/scancontext.cpp.o” failed(我每次一看到这种错误就头疼)

我做了很多的尝试，但是问题应该是出现在scancontext.cpp，于是又从github上重新下载了源码，但还是爆出相同的错误，折腾了好几天之后，我回顾之前写的关于 智能指针.的博客，想到是不是编译器和c++版本的问题，于是我着重看了下源码，注意c++高版本支持的特性，果然发现了一丝端倪。在detectLoopClosureID函数中，有如下一行：

polarcontext_tree_ = std::make_unique<InvKeyTree>(PC_NUM_RING /* dim */, polarcontext_invkeys_to_search_, 10 /* max leaf */ );

显然，make_unique这是c++14以后才支持的写法，于是我对源码做了如下改动：

polarcontext_tree_.reset(new InvKeyTree(PC_NUM_RING /* dim */, polarcontext_invkeys_to_search_, 10 /* max leaf */ )); 

重新catkin_make以后错误消失，编译成功～～～～～～～

4.ScanContext的点云类型问题

using SCPointType = pcl::PointXYZI

在它的注释里也说到这种类型是没有intensity信息的，所以为了和代码框架的点云匹配，修改为:

using SCPointType = pcl::PointXYZ