4PCS点云配准算法实现

news2024/12/23 14:40:54

在这里插入图片描述
在这里插入图片描述

在这里插入图片描述
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述

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.coloradmin.cn/o/1883671.html

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈，一经查实，立即删除！

相关文章

PostgreSQL介绍与安装

PostgreSQL介绍与安装

一、PostgreSQL数据库介绍 1、什么是数据库&#xff1f; 数据库&#xff08;Database&#xff09;是按照数据结构来组织、存储和管理数据的仓库。每个数据库都有一个或多个不同的 API 用于创建&#xff0c;访问&#xff0c;管理&#xff0c;搜索和复制所保存的数据。 我们也…
阅读更多...
JAVA医院绩效考核管理系统源码：系统优势、系统目的、系统原则 （自主研发 功能完善 可直接上项目）

JAVA医院绩效考核管理系统源码：系统优势、系统目的、系统原则 （自主研发 功能完善 可直接上项目）

JAVA医院绩效考核管理系统源码&#xff1a;系统优势、系统目的、系统原则 &#xff08;自主研发 功能完善 可直接上项目&#xff09; 医院绩效考核系统优势 1.实现科室负责人单独考核 对科室负责人可以进行单独考核、奖金发放。 2. 科室奖金支持发放到个人 支持奖金二次分配&…
阅读更多...
同一个excel表格，为什么在有的电脑上会显示#NAME?

同一个excel表格，为什么在有的电脑上会显示#NAME?

一、哪些情况会产生#NAME?的报错 1.公式名称拼写错误 比如求和函数SUM&#xff0c;如果写成SUN就会提示#NAME&#xff1f;报错。 2.公式中的文本值未添加双引号 如下图&#xff1a; VLOOKUP(丙,A:B,2,0) 公式的计算结果会返回错误值#NAME?&#xff0c;这是因为公式中文本…
阅读更多...
GraphPad Prism生物医学数据分析软件下载安装 GraphPad Prism轻松绘制各种图表

GraphPad Prism生物医学数据分析软件下载安装 GraphPad Prism轻松绘制各种图表

Prism软件作为一款功能强大的生物医学数据分析与可视化工具&#xff0c;其绘图功能尤为突出。该软件不仅支持绘制基础的图表类型&#xff0c;如直观明了的柱状图、展示数据分布的散点图&#xff0c;以及描绘变化趋势的曲线图&#xff0c;更能应对复杂的数据呈现需求&#xff0c…
阅读更多...
7-1作业

7-1作业

1.实验目的&#xff1a;完成字符收发 led.h #ifndef __GPIO_H__ #define __GPIO_H__#include "stm32mp1xx_rcc.h" #include "stm32mp1xx_gpio.h" #include "stm32mp1xx_uart.h"//RCC,GPIO,UART初始化 void init();//字符数据发送 void set_tt…
阅读更多...
【2024 插件开发大赛】为 ONLYOFFICE 开发插件，赢取万元奖金！

【2024 插件开发大赛】为 ONLYOFFICE 开发插件，赢取万元奖金！

我们发布了 2024 插件开发大赛&#xff1a;为 ONLYOFFICE 开发适合中国用户的插件&#xff0c;赢取税前5500 – 10000元的结项奖金与证书&#xff01;阅读本文了解详情。 关于 ONLYOFFICE ONLYOFFICE 是一个国际开源项目&#xff0c;由领先的 IT 公司 Ascensio System SIA 开发…
阅读更多...
论坛万能粘贴手（可将任意文件转为文本）

论坛万能粘贴手（可将任意文件转为文本）

该软件可将任意文件转为文本。 还原为原文件的方法&#xff1a;将得到的文本粘贴到记事本&#xff0c;另存为UUE格式&#xff0c;再用压缩软件如winrar解压即可得到原文件。建议用于小软件。 下载地址&#xff1a;https://download.csdn.net/download/wgxds/89505015 使用演示…
阅读更多...
算法基础入门 - 2.栈、队列、链表

算法基础入门 - 2.栈、队列、链表

文章目录 算法基础入门第二章 栈、队列、链表2.1 队列2.2 栈2.3 纸牌游戏2.4 链表如何建立链表？1.我们需要一个头指针(head)指向链表的初始。链表还没建立时头指针head为空2.建立第一个结点3.设置刚创建的这个结点的数据域（左半）和指针域（右半）4.设置头指针，头指针可方便…
阅读更多...
构建高效的数字风控系统：应对现代网络威胁的策略与实践

构建高效的数字风控系统：应对现代网络威胁的策略与实践

文章目录 构建高效的数字风控系统&#xff1a;应对现代网络威胁的策略与实践1. 数字风控基本概念1.1 数字风控&#xff08;数字化风控&#xff09;1.2 数字风控的原理1.3 常见应用场景 2. 数字风控的必要性3. 构建高效的数字风控系统3.1 顶层设计与规划3.2 数据基础建设3.3 风险…
阅读更多...
代码随想录第38天|动态规划

代码随想录第38天|动态规划

