BLAM源码解析（五）—— 回环检测

上一节介绍了BLAM的帧间匹配和帧图匹配，代码简洁明了。

本节介绍BLAM的回环检测模块。具体代码块如下：

 // Check for new loop closures.
  bool new_keyframe;
  if (HandleLoopClosures(msg_filtered, &new_keyframe)) {
    // We found one - regenerate the 3D map.
    PointCloud::Ptr regenerated_map(new PointCloud);
    loop_closure_.GetMaximumLikelihoodPoints(regenerated_map.get());

    mapper_.Reset();
    PointCloud::Ptr unused(new PointCloud);
    mapper_.InsertPoints(regenerated_map, unused.get());

    // Also reset the robot's estimated position.
    localization_.SetIntegratedEstimate(loop_closure_.GetLastPose());
  } else {
    // No new loop closures - but was there a new key frame? If so, add new
    // points to the map.
    if (new_keyframe) {
      localization_.MotionUpdate(gu::Transform3::Identity());
      localization_.TransformPointsToFixedFrame(*msg_filtered, msg_fixed.get());
      PointCloud::Ptr unused(new PointCloud);
      mapper_.InsertPoints(msg_fixed, unused.get());
    }
  }

  // Visualize the pose graph and current loop closure radius.
  loop_closure_.PublishPoseGraph();

我们知道视觉SLAM一般通过词袋模型进行回环，激光SLAM中的佼佼者Cartographer通过分支定界+栅格匹配进行回环，等等。BLAM由于构建的是点云地图，不涉及栅格更新，因此通过最简单的ICP进行回环。接下来逐行解析：

首先使用HandleLoopClosures，向位姿图中加入新位姿节点：

  if (!loop_closure_.AddBetweenFactor(localization_.GetIncrementalEstimate(),  // MJY
                                      covariance, &pose_key)) {

接下来使用AddKeyScanPair，向回环检测模块加入关键帧。

  if (!loop_closure_.AddKeyScanPair(pose_key, scan)) {

然后最重要的：在可回环范围内寻找回环，可回环范围由第一节介绍的几个参数决定。

  if (!loop_closure_.FindLoopClosures(pose_key, &closure_keys)) {

寻找回环的过程中将回环边加入位姿图。

让我们回到BlamSlam.cc，继续HandleLoopClosures以下的内容，使用了如下函数将历史关键帧的点云转换到全局坐标系，合并到一起，加入到回环检测模块的mapper_中：

    loop_closure_.GetMaximumLikelihoodPoints(regenerated_map.get());

注意，这个点云是经过回环校正的点云，因此是经过回环检测优化的map。

然后用回环校正的位姿，更新定位模块localization_中的位姿，即：

void PointCloudLocalization::SetIntegratedEstimate(
    const gu::Transform3& integrated_estimate) {
  integrated_estimate_ = integrated_estimate;

  // Publish transform between fixed frame and localization frame.
  geometry_msgs::TransformStamped tf;
  tf.transform = gr::ToRosTransform(integrated_estimate_);
  tf.header.stamp = stamp_;
  tf.header.frame_id = fixed_frame_id_;
  tf.child_frame_id = base_frame_id_;
  tfbr_.sendTransform(tf);
}

最终发布位姿图。没有订阅者是不发布的。

  loop_closure_.PublishPoseGraph();

至此，BLAM源码介绍完毕，接下来尽情调参，实现你自己特定场景的SLAM吧！！！！