上一篇文章将了如何使用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 frame
double 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 machinery
prev_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 秒。
4.4 运行结果分析
4.4.1 运行时间分析
处理一次plicp需要大概0.5ms的时间，这个时间已经非常短了，能够满足实时的匹配计算．
4.4.2 精度分析
上面的输出是在机器人静止情况下的结果，x, y, theta的值虽然不为0，但是是10的负300次方，这基本上就是0了，也挺好的．
这次的实现没有做位姿的预测，在雷达频率低(10hz)的情况下，最好还是有个位姿的预测，作为plicp的初值，能够提高匹配的精度与速度，将在下次进行实现．
5 Next
本次实现的是帧间匹配，只是最基础的帧间坐标变换的计算．接下来，将添加以下功能来实现激光里程计．
- 位姿预测功能：根据当前的速度，来对下一时刻的位姿进行预测，作为plicp的初值
- 使用 tf2 记录位姿变换，将雷达坐标系的运动转成base_link下的运动，并进行累加
- 将累加的位姿作为里程计发布出去
文章将在 公众号: 从零开始搭SLAM 进行同步更新，欢迎大家关注，以在文章更新的第一时间通知您。
同时，也希望您将这个公众号推荐给您身边做激光SLAM的人们，大家共同进步。
如果您对我写的文章有什么建议，或者想要看哪方面功能如何实现的，请直接在公众号中回复，我可以收到，并将认真考虑您的建议。
评论（1）
您还未登录，请登录后发表或查看评论