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.rhkb.cn/news/288317.html

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

相关文章

vue脚手架创建项目:账号登录（利用element-ui快速开发）(取消eslint强制格式）（修改端口号）

vue脚手架创建项目:账号登录（利用element-ui快速开发）(取消eslint强制格式）（修改端口号）

新手看不懂&#xff0c;老手不用看系列 文章目录 一、准备工作1.1 取消强制格式检查1.2 导入依赖&#xff0c;注册依赖 二、去element-ui官网找样式写Login组件2.1 引用局部组件2.2 运行项目 三、看一下发现没问题&#xff0c;开始修改前端的代码四、修改端口号4.1 修改后端端口…
阅读更多...
一款比 K8S 更好用的编排工具——Nomod 单机部署

一款比 K8S 更好用的编排工具——Nomod 单机部署

上下文 最近公司需要调研类似 EMCHub 这样支持算力共享的服务。第一直觉是使用 K8S 或 K3S&#xff0c;作为 CNCF 孵化的顶级项目&#xff0c;同时也是当前云原生生态使用最广的编排系统。但是在学习 EMC Hub 源码过程中&#xff0c;偶然发现它是基于 Nomad 做的集群管理。 相…
阅读更多...
Python学习笔记------文件操作

Python学习笔记------文件操作

编码 编码就是一种规则集合&#xff0c;记录了内容和二进制间进行相互转换的逻辑。 编码有许多中&#xff0c;我们最常用的是UTF-8编码 计算机只认识0和1&#xff0c;所以需要将内容翻译成0和1才能保存在计算机中。同时也需要编码&#xff0c;将计算机保存的0和1&#xff0c…
阅读更多...
maya导入导出bvh 自动 脚本

maya导入导出bvh 自动 脚本

目录 maya打开脚本编辑器 运行打开bvh脚本 maya导出bvh脚本 maya打开脚本编辑器 打开Maya软件，点击右下角 “脚本编辑器” 运行打开bvh脚本<
阅读更多...
Spring：面试八股

Spring：面试八股

文章目录 参考Spring模块CoreContainerAOP 参考 JavaGuide Spring模块 CoreContainer Spring框架的核心模块&#xff0c;主要提供IoC依赖注入功能的支持。内含四个子模块&#xff1a; Core&#xff1a;基本的核心工具类。Beans&#xff1a;提供对bean的创建、配置、管理功能…
阅读更多...
国内ip地址推荐，畅享网络新体验！

国内ip地址推荐，畅享网络新体验！

在数字化时代&#xff0c;IP地址不仅是网络连接的基石&#xff0c;也是互联网产业发展的重要标志。国内作为全球互联网市场的重要参与者&#xff0c;拥有众多IP地址资源。虎观代理小二旨在探索并推荐一些国内IP地址&#xff0c;分析它们的价值所在&#xff0c;并探讨如何更好地…
阅读更多...
华为数通 HCIP-Datacom H12-831 题库补充（3/27）

华为数通 HCIP-Datacom H12-831 题库补充（3/27）

2024年 HCIP-Datacom&#xff08;H12-831&#xff09;最新题库&#xff0c;完整题库请扫描上方二维码&#xff0c;持续更新。 如图所示&#xff0c;关于R4路由器通过IS-IS计算出来的IPv6路由&#xff0c;哪一选项的描述是错误的&#xff1f; A&#xff1a;R4通过IS—IS只学习到…
阅读更多...
stm32f103c8t6学习笔记（学习B站up江科大自化协）-DMA

stm32f103c8t6学习笔记（学习B站up江科大自化协）-DMA

DMA简介 DMA主要用于协助CPU完成数据转运的工作 DMA&#xff0c;英文全称Direct Memory Access&#xff0c;DMA这个外设是可以直接访问STM32内部存储器的&#xff0c;包括运行内存SRAM&#xff0c;程序存储器flash和寄存器等等&#xff0c;DMA都有权限访问&#xff0c;所以DMA能…
阅读更多...
PHP页面如何实现设置独立访问密码

PHP页面如何实现设置独立访问密码

PHP网页如果需要查看信息必须输入密码&#xff0c;验证后才可显示出内容的代码如何实现&#xff1f; 对某些php页面设置单独的访问密码,如果密码不正确则无法查看内容,相当于对页面进行了一个加密。 如何实现这个效果&#xff0c;详细教程可以参考&#xff1a;PHP页面如何实现…
阅读更多...
基于react native的自定义轮播图

基于react native的自定义轮播图

基于react native的自定义轮播图 效果示例图示例代码 效果示例图 示例代码 import React, {useEffect, useRef, useState} from react; import {Animated,PanResponder,StyleSheet,Text,View,Dimensions, } from react-native; import {pxToPd} from ../../common/js/device;c…
阅读更多...
【Linux】进程状态（R运行状态、S睡眠状态、D磁盘休眠状态、T停止状态、X死亡状态）

【Linux】进程状态（R运行状态、S睡眠状态、D磁盘休眠状态、T停止状态、X死亡状态）

目录 01.运行状态 02.睡眠状态 03.磁盘睡眠状态 04.停止状态 05.死亡状态 进程的状态会随着操作系统的调度和外部事件的发生而不断地发生转换。例如&#xff0c;一个新创建的进程经过初始化后会进入就绪态&#xff0c;等待被调度执行&#xff1b;当调度器分配处理器资源给…
阅读更多...
Windows直接运行python程序

Windows直接运行python程序

Windows直接运行python程序 一、新建bat脚本二、新建vbs脚本 一、新建bat脚本 新建bat批处理脚本&#xff0c;写入以下内容 echo off call conda activate pytorch python app.pyecho off&#xff1a;在此语句后所有运行的命令都不显示命令行本身&#xff0c;但是本身的指令是…
阅读更多...
Android: Gradle 命令

Android: Gradle 命令

一、查看整个项目依赖传递关系 x.x.x (*) 该依赖已经有了&#xff0c;将不再重复依赖。x.x.x -> x.x.x 该依赖的版本被箭头所指的版本代替。x.x.x -> x.x.x(*) 该依赖的版本被箭头所指的版本代替&#xff0c;并且该依赖已经有了&#xff0c;不再重复依赖。 1. gradlew ap…
阅读更多...
申请即管控、差补自动算......企业差旅,想怎么省就怎么省

申请即管控、差补自动算......企业差旅,想怎么省就怎么省

3月开始,企业商旅行为逐步升温。但管控难度也随之而来: ● 同时段、同起终地,预订机、火。企业支付后,总忘记取消未使用行程; ● 同天预订不同城市酒店,非入住酒店经常无法免费取消; ● 短途差旅,依然按习惯预订机票...... 为了提高效率,“出一趟差,办多项事”成了很多企业…
阅读更多...
隐语笔记2 —— 隐私计算开源如何助力数据要素流通

隐语笔记2 —— 隐私计算开源如何助力数据要素流通

数据生命周期 数据流转链路主要包括&#xff1a;采集、存储、加工、使用、提供、传输 数据要素外循环是构建数据要素市场的核心 数据外循环中的信任焦虑 三个代表性问题&#xff1a; 不可信内部人员不按约定使用用户隐私泄漏 数据权属问题 解决方案&#xff1a;从主体信任…
阅读更多...
耳目一新的滑块版登录注册界面~

耳目一新的滑块版登录注册界面~

又到了毕业季&#xff0c;大家做毕设的时候总会参考已有的案例&#xff0c;不过大多产品的样式非常单一雷同。本帖博主给大家分享一个比较别树一帜的登录界面&#xff0c;如下&#xff1a; 如果没有账号&#xff0c;点击“去注册”&#xff0c;则会产生如下的效果&#xff1a; …
阅读更多...
金融投贷通（金融投资+贷款通）项目准备

金融投贷通（金融投资+贷款通）项目准备

金融投贷通&#xff08;金融投资贷款通&#xff09;项目准备 专业术语投资专业术语本息专业术语还款专业术语项目介绍三个子系统技术架构核心流程发布借款标投资业务 项目实施测试流程测试步骤 专业术语 投资专业术语 案例&#xff1a;张三借给李四5W&#xff0c;约定期满1年后…
阅读更多...
特斯拉铺路 小米SU7稳了

特斯拉铺路 小米SU7稳了

文 | AUTO芯球 作者 | 李逵 我把特斯拉的销售拉黑了 拉完又后悔了 因为我欠他一个人情啊 具体怎么回事 看完你就会明白了 今天一大早 特斯拉的销售就发信息给我 说从4月1号开始 特斯拉就要涨价了 我以前去看的Model Y 要提价5000块 而且之前的补贴 什么保险补贴、…
阅读更多...
844. 走迷宫 典bfs

844. 走迷宫 典bfs

AC代码&#xff1a; #include<algorithm> #include<iostream> #include<cstring> #include<queue> #include<algorithm> #include<cmath> using namespace std; const int N 110;int mp[N][N]; int sx,sy; bool vis[N][N]; struct node{i…
阅读更多...
TCP重传机制详解——02SACK

TCP重传机制详解——02SACK

文章目录 TCP重传机制详解——02 SACKSACK是什么&#xff1f;为什么要有SACK&#xff1f;实际场景抓包具体显示信息流程 实战抓包讲解SACK关闭场景下&#xff0c;三次重复ACK后会快速重传SACK打开但是不携带SACK块信息场景下&#xff0c;三次重复ACK也不会快速重传SACK打开并且…
阅读更多...
最新文章
推荐文章

Copyright @ 2022~2023

友情链接： 我的编程经验分享 一条长河网 尧图网站开发