做slam怎么能少得了闭环。

闭环检测的代码在文件include/hdl_graph_slam/loop_detector.hpp中,整个流程分布的很清晰,我们直接进入看代码的环节了

1. 整体流程

在调用闭环检测功能时,实际是调用的函数detect,所以我们直接看这个函数,就可以看出它的流程

std::vector<Loop::Ptr> detect(const std::vector<KeyFrame::Ptr>& keyframes, const std::deque<KeyFrame::Ptr>& new_keyframes, hdl_graph_slam::GraphSLAM& graph_slam) {
    std::vector<Loop::Ptr> detected_loops;
    for(const auto& new_keyframe : new_keyframes) {
      auto candidates = find_candidates(keyframes, new_keyframe);
      auto loop = matching(candidates, new_keyframe, graph_slam);
      if(loop) {
        detected_loops.push_back(loop);
      }
    }

    return detected_loops;
  }

它的输入是:

1)keyframes:已有的关键帧

2)new_keyframes:新增的关键帧

3)graph_slam:这个参数没有被用到,估计作者可能是原计划要用,但后来没用到,也忘了删了

它的输出是:

1)std::vector<Loop::Ptr>:所有检测成功的匹配关系

具体来讲,包括它是由哪两帧匹配的,匹配计算得到的相对位姿是多少。

这个看Loop这个结构提的定义就明白了

struct Loop {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  using Ptr = std::shared_ptr<Loop>;

  Loop(const KeyFrame::Ptr& key1, const KeyFrame::Ptr& key2, const Eigen::Matrix4f& relpose)
    : key1(key1),
      key2(key2),
      relative_pose(relpose)
  {}

public:
  KeyFrame::Ptr key1;
  KeyFrame::Ptr key2;
  Eigen::Matrix4f relative_pose;
};

它包含的就是上面我们所提到的这三个信息。

由于每次检测可能有多个新增的关键帧要参与,有可能检测到多对匹配关系,所以这里用一个vector把所有关系同时输出。

弄明白输入输出,这个函数的功能就可以概括了:
从已有的关键帧中,检测与新增的关键帧是否有闭环关系,如果有,则返回所有检测结果。

而且上面的代码还告诉了我们)进行闭环检测的步骤:

1)检测满足条件的关键帧,以进行下一步匹配

2)对检测到的关键帧进行匹配,找出匹配精度最好的一帧,并返回匹配关系

3)把第2)步找到的匹配关系放入容器中

4)循环上面三步,直到所有新的关键帧遍历完

上面四步中,第3)和第4)部就是几行语句而已,需要展开介绍的是第1)和第2)步,下面我们就分别对这两步进行介绍。

2. 检测满足条件的关键帧

这个功能被封装在了函数find_candidates中,我们看它的代码

std::vector<KeyFrame::Ptr> find_candidates(const std::vector<KeyFrame::Ptr>& keyframes, const KeyFrame::Ptr& new_keyframe) const {
    // too close to the last registered loop edge
    if(new_keyframe->accum_distance - last_edge_accum_distance < distance_from_last_edge_thresh) {
      return std::vector<KeyFrame::Ptr>();
    }

    std::vector<KeyFrame::Ptr> candidates;
    candidates.reserve(32);

    for(const auto& k : keyframes) {
      // traveled distance between keyframes is too small
      if(new_keyframe->accum_distance - k->accum_distance < accum_distance_thresh) {
        continue;
      }

      const auto& pos1 = k->node->estimate().translation();
      const auto& pos2 = new_keyframe->node->estimate().translation();

      // estimated distance between keyframes is too small
      double dist = (pos1.head<2>() - pos2.head<2>()).norm();
      if(dist > distance_thresh) {
        continue;
      }

      candidates.push_back(k);
    }

    return candidates;
  }

从上面的代码中我们可以看到,所谓的满足条件,指的是满足以下条件:

1)新的关键帧离上一个被闭环的帧“里程距离”不能过小

注意,这里特意强调了是“里程距离”,我们举个例子,来弄明白这个概念。

比如第1帧往前走了1米得到了第2帧,第2帧往后走了1米得到了第3帧,那么第1帧和第3帧的“直线距离”就是0,但是总的里程是2米,所以他们的“里程距离”就是2。闭环应该找“直线距离”尽量进而“里程距离”尽量远的,所以这里才要求“里程距离”不能过小。

