PCL拟合并绘制平面(二)

使用RANSAC拟合点云平面

  • 1、C++实现
  • 2、效果图

普通的点云平面拟合方式在一般情况下可以得到较好的平面拟合效果，但是容易出现平面拟合错误或是拟合的平面不是最优的情况。此时就需要根据自己的实际使用情况，调整平面拟合的迭代次数以及收敛条件。

使用RANSAC迭代的方式，获取所有迭代过程中的最优平面，虽然速度上不如普通的平面拟合方式，但是准确度有一定的提升。下面是具体实现的方式：

1、C++实现

随机处理函数：

	//随机处理static bool m_already_seeded = false;inline void SeedRand(int seed){srand(seed);}inline void SeedRandOnce(int seed){//if (!m_already_seeded)//{SeedRand(seed);m_already_seeded = true;//}}inline int RandomInt(int min, int max){int d = max - min + 1;return int(((double)rand() / ((double)RAND_MAX + 1.0)) * d) + min;}

主要函数实现部分：

	//部分用到的参数的初始值 mParams.VoxelSize=3.0 mParams.MaxModelFitIterations=2000//mParams.SegDistanceThreshold=0.3//点云的单位是mmvoid PlaneFittingRansac(PointCloudT::Ptr cloudsource, PointCloudT::Ptr cloudfiltered, PointCloudT::Ptr cloudseg, pcl::ModelCoefficients::Ptr coefficients){//点云下采样PointCloudT::Ptr cloudDownSampling;cloudDownSampling.reset(new(PointCloudT));if (cloudsource->points.size() > 0 && cloudsource->points.size() < 20000){cloudDownSampling = cloudsource;}else if (cloudsource->points.size() > 20000 && cloudsource->points.size() < 60000){mParams.VoxelSize = 1.0;PointCloudVoxelGrid(cloudsource, cloudDownSampling, mParams.VoxelSize);}else if (cloudsource->points.size() > 60000 && cloudsource->points.size() < 100000){mParams.VoxelSize = 2.0;PointCloudVoxelGrid(cloudsource, cloudDownSampling, mParams.VoxelSize);}else{PointCloudVoxelGrid(cloudsource, cloudDownSampling, mParams.VoxelSize);}int PointsNum = 6;std::vector<std::vector<size_t>> mvSets = std::vector<std::vector<size_t>>(mParams.MaxModelFitIterations, std::vector<size_t>(PointsNum, 0));//点的对数const int N = cloudDownSampling->points.size();//新建一个容器vAllIndices存储点云索引，并预分配空间std::vector<size_t> vAllIndices;vAllIndices.reserve(N);//在RANSAC的某次迭代中，还可以被抽取来作为数据样本的索引，所以这里起的名字叫做可用的索引std::vector<size_t> vAvailableIndices;//初始化所有特征点对的索引，索引值0到N-1for (int i = 0; i < N; i++)vAllIndices.push_back(i);//用于进行随机数据样本采样，设置随机数种子SeedRandOnce(0);//开始每一次的迭代 for (int it = 0; it < mParams.MaxModelFitIterations; it++){//迭代开始的时候，所有的点都是可用的vAvailableIndices = vAllIndices;//选择最小的数据样本集for (size_t j = 0; j < PointsNum; j++){// 随机产生一对点的id,范围从0到N-1int randi = RandomInt(0, vAvailableIndices.size() - 1);// idx表示哪一个索引对应的点对被选中int idx = vAvailableIndices[randi];//将本次迭代这个选中的第j个特征点对的索引添加到mvSets中mvSets[it][j] = idx;// 由于这对点在本次迭代中已经被使用了,所以我们为了避免再次抽到这个点,就在"点的可选列表"中,// 将这个点原来所在的位置用vector最后一个元素的信息覆盖,并且删除尾部的元素// 这样就相当于将这个点的信息从"点的可用列表"中直接删除了vAvailableIndices[randi] = vAvailableIndices.back();vAvailableIndices.pop_back();}//依次提取出6个点}//迭代mMaxIterations次，选取各自迭代时需要用到的最小数据集//某次迭代中，点云的坐标PointCloudT::Ptr cloudIn;cloudIn.reset(new(PointCloudT));//保存最优的平面double fError = 0.0;double plane[4] = { 0 };coefficients->values.resize(4);//最优的匹配int BestMatch = 0;pcl::PointIndices::Ptr inliers;inliers.reset(new pcl::PointIndices());//下面进行每次的RANSAC迭代for (int it = 0; it < mParams.MaxModelFitIterations; it++){//选择6个点对进行迭代for (size_t j = 0; j < PointsNum; j++){//从mvSets中获取当前次迭代的某个特征点对的索引信息int idx = mvSets[it][j];pcl::PointXYZ pointcloud = cloudDownSampling->points[idx];if (!isFinite(pointcloud)) //不是有效点continue;cloudIn->push_back(pointcloud);}if (!isSampleGood(cloudIn)) //不是好的平面点(构成直线了)continue;PlaneFitting(cloudIn, plane, fError);//获得内点的数量和索引std::vector<int> TempInlier;for (int i = 0; i < cloudDownSampling->points.size(); i++){pcl::PointXYZ pointcloud = cloudDownSampling->points[i];if (!isFinite(pointcloud)) //不是有效点continue;//计算误差double det = sqrt(plane[0] * plane[0] + plane[1] * plane[1] + plane[2] * plane[2]);double err = abs(plane[0] * cloudDownSampling->points[i].x + plane[1] * cloudDownSampling->points[i].y + plane[2] * cloudDownSampling->points[i].z + plane[3]) / det;if (err < mParams.SegDistanceThreshold)TempInlier.push_back(i);}//更新最优方程if (TempInlier.size() > BestMatch){BestMatch = TempInlier.size();inliers->indices = TempInlier;coefficients->values[0] = plane[0];coefficients->values[1] = plane[1];coefficients->values[2] = plane[2];coefficients->values[3] = plane[3];}cloudIn.reset(new(PointCloudT));}cloudIn.reset();//创建点云提取对象pcl::ExtractIndices<pcl::PointXYZ> extract;extract.setInputCloud(cloudDownSampling);extract.setIndices(inliers);//提取内点extract.setNegative(false);extract.filter(*cloudseg);//提取外点extract.setNegative(true);extract.filter(*cloudfiltered);inliers.reset();//不优化if (mParams.RefinePlane == 0)return;//平面系数优化(这一步需要ceres库，如果没有，直接屏蔽就好)plane[0] = coefficients->values[0];plane[1] = coefficients->values[1];plane[2] = coefficients->values[2];plane[3] = coefficients->values[3];std::vector<cv::Point3f> v3dpointsPlane;for (int i = 0; i < cloudseg->points.size(); i++){pcl::PointXYZ pointcloud = cloudseg->points[i];if (!isFinite(pointcloud)) //不是有效点continue;v3dpointsPlane.push_back(cv::Point3f(pointcloud.x, pointcloud.y, pointcloud.z));}optimizer.BundleAdjustmentPlane(v3dpointsPlane, plane);//归一化存储double det = sqrt(plane[0] * plane[0] + plane[1] * plane[1] + plane[2] * plane[2]);coefficients->values[0] = plane[0] / det;coefficients->values[1] = plane[1] / det;coefficients->values[2] = plane[2] / det;coefficients->values[3] = plane[3] / det;}

