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原理与应用

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.mfbz.cn/a/763234.html

如若内容造成侵权/违法违规/事实不符，请联系我们进行投诉反馈qq邮箱809451989@qq.com，一经查实，立即删除！

相关文章

经典案列|淘宝商品数据爬取与分析

经典案列|淘宝商品数据爬取与分析

商品详情页 API接口测试代码 -- 请求示例 url 默认请求参数已经URL编码处理 curl -i "https://api-服务器.cn/taobao/item_get/?key<您自己的apiKey>&secret<您自己的apiSecret>&num_iid45887133725&is_promotion1" API测试页 商品详情页返…
阅读更多...
解析 Ferret-UI：多模态大模型在移动用户界面理解中的应用

解析 Ferret-UI：多模态大模型在移动用户界面理解中的应用

移动应用的爆炸性增长&#xff0c;用户界面&#xff08;UI&#xff09;的设计越来越复杂&#xff0c;功能也越来越丰富。但现有的多模态大模型&#xff08;MLLMs&#xff09;在理解用户界面时存在局限&#xff0c;尤其是在处理具有特定分辨率和包含众多小型对象&#xff08;如图…
阅读更多...
重生之算法刷题之路之链表初探（三）

重生之算法刷题之路之链表初探（三）

算法刷题之路之链表初探&#xff08;三&#xff09; 今天来学习的算法题是leecode2链表相加&#xff0c;是一道简单的入门题&#xff0c;但是原子在做的时候其实是有些抓耳挠腮&#xff0c;看了官解之后才恍然大悟&#xff01; 条件 项目解释 有题目可以知道&#xff0c;我们需…
阅读更多...
EAGLE-2：一种高效无损的推测性采样方法，提升LLM的推理速度。

EAGLE-2：一种高效无损的推测性采样方法，提升LLM的推理速度。

欢迎关注我的公众号&#xff1a;Halo咯咯 01。概述 北京大学的研究人员联合微软研究院、滑铁卢大学以及Vector研究所共同推出了EAGLE-2&#xff0c;这是一种利用上下文感知的动态草图树来增强推测性采样的方法。EAGLE-2在先前的EAGLE方法基础上进行了改进&#xff0c;不仅显著…
阅读更多...
运维锅总详解RocketMQ

运维锅总详解RocketMQ

本文尝试从Apache RocketMQ的简介、主要组件及其作用、3种部署模式、Controller集群模式工作流程、最佳实践等方面对其进行详细分析。希望对您有所帮助&#xff01; 一、Apache RocketMQ 简介 Apache RocketMQ 是一个开源的分布式消息中间件&#xff0c;由阿里巴巴集团开发并…
阅读更多...
uniapp微信小程序电子签名

uniapp微信小程序电子签名

先上效果图&#xff0c;不满意可以直接关闭这页签 新建成单独的组件&#xff0c;然后具体功能引入&#xff0c;具体功能点击签名按钮&#xff0c;把当前功能页面用样式隐藏掉&#xff0c;v-show和v-if也行&#xff0c;然后再把这个组件显示出来。 【签名-撤销】原理是之前绘画时…
阅读更多...
JVM与Java体系结构

JVM与Java体系结构

1.JVM与Java体系结构 1.1. 前言 作为Java工程师的你曾被伤害过吗&#xff1f;你是否也遇到过这些问题&#xff1f; 运行着的线上系统突然卡死&#xff0c;系统无法访问&#xff0c;甚至直接OOM想解决线上JVM GC问题&#xff0c;但却无从下手新项目上线&#xff0c;对各种JVM…
阅读更多...
运算放大器重要参数详解

运算放大器重要参数详解

运算放大器是一种用于放大电压并实现信号处理和放大的电子设备。以下是运算放大器的一些重要参数及其详解： 增益（Gain）： 定义：增益是运算放大器输出电压与输入电压之比，表示运算放大器在输入信号上的放大倍数。重要性：增益决定了信号的放大程度，是运算放大器的基本功能…
阅读更多...
python基础语法 004-2流程控制- for遍历

python基础语法 004-2流程控制- for遍历

1 遍历 1.1 什么是遍历&#xff1f; 可以遍历的元素&#xff1a;字符串、列表、元组、字典、集合字符串是可以进行for 循环。&#xff08;容器对象&#xff0c;序列&#xff09;可迭代对象iterable 例子&#xff1a; 1 &#xff09;、for遍历字符串&#xff1a; name xiao…
阅读更多...
某安全公司DDoS攻击防御2024年6月报告

某安全公司DDoS攻击防御2024年6月报告

引言&#xff1a; 在2024年6月&#xff0c;网络空间的安全挑战汹涌澎湃。分布式拒绝服务&#xff08;DDoS&#xff09;攻击频发&#xff0c;针对云服务、金融科技及在线教育平台的精密打击凸显出当前网络威胁环境的严峻性。 某安全公司作为网络安全防护的中坚力量&#xff0c…
阅读更多...
中电金信：金Gien乐道 | 6月热门新闻盘点 回顾这一月的焦点事件

