RANSAC法拟合平面的实现

2023-11-10 03:40
文章标签 实现 拟合 平面 ransac

本文主要是介绍RANSAC法拟合平面的实现,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

RANSAC法拟合平面

本文衔接前一篇《最小二乘法实现平面拟合》,基于C++实现了PCL官方的平面拟合,用一个复杂铸件的点云图像进行测试。时间有限,难以确保程序不会出现bug,该文章仅供参考。

效果

请添加图片描述
原点云换一个角度
请添加图片描述
原点云是一个复杂的铸件,通过RANSAC法拟合平面将铸件点云分割为许多个平面。

与PCL官网提供的接口的效果进行对比,下图为官方接口的效果,可以看出自己复现的算法与官方提供的算法接口效果基本相同。

请添加图片描述

RANSAC原理

计算机视觉基本原理——RANSAC

算法逻辑

1、在未经处理过的点云上随机取3个点,用这3个点拟合一个平面(方法参考《PCL最小二乘法拟合平面》)。

2、若拟合平面的内点数量达到一定阈值则将这次迭代的内点与外点都保存到一个容器内。

3、重复迭代1、2步骤一定的次数。

4、选择内点容器内元素中点的数量最多的元素作为结果保存在结果容器中,外点容器中对应的元素作为第一步的输入点云重复上述流程,直到结果容器中的点的个数超过一定阈值,结束迭代。

算法实现

将上述提到的最小二乘法封装成了一个类

dl_planefitting.h

#ifndef DL_PLANEFITTING_H
#define DL_PLANEFITTING_H#include <QObject>
#include<pcl/io/io.h>
#include<pcl/io/pcd_io.h>//pcd 读写类相关的头文件。
#include<pcl/io/ply_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include<Eigen/Dense>
#include <random>
#include <windows.h>//平面拟合类
class DL_Plane : public QObject
{Q_OBJECT
public:explicit DL_Plane(QObject *parent = nullptr);/*https://blog.csdn.net/konglingshneg/article/details/82585868设平面方程为ax+by+cz+d=0两边同时除d则有(a/d)X+(b/d)Y+(c/d)z+1=0令a0=-a/c;a1=-b/c;a2=-d/c 即(a0/a2)X+(a1/a2)Y +(-1/a2)Z+1=0最后直线方程写成AX+BY+CZ+1=0*///平面表示参数double A;double B;double C;//距离阈值double distance = 1.0;//拟合点pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_;//平面内点pcl::PointCloud<pcl::PointXYZ>::Ptr plane_;//平面外点pcl::PointCloud<pcl::PointXYZ>::Ptr nonplane_;//平面拟合Eigen::Vector3d getFlat(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);//设置输入点云void setInputCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);//更新平面参数void updateParameter(Eigen::Vector3d x);//求点到平面距离double getDistance(pcl::PointXYZ point);//求平面点云void planeFitting(double dis);//取平面点云pcl::PointCloud<pcl::PointXYZ>::Ptr getPlanePoints(void);//设置参数void setParameter(double dis);//平面拟合处理void planeProcess(void);//点云可视化void showCloud(std::string windowname,pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);
private://拟合平面参数double a0;double a1;double a2;
signals:public slots:
};//RANSAC法拟合类
class DL_PlaneFitting : public DL_Plane
{
public://总点的数量int all_pointsnum;//迭代次数int iteration_time = 1000;//距离阈值double dis_threshold = 2.0;//平面点数量与点云总数最小比例double min_rate = 0.05;//终止条件double stop_rate = 0.25;//内点点集std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> plane_points_;//最终输出std::vector<std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>> result_;//对应输出点集的int列表std::vector<int> max_output;DL_PlaneFitting();void updatePlaneParameter(DL_Plane *dl_plane,Eigen::Vector3d x);//参数设置void setIterationTime(int num){iteration_time = num;}void setDisThreshold(double dis){dis_threshold = dis;}void setMinRate(double rate){min_rate = rate;}void setStopRate(double rate){stop_rate = rate;}//RANSAC法拟合平面void ransacPlaneFitting();void getResultPlanes();
private:double dis;//随机生成的三个假平面点pcl::PointCloud<pcl::PointXYZ>::Ptr flatpoints_;//从点云中随机生成三个点void randomGenerate(pcl::PointCloud<pcl::PointXYZ>::Ptr restpoints,int p_num);
};#endif // DL_PLANEFITTING_H

