-
更多相关内容
-
C++/PCL:最小二乘拟合平面直线,平面多项式曲线,空间多项式曲线
2019-09-04 20:17:36//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 最小二乘拟合空间直线
2021-12-13 20:51:58 -
空间平面的最小二乘拟合
2012-04-26 18:13:57最小二乘法原理以及其平面拟合c++实现代码 -
基于最小二乘法拟合平面得到点云曲面法矢估计
2019-10-14 15:21:18使用最小二乘法拟合平面,借助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=2x1−x2=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} ⎣⎡1111−11⎦⎤[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} x1⎣⎡111⎦⎤+x2⎣⎡1−11⎦⎤=⎣⎡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=−cax−cby−cd
令 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,...,n−1}
按照最小二乘原则,使得误差平方和最小。
∑ i = 0 n − 1 ( z − z i ) = m i n \sum_{i=0}^{n-1}{(z-z_i)}=min i=0∑n−1(z−zi)=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=0n−1(a0x+a1y+a2−zi)最小,其中 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 ∂ak∂S=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=0n−12(a0xi+a1yi+a2−zi)xi=0对a0求偏导∑i=0n−12(a0xi+a1yi+a2−zi)yi=0对a1求偏导∑i=0n−12(a0xi+a1yi+a2−zi)=0对a2求偏导
化简得 { 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} ⎩⎪⎨⎪⎧a0∑i=0n−1xi2+a1∑i=0n−1xiyi+a2∑i=0n−1xi=∑i=0n−1xizia0∑i=0n−1xiyi+a1∑i=0n−1yi2+a2∑i=0n−1yi=∑i=0n−1yizia0∑i=0n−1xi+a1∑i=0n−1yi+na2=∑i=0n−1zi
可以将上面的式子写成矩阵形式,方便计算
[ ∑ 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=0n−1xi2∑i=0n−1xiyi∑i=0n−1xi∑i=0n−1xiyi∑i=0n−1yi2∑i=0n−1yi∑i=0n−1xi∑i=0n−1yin⎦⎤⎣⎡a0a1a2⎦⎤=⎣⎡∑i=0n−1xizi∑i=0n−1yizi∑i=0n−1zi⎦⎤
现在,我们马上可以得到我们想要的平面方程系数了,解这个方程有多种方法。大部分人对于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=0n−1xi2∑i=0n−1xiyi∑i=0n−1xi∑i=0n−1xiyi∑i=0n−1yi2∑i=0n−1yi∑i=0n−1xi∑i=0n−1yin∣∣∣∣∣∣∣∣∣∣∣∣∑i=0n−1xizi∑i=0n−1yizi∑i=0n−1zi∑i=0n−1xiyi∑i=0n−1yi2∑i=0n−1yi∑i=0n−1xi∑i=0n−1yin∣∣∣∣∣∣a1=∣∣∣∣∣∣∑i=0n−1xi2∑i=0n−1xiyi∑i=0n−1xi∑i=0n−1xiyi∑i=0n−1yi2∑i=0n−1yi∑i=0n−1xi∑i=0n−1yin∣∣∣∣∣∣∣∣∣∣∣∣∑i=0n−1xi2∑i=0n−1xiyi∑i=0n−1xi∑i=0n−1xizi∑i=0n−1yizi∑i=0n−1zi∑i=0n−1xi∑i=0n−1yin∣∣∣∣∣∣a2=∣∣∣∣∣∣∑i=0n−1xi2∑i=0n−1xiyi∑i=0n−1xi∑i=0n−1xiyi∑i=0n−1yi2∑i=0n−1yi∑i=0n−1xi∑i=0n−1yin∣∣∣∣∣∣∣∣∣∣∣∣∑i=0n−1xi2∑i=0n−1xiyi∑i=0n−1xi∑i=0n−1xiyi∑i=0n−1yi2∑i=0n−1yi∑i=0n−1xizi∑i=0n−1yizi∑i=0n−1zi∣∣∣∣∣∣
到此,平面方程系数就完成了,如果有错误的地方烦请指正。
参考链接. -
基于最小二乘发的平面拟合
2018-03-31 16:44:34matlab中对三维点云利用最小二乘法进行平面拟合,该程序本人自己写的一个子程序 -
最小二乘法拟合平面实现
2021-06-06 20:56:38void FitPlaneByLeastSquares(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud) { if (cloud->points.size() < 3) return; double meanX = 0, meanY = 0, meanZ = 0; double meanXX = 0, meanYY = ... -
基于PCL的MLS(移动最小二乘)算法简介与示例
2022-03-10 13:09:502,进行最小二乘拟合,最终得到拟合面的点坐标。 二、示例 mls算法目前广泛应用于三维模型的法线计算,上采样,曲面平滑。 2.1 法线计算 PCL中可以先进行曲面重建,再根据曲面计算法线。 pcl::search::KdTree<pcl:... -
C++实现最小二乘法拟合平面
2020-09-04 11:13:10https://blog.csdn.net/u013541523/article/details/80135568),该链接为了推导介绍方便,只考虑了1种z=ax+by+d的情况,实际代码情况有3种情况,否则某些合理的点会导致逆矩阵为0求不出平面参数。 ... -
最小二乘求解点云平面方程及其对应法向量
2021-06-15 17:59:134show the codes // 10最小二乘拟合平面.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。 // /******************** 作者:xx 时间:2021.06.15 程序功能:c++实现点云最小二乘平面拟合 依赖项:pcl... -
点云拟合—圆柱面 非线性最小二乘实现
2019-01-31 10:09:06首先是 PCL库自带的圆柱模型拟合,由于在查找最佳圆柱面的过程中会过滤很多点,因此考虑利用最小二乘的模型来拟合最接近实际点云的一个圆柱面,code如下,只是简单的调库,原理没仔细看: #include "pch.h" #... -
最小二乘B样条曲线和曲面拟合的渐进和迭代逼近
2021-04-02 17:47:39最小二乘B样条曲线和曲面拟合的渐进和迭代逼近 -
PCL:RANSAC 平面拟合
2021-07-09 14:34:31本文介绍了平面方程的一般式和点法式、PCL中RANSAC平面拟合的原理及算法实现。 -
PCL 最小二乘拟合平面
2021-02-27 09:09:33最小二乘法拟合平面的PCL实现 -
pcl::MovingLeastSquares滑动最小二乘
2021-01-04 17:54:21i:表示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? 前言 这个项目,跟我专业不大相关,准确的说,是帮助学姐的做一... -
最小二乘拟合&基于RANSAC的直线拟合&椭圆拟合
2016-03-31 10:51:131.最小二乘拟合 最小二乘拟合是一种数学上的近似和优化,利用已知的数据得出一条直线或者曲线,使之在坐标系上与已知数据之间的距离的平方和最小。 2.RANSAC算法 参见王荣先老师的博文 ... -
最小二乘法曲面拟合
2015-03-29 12:49:06可以实现利用最小二乘法对直线 圆 球 平面等拟合 ,并给出误差评价 -
点云配准5 -辅助知识 最小二乘法代码实现拟合曲线(C++)
2021-11-20 23:17:23一、原理讲解 通过实验获得一些列的观测数值(假设为三个): 其每个样本观测值对应的精确值为: 这里假设其观测值对应的准确值为: ...代码复现的统计学第一章-最小二乘拟合正弦函数,正则化,其官方py... -
PCL:拟合平面直线和曲线以及空间曲线的原理到算法实现
2020-05-20 16:26:21使用两种思路进行直线拟合: 1.利用逆矩阵思想 --------------进行下列公式的推导需要理解逆矩阵(求A矩阵的逆矩阵,则A矩阵必须是方阵)的知识: (1)为什么要引入逆矩阵呢? 逆矩阵可以类比成数字的倒数,... -
PCL:RANSAC球面拟合
2021-12-16 18:20:19本文介绍了PCL进行RANSAC球面拟合的原理与方法。 -
PCL点云处理之基于主成分分析的平面拟合[PCA](四十七)
2022-01-15 17:33:16点云是离散的,我们一般从中拟合平面或者拟合别的几何形状(曲面,圆柱等)来帮助我们分析点云数据。 -
PCL拟合多条直线
2021-11-16 15:21:14手写ransac拟合多条直线 投影+提取边缘点后的点云,然后拟合,直接上代码。 还是先看效果图吧。。。 恩,,每次选择拟合直线的点数放在了函数里,代码写的较差,见谅哈哈哈 #include <pcl/io/pcd_io.h> #... -
基于最小二乘法估计点云的曲面法向量
2018-04-19 14:17:45转自:https://blog.csdn.net/lming_08/article/details/21171491之前对PCL库计算三维点云数据的曲面法向量有过介绍,点云的曲面法向量估计,PCL库是采用主成份分析方法的,近几天通过理论推导发现最小二乘法应该也... -
CloudCompare——点云平面拟合
2021-12-05 09:34:24使用CloudCompare进行点云的平面拟合 -
PCL教程-点云表面重建之使用移动最小二乘法(MLS)
2021-09-23 15:20:43Point Cloud Library 0.0 documentationhttps://pcl.readthedocs.io/projects/tutorials/en/latest/resampling.html#moving-least-squares 在本篇教程中,将使用移动最小二乘法(MLS)表面重建的方式对噪点数据进行... -
RANSAC算法(2):(拟合平面)本文以地面为基础以及源码分布解读
2020-05-23 23:27:51(1)从样本集N中,随机选n个点作为初始子集(但是所选取的这n个点点必须能够表达待拟合的模型,比如拟合直线的话最少需要2个点,所以n=2(这是正常求解方式),但是基于最小二乘的思想的化最少需要大于2个点,所以n... -
PCL点云处理之ransac多平面提取(六)
2021-08-12 08:45:13RANSAC多平面提取RANSAC思想个人理解实验代码实验结果 ...最后,根据一致性集合中的数据(可以认为是可靠的数据了),用最小二乘的方法重新估计模型。 个人理解 可以简单认为从原始点云中随机选取3个点 -
三维目标检测:(三)用PCL中的RANSAC算法去除点云的平面点
2021-11-20 18:52:39也可以采用其他的方法,比如 SAC_LMEdS 最小中值法: LMedS的做法很简单,就是从样本中随机抽出N个样本子集,使用最大似然(通常是最小二乘) 对每个子集计算模型参数和该模型的偏差,记录该模型参数及子集中所有...