上一篇文章讲解了如何将激光雷达的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::PointXYZtypedef 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_; // 声明一个Subscriberbool 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_ptrPointCloudT::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的人们，大家共同进步。

如果您对我写的文章有什么建议，或者想要看哪方面功能如何实现的，请直接在公众号中回复，我可以收到，并将认真考虑您的建议。

从零开始搭二维激光SLAM --- 基于ICP的帧间匹配相关推荐

从零开始搭二维激光SLAM --- 基于GMapping的栅格地图的构建
上篇文章讲解了如何在ROS中发布栅格地图,以及如何向栅格地图赋值．这篇文章来讲讲如何将激光雷达的数据构建成栅格地图．雷达的数据点所在位置表示为占用,从雷达开始到这点之间的区域表示为空闲． 1 GM ...
从零开始搭二维激光SLAM --- 激光雷达数据效果对比
我们知道,不同品牌的激光雷达产生的数据是不一样的,那这些不同点是如何影响建图效果的呢? 这篇文章就是来分析这个问题,将从不同光强下的点云效果,不同夹角下的点云效果,以及 1 激光雷达的技术指标激光雷 ...
从零开始搭二维激光SLAM --- Hector论文公式推导与相关代码解析
这篇文章将带领大家推导一下hector slam论文中的公式．之后再对这部分公式对应的代码进行讲解下． markdown打公式太费劲了,所以我用手写了．(懒) 然后csdn又限制了图片文件大小,我是照 ...
二维激光SLAM( 使用Laser Scan Matcher )
目录一.Laser Scan Matcher安装配置二.二维激光定位一.Laser Scan Matcher安装配置 ROS自带的laser_scan_matcher库,使用的是CSM(Cano ...
使用二维激光雷达和cartographer_ros实现实时SLAM
在前面已经完成了cartographer_ros的安装和demo的运行了.接下来,就要放到机器人上,实时进行SLAM了. 前一篇内容的链接如下: Cartographer_ros的下载.配置及编译与问 ...
二维码识别基于stm32 ov7725
二维码识别基于stm32 ov7725 二维码识别技术在我们的生活中应用的很方便了,但基本都是基于手机的,目前在公交车上也存在了扫描二维码的机器,上次坏了之后,发现一只小企鹅,原来是基于linux的 ...
室内移动机器人二维激光数据线特征提取算法的总结与开源算法分享
本文章总结并翻译于 A comparison of line extraction algorithms using 2D rangedata for indoor mobile robotics 除 ...
一起做激光SLAM：ICP匹配用于闭环检测
编辑丨古月居目标利用ICP进行闭环检测,完成闭环. 预期效果:通过闭环检测完成起止闭环,下图为加入闭环前后. rosbag数据: https://pan.baidu.com/s/1o-noUxgV ...
时间可以是二维的？基于二维时间图的视频内容片段检测 | AAAI 2020
作者 | 彭厚文.傅建龙来源 | 微软研究院AI头条(ID: MSRAsia) 编者按:当时间从一维走向二维,时序信息处理问题中一种全新的建模思路由此产生.根据这种新思路及其产生的二维时间图概念,微 ...

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