三维重构（10）：PCL点云配准

点云配准主要针对点云的：不完整、旋转错位、平移错位。因此要得到完整点云就需要对局部点云进行配准。为了得到被测物体的完整数据模型，需要确定一个合适的坐标系变换，将从各个视角得到的点集合并到一个统一的坐标系下形成一个完整的数据点云，然后就可以方便地进行可视化，这就是点云数据的配准。
点云配准技术通过计算机技术和统计学规律，通过计算机计算两个点云之间的错位，也就是把在不同的坐标系下的得到的点云进行坐标变换，达到坐标系的统一。其问题的关键在于如何得到坐标系变换参数$R,T$R,T，使得两个视角下测得的三维数据经过坐标系变换后的距离最小。
点云配准分为局部配准和全局配准，PCL中实现了配准相关的基础数据结构与经典配准算法如ICP等，以及配准过程中的对应点估计、错误对应点去除等流程。

PCL配准算法简介

两两配准

一对点云数据集的配准为两两配准，估计$4*4$4∗4的刚体变换矩阵使一个点云数据集精确地与另一个点云数据集进行配准。
配准步骤：

1. 从两个数据集中按照同样的关键点选取彼岸准，提取关键点
2. 对选择的所有关键点分别计算特征描述子
3. 结合特征描述子在两个数据集中的坐标的位置，以二者之间特征和位置的相似度为基础，来估算它们的对应关系，初步估计对应点对
4. 假定数据有噪声，除去对配准有影响的错误的对应点对
5. 利用剩余的正确对应关系估计刚体变换，完成配准
   配准过程中最重要的是关键点的提取和关键点的特征描述，以确保对应估计的准确性和效率。

对应估计

假设已经得到由两次扫描的点云数据获得的两组特征向量，在此基础上，我们必须找到相似特征再确定数据的重叠部分，然后才进行配准。根据特征的类型PCL使用不同的方法来搜索特征之间的对应关系。
点匹配使用$xyz$xyz三维坐标作为特征值，针对有序点云数据和无需点云数据有不同的处理策略。

1. 穷举配准
2. kd树近邻查询
3. 在有序点云数据的图像空间中查找
4. 在无需点云数据的索引空间中查找

在使用特征匹配的时候，不使用点的坐标，而是某些查询点邻域确定的特征，如法向量、局部或者全局形状直方图，方法如下：

1. 穷举配准
2. kd树最近邻查询（FLANN）

迭代最近点算法（iterative closest point ICP）

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

1. 对原始点云数据进行采样
2. 确定初始对应点集
3. 去除错误对应点对
4. 坐标变换的求解

采样一致性初始配准算法（SAC-IA）

配准算法从精度上分为两类：一种是初始的变换矩阵的粗略估计，另一种是像ICP一样的精确地变换矩阵估计。
该SAC-IA算法能够解决贪婪算法计算复杂度高的问题，试图保持相同的对应关系的几何关系而不必尝试了解有限个对应关系的所有组合。从候选的对应关系中进行大量采样并对其进行排名，步骤如下：

1. 从$P$P中选择$s$s个样本点，同时确定他们的配对距离大于用户设定的最小值$d_{min}$dmin​
2. 对每个样本点，在Q中找到满足直方图和样本点直方图相似的点存入一个列表，从这些点中随机选择一些代表采样点的对应关系
3. 酮酸通过采样点定义的刚体变换和其对应变换，计算点云的度量错误来评价转换的质量。
   重复上述三个步骤，直到取得存储了最佳度量错误，并使用暴力配准部分数据。最后再进行非线性优化。

PCL点云配准应用实例

迭代最近点算法的使用（ICP）

计算简单直观，且具有较好的精度，计算速度和质量依赖于初始化。

PCL 代码库示例：

pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
	icp.setInputCloud(cloud_in);
	icp.setInputTarget(cloud_out);
	pcl::PointCloud<pcl::PointXYZ> Final;
	icp.align(Final);

可运行代码：

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#define BOOST_TYPEOF_EMULATION
#include <pcl/registration/icp.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() << 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;//将原始点云x轴平移
	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;
	
	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);
}

结果：

逐步匹配多个点云

这个运行的例子中，有5块点云，代码中对点云分别进行匹配，匹配顺序为1-2,2-3,3-4,4-5.

