基于PCL的三维重建——点云配准（一）ICP算法实现

      在逆向工程，计算机视觉，文物数字化等领域中，由于点云的不完整，旋转错位，平移错位等，使得要得到的完整的点云就需要对局部点云进行配准，为了得到被测物体的完整数据模型，需要确定一个合适的坐标系，将从各个视角得到的点集合并到统一的坐标系下形成一个完整的点云，然后就可以方便进行可视化的操作，这就是点云数据的配准。点云的配准有手动配准依赖仪器的配准，和自动配准，点云的自动配准技术是通过一定的算法或者统计学规律利用计算机计算两块点云之间错位，从而达到两块点云自动配准的效果，其实质就是把不同的坐标系中测得到的数据点云进行坐标系的变换，以得到整体的数据模型，问题的关键是如何让得到坐标变换的参数R（旋转矩阵）和T（平移向量），使得两视角下测得的三维数据经坐标变换后的距离最小，，目前配准算法按照过程可以分为整体配准和局部配准，。PCL中有单独的配准模块，实现了配准相关的基础数据结构，和经典的配准算法如ICP。

     ICP算法对待拼接的2片点云，首先根据一定的准则确立对应点集P与Q，其中对应点对的个数，然后通过最小乘法迭代计算最优的坐标变换，即旋转矩阵R和平移矢量t，使得误差函数最小，ICP处理流程分为四个主要的步骤：

     1. 对原始点云数据进行采样
     2.确定初始对应点集
     3.去除错误对应点对

     4.坐标变换求解

     以下代码读取两个pcd点云配准，用空格控制迭代次数，并用visualization可视化出来，其中白色用来显示原始点云数据，经过一定旋转平移的点云显示为绿色，红色用来显示icp匹配后的点云数据。每次迭代都会打出此时的选择矩阵和平移向量。

#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 :