ROS-3DSLAM（九）lvi-sam源代码阅读7

2021SC@SDUSC

（九）lvi-sam源代码阅读7

lidar_odometry

mapOptmization.cpp

laserInfoHandler

对该回调函数的一些零散的分析：

关键成员变量

• 历史关键帧位姿：cloudKeyPoses3D cloudKeyPoses6D

• 当前帧位姿：transformTobeMapped

  • 前三位是旋转信息 后三位是位置信息

• 当前帧角点、平面点集合：laserCloudCornerLast laserCloudSurfaceLast

• 步骤2中筛选出来的角点、平面点集合（实际上是激光帧集合）：laserCloudCornerFromMap

• 降采样后的数据：…DS

• Last <==> 当前

• lastImuTransformation：imu里程计位姿信息

  lastVinsTransformation：imu里程计位姿信息（带位置信息的）

  • 记录的是：cloudInfo里的odom的相关信息

  根据上一帧激光位姿 ， 施加两帧imu里程计的位姿增量，得到当前帧位姿

• pcl::fromROSMsg(msgIn->cloud_corner, *laserCloudCornerLast);

  pcl::fromROSMsg(msgIn->cloud_surface, *laserCloudSurfLast); //对于角点和平面点的存储

订阅来着featureExtraction的信息

• 消息内容：发布当前激光帧提取特征之后的点云信息，包括的历史数据有：运动畸变矫正，点云数据，初始位姿，姿态角，有效点云数据，角点点云，平面点点云等

初始帧的过程：

​ （如果是第一帧 <==> cloudKeyPoses3D->points.empty()）

• 函数1：

  static Eigen::Affine3f lastImuTransformation;
  
  if (cloudKeyPoses3D->points.empty())
          {
        
        
              //大小为6的数组
              //当前帧位姿的旋转部分，用激光帧信息中的RPY（来自IMU原始数据）初始化
              transformTobeMapped[0] = cloudInfo.imuRollInit;
              transformTobeMapped[1] = cloudInfo.imuPitchInit;
              transformTobeMapped[2] = cloudInfo.imuYawInit;
  
              //在utility.h中定义
              if (!useImuHeadingInitialization)
                  transformTobeMapped[2] = 0;
              //应该是坐标 + 旋转角？？？
              lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); // save imu before return;
              return;
          }
  

• 函数2：

  if (cloudKeyPoses3D->points.empty() == true)
              return; 
  

  这一步的目的是提取之前关键帧的角点和平面点，因为这是第一帧，所以不存在历史关键帧

• 函数3：

  pcl::fromROSMsg(msgIn->cloud_corner,  *laserCloudCornerLast);
  pcl::fromROSMsg(msgIn->cloud_surface, *laserCloudSurfLast);
  
  
  void downsampleCurrentScan()
      {
        
        
          // Downsample cloud from current scan
          //对第二部得到的角点和平面点集合进一步降采样
          laserCloudCornerLastDS->clear();
          downSizeFilterCorner.setInputCloud(laserCloudCornerLast);
          downSizeFilterCorner.filter(*laserCloudCornerLastDS);
          laserCloudCornerLastDSNum = laserCloudCornerLastDS->size();
  
          laserCloudSurfLastDS->clear();
          downSizeFilterSurf.setInputCloud(laserCloudSurfLast);
          downSizeFilterSurf.filter(*laserCloudSurfLastDS);
          laserCloudSurfLastDSNum = laserCloudSurfLastDS->size();
      }
  
  

  降采样：对当前帧的集合进行降采样

  在上层的函数中进行了初始化

  Last <==> 当前

• 函数4：

  if (cloudKeyPoses3D->points.empty())
              return;
  

  不优化