/* \author Radu Bogdan Rusu
 * adaptation Raphael Favier*/
#include <boost/make_shared.hpp>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/point_representation.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/filter.h>
#include <pcl/features/normal_3d.h>
#define BOOST_TYPEOF_EMULATION
#include <pcl/registration/icp.h>
#include <pcl/registration/icp_nl.h>
#include <pcl/registration/transforms.h>
#include <pcl/visualization/pcl_visualizer.h>
using pcl::visualization::PointCloudColorHandlerGenericField;
using pcl::visualization::PointCloudColorHandlerCustom;
//简单类型定义
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;
typedef pcl::PointNormal PointNormalT;
typedef pcl::PointCloud<PointNormalT> PointCloudWithNormals;
//这是一个辅助教程，因此我们可以负担全局变量
	//创建可视化工具
pcl::visualization::PCLVisualizer* p;
//定义左右视点
int vp_1, vp_2;
//处理点云的方便的结构定义
struct PCD
{
	PointCloud::Ptr cloud;
	std::string f_name;
	PCD() : cloud(new PointCloud) {};
};
struct PCDComparator
{
	bool operator () (const PCD& p1, const PCD& p2)
	{
		return (p1.f_name < p2.f_name);
	}
};
//以< x, y, z, curvature >形式定义一个新的点
class MyPointRepresentation : public pcl::PointRepresentation <PointNormalT>
{
	using pcl::PointRepresentation<PointNormalT>::nr_dimensions_;
public:
	MyPointRepresentation()
	{
		//定义尺寸值
		nr_dimensions_ = 4;
	}
	//覆盖copyToFloatArray方法来定义我们的特征矢量
	virtual void copyToFloatArray(const PointNormalT& p, float* out) const
	{
		// < x, y, z, curvature >
		out[0] = p.x;
		out[1] = p.y;
		out[2] = p.z;
		out[3] = p.curvature;
	}
};
////////////////////////////////////////////////////////////////////////////////
/** 在可视化窗口的第一视点显示源点云和目标点云
*
 */
void showCloudsLeft(const PointCloud::Ptr cloud_target, const PointCloud::Ptr cloud_source)
{
	p->removePointCloud("vp1_target");
	p->removePointCloud("vp1_source");
	PointCloudColorHandlerCustom<PointT> tgt_h(cloud_target, 0, 255, 0);
	PointCloudColorHandlerCustom<PointT> src_h(cloud_source, 255, 0, 0);
	p->addPointCloud(cloud_target, tgt_h, "vp1_target", vp_1);
	p->addPointCloud(cloud_source, src_h, "vp1_source", vp_1);
	PCL_INFO("Press q to begin the registration.\n");
	p->spin();
}
////////////////////////////////////////////////////////////////////////////////
/**在可视化窗口的第二视点显示源点云和目标点云
 *
 */
void showCloudsRight(const PointCloudWithNormals::Ptr cloud_target, const PointCloudWithNormals::Ptr cloud_source)
{
	p->removePointCloud("source");
	p->removePointCloud("target");
	PointCloudColorHandlerGenericField<PointNormalT> tgt_color_handler(cloud_target, "curvature");
	if (!tgt_color_handler.isCapable())
		PCL_WARN("Cannot create curvature color handler!");
	PointCloudColorHandlerGenericField<PointNormalT> src_color_handler(cloud_source, "curvature");
	if (!src_color_handler.isCapable())
		PCL_WARN("Cannot create curvature color handler!");
	p->addPointCloud(cloud_target, tgt_color_handler, "target", vp_2);
	p->addPointCloud(cloud_source, src_color_handler, "source", vp_2);
	p->spinOnce();
}
////////////////////////////////////////////////////////////////////////////////
/**加载一组我们想要匹配在一起的PCD文件
  * 参数argc是参数的数量 (pass from main ())
  *参数 argv 实际的命令行参数 (pass from main ())
  *参数models点云数据集的合成矢量
  */