dl_planefitting.cpp

#include "dl_planefitting.h"DL_Plane::DL_Plane(QObject *parent) : QObject(parent)
{pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptr plane (new pcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptr nonplane (new pcl::PointCloud<pcl::PointXYZ>);cloud_ = cloud;plane_ = plane;nonplane_ = nonplane;
}//设置输入点云
void DL_Plane::setInputCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud){cloud_ = cloud;
}//求解点到平面距离(使用前要更新平面参数)
double DL_Plane::getDistance(pcl::PointXYZ point){double up = std::abs(A*point.x+B*point.y+C*point.z+1);double down = std::sqrt(std::pow(A,2)+std::pow(B,2)+std::pow(C,2));double dis = up/down;return dis;
}//平面拟合算法
Eigen::Vector3d DL_Plane::getFlat(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud){Eigen::Matrix3d rot;double x2,xy,x,y2,y,zx,zy,z;for(int i=0;i<cloud->points.size();i++){x2 += cloud->points[i].x * cloud->points[i].x;xy += cloud->points[i].x * cloud->points[i].y;x += cloud->points[i].x;y2 += cloud->points[i].y * cloud->points[i].y;y += cloud->points[i].y;zx += cloud->points[i].x * cloud->points[i].z;zy += cloud->points[i].y * cloud->points[i].z;z += cloud->points[i].z;}//为矩阵赋值rot<<x2,  xy,  x,xy,  y2,  y,x,   y,   cloud->points.size();//为列向量赋值Eigen::Vector3d eq(zx,zy,z);Eigen::Vector3d X = rot.colPivHouseholderQr().solve(eq);
//    std::cout<<X<<std::endl;
//    std::cout<<X[0]<<" "<<X[1]<<" "<<X[2]<<std::endl;return X;
}//更新平面参数
void DL_Plane::updateParameter(Eigen::Vector3d x){a0 = x[0];a1 = x[1];a2 = x[2];A=a0/a2;B=a1/a2;C=-1/a2;
}//求平面内点
void DL_Plane::planeFitting(double dis){plane_->clear();for (int i=0;i<cloud_->points.size();i++){if(getDistance(cloud_->points[i])<dis){plane_->points.push_back(cloud_->points[i]);}else{nonplane_->points.push_back(cloud_->points[i]);//std::cout<<"dis:"<<getDistance(cloud->points[i])<<std::endl;}}
}//取平面点云
pcl::PointCloud<pcl::PointXYZ>::Ptr DL_Plane::getPlanePoints(void){return plane_;
}//设置距离参数
void DL_Plane::setParameter(double dis){distance = dis;
}//平面拟合处理
void DL_Plane::planeProcess(void){Eigen::Vector3d result = getFlat(this->cloud_);updateParameter(result);planeFitting(this->distance);
}//点云可视化
void DL_Plane::showCloud(std::string windowname,pcl::PointCloud<pcl::PointXYZ>::Ptr cloud){pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer (windowname));viewer->setBackgroundColor (0.5, 0.5, 0.5);  //设置背景viewer->addPointCloud<pcl::PointXYZ> (cloud, "sample cloud");  //显示点云viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");  //设置点尺寸viewer->addCoordinateSystem (100.0);  //设置坐标轴尺寸
//    while (!viewer->wasStopped ())
//    {
//      viewer->spinOnce (100);
//      boost::this_thread::sleep (boost::posix_time::microseconds (100000));
//    }//cout<<"Point couting in "<<windowname<<": "<<cloud->size()<<endl;
}DL_PlaneFitting::DL_PlaneFitting(){}//更新平面参数
void DL_PlaneFitting::updatePlaneParameter(DL_Plane *dl_plane, Eigen::Vector3d x){dl_plane->updateParameter(x);
}//随机生成三个点,存在flatpoints中
void DL_PlaneFitting::randomGenerate(pcl::PointCloud<pcl::PointXYZ>::Ptr restpoints,int p_num){// 生成随机的三个点拟合平面LARGE_INTEGER seed;QueryPerformanceFrequency(&seed);QueryPerformanceCounter(&seed);srand(seed.QuadPart);pcl::PointCloud<pcl::PointXYZ>::Ptr flatpoints(new pcl::PointCloud<pcl::PointXYZ>);for (int i=0;i<p_num;i++){int index1 = rand()%restpoints->points.size();flatpoints->points.push_back(restpoints->points[index1]);
//        std::cout<<" "<<i<<":"<<index1;}
//    std::cout<<std::endl;flatpoints_ = flatpoints;
}//RANSAC法拟合平面
void DL_PlaneFitting::ransacPlaneFitting(){//记录原点云数量int total_num = cloud_->points.size();std::cout<<"原点云总数:"<<total_num<<std::endl;//存放结果std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> result;//存放剩余的点pcl::PointCloud<pcl::PointXYZ>::Ptr restpoints(new pcl::PointCloud<pcl::PointXYZ>);restpoints->points = cloud_->points;//存放每次迭代的点云std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> max_Planes;//存放每次迭代剩余的点云std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> rest_Planes;int N=0;while(restpoints->points.size()>stop_rate*total_num){N+=1;std::cout<<"第"<<N<<"次迭代比例为:"<<double((restpoints->points.size()+0.01)/total_num)<<std::endl;max_Planes.clear();rest_Planes.clear();//设置每层迭代次数int n=iteration_time;while(--n>0){randomGenerate(restpoints,3);DL_Plane dl_plane;dl_plane.setInputCloud(restpoints);Eigen::Vector3d result = dl_plane.getFlat(flatpoints_);dl_plane.updateParameter(result);//设置拟合平面点的距离阈值dl_plane.planeFitting(dis_threshold);//如果拟合平面的内点数量符合要求if(dl_plane.plane_->points.size()>min_rate*total_num){max_Planes.push_back(dl_plane.plane_);rest_Planes.push_back(dl_plane.nonplane_);}}//提取最大平面作为其中一个结果int max_num=0;int index = -1;for (int i=0;i<max_Planes.size();i++){if (max_Planes[i]->points.size()>max_num){max_num = max_Planes[i]->points.size();index = i;}}result.push_back(max_Planes[index]);//重新设置剩余的点restpoints->points = rest_Planes[index]->points;std::cout<<"剩余的点的数量"<<restpoints->points.size()<<std::endl;//showCloud("cloud"+std::to_string(index),max_Planes[index]);}for(int i=0;i<result.size();i++){showCloud("cloud"+std::to_string(i),result[i]);}
}

main.cpp

#include <QCoreApplication>
#include<opencv2\opencv.hpp>
#include<pcl/io/io.h>
#include<pcl/io/pcd_io.h>//pcd 读写类相关的头文件。
#include<pcl/io/ply_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include<Eigen/Dense>
#include <dl_planefitting.h>#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/search/kdtree.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>int main(int argc, char *argv[])
{QCoreApplication a(argc, argv);DL_Plane dl_plane;pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);pcl::io::loadPCDFile<pcl::PointXYZ>("D:\\Qt_Project\\result.pcd",*cloud);dl_plane.showCloud("原点云",cloud);/*///PCL提供的平面拟合*/
//    pcl::VoxelGrid<pcl::PointXYZ> vg;
//    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
//    vg.setInputCloud (cloud);
//    vg.setLeafSize (0.1f, 0.1f, 0.1f);
//    vg.filter (*cloud_filtered);//    std::cout << "PointCloud after filtering has: " << cloud_filtered->size ()  << " data points." << std::endl; //*
//    // Create the segmentation object for the planar model and set all the parameters
//    pcl::SACSegmentation<pcl::PointXYZ> seg;
//    pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
//    pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
//    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ());
//    seg.setOptimizeCoefficients (true);
//    seg.setModelType (pcl::SACMODEL_PLANE);
//    seg.setMethodType (pcl::SAC_RANSAC);
//    seg.setMaxIterations (1000);
//    seg.setDistanceThreshold (1.0);
//    int nr_points = (int) cloud_filtered->size ();
//    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
//    int i=0;
//    while (cloud_filtered->size () > 0.3 * nr_points)
//    {
//     // Segment the largest planar component from the remaining cloud
//         seg.setInputCloud (cloud_filtered);
//         seg.segment (*inliers, *coefficients);
//         if (inliers->indices.size () == 0)
//         {
//             std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
//             break;
//         }
//         // Extract the planar inliers from the input cloud
//         pcl::ExtractIndices<pcl::PointXYZ> extract;
//         extract.setInputCloud (cloud_filtered);
//         extract.setIndices (inliers);
//         extract.setNegative (false);
//         // Get the points associated with the planar surface
//         extract.filter (*cloud_plane);
//         if(cloud_plane->size()>500){
//             dl_plane.showCloud("cloud"+std::to_string(i),cloud_plane);
//             i+=1;
//             double time = cloud_filtered->size ()/cloud_plane->points.size();
//             std::cout<<"size"<<i<<" :"<<time<<std::endl;
//         }
//         std::cout << "PointCloud representing the planar component: " << cloud_plane->size () << " data points." << std::endl;
//         // Remove the planar inliers, extract the rest
//         extract.setNegative (true);
//         extract.filter (*cloud_f);
//         *cloud_filtered = *cloud_f;
//     }/*//*/DL_PlaneFitting dl_planefitting;dl_planefitting.cloud_ = cloud;dl_planefitting.setIterationTime(1000);dl_planefitting.setDisThreshold(2.0);dl_planefitting.setMinRate(0.05);dl_planefitting.setStopRate(0.27);dl_planefitting.ransacPlaneFitting();return a.exec();
}

