开源3D激光SLAM项目BLAM

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

安装使用教程：

LiDAR-based real-time 3D localization and mapping

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

官方视频：https://youtu.be/08GTGfNneCI

一、安装依赖包

1、安装ROS

sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
sudo apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net:80 --recv-key 0xB01FA116
sudo apt-get update
sudo apt-get install ros-kinetic-desktop-full
source /opt/ros/kinetic/setup.zsh

2、安装 GTSAM

#Boost >= 1.43
sudo apt-get install libboost-all-dev
#CMake >= 2.6
sudo apt-get install cmake

git clone https://bitbucket.org/gtborg/gtsam.git
cd gtsam
mkdir build
cd build
cmake ..
sudo make install

二、安装BLAM

git clone https://github.com/erik-nelson/blam.git

首先确保ROS_PACKAGE_PATH中没有任何其他ROS工作区

cd blam
./update

常见问题

1、fatal error: ros/ros.h: No such file or directory

cd blam/internal/src/geometry_utils

添加以下两行到 package.xml

roscpp
roscpp

添加以下下两行到CMakeList.txt

find_package(catkin REQUIRED COMPONENTS roscpp)
include_directories(include ${catkin_INCLUDE_DIRS})

2、Invalid (NaN, Inf) point coordinates given to nearestKSearch!

my solution is edit the “point_cloud_filter.cc”， add the following code at the end of the Filter function

if (!points->is_dense)
{
points_filtered->is_dense = false;
std::vector indices;
pcl::removeNaNFromPointCloud(points_filtered,points_filtered, indices);
}

3、按照以上方法修改后，在回环后还是出现Invalid (NaN, Inf) point coordinates given to nearestKSearch

修改”BlamSlam.cc”，修改以下几行（这几行不是连在一起的），即把使用原始msg的地方替换为msg_filtered

//1、change loop_closure_.AddKeyScanPair(0, msg); to
loop_closure_.AddKeyScanPair(0, msg_filtered);  

//2、change if (HandleLoopClosures(msg, &new_keyframe)) to
if (HandleLoopClosures(msg_filtered, &new_keyframe))

//3、localization_.TransformPointsToFixedFrame(*msg, msg_fixed.get());
localization_.TransformPointsToFixedFrame(*msg_filtered, msg_fixed.get());

以上nan问题主要是因为所使用的点云数据is_desne:false也就是说，点云里面可能含有nan所以在filter里面

统一做了去除无效值

三、使用

官方使用的是velodyne的数据

rosbag play velodyne.bag#输出话题rslidar_points

source your_path/blam-master/internal/devel/setup.zsh
roslaunch blam_example test_online.launch

使用自己的数据集，修改test_online.launch文件

使用自定义数据集常见问题

1、效果不好

注意查看自己的点云数据量和velodyne的区别，如果点云数量少就修改point_cloud_filter/config/parameters.yaml

#关闭voxel grid filter
grid_filter: false

2、按照1修改了，效果还不好，旋转特别大

修改point_cloud_localization/config/parameters.yaml

  # Maximum acceptable incremental rotation and translation.
  transform_thresholding: true #false
  max_translation: 0.5 #0.05
  max_rotation: 0.3 #0.1

3、不回环

观察你的pose相差多远，修改laser_loop_closure/config/parameters.yaml

#默认0.5米检测一次回环或记录关键帧，值越大效率越高，发现构图缓慢的时候可以放大这个值
translation_threshold: 1.0 #0.5

#默认是在1.5米范围内匹配，根据你自己的回环误差放大这个值
proximity_threshold: 10 #1.5

#ICP "fitness score" must be less than this number，就是越小越难回环
max_tolerable_fitness: 5 #0.15

#根据你自己的地图大小适当放宽跳过回环的点数，一般地图越大值越大
skip_recent_poses: 100 #40
poses_before_reclosing: 100 #40

4、看到位姿回环并修正了，但是点云没有被修正

取消订阅/blam/blam_slam/octree_map_updates再重新订阅就看到了

使用自己的rslidar室外数据集，效果如下(网格大小10m)：

首次构图，还未回环，几乎无偏差

经过一圈之后已经产生了角度的偏差，回环之后依然良好，最终图如下

总结：
效果还不错，首次构图误差小，回环速度够快，调参也方便，关键是不需要其他传感器

---

作者：Cayla梦云
来源：CSDN
原文：https://blog.csdn.net/xmy306538517/article/details/81122663
版权声明：本文为博主原创文章，转载请附上博文链接！

