上一篇文章讲解了如何将激光雷达的sensor_msgs/LaserScan格式转换成pcl::PointCloud< pcl::PointXYZ>格式, 本篇文章将要讲解如何使用这个格式调用ICP算法进行相邻2帧雷达数据间坐标变换的计算.
1 ICP算法
迭代最近点(Iterative Closest Point, 下简称ICP)算法是一种点云匹配算法。
其求解思路为:
- 首先对于一幅点云中的每个点,在另一幅点云中计算匹配点(最近点)
- 极小化匹配点间的匹配误差,计算位姿
- 然后将计算的位姿作用于点云
- 再重新计算匹配点
- 如此迭代,直到迭代次数达到阈值,或者极小化函数的变化量小于设定阈值
ICP算法思路很简单,这里不再进行过多介绍,不了解这个算法的读者可以自行百度一下.
2 为什么主流SLAM里不用ICP做帧间匹配
ICP算法的思路很简单,但是为什么主流SLAM里不用ICP做帧间匹配呢?
ICP算法有一些不足:
首先,ICP对初值比较敏感,初值给的不好,就需要花费更多的迭代次数进行匹配.
其次,由于它是迭代很多次的,所以其花费的时间很长,这一点是非常致命的,之后我会通过程序来让大家体验ICP的费时.
再次,精度与速度是矛盾的,ICP算法理论上可以实现很高的精度,但是要很多很多的迭代次数,以及很长的时间.所以,当限制了迭代次数的情况下,精度就不一定能保证了.
接下来,我将通过代码带着大家感受一下ICP的不足,感受其固定迭代次数时的精度差,以及耗时长.
3 代码
3.1 获取代码
代码已经提交在github上了,如果不知道github的地址的朋友, 请在我的公众号: 从零开始搭激光SLAM 中回复 开源地址 获得。
推荐使用 git clone 的方式进行下载, 因为代码是正处于更新状态的, git clone 下载的代码可以使用 git pull 很方便地进行更新.
3.2 代码解析
3.2.1 头文件
对应的文件目录为
Creating-2D-laser-slam-from-scratch/lesson2/include/lesson2/scan_match_icp.h
头文件中首先include了ros,pcl的头文件.
之后声明了2个指针,代表着当前雷达数据以及上一帧的雷达数据,这里说的雷达数据指的是转成pcl的点云格式的数据.
之后,声明了2个方法,分别用于对雷达数据进行转换,以及调用ICP.
#ifndef LESSON2_SCAN_MATCH_ICP
#define LESSON2_SCAN_MATCH_ICP
// ros
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
// pcl_ros
#include <pcl_ros/point_cloud.h>
// pcl
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
class ScanMatchICP
{
// 使用PCL中点的数据结构 pcl::PointXYZ
typedef pcl::PointXYZ PointT;
// 使用PCL中点云的数据结构 pcl::PointCloud<pcl::PointXYZ>
typedef pcl::PointCloud<PointT> PointCloudT;
private:
ros::NodeHandle node_handle_; // ros中的句柄
ros::NodeHandle private_node_; // ros中的私有句柄
ros::Subscriber laser_scan_subscriber_; // 声明一个Subscriber
bool is_first_scan_; // 判断是否是第一个雷达数据
pcl::PointCloud<pcl::PointXYZ>::Ptr current_pointcloud_; // 当前帧雷达数据
pcl::PointCloud<pcl::PointXYZ>::Ptr last_pointcloud_; // 前一帧雷达数据
// icp算法
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp_;
void ScanMatchWithICP(const sensor_msgs::LaserScan::ConstPtr &scan_msg);
void ConvertScan2PointCloud(const sensor_msgs::LaserScan::ConstPtr &scan_msg);
public:
ScanMatchICP();
~ScanMatchICP();
void ScanCallback(const sensor_msgs::LaserScan::ConstPtr &scan_msg);
};
#endif // LESSON2_SCAN_MATCH_ICP
3.2.2 源文件
对应的文件目录为
Creating-2D-laser-slam-from-scratch/lesson2/src/scan_match_icp.cc
通过各个函数来进行讲解:
3.2.2.1 构造函数
/*
* 构造函数
*/
ScanMatchICP::ScanMatchICP()
{
// \033[1;32m,\033[0m 终端显示成绿色
ROS_INFO_STREAM("\033[1;32m----> Scan Match with ICP started.\033[0m");
laser_scan_subscriber_ = node_handle_.subscribe(
"laser_scan", 1, &ScanMatchICP::ScanCallback, this);
// 第一帧数据的标志
is_first_scan_ = true;
// 指针的初始化
current_pointcloud_ = boost::shared_ptr<PointCloudT>(new PointCloudT());
last_pointcloud_ = boost::shared_ptr<PointCloudT>(new PointCloudT());
}
is_first_scan_是第一帧雷达数据到来的标志,因为第一帧数据到来时,只有一帧数据,是没办法进行匹配的,所以要对第一帧数据进行特殊处理.
current_pointcloud_ 与 last_pointcloud_ 分别保存的是当前帧雷达数据转成pcl点云格式后的数据,以及上一帧雷达数据转成pcl点云格式后的数据.
这里是对这两个智能指针进行初始化,他们的类型为 boost::shared_ptr .
3.2.2.2 回调函数
/*
* 回调函数 进行数据处理
*/
void ScanMatchICP::ScanCallback(const sensor_msgs::LaserScan::ConstPtr &scan_msg)
{
// step1 进行数据类型转换
std::chrono::steady_clock::time_point start_time = std::chrono::steady_clock::now();
// 对第一帧数据进行特殊处理
if (is_first_scan_ == true)
{
// 进行第一帧数据的处理,只转换数据类型 并 保存到current_pointcloud_
ConvertScan2PointCloud(scan_msg);
is_first_scan_ = false;
return;
}
else
// 在将新一帧数据转换到当前帧之前,
// 先将current_pointcloud_赋值到last_pointcloud_进行保存
*last_pointcloud_ = *current_pointcloud_;
// 进行数据类型转换
ConvertScan2PointCloud(scan_msg);
std::chrono::steady_clock::time_point end_time = std::chrono::steady_clock::now();
std::chrono::duration<double> time_used = std::chrono::duration_cast<std::chrono::duration<double>>(end_time - start_time);
std::cout << "\n转换数据格式用时: " << time_used.count() << " 秒。" << std::endl;
// step2 使用ICP计算 雷达前后两帧间的坐标变换
start_time = std::chrono::steady_clock::now();
// 调用ICP进行计算
ScanMatchWithICP(scan_msg);
end_time = std::chrono::steady_clock::now();
time_used = std::chrono::duration_cast<std::chrono::duration<double>>(end_time - start_time);
std::cout << "ICP计算用时: " << time_used.count() << " 秒。" << std::endl;
}
下面这段代码主要是对第一帧数据进行特殊处理,因为这时只有一帧数据没法进行匹配,所以第一帧数据的处理就只做保存的操作,不进行匹配.
// 对第一帧数据进行特殊处理
if (is_first_scan_ == true)
{
// 进行第一帧数据的处理,只转换数据类型 并 保存到current_pointcloud_
ConvertScan2PointCloud(scan_msg);
is_first_scan_ = false;
return;
}
else
// 在将新一帧数据转换到当前帧之前,
// 先将current_pointcloud_赋值到last_pointcloud_进行保存
*last_pointcloud_ = *current_pointcloud_;
同时,我们通过std::chrono库分别计算了数据格式转换需要的时间,以及进行ICP的时间,并打印出来.
之后就是调用数据格式函数的函数以及进行ICP计算的函数.
3.2.2.3 ConvertScan2PointCloud
/*
* 将LaserScan消息类型转换为PCL的pcl::PointCloud类型
*/
void ScanMatchICP::ConvertScan2PointCloud(const sensor_msgs::LaserScan::ConstPtr &scan_msg)
{
// PointCloudT::Ptr的数据类型为boost::shared_ptr
PointCloudT::Ptr cloud_msg = boost::shared_ptr<PointCloudT>(new PointCloudT());
// 对容器进行初始化
cloud_msg->points.resize(scan_msg->ranges.size());
for (unsigned int i = 0; i < scan_msg->ranges.size(); ++i)
{
// 首先声明一个 cloud_msg第i个点的 引用
pcl::PointXYZ &point_tmp = cloud_msg->points[i];
// 获取scan的第i个点的距离值
float range = scan_msg->ranges[i];
// 将 inf 与 nan 点 设置为无效点
if (!std::isfinite(range))
continue;
// 有些雷达驱动会将无效点设置成 range_max+1
// 所以要根据雷达的range_min与range_max进行有效值的判断
if (range > scan_msg->range_min && range < scan_msg->range_max)
{
// 获取第i个点对应的角度
float angle = scan_msg->angle_min + i * scan_msg->angle_increment;
// 获取第i个点在笛卡尔坐标系下的坐标
point_tmp.x = range * cos(angle);
point_tmp.y = range * sin(angle);
point_tmp.z = 0.0;
}
}
// 高度为1的情况下, width即为所有点的个数
cloud_msg->width = scan_msg->ranges.size();
cloud_msg->height = 1;
cloud_msg->is_dense = true; // not contains nans
// 将scan_msg的消息头 赋值到 pcl::PointCloud<pcl::PointXYZ>的消息头
pcl_conversions::toPCL(scan_msg->header, cloud_msg->header);
// 将转换完的数据赋值到current_pointcloud_中保存下来
*current_pointcloud_ = *cloud_msg;
}
代码与上一篇文章基本一样,就不再多说了.有两块需要说明的点为:
这段代码去掉了 对无效点赋值为nan的操作,这是因为ICP算法里不能有nan.
以及
// 将转换完的数据赋值到current_pointcloud_中保存下来
*current_pointcloud_ = *cloud_msg;
这行代码的目的是为了将临时变量整体拷贝下来,放到全局变量current_pointcloud_里.
3.2.2.4 ScanMatchWithICP
/*
* 调用ICP进行帧间位姿的计算
*/
void ScanMatchICP::ScanMatchWithICP(const sensor_msgs::LaserScan::ConstPtr &scan_msg)
{
// ICP 输入数据,输出数据的设置,还可以进行参数配置,这里使用默认参宿
icp_.setInputSource(last_pointcloud_);
icp_.setInputTarget(current_pointcloud_);
// 开始迭代计算
pcl::PointCloud<pcl::PointXYZ> unused_result;
icp_.align(unused_result);
// std::cout << "has converged:" << icp_.hasConverged() << " score: " << icp_.getFitnessScore() << std::endl;
// 如果迭代没有收敛,不进行输出
if (icp_.hasConverged() == false)
{
std::cout << "not Converged" << std::endl;
return;
}
else
{
// 收敛了之后, 获取坐标变换
Eigen::Affine3f transfrom;
transfrom = icp_.getFinalTransformation();
// 将Eigen::Affine3f转换成x, y, theta, 并打印出来
float x, y, z, roll, pitch, yaw;
pcl::getTranslationAndEulerAngles(transfrom, x, y, z, roll, pitch, yaw);
std::cout << "transfrom: (" << x << ", " << y << ", " << yaw * 180 / M_PI << ")" << std::endl;
}
}
这段代码就是调用ICP的过程,首先进行ICP的数据输入,将前后两帧数据传入ICP.
// ICP 输入数据,输出数据的设置,还可以进行参数配置,这里使用默认参数
icp_.setInputSource(last_pointcloud_);
icp_.setInputTarget(current_pointcloud_);
注意:
- 我这里没有对ICP进行额外的参数配置,因为只是体验一下其过程,不需要精确调参.
- 输入数据分别是setInputSource, 以及setInputTarget,我这里没有考虑计算出来的坐标变换 是从source指向target的的,还是从target指向source的.
// 开始迭代计算
pcl::PointCloud<pcl::PointXYZ> unused_result;
icp_.align(unused_result);
上面这段代码就是进行ICP的求解
// 收敛了之后, 获取坐标变换
Eigen::Affine3f transfrom;
transfrom = icp_.getFinalTransformation();
// 将Eigen::Affine3f转换成x, y, theta, 并打印出来
float x, y, z, roll, pitch, yaw;
pcl::getTranslationAndEulerAngles(transfrom, x, y, z, roll, pitch, yaw);
std::cout << "transfrom: (" << x << ", " << y << ", " << yaw * 180 / M_PI << ")" << std::endl;
上面这段代码是获取ICP计算的结果,并转换成(x,y,theta)的形式,打印出来.
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="lesson2_scan_match_icp_node"
pkg="lesson2" type="lesson2_scan_match_icp_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 lesson2 scan_match_icp.launch
4.3 运行结果
运行之后,rviz中会产生如下图像,可见,最开始的时候雷达是不动的,处于静止状态.
之后,切换到运行roslaunch的终端,会打印处如下内容
转换数据格式用时: 0.000179439 秒。
transfrom: (-0.000889364, -0.0002404, -0.0694536)
ICP计算用时: 0.116704 秒。
转换数据格式用时: 0.000179154 秒。
transfrom: (0.00286111, 0.00962993, 0.202655)
ICP计算用时: 0.124019 秒。
4.4 运行结果分析
通过终端打印出来的内容,可知,
-
转换一帧雷达数据的用时为0.00017秒,也就是0.2ms左右
-
而计算一次ICP所用的时间竟然高达0.12秒.
-
最初时刻,雷达是处于静止状态的,而ICP输出的位姿竟然有3mm的x方向的平移.
4.4.1 运行时间分析
0.12秒是一个很可怕的时长,我的雷达比较便宜,一圈大概1440个点,有效点大概700多个,处理这么少的点竟然还需要花费这么久的时间.(虽然用的是默认参数)
通过 rostopic hz /laser_scan 命令可以查看雷达的频率
雷达的频率大概为10hz,也就是0.1秒返回一帧数据,而0.12秒才进行完一次ICP,这意味着,我们在下一帧数据到来之前是计算不完的,也就是说算出来的位置始终是延迟的.
而且,由于计算不完,就会导致雷达数据被丢弃掉.
4.4.2 精度分析
在初始时刻,雷达是处于静止状态的,而由于ICP算法输出的值是(0.00286111, 0.00962993, 0.202655).在xy和转角上都不为0,这是完全不可接受的.
虽然我没有进行其他参数的设置,但是在0.12秒的时间下返回的精度还是这么差,我想大家都明白为啥没人用ICP进行帧间匹配了.
5 Next
本篇文章是一个试错的过程,只要知道这个算法的效果达不到帧间匹配的要求即可.
下篇文章,我将通过PL-ICP(一种ICP的改进算法)来进行帧间匹配的计算.
我将参考http://wiki.ros.org/laser_scan_matcher?distro=kinetic 进行代码的实现,感兴趣的同学可以提前看一下这个包的代码.
scan_tools是一个很好的处理雷达数据的工具包,里边有很多处理雷达数据的小功能,也会在下篇文章中进行说明.
文章将在 公众号: 从零开始搭SLAM 进行同步更新,欢迎大家关注,以在文章更新的第一时间通知您。
同时,也希望您将这个公众号推荐给您身边做激光SLAM的人们,大家共同进步。
如果您对我写的文章有什么建议,或者想要看哪方面功能如何实现的,请直接在公众号中回复,我可以收到,并将认真考虑您的建议。