上述完整代码
上传了工程代码与数据集。但是这个数据集是后面重新导出的,没有很认真做预处理,所以在迭代的时候可能会失败无法迭代出结果,可以调整参数集合,多试几次。
想要拿到原始数据集的话可以看我的这篇博客
tiff转PCL
这篇介绍了如何从一张记录了点云三个维度信息的.tiff文件转为点云文件。

因为本文只是对着原理简单实现了RANSAC的平面拟合,主要是为了参考学习,当然还有很多不足之处----速度远低于官方接口,也没有对失败迭代的情况进行回溯,难以避免会有拟合失败的情况出现。

成功转成点云之后不能直接拿过来RANSAC拟合,要效仿官方接口对其进行预处理。(直通滤波、离群滤波之类的)将数据集处理成比较漂亮的时候迭代成功率会很高。

这篇关于RANSAC法拟合平面的实现的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/380198

相关文章

hdu1043(八数码问题,广搜 + hash(实现状态压缩) )

利用康拓展开将一个排列映射成一个自然数,然后就变成了普通的广搜题。 #include<iostream>#include<algorithm>#include<string>#include<stack>#include<queue>#include<map>#include<stdio.h>#include<stdlib.h>#include<ctype.h>#inclu

【C++】_list常用方法解析及模拟实现

