ICP 点云配准c++代码

点云配准是一个常见的问题，有许多方法可以实现。这里提供一种基于ICP的点云配准c++代码示例，供参考：

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

int main(int argc, char** argv)
{
    // 读取点云数据
    pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile<pcl::PointXYZ>(argv[1], *source_cloud) == -1 ||
        pcl::io::loadPCDFile<pcl::PointXYZ>(argv[2], *target_cloud) == -1)
    {
        PCL_ERROR("Couldn't read file\n");
        return (-1);
    }

    // 定义ICP对象
    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
    icp.setInputSource(source_cloud);  // 设置源点云
    icp.setInputTarget(target_cloud);  // 设置目标点云

    // 设置ICP参数
    icp.setMaxCorrespondenceDistance(0.05);  // 最大对应距离
    icp.setTransformationEpsilon(1e-8);  // 优化阈值
    icp.setMaximumIterations(50);  // 最大迭代次数

    // 执行ICP
    pcl::PointCloud<pcl::PointXYZ> aligned_cloud;
    icp.align(aligned_cloud);

    // 输出结果
    std::cout << "ICP converged with score: " << icp.getFitnessScore() << std::endl;
    std::cout << "Transformation matrix: " << std::endl << icp.getFinalTransformation() << std::endl;

    // 保存结果
    pcl::io::savePCDFileASCII("aligned_cloud.pcd", aligned_cloud);

    return 0;
}

这个示例使用了PCL库中的Iterative Closest Point (ICP)算法，可以对两个点云进行配准。在代码中，我们首先读取了两个点云文件，然后创建了一个ICP对象，并设置了一些ICP参数。接下来，我们调用ICP的align()函数进行配准，得到变换矩阵和配准后的点云。最后，我们输出了配准结果，并将配准后的点云保存到文件中。

需要注意的是，ICP算法可能会陷入局部最优解，因此在实际应用中需要根据具体情况选择合适的算法和参数。