• 函数5：

  // 计算当前帧与前一帧位姿变换，如果变化太小，不设为关键帧，反之设为关键帧
          if (saveFrame() == false)
              return;
  
  void addOdomFactor()
   // 检查是否有关键帧点
          if (cloudKeyPoses3D->points.empty())
          {
        
        
              // 第一帧初始化先验因子
              noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Variances((Vector(6) << 1e-2, 1e-2, M_PI*M_PI, 1e8, 1e8, 1e8).finished()); // rad*rad, meter*meter
              gtSAMgraph.add(PriorFactor<Pose3>(0, trans2gtsamPose(transformTobeMapped), priorNoise));
              // 变量节点设置初始值
              initialEstimate.insert(0, trans2gtsamPose(transformTobeMapped));
          }
  
  //闭环因子的添加跟这个无关
  
  //根据优化器的计算结果：
  // cloudKeyPoses3D 加入当前帧位姿
  // cloudKeyPoses6D 加入当前帧位姿
  // transformTobeMapped更新当前帧位姿
  
  // 保存特征点降采样集合
  // 更新里程计轨迹，也就是将 thisPose6D 的位姿加入到 "/lidar/mapping/path" 中去
  

  saveFrame函数里：如果是第一帧，一定会返回true

  cloudKeyPoses3D在这里被初始化和更新

• 函数6：

   if (cloudKeyPoses3D->points.empty())
              return;
  

• 函数7 8

  设计当前帧信息的发布；历史帧的不会发布

scan2map中的迭代过程

• transformTobeMapped在函数updatePointAssociateToMap中得到新的transPointAssociateToMap

• 对于每个角点/平面点（初始处在雷达系中），将其变换到map坐标系中（迭代的关键），这个变化需要用的transPointAssociateToMap（第一步中被更新）

• 根据变换后的点集合求相关点集

• LMOptimization函数中，会对收集到的信息做一系列操作，然后更新transformTobeMapped，并判断更新误差

  • 雷达系帧位姿–>map系帧位姿–>map系特征点–>匹配点–>LMOptimization–>雷达系帧位姿

loopHandler

闭环检测回调函数

void loopHandler(const std_msgs::Float64MultiArray::ConstPtr& loopMsg)
{
    
    
    // control loop closure frequency
    // 控制闭环检测频率，减轻计算负载
    static double last_loop_closure_time = -1;
    {
    
    
        // std::lock_guard<std::mutex> lock(mtx);
        //如果上一次该函数执行的时间与 最近一帧激光帧的时间间隔<5 就不执行本次闭环检测（控制频率）
        if (timeLaserInfoCur - last_loop_closure_time < 5.0)
            return;
        else
        //更新闭环检测时间
            last_loop_closure_time = timeLaserInfoCur;
    }
    // 闭环scan-to-map，icp优化位姿
    performLoopClosure(*loopMsg);
}

