本章代碼是本人根據(jù)一個(gè)未曾謀面的好人學(xué)習(xí)的（要懷抱希望，世界上好人真的是很多的，我要做一個(gè)去給別人帶去正能量積極態(tài)度的人，加油嘍），如需轉(zhuǎn)載學(xué)習(xí)請(qǐng)注明。謝謝

?

---------------基于ransac算法平面檢測(cè)：

1、確定迭代次數(shù)；

2、在迭代次數(shù)內(nèi)：

? ?? 2.1 隨機(jī)選擇三個(gè)點(diǎn)組成平面（判斷三個(gè)點(diǎn)是否共線）；

? ?? 2.2 構(gòu)造坐標(biāo)矩陣；

? ?? 2.3 求平面方程；

? ?? 2.4 求所有點(diǎn)到平面的距離；

? ?? 2.5 統(tǒng)計(jì)inlier個(gè)數(shù)（距離小于閾值）；

3、迭代選擇inlier個(gè)數(shù)最多的平面。

PS：如果你進(jìn)行的是地面分割，只使用inlier作為判斷條件的不足；導(dǎo)致某個(gè)點(diǎn)數(shù)較多的非地面平面占據(jù)inlier個(gè)數(shù)；為了避免將平直墻面檢測(cè)為地面，必須將夾角加入判斷條件；（夾角就是法向量與Z軸（0，0，1）的夾角。

判斷三個(gè)點(diǎn)是否共線的兩種方法：

1、滿足滿秩矩陣（利用滿秩行列式不等于0）用三點(diǎn)其中任意兩點(diǎn)組成向量,求這樣兩個(gè)不同向量是否平行就行了.也就是說三個(gè)點(diǎn)的坐標(biāo)組成一個(gè)三階行列式,只要三階行列式為0,且該矩陣秩為1,則必定是共線的。
2、利用比例關(guān)系（就是兩兩向量成比例）為了方便,我數(shù)字設(shè)簡(jiǎn)單一點(diǎn)比如(0,0,0),(1,2,3),(4,5,6)三點(diǎn)先任取兩個(gè)想減得到(1-0,2-0,3-0)和(4-1,5-2,6-3)兩個(gè)向量即(1,2,3)和(3,3,3)這兩個(gè)向量然后設(shè)一個(gè)比例常數(shù)t使1*t=3解得t=3帶入2*t得6與對(duì)應(yīng)的y=3不等所以不共線。（即設(shè)三點(diǎn)為A、B、C .利用向量證明:λAB=AC(其中λ為非零實(shí)數(shù))）

3、利用點(diǎn)差法求出ab斜率和ac斜率 相等即三點(diǎn)共線

4、取兩點(diǎn)確立一條直線，計(jì)算該直線的解析式，代入第三點(diǎn)坐標(biāo) 看是否滿足該解析式。

5、證明平角即可例如,三點(diǎn)ABC,有任意一點(diǎn)D,若角DBA+角DBC=180度,即角ABC=180度,則點(diǎn)ABC三點(diǎn)共線幾何表達(dá):因?yàn)榻茿BC=180度所以點(diǎn)ABC三點(diǎn)共線（初中方法）

ransac算法平面檢測(cè)數(shù)學(xué)知識(shí)擴(kuò)展：

三點(diǎn)式平面方程：ax+by+cz=d ? ?? 點(diǎn)到平面的距離公式：

（1）滿秩矩陣：設(shè)A是n階矩陣, 若r（A） = n（記為rank=n）, 則稱A為滿秩矩陣。但滿秩不局限于n階矩陣。如果n階方陣A滿秩,就是A的秩為n,則A有一個(gè)n階子式不等于0,因?yàn)锳只有一個(gè)n階子式,即其本身,所以|A|≠0（滿秩行列式不等于0）。

于 mxn的非方陣而言，它可能的最大秩為 min{m,n}. 當(dāng) rank=m時(shí)，稱其行滿秩；當(dāng) rank=n時(shí)，稱其列滿秩。

若矩陣秩等于行數(shù)，稱為行滿秩；若矩陣秩等于列數(shù)，稱為列滿秩。既是行滿秩又是列滿秩則為n階矩陣即n階方陣。行滿秩矩陣就是行向量線性無關(guān)，列滿秩矩陣就是列向量線性無關(guān)；所以如果是方陣,行滿秩矩陣與列滿秩矩陣是等價(jià)的。

