从零开始搭二维激光SLAM --- 基于PL-ICP的帧间匹配

本文主要是介绍从零开始搭二维激光SLAM --- 基于PL-ICP的帧间匹配，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

上一篇文章将了如何使用PCL中的ICP算法进行相邻帧间的坐标变换，ICP的计算时间以及精度都不太好．

这篇文章我使用一种ICP的改进算法PL-ICP算法来计算相邻帧间的坐标变换．

1 PL-ICP

PL-ICP(Point to Line ICP) 使用点到线距离最小的方式进行ICP的计算，收敛速度快很多，同时精度也更高一些．

具体的pl-icp的介绍请看其论文，作者也开源了pl-icp的代码，作者将实现pl-icp的代码命名为csm( Canonical Scan Matcher).

论文与代码的详情请看作者的网站https://censi.science/software/csm/.

看看情况以后再来对这个算法进行一下公式推倒．

2 scan_tools

ros中有人使用使用csm包进行了ros下的实现，进行了扫描匹配与位姿累加的实现，但是没有发布odometry的topic与tf．包的名字为 laser_scan_matcher，是scan_tools包集中的一个．

首先介绍一下scan_tools包集，这个包里提供了很多操作二维激光雷达数据的功能，虽然不一定能直接用，但是其处理数据的思路是非常值得借鉴的．

2.1 wiki地址

其wiki的地址为: http://wiki.ros.org/scan_tools?distro=kinetic

2.2 功能简介

其包含的功能包名字如下所示，并对其功能进行了简要介绍．

laser_ortho_projector: 将切斜的雷达数据投影到平面上．
laser_scan_matcher: 基于pl-icp的扫描匹配的实现，并进行了位姿累加
laser_scan_sparsifier: 对雷达数据进行稀疏处理
laser_scan_splitter: 将一帧雷达数据分段，并发布出去
ncd_parser: 读取New College Dataset，转换成ros的scan 与 odometry 发布出去
polar_scan_matcher: 基于Polar Scan Matcher的扫描匹配器的ros实现
scan_to_cloud_converter: 将 sensor_msgs/LaserScan 数据转成 sensor_msgs/PointCloud2 的数据格式．

我之前的那篇将雷达数据转成 sensor_msgs/PointCloud2 的文章，就是参考scan_to_cloud_converter包来实现的．

如果想要深入学习一下的请去wiki的网址，看介绍及源码，这里就不再过多介绍了．

3 代码

3.1 获取代码

代码已经提交在github上了，如果不知道github的地址的朋友, 请在我的公众号: 从零开始搭激光SLAM 中回复 开源地址 获得。

推荐使用 git clone 的方式进行下载, 因为代码是正处于更新状态的, git clone 下载的代码可以使用 git pull 很方便地进行更新.

本篇文章对应的代码为lesson3

3.2 依赖

由于这次的包依赖了csm这个包，所以要先安装一下csm

sudo apt-get install ros-kinetic-csm

3.3 CMakeLists.txt

需要在CMakeLists.txt中添加如下内容，以在程序中使用csm

# Find csm project
find_package(PkgConfig)
pkg_check_modules(csm REQUIRED csm)
include_directories(include${catkin_INCLUDE_DIRS}${csm_INCLUDE_DIRS} 
)
link_directories(${csm_LIBRARY_DIRS})#Create library
add_executable(${PROJECT_NAME}_scan_match_plicp_node src/scan_match_plicp.cc)#Note we don't link against pcl as we're using header-only parts of the library
target_link_libraries( ${PROJECT_NAME}_scan_match_plicp_node 
${catkin_LIBRARIES} 
${csm_LIBRARIES}
)add_dependencies(${PROJECT_NAME}_scan_match_plicp_node 
${csm_EXPORTED_TARGETS} 
${catkin_EXPORTED_TARGETS}
)

3.4 代码解析

3.4.1 回调函数