1049. 最后一块石头的重量 II 参考 备注: 当物体容量也等同于价值时, 01背包问题的含义则是利用好最大的背包容量sum/2, 使得结果尽可能的接近或者小于 sum/2 等价: 尽可能的平分成相同的两堆, 其差则为结果, 比如 (abc)-d, (ac)-(bd) , 最终的结果是一堆减去另外一堆的和, 问…
阅读更多...
Nik Collection by DxO：摄影师的创意利器与调色宝典

Nik Collection by DxO：摄影师的创意利器与调色宝典

在数码摄影的世界里&#xff0c;后期处理是摄影师们展现创意、调整细节、提升作品质量的重要步骤。而Nik Collection by DxO作为一款由DxO公司开发的强大照片编辑插件套件&#xff0c;为摄影师们提供了一套全面的、功能丰富的工具集&#xff0c;让他们的创意得以充分发挥。 Ni…
阅读更多...
数据结构 - C/C++ - 栈

数据结构 - C/C++ - 栈

目录 结构特性 结构实现 结构容器 结构设计 顺序栈 链式栈 结构特性 栈(stack)是线性表的一种形式&#xff0c;限定仅在表的一端进行插入或者删除的操作。 栈顶 - 表中允许插入、删除的一端称为栈顶(top)&#xff0c;栈顶位置是可以发生变化的。 插入 - 进栈、入栈、压栈…
阅读更多...
10月开始，所有新来日本的外国人都必须加入公共年金体系!

10月开始，所有新来日本的外国人都必须加入公共年金体系!

为了吸引更多外国人来日本工作并为他们提供更好的养老保障&#xff0c;日本厚生劳动省最近宣布了一项新政策。 从今年10月开始&#xff0c;所有新来日本的外国人都必须加入公共年金体系。 虽然之前已经有这个要求&#xff0c;但还是有不少人没加入。 因此&#xff0c;日本年金机…
阅读更多...
每日Attention学习7——Frequency-Perception Module

每日Attention学习7——Frequency-Perception Module

模块出处 [link] [code] [ACM MM 23] Frequency Perception Network for Camouflaged Object Detection 模块名称 Frequency-Perception Module (FPM) 模块作用 获取频域信息&#xff0c;更好识别伪装对象 模块结构 模块代码 import torch import torch.nn as nn import to…
阅读更多...
Win11找不到组策略编辑器（gpedit.msc）解决

Win11找不到组策略编辑器（gpedit.msc）解决

由于需要同时连接有线网络和无线网络&#xff0c;且重启后双网络都自动连接&#xff0c;因此需要配置组策略。 但是win11找不到组策略编辑器。 灵感来源&#xff1a;Win11找不到组策略编辑器&#xff08;gpedit.msc&#xff09;解决教程 - 知乎 (zhihu.com) 在Win11中&#…
阅读更多...
基于隐马尔可夫模型的股票预测【HMM】

基于隐马尔可夫模型的股票预测【HMM】

基于机器学习方法的股票预测系列文章目录 一、基于强化学习DQN的股票预测【股票交易】 二、基于CNN的股票预测方法【卷积神经网络】 三、基于隐马尔可夫模型的股票预测【HMM】 文章目录 基于机器学习方法的股票预测系列文章目录一、HMM模型简介&#xff08;1&#xff09;前向后…
阅读更多...
Eclipse 2024最新版本分享

Eclipse 2024最新版本分享

一、软件介绍 Eclipse是一个开源的、基于Java的可扩展开发平台&#xff0c;最初由IBM公司开发&#xff0c;后于2001年贡献给开源社区&#xff0c;并由Eclipse基金会负责管理和开发。 如果在官网上下载比较慢&#xff0c;可以试试从云盘中下载&#xff0c;解压即可使用。 二、下…
阅读更多...
Flutter循序渐进==＞数据结构（列表、映射和集合）和错误处理

Flutter循序渐进==＞数据结构（列表、映射和集合）和错误处理

导言 填鸭似的教育确实不行&#xff0c;我高中时学过集合&#xff0c;不知道有什么用&#xff0c;毫无兴趣&#xff0c;等到我学了一门编程语言后&#xff0c;才发现集合真的很有用&#xff1b;可以去重&#xff0c;可以看你有我没有的&#xff0c;可以看我有你没有的&#xf…
阅读更多...
获取onnx模型输入输出结构信息的3种方式：ONNX、onnxruntime、netron

获取onnx模型输入输出结构信息的3种方式：ONNX、onnxruntime、netron

《博主简介》 小伙伴们好&#xff0c;我是阿旭。专注于人工智能、AIGC、python、计算机视觉相关分享研究。 ✌更多学习资源&#xff0c;可关注公-仲-hao:【阿旭算法与机器学习】&#xff0c;共同学习交流~ &#x1f44d;感谢小伙伴们点赞、关注&#xff01; 《------往期经典推…
阅读更多...
机器人控制系列教程之任务空间运动控制器搭建(2)

机器人控制系列教程之任务空间运动控制器搭建(2)

Simulink中的实例 推文《机器人控制系列教程之任务空间运动控制器搭建(1)》中&#xff0c;我们详细的讲解了Simulink中的taskSpaceMotionModel模块&#xff0c;实现的方式可以按照如下的步骤。 可以控制器模型替换为taskSpaceMotionModel模块后&#xff0c;该模块的输入分别为…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023