PCL点云库动手实践知识点3——ICP点云配准

ICP迭代最近点算法
思想：对所有的点云进行变换，使得都与第一个点云在统一坐标系中。在每个连贯的点云之间找到最佳的变换，并累积这些变换到全部的点云

#include<iostream>//标准输入输出头文件
// 包含pcd的输入输出头文件——pcd文件IO操作定义的头文件
#include<pcl/io/pcd_io.h>
// 包含点类型、结构的数据定义——包含若干PointT数据结构定义的头文件
#include<pcl/point_types.h>
#include<pcl/registration/icp.h>//ICP配准类 相关的头文件
int main(int argc, char *argv[])
{
    //智能指针创建一个点云指针对象
    // 创建一个 pcl::PointCloud<pcl::PointXYZ>共享指针
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZ>);
   // 1-1 用产生的随机点值构造源点云cloud_in，并设置合适的参数，同时打印出保存的数量
    // 与他们的实际坐标值
    cloud_in->width=5;
    cloud_in->height=11;
    cloud_in->is_dense=false;
    cloud_in->points.resize(cloud_in->width*cloud_in->height);    
     // 随机数据填充点云数据
    for (size_t i = 0; i < cloud_in->size(); i++)
    {
       cloud_in->points[i].x=1024*rand()/(RAND_MAX+1.0f);
       cloud_in->points[i].y=1024*rand()/(RAND_MAX+1.0f);
       cloud_in->points[i].z=1024*rand()/(RAND_MAX+1.0f);
    }
    std::cout<<"Saved"<<cloud_in->points.size()<<"data points to input:"<<std::endl;   
    // 打印原始点云数据 
    for (size_t i = 0; i <  cloud_in->size(); i++)
    {
        std::cout<<" "<<cloud_in->points[i].x<<" "<<cloud_in->points[i].y<<" "<<cloud_in->points[i].z<<std::endl;      
        // std::cout<<" "<<cloud_out->points[i].x<<" "<<cloud_out->points[i].y<<" "<<cloud_out->points[i].z<<std::endl;
    }
    //目标点云的处理
      *cloud_out= *cloud_in; //该含义还不是特别清楚,暂时默认为赋值操作
        std::cout<<"size:"<<cloud_out->points.size()<<std::endl;
          // 1-2 cloud_out点云数据的填充
        for (size_t i = 0; i <cloud_in->points.size(); i++)
        {
            cloud_out->points[i].x=cloud_in->points[i].x+0.7f;
        }
          std::cout<<"Transformed"<<cloud_in->points.size()<<"data points:"<<std::endl;
      
        for (size_t i = 0; i <cloud_out->points.size(); i++)
        {
           std::cout<< "   "<<cloud_out->points[i].x<< "   "<<cloud_out->points[i].y<<"   "<<cloud_out->points[i].z<<std::endl;
        }
        //ICP迭代算法
        //创建一个IterativeClosestPoint对象
        pcl::IterativeClosestPoint<pcl::PointXYZ,pcl::PointXYZ> icp;
        // 设置源点云
        icp.setInputCloud(cloud_in);
        //设置目标点云
        icp.setInputTarget(cloud_out);

        // 存储经过配准变换源点云后的点云
        pcl::PointCloud<pcl::PointXYZ> Final;
        icp.align(Final);
        std::cout<<"has converged:"<<icp.hasConverged()<<"score:"<<icp.getFitnessScore()<<std::endl;
        std::cout<<icp.getFinalTransformation()<<std::endl;   //打印输出最终估计的变换矩阵
        
    
    return 0;
}