首先要进行初始化，初始化的工作做了第一帧雷达数据的赋值，第一帧时间的赋值．

并且计算了雷达数据对应的每个角度值的cos与sin，用于节省计算量．

之后再将雷达数据转换成csm需要的格式.

再调用ScanMatchWithPLICP进行匹配计算．

/** 回调函数 进行数据处理*/
void ScanMatchPLICP::ScanCallback(const sensor_msgs::LaserScan::ConstPtr &scan_msg)
{// 如果是第一帧数据，首先进行初始化if (!initialized_){// 将雷达各个角度的sin与cos值保存下来，以节约计算量CreateCache(scan_msg);// 将 prev_ldp_scan_,last_icp_time_ 初始化LaserScanToLDP(scan_msg, prev_ldp_scan_);last_icp_time_ = scan_msg->header.stamp;initialized_ = true;}// step1 进行数据类型转换start_time_ = std::chrono::steady_clock::now();LDP curr_ldp_scan;LaserScanToLDP(scan_msg, curr_ldp_scan);end_time_ = std::chrono::steady_clock::now();time_used_ = std::chrono::duration_cast<std::chrono::duration<double>>(end_time_ - start_time_);std::cout << "\n转换数据格式用时: " << time_used_.count() << " 秒。" << std::endl;// step2 使用PLICP计算雷达前后两帧间的坐标变换start_time_ = std::chrono::steady_clock::now();ScanMatchWithPLICP(curr_ldp_scan, scan_msg->header.stamp);end_time_ = std::chrono::steady_clock::now();time_used_ = std::chrono::duration_cast<std::chrono::duration<double>>(end_time_ - start_time_);std::cout << "PLICP计算用时: " << time_used_.count() << " 秒。" << std::endl;
}

3.4.2 计算sin, cos值

由于雷达数据的每个点的角度值是固定的，所以对应的cos与sin值都是一样的．可以提前计算出来，以节省计算量．(本篇文章并没有用到提前计算好的cos值…)

/*** 雷达数据间的角度是固定的，因此可以将对应角度的cos与sin值缓存下来，不用每次都计算*/
void ScanMatchPLICP::CreateCache(const sensor_msgs::LaserScan::ConstPtr &scan_msg)
{a_cos_.clear();a_sin_.clear();double angle;for (unsigned int i = 0; i < scan_msg->ranges.size(); i++){angle = scan_msg->angle_min + i * scan_msg->angle_increment;a_cos_.push_back(cos(angle));a_sin_.push_back(sin(angle));}// min_reading 与 max_reading也不会变了，可以直接赋值进去input_.min_reading = scan_msg->range_min;input_.max_reading = scan_msg->range_max;
}

3.4.3 数据格式转换函数

要将ros的雷达数据格式，转换成csm所需要的数据格式．操作和之前的基本都一样，不再进行详细说明了．

/*** 将雷达的数据格式转成 csm 需要的格式*/
void ScanMatchPLICP::LaserScanToLDP(const sensor_msgs::LaserScan::ConstPtr &scan_msg, LDP &ldp)
{unsigned int n = scan_msg->ranges.size();// 调用csm里的函数进行申请空间ldp = ld_alloc_new(n);for (unsigned int i = 0; i < n; i++){// calculate position in laser framedouble r = scan_msg->ranges[i];if (r > scan_msg->range_min && r < scan_msg->range_max){// 填充雷达数据ldp->valid[i] = 1;ldp->readings[i] = r;}else{ldp->valid[i] = 0;ldp->readings[i] = -1; // for invalid range}ldp->theta[i] = scan_msg->angle_min + i * scan_msg->angle_increment;ldp->cluster[i] = -1;}ldp->min_theta = ldp->theta[0];ldp->max_theta = ldp->theta[n - 1];ldp->odometry[0] = 0.0;ldp->odometry[1] = 0.0;ldp->odometry[2] = 0.0;ldp->true_pose[0] = 0.0;ldp->true_pose[1] = 0.0;ldp->true_pose[2] = 0.0;
}

