PCL学习笔记（二十一）-- 迭代最近点算法

一、简介

    ICP算法对待拼接的2片点云，首先根据一定的准则确立对应点集P与Q，其中对应点对的个数为n。然后通过最小二乘法迭代计算最优的坐标变化，即旋转矩阵R和平移矢量t，使得误差函数最小。ICP算法计算简便直观且可使拼接具有较好的精度，但是算法的运行速度以及向全局最优的收敛性却在很大程度上依赖于给定的初始变换估计以及在迭代过程中对应关系的确立。各种粗拼接技术可以为ICP算法提供较好的初始位置，所以迭代过程中确立正确对应点集以避免迭代陷入局部极值成为各种改进算法的关键，决定了算法的收敛速度与最终的拼接精度。ICP处理流程分为4个主要步骤：

    1）对原始点云数据进行采样；

    2）确定初始对应点集；

    3）去除错误对应点对；

    4）坐标变换的求解。

二、代码分析

    1）首先，创建两个点云共享指针，并用产生的随机点值构造源点云cloud_in，并设置合适的参数，同时，打印出保存的点数量和它们的实际坐标值：

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZ>);
  // 填入点云数据
  cloud_in->width    = 5;
  cloud_in->height   = 1;
  cloud_in->is_dense = false;
  cloud_in->points.resize (cloud_in->width * cloud_in->height);
  for (size_t i = 0; i < cloud_in->points.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->points.size (); ++i) std::cout << "    " <<
      cloud_in->points[i].x << " " << cloud_in->points[i].y << " " <<
      cloud_in->points[i].z << std::endl;
  *cloud_out = *cloud_in;
  std::cout << "size:" << cloud_out->points.size() << std::endl;

    2）接着，实现一个简单的点云刚体变换，以构造目标点云，并再次打印出点云数据：

  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;

    3）然后，设置目标点云和源点云之间的对应关系：

  pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
  icp.setInputSource(cloud_in);
  icp.setInputTarget(cloud_out);

    4）最后，输出刚体变换后的点云信息：

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

    5）整体代码：

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

int main(int argc, char** argv)
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>);												//智能指针初始化点云
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZ>);

	cloud_in->width = 5;
	cloud_in->height = 1;
	cloud_in->is_dense = false;
	cloud_in->points.resize(cloud_in->width * cloud_in->height);
	for (size_t i = 0; i < cloud_in->points.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->points.size();++i)
	{
		std::cout << " " << cloud_in->points[i].x << " " << cloud_in->points[i].y << " " << cloud_in->points[i].z << std::endl;
	}
	*cloud_out = *cloud_in;																											//将输入点云复制给输出点云
	std::cout << "Size:" << cloud_out->points.size() << "data points to output" << std::endl;
	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_out->points.size() << "data points to output:" << 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;
	}
	pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ>icp;																	//实例化一个icp对象
	icp.setInputSource(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);
}

三、编译效果