相信自己的力量,只要对自己始终保持信心,尽自己最大努力去完成任何事,就算事情最终结果是失败了,努力了也不留遗憾。💓💓💓 目录   ✨说在前面 🍋知识点一:什么是list? •🌰1.list的定义 •🌰2.list的基本特性 •🌰3.常用接口介绍 🍋知识点二:list常用接口 •🌰1.默认成员函数 🔥构造函数(⭐) 🔥析构函数 •🌰2.list对象

【Prometheus】PromQL向量匹配实现不同标签的向量数据进行运算

✨✨ 欢迎大家来到景天科技苑✨✨ 🎈🎈 养成好习惯,先赞后看哦~🎈🎈 🏆 作者简介:景天科技苑 🏆《头衔》:大厂架构师,华为云开发者社区专家博主,阿里云开发者社区专家博主,CSDN全栈领域优质创作者,掘金优秀博主,51CTO博客专家等。 🏆《博客》:Python全栈,前后端开发,小程序开发,人工智能,js逆向,App逆向,网络系统安全,数据分析,Django,fastapi

让树莓派智能语音助手实现定时提醒功能

最初的时候是想直接在rasa 的chatbot上实现,因为rasa本身是带有remindschedule模块的。不过经过一番折腾后,忽然发现,chatbot上实现的定时,语音助手不一定会有响应。因为,我目前语音助手的代码设置了长时间无应答会结束对话,这样一来,chatbot定时提醒的触发就不会被语音助手获悉。那怎么让语音助手也具有定时提醒功能呢? 我最后选择的方法是用threading.Time

