使用兩種思路進(jìn)行直線(xiàn)擬合：

1.利用逆矩陣思想

--------------進(jìn)行下列公式的推導(dǎo)需要理解逆矩陣（求A矩陣的逆矩陣，則A矩陣必須是方陣）的知識(shí)：

（1）為什么要引入逆矩陣呢？

逆矩陣可以類(lèi)比成數(shù)字的倒數(shù)，比如數(shù)字5的倒數(shù)是1/5，矩陣A的“倒數(shù)”是A的逆矩陣。5*(1/5)=1, A*（A的逆矩陣） = I，I是單位矩陣。引入逆矩陣的原因之一是用來(lái)實(shí)現(xiàn)矩陣的除法。比如有矩陣X，A，B，其中X*A = B，我們要求X矩陣的值。本能來(lái)說(shuō)，我們只需要將B/A就可以得到X矩陣了。但是對(duì)于矩陣來(lái)說(shuō)，不存在直接相除的概念。我們需要借助逆矩陣，間接實(shí)現(xiàn)矩陣的除法。具體的做法是等式兩邊在相同位置同時(shí)乘以矩陣A的逆矩陣，如下所示，X*A*（A的逆矩陣）= B*（A的逆矩陣）。由于A(yíng)*（A的逆矩陣） = I，即單位矩陣，任何矩陣乘以單位矩陣的結(jié)果都是其本身。所以，我們可以得到X = ?B*（A的逆矩陣）。

（2）什么是逆矩陣？

設(shè)A是數(shù)域上的一個(gè)n階方陣，若在相同數(shù)域上存在另一個(gè)n階矩陣B，使得： AB=BA=E。 則我們稱(chēng)B是A的逆矩陣，而A則被稱(chēng)為可逆矩陣。E為n階單位矩陣。

（3）逆矩陣有哪些性質(zhì)？

（4）如何計(jì)算一個(gè)n階方陣的逆矩陣？

https://www.sohu.com/a/226465524_224832

首先利用逆矩陣思想來(lái)進(jìn)行平面直線(xiàn)的推導(dǎo)過(guò)程如下：

? ?

平面中的多項(xiàng)式表示平面中的曲線(xiàn)：（曲線(xiàn)系數(shù)推導(dǎo)過(guò)程如上右圖）

以下是詳細(xì)代碼展示：（后邊代碼沒(méi)有注釋啊啊，因?yàn)楹筮呌械奈乙膊皇呛苊靼?#xff09;