系统概述

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

代码主要分为下列几个大部分

• 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)**函数中完成，它传入一个const PointCloud::ConstPtr& 类型，即点云常指针的引用，程序源代码如下

void BlamSlam::ProcessPointCloudMessage(const PointCloud::ConstPtr& msg) {


  static int T=0;
  ROS_INFO("The times is:%d",T++);  
  // 进行一些基本的过滤  提高后面运算速度  降低数据存储量
  PointCloud::Ptr msg_filtered(new PointCloud);
  filter_.Filter(msg, msg_filtered);


  // 激光里程计 得到粗略的 变换矩阵 ， 第一次执行时候进入if语句初始化地图
  if (!odometry_.UpdateEstimate(*msg_filtered)) {
    // First update ever.
    PointCloud::Ptr unused(new PointCloud);
    mapper_.InsertPoints(msg_filtered, unused.get());
    loop_closure_.AddKeyScanPair(0, msg);
    return;
  }

  PointCloud::Ptr msg_transformed(new PointCloud);
  PointCloud::Ptr msg_neighbors(new PointCloud);
  PointCloud::Ptr msg_base(new PointCloud);
  PointCloud::Ptr msg_fixed(new PointCloud);

  // 将当前帧数据通过前面的变换矩阵 转换到 地图坐标系下
  localization_.MotionUpdate(odometry_.GetIncrementalEstimate());
  localization_.TransformPointsToFixedFrame(*msg_filtered,
                                            msg_transformed.get());

  // 在地图坐标系下得到当前帧数据对应的最近邻点
  mapper_.ApproxNearestNeighbors(*msg_transformed, msg_neighbors.get());

  // 最近邻点转换回传感器的坐标系 与当前帧再进行一次匹配 得到精确的位姿变换矩阵
  localization_.TransformPointsToSensorFrame(*msg_neighbors, msg_neighbors.get());
  localization_.MeasurementUpdate(msg_filtered, msg_neighbors, msg_base.get());

  // 回环检测
  bool new_keyframe;
  if (HandleLoopClosures(msg, &new_keyframe)) {
    T=0;

    // 找到了一个回环，对地图进行调整
    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());

    // 对储存的机器人位姿也重新进行调整
    localization_.SetIntegratedEstimate(loop_closure_.GetLastPose());
  } else {
    if (new_keyframe) { // 不是回环检测，但是是一个**关键帧**添加点云数据到地图中
      localization_.MotionUpdate(gu::Transform3::Identity());
      //当前帧数据使用上述的精确位姿变换矩阵 转换到地图坐标系下 
      localization_.TransformPointsToFixedFrame(*msg, msg_fixed.get()); 
      PointCloud::Ptr unused(new PointCloud);
      loop_closure_.GetMapPoints(msg_fixed.get());
      ROS_INFO("The size of get::%d",msg_fixed->points.size());
      //加入到地图中
      mapper_.InsertPoints(msg_fixed, unused.get());
    }
  }

  // 发布位姿节点，里程计数据等
  loop_closure_.PublishPoseGraph();

  // 发布当前帧点云数据
  if (base_frame_pcld_pub_.getNumSubscribers() != 0) {
    PointCloud base_frame_pcld = *msg;
    base_frame_pcld.header.frame_id = base_frame_id_;
    base_frame_pcld_pub_.publish(base_frame_pcld);
  }
}

程序有一点长，不过相对于其他开源激光SLAM项目还是挺好理解的。下面就一段段的分析

  static int T=0;
  ROS_INFO("The times is:%d",T++);  
  // 进行一些基本的过滤  提高后面运算速度  降低数据存储量
  PointCloud::Ptr msg_filtered(new PointCloud);
  filter_.Filter(msg, msg_filtered);


  // 激光里程计 得到粗略的 变换矩阵 ， 第一次执行时候进入if语句初始化地图
  if (!odometry_.UpdateEstimate(*msg_filtered)) {
    // First update ever.
    PointCloud::Ptr unused(new PointCloud);
    mapper_.InsertPoints(msg_filtered, unused.get());
    loop_closure_.AddKeyScanPair(0, msg);
    return;
  }