void performLoopClosure(const std_msgs::Float64MultiArray& loopMsg)
{
    
    
    pcl::PointCloud<PointTypePose>::Ptr copy_cloudKeyPoses6D(new pcl::PointCloud<PointTypePose>());
    {
    
    
        std::lock_guard<std::mutex> lock(mtx);
        *copy_cloudKeyPoses6D = *cloudKeyPoses6D;
    }

    // get lidar keyframe id
    int key_cur = -1; // latest lidar keyframe id
    int key_pre = -1; // previous lidar keyframe id
    {
    
    
        // 前后最近的关键帧下标不合法
        // 在该函数中，如果合法的话，会对key_cur和key_pre进行赋值
        /*
            直接return
            上界是最近一帧 || 所有帧的时间戳都大于回环时间戳上界（这样应该是出bug了）
            下界是初始帧 || 所有帧的时间戳都小于回环时间戳下界（这样应该是出bug了）
            两个key重叠了（就是同一帧）
        */
        loopFindKey(loopMsg, copy_cloudKeyPoses6D, key_cur, key_pre);
        if (key_cur == -1 || key_pre == -1 || key_cur == key_pre)// || abs(key_cur - key_pre) < 25)
            return;
    }
    // check if loop added before
    // 检查该帧是否已经被回环过，因为图像回环，很多图像可能指向同一个关键帧
    {
    
    
        // if image loop closure comes at high frequency, many image loop may point to the same key_cur如果图像循环闭合频率很高，则许多图像循环可能指向相同的 key_cur
        auto it = loopIndexContainer.find(key_cur);
        //该帧之前被回环过（需要看后面，是start 还是 end）---- loopIndexContainer[key_cur] = key_pre;
        if (it != loopIndexContainer.end())
            return;
    }
    // get lidar keyframe cloud
    pcl::PointCloud<PointType>::Ptr cureKeyframeCloud(new pcl::PointCloud<PointType>());
    pcl::PointCloud<PointType>::Ptr prevKeyframeCloud(new pcl::PointCloud<PointType>());
    {
    
    
        loopFindNearKeyframes(copy_cloudKeyPoses6D, cureKeyframeCloud, key_cur, 0);
        loopFindNearKeyframes(copy_cloudKeyPoses6D, prevKeyframeCloud, key_pre, historyKeyframeSearchNum);
        // cureKeyframeCloud 中存储的是提取自多个关键帧的特征点集合
        /*
        *nearKeyframes += *transformPointCloud(cornerCloudKeyFrames[key_near], &copy_cloudKeyPoses6D->points[key_near]);
        *nearKeyframes += *transformPointCloud(surfCloudKeyFrames[key_near],   &copy_cloudKeyPoses6D->points[key_near]);
        */
       // 特征点太少
        if (cureKeyframeCloud->size() < 300 || prevKeyframeCloud->size() < 1000)
            return;
        if (pubHistoryKeyFrames.getNumSubscribers() != 0)
        // 发布闭环匹配关键帧局部map
            publishCloud(&pubHistoryKeyFrames, prevKeyframeCloud, timeLaserInfoStamp, "odom");
    }
    // get keyframe pose
    // 求前后两帧位置之间的 tf
    Eigen::Affine3f pose_cur;
    Eigen::Affine3f pose_pre;
    Eigen::Affine3f pose_diff_t; // serves as initial guess 作为初始猜测
    {
    
    
        // 格式转换
        pose_cur = pclPointToAffine3f(copy_cloudKeyPoses6D->points[key_cur]);
        pose_pre = pclPointToAffine3f(copy_cloudKeyPoses6D->points[key_pre]);

        Eigen::Vector3f t_diff;
        t_diff.x() = - (pose_cur.translation().x() - pose_pre.translation().x());
        t_diff.y() = - (pose_cur.translation().y() - pose_pre.translation().y());
        t_diff.z() = - (pose_cur.translation().z() - pose_pre.translation().z());
        if (t_diff.norm() < historyKeyframeSearchRadius)
            t_diff.setZero();
        pose_diff_t = pcl::getTransformation(t_diff.x(), t_diff.y(), t_diff.z(), 0, 0, 0);
    }

    // transform and rotate cloud for matching 变换和旋转云进行匹配
    pcl::IterativeClosestPoint<PointType, PointType> icp;
    // pcl::GeneralizedIterativeClosestPoint<PointType, PointType> icp;
    // ICP参数设置
    icp.setMaxCorrespondenceDistance(historyKeyframeSearchRadius * 2);
    icp.setMaximumIterations(100);
    icp.setRANSACIterations(0);
    icp.setTransformationEpsilon(1e-3);
    icp.setEuclideanFitnessEpsilon(1e-3);

    // initial guess cloud
    pcl::PointCloud<PointType>::Ptr cureKeyframeCloud_new(new pcl::PointCloud<PointType>());
    pcl::transformPointCloud(*cureKeyframeCloud, *cureKeyframeCloud_new, pose_diff_t);

    // match using icp
    //设置输入的点云
    icp.setInputSource(cureKeyframeCloud_new);
    //设置目标点云
    icp.setInputTarget(prevKeyframeCloud);
    pcl::PointCloud<PointType>::Ptr unused_result(new pcl::PointCloud<PointType>());
    icp.align(*unused_result);

    if (pubIcpKeyFrames.getNumSubscribers() != 0)
    {
    
    
        //发布
        pcl::PointCloud<PointType>::Ptr closed_cloud(new pcl::PointCloud<PointType>());
        pcl::transformPointCloud(*cureKeyframeCloud_new, *closed_cloud, icp.getFinalTransformation());
        publishCloud(&pubIcpKeyFrames, closed_cloud, timeLaserInfoStamp, "odom");
    }

    // add graph factor
    // 已收敛，或者匹配分数足够好
    if (icp.getFitnessScore() < historyKeyframeFitnessScore && icp.hasConverged() == true)
    {
    
    
        // get gtsam pose
        gtsam::Pose3 poseFrom = affine3fTogtsamPose3(Eigen::Affine3f(icp.getFinalTransformation()) * pose_diff_t * pose_cur);
        gtsam::Pose3 poseTo   = pclPointTogtsamPose3(copy_cloudKeyPoses6D->points[key_pre]);
        // get noise
        float noise = icp.getFitnessScore();
        gtsam::Vector Vector6(6);
        Vector6 << noise, noise, noise, noise, noise, noise;
        noiseModel::Diagonal::shared_ptr constraintNoise = noiseModel::Diagonal::Variances(Vector6);
        // save pose constraint
        // 保存当前回环组合的下标、位姿变换、噪声
        mtx.lock();
        loopIndexQueue.push_back(make_pair(key_cur, key_pre));
        loopPoseQueue.push_back(poseFrom.between(poseTo));
        loopNoiseQueue.push_back(constraintNoise);
        mtx.unlock();
        // add loop pair to container
        // 在容器中加入两个下标关联，以后可以防止重复处理
        loopIndexContainer[key_cur] = key_pre;
    }

    // visualize loop constraints
    // rviz展示闭环边
    if (!loopIndexContainer.empty())
    {
    
    
        //...不做具体分析了
    }
}