void loadData(int argc, char** argv, std::vector<PCD, Eigen::aligned_allocator<PCD> >& models)
{
	char bug[100];
	for (int i = 1; i < 6; i++)
	{
		sprintf(bug, "capture000%d.pcd", i);
		PCD m;
		m.f_name = bug;
		pcl::io::loadPCDFile(bug, *m.cloud);
		//从点云中移除NAN点
		std::vector<int> indices;
		pcl::removeNaNFromPointCloud(*m.cloud, *m.cloud, indices);
		models.push_back(m);
	}

	/////以下为原来的代码
	//std::string extension(".pcd");
	////假定第一个参数是实际测试模型
	//for (int i = 1; i < argc; i++)
	//{
	//	std::string fname = std::string(argv[i]);
	//	// 至少需要5个字符长（因为.plot就有 5个字符）
	//	if (fname.size() <= extension.size())
	//		continue;
	//	std::transform(fname.begin(), fname.end(), fname.begin(), (int(*)(int))tolower);
	//	//检查参数是一个pcd文件
	//	if (fname.compare(fname.size() - extension.size(), extension.size(), extension) == 0)
	//	{
	//		//加载点云并保存在总体的模型列表中
	//		PCD m;
	//		m.f_name = argv[i];
	//		pcl::io::loadPCDFile(argv[i], *m.cloud);
	//		//从点云中移除NAN点
	//		std::vector<int> indices;
	//		pcl::removeNaNFromPointCloud(*m.cloud, *m.cloud, indices);
	//		models.push_back(m);
	//	}
	//}
}
////////////////////////////////////////////////////////////////////////////////
/**匹配一对点云数据集并且返还结果
  *参数 cloud_src 是源点云
  *参数 cloud_src 是目标点云
  *参数output输出的配准结果的源点云
  *参数final_transform是在来源和目标之间的转换
  */