filter_.Filter(msg, msg_filtered); 这里就是调用PCL库进行一些基本的数据过滤，其配置在config.yaml文件进行设置，比较见到就不进去看了，不了解的可以去PCL官网教程里去看一下(http://pointclouds.org/documentation/tutorials/)

odometry_.UpdateEstimate(msg_filtered) ;odometry_是 odometry类实例化的一个对象，odometry类定义在point_cloud_odometry这个package下。进入这个函数

bool PointCloudOdometry::UpdateEstimate(const PointCloud& points) {
  // 和pcl到ros的时间戳转换有关 转换成合理的形式
  stamp_.fromNSec(points.header.stamp*1e3);

  // 如果这是第一个消息  储存下来等待下一个消息 
  if (!initialized_) {
    copyPointCloud(points, *query_);
    initialized_ = true;
    return false;
  }

  // Move current query points (acquired last iteration) to reference points.
  copyPointCloud(*query_, *reference_);
  // Set the incoming point cloud as the query point cloud.
  copyPointCloud(points, *query_);

  // 有了两帧数据 执行icp
  return UpdateICP();
}

可以看到这里是保存当前帧和上一帧的数据,通过两帧数据做匹配。下面就进入匹配的函数

bool PointCloudOdometry::UpdateICP() {

  // 计算两帧之间的转换---incremental transform
  GeneralizedIterativeClosestPoint icp;  //这里使用的GICP
  icp.setTransformationEpsilon(params_.icp_tf_epsilon);
  icp.setMaxCorrespondenceDistance(params_.icp_corr_dist);
  icp.setMaximumIterations(params_.icp_iterations);
  icp.setRANSACIterations(0);

  icp.setInputSource(query_);
  icp.setInputTarget(reference_);

  PointCloud unused_result;
  icp.align(unused_result);  //执行转换 但是不需要用到对齐后的点云

  const Eigen::Matrix4f T = icp.getFinalTransformation();  //得到粗略的  位姿变换

  //将Eigen库的Matrix4f 转换到 blam自定义的 位姿变换矩阵 transform3
  incremental_estimate_.translation = gu::Vec3(T(0, 3), T(1, 3), T(2, 3));  //4*4的位姿变换矩阵 设置平移量
  incremental_estimate_.rotation = gu::Rot3(T(0, 0), T(0, 1), T(0, 2),      //得到旋转矩阵  
                                            T(1, 0), T(1, 1), T(1, 2),
                                            T(2, 0), T(2, 1), T(2, 2));

  //如果变换矩阵比较小--说明转换是正确的 如果比较大则舍弃
  if (!transform_thresholding_ ||  
      (incremental_estimate_.translation.Norm() <= max_translation_ &&
       incremental_estimate_.rotation.ToEulerZYX().Norm() <= max_rotation_)) {
    integrated_estimate_ =
        gu::PoseUpdate(integrated_estimate_, incremental_estimate_);  //通过两帧之间的变换矩阵进行累计  得到累计的位姿变换 即当前位置
  } else {
    ROS_WARN(
        "%s: Discarding incremental transformation with norm (t: %lf, r: %lf)",
        name_.c_str(), incremental_estimate_.translation.Norm(),
        incremental_estimate_.rotation.ToEulerZYX().Norm());
  }

  // Convert pose estimates to ROS format and publish.
  PublishPose(incremental_estimate_, incremental_estimate_pub_);   //发布两帧之间的位姿变换
  PublishPose(integrated_estimate_, integrated_estimate_pub_);     //发布累计的位姿变换

  // Publish point clouds for visualization.
  PublishPoints(query_, query_pub_);
  PublishPoints(reference_, reference_pub_);

  // Convert transform between fixed frame and odometry frame.
  geometry_msgs::TransformStamped tf;
  tf.transform = gr::ToRosTransform(integrated_estimate_);         //发布tf变换
  tf.header.stamp = stamp_;
  tf.header.frame_id = fixed_frame_id_;
  tf.child_frame_id = odometry_frame_id_;
  tfbr_.sendTransform(tf);

  return true;
}

这段代码的主要功能就是计算两帧之间的位姿变换，注意这里只是一个粗略的计算，后面还有精确计算。另外就是得到累计的位姿估计。如果对上述代码注释中的GICP , ICP , TF变换 , 位姿矩阵等名词不熟悉的话 需要自行百度去了解一下 再回头来看这段代码。

---

作者：30M
来源：CSDN
原文：https://blog.csdn.net/Adam_996/article/details/82256435
版权声明：本文为博主原创文章，转载请附上博文链接！

---

作者：30M
来源：CSDN
原文：https://blog.csdn.net/Adam_996/article/details/81303505
版权声明：本文为博主原创文章，转载请附上博文链接！