点云配准与匹配

配准与匹配是在两个数据集中寻找共同的结构或特征，然后利用它们将数据集拼接到一起。

PCL提供了迭代最近点算法执行配准与匹配。

源文件pcl_matching.cpp：

#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl/registration/icp.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>

class cloudHandler
{
public:
    cloudHandler()
    {
        pcl_sub = nh.subscribe("pcl_downsampled", 10, &cloudHandler::cloudCB, this);
        pcl_pub = nh.advertise<sensor_msgs::PointCloud2>("pcl_matched", 1);
    }

    void cloudCB(const sensor_msgs::PointCloud2 &input)
    {
        pcl::PointCloud<pcl::PointXYZ> cloud_in;
        pcl::PointCloud<pcl::PointXYZ> cloud_out;
        pcl::PointCloud<pcl::PointXYZ> cloud_aligned;
        sensor_msgs::PointCloud2 output;

        pcl::fromROSMsg(input, cloud_in);

        cloud_out = cloud_in;

        for (size_t i = 0; i < cloud_in.points.size (); ++i)
        {
            cloud_out.points[i].x = cloud_in.points[i].x + 0.7f;
        }

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

        icp.setMaxCorrespondenceDistance(5);
        icp.setMaximumIterations(100);
        icp.setTransformationEpsilon (1e-12);
        icp.setEuclideanFitnessEpsilon(0.1);

        icp.align(cloud_aligned);

        pcl::toROSMsg(cloud_aligned, output);
        pcl_pub.publish(output);
    }

protected:
    ros::NodeHandle nh;
    ros::Subscriber pcl_sub;
    ros::Publisher pcl_pub;
};

main(int argc, char **argv)
{
    ros::init(argc, argv, "pcl_matching");

    cloudHandler handler;

    ros::spin();

    return 0;
}

为了提高算法的执行效率，这个示例使用pcl_downsampled主题作为点云的输入源，最后的结果通过pcl_matched主题发布。

此算法使用三个点云进行匹配和配准：

• 要转换的点云
• 需要第一个点云与之对齐的固定点云
• 最终结果的点云

由于没有连续的点云源，为了简化问题，使用相同的原始点云作为固定点云，但是沿x轴进行了平移。

调用迭代最近点算法进行配准和匹配，这个算法使用奇异值分解（SVD），朝着输入点云到固定点云之间间隔减小的方向求解转换方程。这个算法有三个基本的停止条件：

• 上一个转换和当前转换之间的差别小于特定的阈值，这个阈值可以通过setTransformationEpsilon函数设置。
• 迭代次数已经达到用户设置的最大值，最大值可以通过setMaximumIterations函数设置。
• 最后，在循环中两次连续步骤之间的欧几里得平方误差之和低于特定的阈值，这个阈值可以通过setEuclideanFitnessEpsilon函数设置。

另一个用来提高精度的参数是对应距离（Correspondance Distance)，可以通过setMaxCorrespondenceDistance函数设置，这个参数定义了对准过程中两个对应点之间应具有的最小距离。

运行示例

1.启动roscore

roscore

2.运行pcl_read

rosrun chapter6_tutorials pcl_read

3.运行pcl_filter

rosrun chapter6_tutorials pcl_filter

4.运行downsampling

rosrun chapter6_tutorials pcl_downsampling

5.运行pcl_matching

rosrun chapter6_tutorials pcl_matching

6.运行rviz

rviz

很奇怪，没修改代码，按书上的步骤操作，但是这并不是配准与匹配后的点云啊！