對(duì)于向量組而言，要考慮向量的維數(shù)和個(gè)數(shù)：

如果向量個(gè)數(shù)大于其維數(shù)（比如說10個(gè)三維向量），則該向量組必線性相關(guān)，也就是下面左圖情況。也就是說，向量組的秩，不超過 min{向量的個(gè)數(shù)，向量的維數(shù)}。

??

解釋為什么線性代數(shù)中向量個(gè)數(shù)大于向量維數(shù),那么這幾個(gè)向量就線性相關(guān)呢??答案就是：判斷向量組的線性相關(guān)性就是看方程x1A1+x2A2+...+xkAk=0有沒有非零解.把它展開就是一個(gè)線性方程組,系數(shù)矩陣有k列,其行數(shù)就是向量的維數(shù).若向量的維數(shù)小于k（是表示方程組的個(gè)數(shù)比未知數(shù)多嗎？？?）,那么方程組有非零解（方程個(gè)數(shù)小于未知量個(gè)數(shù)時(shí),齊次線性方程組非零解,因?yàn)橄禂?shù)矩陣的秩≤行數(shù)＜未知量個(gè)數(shù)）（向量組線性相關(guān)的充分必要條件是它們所拼成的矩陣的秩小于向量的個(gè)數(shù)。當(dāng)向量個(gè)數(shù)大于維數(shù)時(shí)，矩陣的秩≤行數(shù)=向量維數(shù)<向量個(gè)數(shù)，所以向量組一定線性相關(guān)。 ）

我印象中好像教科書上沒有介紹非方陣和向量組的滿秩的定義。。所以也可以不用糾結(jié)，強(qiáng)調(diào)是行滿秩還是列滿秩就行了。我個(gè)人認(rèn)為，向量組滿秩可以定義為向量組的秩等于向量的個(gè)數(shù)，那么下圖一種情況可能滿秩，上圖一種情況不可能滿秩因?yàn)樯蠄D中響亮的個(gè)數(shù)為10，但是向量組的秩不超過3（因?yàn)槿∽钚÷?#xff09;。

（2）滿秩矩陣：秩=階數(shù)的方陣。滿秩矩陣也可以被稱為可逆矩陣；（初等矩陣是由單位陣E經(jīng)過初等變換得到的矩陣，這句話跟此處無關(guān)只是看到了記一下）

三個(gè)向量行列式為零,這說明三個(gè)向量組成的矩陣不滿秩,也就是說向量組的極大無關(guān)組里,向量的個(gè)數(shù)小于3,就是說,一定有向量可以由其他向量線性表制示,這就說說明三個(gè)向量共面（不共線）。

行列式在數(shù)學(xué)中，是一個(gè)函數(shù)，其定義域?yàn)閐et的矩陣A，取值為一個(gè)標(biāo)量，寫作det(A)或 | A | 。無論是在線性代數(shù)、多項(xiàng)式理論，還是在微積分學(xué)中（比如說換元積分法中），行列式作為基本的數(shù)學(xué)工具，都有著重要的應(yīng)用。

行列式可以看做是有向面積或體積的概道念在一般的歐幾里得空間中的推廣。或者說，在 n 維歐幾里得空間中，行列式描述的是一個(gè)線性變換對(duì)“體積”所造成的影響。

貼一個(gè)點(diǎn)法式的平面方程表達(dá)形式：

已知三個(gè)三維點(diǎn)，求他們的平面方程：

已知三個(gè)點(diǎn)坐標(biāo)為P1(x1,y1,z1), P2(x2,y2,z2), P3(x3,y3,z3)

所以可以設(shè)方程為A(x - x1) + B(y - y1) + C(z - z1) = 0 (點(diǎn)法式) (也可設(shè)為過另外兩個(gè)點(diǎn))

核心代碼：

//在此之前寫好錄入三個(gè)三維點(diǎn)的代碼，然后就是處理待定系數(shù)，如下：

A = (y3 - y1)*(z3 - z1) - (z2 -z1)*(y3 - y1);

B = (x3 - x1)*(z2 - z1) - (x2 - x1)*(z3 - z1);

