PCL点云配准：ICP使用

目录

ICP流程分为4个主要步骤

使用一点感受

实现代码

ICP流程分为4个主要步骤

1、对原始点云数据进行采样，得到初始的点云集合；

2、确定初始对应点集，模板点集在待匹配点集的位置，对结果有至关重要的影响；

3、去除错误对应点对，通过最小二乘法迭代获取正确的匹配点；

4、坐标变换求解，得到旋转矩阵R和平移矢量t；

完成高精度的变换矩阵估计；

使用一点感受

初始点集合，非常非常非常之重要，对最后结果有至关重要的作用；

迭代次数多了之后，或者点云大了之后，运算速度确实慢；

是一种精度比较高的变换矩阵估计，但是精度多高还得结合实际情况，如果需要精度特别高的话还得测试；

实现代码

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>

int main(int argc, char** argv)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>(5, 1));
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZ>);

    // Fill in the CloudIn data
    for (auto& point : *cloud_in)
    {
        point.x = 1024 * rand() / (RAND_MAX + 1.0f);
        point.y = 1024 * rand() / (RAND_MAX + 1.0f);
        point.z = 1024 * rand() / (RAND_MAX + 1.0f);
    }

    std::cout << "Saved " << cloud_in->size() << " data points to input:" << std::endl;

    for (auto& point : *cloud_in)
        std::cout << point << std::endl;

    *cloud_out = *cloud_in;

    std::cout << "size:" << cloud_out->size() << std::endl;
    for (auto& point : *cloud_out)
        point.x += 0.7f;

    std::cout << "Transformed " << cloud_in->size() << " data points:" << std::endl;

    for (auto& point : *cloud_out)
        std::cout << point << std::endl;

    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
    icp.setInputSource(cloud_in);
    icp.setInputTarget(cloud_out);

    pcl::PointCloud<pcl::PointXYZ> Final;
    icp.align(Final);

    std::cout << "has converged:" << icp.hasConverged() << " score: " <<
        icp.getFitnessScore() << std::endl;
    std::cout << icp.getFinalTransformation() << std::endl;

    return (0);
}

参考：https://pcl.readthedocs.io/projects/tutorials/en/latest/iterative_closest_point.html#iterative-closest-point

《点云库PCL从入门到精通》