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，一经查实，立即删除！

相关文章

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

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

对于做测评的环境系统&#xff0c;确保稳定性和成功率是非常重要的。市面上有各种环境方案&#xff0c;如虚拟机、模拟机、gcs、云手机、VPS等。然而&#xff0c;这些方案不仅成本高&#xff0c;而且成功率很低。因此&#xff0c;一个好的环境系统是成功的基础。 亚马逊平台的…
阅读更多...
记录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…
阅读更多...
深入理解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…
阅读更多...
java数组与集合框架（一） -- 数据结构，数组

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

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

ctfshow web入门 XXE

XXE基础知识 XXE&#xff08;XML External Entity&#xff09;攻击是一种针对XML处理漏洞的网络安全攻击手段。攻击者利用应用程序在解析XML输入时的漏洞&#xff0c;构造恶意的XML数据&#xff0c;进而实现各种恶意目的。 所以要学习xxe就需要了解xml xml相关&#xff1a; …
阅读更多...
计算数组元素中每个元素与其之前各元素的累积乘积ndarray.cumprod()

计算数组元素中每个元素与其之前各元素的累积乘积ndarray.cumprod()

【小白从小学Python、C、Java】 【计算机等考500强证书考研】 【Python-数据分析】 计算数组元素中每个元素 与其之前各元素的累积乘积 ndarray.cumprod() 选择题 关于以下代码输出的结果说法正确的是&#xff1f; import numpy as np a np.array([2,4,6]) print(【显示】a ,…
阅读更多...
彩虹外链网盘界面UI美化版超级简洁好看

彩虹外链网盘界面UI美化版超级简洁好看

彩虹外链网盘界面UI美化版 彩虹外链网盘&#xff0c;是一款PHP网盘与外链分享程序&#xff0c;支持所有格式文件的上传&#xff0c;可以生成文件外链、图片外链、音乐视频外链&#xff0c;生成外链同时自动生成相应的UBB代码和HTML代码&#xff0c;还可支持文本、图片、音乐、…
阅读更多...
Diffusion添加噪声noise的方式有哪些？怎么向图像中添加噪声？

Diffusion添加噪声noise的方式有哪些？怎么向图像中添加噪声？

添加噪声的方式大致分为两种&#xff0c;一种是每张图像在任意timestep都加入一样的均匀噪声&#xff0c;另一种是按照timestep添加不同程度的噪声 一、在任意timestep都加入一样的noise batch_size 32x_start torch.rand(batch_size,3,256,256) noise torch.randn_like(x_…
阅读更多...
XUbuntu22.04之激活Linux最新Typora版本(二百二十五)

XUbuntu22.04之激活Linux最新Typora版本(二百二十五)

简介&#xff1a; CSDN博客专家&#xff0c;专注Android/Linux系统&#xff0c;分享多mic语音方案、音视频、编解码等技术&#xff0c;与大家一起成长&#xff01; 优质专栏&#xff1a;Audio工程师进阶系列【原创干货持续更新中……】&#x1f680; 优质专栏&#xff1a;多媒…
阅读更多...
技巧 Win10电脑打开SMB协议共享文件，手机端查看

技巧 Win10电脑打开SMB协议共享文件，手机端查看

一. 打开 SMB1.0/CIFS文件共享支持 ⏹如下图所示&#xff0c;打开SMB1.0/CIFS文件共享支持 二. 开启网络发现 ⏹开启网络发现&#xff0c;确保共享的文件能在局域网内被发现 三. 共享文件夹到局域网 ⏹根据需要勾选需要共享的文件夹&#xff0c;共享到局域网 四. 共享文件查…
阅读更多...
Linux重点思考(下）--shell脚本使用以及内核开发

Linux重点思考(下）--shell脚本使用以及内核开发

Linux重点思考(下&#xff09;--shell脚本使用和组合拳 shell脚本的基础算法shell脚本写123...n的值&#xff0c;说思路Shell 脚本用于执行服务器性能测试的死循环Shell 脚本备份和定时清理垃圾文件 shell脚本的内核开发正向映射反向映射 shell脚本的基础算法 shell脚本写123……
阅读更多...
JDBC远程连接mysql报错：NotBefore: Sat Mar 30 16:37:41 UTC 2024

JDBC远程连接mysql报错：NotBefore: Sat Mar 30 16:37:41 UTC 2024

虚拟机docker已经部署了mysql&#xff0c;用navicat可以直接远程连接&#xff0c;datagrip却不能&#xff0c;如图&#xff1a; 需要在最后加上 ?useSSLfalse , 如&#xff1a;jdbc:mysql://192.168.30.128:3306?useSSLfalse navicat不用加的原因是没有使用jdbc连接&#x…
阅读更多...
java数据结构与算法刷题-----LeetCode1091. 二进制矩阵中的最短路径

java数据结构与算法刷题-----LeetCode1091. 二进制矩阵中的最短路径

java数据结构与算法刷题目录&#xff08;剑指Offer、LeetCode、ACM&#xff09;-----主目录-----持续更新(进不去说明我没写完)&#xff1a;https://blog.csdn.net/grd_java/article/details/123063846 文章目录 广度优先双分裂蛇 广度优先双分裂蛇 双分裂蛇&#xff1a;是求二…
阅读更多...
Linux 学习之路--工具篇--yum

Linux 学习之路--工具篇--yum

前面介绍了权限有关的内容&#xff0c;这里继续介绍有关Linux里面常用的工具之一yum 目录 一、简单介绍 <1> 源代码安装 <2>rpm 包安装 <3>yum / apt-get(ubuntu) 安装 二、简单使用 <1>安装包介绍 <2> yum 的基本指令 -- install <…
阅读更多...
C++：数据类型—布尔（12）

C++：数据类型—布尔（12）

布尔类型代表就是真和假&#xff08;bool&#xff09; 真就是1&#xff08;true&#xff09; 假就是0&#xff08;false&#xff09; 也可以任务非0即为真 bool 直占用1个字节大小 语法&#xff1a;bool 变量名 (true | false&#xff09; 提示&#xff1a;bool在后期判断也是…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023