C = (x2 - x1)*(y3 - y1) - (x3 - x1)*(y2 - y1);

即得過P1，P2，P3的平面方程

方程也可寫為 Ax + By + Cz + D = 0 (一般式) 其中D = -(A * x1 + B * y1 + C * z1)

改天分析代碼吧，先睡覺去了。接著把代碼都注釋了

求點(diǎn)到平面的距離公式：第一種向量法；第二種一般公式。

第一種：向量法（第一種是我這種方式： fabs((x4-x2)*p.a+(y4-y2)*p.b+(z4-z2)*p.c)：表示當(dāng)前點(diǎn)的向量減去平面上的一個(gè)點(diǎn)的向量，然后把他倆相減得到的向量乘以平面法向量（a,b,c）就得到了我的分子。第二種方式就是 fabs(x4*p.a+y4*p.b+z4*p.c+p.d)，兩種方法是等價(jià)的。）

（思路：由于原始點(diǎn)云的數(shù)量太多，ransac可能需要迭代很多次才能找到正確的平面，又因?yàn)榻挡蓸硬粫?huì)改變點(diǎn)云的分布，因此先降采樣點(diǎn)云進(jìn)行一次ransac。
利用降采樣的點(diǎn)云得到了平面，之后就需要去除地面點(diǎn)了，因此分別對(duì)所有點(diǎn)云計(jì)算到平面的距離，并篩選出小于threshold的所有點(diǎn)云作為地面點(diǎn)）

頭文件：

