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;
}