激光雷达点云配准算法

最新推荐文章于 2024-03-30 11:59:37 发布

ego_vehicle

最新推荐文章于 2024-03-30 11:59:37 发布

阅读量1.3k

点赞数 3

文章标签：算法矩阵 c++ 自动驾驶

本文链接：https://blog.csdn.net/qq_53576285/article/details/129679830

版权

最近做了一些关于激光雷达的实验，并了解了一些雷达点云配准的算法在这里给大家分享一下，也算是记录一下学习的过程，留下一点自己的理解。

1.为什么要点云配准

因为雷达采集到的点云信息需要进行数据融合，得到效果更好的点云数据。这里点云信息的不同主要体现在三个方面：

不同的时间
不同视角
不同设备

目前应用最广泛的点云精配准算法是：迭代最近点（ICP）和正态分布变化算法(NDT)

2.点云配准的过程

通过一定的旋转和平移变化将不同坐标系下的两组或者多组点云数据统一到同一坐标系下。

这个过程可以通过旋转矩阵和平移矩阵来完成。

这里面相对复杂一点是旋转矩阵，旋转角度直接作用于点上，在x，y，z轴上的旋转举证可以写成：

这里的选择矩阵是正交矩阵，此矩阵乘以乘以自身的转置等于单位阵。具体原理可以参考：

【线性代数笔记】正交矩阵的性质_seh_sjlj的博客-CSDN博客https://blog.csdn.net/qaqwqaqwq/article/details/122978068

z点云的刚性变化也主要涉及到旋转矩阵以及平移矩阵：

下面的内容会简要介绍一下ICP算法的原理，自己比较菜，可能理解不太到位。

3.点云配准ICP算法

ICP算法的本质是基于最小二乘法的最优配准方法。该算法重复选择对应的关系点对，并计算最优的刚体变化，直到满足正确配准的收敛精度要求。

算法的输入：源点云和目标点云，停止迭代的标准

算法的输出：旋转和平移矩阵，即转换矩阵

在推导ICP算法之前，我们可以对ICP算法的流程有一个清晰的了解：

下面是ICP算法的推导，主要涉及矩阵的一些运算，我数学水平有限，可能讲不太清楚，只需要代码的同学可以直接跳转到第四部分。

1.确定源点云和目标点云（要目前点云配准任务的目的就是将源点云通过坐标的刚性变换转换为目标点云）

2.寻找对应点云

这一步我们需要在源点云找出一簇点，在目标点云中找到距离这一簇点对应的最近的一簇点云。数学表达如下：

3.根据对应点，计算矩阵R（旋转）和向量T（平移）

这一部分是ICP算法推导最重要的一个部分，可以看成是利用矩阵的性质求解一个优化问题。

优化问题我们就需要构造目标函数，经过R和T的变化后，使得两帧点云的相对位置误差尽量小，目标函数我们可以这样构造:

这个函数有两个变量，旋转矩阵R和平移矩阵T。因此我们对于T求偏导，变化过程如下：

将偏导结果带入到之前我们构造的目标函数中：

这时可以用到R是正交矩阵的性质，简化上面的公式为：

这个时候回到我们的目的，是为了求旋转矩阵R，因此在简化目标函数为：

我们把这个式子展开：

问题就变成了求一个矩阵的迹（也就是对角线元素和）。有矩阵迹的定义我们很容易能想到下面这个性质：

但这个迹我们还是不知道怎么求其最大值下的R矩阵，别急有了这个式子我们就能使用矩阵的SVD分解来解决问题。

SVD分解原理可以参考：

【彻底搞懂】矩阵奇异值分解（SVD） - 知乎 (zhihu.com)https://zhuanlan.zhihu.com/p/480389473

总之对于S进行SVD 分解后可以得到这样一个结果：

对于上图的表达式由矩阵迹的性质以及正交矩阵相乘仍然是正交矩阵可以得到：

到这一步我们的问题就要解决了

可以的目标函数表达式为：

其中M是正交矩阵，那么当且仅当M为单位阵的时候能达到目标的最大值：

因此可得：

最后得到如下结果：

V和U在SVD分解中已经被求得，这是因为源点云P和目标点云Q都是知道其坐标的，因此能分解出具体的V和U，这样就能求解出我们需要的旋转矩阵R，再将旋转矩阵带入之前推导的旋转矩阵与平移矩阵关系式可以得到平移矩阵。

4.得到R和T后对点云坐标变化，计算误差函数

5.判断后继续迭代知道满足误差函数要求。

以上是ICP算法大致流程和推导。

4.ICP算法代码

#include <iostream>
#include <Eigen/Dense>

#include <pcl/io/pcd_io.h>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

#include <pcl/filters/filter.h>

#include <pcl/registration/icp.h>

#include <pcl/common/time.h>

#include <pcl/visualization/pcl_visualizer.h>

bool next_iteration = false;

// 此函数是可视化窗口的回调函数。当可视化窗口被激活时，只要按空格键，就会调用此函数，将布尔变量next_iteration 设置为 true
void keyboardEventOccurred (const pcl::visualization::KeyboardEvent& event, void* nothing)
{
    if (event.getKeySym () == "space" && event.keyDown ())
        next_iteration = true;
}

