精华内容
下载资源
问答
  • PCL 最小二乘拟合二维直线
    2021-12-12 11:08:00

    一、算法原理

      平面直线的表达式为:
    y = k x + b (1) y=kx+b \tag{1}

    更多相关内容
  • //fitting.h ...pcl/point_types.h> #include <vector> #include <Eigen/dense> #include <vtkPolyLine.h> #include <pcl/visualization/pcl_visualizer.h> #include <pcl/...

    最小二乘原理


    在这里插入图片描述
    在这里插入图片描述

    //fitting.h
    #include <pcl/point_types.h>
    #include <vector>
    #include <Eigen/dense>
    #include <vtkPolyLine.h>
    #include <pcl/visualization/pcl_visualizer.h>
    #include <pcl/visualization/pcl_plotter.h>
    #include <pcl/common/common.h>
    using namespace std;
    using namespace pcl;
    using namespace Eigen;
    typedef PointXYZ PointT;
    
    class fitting
    {
    public:
    	fitting();
    	~fitting();
    	void setinputcloud(PointCloud<PointT>::Ptr input_cloud);//点云输入
    	void grid_mean_xyz(double x_resolution,double y_resolution, vector<double>&x_mean, vector<double> &y_mean, vector<double>&z_mean, PointCloud<PointT>::Ptr &new_cloud);//投影至XOY,规则格网,求每个格网内点云坐标均值
    	void grid_mean_xyz_display(PointCloud<PointT>::Ptr new_cloud);//均值结果三维展示
    	void line_fitting(vector<double>x, vector<double>y, double &k, double &b);//y=kx+b
    	void polynomial2D_fitting(vector<double>x, vector<double>y, double &a, double &b, double &c);//y=a*x^2+b*x+c;
    	void polynomial3D_fitting(vector<double>x, vector<double>y, vector<double>z, double &a, double &b, double &c);//z=a*(x^2+y^2)+b*sqrt(x^2+y^2)+c
    	void polynomial3D_fitting_display(double step_);//三维曲线展示
    	void display_point(vector<double>vector_1,vector<double>vector_2);//散点图显示
    	void display_line(vector<double>vector_1, vector<double>vector_2, double c, double b, double a = 0);//拟合的平面直线或曲线展示
    private:
    	PointCloud<PointT>::Ptr cloud;
    	PointT point_min;
    	PointT point_max;
    	double a_3d;
    	double b_3d;
    	double c_3d;
    	double k_line;
    	double b_line;
    };
    
    //fitting.cpp
    #include "fitting.h"
    fitting::fitting()
    {
    }
    fitting::~fitting()
    {
    	cloud->clear();
    }
    void fitting::setinputcloud(PointCloud<PointT>::Ptr input_cloud){
    	cloud = input_cloud;
    	getMinMax3D(*input_cloud, point_min, point_max);
    }
    void fitting::grid_mean_xyz(double x_resolution, double y_resolution, vector<double>&x_mean, vector<double> &y_mean, vector<double>&z_mean, PointCloud<PointT>::Ptr &new_cloud){
    	if (y_resolution<=0)
    	{
    		y_resolution=point_max.y - point_min.y;
    	}
    	int raster_rows, raster_cols;
    	raster_rows = ceil((point_max.x - point_min.x) / x_resolution);
    	raster_cols = ceil((point_max.y - point_min.y) / y_resolution);
    	vector<int>idx_point;
    	vector<vector<vector<float>>>row_col;
    	vector<vector<float>>col_;
    	vector<float>vector_4;
    	vector_4.resize(4);
    	col_.resize(raster_cols, vector_4);
    	row_col.resize(raster_rows, col_);
    	int point_num = cloud->size();
    	for (int i_point = 0; i_point < point_num; i_point++)
    	{
    		int row_idx = ceil((cloud->points[i_point].x - point_min.x) / x_resolution) - 1;
    		int col_idx = ceil((cloud->points[i_point].y - point_min.y) / y_resolution) - 1;
    		if (row_idx < 0)row_idx = 0;
    		if (col_idx < 0)col_idx = 0;
    		row_col[row_idx][col_idx][0] += cloud->points[i_point].x;
    		row_col[row_idx][col_idx][1] += cloud->points[i_point].y;
    		row_col[row_idx][col_idx][2] += cloud->points[i_point].z;
    		row_col[row_idx][col_idx][3] += 1;
    	}
    	PointT point_mean_tem;
    	for (int i_row = 0; i_row < row_col.size(); i_row++)
    	{
    		for (int i_col = 0; i_col < row_col[i_row].size(); i_col++)
    		{
    			if (row_col[i_row][i_col][3] != 0)
    			{
    				double x_mean_tem = row_col[i_row][i_col][0] / row_col[i_row][i_col][3];
    				double y_mean_tem = row_col[i_row][i_col][1] / row_col[i_row][i_col][3];
    				double z_mean_tem = row_col[i_row][i_col][2] / row_col[i_row][i_col][3];
    				x_mean.push_back(x_mean_tem);
    				y_mean.push_back(y_mean_tem);
    				z_mean.push_back(z_mean_tem);
    				point_mean_tem.x = x_mean_tem;
    				point_mean_tem.y = y_mean_tem;
    				point_mean_tem.z = z_mean_tem;
    				new_cloud->push_back(point_mean_tem);
    			}
    		}
    	}
    }
    void fitting::grid_mean_xyz_display(PointCloud<PointT>::Ptr new_cloud){
    	visualization::PCLVisualizer::Ptr view(new visualization::PCLVisualizer("分段质心拟合"));
    	visualization::PointCloudColorHandlerCustom<PointT>color_1(new_cloud, 255, 0, 0);
    	view->addPointCloud(new_cloud, color_1, "11");
    	view->setPointCloudRenderingProperties(visualization::PCL_VISUALIZER_POINT_SIZE, 3, "11");
    	PointCloud<PointT>::Ptr new_cloud_final(new PointCloud<PointT>);
    	for (int i_point = 0; i_point < cloud->size(); i_point++)
    	{
    		PointT tem_point;
    		tem_point.x = cloud->points[i_point].x;
    		tem_point.y = cloud->points[i_point].y;
    		tem_point.z = cloud->points[i_point].z;
    		new_cloud_final->push_back(tem_point);
    	}
    	view->addPointCloud(new_cloud_final, "22");
    	view->spin();
    }
    void fitting::line_fitting(vector<double>x, vector<double>y, double &k, double &b){
    	MatrixXd A_(2, 2), B_(2, 1), A12(2, 1);
    	int num_point = x.size();
    	double A01(0.0), A02(0.0), B00(0.0), B10(0.0);
    	for (int i_point = 0; i_point < num_point; i_point++)
    	{
    		A01 += x[i_point] * x[i_point];
    		A02 += x[i_point];
    		B00 += x[i_point] * y[i_point];
    		B10 += y[i_point];
    	}
    	A_ << A01, A02,
    		A02, num_point;
    	B_ << B00,
    		B10;
    	A12 = A_.inverse()*B_;
    	k = A12(0, 0);
    	b = A12(1, 0);
    }
    void fitting::polynomial2D_fitting(vector<double>x, vector<double>y, double &a, double &b, double &c){
    	MatrixXd A_(3, 3), B_(3, 1), A123(3, 1);
    	int num_point = x.size();
    	double A01(0.0), A02(0.0), A12(0.0), A22(0.0), B00(0.0), B10(0.0), B12(0.0);
    	for (int i_point = 0; i_point < num_point; i_point++)
    	{
    		A01 += x[i_point];
    		A02 += x[i_point] * x[i_point];
    		A12 += x[i_point] * x[i_point] * x[i_point];
    		A22 += x[i_point] * x[i_point] * x[i_point] * x[i_point];
    		B00 += y[i_point];
    		B10 += x[i_point] * y[i_point];
    		B12 += x[i_point] * x[i_point] * y[i_point];
    	}
    	A_ << num_point, A01, A02,
    		A01, A02, A12,
    		A02, A12, A22;
    	B_ << B00,
    		B10,
    		B12;
    	A123 = A_.inverse()*B_;
    	a = A123(2, 0);
    	b = A123(1, 0);
    	c = A123(0, 0);
    }
    void fitting::polynomial3D_fitting(vector<double>x, vector<double>y, vector<double>z, double &a, double &b, double &c){
    	int num_point = x.size();
    	MatrixXd A_(3, 3), B_(3, 1), A123(3, 1);
    	double A01(0.0), A02(0.0), A12(0.0), A22(0.0), B00(0.0), B10(0.0), B12(0.0);
    	for (int i_point = 0; i_point < num_point; i_point++)
    	{
    		double x_y = sqrt(pow(x[i_point], 2) + pow(y[i_point], 2));
    		A01 += x_y;
    		A02 += pow(x_y, 2);
    		A12 += pow(x_y, 3);
    		A22 += pow(x_y, 4);
    		B00 += z[i_point];
    		B10 += x_y * z[i_point];
    		B12 += pow(x_y, 2) * z[i_point];
    	}
    	A_ << num_point, A01, A02,
    		A01, A02, A12,
    		A02, A12, A22;
    	B_ << B00,
    		B10,
    		B12;
    	A123 = A_.inverse()*B_;
    	line_fitting(x, y, k_line, b_line);
    	a = A123(2, 0);
    	b = A123(1, 0);
    	c = A123(0, 0);
    	c_3d = c;
    	b_3d = b;
    	a_3d = a;
    }
    void fitting::polynomial3D_fitting_display(double step_){
    	PointT point_min_, point_max_;
    	getMinMax3D(*cloud, point_min_, point_max_);
    	//利用最小外包框的x值,向拟合的直线做垂足,垂足的交点即为三维曲线的端点值***********
    	int idx_minx, idx_maxy;//x取到最大值和最小值的点号索引
    	for (int i_point = 0; i_point < cloud->size();i_point++)
    	{
    		if (cloud->points[i_point].x == point_min_.x) idx_minx = i_point;
    		if (cloud->points[i_point].x == point_max_.x) idx_maxy = i_point;
    	}
    	float m_min = cloud->points[idx_minx].x + k_line*cloud->points[idx_minx].y;
    	float m_max = cloud->points[idx_maxy].x + k_line*cloud->points[idx_maxy].y;
    
    	float x_min = (m_min - b_line*k_line) / (1 + k_line*k_line);
    	float x_max= (m_max - b_line*k_line) / (1 + k_line*k_line);
    	//---------------------------------------------------------------------------------------
    	vector<double>xx, yy, zz;
    	int step_num = ceil((x_max - x_min) / step_);
    	vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
    	for (int i_ = 0; i_ < step_num + 1; i_++)
    	{
    		double tem_value = x_min + i_*step_;
    		if (tem_value>x_max)
    		{
    			tem_value = x_max;
    		}
    		xx.push_back(tem_value);
    		yy.push_back(k_line*xx[i_] + b_line);
    		double xxyy = sqrt(pow(xx[i_], 2) + pow(yy[i_], 2));
    		zz.push_back(c_3d + b_3d*xxyy + a_3d*pow(xxyy, 2));
    		points->InsertNextPoint(xx[i_], yy[i_], zz[i_]);
    	}
    	vtkSmartPointer<vtkPolyLine> polyLine = vtkSmartPointer<vtkPolyLine>::New();
    	vtkSmartPointer<vtkPolyData> polyData = vtkSmartPointer<vtkPolyData>::New();
    	vtkSmartPointer<vtkCellArray> cells = vtkSmartPointer<vtkCellArray>::New();
    	polyData->SetPoints(points);
    	polyLine->GetPointIds()->SetNumberOfIds(points->GetNumberOfPoints());
    	for (unsigned int i = 0; i < points->GetNumberOfPoints(); i++)
    		polyLine->GetPointIds()->SetId(i, i);
    	cells->InsertNextCell(polyLine);
    	polyData->SetLines(cells);
    	visualization::PCLVisualizer::Ptr viewer(new visualization::PCLVisualizer("最后拟合的多项式曲线"));
    	viewer->addModelFromPolyData(polyData, "1");
    	//*******************************************
    	PointCloud<PointT>::Ptr tem_point(new PointCloud<PointT>);
    	for (int i = 0; i < xx.size(); i++)
    	{
    		PointT point_;
    		point_.x = xx[i];
    		point_.y = yy[i];
    		point_.z = zz[i];
    		tem_point->push_back(point_);
    	}
    	visualization::PointCloudColorHandlerCustom<PointT>color1(tem_point, 255, 0, 0);
    	viewer->addPointCloud(tem_point, color1, "point1");
    	viewer->setPointCloudRenderingProperties(visualization::PCL_VISUALIZER_POINT_SIZE, 3, "point1");
    
    	PointCloud<PointT>::Ptr tem_point1(new PointCloud<PointT>);
    	for (int i = 0; i < cloud->size(); i++)
    	{
    		PointT point_1;
    		point_1.x = cloud->points[i].x;
    		point_1.y = cloud->points[i].y;
    		point_1.z = cloud->points[i].z;
    		tem_point1->push_back(point_1);
    	}
    	viewer->addPointCloud(tem_point1, "orginal");
    	viewer->setPointCloudRenderingProperties(visualization::PCL_VISUALIZER_POINT_SIZE, 2, "orginal");
    	//显示端点
    	PointCloud<PointT>::Ptr duandian_point(new PointCloud<PointT>);
    	duandian_point->push_back(tem_point->points[0]);
    	duandian_point->push_back(tem_point->points[tem_point->size() - 1]);
    	visualization::PointCloudColorHandlerCustom<PointT>color2(duandian_point, 0, 255, 255);
    	viewer->addPointCloud(duandian_point, color2, "duandian");
    	viewer->setPointCloudRenderingProperties(visualization::PCL_VISUALIZER_POINT_SIZE, 5, "duandian");
    	cout << "端点值1为:" << "X1= " << duandian_point->points[0].x << ", " << "Y1= " << duandian_point->points[0].y << ", " << "Z1= " << duandian_point->points[0].z << endl;
    	cout << "端点值2为:" << "X2= " << duandian_point->points[1].x << ", " << "Y2= " << duandian_point->points[1].y << ", " << "Z2= " << duandian_point->points[1].z << endl;
    	cout << "空间多项式曲线方程为: " << "z=" << a_3d << "*(x^2+y^2)+" << b_3d << "*sqrt(x^2+y^2)+" << c_3d << endl;
    	viewer->spin();
    	//拟合曲线+端点值+散点图二维平面展示,有需要可以取消注释----------------------------------------------------------
    	/*vector<double>vector_1, vector_2, vector_3, vector_4;
    	vector_1.push_back(duandian_point->points[0].x);
    	vector_1.push_back(duandian_point->points[1].x);
    	vector_2.push_back(duandian_point->points[0].y);
    	vector_2.push_back(duandian_point->points[1].y);
    	for (int i = 0; i < cloud->size();i++)
    	{
    		vector_3.push_back(cloud->points[i].x);
    		vector_4.push_back(cloud->points[i].y);
    	}
    	std::vector<double> func1(2, 0);
    	func1[0] = b_line;
    	func1[1] = k_line;
    	visualization::PCLPlotter *plot_line1(new visualization::PCLPlotter);
    	plot_line1->addPlotData(func1, vector_1[0], vector_1[1]);
    	plot_line1->addPlotData(vector_3, vector_4, "display", vtkChart::POINTS);//X,Y均为double型的向量
    	plot_line1->addPlotData(vector_1, vector_2, "display", vtkChart::POINTS);//X,Y均为double型的向量
    	plot_line1->setShowLegend(false);
    	plot_line1->plot();*/
    }
    void fitting::display_point(vector<double>vector_1, vector<double>vector_2){
    	visualization::PCLPlotter *plot_line1(new visualization::PCLPlotter);
    	plot_line1->addPlotData(vector_1, vector_2, "display", vtkChart::POINTS);//X,Y均为double型的向量
    	plot_line1->setShowLegend(false);
    	plot_line1->plot();
    }
    void fitting::display_line(vector<double>vector_1, vector<double>vector_2,double c, double b, double a){
    	visualization::PCLPlotter *plot_line1(new visualization::PCLPlotter);
    	std::vector<double> func1(3, 0);
    	func1[0] = c;
    	func1[1] = b;
    	func1[2] = a;
    	plot_line1->addPlotData(func1, point_min.x, point_max.x);
    	plot_line1->addPlotData(vector_1, vector_2, "display", vtkChart::POINTS);//X,Y均为double型的向量
    	plot_line1->setShowLegend(false);
    	plot_line1->plot();
    }
    
    //主函数
    #include <pcl/io/pcd_io.h>
    #include "fitting.h"
    using namespace std;
    using namespace pcl;
    using namespace Eigen;
    
    typedef PointXYZ PointT;
    
    int main() {
    	PointCloud<PointT>::Ptr cloud(new PointCloud<PointT>);
    	string ss("C:\\Users\\admin\\Desktop\\TEST22.pcd");
    	io::loadPCDFile(ss, *cloud);
    
    	vector<double>X, Y, Z;
    	for (int i_point = 0; i_point < cloud->size(); i_point++)
    	{
    		X.push_back(cloud->points[i_point].x);
    		Y.push_back(cloud->points[i_point].y);
    		Z.push_back(cloud->points[i_point].z);
    	}
    	vector<double>x_mean, y_mean, z_mean;
    	PointCloud<PointT>::Ptr point_mean(new PointCloud<PointT>);
    	double a, b, c,k_line, b_line;
    	fitting fit_;
    	fit_.setinputcloud(cloud);//点云输入
    	fit_.line_fitting(X, Y, k_line, b_line);//直线拟合
    	fit_.display_line(X, Y, b_line, k_line);//显示拟合的直线,必须先输入常量
    	fit_.polynomial2D_fitting(X, Z, a, b, c);
    	fit_.display_line(X, Z, c, b, a);//显示拟合的平面多项式曲线,输入顺序为 常量,一阶系数,二阶系数
    	fit_.grid_mean_xyz(0.5, -1, x_mean, y_mean, z_mean, point_mean);//0.5表示x方向的步长,-1(小于0就行)表示y方向不分段,如需分段,则设置相应步长
    	fit_.grid_mean_xyz_display(point_mean);//展示均值结果
    	fit_.display_point(X, Y);//显示散点
    	fit_.display_point(x_mean, y_mean);//显示均值散点
    	fit_.polynomial3D_fitting(x_mean, y_mean, z_mean, a, b, c);//用分段质心的均值去拟合3维曲线
    	//fit_.polynomial3D_fitting(X, Y, Z, a, b, c);//直接拟合
    	fit_.polynomial3D_fitting_display(0.5);//三维曲线展示
    
    	return 0;
    }
    

    运行结果:
    1.点云XOY平面直线拟合
    在这里插入图片描述
    放大后:
    在这里插入图片描述
    2. 平面多项式拟合
    在这里插入图片描述
    3. 分段质心展示
    在这里插入图片描述
    放大后,可以用分段质心结果去进行后续拟合:
    在这里插入图片描述
    4. 散点图:
    在这里插入图片描述
    分段质心:
    在这里插入图片描述
    5. 空间多项式拟合结果
    在这里插入图片描述
    放大后:
    在这里插入图片描述
    蓝色点为端点:
    在这里插入图片描述
    在这里插入图片描述
    拟合方程和端点值(只做了三维点的输出,其他方程自己看一下就好):
    在这里插入图片描述

    问题:先记录一下:

    过短的线,拟合出来有问题…后续解决

    展开全文
  • PCL 最小二乘拟合空间直线,输出结果为:一般式的表示形式和点向式的表示形式。

    一、算法原理

    1、一般式

      直线方程的一般式,即使用两平面方程联立的线性方程组表示:

    { a 11

    展开全文
  • 最小二乘法原理以及其平面拟合c++实现代码
  • 使用最小二乘法拟合平面,借助pcl点云库中的估计法矢的类来得到模型中点云曲面法矢估计。 是一个较为简单,常用的代码。txt文件
  • PCL最小二乘法进行平面拟合原理

    千次阅读 2022-01-24 21:07:38
    最小二乘法进行平面拟合一级目录二级目录三级目录 一级目录 二级目录 三级目录

    最小二乘法进行平面拟合原理

    1 最小二乘原理

    最小二乘法(又称最小平方法)是一种数学优化技术。它通过最小化误差的平方和寻找数据的最佳函数匹配。利用最小二乘法可以简便地求得未知的数据,并使得这些求得的数据与实际数据之间误差的平方和为最小 。在图像领域,最小二乘法常用于直线、曲线拟合、平面拟合等。
    首先我们来熟悉下最小二乘问题。考虑线性方程组Ax=b,其中A为mxn矩阵且m>n。这个方程一般不存在解x。因此,我们的任务是求最小化范数||A x ˉ \bar x xˉ-b||的向量 x ˉ \bar x xˉ。当x取遍所有值时,Ax将遍历A的整个列空间。因此我们的任务是在A的列空间中寻求最接近b的那个向量。因此,A x ˉ \bar x xˉ-b必然是与A的列空间垂直的向量。因此
    A T ( A x ˉ − b ) = 0 A^T(A\bar x-b)=0 AT(Axˉb)=0
    于是我们得到一个nxm的线性方程
    ( A T A ) x ˉ = A T b (A^TA)\bar x=A^Tb (ATA)xˉ=ATb
    可以通过 x ˉ = ( A T A ) − 1 A T b \bar x=(A^TA)^{-1}A^Tb xˉ=(ATA)1ATb来求解
    这个方程有多个叫法,有些称为正规方程,有些称为法线方程。这个解 x ˉ \bar x xˉ其实就是Ax=b的最小二乘解。
    很多人可能觉得这个不够直观,那么可以从另外一个角度去解释,举个例子:
    { x 1 + x 2 = 2 x 1 − x 2 = 1 x 1 + x 2 = 3 \begin{cases}x_1+x_2=2\\x_1-x_2=1\\x_1+x_2=3\end{cases} x1+x2=2x1x2=1x1+x2=3
    根据线性代数的知识,m个方程n个未知量m>n时通常无解,但是虽然不能求出Ax=b的解,那何不退而求其次,去寻找与解近似的向量 x ˉ \bar x xˉ
    那么如何定义与解相似,一般使用欧氏距离来进行度量,即两点间的距离,这其实很好理解,越相似,欧氏距离越近,这样求出的 x ˉ \bar x xˉ被称为最小二乘解。
    将我们开始举的例子写成矩阵形式:
    [ 1 1 1 − 1 1 1 ] [ x 1 x 2 ] = [ 2 1 3 ] \begin{bmatrix}1&1\\1&-1\\1&1\end{bmatrix}\begin{bmatrix}x_1\\x_2\end{bmatrix}=\begin{bmatrix}2\\1\\3\end{bmatrix} 111111[x1x2]=213
    写成等价方程为:
    x 1 [ 1 1 1 ] + x 2 [ 1 − 1 1 ] = [ 2 1 3 ] x_1\begin{bmatrix}1\\1\\1\end{bmatrix}+x_2\begin{bmatrix}1\\-1\\1\end{bmatrix}=\begin{bmatrix}2\\1\\3\end{bmatrix} x1111+x2111=213
    对于任意 mxn 方程组Ax=b都可以看做向量方程:
    x 1 v 1 + x 2 v 2 + . . . + x n v n = b x_1v_1+x_2v_2+...+x_nv_n=b x1v1+x2v2+...+xnvn=b
    其实也就是把b 看做A的列向量的线性组合,对应的系数即为 x i x_i xi ,对于举的例子来说,就是把b表示为另外两个三维向量的线性组合,由于三维空间中两个三维向量的组合生成一个平面,方程仅当b在这个平面上才有解,推广至m个方程n个未知量m>n 时也是相同的情况。如下图所示,向量A x ˉ \bar x xˉ-b(右下图虚线部分)与A所在平面垂直,也就是该平面的法向量。
    在这里插入图片描述
    以上就是对最小二乘的直观上的解释。当然,想把最小二乘法学透彻光看这些还是不够。因为还存在非线性,带约束和不带约束等情况。

    2 最小二乘拟合平面

    下面来介绍下最小二乘拟合平面的原理,已知空间中的一些离散点,对其进行平面拟合。首先,平面方程的一般式如下:
    a x + b y + c z + d = 0 ax+by+cz+d=0 ax+by+cz+d=0
    我们假设 c ≠ 0 c\neq0 c=0的情况。那么 z = − a c x − b c y − d c z=-\frac a c x- \frac b c y- \frac d c z=caxcbycd
    a 0 = − a c a_0=-\frac ac a0=ca, a 1 = − b c a_1=-\frac bc a1=cb , a 2 = − d c a_2=-\frac dc a2=cd
    于是 z = a 0 x + a 1 y + a 2 z=a_0 x+a_1 y+a_2 z=a0x+a1y+a2
    如果该平面内存在一系列的点集 { ( x , y , z ) ∣ ( x , y , z ) ∈ ( x i , y i , z i ) , i = 0 , 1 , 2 , . . . , n − 1 } \{(x,y,z)|(x,y,z)\in(x_i,y_i,z_i),i=0,1,2,...,n-1\} {(x,y,z)(x,y,z)(xi,yi,zi),i=0,1,2,...,n1}
    按照最小二乘原则,使得误差平方和最小。
    ∑ i = 0 n − 1 ( z − z i ) = m i n \sum_{i=0}^{n-1}{(z-z_i)}=min i=0n1(zzi)=min
    也就是指 S = ∑ i = 0 n − 1 ( a 0 x + a 1 y + a 2 − z i ) S=\sum_{i=0}^{n-1}{(a_0 x+a_1 y+a_2-z_i)} S=i=0n1(a0x+a1y+a2zi)最小,其中 a 0 , a 1 , a 2 a_0 ,a_1,a_2 a0,a1,a2是未知数。
    为了使得上式最小,要求 ∂ S ∂ a k = 0 , k = 0 , 1 , 2 \frac{\partial{S}} {\partial{a_k}}=0, k=0,1,2 akS=0,k=0,1,2
    { ∑ i = 0 n − 1 2 ( a 0 x i + a 1 y i + a 2 − z i ) x i = 0 对 a 0 求 偏 导 ∑ i = 0 n − 1 2 ( a 0 x i + a 1 y i + a 2 − z i ) y i = 0 对 a 1 求 偏 导 ∑ i = 0 n − 1 2 ( a 0 x i + a 1 y i + a 2 − z i ) = 0 对 a 2 求 偏 导 \begin{cases}\sum_{i=0}^{n-1}{2(a_0 x_i+a_1 y_i+a_2-z_i)x_i}=0\quad对a_0求偏导\\\sum_{i=0}^{n-1}{2(a_0 x_i+a_1 y_i+a_2-z_i)y_i}=0\quad对a_1求偏导\\\sum_{i=0}^{n-1}{2(a_0 x_i+a_1 y_i+a_2-z_i)}=0\quad\quad对a_2求偏导\end{cases} i=0n12(a0xi+a1yi+a2zi)xi=0a0i=0n12(a0xi+a1yi+a2zi)yi=0a1i=0n12(a0xi+a1yi+a2zi)=0a2
    化简得 { a 0 ∑ i = 0 n − 1 x i 2 + a 1 ∑ i = 0 n − 1 x i y i + a 2 ∑ i = 0 n − 1 x i = ∑ i = 0 n − 1 x i z i a 0 ∑ i = 0 n − 1 x i y i + a 1 ∑ i = 0 n − 1 y i 2 + a 2 ∑ i = 0 n − 1 y i = ∑ i = 0 n − 1 y i z i a 0 ∑ i = 0 n − 1 x i + a 1 ∑ i = 0 n − 1 y i + n a 2 = ∑ i = 0 n − 1 z i \begin{cases}a_0\sum_{i=0}^{n-1}{x_i^2}+a_1\sum_{i=0}^{n-1}{x_i y_i}+a_2\sum_{i=0}^{n-1}{x_i}=\sum_{i=0}^{n-1}{x_i z_i}\\a_0\sum_{i=0}^{n-1}{x_i y_i}+a_1\sum_{i=0}^{n-1}{y_i^2}+a_2\sum_{i=0}^{n-1}{y_i}=\sum_{i=0}^{n-1}{y_i z_i}\\a_0\sum_{i=0}^{n-1}{x_i}+a_1\sum_{i=0}^{n-1}{y_i}+na_2=\sum_{i=0}^{n-1}{z_i}\end{cases} a0i=0n1xi2+a1i=0n1xiyi+a2i=0n1xi=i=0n1xizia0i=0n1xiyi+a1i=0n1yi2+a2i=0n1yi=i=0n1yizia0i=0n1xi+a1i=0n1yi+na2=i=0n1zi
    可以将上面的式子写成矩阵形式,方便计算
    [ ∑ i = 0 n − 1 x i 2 ∑ i = 0 n − 1 x i y i ∑ i = 0 n − 1 x i ∑ i = 0 n − 1 x i y i ∑ i = 0 n − 1 y i 2 ∑ i = 0 n − 1 y i ∑ i = 0 n − 1 x i ∑ i = 0 n − 1 y i n ] [ a 0 a 1 a 2 ] = [ ∑ i = 0 n − 1 x i z i ∑ i = 0 n − 1 y i z i ∑ i = 0 n − 1 z i ] \begin{bmatrix}\sum_{i=0}^{n-1}{x_i^2}&\sum_{i=0}^{n-1}{x_i y_i}&\sum_{i=0}^{n-1}{x_i}\\ \sum_{i=0}^{n-1}{x_i y_i}&\sum_{i=0}^{n-1}{y_i^2}&\sum_{i=0}^{n-1}{y_i}\\ \sum_{i=0}^{n-1}{x_i}&\sum_{i=0}^{n-1}{y_i}&n \end{bmatrix}\begin{bmatrix} a_0\\a_1\\a_2\end{bmatrix}=\begin{bmatrix} \sum_{i=0}^{n-1}{x_i z_i}\\\sum_{i=0}^{n-1}{y_i z_i}\\\sum_{i=0}^{n-1}{z_i}\end{bmatrix} i=0n1xi2i=0n1xiyii=0n1xii=0n1xiyii=0n1yi2i=0n1yii=0n1xii=0n1yina0a1a2=i=0n1xizii=0n1yizii=0n1zi
    现在,我们马上可以得到我们想要的平面方程系数了,解这个方程有多种方法。大部分人对于SVD分解求解的方式比较熟悉,那下面介绍一种不怎么常用的方法,就是使用克拉默法则。那这个法则是什么意思呢?可以参考 链接.
    总的来说,如果求解 A x = b Ax=b Ax=b就是用b分别去替换等式坐标矩阵 A A A的每一列,求出替换后的 A ′ A^{'} A行列式,然后除以替换前的 A A A的行列式。分别求出 x i {x_i} xi.
    所以,所得的方程系数为
    a 0 = ∣ ∑ i = 0 n − 1 x i z i ∑ i = 0 n − 1 x i y i ∑ i = 0 n − 1 x i ∑ i = 0 n − 1 y i z i ∑ i = 0 n − 1 y i 2 ∑ i = 0 n − 1 y i ∑ i = 0 n − 1 z i ∑ i = 0 n − 1 y i n ∣ ∣ ∑ i = 0 n − 1 x i 2 ∑ i = 0 n − 1 x i y i ∑ i = 0 n − 1 x i ∑ i = 0 n − 1 x i y i ∑ i = 0 n − 1 y i 2 ∑ i = 0 n − 1 y i ∑ i = 0 n − 1 x i ∑ i = 0 n − 1 y i n ∣ a 1 = ∣ ∑ i = 0 n − 1 x i 2 ∑ i = 0 n − 1 x i z i ∑ i = 0 n − 1 x i ∑ i = 0 n − 1 x i y i ∑ i = 0 n − 1 y i z i ∑ i = 0 n − 1 y i ∑ i = 0 n − 1 x i ∑ i = 0 n − 1 z i n ∣ ∣ ∑ i = 0 n − 1 x i 2 ∑ i = 0 n − 1 x i y i ∑ i = 0 n − 1 x i ∑ i = 0 n − 1 x i y i ∑ i = 0 n − 1 y i 2 ∑ i = 0 n − 1 y i ∑ i = 0 n − 1 x i ∑ i = 0 n − 1 y i n ∣ a 2 = ∣ ∑ i = 0 n − 1 x i 2 ∑ i = 0 n − 1 x i y i ∑ i = 0 n − 1 x i z i ∑ i = 0 n − 1 x i y i ∑ i = 0 n − 1 y i 2 ∑ i = 0 n − 1 y i z i ∑ i = 0 n − 1 x i ∑ i = 0 n − 1 y i ∑ i = 0 n − 1 z i ∣ ∣ ∑ i = 0 n − 1 x i 2 ∑ i = 0 n − 1 x i y i ∑ i = 0 n − 1 x i ∑ i = 0 n − 1 x i y i ∑ i = 0 n − 1 y i 2 ∑ i = 0 n − 1 y i ∑ i = 0 n − 1 x i ∑ i = 0 n − 1 y i n ∣ a_0=\frac {\left|\begin{array}{cccc} \sum_{i=0}^{n-1}{x_i z_i} & \sum_{i=0}^{n-1}{x_i y_i}&\sum_{i=0}^{n-1}{x_i}\\ \sum_{i=0}^{n-1}{y_i z_i} & \sum_{i=0}^{n-1}{y_i^2} & \sum_{i=0}^{n-1}{y_i}\\ \sum_{i=0}^{n-1}{z_i} & \sum_{i=0}^{n-1}{y_i} & n \end{array}\right|} {\left|\begin{array}{cccc} \sum_{i=0}^{n-1}{x_i^2}&\sum_{i=0}^{n-1}{x_i y_i}&\sum_{i=0}^{n-1}{x_i}\\\sum_{i=0}^{n-1}{x_i y_i}&\sum_{i=0}^{n-1}{y_i^2}&\sum_{i=0}^{n-1}{y_i}\\\sum_{i=0}^{n-1}{x_i}&\sum_{i=0}^{n-1}{y_i}&n \end{array}\right|} \quad a_1=\frac {\left|\begin{array}{cccc} \sum_{i=0}^{n-1}{x_i^2}& \sum_{i=0}^{n-1}{x_i z_i} & \sum_{i=0}^{n-1}{x_i}\\ \sum_{i=0}^{n-1}{x_i y_i} & \sum_{i=0}^{n-1}{y_i z_i} & \sum_{i=0}^{n-1}{y_i}\\ \sum_{i=0}^{n-1}{x_i} & \sum_{i=0}^{n-1}{z_i} & n \end{array}\right|} {\left|\begin{array}{cccc} \sum_{i=0}^{n-1}{x_i^2}&\sum_{i=0}^{n-1}{x_i y_i}&\sum_{i=0}^{n-1}{x_i}\\\sum_{i=0}^{n-1}{x_i y_i}&\sum_{i=0}^{n-1}{y_i^2}&\sum_{i=0}^{n-1}{y_i}\\\sum_{i=0}^{n-1}{x_i}&\sum_{i=0}^{n-1}{y_i}&n \end{array}\right|} \quad a_2=\frac {\left|\begin{array}{cccc} \sum_{i=0}^{n-1}{x_i^2} & \sum_{i=0}^{n-1}{x_i y_i}& \sum_{i=0}^{n-1}{x_i z_i}\\ \sum_{i=0}^{n-1}{x_i y_i} & \sum_{i=0}^{n-1}{y_i^2}& \sum_{i=0}^{n-1}{y_i z_i}\\ \sum_{i=0}^{n-1}{x_i} & \sum_{i=0}^{n-1}{y_i} & \sum_{i=0}^{n-1}{z_i} \end{array}\right|} {\left|\begin{array}{cccc} \sum_{i=0}^{n-1}{x_i^2}&\sum_{i=0}^{n-1}{x_i y_i}&\sum_{i=0}^{n-1}{x_i}\\\sum_{i=0}^{n-1}{x_i y_i}&\sum_{i=0}^{n-1}{y_i^2}&\sum_{i=0}^{n-1}{y_i}\\\sum_{i=0}^{n-1}{x_i}&\sum_{i=0}^{n-1}{y_i}&n \end{array}\right|} a0=i=0n1xi2i=0n1xiyii=0n1xii=0n1xiyii=0n1yi2i=0n1yii=0n1xii=0n1yini=0n1xizii=0n1yizii=0n1zii=0n1xiyii=0n1yi2i=0n1yii=0n1xii=0n1yina1=i=0n1xi2i=0n1xiyii=0n1xii=0n1xiyii=0n1yi2i=0n1yii=0n1xii=0n1yini=0n1xi2i=0n1xiyii=0n1xii=0n1xizii=0n1yizii=0n1zii=0n1xii=0n1yina2=i=0n1xi2i=0n1xiyii=0n1xii=0n1xiyii=0n1yi2i=0n1yii=0n1xii=0n1yini=0n1xi2i=0n1xiyii=0n1xii=0n1xiyii=0n1yi2i=0n1yii=0n1xizii=0n1yizii=0n1zi
    到此,平面方程系数就完成了,如果有错误的地方烦请指正。
    参考链接.

    展开全文
  • matlab中对三维点云利用最小二乘法进行平面拟合,该程序本人自己写的一个子程序
  • void FitPlaneByLeastSquares(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud) { if (cloud->points.size() < 3) return; double meanX = 0, meanY = 0, meanZ = 0; double meanXX = 0, meanYY = ...
  • 2,进行最小二乘拟合,最终得到拟合面的点坐标。 二、示例 mls算法目前广泛应用于三维模型的法线计算,上采样,曲面平滑。 2.1 法线计算 PCL中可以先进行曲面重建,再根据曲面计算法线。 pcl::search::KdTree<pcl:...
  • C++实现最小二乘法拟合平面

    千次阅读 2020-09-04 11:13:10
    https://blog.csdn.net/u013541523/article/details/80135568),该链接为了推导介绍方便,只考虑了1种z=ax+by+d的情况,实际代码情况有3种情况,否则某些合理的点会导致逆矩阵为0求不出平面参数。 ...
  • 4show the codes // 10最小二乘拟合平面.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。 // /******************** 作者:xx 时间:2021.06.15 程序功能:c++实现点云最小二乘平面拟合 依赖项:pcl...
  • 点云拟合—圆柱面 非线性最小二乘实现

    万次阅读 多人点赞 2019-01-31 10:09:06
    首先是 PCL库自带的圆柱模型拟合,由于在查找最佳圆柱面的过程中会过滤很多点,因此考虑利用最小二乘的模型来拟合最接近实际点云的一个圆柱面,code如下,只是简单的调库,原理没仔细看: #include "pch.h" #...
  • 最小二乘B样条曲线和曲面拟合的渐进和迭代逼近
  • PCL:RANSAC 平面拟合

    千次阅读 热门讨论 2021-07-09 14:34:31
    本文介绍了平面方程的一般式和点法式、PCL中RANSAC平面拟合的原理及算法实现。
  • PCL 最小二乘拟合平面

    万次阅读 热门讨论 2021-02-27 09:09:33
    最小二乘法拟合平面PCL实现
  • i:表示q_i在平面H中的坐标xi​,yi​:表示qi​在平面H中的坐标 拟合多项式的最小二乘误差如下:拟合多项式的最小二乘误差如下:拟合多项式的最小二乘误差如下: ∑i=1N(g(xi.yI)−fi)2θ(∣∣pi−q∣∣)\sum _{i=1}^N...
  • 基于RANSAC算法PCL点云拟合平面整理

    万次阅读 多人点赞 2018-11-30 13:51:50
    文章目录前言首先讲一下PCL是什么接下来是RANSAC算法解决思路PCL拟合平面手写RANSAC算法代码流程然而,出现了问题!!!发现问题mt19937 is what? 前言 这个项目,跟我专业不大相关,准确的说,是帮助学姐的做一...
  • 1.最小二乘拟合 最小二乘拟合是一种数学上的近似和优化,利用已知的数据得出一条直线或者曲线,使之在坐标系上与已知数据之间的距离的平方和最小。 2.RANSAC算法 参见王荣先老师的博文 ...
  • 最小二乘法曲面拟合

    2015-03-29 12:49:06
    可以实现利用最小二乘法对直线 圆 球 平面拟合 ,并给出误差评价
  • 一、原理讲解 通过实验获得一些列的观测数值(假设为三个): 其每个样本观测值对应的精确值为: 这里假设其观测值对应的准确值为: ...代码复现的统计学第一章-最小二乘拟合正弦函数,正则化,其官方py...
  • 使用两种思路进行直线拟合: 1.利用逆矩阵思想 --------------进行下列公式的推导需要理解逆矩阵(求A矩阵的逆矩阵,则A矩阵必须是方阵)的知识: (1)为什么要引入逆矩阵呢? 逆矩阵可以类比成数字的倒数,...
  • PCL:RANSAC球面拟合

    千次阅读 2021-12-16 18:20:19
    本文介绍了PCL进行RANSAC球面拟合的原理与方法。
  • 点云是离散的,我们一般从中拟合平面或者拟合别的几何形状(曲面,圆柱等)来帮助我们分析点云数据。
  • PCL拟合多条直线

    千次阅读 热门讨论 2021-11-16 15:21:14
    手写ransac拟合多条直线 投影+提取边缘点后的点云,然后拟合,直接上代码。 还是先看效果图吧。。。 恩,,每次选择拟合直线的点数放在了函数里,代码写的较差,见谅哈哈哈 #include <pcl/io/pcd_io.h> #...
  • 转自:https://blog.csdn.net/lming_08/article/details/21171491之前对PCL库计算三维点云数据的曲面法向量有过介绍,点云的曲面法向量估计,PCL库是采用主成份分析方法的,近几天通过理论推导发现最小二乘法应该也...
  • CloudCompare——点云平面拟合

    千次阅读 2021-12-05 09:34:24
    使用CloudCompare进行点云的平面拟合
  • Point Cloud Library 0.0 documentationhttps://pcl.readthedocs.io/projects/tutorials/en/latest/resampling.html#moving-least-squares 在本篇教程中,将使用移动最小二乘法(MLS)表面重建的方式对噪点数据进行...
  • (1)从样本集N中,随机选n个点作为初始子集(但是所选取的这n个点点必须能够表达待拟合的模型,比如拟合直线的话最少需要2个点,所以n=2(这是正常求解方式),但是基于最小二乘的思想的化最少需要大于2个点,所以n...
  • RANSAC多平面提取RANSAC思想个人理解实验代码实验结果 ...最后,根据一致性集合中的数据(可以认为是可靠的数据了),用最小二乘的方法重新估计模型。 个人理解 可以简单认为从原始点云中随机选取3个点
  • 也可以采用其他的方法,比如 SAC_LMEdS 最小中值法: LMedS的做法很简单,就是从样本中随机抽出N个样本子集,使用最大似然(通常是最小二乘) 对每个子集计算模型参数和该模型的偏差,记录该模型参数及子集中所有...

空空如也

空空如也

1 2 3 4 5 ... 9
收藏数 172
精华内容 68
关键字:

pcl使用最小二乘拟合平面