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

李太白lx

分类：建图导航

发布时间 2021.09.29阅读数 5083 评论数 0

上一篇文章讲解了如何将激光雷达的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的人们，大家共同进步。

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

在这里插入图片描述

ROS SLAM PCL 二维激光消息类型转换

打赏 0

上一篇：从零开始搭二维激光SLAM --- 使用PCL进行雷达的消息类型转换

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

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

李太白lx

1 ICP算法

2 为什么主流SLAM里不用ICP做帧间匹配

3 代码

3.1 获取代码

3.2 代码解析

3.2.1 头文件

3.2.2 源文件

3.2.2.1 构造函数

3.2.2.2 回调函数

3.2.2.3 ConvertScan2PointCloud

3.2.2.4 ScanMatchWithICP

4 运行

4.1 launch文件

4.2 编译与运行

4.3 运行结果

4.4 运行结果分析

4.4.1 运行时间分析

4.4.2 精度分析

5 Next

为你推荐

机器人定位技术：robot_localization

无人驾驶技术入门（四）| 百度无人车传感器 IMU 深入剖析

ALOAM:激光雷达的运动畸变补偿代码解析

轮式里程计与激光雷达进行标定--线性最小二乘--运动学模型方法（理论+实现代码）

精选通过gazebo对 livox mid360 激光雷达进行仿真

我猜你不懂矩阵分析？？？机器人控制、SLAM先导知识 ——（1.2）线性映射

评论（0）

关于作者

李太白lx

29

0

1

2

LIO-SAM探秘第一章之论文解析

cartographer探秘第四章之代码解析（七）--- pose_extrapolator.h 及 imu_tracker.h

LIO-SAM探秘第三章之代码解析（五） --- imuPreintegration.cpp

相关推荐

经典文献阅读之--Fast-BEV(实时鸟瞰图感知)

hdl_graph_slam源码解读(二)：g2o顶点和边的管理

A星算法优化（三）搜索邻域

深度优先搜索（DFS）和广度优先搜索（BFS）

ORB_SLAM3原理源码解读系列（4）——视觉词袋模型

经典文献阅读之--OverlapNet

热门泡泡

30积分 失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分 想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

给作者打赏

忘记密码

修改头像

添加你感兴趣的标签

举报类型（必选）

举报详情（选填）

30积分失眠，聊聊自己搞ROS的心得体会吧

5积分想买能用ROS2的开发套件。或者开发板