//頭文件
#pragma once
//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();//構(gòu)造函數(shù)~fitting();//析構(gòu)函數(shù)//以下都是聲明函數(shù)即類(lèi)的成員函數(shù)void setinputcloud(PointCloud<PointT>::Ptr input_cloud);//點(diǎn)云輸入//投影至XOY，規(guī)則格網(wǎng)，求每個(gè)格網(wǎng)內(nèi)點(diǎn)云坐標(biāo)均值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，規(guī)則格網(wǎng)，求每個(gè)格網(wǎng)內(nèi)點(diǎn)云坐標(biāo)均值void grid_mean_xyz_display(PointCloud<PointT>::Ptr new_cloud);//均值結(jié)果三維展示void line_fitting(vector<double>x, vector<double>y, double &k, double &b);//y=kx+b//平面直線(xiàn)void polynomial2D_fitting(vector<double>x, vector<double>y, double &a, double &b, double &c);//y=a*x^2+b*x+c;//平面曲線(xiàn)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//空間曲線(xiàn)void polynomial3D_fitting_display(double step_);//三維曲線(xiàn)展示void display_point(vector<double>vector_1, vector<double>vector_2);//散點(diǎn)圖顯示void display_line(vector<double>vector_1, vector<double>vector_2, double c, double b, double a = 0);//擬合的平面直線(xiàn)或曲線(xiàn)展示
private://類(lèi)的私有成員通過(guò)成員函數(shù)進(jìn)行訪(fǎng)問(wèn)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()//構(gòu)造函數(shù)
{
}
fitting::~fitting()//析構(gòu)函數(shù)
{cloud->clear();
}
//定義點(diǎn)云輸入函數(shù)
void fitting::setinputcloud(PointCloud<PointT>::Ptr input_cloud) 
{cloud = input_cloud;getMinMax3D(*input_cloud, point_min, point_max);//getMinMax3D該函數(shù)輸入點(diǎn)云數(shù)據(jù)，將所有xyz中最小的值和xyz中最大的值輸出到pcl::PointXYZ中，即pcl::PointXYZ min_p, max_p;
}
//將點(diǎn)云數(shù)據(jù)投影至XOY，并劃分為規(guī)則格網(wǎng)，求每個(gè)格網(wǎng)內(nèi)點(diǎn)云坐標(biāo)均值
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)//表示如果輸入的分辨率小于0，則不進(jìn)行Y方向的分段處理{y_resolution = point_max.y - point_min.y;//輸入的分辨率小于0則Y方向的分辨率重新調(diào)整為Y值的最大值與最小值之差}int raster_rows, raster_cols;//定義行數(shù)row和列數(shù)colraster_rows = ceil((point_max.x - point_min.x) / x_resolution);//ceil() 函數(shù)向上舍入為最接近的整數(shù)。如需向下舍入為最接近的整數(shù)，請(qǐng)查看 floor(),函數(shù)floor(123.5)返回123。如需對(duì)浮點(diǎn)數(shù)進(jìn)行四舍五入，請(qǐng)查看 round() 函數(shù)。raster_cols = ceil((point_max.y - point_min.y) / y_resolution);// ceil(123.5)返回大于或者等于指定表達(dá)式的最小整數(shù)，返回124//容器都是類(lèi)模板，是一種可變長(zhǎng)數(shù)組。它們實(shí)例化后就成為容器類(lèi)。用容器類(lèi)定義的對(duì)象稱(chēng)為容器對(duì)象。//vector 是順序容器的一種。vector 是可變長(zhǎng)的動(dòng)態(tài)數(shù)組vector<int>idx_point;//idx_point就是容器對(duì)象//點(diǎn)的索引vector<vector<vector<float>>>row_col;//row_col是一個(gè)三重?cái)?shù)組，第一重是實(shí)際行號(hào)（就是有點(diǎn)的）（因?yàn)樯鲜鰟澐值膔aster_rows是總行數(shù)有可能有的行列里邊沒(méi)有投影進(jìn)去點(diǎn)），第二重是實(shí)際列號(hào)第三重是該行列中的點(diǎn)坐標(biāo)及索引即（行，列，XYZ索引）vector<vector<float>>col_;//col_是一個(gè)二重?cái)?shù)組，表示上述三重?cái)?shù)組中第二重存放的是列vector<float>vector_4;//vector_4是個(gè)一重?cái)?shù)組，存放的是點(diǎn)坐標(biāo)及索引vector_4.resize(4);//將vector_4的現(xiàn)有元素個(gè)數(shù)調(diào)整至4個(gè)，多則刪，少則補(bǔ)，其值隨機(jī)col_.resize(raster_cols, vector_4);//將col_的現(xiàn)有元素個(gè)數(shù)調(diào)整至raster_cols個(gè)，多則刪，少則補(bǔ)，其值為col_（因?yàn)閞aster_cols是劃分定義的行數(shù)）row_col.resize(raster_rows, col_);//將row_col的現(xiàn)有元素個(gè)數(shù)調(diào)整至raster_rows個(gè)，多則刪，少則補(bǔ)，其值為col_int point_num = cloud->size();//填充三重?cái)?shù)組for (int i_point = 0; i_point < point_num; i_point++){//找到當(dāng)前點(diǎn)所在的(行)(列)int row_idx = ceil((cloud->points[i_point].x - point_min.x) / x_resolution) - 1;//該點(diǎn)當(dāng)前所在的行（有可能很多個(gè)點(diǎn)算出來(lái)都在這個(gè)行內(nèi)比如都是3.3就取第三行）int col_idx = ceil((cloud->points[i_point].y - point_min.y) / y_resolution) - 1;//計(jì)算當(dāng)前Y值被劃分到了哪個(gè)列if (row_idx < 0)row_idx = 0;//如果行列都小于0,則將row_idx賦值為0；（此處不存在因?yàn)闇p去的就是最小值不可能為負(fù)）if (col_idx < 0)col_idx = 0;//找到了行列后把(xyz索引)塞進(jìn)去row_col[row_idx][col_idx][0] += cloud->points[i_point].x;//把落盡該格子內(nèi)的所有點(diǎn)的所有x值相加，為了下邊求格網(wǎng)的平均值row_col[row_idx][col_idx][1] += cloud->points[i_point].y;//把點(diǎn)云坐標(biāo)的所有y值相加row_col[row_idx][col_idx][2] += cloud->points[i_point].z;//把點(diǎn)云坐標(biāo)的所有z值相加row_col[row_idx][col_idx][3] += 1;//落到該網(wǎng)格的總的點(diǎn)數(shù)就是一共有多少個(gè)點(diǎn)落到目前這個(gè)網(wǎng)格子}PointT point_mean_tem;//求每個(gè)網(wǎng)格的平均值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)//不等于0就表明{double x_mean_tem = row_col[i_row][i_col][0] / row_col[i_row][i_col][3];//求x的平均（該網(wǎng)格內(nèi)點(diǎn)的x總和除以點(diǎn)的總個(gè)數(shù)）double y_mean_tem = row_col[i_row][i_col][1] / row_col[i_row][i_col][3];//求y的平均double z_mean_tem = row_col[i_row][i_col][2] / row_col[i_row][i_col][3];//求z的平均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);//將他們的平均值保存在新的點(diǎn)云中}}}
}
//每個(gè)格網(wǎng)內(nèi)點(diǎn)云坐標(biāo)均值結(jié)果三維展示
void fitting::grid_mean_xyz_display(PointCloud<PointT>::Ptr new_cloud) {visualization::PCLVisualizer::Ptr view(new visualization::PCLVisualizer("分段質(zhì)心擬合"));//可視化界面標(biāo)題//pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> sources_cloud_color(source,250,0,0); //這句話(huà)的意思是：對(duì)輸入為pcl::PointXYZ類(lèi)型的點(diǎn)云，著色為紅色。其中，source表示真正處理的點(diǎn)云，sources_cloud_color表示處理結(jié)果.visualization::PointCloudColorHandlerCustom<PointT>color_1(new_cloud, 255, 0, 0);//創(chuàng)建一個(gè)自定義顏色處理器PointCloudColorHandlerCustom對(duì)象，給點(diǎn)云著色并設(shè)置顏色為紅色//view->addPointCloud(source,sources_cloud_color,"sources_cloud_v1",v1); //將點(diǎn)云source,處理結(jié)果sources_cloud_color,添加到視圖中，其中,雙引號(hào)中的sources_cloud_v1,表示該點(diǎn)云的”標(biāo)簽“，我們依然可以稱(chēng)之為”名字“，之所以設(shè)置各個(gè)處理點(diǎn)云的名字，是為了在后續(xù)處理中易于區(qū)分; v1表是添加到哪個(gè)視圖窗口（pcl中可設(shè)置多窗口模式）view->addPointCloud(new_cloud, color_1, "11");//加載點(diǎn)云//view->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,3,"sources_cloud_v1"); //設(shè)置點(diǎn)云屬性. 其中PCL_VISUALIZER_POINT_SIZE表示設(shè)置點(diǎn)的大小為3,雙引號(hào)中”sources_cloud_v1“,就是步驟2中所說(shuō)的標(biāo)簽。//view->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY,1,"sources_cloud_v1"); //主要用來(lái)設(shè)置標(biāo)簽點(diǎn)云的不透明度，表示對(duì)標(biāo)簽名字為"sources_cloud_v1"的標(biāo)簽點(diǎn)云設(shè)置不透明度為1,也就是說(shuō)透明度為0. 默認(rèn)情況下完全不透明。view->setPointCloudRenderingProperties(visualization::PCL_VISUALIZER_POINT_SIZE, 3, "11");PointCloud<PointT>::Ptr new_cloud_final(new PointCloud<PointT>);//創(chuàng)建一個(gè)新的點(diǎn)云用于存儲(chǔ)結(jié)果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();
}
//擬合平面直線(xiàn)y=kx+b
//引用的本質(zhì)：在定義或聲明函數(shù)時(shí)，我們可以將函數(shù)的形參指定為引用的形式，這樣在調(diào)用函數(shù)時(shí)就會(huì)將實(shí)參和形參綁定在一起，讓它們都指代同一份數(shù)據(jù)。如此一來(lái)，如果在函數(shù)體中修改了形參的數(shù)據(jù)，那么實(shí)參的數(shù)據(jù)也會(huì)被修改，從而擁有“在函數(shù)內(nèi)部影響函數(shù)外部數(shù)據(jù)”的效果。
void fitting::line_fitting(vector<double>x, vector<double>y, double &k, double &b) //此函數(shù)輸入是x,y；輸出是k,b；是C++中的引用知識(shí)（“在函數(shù)內(nèi)部影響函數(shù)外部數(shù)據(jù)”）
{MatrixXd A_(2, 2), B_(2, 1), A12(2, 1);//A_是個(gè)矩陣兩行兩列，其他同理int num_point = x.size();//num_point等于輸入的x的總個(gè)數(shù)double A01(0.0), A02(0.0), B00(0.0), B10(0.0);//A01表示第0行第1列//A01(0.0), A02(0.0)表明A矩陣的第一行都初始化為0for (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;//把這四個(gè)數(shù)塞給A矩陣；A矩陣就是推導(dǎo)中的x轉(zhuǎn)置乘x的逆B_ << B00,B10;//B矩陣就是X轉(zhuǎn)置乘YA12 = A_.inverse()*B_;//A_.inverse()是求A的逆矩陣；k = A12(0, 0);//A12是兩行一列，那么k就是第0行第0列b = A12(1, 0);//b就是第1行第0列
}
//擬合y=a*x^2+b*x+c;//平面曲線(xiàn)
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);
}
//擬合空間曲線(xiàn)
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;
}
//空間曲線(xiàn)展示
void fitting::polynomial3D_fitting_display(double step_) //傳入的參數(shù)相當(dāng)于步長(zhǎng)
{PointT point_min_, point_max_;getMinMax3D(*cloud, point_min_, point_max_);//利用最小外包框的x值，向擬合的直線(xiàn)做垂足，垂足的交點(diǎn)即為三維曲線(xiàn)的端點(diǎn)值***********int idx_minx, idx_maxy;//x取到最大值和最小值的點(diǎn)號(hào)索引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("最后擬合的多項(xiàng)式曲線(xiàn)"));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");//顯示端點(diǎn)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 << "端點(diǎn)值1為：" << "X1= " << duandian_point->points[0].x << ", " << "Y1= " << duandian_point->points[0].y << ", " << "Z1= " << duandian_point->points[0].z << endl;cout << "端點(diǎn)值2為：" << "X2= " << duandian_point->points[1].x << ", " << "Y2= " << duandian_point->points[1].y << ", " << "Z2= " << duandian_point->points[1].z << endl;cout << "空間多項(xiàng)式曲線(xiàn)方程為： " << "z=" << a_3d << "*(x^2+y^2)+" << b_3d << "*sqrt(x^2+y^2)+" << c_3d << endl;viewer->spin();//擬合曲線(xiàn)+端點(diǎn)值+散點(diǎn)圖二維平面展示，有需要可以取消注釋----------------------------------------------------------/*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();
}