//ransac.h
#include<iostream>
#include<vector>
#include<Eigen/Core>
#include<Eigen/Dense>
#include<pcl-1.9/pcl/io/pcd_io.h>
#include<pcl-1.9/pcl/filters/voxel_grid.h>
#include<pcl-1.9/pcl/point_types.h>
#include<pcl-1.9/pcl/visualization/cloud_viewer.h>
#include<pcl-1.9/pcl/kdtree/kdtree.h>
#include<pcl-1.9/pcl/common/transforms.h>
#include<pcl-1.9/pcl/point_cloud.h>
#include<pcl-1.9/pcl/octree/octree.h>
#include<chrono>
#include <algorithm>
#include <cstdlib>
#include <ctime>
#include <cmath>
#include<boost/format.hpp>
using namespace std;
using namespace pcl;class Platform//定義類
{public:Platform(){}//構(gòu)造函數(shù)Eigen::Vector3d calnormal()//Vector3d實(shí)質(zhì)上是Eigen::Matrix<double,3,1>,即三維向量{return Eigen::Vector3d(a,b,c);//返回的向量}double a;double b;double c;double d;int num;//在范圍內(nèi)的點(diǎn)數(shù)量};
pcl::PointCloud<PointXYZ>::Ptr ransac(pcl::PointCloud<PointXYZ>::Ptr all_db,pcl::PointCloud<PointXYZ>::Ptr db,int max_iter,float threshold);
void delet_subplat(vector<pcl::PointCloud<PointXYZ>::Ptr> &cluster_list,std::vector<pair<int,int>> &cluster_sort);

源文件：

#include "ransac.h"pcl::PointCloud<PointXYZ>::Ptr ransac(pcl::PointCloud<PointXYZ>::Ptr all_db,pcl::PointCloud<PointXYZ>::Ptr db,int max_iter,float threshold)
{//all_db代表所有的點(diǎn)；db表示輸入濾波后的點(diǎn)云；max_iter最大迭代次數(shù)；threshold點(diǎn)距離平面的閾值srand(time(0));//從0開始的自動(dòng)隨機(jī)種子；//用函數(shù)srand()初始化隨機(jī)數(shù)種子。srand()的參數(shù)，用time函數(shù)值（即當(dāng)前時(shí)間），因?yàn)閮纱握{(diào)用rand()函數(shù)的時(shí)間通常是不同的，這樣就可以保證隨機(jī)性了std::vector<int> index_final;PointXYZ plat_point;Eigen::Vector3d ABC;//Vector3d表示長(zhǎng)度為3的類型為double的向量。//VectorXd v(3);//定義維度為3的列向量v;Vector3d v(1,2,3); 表示"Vector3d"直接定義了一個(gè)維度為3的列向量index_final.clear();//清除sizewhile(--max_iter)//設(shè)置循環(huán)的次數(shù)（這個(gè)循環(huán)次數(shù)ransac的迭代次數(shù)是為了多次循環(huán)用于找到最大平面）{std::vector<int> index;//index是容器index.clear();for(int k =0;k<3;++k){index.push_back(rand()%db->size());//隨機(jī)選取三個(gè)點(diǎn) db是個(gè)點(diǎn)云指針 //rand()%db->size表示產(chǎn)生從0到db之間的隨機(jī)數(shù)db->size指的是點(diǎn)云中包含的點(diǎn)數(shù)，rand()對(duì)db->size()取余是為了防止產(chǎn)生的隨機(jī)數(shù)超過db->所指向的點(diǎn)云的點(diǎn)的數(shù)量}double x1, y1,z1, x2, y2,z2, x3, y3,z3;//要訪問順序容器和關(guān)聯(lián)容器中的元素，需要通過“迭代器（iterator）”進(jìn)行。迭代器是一個(gè)變量，相當(dāng)于容器和操縱容器的算法之間的中介。迭代器可以指向容器中的某個(gè)元素，通過迭代器就可以讀寫它指向的元素。從這一點(diǎn)上看，迭代器和指針類似。//通過迭代器可以讀取它指向的元素，*迭代器名就表示迭代器指向的元素。通過非常量迭代器還能修改其指向的元素。//vector<int>::iterator i;  //定義正向迭代器//*i 就是迭代器i指向的元素auto idx = index.begin();//這里idx是迭代器x1 = db->points[*idx].x;//這里idx是迭代器，*idx是迭代器idx所指向的元素y1 = db->points[*idx].y;z1 = db->points[*idx].z;++idx;//對(duì)正向迭代器進(jìn)行++操作時(shí)，迭代器會(huì)指向容器中的后一個(gè)元素；x2 = db->points[*idx].x;y2 = db->points[*idx].y;z2 = db->points[*idx].z;++idx;x3 = db->points[*idx].x;這里idx是迭代器，用于依次取出index容器中的前三個(gè)隨機(jī)數(shù)，然后把隨機(jī)數(shù)作為點(diǎn)云下標(biāo)，取出三個(gè)點(diǎn)賦值（x1, y1,z1）(x2, y2,z2)(x3, y3,z3)y3 = db->points[*idx].y;z3 = db->points[*idx].z;//將上述for循環(huán)查找到的三個(gè)點(diǎn)賦值給（x1, y1,z1）(x2, y2,z2)(x3, y3,z3)//這里idx是迭代器，用于依次取出index容器中的前三個(gè)隨機(jī)數(shù)，然后把隨機(jī)數(shù)作為點(diǎn)云下標(biāo)，取出三個(gè)點(diǎn)賦值（x1, y1,z1）(x2, y2,z2)(x3, y3,z3)Platform p;//p是個(gè)類   平面的一般公式是ax+by+cz+d=0；p.a = (y2 - y1)*(z3 - z1) - (z2-z1)*(y3 - y1);p.b = (z2 - z1)*(x3 - x1) - (x2-x1)*(z3 - z1);p.c = (x2 - x1)*(y3 - y1) - (y2-y1)*(x3 - x1);p.d = -(p.a*x2 + p.b*y2 + p.c*z2);//這里的a,b,c,d是在算平面點(diǎn)法式的系數(shù)for(auto db_index = db->begin();db_index !=db->end();++db_index)//db是濾波后的點(diǎn)云{double x4 = db_index->x;double y4 = db_index->y;double z4 = db_index->z;//（這個(gè)x4y4z4是開始遍歷濾波后的所有的點(diǎn)，并計(jì)算每個(gè)點(diǎn)到以計(jì)算出的平面的距離）double dis = fabs((x4-x2)*p.a+(y4-y2)*p.b+(z4-z2)*p.c)/sqrt(p.a*p.a+p.b*p.b+p.c*p.c);//點(diǎn)到平面的距離公式有兩種見博客文字解釋此處采用的向量法第一種。if(dis<0.12)//如果點(diǎn)到平面的距離小于0.12就把他歸為平面上的點(diǎn)index.push_back(db_index - db->begin());}}//更新集合if(index.size()>index_final.size())//對(duì)新更新的平面進(jìn)行索引統(tǒng)計(jì)？？？{index_final = index;//每次循環(huán)會(huì)選取隨機(jī)三個(gè)點(diǎn)，并求出三個(gè)點(diǎn)組成的平面，然后計(jì)算查詢得到在平面一定范圍內(nèi)的點(diǎn)的下標(biāo)，保存進(jìn)index這個(gè)容器中，而index_final是用于記錄包含點(diǎn)最多的那組index，如果這次循環(huán)得到在平面一定范圍內(nèi)點(diǎn)數(shù)量比index_final的多，就用index去更新index_final.plat_point = PointXYZ(x1,y1,z1);//這里保存了周圍點(diǎn)數(shù)量最多的那個(gè)平面上的一個(gè)點(diǎn)，用于全體點(diǎn)云計(jì)算到平面的距離ABC = Eigen::Vector3d(p.a,p.b,p.c);//這里就是平面的法向量，用于之后計(jì)算點(diǎn)到平面的距離。}}/*對(duì)所有點(diǎn)云進(jìn)行計(jì)算離平面距離,并提取對(duì)應(yīng)的index*/std::vector<int> platform_index;//存放平面點(diǎn)index的容器std::vector<int> unplatform_index;//存放非平面點(diǎn)index的容器for(auto all_db_index=all_db->begin();all_db_index != all_db->end();++all_db_index){Eigen::Vector3d dis_vector;dis_vector[0] = all_db_index->x - plat_point.x;//計(jì)算濾波后的點(diǎn)到平面上的一點(diǎn)的距離dis_vector[1] = all_db_index->y - plat_point.y;dis_vector[2] = all_db_index->z - plat_point.z;double dis;dis = fabs(dis_vector.dot(ABC))/sqrt(ABC.squaredNorm());if(dis < threshold)//如果距離小于閾值{platform_index.push_back(all_db_index - all_db->begin());//就把該點(diǎn)納為平面內(nèi)的點(diǎn)}else{unplatform_index.push_back(all_db_index - all_db->begin());//否則作為非平面內(nèi)的點(diǎn)}}pcl::PointCloud<PointXYZ>::Ptr unplat_points(new pcl::PointCloud<PointXYZ>);for(auto index:unplatform_index){unplat_points->push_back(*(all_db->begin()+index));}return unplat_points;
}
void delet_subplat(vector<pcl::PointCloud<PointXYZ>::Ptr> &cluster_list,std::vector<pair<int,int>> &cluster_sort)
{for(int i = 0;i < cluster_list.size();i++){int size = cluster_list[i]->size();cluster_sort.push_back(make_pair(i,size));}sort(cluster_sort.begin(), cluster_sort.end(), [](const pair<int, int> &x, const pair<int, int> &y) -> int {return x.second > y.second;});/*再次用ransac篩選最大連通域中的地面*/int max_index = cluster_sort[0].first;/*去除斜面降采樣*/pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered1(new pcl::PointCloud<pcl::PointXYZ>);pcl::VoxelGrid<pcl::PointXYZ> sor1;sor1.setInputCloud(cluster_list[max_index]);sor1.setLeafSize(1.f, 1.f, 1.f);sor1.filter(*cloud_filtered1);cluster_list[max_index] = ransac(cluster_list[max_index],cloud_filtered1,200,0.2);// cout << "最大數(shù)量cluster的數(shù)量:" << cluster_list[max_index]->size() << endl;/*再次對(duì)最大連通域聚類*/
}

主函數(shù)：

#include "base.h"
#include "knn.h"
#include "ransac.h" 
#include "dbscan.h"
#include "show.h"
const float search_radius = 0.5;
const string db_list = "../data/000005.bin";
int io_point(pcl::PointCloud<PointXYZ>::Ptr &db){ifstream fin;fin.open(db_list,ios::binary);if(!fin){cout<<"open error!"<<endl;return -1;}for (int i=0; fin.good() && !fin.eof(); i++) {PointXYZ point;fin.read((char *) &point.x, 4*sizeof(float));db->push_back(point);}return 0;
}
int main(int argc, char const *argv[])
{pcl::PointCloud<PointXYZ>::Ptr points (new pcl::PointCloud<PointXYZ>);io_point(points);chrono::steady_clock::time_point t8 = chrono::steady_clock::now();/*降采樣*/pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);pcl::VoxelGrid<pcl::PointXYZ> sor;sor.setInputCloud(points);sor.setLeafSize(2.f, 2.f, 2.f);sor.filter(*cloud_filtered);cout << "濾波后的點(diǎn)云數(shù)量：" << cloud_filtered->size() << endl;/*去除地面*/pcl::PointCloud<pcl::PointXYZ>::Ptr unplat_points(new pcl::PointCloud<pcl::PointXYZ>);unplat_points = ransac(points,cloud_filtered,300,0.25);/*建立八叉樹*/ float resolution=1.0f; //分辨率pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution);//初始化octreeoctree.setInputCloud(unplat_points);octree.addPointsFromInputCloud();/*dbscan搜索*/dbscan scan(unplat_points,octree,search_radius,8);//radius與min_samplescan.run();/*找出點(diǎn)云數(shù)最多的聚類*/int cluster_num = *max_element(scan.cluster_state->begin(),scan.cluster_state->end()) + 1;vector<pcl::PointCloud<PointXYZ>::Ptr> cluster_list;for(int i=0;i<cluster_num;i++){pcl::PointCloud<PointXYZ>::Ptr cloud_ptr(new pcl::PointCloud<PointXYZ>);cluster_list.push_back(cloud_ptr);}for(int index =0;index<unplat_points->size();index++){int reslut = (*scan.cluster_state)[index];cluster_list[reslut]->push_back((*unplat_points)[index]);}/*去除噪點(diǎn)*/cluster_list.erase(cluster_list.begin(),cluster_list.begin()+1);/*刪除沒識(shí)別到的地面*/std::vector<pair<int,int>> cluster_sort;delet_subplat(cluster_list,cluster_sort);chrono::steady_clock::time_point t9 = chrono::steady_clock::now();chrono::duration<double> time_used5 = chrono::duration_cast<chrono::duration<double>>(t9 - t8)*1000;cout << "總體用時(shí) = " << time_used5.count() << " ms.    " << endl;/*顯示*/show_point_cloud(cluster_list,cluster_num,cluster_sort);return 0;
}

