Ubuntu 20.04.06 PCL C++学习记录（二十七）【附所用点云】

@[TOC]PCL中点云配准模块的学习

学习背景

参考书籍：《点云库PCL从入门到精通》以及官方代码PCL官方代码链接,，PCL版本为1.10.0，CMake版本为3.16，可用点云下载地址

学习内容

使用正态分布算法来确定两个大型点云之间的刚体变换，正态分布变换算法是一个配准算法，它应用于三维点的统计模型，使用标准最优化技术来确定两个点云间的最优匹配，因为其在配准的过程中不利用对应点的特征计算和匹配，所以时间比其他方法比较快，本章点云下载room_scan1.pcd,room_scan2.pcd

源代码及所用函数

源代码

cpp 复制代码

//使用正态分布变换进行配准的实验 。其中room_scan1.pcd  room_scan2.pcd这些点云包含同一房间360不同视角的扫描数据
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include<thread>//C++管理线程头文件
#include<pcl/registration/ndt.h>//用于执行正态分布变换(Normal Distributions Transform, NDT)配准算法的头文件
#include<pcl/filters/approximate_voxel_grid.h>//滤波头文件

using namespace std::chrono_literals;

int main()
{
    //********************************************加载房间的第一次扫描点云数据作为目标***************************//
    pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    if(pcl::io::loadPCDFile<pcl::PointXYZ>("/home/jojo/PointCloud/PCLB/Registration/03/room_scan1.pcd",*target_cloud) == -1)
    {
        //std::cout << " 加载文件失败" << std::endl;
        PCL_ERROR("加载文件room_scan1.pcd失败\n");
        return -1;
    }
    std::cout << "加载 " << target_cloud->size () << " 个点从room_scan1.pcd中" << std::endl;
    //*************************************加载从新视角得到的第二次扫描点云数据作为源点云***************************//
    pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile<pcl::PointXYZ> ("/home/jojo/PointCloud/PCLB/Registration/03/room_scan2.pcd", *input_cloud) == -1)
    {
        PCL_ERROR ("加载文件room_scan2.pcd失败\n \n");
        return (-1);
    }
    std::cout << "加载 " << input_cloud->size () << " 个点从room_scan2.pcd中" << std::endl;
    //************************将输入扫描内容过滤为原稿大小的 10%左右，以提高套准速度*********************//
    //以上的代码加载了两个PCD文件得到共享指针，后续配准是完成对源点云到目标点云的参考坐标系的变换矩阵的估计，得到第二组点云变换到第一组点云坐标系下的变换矩阵
    //因为在NDT算法中在目标点云对应的体素网格数据结构的统计计算不使用单个点，而是使用包含在每个体素单元格中的点的统计数据
    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->size ()
            << " 个点从room_scan2.pcd中" << std::endl;
    //*****************************************************初始化正态分布变换************************//
    pcl::NormalDistributionsTransform<pcl::PointXYZ,pcl::PointXYZ> ndt;
    //设置与刻度相关的无损检测参数
    //为终止条件设置最小变换差。
    ndt.setTransformationEpsilon(0.01);
    //设置 More-Thuente 线搜索的最大步长
    ndt.setStepSize(0.1);
    //设置NDT网格网格结构的分辨率
    ndt.setResolution (1.0); 
    //以上参数在使用房间尺寸比例下运算比较好，但是如果需要处理例如一个咖啡杯子的扫描之类更小的物体，需要对参数进行很大程度的缩小

    //设置匹配迭代的最大次数，这个参数控制程序运行的最大迭代次数，一般来说这个限制值之前优化程序会在epsilon变换阀值下终止
    //添加最大迭代次数限制能够增加程序的鲁棒性阻止了它在错误的方向上运行时间过长
    ndt.setMaximumIterations(35);

    //设置要对齐的点云
    ndt.setInputSource(filtered_cloud);
    //设置对其目标点云
    ndt.setInputTarget(target_cloud);

    // 设置使用机器人测距法得到的粗略初始变换矩阵结果
    Eigen::AngleAxisf init_rotation(0.6931,Eigen::Vector3f::UnitZ());
    Eigen::Translation3f init_translation(1.79387, 0.720047, 0);
    //将平移矩阵init_translation与从四元数init_rotation获得的旋转矩阵相乘,初始化了一个4x4变换矩阵init_guess。
    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);//点云配准
    //这个地方的output_cloud不能作为最终的源点云变换，因为上面对点云进行了滤波处理
    std::cout << "正态分布变换已收敛：" << ndt.hasConverged ()
            << " 分数为: " << ndt.getFitnessScore () << std::endl;
    //使用创建的变换对为过滤的输入点云进行变换
    pcl::transformPointCloud(*input_cloud,*output_cloud,ndt.getFinalTransformation());
    //保存转换后的源点云作为最终的变换输出
    pcl::io::savePCDFileASCII("/home/jojo/PointCloud/PCLB/Registration/03/room_scan2_transformed.pcd",*output_cloud);
    //***************************************可视化点云***********************************//
    pcl::visualization::PCLVisualizer::Ptr viewer_final (new pcl::visualization::PCLVisualizer ("3D Viewer"));
    viewer_final->setBackgroundColor (0, 0, 0);
    //着色和可视化目标云（红色）
    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,2,"target 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,2,"output cloud");
    // 启动可视化
    viewer_final->addCoordinateSystem(1.0);//显示XYZ指示轴
    viewer_final->initCameraParameters();//使用默认摄像头参数
    while (!viewer_final->wasStopped ())
    {
        viewer_final->spinOnce (100);
        std::this_thread::sleep_for(100ms);
    }
    return (0);

}

CMakeLists.txt