PCL 4PCS点云粗配准（Registration_FPCS）

4PCS用于点云粗配准，ICP用于粗配准之后的精配准。

本文算法原理参考于：3D点云配准算法-4PCS（4点全等集配准算法）

1.4PCS算法原理

核心思想：使用四个近似共面点来描述两个三维点云之间的刚性变换关系，因此由称”四点匹配算法“。即不停迭代计算源点云Q的四点集Qa，与目标点云P中与Qa重叠度最高的四点集Pb，二者之间的最优变换矩阵T，得到粗配准的变换矩阵。

如下图：

1.点云四点集的确定

（1）选择三个共面点

1）在源点云中随机选择三个点，要求这三点所构成的三角形面积尽量的大（三点确定一个平面，向量叉积可以计算面积），但是三点间的距离不能超过一定的阈值上限，该上限由两片点云的给定重叠率 f 确定。

2）因为三点之间距离越大，配准的鲁棒性越高；但距离过大，三点均在两点云的重叠区域之外了，配准效果又不好。因此需要在满足上限的基础之上，尽可能保证大的三级形面积。

3）若没有给定点云重叠率f，则可以进行f=1.0,0.75,0.5...重叠率递减测试，选择最优变换。

（2）选择第四个共面点

1）确定三点后，遍历源点云中所有的点，对每一个点进行计算验证选择最优的第四点。

2）第四点需要与其他三点组成的平面尽可能的共面（即不强制要求四点共面，但第四点到其他三点平面的距离尽可能小），并且第四点与其他三点的距离也要满足距离阈值范围（不能太近不能太远）。

2.4PCS计算

计算其四点构成的两线段交点的变换不变比，根据不变比在目标点云中遍历搜索对应的满足该约束所有四点集，得到的四点集，即近似全等四点集。

具体流程：

（1）选取源点云四点集

1）在源点云P中，使用上述的四点集的选择方法随机选择一个四点集B={b1,b2,b3,b4}，其中(b1,b2)确定线段1，(b3,b4)确定线段2。

2）计算不变量d1=|b1-b2|，d2=|b3-b4|（约束1），不变比r1=|b1-e| / |b1-b2|，r2=|b3-e| / |b3-b4|（约束2）。注意因为四点不一定共面，两条线段也不一定相交，所以可以使用连接两个线段的最近点的中心点作为“交点”。

（2）遍历目标点云点对，筛选得到目标点云中的点对集合。

1）在目标点云Q中，遍历所有的点对，筛选满足约束1（允许有一定的误差）的点对集合R1，R2。其表示为：

计算R1 ={ (pi, pj) | pi, pj ∈ Q) }，使|| pi - pj ||∈ [ d1- δ,d1+δ ].

计算R2 ={ (pi, pj) | pi, pj ∈ Q) }，使|| pi - pj ||∈ [ d2- δ,d2+δ ].

（3）构建映射

1）遍历点对集合R1中的所有点对元素，计算其线段上满足不变比r1的目标交点。

2）所有计算结果e存入搜索树ANN Tree（近似最邻近搜索树，最常见的是K-D Tree算法），并构建相应的映射 

（4）求解近似全等四点集

1）遍历点对集合R2中的所有点对元素，计算其线段上满足不变比r2的目标交点，并构建相应的映射。

2）遍历所有的点，在之前构建的ANN Tree中搜索可接受误差范围内的重合点，若可找到则说明能在Q中找到一个对应的近似全等四点集。最终求得所有的近似全等四点集

（5）得到本次迭代最优变换矩阵T

1）遍历所有的近似全等四点集，对每一个，通过最小二乘法计算其与B的对应变换矩阵。

2）使用该变换矩阵对源点云P进行变换得到P'，统计P'与Q中的最大公共点集（LCP），记max(LCP)的变换矩阵为本次迭代的最优变换矩阵T并保留。

（6）不断迭代这个过程，记录最优的变换。迭代结束后得到的变换矩阵即为最优变换矩阵。

2.关键函数

（1）设置源和目标之间的近似重叠度

registration.setApproxOverlap(0.5)

（2）设置常数因子delta，用于对内部计算的参数进行加权

registration.setDelta(0.01)

（3）设置验证配准效果时要使用的采样点数量

registration.setNumberOfSamples(100)

3.完整代码

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/ia_fpcs.h>
#include <boost/thread/thread.hpp>
#include <pcl/visualization/pcl_visualizer.h>

int main()
{
	/****************FPCS配准********************/
	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/B30.pcd", *source);
	pcl::io::loadPCDFile("D:/code/csdn/data/B31.pcd", *target);
	pcl::PointCloud<pcl::PointXYZ>::Ptr result(new pcl::PointCloud<pcl::PointXYZ>);			// 目标点云转换后的结果

	// FPCS配准
	pcl::registration::FPCSInitialAlignment<pcl::PointXYZ, pcl::PointXYZ> registration;
	registration.setInputSource(source);				// 源点云
	registration.setInputTarget(target);				// 目标点云
	registration.setApproxOverlap(0.5);					// 设置源和目标之间的近似重叠度。
	registration.setDelta(0.01);						// 设置常数因子delta，用于对内部计算的参数进行加权
	registration.setNumberOfSamples(100);				// 设置验证配准效果时要使用的采样点数量
	registration.align(*result);

	/****************展示********************/
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("FPCS"));
	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.结果展示