4PCS点云配准算法实现

4PCS点云配准算法的C++实现如下：

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/common.h>
#include <pcl/common/distances.h>
#include <pcl/common/transforms.h>
#include <pcl/search/kdtree.h> 


struct Points4
{
	pcl::PointXYZ p1;
	pcl::PointXYZ p2;
	pcl::PointXYZ p3;
	pcl::PointXYZ p4;
};


int compute_LCP(const pcl::PointCloud<pcl::PointXYZ>& cloud, pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree, float radius)
{
	std::vector<int>index(1);
	std::vector<float>distance(1);
	int count = 0;
	for (size_t i = 0; i < cloud.size(); i++)
	{
		kdtree->nearestKSearch(cloud.points[i], 1, index, distance);
		if (distance[0] < radius)
			count = count + 1;
	}
	return count;
}


int main(int argc, char** argv)
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr pcs_cloud(new pcl::PointCloud<pcl::PointXYZ>);

	pcl::io::loadPCDFile("bunny1.pcd", *source_cloud);
	pcl::io::loadPCDFile("bunny2.pcd", *target_cloud);

	pcl::PointXYZ min_pt, max_pt;
	pcl::getMinMax3D(*source_cloud, min_pt, max_pt);
	float d_max = pcl::euclideanDistance(min_pt, max_pt);

	//srand(time(0));
	int iters = 100;
	float s_max = 0;
	float f = 0.5;
	float ff = 0.1;
	float delta = 0.0001;
	int index1 = -1, index2 = -1, index3 = -1, index4 = -1;
	for (size_t i = 0; i < iters; i++)
	{
		int n1 = rand() % source_cloud->size(), n2 = rand() % source_cloud->size(), n3 = rand() % source_cloud->size();
		pcl::PointXYZ p1 = source_cloud->points[n1], p2 = source_cloud->points[n2], p3 = source_cloud->points[n3];
		Eigen::Vector3f a(p1.x - p2.x, p1.y - p2.y, p1.z - p2.z);
		Eigen::Vector3f b(p1.x - p3.x, p1.y - p3.y, p1.z - p3.z);
		float s = 0.5 * sqrt(pow(a.y() * b.z() - b.y() * a.z(), 2) + pow(a.x() * b.z() - b.x() * a.z(), 2) + pow(a.x() * b.y() - b.x() * a.y(), 2));
		if (s > s_max && pcl::euclideanDistance(source_cloud->points[n1], source_cloud->points[n2]) < f* d_max 
			&&	pcl::euclideanDistance(source_cloud->points[n1], source_cloud->points[n3]) < f * d_max)
		{
			index1 = n1;
			index2 = n2;
			index3 = n3;
		}
	}
	if (index1 == -1 || index2 == -1 || index3 == -1)
	{
		std::cout << "find three points error!" << std::endl;
		return -1;
	}

	pcl::PointXYZ p1 = source_cloud->points[index1], p2 = source_cloud->points[index2], p3 = source_cloud->points[index3];
	float A = (p2.y - p1.y) * (p3.z - p1.z) - (p2.z - p1.z) * (p3.y - p1.y);	
	float B = (p2.z - p1.z) * (p3.x - p1.x) - (p2.x - p1.x) * (p3.z - p1.z);
	float C = (p2.x - p1.x) * (p3.y - p1.y) - (p2.y - p1.y) * (p3.x - p1.x);
	float D = -(A * p1.x + B * p1.y + C * p1.z);

	for (size_t i = 0; i < source_cloud->size(); i++)
	{
		pcl::PointXYZ p = source_cloud->points[i];
		float d = fabs(A * p.x + B * p.y + C * p.z + D) / sqrt(A * A + B * B + C * C);
		bool flag = (pcl::euclideanDistance(p, p1) < f * d_max && pcl::euclideanDistance(p, p2) < f * d_max && pcl::euclideanDistance(p, p3) < f * d_max
			&& pcl::euclideanDistance(p, p1) > ff * d_max && pcl::euclideanDistance(p, p2) > ff * d_max && pcl::euclideanDistance(p, p3) > ff * d_max);
		if (d < delta * d_max && flag)
		{
			index4 = i;
		}
	}

	if (index4 == -1)
	{
		std::cout << "find fouth point error!" << std::endl;
		return -1;
	}
	pcl::PointXYZ p4 = source_cloud->points[index4];

	pcl::PointCloud<pcl::PointXYZ>::Ptr four_points(new pcl::PointCloud<pcl::PointXYZ>);
	four_points->push_back(p1);
	four_points->push_back(p2);
	four_points->push_back(p3);
	four_points->push_back(p4);
	pcl::io::savePCDFile("four_points.pcd", *four_points);

	Eigen::VectorXf line_a(6), line_b(6);
	line_a << p1.x, p1.y, p1.z, p1.x - p2.x, p1.y - p2.y, p1.z - p2.z;
	line_b << p3.x, p3.y, p3.z, p3.x - p4.x, p3.y - p4.y, p3.z - p4.z;

	Eigen::Vector4f pt1_seg, pt2_seg;
	pcl::lineToLineSegment(line_a, line_b, pt1_seg, pt2_seg);
	pcl::PointXYZ p5((pt1_seg[0]+ pt2_seg[0])/2, (pt1_seg[1] + pt2_seg[1]) / 2, (pt1_seg[2] + pt2_seg[2]) / 2);

	float d1 = pcl::euclideanDistance(p1, p2);	//d1=|b1-b2|
	float d2 = pcl::euclideanDistance(p3, p4);	//d2=|b3-b4|
	float r1 = pcl::euclideanDistance(p1, p5) / d1;	//r1=|b1-e| / |b1-b2|
	float r2 = pcl::euclideanDistance(p3, p5) / d2;	//r2=|b3-e| / |b3-b4|
	std::cout << d1 << " " << d2 << " " << r1 << " " << r2 <<  std::endl;

	std::vector<std::pair<pcl::PointXYZ, pcl::PointXYZ>> R1, R2;
	for (size_t i = 0; i < target_cloud->size(); i++)
	{
		pcl::PointXYZ pt1 = target_cloud->points[i];
		for (size_t j = i + 1; j < target_cloud->size(); j++)
		{
			pcl::PointXYZ pt2 = target_cloud->points[j];
			if (pcl::euclideanDistance(pt1, pt2) > d1 * (1 - delta) && pcl::euclideanDistance(pt1, pt2) < d1 * (1 + delta))
			{
				R1.push_back(std::pair<pcl::PointXYZ, pcl::PointXYZ>(pt1, pt2));
			}
			else if (pcl::euclideanDistance(pt1, pt2) > d2 * (1 - delta) && pcl::euclideanDistance(pt1, pt2) < d2 * (1 + delta))
			{
				R2.push_back(std::pair<pcl::PointXYZ, pcl::PointXYZ>(pt1, pt2));
			}
		}
	}
	std::cout << R1.size() << " " << R2.size() << std::endl;

	std::vector<std::pair<float, std::vector<pcl::PointXYZ>>> Map1, Map2;
	pcl::PointCloud<pcl::PointXYZ>::Ptr pts1(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr pts2(new pcl::PointCloud<pcl::PointXYZ>);
	for (auto r : R1)
	{
		pcl::PointXYZ p1, p2; p3;
		p1 = r.first;
		p2 = r.second;
		p3 = pcl::PointXYZ(p1.x + r1 * (p2.x - p1.x), p1.y + r1 * (p2.y - p1.y), p1.z + r1 * (p2.z - p1.z));
		Map1.push_back(std::pair<float, std::vector<pcl::PointXYZ>>(r1, { p1, p2, p3 }));
		pts1->push_back(p3);
	}
	for (auto r : R2)
	{
		pcl::PointXYZ p1, p2; p3;
		p1 = r.first;
		p2 = r.second;
		p3 = pcl::PointXYZ(p1.x + r2 * (p2.x - p1.x), p1.y + r2 * (p2.y - p1.y), p1.z + r2 * (p2.z - p1.z));
		Map2.push_back(std::pair<float, std::vector<pcl::PointXYZ>>(r2, { p1, p2, p3 }));
		pts2->push_back(p3);
	}

	pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
	kdtree->setInputCloud(pts1);

	std::vector<Points4> Uvec;
	for (size_t i = 0; i < pts2->size(); i++)
	{
		std::vector<int> indices;
		std::vector<float> distance;
		if (kdtree->radiusSearch(pts2->points[i], 0.001 * d_max, indices, distance) > 0)
		{
			for (int indice: indices)
			{
				Points4 points4;
				points4.p1 = Map1[indice].second[0];
				points4.p2 = Map1[indice].second[1];
				points4.p3 = Map2[i].second[0];
				points4.p4 = Map2[i].second[1];
				Uvec.push_back(points4);

				//points4.p1 = Map1[indice].second[1];
				//points4.p2 = Map1[indice].second[0];
				//points4.p3 = Map2[i].second[0];
				//points4.p4 = Map2[i].second[1];
				//Uvec.push_back(points4);

				//points4.p1 = Map1[indice].second[0];
				//points4.p2 = Map1[indice].second[1];
				//points4.p3 = Map2[i].second[1];
				//points4.p4 = Map2[i].second[0];
				//Uvec.push_back(points4);

				//points4.p1 = Map1[indice].second[1];
				//points4.p2 = Map1[indice].second[0];
				//points4.p3 = Map2[i].second[1];
				//points4.p4 = Map2[i].second[0];
				//Uvec.push_back(points4);
			}
		}
	}
	std::cout << Uvec.size() << std::endl;

	int max_count = 0;
	Eigen::Matrix4f transformation;
	kdtree->setInputCloud(target_cloud);
	for (int i = 0; i < Uvec.size(); i++)
	{
		//if (i % 1000 == 0 && i> 0)	
		//std::cout << i << std::endl;

		pcl::PointCloud<pcl::PointXYZ>::Ptr temp(new pcl::PointCloud<pcl::PointXYZ>);
		temp->resize(4);
		temp->points[0] = Uvec[i].p1;
		temp->points[1] = Uvec[i].p2;
		temp->points[2] = Uvec[i].p3;
		temp->points[3] = Uvec[i].p4;

		Eigen::Vector4f pts1_centroid, pts2_centroid;
		pcl::compute3DCentroid(*four_points, pts1_centroid);
		pcl::compute3DCentroid(*temp, pts2_centroid);

		Eigen::MatrixXf pts1_cloud, pts2_cloud;
		pcl::demeanPointCloud(*four_points, pts1_centroid, pts1_cloud);
		pcl::demeanPointCloud(*temp, pts2_centroid, pts2_cloud);

		Eigen::MatrixXf W = (pts1_cloud * pts2_cloud.transpose()).topLeftCorner(3, 3);
		Eigen::JacobiSVD<Eigen::MatrixXf> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
		Eigen::Matrix3f V = svd.matrixV(), U = svd.matrixU();
		if (U.determinant() * V.determinant() < 0)
		{
			for (int x = 0; x < 3; ++x)
				V(x, 2) *= -1;
		}
		Eigen::Matrix3f R = V * U.transpose();
		Eigen::Vector3f T = pts2_centroid.head(3) - R * pts1_centroid.head(3);
		Eigen::Matrix4f H;
		H << R, T, 0, 0, 0, 1;
		//std::cout << H << std::endl;

		pcl::transformPointCloud(*source_cloud, *pcs_cloud, H);
		int count = compute_LCP(*pcs_cloud, kdtree, 0.0001 * d_max);
		if (count > max_count)
		{
			std::cout << count << std::endl;
			std::cout << H << std::endl;
			max_count = count;
			transformation = H;
		}
	}

	pcl::transformPointCloud(*source_cloud, *pcs_cloud, transformation);
	pcl::io::savePCDFile("result.pcd", *pcs_cloud);

	return 0;
}

算法的流程基本上和原理能对得上，但是实现过程中发现该算法结果不太稳定。可能实现有些问题吧，希望有懂的大神指出来（逃~）

参考：
3D点云配准算法-4PCS（4点全等集配准算法）
点云配准–4PCS原理与应用