PCL最小二乘法拟合平面

效果

过滤掉不属于拟合平面的点（点到平面距离处于阈值外的点）

原理参考

最小二分法拟合平面
过程推导如下

PCL实现

#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>//点云可视化
void 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;
}// 算点到平面距离
//设平面方程为Ax+By+Cz+D=0
//则有A/D X+B/D Y+ C/D Z+1=0 即 a0/a2 X+ a1/a2 Y +-1/a2 Z +1=0
double getDistance(double a0,double a1,double a2,pcl::PointXYZ point){double A=a0/a2;double B=a1/a2;double C=-1/a2;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 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;
}int main(int argc, char *argv[])
{QCoreApplication a(argc, argv);pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);pcl::io::loadPCDFile<pcl::PointXYZ>("D:\\Qt_Project\\flat.pcd",*cloud);showCloud("原点云",cloud);Eigen::Vector3d flat = getFlat(cloud);pcl::PointCloud<pcl::PointXYZ>::Ptr flatPoints(new pcl::PointCloud<pcl::PointXYZ>);int n=0;for (int i=0;i<cloud->points.size();i++){if(getDistance(flat[0],flat[1],flat[2],cloud->points[i])<0.15){  //阈值可调 flatPoints->points.push_back(cloud->points[i]);}else{//std::cout<<"dis:"<<getDistance(flat[0],flat[1],flat[2],cloud->points[i])<<std::endl;}}std::cout<<"原size:"<<cloud->size()<<std::endl;std::cout<<"平面size:"<<flatPoints->size()<<std::endl;showCloud("符合要求的平面",flatPoints);return a.exec();
}

PCL最小二乘法拟合平面相关推荐

Matlab 最小二乘法拟合平面（SVD）
文章目录一.简介 1.1最小二乘法拟合平面 1.2 SVD角度二.实现代码三.实现效果参考资料一.简介 1.1最小二乘法拟合平面之前我们使用过最为经典的方式对平面进行了最小二乘拟合(点云最 ...
Matlab 最小二乘法拟合平面
一.原理推导最小二乘法拟合平面是我们最常用的拟合平面的方法,但是有特殊的情况是用这种方法是不能拟合的,后续会加上这种拟合方法(RANSAC). matlab 最小二乘拟合平面(方法一) - 灰信网 ...
最小二乘法拟合平面原理MATLABC++实现
文章目录最小二乘法拟合平面原理MATLAB&C++实现最小二乘法拟合平面原理 MATLAB实现 c++实现最小二乘法拟合平面原理MATLAB&C++实现最小二乘法拟合平面原理 ...
PCL最小二乘法进行平面拟合原理
最小二乘法进行平面拟合原理 1 最小二乘原理 2 最小二乘拟合平面 1 最小二乘原理最小二乘法(又称最小平方法)是一种数学优化技术.它通过最小化误差的平方和寻找数据的最佳函数匹配.利用最小二乘法可以 ...
PCL：拟合平面直线和曲线以及空间曲线的原理到算法实现
使用两种思路进行直线拟合: 1.利用逆矩阵思想 --------------进行下列公式的推导需要理解逆矩阵(求A矩阵的逆矩阵,则A矩阵必须是方阵)的知识: (1)为什么要引入逆矩阵呢? 逆矩阵可以类 ...
python 最小二乘法三维坐标拟合平面_【MQ笔记】超简单的最小二乘法拟合平面（Python）...
这篇笔记中,我主要通过解决"由离散点拟合平面"这个小问题,学习了超定方程最小二乘解的求解方法.在这里我整理了两种求解思路用以交流. 直接求解超定方程. 我们知道,对于一个平面,其方 ...
点云最小二乘法拟合平面
文章目录一.简介二.算法实现三.实现效果参考文献一.简介点云平面拟合的实质其实就是用一个拟合平面取代近似位于同一平面的点云,使点云中的所有点到拟合平面的距离平方和最小, 达到点云与拟合平面 ...
最小二乘法拟合平面直线
前言: 最近要实现一个算法,"对一系列点拟合出一条线,且区分出不属于该线的点".在网上找了许多资料,用数学公式解释原理以及用matlab实现的居多,本文章主要解释用最小二乘法的进行 ...
C++最小二乘法拟合平面
.`bool AlgFitPlane::FitPlane3D(std::vector& points, float& A, float& B, float& C, fl ...

PCL最小二乘法拟合平面

PCL最小二乘法拟合平面

效果

原理参考

PCL实现

PCL最小二乘法拟合平面相关推荐

最新文章

热门文章