PCL GICP配准（Registration_GICP）

注意：传统ICP算法是点对点、点对面的配准，GICP通过拟合局部平面，计算协方差矩阵，将其拓展为面对面的配准。

协方差矩阵相关介绍

ICP配准

1.原理

GICP 实质上是一种面对面的ICP算法。其将概率信息（使用协方差矩阵计算）和点对点的ICP 结合在一起，生成gicp统一模型（Generalized-ICP）。

其中通过局部点集协方差矩阵起到类似于权重的作用，消除某些不好的对应点在求解过程中的作用。

GICP介绍

（1）计算源点云P和目标点云Q中的最邻近点集对(Pi,Qi)。

（2）通过KD树，拟合生成Pi和Qi的局部平面。

（2）分别计算点集Pi和Qi所拟合平面的协方差矩阵Mp、Mq。

（3）计算两拟合平面之间的变换矩阵Mt。

（4）判断误差函数是否满足收敛条件，如果满足，则记录为结果，若不满足，则返回(2)，重新进行迭代。

2.关键函数

1）为终止条件设置最小转换差异，即最优点对距离的阈值（本值一般都设很小）

registration.setTransformationEpsilon(1e-10);                            // 为终止条件设置最小转换差异

（2）设置对应点对之间的最大距离，即pcl::registration::CorrespondenceEstimation调用determineCorrespondences(*correspondences, dis)时的dis的值。

registration.setMaxCorrespondenceDistance(1.5);                            // 设置对应点对之间的最大距离

（3）设置收敛条件是均方误差和小于阈值， 停止迭代；

registration.setEuclideanFitnessEpsilon(0.01);

（4）最大迭代次数

registration.setMaximumIterations(500);

（5）设置为true,则使用点对对应关系【需设置为false】

registration.setUseReciprocalCorrespondences(false);                    // 设置为true,则使用点对对应关系

3.完整代码

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/gicp.h>  // GICP头文件
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <pcl/console/time.h>       // 利用控制台计算时间

using namespace std;

int main()
{
	/****************ICP配准********************/
	pcl::PointCloud<pcl::PointXYZ>::Ptr source(new pcl::PointCloud<pcl::PointXYZ>);			// 源点云
	pcl::PointCloud<pcl::PointXYZ>::Ptr target(new pcl::PointCloud<pcl::PointXYZ>);			// 目标点云
	pcl::io::loadPCDFile("D:/code/csdn/data/tree0.pcd", *source);
	pcl::io::loadPCDFile("D:/code/csdn/data/tree1.pcd", *target);
	pcl::PointCloud<pcl::PointXYZ>::Ptr result(new pcl::PointCloud<pcl::PointXYZ>);			// 目标点云转换后的结果

	// GICP配准
	pcl::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> registration;				// gicp对象
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree1(new pcl::search::KdTree<pcl::PointXYZ>);	// 建树
	tree1->setInputCloud(source);
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree2(new pcl::search::KdTree<pcl::PointXYZ>);
	tree2->setInputCloud(target);
	registration.setSearchMethodSource(tree1);
	registration.setSearchMethodTarget(tree2);
	registration.setInputSource(source);						// 源点云
	registration.setInputTarget(target);						// 目标点云
	registration.setMaxCorrespondenceDistance(1.5);				// 设置对应点对之间的最大距离
	registration.setTransformationEpsilon(1e-10);				// 为终止条件设置最小转换差异
	registration.setEuclideanFitnessEpsilon(0.01);				// 设置收敛条件是均方误差和小于阈值， 停止迭代
	registration.setMaximumIterations(500);						// 最大迭代次数
	registration.setUseReciprocalCorrespondences(false);
	registration.align(*result);

	std::cout << "点对匹配分数：" << registration.getFitnessScore() << std::endl;
	std::cout << "刚体变换矩阵：" << registration.getFinalTransformation() << std::endl;

	/****************展示********************/
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("GICP"));
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>target_color(target, 255, 0, 0);		// 目标点云
	viewer->addPointCloud<pcl::PointXYZ>(target, target_color, "target cloud");
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>result_color(result, 0, 255, 0);		// 配准结果点云
	viewer->addPointCloud<pcl::PointXYZ>(result, result_color, "result cloud");

	while (!viewer->wasStopped())
	{
		viewer->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}

	return (0);
}

4.结果展示