在历史关键帧中寻找在时间上与两个回环时间点相匹配的帧

// copy_cloudKeyPoses6D 历史关键帧集合
//*copy_cloudKeyPoses6D = *cloudKeyPoses6D;
//loopFindKey(loopMsg, copy_cloudKeyPoses6D, key_cur, key_pre);
void loopFindKey(const std_msgs::Float64MultiArray& loopMsg, 
                 const pcl::PointCloud<PointTypePose>::Ptr& copy_cloudKeyPoses6D,
                 int& key_cur, int& key_pre)
{
    
    
    // 对 loopMsg 可能存在的赋值错误的情况
    if (loopMsg.data.size() != 2)
        return;

    // 提取当前和上一帧的回环时间戳
    double loop_time_cur = loopMsg.data[0];
    double loop_time_pre = loopMsg.data[1];

    // 如果两个时间戳之间的时间间隔小于某个值，则返回
    if (abs(loop_time_cur - loop_time_pre) < historyKeyframeSearchTimeDiff)
        return;

    int cloudSize = copy_cloudKeyPoses6D->size();
    //如果历史关键帧的个数<2 则返回
    if (cloudSize < 2)
        return;

    // latest key
    key_cur = cloudSize - 1;
    // 找到第一个小于等于 loop_time_cur 的时间戳下标（之后的时间）
    for (int i = cloudSize - 1; i >= 0; --i)
    {
    
    
        if (copy_cloudKeyPoses6D->points[i].time > loop_time_cur)
            key_cur = round(copy_cloudKeyPoses6D->points[i].intensity);
        else
            break;
    }

    // previous key
    // 找到第一个大于等于 loop_time_cur 的时间戳下标
    key_pre = 0;
    for (int i = 0; i < cloudSize; ++i)
    {
    
    
        if (copy_cloudKeyPoses6D->points[i].time < loop_time_pre)
            key_pre = round(copy_cloudKeyPoses6D->points[i].intensity);
        else
            break;
    }
}

const std_msgs::Float64MultiArray& loopMsg ：

• 里面只有两个东西（估计是两个关键帧 ，两个时间戳）

楼上函数做的事情如下图：

提取闭环匹配关键帧前后相邻若干帧的关键帧特征点集合，降采样

/*

loopFindNearKeyframes(copy_cloudKeyPoses6D, cureKeyframeCloud, key_cur, 0);
loopFindNearKeyframes(copy_cloudKeyPoses6D, prevKeyframeCloud, key_pre, historyKeyframeSearchNum);
*/
void loopFindNearKeyframes(const pcl::PointCloud<PointTypePose>::Ptr& copy_cloudKeyPoses6D,
                           pcl::PointCloud<PointType>::Ptr& nearKeyframes, 
                           const int& key, const int& searchNum)
{
    
    
    // extract near keyframes
    // 提取key索引的关键帧前后相邻若干帧的关键帧特征点集合
    nearKeyframes->clear();
    // key_cur的searchNum = 0  nearKeyframes里只会添加自己
    // key_pre就是正常添加了（默认值是25）
    int cloudSize = copy_cloudKeyPoses6D->size();
    for (int i = -searchNum; i <= searchNum; ++i)
    {
    
    
        int key_near = key + i;
        if (key_near < 0 || key_near >= cloudSize )
            continue;
        *nearKeyframes += *transformPointCloud(cornerCloudKeyFrames[key_near], &copy_cloudKeyPoses6D->points[key_near]);
        *nearKeyframes += *transformPointCloud(surfCloudKeyFrames[key_near],   &copy_cloudKeyPoses6D->points[key_near]);
    }

    // 没找到相邻帧
    if (nearKeyframes->empty())
        return;

    // downsample near keyframes 降采样
    pcl::PointCloud<PointType>::Ptr cloud_temp(new pcl::PointCloud<PointType>());
    downSizeFilterICP.setInputCloud(nearKeyframes);
    downSizeFilterICP.filter(*cloud_temp);
    *nearKeyframes = *cloud_temp;
}