void down_sample(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud_in,
                 const pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud_out)
{
    pcl::ScopeTime time("down sample");

    int down_sample_rate = 8;

    int height = cloud_in->height;
    int width = cloud_in->width;

    int height_ = height / down_sample_rate;
    int width_ = width / down_sample_rate;

    cloud_out->resize(height_ * width_);

    for (int i = 0; i < height; i = i + down_sample_rate)
    {
        for (int j = 0; j < width; j = j + down_sample_rate)
        {
            int index = i * width + j;
            int index_ = i * width / down_sample_rate / down_sample_rate + j / down_sample_rate;

            cloud_out->points[index_] = cloud_in->points[index];
        }
    }

    cloud_out->height = height_;
    cloud_out->width = width_;
}

void remove_nan(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud_1,
                const pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud_2)
{
    // 如果想正确使用 pcl::removeNaNFromPointCloud 函数，最好先把点云的 is_dense 属性设置为 false
    // 因为 pcl::removeNaNFromPointCloud 函数会先检查 is_dense 属性，如果该属性原本为 true 那么该函数将不会对点云进行任何处理
    cloud_1->is_dense = false;
    cloud_2->is_dense = false;

    std::vector<int> indices_1;
    pcl::removeNaNFromPointCloud(*cloud_1, *cloud_1, indices_1);

    std::vector<int> indices_2;
    pcl::removeNaNFromPointCloud(*cloud_2, *cloud_2, indices_2);

    cloud_1->is_dense = true;
    cloud_2->is_dense = true;
}

void icp_function(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud_source,
                  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud_target)
{
    int iterations = 10;

    // ICP 算法
    pcl::IterativeClosestPoint<pcl::PointXYZRGB, pcl::PointXYZRGB> icp;
    icp.setMaximumIterations (iterations);  // 设置要执行的初始迭代次数（默认值为1）
    icp.setInputSource(cloud_source);
    icp.setInputTarget(cloud_target);
    icp.align(*cloud_source);
    icp.setMaximumIterations(1);  // 在第一次对齐之后，将这个变量设置为1，下次调用.align（）函数的时候就只会迭代一次

    if (icp.hasConverged ())
    {
        std::cout << "\nICP has converged, score is " << icp.getFitnessScore () << std::endl;
        std::cout << "\nICP transformation " << iterations << " : cloud_source -> cloud_target" << std::endl;

        Eigen::Matrix4d transformation_matrix = icp.getFinalTransformation ().cast<double>();
        std::cout << transformation_matrix << std::endl;
    }
    else
    {
        std::cerr << "\nICP has not converged." << std::endl;
    }

    // 可视化部分
    pcl::visualization::PCLVisualizer viewer;

    // 目标点云为白色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> white(cloud_target, 250, 250, 250);

    viewer.addPointCloud<pcl::PointXYZRGB> (cloud_target, white, "cloud_target");

    // 原点云为红色
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> red (cloud_source, 250, 0, 0);
    viewer.addPointCloud (cloud_source, red, "cloud_source");

    // 键盘回调函数
    viewer.registerKeyboardCallback (&keyboardEventOccurred, (void*) nullptr);

    // 最终可视化的时候会看到，每按一次空格键，红色的原点云会逐渐逼近白色的目标点云
    while (!viewer.wasStopped())
    {
        viewer.spinOnce ();

        // if you pressed "space"
        if (next_iteration)
        {
            icp.align (*cloud_source);  // 每一次循环，只迭代一次

            if (icp.hasConverged ())
            {
                std::cout << "\nICP has converged, score is " << icp.getFitnessScore () << std::endl;
                std::cout << "\nICP transformation " << ++iterations << " : cloud_source -> cloud_target" << std::endl;

                Eigen::Matrix4d transformation_matrix = icp.getFinalTransformation ().cast<double>();
                std::cout << transformation_matrix << std::endl;

                viewer.updatePointCloud (cloud_source, red, "cloud_source");
            }
            else
            {
                std::cerr << "\nICP has not converged.\n" << std::endl;
            }
        }

        next_iteration = false;
    }
}

int main()
{
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_source (new pcl::PointCloud<pcl::PointXYZRGB>);  // 原点云
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_target (new pcl::PointCloud<pcl::PointXYZRGB>);  // 目标点云
//放自己的点云路径
    std::string name_1 = "../room_scan1.pcd" ;
    pcl::io::loadPCDFile<pcl::PointXYZRGB>(name_1, *cloud_source);

    std::string name_2 = "../room_scan2.pcd" ;
    pcl::io::loadPCDFile<pcl::PointXYZRGB>(name_2, *cloud_target);

    std::cout << "[Cloud 1] Number of Points = " << cloud_source->points.size () << std::endl;
    std::cout << "[Cloud 2] Number of Points = " << cloud_target->points.size () << std::endl;

    // 进行降采样，把原始点云的尺寸从 640 * 480 降采样到 80 * 60
    //down_sample(cloud_source, cloud_source);
    //down_sample(cloud_target, cloud_target);

    //std::cout << "[Cloud 1] Number of Points = " << cloud_source->points.size () << std::endl;
    //std::cout << "[Cloud 2] Number of Points = " << cloud_target->points.size () << std::endl;

    // 点云中有些点是无效的，要将其去除，要不然 ICP 会报错
    remove_nan(cloud_source, cloud_target);

    icp_function(cloud_source, cloud_target);

    return 0;
}

后续我还会介绍一下点云配准NDT算法，主要是基于统计学的一种点云配准融合的方法。