4PCS、Super4PCS简介

和前面的PFH和FPFH一样，super4PCS也是在4PCS的基础上进行了改良，4PCS和super4PCS都是通过找出目标点云和原始点云中对应的两组点进行旋转平移求解出T，然后在众多的候选T中选择一组最大重合的T，具有随机性。4PCS和super4PCS通过对应的同一平面四点组仿射不变性去找。该方法适用于重叠区域较小或者重叠区域发生较大变化场景点云配准。

二、4PCS简介

给定两个点云P,Q，首先在点云Q中随机选择3个点，再根据点云P,Q的重叠比例选择距离3个点足够远的第四个近似共面点组成共面四点基B。根据仿射不变比从点云P中提取出所有在一定距离d内可能与B相符合的4点集合U，计算B和U的变换矩阵。

2.1 计算比例因子

计算四点的比例因子r1,r2，两比例因子在点云旋转和平移变换中具有仿射不变性，同时就是依靠他的仿射不变性，对点云进行配准。这是比例因子计算方法：
r1 = ||a − e|| /|| a − b||
r2 = ||c − e|| /|| c − d||

2.2 寻找对应的一致全等四点

计算所有Q中两点云的交点坐标，e1约等于e2表示寻找到对应的一致全等四点。
对于Q中的每对点 q1, q2，计算两个中间点：
e1 = q1 + r1(q2 - q1)
e2 = q1 + r2(q2 - q1)

在图中，q1和q4的连线的中间点与q3和q5的连线的中间点相交，说明找到了对应的一致全等四点，将找到的所以对应的一致全等四点组成一个集合。

2.3 寻找最优全等四点对

在全等四点集中寻找最优全等四点对（即计算旋转和平移的参数），根据最优全等四点对计算两个点云的变换矩阵。然后运用到全局点云转化，完成粗配准。

任何两对点，如果它们的中间点（一个由 r1 得到，另一个由 r2 得到）重合。选定四个特征点后，算法会计算它们之间的相对关系，如距离和角度。然后，在另一个点云中寻找与这四个特征点具有类似关系的点组。通过找到匹配的点组，算法可以估计点云之间的变换矩阵，从而实现点云的配准。

三、Super4PCS简介

3.1 排除无效点

记录两直线相交的角度，在点集中寻找匹配时，额外计算两条直线相交的角度是否在一定范围内，从而排除一些无效的匹配。

3.2 在一定范围内找匹配对

类似于在球面上画圆，以P中的一个点为球心，以原来在Q中找到的两条线段的长度为半径去画球，在球面内的点是满足要求的。

四、总结

4.1 代码

注意代码中，导入的是带法向量的点云。同时此代码需要用cmake配置环境。

#include <iostream>
#include <pcl/registration/ia_fpcs.h>
#include <gr/algorithms/match4pcsBase.h>
#include <gr/algorithms/FunctorSuper4pcs.h>
#include <pcl/registration/super4pcs.h>
#include <gr/algorithms/match4pcsBase.h>
#include <gr/algorithms/FunctorSuper4pcs.h>
#include <pcl/registration/super4pcs.h>
#include <pcl/point_types.h>
#include <pcl/io/ply_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/registration/ia_kfpcs.h>
#include <time.h>
#include <boost/thread/thread.hpp>
#include <pcl/console/time.h>   // 控制台计算时间
#include <fstream>

using namespace std;