其实我觉得给每一个关键帧一个编号,编号顺序递增,用编号计算一个“编号距离”应该更简单,不用每次还要计算“里程距离”这么麻烦。

弄明白了它的原理,我们就清楚它的作用了,就是把闭环关系弄的稀疏一些,别整那么多,如果来了是个关键帧,没一帧都弄了一个闭环边添加到图里,也没那个必要,所以有了这一步以后,我们会在所有新的关键帧中取一小部分建立闭环关系,既能起到约束作用,又不至于把模型弄的复杂。

2)已有关键帧离新的关键帧的“里程距离”不能过小

3)已有关键帧和新的关键帧之间的“直线距离”要足够小

能够同时满足这三个条件的就可以进入下一步的匹配环节了。

3. 闭环匹配

上一步索引出来的关键帧还要通过这一步的检验,才能够算是真正成功实现了闭环匹配。这一步在函数matching中,我们还是直接看代码

Loop::Ptr matching(const std::vector<KeyFrame::Ptr>& candidate_keyframes, const KeyFrame::Ptr& new_keyframe, hdl_graph_slam::GraphSLAM& graph_slam) {
    if(candidate_keyframes.empty()) {
      return nullptr;
    }

    registration->setInputTarget(new_keyframe->cloud);

    double best_score = std::numeric_limits<double>::max();
    KeyFrame::Ptr best_matched;
    Eigen::Matrix4f relative_pose;

    std::cout << std::endl;
    std::cout << "--- loop detection ---" << std::endl;
    std::cout << "num_candidates: " << candidate_keyframes.size() << std::endl;
    std::cout << "matching" << std::flush;
    auto t1 = ros::Time::now();

    pcl::PointCloud<PointT>::Ptr aligned(new pcl::PointCloud<PointT>());
    for(const auto& candidate : candidate_keyframes) {
      registration->setInputSource(candidate->cloud);
      Eigen::Matrix4f guess = (new_keyframe->node->estimate().inverse() * candidate->node->estimate()).matrix().cast<float>();
      guess(2, 3) = 0.0;
      registration->align(*aligned, guess);
      std::cout << "." << std::flush;

      double score = registration->getFitnessScore(fitness_score_max_range);
      if(!registration->hasConverged() || score > best_score) {
        continue;
      }

      best_score = score;
      best_matched = candidate;
      relative_pose = registration->getFinalTransformation();
    }

    auto t2 = ros::Time::now();
    std::cout << " done" << std::endl;
    std::cout << "best_score: " << boost::format("%.3f") % best_score << "    time: " << boost::format("%.3f") % (t2 - t1).toSec() << "[sec]" << std::endl;

    if(best_score > fitness_score_thresh) {
      std::cout << "loop not found..." << std::endl;
      return nullptr;
    }

    std::cout << "loop found!!" << std::endl;
    std::cout << "relpose: " << relative_pose.block<3, 1>(0, 3) << " - " << Eigen::Quaternionf(relative_pose.block<3, 3>(0, 0)).coeffs().transpose() << std::endl;

    last_edge_accum_distance = new_keyframe->accum_distance;

    return std::make_shared<Loop>(new_keyframe, best_matched, relative_pose);
  }

从上面的代码中我们可以看到,它的核心流程可以概括如下:

1)对每一个关键帧都和新帧进行点云匹配,看匹配精度满足闭环的基本要求

2)如果满足,则在所有满足条件的关键帧中找出匹配精度最高的那一帧

4. 一点思考

在上面的流程中,我认为还是有一些不合理的地方

1)匹配点云的时候还是只对两帧进行匹配,这一点我们在讲前端里程计的时候就提到过,我认为把关键帧的前后几帧都找出来,拼成一个局部地图再去匹配应该精度会更高,毕竟两帧点云匹配太稀疏了。

2)对所有满足条件的关键帧都做一次点云匹配太浪费时间了,帧间匹配还是挺耗时间的,当地图稍微大一些,有几千个关键帧的时候,一个闭环检测要匹配多少次,这肯定不行。我认为找一个距离最近的关键帧,然后以它为中心取前后几帧构建一个局部地图去匹配就行了,精度满足就建一条闭环边,不满足就不要了,等下一次。