//主函數(shù)
#include <pcl/io/pcd_io.h>
#include "fitting.h"
using namespace std;
using namespace pcl;
using namespace Eigen;//Eigen是可以用來(lái)進(jìn)行線(xiàn)性代數(shù)、矩陣、向量操作等運(yùn)算的C++庫(kù)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);//大X是原始點(diǎn)云的所有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;//所有的函數(shù)中傳出的參數(shù)fitting fit_;fit_.setinputcloud(cloud);//點(diǎn)云輸入fit_.line_fitting(X, Y, k_line, b_line);//直線(xiàn)擬合X,Y是傳入?yún)?shù)；k_line, b_line是傳出參數(shù)（C++中的引用知識(shí)）fit_.display_line(X, Y, b_line, k_line);//顯示擬合的直線(xiàn)，必須先輸入常量fit_.polynomial2D_fitting(X, Z, a, b, c);fit_.display_line(X, Z, c, b, a);//顯示擬合的平面多項(xiàng)式曲線(xiàn)，輸入順序?yàn)?常量，一階系數(shù)，二階系數(shù)fit_.grid_mean_xyz(0.5, -1, x_mean, y_mean, z_mean, point_mean);//0.5表示x方向的步長(zhǎng)，-1(小于0就行)表示y方向不分段，如需分段，則設(shè)置相應(yīng)步長(zhǎng)fit_.grid_mean_xyz_display(point_mean);//展示均值結(jié)果fit_.display_point(X, Y);//顯示散點(diǎn)fit_.display_point(x_mean, y_mean);//顯示均值散點(diǎn)fit_.polynomial3D_fitting(x_mean, y_mean, z_mean, a, b, c);//用分段質(zhì)心的均值去擬合3維曲線(xiàn)//fit_.polynomial3D_fitting(X, Y, Z, a, b, c);//直接擬合fit_.polynomial3D_fitting_display(0.5);//三維曲線(xiàn)展示return 0;
}

