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;
}