开源激光SLAM项目BLAM－－－－－１

最近在学习SLAM和ROS，首先接触到的是github上的开源项目BLAM，是berkely的一位小哥所写，油管上有相关的视频。这篇教程面向于SLAM和ROS的初学者，如果有问题还希望各位大神进行指正。

---

先贴一张github上附带的效果图
　　

---

系统概述

BLAM使用PointCloudXYZ类型点云数据作为输入，在视频里使用一个velodyneVLP16激光雷达作为传感器，逐步的绘制出整个点云地图。
BLAM系统框架很简单:

Created with Raphaël 2.2.0 PCL点云数据处理 定位 非线性优化 回环? 调整地图 建立地图 yes no

代码主要分为下列几个大部分
- point_cloud_filter　：对输入的点云数据进行处理，得到过滤后的点云数据
- point_cloud_odometry　：通过GICP算法计算两帧点云数据的位姿变换（初步计算）
- point_cloud_localization　：通过初步计算的位姿变换来获取当前帧对应的地图中最近临点，再次执行GICP得到精确的位姿变换（第二次计算）
- laser_loop_closure　：对当前帧的点云数据与历史点云数据对比判断回环是否发生，发布位姿数据
- point_cloud_mapper　：构建点云地图，发布地图数据

---

BLAM的整个点云数据处理从 blam_slam.cc起始

#include 
#include 

int main(int argc, char** argv) {
  ros::init(argc, argv, "blam_slam");　　//初始化ROS节点并命名
  ros::NodeHandle n("~");　　//定义私有命名空间　　详情请看另一篇博文

  BlamSlam bs;
  if (!bs.Initialize(n, false /* online processing */)) {　//　　调用Initialize函数，采用离线模式
    ROS_ERROR("%s: Failed to initialize BLAM SLAM.",
              ros::this_node::getName().c_str());
    return EXIT_FAILURE;
  }
  ros::spin();  　

  return EXIT_SUCCESS;
}

---

这里定义了句柄n并执行了初始化函数，所以我们进入到BlamSlam::Initialize里

bool BlamSlam::Initialize(const ros::NodeHandle& n, bool from_log) {
  name_ = ros::names::append(n.getNamespace(), "BlamSlam");

  if (!filter_.Initialize(n)) {   //内部主要工作为提取.yaml文件参数　　
    ROS_ERROR("%s: Failed to initialize point cloud filter.", name_.c_str());
    return false;
  }

  if (!odometry_.Initialize(n)) {    //参数为NodeHandle 保证在同一个命名空间
    ROS_ERROR("%s: Failed to initialize point cloud odometry.", name_.c_str());
    return false;
  }

  if (!loop_closure_.Initialize(n)) {
    ROS_ERROR("%s: Failed to initialize laser loop closure.", name_.c_str());
    return false;
  }

  if (!localization_.Initialize(n)) {
    ROS_ERROR("%s: Failed to initialize localization.", name_.c_str());
    return false;
  }

  if (!mapper_.Initialize(n)) {
    ROS_ERROR("%s: Failed to initialize mapper.", name_.c_str());
    return false;
  }

  if (!LoadParameters(n)) {
    ROS_ERROR("%s: Failed to load parameters.", name_.c_str());
    return false;
  }

  if (!RegisterCallbacks(n, from_log)) {
    ROS_ERROR("%s: Failed to register callbacks.", name_.c_str());
    return false;
  }

  return true;
}

这里各个模块的Initialize()函数主要作用是提取.yaml文件里的参数。不太重要，有兴趣的可以自行看一下
看到最后一个**RegisterCallbacks(n, from_log)**是初始化函数的重点，具体内容是

bool BlamSlam::RegisterCallbacks(const ros::NodeHandle& n, bool from_log) {
  // Create a local nodehandle to manage callback subscriptions.
  ros::NodeHandle nl(n);  //以n为父节点

  visualization_update_timer_ = nl.createTimer(
      visualization_update_rate_, &BlamSlam::VisualizationTimerCallback, this);
  //创建一个timer定时调用VisualizationTimerCallback,其内容是发布**地图**话题
  if (from_log)   //这里是false 所以进入online
    return RegisterLogCallbacks(n);
  else
    return RegisterOnlineCallbacks(n);  //进入这个
    
}

---

下面我们继续进入**RegisterOnlineCallbacks(n)**函数

bool BlamSlam::RegisterOnlineCallbacks(const ros::NodeHandle& n) {
  ROS_INFO("%s: Registering online callbacks.", name_.c_str());

  // Create a local nodehandle to manage callback subscriptions.
  ros::NodeHandle nl(n);

  estimate_update_timer_ = nl.createTimer(
      estimate_update_rate_, &BlamSlam::EstimateTimerCallback, this); 
 //定时器
  pcld_sub_ = nl.subscribe("pcld", 100, &BlamSlam::PointCloudCallback, this);
 //消息订阅
  return CreatePublishers(n);
}

首先先看到订阅者，订阅了一个名为pcld的相对话题名称，在launch文件里这个话题名被重映射到了/PointCloudXYZ，所以这里订阅的是PointCloudXYZ的点云数据

另外是一个以**estimate_update_rate_为调用间隔的EstimateTimerCallback()**函数，我们继续进入

void BlamSlam::EstimateTimerCallback(const ros::TimerEvent& ev) {
  // Sort all messages accumulated since the last estimate update.
  synchronizer_.SortMessages();　　　                      //对点云数据根据时间排序，并得到index数组

  // Iterate through sensor messages, passing to update functions.
  MeasurementSynchronizer::sensor_type type;
  unsigned int index = 0;
  while (synchronizer_.GetNextMessage(&type, &index)) {   //找到下一个点云数据的index
    switch(type) {

      // Point cloud messages.
      case MeasurementSynchronizer::PCL_POINTCLOUD: {　　　　//根据类型进入这个分支
        const MeasurementSynchronizer::Message::ConstPtr& m =
            synchronizer_.GetPCLPointCloudMessage(index);  //通过index得到pointcloud数据

        ProcessPointCloudMessage(m->msg); //执行数据处理 *********
        break;
      }

      // Unhandled sensor messages.
      default: {
        ROS_WARN("%s: Unhandled measurement type (%s).", name_.c_str(),
                 MeasurementSynchronizer::GetTypeString(type).c_str());
        break;
      }
    }
  }

  // Remove processed messages from the synchronizer.
  synchronizer_.ClearMessages();
}

这个函数主要工作是，根据timestamp对暂存的点云数据进行排序，根据排序结果依次对点云数据进行处理(调用**ProcessPointCloudMessage(m->msg)**函数)

所有数据处理工作都在**ProcessPointCloudMessage(m->msg)**函数中完成，这一章主要介绍了整个程序的的入口，下一章里会介绍点云处理过程

---

　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　转载请注明出处
　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　欢迎讨论交流