BLAM源码解析（一）—— 模块初始化

接下来写一个开源SLAM算法系列吧，本期介绍BLAM算法。BLAM算法是伯克利的一位大牛写的，首先名字就很有意思，BLAM表示

B(erkeley) L(localization) A(nd) M(apping)！

早在三年前就已经进行了开源，算法使用iSAM2搭建因子图，属于SAM系列弄潮儿之一，常常与LEGO-LOAM、LIO-SAM等算法一同被提起。

BLAM对新手友好，因此决定在这里写几篇博客，对BLAM在线部分进行解析，分享自己的见解，欢迎交流、点赞！

========================================================================

Source Code:

github：https://github.com/erik-nelson/blam

码云：https://gitee.com/meadowair/BLAM

========================================================================

从整个系统的main函数读起，找到其位于src\blam_slam\src\blam_slam.cc中。

blam_slam.cc文件直接进行初始化：

  if (!bs.Initialize(n, false /* online processing */)) {

其中两个参数，n代表ros句柄，false代表不进行日志记录。

让我们跳转到BlamSlam.cc，看一下初始化的作用：

首先在句柄n的命名空间blam_slam中，附加命名空间BlamSlam：

name_ = ros::names::append(n.getNamespace(), "BlamSlam");

这样这个节点所发出的所有话题、订阅者发布者等，在名称前均被冠以“blam_slam/BlamSlam”

过滤器初始化

接下来初始化过滤器filter_：

  if (!filter_.Initialize(n)) {

后面的内容先放放，接下来就看看初始化过滤器做了什么：
跳转至src\point_cloud_filter\src\PointCloudFilter.cc，

首先在命名空间后方继续追加PointCloudFilter:

  name_ = ros::names::append(n.getNamespace(), "PointCloudFilter");

紧接着加载参数：

  if (!LoadParameters(n)) {

下面列出加载的参数，均存在于src\point_cloud_filter\config\parameters.yaml：

bool PointCloudFilter::LoadParameters(const ros::NodeHandle& n) {
  // Load filtering parameters.
  if (!pu::Get("filtering/grid_filter", params_.grid_filter)) return false;
  if (!pu::Get("filtering/grid_res", params_.grid_res)) return false;

  if (!pu::Get("filtering/random_filter", params_.random_filter)) return false;
  if (!pu::Get("filtering/decimate_percentage", params_.decimate_percentage))
    return false;

  if (!pu::Get("filtering/outlier_filter", params_.outlier_filter)) return false;
  if (!pu::Get("filtering/outlier_std", params_.outlier_std)) return false;
  if (!pu::Get("filtering/outlier_knn", params_.outlier_knn)) return false;

  if (!pu::Get("filtering/radius_filter", params_.radius_filter)) return false;
  if (!pu::Get("filtering/radius", params_.radius)) return false;
  if (!pu::Get("filtering/radius_knn", params_.radius_knn)) return false;

  // Cap to [0.0, 1.0].
  params_.decimate_percentage =
      std::min(1.0, std::max(0.0, params_.decimate_percentage));

  return true;
}

过滤器参数有：

* grid_filter：是否使用体素滤波，默认为true；

* grid_res：体素滤波分辨率，默认为0.2，即20cm；

* random_filter：是否使用随机滤波，默认为true；

* decimate_percentage：随机滤波百分比，即滤波后保留多少，默认为0.9，即保留90%；

* outlier_filter：是否使用统计滤波，默认为false，我不建议打开统计滤波，因为较为消耗计算量；

* outlier_std：统计滤波标准差阈值，默认为1；

* outlier_knn：统计滤波聚类阈值，默认为10；

* radius_filter：是否使用半径滤波，默认为false，当点云稀疏时不必打开；

* radius：半径滤波的作用域，默认为0.15，即每个点0.15cm的范围内进行寻找临近点；

* radius_knn：半径内临近点阈值，默认为3，即此点作用域内没有超过3个的其他点，则此点被过滤。

接下来继续过滤器的初始化。创建一个局部的ros句柄来管理回调。

bool PointCloudFilter::RegisterCallbacks(const ros::NodeHandle& n) {
  // Create a local nodehandle to manage callback subscriptions.
  ros::NodeHandle nl(n);

  return true;
}

里程计初始化

让我们回到src\blam_slam\src\BlamSlam.cc，看到接下来是里程计odometry_的初始化：

追加命名空间：

name_ = ros::names::append(n.getNamespace(), "PointCloudOdometry");

加载里程计相关参数，均位于src\blam_example\config\blam_frames.yaml和src\point_cloud_odometry\config\parameters.yaml和：

bool PointCloudOdometry::LoadParameters(const ros::NodeHandle& n) {
  // Load frame ids.
  if (!pu::Get("frame_id/fixed", fixed_frame_id_)) return false;
  if (!pu::Get("frame_id/odometry", odometry_frame_id_)) return false;

  // Load initial position.
  double init_x = 0.0, init_y = 0.0, init_z = 0.0;
  double init_roll = 0.0, init_pitch = 0.0, init_yaw = 0.0;
  if (!pu::Get("init/position/x", init_x)) return false;
  if (!pu::Get("init/position/y", init_y)) return false;
  if (!pu::Get("init/position/z", init_z)) return false;
  if (!pu::Get("init/orientation/roll", init_roll)) return false;
  if (!pu::Get("init/orientation/pitch", init_pitch)) return false;
  if (!pu::Get("init/orientation/yaw", init_yaw)) return false;

  gu::Transform3 init;
  init.translation = gu::Vec3(init_x, init_y, init_z);
  init.rotation = gu::Rot3(init_roll, init_pitch, init_yaw);
  integrated_estimate_ = init;

  // Load algorithm parameters.
  if (!pu::Get("icp/tf_epsilon", params_.icp_tf_epsilon)) return false;
  if (!pu::Get("icp/corr_dist", params_.icp_corr_dist)) return false;
  if (!pu::Get("icp/iterations", params_.icp_iterations)) return false;

  if (!pu::Get("icp/transform_thresholding", transform_thresholding_)) return false;
  if (!pu::Get("icp/max_translation", max_translation_)) return false;
  if (!pu::Get("icp/max_rotation", max_rotation_)) return false;

  return true;
}

里程计参数有：

* frame_id/fixed：全局固定系，默认为world；

* frame_id/odometry：里程计坐标系，也就是机器人系，默认为base；

* init_x，init_y，init_z，init_roll，init_pitch，init_yaw：初始位姿，应用时可均设置为0，以上内容被综合为init.translation和init.rotation，init类型为gu::Transform3，底层也是由Eigen进行搭建的；

* integrated_estimate_：积分估计，这里先被赋予init；

* icp_tf_epsilon：icp迭代的阈值，小于此值icp终止。默认为0.0000000001；

* icp_corr_dist：icp过程中，大于此距离的对应点认为是不可靠点，进行滤除，默认为2m；

* icp_iterations：icp迭代次数，默认为10；

* transform_thresholding_：是否使用icp相对变换阈值，超过阈值则认为此icp结果不被接受，默认为false；

* max_translation_：最大平移量阈值0.09，即9cm；

* max_rotation_：最大旋转量阈值0.1弧度，即大概5.7度。

接下来使用RegisterCallbacks创建局部句柄，值得注意的是之后创建了发布者：

  query_pub_ = nl.advertise<PointCloud>("odometry_query_points", 10, false);
  reference_pub_ =
      nl.advertise<PointCloud>("odometry_reference_points", 10, false);
  incremental_estimate_pub_ = nl.advertise<geometry_msgs::PoseStamped>(
      "odometry_incremental_estimate", 10, false);
  integrated_estimate_pub_ = nl.advertise<geometry_msgs::PoseStamped>(
      "odometry_integrated_estimate", 10, false);

query_pub_、reference_pub_均发布点云类型pcl::PointCloud，具体干什么以后再说，incremental_estimate_pub_和integrated_estimate_pub_均发布位姿数据，前者是强调“递增”，后者强调“积分”，不知道为什么，以后再说。

回环检测初始化

让我们回到src\blam_slam\src\BlamSlam.cc，看到接下来是回环模块loop_closure_的初始化：

追加命名空间：

  name_ = ros::names::append(n.getNamespace(), "LaserLoopClosure");

值得注意的是，回环模块有自己的过滤器，也就是loop_closure_自己的成员变量，但是加载的参数是与外部的过滤器保持一致的：

  if (!filter_.Initialize(n)) {

接下来加载回环模块的参数，与前面的有部分重复，包括坐标系名称、icp的相关参数、初始位姿、位姿噪声，都是加载的同样的参数，就不一一列举了，列举关键参数，主要来自于src\laser_loop_closure\config\parameters.yaml：

* SAM框架参数，relinearize_skip，由于对sam理解不深，因此目前无法做出解释，但是一般设置为1，此处也默认为1；

  if (!pu::Get("relinearize_skip", relinearize_skip)) return false;

* 平移阈值，translation_threshold，即机器人运动了多长的平移距离后，才考虑向因子图中加入新的位姿节点（以下说的位姿，均指位姿节点），默认为0.5m，增大此值可以降低计算量，提高实时性；

  if (!pu::Get("translation_threshold", translation_threshold_)) return false;

* 回环检测的搜索范围，proximity_threshold，即机器人处在当前位姿进行回环的时候，仅找此范围内的旧位姿进行回环检测，增加他会提高计算量，降低实时性，但是可以检测到更广的回环；

  if (!pu::Get("proximity_threshold", proximity_threshold_)) return false;

* 回环检测分数阈值，max_tolerable_fitness，当前位姿与旧位姿进行回环检测icp的时候，低于此阈值才认为回环正确，大于此值则认为并不存在回环。简而言之，此值越小，回环越难；

  if (!pu::Get("max_tolerable_fitness", max_tolerable_fitness_)) return false;

* 回环检测排除范围，skip_recent_poses，即当前位姿与刚过去的几个位姿不要去检测回环，没什么必要，默认为20，即刚过去的20个位姿不要去检测回环；

  if (!pu::Get("skip_recent_poses", skip_recent_poses_)) return false;

* 回环检测关闭区段，poses_before_reclosing，即当完成一次正确的闭环后，多少个位姿之内不要再进行闭环。也就是说，当机器人闭环一次后，至少需要行走poses_before_reclosing*translation_threshold米才能进行新的回环。translation_threshold是位姿节点加入的间隔距离；

  if (!pu::Get("poses_before_reclosing", poses_before_reclosing_)) return false;

* 下面构建SAM结构，为：

  // Create the ISAM2 solver.
  ISAM2Params parameters;
  parameters.relinearizeSkip = relinearize_skip;
  parameters.relinearizeThreshold = relinearize_threshold;
  isam_.reset(new ISAM2(parameters));

  // Set the initial position.
  Vector3 translation(init_x, init_y, init_z);
  Rot3 rotation(Rot3::RzRyRx(init_roll, init_pitch, init_yaw));
  Pose3 pose(rotation, translation);

  // Set the covariance on initial position.
  Vector6 noise;
  noise << sigma_x, sigma_y, sigma_z, sigma_roll, sigma_pitch, sigma_yaw;
  LaserLoopClosure::Diagonal::shared_ptr covariance(
      LaserLoopClosure::Diagonal::Sigmas(noise));

  // Initialize ISAM2.
  NonlinearFactorGraph new_factor;
  Values new_value;
  new_factor.add(MakePriorFactor(pose, covariance));
  new_value.insert(key_, pose);  // 插入初始位姿的因子

  isam_->update(new_factor, new_value);  // 更新因子图
  values_ = isam_->calculateEstimate();  // 得到当前的估计
  key_++; // 因子键向后移动

  // Set the initial odometry.  // 设置初始里程计，gtsam::Pose3类型
  odometry_ = Pose3::identity();

接下来继续回环模块的初始化，RegisterCallbacks

  if (!RegisterCallbacks(n)) {

除了创建局部句柄以外，设定了6个发布者，odometry_edge_pub_、loop_edge_pub_、graph_node_pub_、closure_area_pub_、scan1_pub_、scan2_pub_，均为可视化回环检测相关结果的。

定位模块初始化

回到BlamSlam.cc的

  if (!localization_.Initialize(n)) {

关注定位模块localization_的初始化。即在src\point_cloud_localization\src\PointCloudLocalization.cc中：

还是同样的方式，追加局部命名空间，然后加载参数，关键参数主要位于src\point_cloud_localization\config\parameters.yaml，具体为：

* integrated_estimate_：对积分估计初始化，与里程计模块类似。

* 与icp相关的，之前也都介绍过，但是这是位于不同配置文件的：

  tf_epsilon: 0.0000000001

  corr_dist: 1.0

  iterations: 10

  transform_thresholding: false

  max_translation: 0.05

  max_rotation: 0.1

接下来仍然是RegisterCallbacks，除了创建局部句柄外，定义了5个发布者，分别为发布点云类型的query_pub_、reference_pub_、aligned_pub_，和发布位姿类型的incremental_estimate_pub_、integrated_estimate_pub_。

建图模块初始化

仍然是回到BlamSlam.cc，接着向下走

  if (!mapper_.Initialize(n)) {

加载的参数就一个位于src\point_cloud_mapper\config\parameters.yaml中

* octree_resolution：八叉树地图分辨率，默认为0.05 （这个八叉树是不是可以考虑到为规划提供地图？？？？）

紧接着直接建立八叉树地图：

  map_octree_.reset(new Octree(octree_resolution_));
  map_octree_->setInputCloud(map_data_);

接下来仍然是不出意外的RegisterCallbacks，除了建立局部句柄以外，建立两个发布者，发布当前地图和累积的地图，类型都是点云类型PointCloud。

总参数加载

仍然是回到BlamSlam.cc，接着向下走

  if (!LoadParameters(n)) {

附上代码

bool BlamSlam::LoadParameters(const ros::NodeHandle& n) {
  // Load update rates.
  if (!pu::Get("rate/estimate", estimate_update_rate_)) return false;
  if (!pu::Get("rate/visualization", visualization_update_rate_)) return false;

  // Load frame ids.
  if (!pu::Get("frame_id/fixed", fixed_frame_id_)) return false;
  if (!pu::Get("frame_id/base", base_frame_id_)) return false;

  return true;
}

值得注意的是，除了位于src\blam_example\config\blam_frames.yaml的坐标系配置外，加载了位于src\blam_example\config\blam_rates.yaml文件中的帧率信息，即：

* estimate：位姿更新频率；

* visualization：可视化信息更新频率，当可视化消耗太多资源时，可降低数值。

总回调注册

生成局部句柄外，根据from_log参数，也就是一开始在  if (!bs.Initialize(n, false /* online processing */)) {的时候加载的参数，默认为false，根据他决定是进入RegisterLogCallbacks还是RegisterOnlineCallbacks，默认进入RegisterOnlineCallbacks，因此我们重点关注RegisterOnlineCallbacks。

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);
}

estimate_update_timer_是一个定时器，由createTimer函数进行创建，定时进行回调，回调的频率由第一个参数设置，回调的函数为第二个参数，第三个参数表示将this传到回调函数中。

pcld_sub_为订阅者，订阅激光数据，因此可以看作程序的入口，下一节我们将从这里入手进行介绍，也就是回调函数&BlamSlam::PointCloudCallback。

CreatePublishers创建了发布者，base_frame_pcld_pub_，即发布当前帧点云，rviz显示的那些好看的、车辆周围的点云就是他发布的。

下一节从激光雷达回调函数入手，看看激光数据是如何激活整个SLAM的。