中电金信：金Gien乐道 | 6月热门新闻盘点 回顾这一月的焦点事件

“以检之力 e企守护”——上海市检一分院与中电金信开展联学联建 6月24日&#xff0c;上海市人民检察院第一分院与中电金信数字科技集团股份有限公司联合开展“以检之力 e企守护”联学联建活动。双方共同参观了全国检察机关证券期货犯罪办案基地和重大职务犯罪案件办理&#xf…
阅读更多...
【办公类-21-18】20240701 养老护理员初级选择题488，制作PyQt5图形界面GUI

【办公类-21-18】20240701 养老护理员初级选择题488，制作PyQt5图形界面GUI

背景需求&#xff1a; 6月16日育婴师高级考完了。运气好&#xff0c;抽到的是”护理患腹泻的幼儿”&#xff0c;“晨检与家长沟通”&#xff0c;“4个月婴儿喂蛋黄”&#xff0c;“21个月食谱”&#xff0c;都是我背过的题目&#xff08;没有抽到感统&#xff09; 于是一放假&…
阅读更多...
【吴恩达机器学习-week2】可选实验：特征工程和多项式回归【Feature Engineering and Polynomial Regression】

【吴恩达机器学习-week2】可选实验：特征工程和多项式回归【Feature Engineering and Polynomial Regression】

支持我的工作 &#x1f389; 如果您想看到更详细、排版更精美的该系列文章&#xff0c;请访问&#xff1a;2022吴恩达机器学习Deeplearning.ai课程作业 可选实验&#xff1a;特征工程和多项式回归 目标 在本次实验中&#xff0c;你将&#xff1a;探索特征工程和多项式回归&a…
阅读更多...
PTFE铲子聚四氟乙烯物料特氟龙铲粉料铲耐酸碱塑料药铲

PTFE铲子聚四氟乙烯物料特氟龙铲粉料铲耐酸碱塑料药铲

四氟铲子主要适用于药厂、药企、医药行业专用&#xff0c;用于粉末状及颗粒物状样品的铲取和搅匀等。因为粉料物料对铲子材质要求无污染、本底值低&#xff0c;所以四氟材质成为选择。 其主要特点有&#xff1a; 1.外观纯白色。 2.耐高低温性&#xff1a;可使用温度-200℃&am…
阅读更多...
Django学习第一天

Django学习第一天

Django安装&#xff1a; pip install Django -i https://mirrors.aliyun.com/pypi/simple/ 在需要创建文件的文件目录下写这个命令 django-admin startproject mysite 注意&#xff1a;C:\Users\Administrator\AppData\Local\Programs\Python\Python311\Scripts已加入环境变…
阅读更多...
2024/6/30 英语每日一段

2024/6/30 英语每日一段

Years of economic and political turbulence have brought stagnation.“In a world where there is more risk and uncertainty, people become reluctant to voluntarily move jobs and find better jobs,” says Manning. At the same time, businesses have cut back on i…
阅读更多...
STM32实现串口发送字符和字符串

STM32实现串口发送字符和字符串

效果&#xff1a;(字符串的收发还是有问题&#xff0c;只能实现预置字符串的发送&#xff0c;无法实现输入字符串回响) 代码: #include "uart4.h"void hal_uart4_init() {// GPIOB使能RCC->MP_AHB4ENSETR | (0x1 << 1);// GPIOG使能RCC->MP_AHB4ENSETR …
阅读更多...
VUE2及其生态查漏补缺

VUE2及其生态查漏补缺

1、数据代理概括 数据代理过程相当于是进行了 vm 代理 vm_data中的属性&#xff0c;vm._data 是与 我们vue文件中写的 data是全等的 //创建Vue实例let data { //data中用于存储数据&#xff0c;数据供el所指定的容器去使用&#xff0c;值我们暂时先写成一个对象。name:atguig…
阅读更多...
自然语言处理基本知识(1)

自然语言处理基本知识(1)

一 分词基础 NLP:搭建了计算机语言和人类语言之间的转换 1 精确分词&#xff0c;试图将句子最精确的分开&#xff0c;适合文本分析 >>> import jieba >>> content "工信处女干事每月经过下属科室" >>> jieba.cut(content,cut_all …
阅读更多...
妙笔 WonderPen 专业版会员值得购买吗？

妙笔 WonderPen 专业版会员值得购买吗？

在信息爆炸的时代&#xff0c;写作已经是一项重要技能。无论是学生、职场人士还是自由职业者&#xff0c;都经常需要写点东西。 一个好的工具能让写作成为享受&#xff0c;今天就为大家推荐专业写作工具&#xff1a;妙笔。为长文写作设计&#xff0c;且有云服务功能&#xff0…
阅读更多...
最新文章

Copyright @ 2022~2023