PCL库简单的icp配准

news2024/10/2 17:50:00

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

int main(int argc, char** argv)
{
    // 确保提供了两个PCD文件作为输入
    if (argc != 3) {
        PCL_ERROR("请提供两个PCD文件作为输入。\n");
        return (-1);
    }

    // 读取源点云和目标点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_reg(new pcl::PointCloud<pcl::PointXYZ>);

    if (pcl::io::loadPCDFile<pcl::PointXYZ>(argv[1], *cloud_in) == -1) {
        PCL_ERROR("无法读取文件 %s\n", argv[1]);
        return (-1);
    }
    if (pcl::io::loadPCDFile<pcl::PointXYZ>(argv[2], *cloud_out) == -1) {
        PCL_ERROR("无法读取文件 %s\n", argv[2]);
        return (-1);
    }

    // 创建ICP配准对象
    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
    icp.setInputSource(cloud_in);
    icp.setInputTarget(cloud_out);

    // 进行配准
    icp.align(*cloud_reg);

    if (icp.hasConverged())
    {
        std::cout << "配准成功！" << std::endl;
        std::cout << "最终变换矩阵:" << icp.getFinalTransformation() << std::endl;
        std::cout << "配准后的点云:" << std::endl;
        for (size_t i = 0; i < cloud_reg->size(); ++i)
            std::cout << "    " << cloud_reg->points[i].x
                      << " " << cloud_reg->points[i].y
                      << " " << cloud_reg->points[i].z << std::endl;
    // 保存配准后的点云为PCD文件
        pcl::io::savePCDFile("aligned_cloud.pcd", *cloud_reg);
        std::cout << "已保存配准后的点云为 'aligned_cloud.pcd'" << std::endl;
    }
    else
    {
        std::cout << "配准失败！" << std::endl;
    }

    return 0;
}

测试了一下PCL库的icp效果，对于40度左右的旋转的配准效果还是不好。

joey@joey-Legion-Y7000P-IRX9:~/code/pclregistration/build$ ./simple_icp '/home/joey/code/pclregistration/data/1564024971.235044000.pcd' '/home/joey/code/pclregistration/data/1564025000.934826000.pcd'

在命令行调用生成的可执行文件。

可以看到初始点云（紫色），目标点云（白色），配准结果（蓝色）。

可以看到在角度比较大的时候，icp的效果不是很好。

简单调了一下参数，效果显著提升。


    // 设置ICP参数
    icp.setMaxCorrespondenceDistance(1); // 设置最大对应距离
    icp.setEuclideanFitnessEpsilon(0.0001); // 设置欧几里得拟合度阈值
    icp.setMaximumIterations(200); // 设置最大迭代次数
    icp.convergence_criteria_->setAbsoluteMSE(1e-5); // 设置绝对均方误差
    icp.convergence_criteria_->setMaximumIterationsSimilarTransforms(10); // 设置最大相似变换迭代次数

setMaxCorrespondenceDistance(float distance):
- 作用：设置最大对应距离，即源点云中的点与目标点云中最近点之间的最大允许距离。如果距离超过这个值，这对点将不会被考虑在内，以避免错误的匹配。
- 参数：1 表示最大距离为1米。这个值越小，算法越倾向于只匹配非常接近的点对，但可能会导致有效匹配点对数量减少；值越大，可能会包含错误的匹配，但匹配点对数量会增加。
setEuclideanFitnessEpsilon(float epsilon):
- 作用：设置欧几里得拟合度阈值，用于确定算法是否收敛。如果连续两次迭代的均方误差（MSE）的相对变化小于这个值，则认为算法已经收敛。
- 参数：0.0001 表示相对误差阈值为0.01%。这是一个不太严格的收敛条件，意味着算法会在接近最优解时停止迭代。
setMaximumIterations(int iterations):
- 作用：设置最大迭代次数。无论算法是否收敛，达到最大迭代次数后都会停止。
- 参数：200 表示最大迭代次数为200次。这个值可以根据实际情况调整，较大的值可能会得到更精确的结果，但计算时间会更长。
convergence_criteria_->setAbsoluteMSE(double mse):
- 作用：设置绝对均方误差（MSE）阈值。如果当前迭代的MSE小于这个值，则认为算法已经收敛。
- 参数：1e-5 表示绝对MSE阈值为0.00001。这意味着如果算法的MSE降至这个值以下，即使没有达到最大迭代次数，算法也会停止。
convergence_criteria_->setMaximumIterationsSimilarTransforms(int iterations):
- 作用：设置在算法收敛前，允许的最大连续迭代次数，其中变换矩阵变化不大（即两次迭代的MSE变化很小）。
- 参数：10 表示如果连续10次迭代的变换矩阵变化都很小，则认为算法已经收敛。这有助于避免在接近最优解时进行不必要的额外迭代。