2.使用最小二乘原理

PS：首先對(duì)于最小二乘思想大家應(yīng)該掌握，最小二乘法（又稱(chēng)最小平方法）是一種數(shù)學(xué)優(yōu)化技術(shù)。它通過(guò)最小化誤差的平方和尋找數(shù)據(jù)的最佳函數(shù)匹配。利用最小二乘法可以簡(jiǎn)便地求得未知的數(shù)據(jù)，并使得這些求得的數(shù)據(jù)與實(shí)際數(shù)據(jù)之間誤差的平方和為最小。最小二乘法還可用于曲線(xiàn)擬合。其他一些優(yōu)化問(wèn)題也可通過(guò)最小化能量或最大化熵用最小二乘法來(lái)表達(dá)。

最小二乘法（英文：least square method)是一種常用的數(shù)學(xué)優(yōu)化方法，所謂二乘就是平方的意思。這平方一詞指的是在擬合一個(gè)函數(shù)的時(shí)候，通過(guò)最小化誤差的平方來(lái)確定最佳的匹配函數(shù)，所以最小二乘、最小平方指的就是擬合的誤差平方達(dá)到最小。

以直線(xiàn)擬合為例，已知有一組平面上的點(diǎn)集?；谶@些點(diǎn)擬合一條直線(xiàn)，設(shè)直線(xiàn)方程為:

? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?

那么那么算法的輸入就是這些點(diǎn)集，需要求取的為直線(xiàn)方程的參數(shù)a,b。

（1）平方偏差之和為:

? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?

? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ??

那么，以上公式已知的是xi,yi, 未知且要求取的是a、b。不同的a、b會(huì)得到不同的S，求取的是在S最小的時(shí)候求取a、b。這是一個(gè)二元a,b函數(shù),此問(wèn)題實(shí)際上就是多元函數(shù)的極值與最值問(wèn)題，要求解函數(shù)極值時(shí)二元變量數(shù)值，這里要用到二元函數(shù)取極值的必要條件：

? ? ?

部分代碼：

void mv::LineFit::FitLineByRegression()
{// 設(shè)置權(quán)重 | weights setting_weigths = std::vector<double>(_points.size(), 1);// AX = B// 構(gòu)造A矩陣 | Construct A matconst int N = 2;cv::Mat A = cv::Mat::zeros(N, N, CV_64FC1);for (int row = 0;row < A.rows;row++){for (int col = 0; col < A.cols;col++){for (int k = 0;k < _points.size();k++){A.at<double>(row, col) = A.at<double>(row, col) + pow(_points[k].x, row + col) * _weigths[k];}}}//構(gòu)造B矩陣 | Construct B matcv::Mat B = cv::Mat::zeros(N, 1, CV_64FC1);for (int row = 0;row < B.rows;row++){for (int k = 0;k < _points.size();k++){B.at<double>(row, 0) = B.at<double>(row, 0) + pow(_points[k].x, row)*_points[k].y * _weigths[k];}}// 求解A*X = B | Solve the A*X = Bcv::Mat X;cv::solve(A, B, X,cv::DECOMP_LU);// y = b + ax_result.b = X.at<double>(0,0);_result.a = X.at<double>(1, 0);
}//FitLineByRegression

完整代碼鏈接：https://github.com/mangosroom/machine-vision-algorithms-library/tree/master/src/linefit

參照博客：https://mangoroom.cn/opencv/least-square-method-line-fit.html

? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?

?

總結(jié)

以上是生活随笔為你收集整理的PCL：拟合平面直线和曲线以及空间曲线的原理到算法实现的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問(wèn)題。

如果覺(jué)得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。