前置知识

回环检测

之前已经初步的了解过这一概念，现在需要具体的学习，现在的打算是学习SLAM14讲中有关回环检测的内容，也为下一阶段视觉部分的学习打基础。

icp优化

ICP（Iterative Closest Point），即最近点迭代算法，是最为经典的数据配准算法。

SLAM14讲 第11讲 回环检测

11.1 概述

11.1.1 回环检测的意义

消除累计误差

关系到估计的轨迹和地图在长时间下的正确性

11.1.2 回环检测的方法

朴素思路：全都检查 or 随机检查

基于里程计的几何关系

• 由于累计误差的存在，无法正确发现 运动到了之前某个位置附近 这一事实

基于外观的几何关系：主流做法

GPS（室外）

基于外观的回环检测算法中，核心问题是如何计算图像间的相似性

11.1.3 准确率和召回率

算法/事实	是回环	不是回环
是回环	真阳性	假阳性
不是回环	假阴性	真阴性

真阳性（true positive TP） 假阳性（false positive FP） 假阴性（false negative FN） 真阴性（true negative TN）

准确率（precision） 召回率（caller）
$$precison=TP/(TP+FP);caller=TP/(TP+FN)$$
准确率和召回率通常是矛盾的

• 提高阈值 --> 算法变严格 --> 检测出更少的回环 --> 准确率提高，召回率降低
• 反之相反

在SLAM中，对准确率的要求很高，而对召回率则相对宽松

• 假阳性回环将在后端的位姿中添加根本错误的边，有时候会导致优化算法给出完全错误的结果，导致整个地图失效
• 召回率低只会导致部分回环没有被检测到

so，在选择回环算法时，倾向把参数设置的更严格，或者在检测之后加上回环验证的步骤

11.2 词袋模型

词袋（Bag-of-Words BoW），目的是用图片上有哪几种特征来描述一副画像。

小例子：某张照片中有一辆车，一个人；另一张照片中有两个人、一只狗。根据这样的描述，就可以度量两张图片的相似性。

• 确定 人、车、狗等概念 ---- 对应于BoW中的单词（Word），许多单词放到一起，组成了字典（Dictionary）
• 确定一幅图片中出现了哪些在字典中定义的概念 ---- 用单词出现的情况（直方图）描述整幅图像，从而把一幅图像转换成了一个向量的描述
• 比较上一步中的描述的相似程度

设每个单词为w1, w2, w3，对于图像，根据他们含有的单词，可以记为：
$$A=1\cdot w_1+1\cdot w_2+0\cdot w_3$$
字典是固定的，所以只要用 [1,1,0]T 这个向量可以表达A

• 向量描述的是图像中是否含有某些信息，比单纯的灰度值更加稳定
• 不关心特征在哪出现，所以与物体的空间位置和排列顺序无关
• 向量中的值，可以表示出现的次数，也可以仅考虑是否出现而不考虑数量

11.3 字典

字典由很多单词构成，每个单词代表了一类概念。

一个单词与一个特征点不同，他不是从单幅图片上提取出来的，而是某一类特征的组合。所以，字典生成问题类似于一个聚类（Clustering）问题（聚类问题再无监督机器学习（Unsupervised ML）中很常见，用于让机器自行寻找数据中的规律）。

问题：对大量的图像提取了N个特征点，并寻找一个k个单词的字典，每个单词可以看作局部相邻特征点的集合。

方法：K-means（K-均值）算法。

步骤：

• 随机选取k个中心点：c1…ck
• 对于每个样本，计算它与中心点之间的距离，去最小的作为他的归类
• 重新计算每个类的中心点
• 如果每个中心点都变化很小，则算法收敛；否则重复第二步

可以使用k叉树来表达字典。类似于层次聚类，是K-means的直接扩展。

假设有N个特征点，希望构建一个深度为d，每次分叉为k的树，那么做法如下：

