上一节介绍了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吧！！！！
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