体素滤波：

	void PointCloudVoxelGrid(PointCloudT::Ptr cloudsource, PointCloudT::Ptr cloudfiltered, float voxelsize){pcl::VoxelGrid<PointT> sor;							  //创建滤波对象sor.setInputCloud(cloudsource);                       //设置需要过滤的点云给滤波对象sor.setLeafSize(voxelsize, voxelsize, voxelsize);     //设置滤波时创建的体素体积为1cm的立方体sor.filter(*cloudfiltered);                           //执行滤波处理，存储输出}

判断是否前3个点共线：

	bool isSampleGood(PointCloudT::Ptr cloudsource){// Need an extra check in case the sample selection is emptyif (cloudsource->points.size() < 3)return (false);// Get the values at the two pointspcl::Array4fMap p0 = cloudsource->points[0].getArray4fMap();pcl::Array4fMap p1 = cloudsource->points[1].getArray4fMap();pcl::Array4fMap p2 = cloudsource->points[2].getArray4fMap();Eigen::Array4f dy1dy2 = (p1 - p0) / (p2 - p0);return ((dy1dy2[0] != dy1dy2[1]) || (dy1dy2[2] != dy1dy2[1]));}

平面拟合：

	void PlaneFitting(PointCloudT::Ptr cloudIn, double* plane, double& fError){CvMat* points = cvCreateMat(cloudIn->points.size(), 3, CV_32FC1);for (int i = 0; i < cloudIn->points.size(); ++i){cvmSet(points, i, 0, cloudIn->points[i].x);cvmSet(points, i, 1, cloudIn->points[i].y);cvmSet(points, i, 2, cloudIn->points[i].z);}int nrows = points->rows;int ncols = points->cols;int type = points->type;CvMat* centroid = cvCreateMat(1, ncols, type);cvSet(centroid, cvScalar(0));for (int c = 0; c < ncols; c++){for (int r = 0; r < nrows; r++){centroid->data.fl[c] += points->data.fl[ncols*r + c];}centroid->data.fl[c] /= nrows;}// Subtract geometric centroid from each point.  CvMat* points2 = cvCreateMat(nrows, ncols, type);for (int r = 0; r < nrows; r++)for (int c = 0; c < ncols; c++)points2->data.fl[ncols*r + c] = points->data.fl[ncols*r + c] - centroid->data.fl[c];// Evaluate SVD of covariance matrix.  CvMat* A = cvCreateMat(ncols, ncols, type);CvMat* W = cvCreateMat(ncols, ncols, type);CvMat* V = cvCreateMat(ncols, ncols, type);cvGEMM(points2, points, 1, NULL, 0, A, CV_GEMM_A_T);cvSVD(A, W, NULL, V, CV_SVD_V_T);// Assign plane coefficients by singular vector corresponding to smallest singular value.  plane[ncols] = 0;for (int c = 0; c < ncols; c++){plane[c] = V->data.fl[ncols*(ncols - 1) + c];plane[ncols] += plane[c] * centroid->data.fl[c];}//计算拟合误差//Ax+By+Cz=D A=plane[0],B=plane[1],C=plane[2],D=plane[3]double sum_error = 0;double det = sqrt(plane[0] * plane[0] + plane[1] * plane[1] + plane[2] * plane[2]);for (int i = 0; i < cloudIn->points.size(); ++i){double  a = cloudIn->points[i].x;double  b = cloudIn->points[i].y;double  c = cloudIn->points[i].z;double err = abs(plane[0] * a + plane[1] * b + plane[2] * c - plane[3]) / det;sum_error = sum_error + err;}fError = sum_error / cloudIn->points.size();//归一化平面向量，并将方程修改为Ax+By+Cz+D=0plane[0] = plane[0] / det;plane[1] = plane[1] / det;plane[2] = plane[2] / det;plane[3] = -plane[3] / det;cvReleaseMat(&points);cvReleaseMat(&points2);cvReleaseMat(&A);cvReleaseMat(&W);cvReleaseMat(&V);}

平面拟合的另一种方式：OpenCV最小二乘法拟合空间平面

2、效果图

在这里插入图片描述

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

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

相关文章

智慧工地整体解决方案（1）

智慧工地整体解决方案（1）

背景 建筑行业是我国国民经济的重要物质生产部门和支柱产业之一,在改善居住条件、完善基础设施、吸纳劳动力就业、推动经济增长等方面发挥着重要作用。与此同时，建筑业也是一个安全事故多发的高危行业。近年来，在国家、各级地方政府主管部门和行业主体的高度关注和共同努力下…
阅读更多...
亚马逊测评新策略：解决底层环境防关联，提升下单成功率

亚马逊测评新策略：解决底层环境防关联，提升下单成功率

对于做测评的环境系统&#xff0c;确保稳定性和成功率是非常重要的。市面上有各种环境方案&#xff0c;如虚拟机、模拟机、gcs、云手机、VPS等。然而&#xff0c;这些方案不仅成本高&#xff0c;而且成功率很低。因此&#xff0c;一个好的环境系统是成功的基础。 亚马逊平台的…
阅读更多...
Python 从0开始 一步步基于Django创建项目（8）使用表单编辑既有条目

Python 从0开始 一步步基于Django创建项目（8）使用表单编辑既有条目

与《Python 从0开始 一步步基于Django创建项目&#xff08;6&#xff09;》中的表单应用不同。 前者&#xff0c;是使用表单&#xff0c;提交新数据&#xff0c;新增内容。 本文&#xff0c;是使用表单&#xff0c;对既有数据&#xff0c;进行修改。 因为是对既有数据进行修…
阅读更多...
vue v-model(二)

vue v-model(二)

v-model 的参数​ 组件上的 v-model 也可以接受一个参数&#xff1a; <MyComponent v-model:title"bookTitle" /> 在子组件中&#xff0c;我们可以通过将字符串作为第一个参数传递给 defineModel() 来支持相应的参数&#xff1a; <!-- MyComponent.vue …
阅读更多...
蓝桥杯23年第十四届省赛-接龙数列|DFS、线性DP

蓝桥杯23年第十四届省赛-接龙数列|DFS、线性DP

题目链接&#xff1a; 1.接龙数列 - 蓝桥云课 (lanqiao.cn) 蓝桥杯2023年第十四届省赛真题-接龙数列 - C语言网 (dotcpp.com) 说明&#xff1a; dfs要注意&#xff0c;不能连上的时候绝对不选&#xff1b;能连上的时候可以选&#xff0c;也可以不选。不选的dfs分支是必须经过…
阅读更多...
记录rocketMQ5.+启动报错解决过程

记录rocketMQ5.+启动报错解决过程

1.根据官方文档指引下载对应的rocketMQ源码包&#xff0c;上传到服务器解压 2. 启动NameServer nohup sh bin/mqnamesrv & 验证namesrv是否启动成功 tail -f ~/logs/rocketmqlogs/namesrv.log The Name Server boot success… 3.启动BrokerProxy nohup sh bin/mqbroker -n …
阅读更多...
第3章.引导ChatGPT精准角色扮演：高效输出专业内容

第3章.引导ChatGPT精准角色扮演：高效输出专业内容

角色提示技术 角色提示技术&#xff08;role prompting technique&#xff09;&#xff0c;是通过模型扮演特定角色来产出文本的一种方法。用户为模型设定一个明确的角色&#xff0c;它就能更精准地生成符合特定上下文或听众需求的内容。 比如&#xff0c;想生成客户服务的回复…
阅读更多...
Java作业3-字符串

Java作业3-字符串

题目一 代码 import java.util.*; public class Main {public static void main(String[] args) {Scanner input new Scanner( System.in );String str input.nextLine();int len str.length();StringBuilder s new StringBuilder(len);//StringBuilder类参考菜鸟教程for…
阅读更多...
Kotlin 协程和 Java 线程有什么区别？

Kotlin 协程和 Java 线程有什么区别？

Kotlin 协程和 Java 线程是两种不同的并发编程工具&#xff0c;它们在设计哲学、实现机制和使用方式上都有所区别。以下是 Kotlin 协程和 Java 线程之间的一些主要区别&#xff1a; ### 轻量级与资源消耗 - **Kotlin 协程**&#xff1a; - 协程是一种轻量级的并发单位&…
阅读更多...
zookeeper--znode数据结构详解

zookeeper--znode数据结构详解

https://www.jianshu.com/p/cbe5f0dd6cca 一、概念详解 zxid Zookeeper中每个状态变化都会产生一个全局唯一的zxid (ZooKeeper Transaction Id)&#xff0c;通过它可确定更新操作的先后顺序&#xff0c;每个更改都将有一个唯一的 zxid。例如&#xff0c;zxid1小于zxid2&…
阅读更多...
Centos搭建docker mysql集群

Centos搭建docker mysql集群

1.执行命令 &#xff1a;docker search mysql 看到有percona 之后docker pull percona/percona-xtradb-cluster:5.7.21 2.docker images 查看该镜像&#xff0c;tag重命名 docker tag percona/percona-xtradb-cluster:5.7.21 pxc 也可以直接下载一下&#xff0c;之后dock…
阅读更多...
深入理解HDFS工作原理：大数据存储和容错性机制解析

深入理解HDFS工作原理：大数据存储和容错性机制解析

** 引言&#xff1a; ** 在当今数据爆炸的时代&#xff0c;存储和管理大规模数据成为了许多组织面临的重要挑战。为了解决这一挑战&#xff0c;分布式文件系统应运而生。Hadoop分布式文件系统&#xff08;HDFS&#xff09;作为Apache Hadoop生态系统的核心组件之一&#xff…
阅读更多...
是德科技keysight N9000B 信号分析仪

是德科技keysight N9000B 信号分析仪

181/2461/8938产品概述&#xff1a; 工程的内涵就是将各种创意有机地联系起来&#xff0c;并解决遇到的问题。 CXA 信号分析仪具有出色的实际性能&#xff0c;它是一款出类拔萃、经济高效的基本信号表征工具。 它的功能十分强大&#xff0c;为一般用途和教育行业的用户执行测试…
阅读更多...
深入探讨分布式ID生成方案

深入探讨分布式ID生成方案

✨✨谢谢大家捧场&#xff0c;祝屏幕前的小伙伴们每天都有好运相伴左右&#xff0c;一定要天天开心哦&#xff01;✨✨ &#x1f388;&#x1f388;作者主页&#xff1a; 喔的嘛呀&#x1f388;&#x1f388; ✨✨ 帅哥美女们&#xff0c;我们共同加油&#xff01;一起进步&am…
阅读更多...
vue3数据库中存头像图片相对路径在前端用prop只能显示路径或无法显示图片只能显示alt中内容的问题的解决

vue3数据库中存头像图片相对路径在前端用prop只能显示路径或无法显示图片只能显示alt中内容的问题的解决

不想看前情可以直接跳到头像部分代码 前情&#xff1a; 首先我们是在数据库中存图片相对路径&#xff0c;这里我们是在vue的src下的assets专门建一个文件夹img存头像图片。 然后我们如果用prop"avatar" label"头像"是只能显示图片路径的&#xff0c;即lo…
阅读更多...
一个好用的日常文字发布模版

一个好用的日常文字发布模版

01 时间轴&#xff1a;认真感受每一天 每一天都是一幅独特的画卷&#xff0c;等待我们用心去感受和品味。我珍视每一个瞬间&#xff0c;无论是喜悦的欢笑&#xff0c;还是沉思的寂静&#xff0c;都是生命赐予我的宝贵礼物。 清晨&#xff0c;当第一缕阳光洒进窗户&#xff0…
阅读更多...
c++11 标准模板（STL）本地化库 - std::tolower(std::locale) 用本地环境的 ctype 刻面将字符转换为小写

c++11 标准模板（STL）本地化库 - std::tolower(std::locale) 用本地环境的 ctype 刻面将字符转换为小写

本地化库 本地环境设施包含字符分类和字符串校对、数值、货币及日期/时间格式化和分析&#xff0c;以及消息取得的国际化支持。本地环境设置控制流 I/O 、正则表达式库和 C 标准库的其他组件的行为。 用本地环境的 ctype 刻面将字符转换为小写 std::tolower(std::locale) temp…
阅读更多...
java数组与集合框架（一） -- 数据结构，数组

java数组与集合框架（一） -- 数据结构，数组

数据结构 概述 为什么要讲数据结构&#xff1f; 任何一个有志于从事IT领域的人员来说&#xff0c;数据结构&#xff08;Data Structure&#xff09;是一门和计算机硬件与软件都密切相关的学科&#xff0c;它的研究重点是在计算机的程序设计领域中探讨如何在计算机中组织和存储…
阅读更多...
座次问题（蓝桥杯）

座次问题（蓝桥杯）

文章目录 座次问题题目描述回溯算法 座次问题 题目描述 小 A 的学校&#xff0c;老师好不容易解决了蓝桥杯的报名问题&#xff0c;现在老师又犯愁了。 现在有 N 位同学参加比赛&#xff0c;但是老师想给他们排座位&#xff0c;但是排列方式太多了。 老师非常想弄明白最后的…
阅读更多...
机器学习和神经网络8

机器学习和神经网络8

在人工智能领域&#xff0c;神经网络和随机森林是两种强大的机器学习算法。神经网络&#xff0c;特别是深度学习网络&#xff0c;因其在图像和语音识别等复杂任务中的卓越性能而闻名。另一方面&#xff0c;随机森林是一种基于决策树的集成学习技术&#xff0c;它在处理分类和回…
阅读更多...
最新文章

Copyright @ 2022~2023