• 在根节点，用K-means把所有样本聚成k类（实际中为了保证聚类均匀会使用K-means++）。得到了第一层
• 对第一层的每个节点，把属于该节点的样本再聚成k类，得到下一层
• 依次类推，最后得到叶子层。叶子层即所谓的Words

上述结构可容纳k^d个单词。在查找某个给定单词对应的单词时，只需要将它与每个中间节点的聚类中心比较（一共d次）。

11.4 相似度计算

11.4.1 理论部分

有了字典以后，给定任意特征f，只要在字典树中逐层查找，最后都能找到与之对应的单词w ---- 当字典足够大时，我们认为f和w来自同一类物体。

从一副图片中提取了N个特征 --> 找到N个特征对应的单词 --> 拥有了该图像在单词列表中的分布。

问题：希望对单词的区分性或者重要性加以评估，给他们不同的权值以起到更好的效果。

TF-IDF（Term Frequency-Inverse Document Frequency），译频率 - 逆文档频率，是文本检索中的一项常见的加权方式。

• TF：某单词在一幅图像中经常出现，他的区分度就高
• IDF：某单词在字典中出现的频率越低，分类图像时区分度越高
• 如果某个单词在一篇文章中出现的频率高，并且在其他文章中很少出现，则认为此词或者短语具有很好的类别区分能力，适合用来分类。

IDF：统计某叶子节点wi中的特征数量相对于所有特征数量的比例（构建时有多少特征属于该单词），作为IDF的部分。假设所有的特征数量为n，wi数量为ni，则该单词的IDF为：
$$ID{F}_i=logn/n_i$$
https://zhuanlan.zhihu.com/p/45573552

TF：TF部分则是指某个特征在单幅图像中出现的频率。假设图像A中单词wi出现了ni次，而一共出现的单词次数为n，那么TF：
$$T{F}_i=n_i/n$$
wi的权重等于TF与IDF之积：
$${\mu }_i=T{F}_i\times ID{F}_i$$
考虑权重后，对于某幅图像A，它的特征点可以对应到许多个单词，组成它的BoW：
A = { ( w 1 . μ 1 ) , ( w 2 , μ 2 ) . . . . . . ( w N , μ N ) } , d e f i n e = v A A = \{(w_1.μ_1),(w_2,μ_2)......(w_N,μ_N) \} ,define=v_A A={ (w1​.μ1​),(w2​,μ2​)......(wN​,μN​)},define=vA​
vA中会存在大量的零，非零部分指示了图像A中含有哪些单词，这些部分的值是TF-IDF值

问题：对于给定的vA和vB，如何计算差异？存在若干种解决方案，具体问题具体分析。

11.5 实验分析与评价

11.5.1 增加字典规模

当增加字典规模时，无关图像的相似性明显变小。而本身相似的图像，虽然分值也会下降，但是相对于其他图像的评分，却变得更加显著了。这说明增加字典训练样本是有益的

11.5.2 相似性评分的处理

有时候相似性评分的绝对大小对算法不一定有帮助（eg：有的环境的外观本身就很相似，如办公室中有很多的同款桌椅；有的环境则各个地方有很大的不同）。考虑这种情况，会取一个先验相似度，表示某时刻关键帧图像与上一时刻关键帧的相似性，然后其他分值都依照这个值进行归一化

11.5.3 关键帧的处理

• 从实践来说，用于回环检测的帧最好稀疏一点，彼此之间不太相同，又能涵盖整个环境。
• 把相近的回环聚成一类，使算法不要反复地检测同一类地回环（eg：第1帧与第n帧形成回环，接下来的n+1帧、n+2帧可能与第1帧形成回环，但是意义不大）

11.5.4 检测之后的验证

词袋的回环检测算法的问题：

• 只考虑图像的外观而不利用几何信息，导致任何外观相似的图像容易被当成回环
• 由于词袋不在乎单词顺序，容易引发感知偏差

验证方法：

• 设立回环的缓存机制，认为单次检测到的回环并不足以构成良好的约束，而在一段时间内一直检测到的回环，才是真正的回环
  • 可以看成是时间上的一致性检测
• 对回环检测到的两个帧进行特征匹配，估计相机的运动。然后把运动放到之前的位姿图中，检查与之前的估计是否有很大的出入。
  • 空间上的一致性检测