C++实现点云NDT配准

C++实现点云NDT配准

来自https://adamshan.blog.csdn.net/article/details/79230612

读取点云信息

pcl::PointCloud<pcl::PointXYZ>::Ptr read_cloud_point(std::string const &file_path){
    // Loading first scan.
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile<pcl::PointXYZ> (file_path, *cloud) == -1)
    {
        PCL_ERROR ("Couldn't read the pcd file\n");
        return nullptr;
    }
    return cloud;
}

在

main

函数中分别读取点云：

    auto target_cloud = read_cloud_point("cloud1.pcd");
    std::cout << "Loaded " << target_cloud->size () << " data points from cloud1.pcd" << std::endl;

    auto input_cloud = read_cloud_point("cloud2.pcd");
    std::cout << "Loaded " << input_cloud->size () << " data points from cloud2.pcd" << std::endl;

从两个pcd文件中读取出来的点的数量：

Loaded 112586 data points from cloud1.pcd
Loaded 112624 data points from cloud2.pcd

点云滤波处理

对如此多点做优化是非常耗时的，我们使用

voxel_filter

对输入的点云进行过滤，这里只对

input_cloud

进行了滤波处理，减少其数据量到10%左右，而

target_cloud

不做滤波处理:

    pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;
    approximate_voxel_filter.setLeafSize(0.2, 0.2, 0.2);
    approximate_voxel_filter.setInputCloud(input_cloud);
    approximate_voxel_filter.filter(*filtered_cloud);

    std::cout<<"Filtered cloud contains "<< filtered_cloud->size() << "data points from cloud2.pcd" << std::endl;

NDT初始化以及参数设置

    pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
    ndt.setTransformationEpsilon(0.01);
    ndt.setStepSize(0.1);
    ndt.setResolution(1.0);

    ndt.setMaximumIterations(35);
    ndt.setInputSource(filtered_cloud);
    ndt.setInputTarget(target_cloud);

其中 ndt.setTransformationEpsilon() 即设置变换的 ϵϵ（两个连续变换之间允许的最大差值），这是判断我们的优化过程是否已经收敛到最终解的阈值。ndt.setStepSize(0.1) 即设置牛顿法优化的最大步长。ndt.setResolution(1.0) 即设置网格化时立方体的边长，网格大小设置在NDT中非常重要，太大会导致精度不高，太小导致内存过高，并且只有两幅点云相差不大的情况才能匹配。ndt.setMaximumIterations(35) 即优化的迭代次数，我们这里设置为35次，即当迭代次数达到35次或者收敛到阈值时，停止优化。

初始化参数并开始优化

我们对变换参数 p 进行初始化(给一个估计值)，虽然算法不指定初值也可以运行，但是有了它，易于得到更好的结果，尤其是当两块点云差异较大时。变换参数的初始化数据往往来自与测量数据：

    Eigen::AngleAxisf init_rotation(0.6931, Eigen::Vector3f::UnitZ());
    Eigen::Translation3f init_translation (1.79387, 0.720047, 0);
    Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix();

    pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZ>);

    ndt.align(*output_cloud, init_guess);
    std::cout << "Normal Distribution Transform has converged:" << ndt.hasConverged()
              << "score: " << ndt.getFitnessScore() << std::endl;

保存配准后的点云图，输出cloud3.pcd

pcl::transformPointCloud(*input_cloud, *output_cloud, ndt.getFinalTransformation());
pcl::io::savePCDFileASCII("cloud3.pcd", *output_cloud);

将配准以后的点云图可视化
我们写一个函数用于现实配准以后的点云,其中，目标点云（即我们已有的高精度地图）用红点绘制，而输入点云用绿点绘制：

void visualizer(pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud){
    // Initializing point cloud visualizer
    boost::shared_ptr<pcl::visualization::PCLVisualizer>
            viewer_final (new pcl::visualization::PCLVisualizer ("3D Viewer"));
    viewer_final->setBackgroundColor (0, 0, 0);

    // Coloring and visualizing target cloud (red).
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
            target_color (target_cloud, 255, 0, 0);
    viewer_final->addPointCloud<pcl::PointXYZ> (target_cloud, target_color, "target cloud");
    viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
                                                    1, "target cloud");

    // Coloring and visualizing transformed input cloud (green).
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
            output_color (output_cloud, 0, 255, 0);
    viewer_final->addPointCloud<pcl::PointXYZ> (output_cloud, output_color, "output cloud");
    viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
                                                    1, "output cloud");

    // Starting visualizer
    viewer_final->addCoordinateSystem (1.0, "global");
    viewer_final->initCameraParameters ();

    // Wait until visualizer window is closed.
    while (!viewer_final->wasStopped ())
    {
        viewer_final->spinOnce (100);
        boost::this_thread::sleep (boost::posix_time::microseconds (100000));
    }
}

}

效果

完整代码：NDT_PCL_demo

在实际运行代码我们发现我们这个demo的NDT计算比较耗时，其计算量主要集中在使用大量扫描点来优化目标函数上面。我们可以通过一些方法来提高NDT的运算效率，其中比较直接的方案就是使用GPU计算。在Autoware （https://github.com/CPFL/Autoware） 项目中，其定位模块就是基于NDT算法实现的，其通过使用CUDA实现的 fast_pcl package实现了对NDT优化过程的并行加速，
————————————————
版权声明：本文为CSDN博主「AdamShan」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。
原文链接：https://blog.csdn.net/AdamShan/article/details/79230612