开源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 cmakegit 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

<build_depend>roscpp</build_depend>
<run_depend>roscpp</run_depend>

添加以下下两行到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_pointssource your_path/blam-master/internal/devel/setup.zsh
roslaunch blam_example test_online.launch

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

<remap from="~pcld" to="/rslidar_points"/> <!--/rslidar_points替换成自己的话题名-->

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

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 #falsemax_translation: 0.5 #0.05max_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 <ros/ros.h>
#include <blam_slam/BlamSlam.h>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 所以进入onlinereturn RegisterLogCallbacks(n);elsereturn 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)) {   //找到下一个点云数据的indexswitch(type) {// Point cloud messages.case MeasurementSynchronizer::PCL_POINTCLOUD: {　　　　//根据类型进入这个分支const MeasurementSynchronizer::Message<PointCloud>::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_);// 有了两帧数据 执行icpreturn UpdateICP();
}

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

bool PointCloudOdometry::UpdateICP() {// 计算两帧之间的转换---incremental transformGeneralizedIterativeClosestPoint<PointXYZ, PointXYZ> icp;  //这里使用的GICPicp.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自定义的 位姿变换矩阵 transform3incremental_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
版权声明：本文为博主原创文章，转载请附上博文链接！