PCL ICP点云配准

 随着计算机辅助设计技术的发展，通过实物模型产生数字模型的逆向工程技术由于它的独特魅力获得了越来越广泛的应用，与此同时，硬件设备的日趋完善也为数字模型操作了足够的技术支持。在逆向工程、计算机视觉、文物数字化等领域中,由于点云的不完整、旋转错位、平移错位等，使得要得到完整点云就需要对局部点去进行配准。为了得到被测物体的完整数据模型，需要确定一个合适的坐标变换，将从各个视角得到的点集合并到一个统一的坐标系下形成一个完整的数据点云，然后就可以方便地进行可视化等操作,这就是点云数据的配准。点云配准有手动配准、依赖仪器的配准和自动配准。通常我们所说的点云配准技术即是指最后一种自动配准。点云自动配准技术是通过一定的算法或者统计学规律，利用计算机计算两块点云之间的错位,从而达到把两片点云自动配准的效果。其实质是把在不同的坐标系中测量得到的数据点云进行坐标变换,以得到整体的数据模型。问题的关键是如何求得坐标变换参数R(旋转矩阵)和T(平移向量),使得两视角下测得的三维数据经坐标变换后的距离最小。目前，配准算法按照实现过程可以分为整体配准和局部配准。PCL中有单独的配准模块实现了配准相关的基础数据结构与经典配准算法如ICP等，以及配准过程中的对应点估计、错误对应点去除等流程。

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

代码实现如下：

#include<iostream>
#include<pcl/io/pcd_io.h>
#include<pcl/point_types.h>
#include<pcl/registration/icp.h>
#include <pcl/visualization/pcl_visualizer.h>//可视化头文件

typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloudT;

bool next_iteration = false;

void print4x4Matrix(const Eigen::Matrix4d & matrix)    //打印旋转矩阵和平移矩阵
{
	printf("Rotation matrix :\n");
	printf("    | %6.3f %6.3f %6.3f | \n", matrix(0, 0), matrix(0, 1), matrix(0, 2));
	printf("R = | %6.3f %6.3f %6.3f | \n", matrix(1, 0), matrix(1, 1), matrix(1, 2));
	printf("    | %6.3f %6.3f %6.3f | \n", matrix(2, 0), matrix(2, 1), matrix(2, 2));
	printf("Tran