下面是：PCL 實(shí)現(xiàn)最小二乘平面擬合（參考博客：https://blog.csdn.net/qq_36686437/article/details/109137124）

一、算法原理

參考文獻(xiàn)：[1]曹詩卉,亓迎川.基于最小二乘法平面擬合的點(diǎn)云法矢算法[J].空軍預(yù)警學(xué)院學(xué)報(bào),2016,30(01):41-43+48.

代碼：

#include <iostream>
#include <vector>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>using namespace std;
using namespace Eigen;/* 最小二乘法擬合平面：Ax + By + Cz + D = 0 */
/* Parameters：返回參數(shù)A B C D */
/* point：存放輸入點(diǎn) */
bool FitPlaneByLeastSquares(pcl::PointCloud<pcl::PointXYZ>&point,vector<double>& Parameters)
{Parameters.clear();int count = point.size();if (count < 3){return false;}double meanX = 0, meanY = 0, meanZ = 0;double meanXX = 0, meanYY = 0, meanZZ = 0;double meanXY = 0, meanXZ = 0, meanYZ = 0;for (int i = 0; i < count; i++){meanX += point[i].x;meanY += point[i].y;meanZ += point[i].z;meanXX += point[i].x * point[i].x;meanYY += point[i].y * point[i].y;meanZZ += point[i].z * point[i].z;meanXY += point[i].x * point[i].y;meanXZ += point[i].x * point[i].z;meanYZ += point[i].y * point[i].z;}meanX /= count;meanY /= count;meanZ /= count;meanXX /= count;meanYY /= count;meanZZ /= count;meanXY /= count;meanXZ /= count;meanYZ /= count;/* eigenvector */Matrix3d eMat;eMat(0, 0) = meanXX - meanX * meanX; eMat(0, 1) = meanXY - meanX * meanY; eMat(0, 2) = meanXZ - meanX * meanZ;eMat(1, 0) = meanXY - meanX * meanY; eMat(1, 1) = meanYY - meanY * meanY; eMat(1, 2) = meanYZ - meanY * meanZ;eMat(2, 0) = meanXZ - meanX * meanZ; eMat(2, 1) = meanYZ - meanY * meanZ; eMat(2, 2) = meanZZ - meanZ * meanZ;Eigen::EigenSolver<Eigen::Matrix3d> PlMat(eMat);Matrix3d eValue = PlMat.pseudoEigenvalueMatrix();Matrix3d eVector = PlMat.pseudoEigenvectors();/* the eigenvector corresponding to the minimum eigenvalue */double v1 = eValue(0, 0);double v2 = eValue(1, 1);double v3 = eValue(2, 2);int minNumber = 0;if ((abs(v2) <= abs(v1)) && (abs(v2) <= abs(v3))){minNumber = 1;}if ((abs(v3) <= abs(v1)) && (abs(v3) <= abs(v2))){minNumber = 2;}double A = eVector(0, minNumber);double B = eVector(1, minNumber);double C = eVector(2, minNumber);double D = -(A * meanX + B * meanY + C * meanZ);/* result */if (C < 0){A *= -1.0;B *= -1.0;C *= -1.0;D *= -1.0;}Parameters.push_back(A);Parameters.push_back(B);Parameters.push_back(C);Parameters.push_back(D);Parameters.push_back(meanX);Parameters.push_back(meanY);Parameters.push_back(meanZ);return true;}//  ----------------可視化，左窗口為原始點(diǎn)云，右窗口為根據(jù)點(diǎn)云擬合出來的平面--------------
void visualize_pcd(pcl::PointCloud<pcl::PointXYZ>::Ptr &src, pcl::ModelCoefficients &PlaneCoff)
{boost::shared_ptr<pcl::visualization::PCLVisualizer>viewer(new pcl::visualization::PCLVisualizer("Viewer"));viewer->setBackgroundColor(0, 0, 0);int v1 = 0;int v2 = 1;viewer->createViewPort(0, 0, 0.5, 1, v1);viewer->createViewPort(0.5, 0, 1, 1, v2);viewer->setBackgroundColor(0, 0, 0, v1);viewer->setBackgroundColor(0.05, 0, 0, v2);// ---------------------點(diǎn)云按照z字段進(jìn)行渲染--------------------pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> fildColor(src, "z");viewer->addPointCloud<pcl::PointXYZ>(src, fildColor, "sample cloud", v1);//--------------------可視化擬合的平面-----------------------viewer->addPlane(PlaneCoff, "Plane", v2);while (!viewer->wasStopped()){viewer->spinOnce(100);boost::this_thread::sleep(boost::posix_time::microseconds(100000));}}int
main(int argc, char** argv)
{// 加載點(diǎn)云pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);pcl::io::loadPCDFile<pcl::PointXYZ>("desk2.pcd", *cloud);pcl::PointCloud<pcl::PointXYZ>target;target.resize(cloud->points.size());target.width = 1;target.height = cloud->points.size();target.is_dense = false;for (size_t i = 0; i < cloud->points.size(); ++i){target[i].x = cloud->points[i].x;target[i].y = cloud->points[i].y;target[i].z = cloud->points[i].z;}vector<double>Coff;FitPlaneByLeastSquares(target, Coff);pcl::ModelCoefficients plane_coeff;plane_coeff.values.resize(4);    // We need 4 valuesplane_coeff.values[0] = Coff[0];plane_coeff.values[1] = Coff[1];plane_coeff.values[2] = Coff[2];plane_coeff.values[3] = Coff[3];cout << "擬合系數(shù):" << "\n A=" << Coff[0] << "\n B=" << Coff[1] << "\n C=" << Coff[2] << "\n D=" << Coff[3] << endl;visualize_pcd(cloud, plane_coeff);return (0);
}

結(jié)果顯示：

四、擬合顯示：addPlane()

https://pointclouds.org/documentation/classpcl_1_1visualization_1_1_p_c_l_visualizer.html#a27a459da46f56faed4b44ef1c57bbbca

總結(jié)

以上是生活随笔為你收集整理的RANSAC算法（2）：（拟合平面）本文以地面为基础以及源码分布解读的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問題。

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