void pairAlign(const PointCloud::Ptr cloud_src, const PointCloud::Ptr cloud_tgt, PointCloud::Ptr output, Eigen::Matrix4f& final_transform, bool downsample = false)
{
	//
	//为了一致性和高速的下采样
	//注意：为了大数据集需要允许这项
	PointCloud::Ptr src(new PointCloud);
	PointCloud::Ptr tgt(new PointCloud);
	pcl::VoxelGrid<PointT> grid;
	if (downsample)
	{
		grid.setLeafSize(0.05, 0.05, 0.05);
		grid.setInputCloud(cloud_src);
		grid.filter(*src);
		grid.setInputCloud(cloud_tgt);
		grid.filter(*tgt);
	}
	else
	{
		src = cloud_src;
		tgt = cloud_tgt;
	}
	//计算曲面法线和曲率
	PointCloudWithNormals::Ptr points_with_normals_src(new PointCloudWithNormals);
	PointCloudWithNormals::Ptr points_with_normals_tgt(new PointCloudWithNormals);
	pcl::NormalEstimation<PointT, PointNormalT> norm_est;
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
	norm_est.setSearchMethod(tree);
	norm_est.setKSearch(30);
	norm_est.setInputCloud(src);
	norm_est.compute(*points_with_normals_src);
	pcl::copyPointCloud(*src, *points_with_normals_src);
	norm_est.setInputCloud(tgt);
	norm_est.compute(*points_with_normals_tgt);
	pcl::copyPointCloud(*tgt, *points_with_normals_tgt);
	//
	//举例说明我们自定义点的表示（以上定义）
	MyPointRepresentation point_representation;
	//调整'curvature'尺寸权重以便使它和x, y, z平衡
	float alpha[4] = { 1.0, 1.0, 1.0, 1.0 };
	point_representation.setRescaleValues(alpha);
	//
	// 配准
	pcl::IterativeClosestPointNonLinear<PointNormalT, PointNormalT> reg;
	reg.setTransformationEpsilon(1e-6);
	//将两个对应关系之间的(src<->tgt)最大距离设置为10厘米
	//注意：根据你的数据集大小来调整
	reg.setMaxCorrespondenceDistance(0.1);
	//设置点表示
	reg.setPointRepresentation(boost::make_shared<const MyPointRepresentation>(point_representation));
	reg.setInputCloud(points_with_normals_src);
	reg.setInputTarget(points_with_normals_tgt);
	//
	//在一个循环中运行相同的最优化并且使结果可视化
	Eigen::Matrix4f Ti = Eigen::Matrix4f::Identity(), prev, targetToSource;
	PointCloudWithNormals::Ptr reg_result = points_with_normals_src;
	reg.setMaximumIterations(2);
	for (int i = 0; i < 30; ++i)
	{
		PCL_INFO("Iteration Nr. %d.\n", i);
		//为了可视化的目的保存点云
		points_with_normals_src = reg_result;
		//估计
		reg.setInputCloud(points_with_normals_src);
		reg.align(*reg_result);
		//在每一个迭代之间累积转换
		Ti = reg.getFinalTransformation() * Ti;
		//如果这次转换和之前转换之间的差异小于阈值
		//则通过减小最大对应距离来改善程序
		if (fabs((reg.getLastIncrementalTransformation() - prev).sum()) < reg.getTransformationEpsilon())
			reg.setMaxCorrespondenceDistance(reg.getMaxCorrespondenceDistance() - 0.001);
		prev = reg.getLastIncrementalTransformation();
		//可视化当前状态
		showCloudsRight(points_with_normals_tgt, points_with_normals_src);
	}
	//
  // 得到目标点云到源点云的变换
	targetToSource = Ti.inverse();
	//
	//把目标点云转换回源框架
	pcl::transformPointCloud(*cloud_tgt, *output, targetToSource);
	p->removePointCloud("source");
	p->removePointCloud("target");
	PointCloudColorHandlerCustom<PointT> cloud_tgt_h(output, 0, 255, 0);
	PointCloudColorHandlerCustom<PointT> cloud_src_h(cloud_src, 255, 0, 0);
	p->addPointCloud(output, cloud_tgt_h, "target", vp_2);
	p->addPointCloud(cloud_src, cloud_src_h, "source", vp_2);
	PCL_INFO("Press q to continue the registration.\n");
	p->spin();
	p->removePointCloud("source");
	p->removePointCloud("target");
	//添加源点云到转换目标
	*output += *cloud_src;
	final_transform = targetToSource;
}
/* ---[ */
int main(int argc, char** argv)
{
	// 加载数据
	std::vector<PCD, Eigen::aligned_allocator<PCD> > data;
	loadData(argc, argv, data);
	//检查用户输入
	if (data.empty())
	{
		PCL_ERROR("Syntax is: %s <source.pcd> <target.pcd> [*]", argv[0]);
		PCL_ERROR("[*] - multiple files can be added. The registration results of (i, i+1) will be registered against (i+2), etc");
		PCL_INFO("Example: %s `rospack find pcl`/test/bun0.pcd `rospack find pcl`/test/bun4.pcd", argv[0]);
		return (-1);
	}
	PCL_INFO("Loaded %d datasets.", (int)data.size());
	//创建一个PCL可视化对象
	p = new pcl::visualization::PCLVisualizer(argc, argv, "Pairwise Incremental Registration example");
	p->createViewPort(0.0, 0, 0.5, 1.0, vp_1);
	p->createViewPort(0.5, 0, 1.0, 1.0, vp_2);
	PointCloud::Ptr result(new PointCloud), source, target;
	Eigen::Matrix4f GlobalTransform = Eigen::Matrix4f::Identity(), pairTransform;
	for (size_t i = 1; i < data.size(); ++i)
	{
		source = data[i - 1].cloud;
		target = data[i].cloud;
		//添加可视化数据
		showCloudsLeft(source, target);
		PointCloud::Ptr temp(new PointCloud);
		PCL_INFO("Aligning %s (%d) with %s (%d).\n", data[i - 1].f_name.c_str(), source->points.size(), data[i].f_name.c_str(), target->points.size());
		pairAlign(source, target, temp, pairTransform, true);
		//把当前的两两配对转换到全局变换
		pcl::transformPointCloud(*temp, *result, GlobalTransform);
		//update the global transform更新全局变换
		GlobalTransform = pairTransform * GlobalTransform;
		//保存配准对，转换到第一个点云框架中
		std::stringstream ss;
		ss << i << ".pcd";
		pcl::io::savePCDFile(ss.str(), *result, true);
	}
}
/* ]--- */

匹配前：

匹配后：

使用正太分布变换配准

实验数据缺失，使用替代数据结果怪异。就不往上贴啦

Summary

点云配准不十分适用于简单的双目视觉得到的点云。因为在工业领域不会给予足够的时间进行点云处理。这里也算是浅尝辄止了一下，知晓了点云配准的一些操作。
下一步根据项目需要和兴趣分解代码，熟悉代码操作。