int main(int argc, char** argv)
{
	pcl::console::TicToc time;
	// -----------------加载目标点云---------------------
	pcl::PointCloud<pcl::PointNormal>::Ptr target_cloud(new pcl::PointCloud<pcl::PointNormal>);
	if (pcl::io::loadPLYFile<pcl::PointNormal>("C:\\Users\\Administrator\\Desktop\\data2\\target.ply", *target_cloud) == -1)
	{
		PCL_ERROR("读取目标点云失败 \n");
		return (-1);
	}
	cout << "从目标点云中读取 " << target_cloud->size() << " 个点" << endl;

	// ------------------加载源点云---------------------
	pcl::PointCloud<pcl::PointNormal>::Ptr source_cloud(new pcl::PointCloud<pcl::PointNormal>);
	if (pcl::io::loadPLYFile<pcl::PointNormal>("C:\\Users\\Administrator\\Desktop\\data2\\source.ply", *source_cloud) == -1)
	{
		PCL_ERROR("读取源标点云失败 \n");
		return (-1);
	}
	cout << "从源点云中读取 " << source_cloud->size() << " 个点" << endl;
	time.tic();
	//--------------初始化S4PCS配准对象-------------------
	pcl::Super4PCS<pcl::PointNormal, pcl::PointNormal>s4pcs;
	s4pcs.setInputSource(source_cloud);  // 源点云
	s4pcs.setInputTarget(target_cloud);  // 目标点云
	s4pcs.setOverlap(0.3);         // 设置源和目标之间的近似重叠度。0.7
	s4pcs.setDelta(1.5);                // 设置常数因子delta，用于对内部计算的参数进行加权。
	s4pcs.setMaxTimeSeconds(500);  // 设置最大计算时间(以秒为单位)。
	s4pcs.setSampleSize(5000);       // 设置验证配准效果时要使用的采样点数量 200 1000 5000
	pcl::PointCloud<pcl::PointNormal>::Ptr pcs(new pcl::PointCloud<pcl::PointNormal>);
	s4pcs.align(*pcs);                   // 计算变换矩阵

	double overlap = s4pcs.getOverlap(); // 假设存在一个名为 getOverlap 的方法用于获取重叠度
	cout << "Overlap value: " << overlap << endl;

	cout << "s4pcs配准用时： " << time.toc() / 1000 << " s" << endl;
	cout << "得分:" << s4pcs.getFitnessScore() << endl;
	cout << "变换矩阵：\n" << s4pcs.getFinalTransformation() << endl;
	std::ofstream outputFile("C:\\Users\\Administrator\\Desktop\\RT.txt");
	if (outputFile.is_open())
	{
		outputFile << s4pcs.getFinalTransformation() << std::endl;
		outputFile.close();
	}
	else
	{
		std::cerr << "Failed to open output file" << std::endl;
		return -1;
	}
	// 使用创建的变换对为输入点云进行变换
	//pcl::PointCloud<pcl::PointXYZ>::Ptr source_tr(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::transformPointCloud(*source_cloud, *pcs, s4pcs.getFinalTransformation());
	//pcl::transformPointCloud(*source_cloud, *source_tr, s4pcs.getFinalTransformation());
	// 保存转换后的源点云作为最终的变换输出
	/*pcl::io::savePLYFileASCII("D:\\Multi-View Registration\\Multi-View Registration\\data\\U3M_1\\data_2\\Super4pcs_trans.ply", *pcs);*/

	// 初始化点云可视化对象
	boost::shared_ptr<pcl::visualization::PCLVisualizer>viewer(new pcl::visualization::PCLVisualizer("显示点云"));
	viewer->setBackgroundColor(0, 0, 0);  //设置背景颜色为黑色

	// 对目标点云着色可视化 (red).
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointNormal>target_color(target_cloud, 255, 0, 0);
	viewer->addPointCloud<pcl::PointNormal>(target_cloud, target_color, "target cloud");
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "target cloud");
	// 对源点云着色可视化 (blue).
	//pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>input_color(source_cloud, 0, 0, 255);
	//viewer->addPointCloud<pcl::PointXYZ>(source_cloud, input_color, "input cloud");
	//viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "input cloud");
	// 对转换后的源点云着色 (green)可视化.
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointNormal>output_color(pcs, 0, 255, 0);
	viewer->addPointCloud<pcl::PointNormal>(pcs, output_color, "output cloud");
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "output cloud");
	// 等待直到可视化窗口关闭
	while (!viewer->wasStopped())
	{
		viewer->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(1000));
	}

	return (0);
}