• 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->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->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("最后拟合的多项式曲线"));
//*******************************************
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->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->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->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->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->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->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>);

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文件
• 最小二乘法进行平面拟合一级目录二级目录三级目录 一级目录 二级目录 三级目录

# 1 最小二乘原理

最小二乘法（又称最小平方法）是一种数学优化技术。它通过最小化误差的平方和寻找数据的最佳函数匹配。利用最小二乘法可以简便地求得未知的数据，并使得这些求得的数据与实际数据之间误差的平方和为最小 。在图像领域，最小二乘法常用于直线、曲线拟合、平面拟合等。
首先我们来熟悉下最小二乘问题。考虑线性方程组Ax=b，其中A为mxn矩阵且m>n。这个方程一般不存在解x。因此，我们的任务是求最小化范数||A x ˉ \bar x -b||的向量 x ˉ \bar x 。当x取遍所有值时，Ax将遍历A的整个列空间。因此我们的任务是在A的列空间中寻求最接近b的那个向量。因此，A x ˉ \bar x -b必然是与A的列空间垂直的向量。因此
A T ( A x ˉ − b ) = 0 A^T(A\bar x-b)=0
于是我们得到一个nxm的线性方程
( A T A ) x ˉ = A T b (A^TA)\bar x=A^Tb
可以通过 x ˉ = ( A T A ) − 1 A T b \bar x=(A^TA)^{-1}A^Tb 来求解
这个方程有多个叫法，有些称为正规方程，有些称为法线方程。这个解 x ˉ \bar 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}
根据线性代数的知识，m个方程n个未知量m>n时通常无解，但是虽然不能求出Ax=b的解，那何不退而求其次，去寻找与解近似的向量 x ˉ \bar x
那么如何定义与解相似，一般使用欧氏距离来进行度量，即两点间的距离，这其实很好理解，越相似，欧氏距离越近，这样求出的 x ˉ \bar 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}
写成等价方程为：
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}
对于任意 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
其实也就是把b 看做A的列向量的线性组合，对应的系数即为 x i x_i ，对于举的例子来说，就是把b表示为另外两个三维向量的线性组合，由于三维空间中两个三维向量的组合生成一个平面，方程仅当b在这个平面上才有解，推广至m个方程n个未知量m>n 时也是相同的情况。如下图所示，向量A x ˉ \bar x -b(右下图虚线部分)与A所在平面垂直，也就是该平面的法向量。

以上就是对最小二乘的直观上的解释。当然，想把最小二乘法学透彻光看这些还是不够。因为还存在非线性，带约束和不带约束等情况。

# 2 最小二乘拟合平面

下面来介绍下最小二乘拟合平面的原理，已知空间中的一些离散点，对其进行平面拟合。首先，平面方程的一般式如下：
a x + b y + c z + d = 0 ax+by+cz+d=0
我们假设 c ≠ 0 c\neq0 的情况。那么 z = − a c x − b c y − d c z=-\frac a c x- \frac b c y- \frac d c
a 0 = − a c a_0=-\frac ac , a 1 = − b c a_1=-\frac bc , a 2 = − d c a_2=-\frac dc
于是 z = a 0 x + a 1 y + a 2 z=a_0 x+a_1 y+a_2
如果该平面内存在一系列的点集 { ( 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\}
按照最小二乘原则，使得误差平方和最小。
∑ i = 0 n − 1 ( z − z i ) = m i n \sum_{i=0}^{n-1}{(z-z_i)}=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)} 最小，其中 a 0 , a 1 , a 2 a_0 ,a_1,a_2 是未知数。
为了使得上式最小，要求 ∂ S ∂ a k = 0 , k = 0 , 1 , 2 \frac{\partial{S}} {\partial{a_k}}=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}
化简得 { 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}
可以将上面的式子写成矩阵形式，方便计算
[ ∑ 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}
现在，我们马上可以得到我们想要的平面方程系数了,解这个方程有多种方法。大部分人对于SVD分解求解的方式比较熟悉，那下面介绍一种不怎么常用的方法，就是使用克拉默法则。那这个法则是什么意思呢？可以参考 链接.
总的来说,如果求解 A x = b Ax=b 就是用b分别去替换等式坐标矩阵 A A 的每一列，求出替换后的 A ′ A^{'} 行列式，然后除以替换前的 A A 的行列式。分别求出 x i {x_i} .
所以，所得的方程系数为
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|}
到此，平面方程系数就完成了，如果有错误的地方烦请指正。
参考链接.

展开全文
• 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:...
• 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...
• 首先是 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算法 参见王荣先老师的博文 ...
• 可以实现利用最小二乘法对直线 圆 球 平面拟合 ，并给出误差评价
• 一、原理讲解 通过实验获得一些列的观测数值(假设为三个)： 其每个样本观测值对应的精确值为： 这里假设其观测值对应的准确值为： ...代码复现的统计学第一章-最小二乘拟合正弦函数，正则化，其官方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进行点云的平面拟合
• 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个样本子集，使用最大似然（通常是最小二乘） 对每个子集计算模型参数和该模型的偏差，记录该模型参数及子集中所有...

...