3.4.3 进行plicp匹配

调用sm_icp函数进行帧间位姿的计算．

input_.first_guess是提供plicp匹配的初值，可以通过速度估计出来，也可以通过里程计或者imu来估计出来，这里的代码直接给0了，就是不提供先验，所以在机器人动起来的情况下匹配的效果可能不好．会在下篇文章中进行实现．

/*** 使用PLICP进行帧间位姿的计算*/
void ScanMatchPLICP::ScanMatchWithPLICP(LDP &curr_ldp_scan, const ros::Time &time)
{// CSM is used in the following way:// The scans are always in the laser frame// The reference scan (prevLDPcan_) has a pose of [0, 0, 0]// The new scan (currLDPScan) has a pose equal to the movement// of the laser in the laser frame since the last scan// The computed correction is then propagated using the tf machineryprev_ldp_scan_->odometry[0] = 0.0;prev_ldp_scan_->odometry[1] = 0.0;prev_ldp_scan_->odometry[2] = 0.0;prev_ldp_scan_->estimate[0] = 0.0;prev_ldp_scan_->estimate[1] = 0.0;prev_ldp_scan_->estimate[2] = 0.0;prev_ldp_scan_->true_pose[0] = 0.0;prev_ldp_scan_->true_pose[1] = 0.0;prev_ldp_scan_->true_pose[2] = 0.0;input_.laser_ref = prev_ldp_scan_;input_.laser_sens = curr_ldp_scan;// 位姿的预测值为0，就是不进行预测input_.first_guess[0] = 0;input_.first_guess[1] = 0;input_.first_guess[2] = 0;// 调用csm里的函数进行plicp计算帧间的匹配，输出结果保存在output里sm_icp(&input_, &output_);if (output_.valid){std::cout << "transfrom: (" << output_.x[0] << ", " << output_.x[1] << ", " << output_.x[2] * 180 / M_PI << ")" << std::endl;}else{std::cout << "not Converged" << std::endl;}// 删除prev_ldp_scan_，用curr_ldp_scan进行替代ld_free(prev_ldp_scan_);prev_ldp_scan_ = curr_ldp_scan;last_icp_time_ = time;
}

4 运行

4.1 launch文件

本篇文章对应的数据包, 请在我的公众号中回复 lesson1 获得，并将launch中的bag_filename更改成您实际的目录名。

<launch><!-- bag的地址与名称 --><arg name="bag_filename" default="/home/lx/bagfiles/lesson1.bag"/><!-- 使用bag的时间戳 --><param name="use_sim_time" value="true" /><!-- 启动节点 --><node name="lesson3_scan_match_plicp_node"pkg="lesson3" type="lesson3_scan_match_plicp_node" output="screen" /><!-- launch rviz --><!-- <node name="rviz" pkg="rviz" type="rviz" required="false"args="-d $(find lesson2)/launch/scan_match_icp.rviz" /> --><!-- play bagfile --><node name="playbag" pkg="rosbag" type="play"args="--clock $(arg bag_filename)" /></launch>

4.2 编译与运行

下载代码后，请放入您自己的工作空间中，通过 catkin_make 进行编译．

由于是新增的包，所以需要通过 rospack profile 命令让ros找到这个新的包．

之后, 使用source命令，添加ros与工作空间的地址到当前终端下．

roslaunch lesson3 scan_match_plicp.launch

4.3 运行结果

运行之后，终端中会显示如下内容

转换数据格式用时: 0.00018146 秒。
transfrom: (6.94938e-310, 1.20452e-316, 9.05622e-321)
PLICP计算用时: 0.000325277 秒。转换数据格式用时: 0.000162974 秒。
transfrom: (6.94938e-310, 1.20452e-316, 9.05622e-321)
PLICP计算用时: 0.00057314 秒。