Android实现任意版本设置默认的锁屏壁纸和桌面壁纸(两张壁纸可不一致)

客户有些需求需要设置默认壁纸和锁屏壁纸  在默认情况下 这两个壁纸是相同的  如果需要默认的锁屏壁纸和桌面壁纸不一样 需要额外修改 Android13实现 替换默认桌面壁纸: 将图片文件替换frameworks/base/core/res/res/drawable-nodpi/default_wallpaper.*  (注意不能是bmp格式) 替换默认锁屏壁纸: 将图片资源放入vendo

C#实战|大乐透选号器[6]:实现实时显示已选择的红蓝球数量

哈喽,你好啊,我是雷工。 关于大乐透选号器在前面已经记录了5篇笔记,这是第6篇; 接下来实现实时显示当前选中红球数量,蓝球数量; 以下为练习笔记。 01 效果演示 当选择和取消选择红球或蓝球时,在对应的位置显示实时已选择的红球、蓝球的数量; 02 标签名称 分别设置Label标签名称为:lblRedCount、lblBlueCount

Kubernetes PodSecurityPolicy:PSP能实现的5种主要安全策略

Kubernetes PodSecurityPolicy:PSP能实现的5种主要安全策略 1. 特权模式限制2. 宿主机资源隔离3. 用户和组管理4. 权限提升控制5. SELinux配置 💖The Begin💖点点关注,收藏不迷路💖 Kubernetes的PodSecurityPolicy(PSP)是一个关键的安全特性,它在Pod创建之前实施安全策略,确保P

工厂ERP管理系统实现源码(JAVA)

工厂进销存管理系统是一个集采购管理、仓库管理、生产管理和销售管理于一体的综合解决方案。该系统旨在帮助企业优化流程、提高效率、降低成本,并实时掌握各环节的运营状况。 在采购管理方面,系统能够处理采购订单、供应商管理和采购入库等流程,确保采购过程的透明和高效。仓库管理方面,实现库存的精准管理,包括入库、出库、盘点等操作,确保库存数据的准确性和实时性。 生产管理模块则涵盖了生产计划制定、物料需求计划、

C++——stack、queue的实现及deque的介绍

目录 1.stack与queue的实现 1.1stack的实现  1.2 queue的实现 2.重温vector、list、stack、queue的介绍 2.1 STL标准库中stack和queue的底层结构  3.deque的简单介绍 3.1为什么选择deque作为stack和queue的底层默认容器  3.2 STL中对stack与queue的模拟实现 ①stack模拟实现

基于51单片机的自动转向修复系统的设计与实现

文章目录 前言资料获取设计介绍功能介绍设计清单具体实现截图参考文献设计获取 前言 💗博主介绍:✌全网粉丝10W+,CSDN特邀作者、博客专家、CSDN新星计划导师,一名热衷于单片机技术探索与分享的博主、专注于 精通51/STM32/MSP430/AVR等单片机设计 主要对象是咱们电子相关专业的大学生,希望您们都共创辉煌!✌💗 👇🏻 精彩专栏 推荐订阅👇🏻 单片机