本章代碼是本人根據一個未曾謀面的好人學習的（要懷抱希望，世界上好人真的是很多的，我要做一個去給別人帶去正能量積極態度的人，加油嘍），如需轉載學習請註明。謝謝
---------------基於ransac算法平面檢測：
1、確定迭代次數；
2、在迭代次數內：
2.1 隨機選擇三個點組成平面（判斷三個點是否共線）；
2.2 構造座標矩陣；
2.3 求平面方程；
2.4 求所有點到平面的距離；
2.5 統計inlier個數（距離小於閾值）；
3、迭代選擇inlier個數最多的平面。
PS：如果你進行的是地面分割，只使用inlier作爲判斷條件的不足；導致某個點數較多的非地面平面佔據inlier個數；爲了避免將平直牆面檢測爲地面，必須將夾角加入判斷條件；（夾角就是法向量與Z軸（0，0，1）的夾角。
判斷三個點是否共線的兩種方法：
1、滿足滿秩矩陣（利用滿秩行列式不等於0）用三點其中任意兩點組成向量,求這樣兩個不同向量是否平行就行了.也就是說三個點的座標組成一個三階行列式,只要三階行列式爲0,且該矩陣秩爲1,則必定是共線的。
2、利用比例關係（就是兩兩向量成比例）爲了方便,我數字設簡單一點比如(0,0,0),(1,2,3),(4,5,6)三點先任取兩個想減得到(1-0,2-0,3-0)和(4-1,5-2,6-3)兩個向量即(1,2,3)和(3,3,3)這兩個向量然後設一個比例常數t使1*t=3解得t=3帶入2*t得6與對應的y=3不等所以不共線。（即設三點爲A、B、C .利用向量證明:λAB=AC(其中λ爲非零實數)）
3、利用點差法求出ab斜率和ac斜率 相等即三點共線
4、取兩點確立一條直線，計算該直線的解析式，代入第三點座標 看是否滿足該解析式。
5、證明平角即可例如,三點ABC,有任意一點D,若角DBA+角DBC=180度,即角ABC=180度,則點ABC三點共線幾何表達:因爲角ABC=180度所以點ABC三點共線（初中方法）
ransac算法平面檢測數學知識擴展：
三點式平面方程：ax+by+cz=d 點到平面的距離公式：
（1）滿秩矩陣：設A是n階矩陣, 若r（A） = n（記爲rank=n）, 則稱A爲滿秩矩陣。但滿秩不侷限於n階矩陣。如果n階方陣A滿秩,就是A的秩爲n,則A有一個n階子式不等於0,因爲A只有一個n階子式,即其本身,所以|A|≠0（滿秩行列式不等於0）。
於 mxn的非方陣而言，它可能的最大秩爲 min{m,n}. 當 rank=m時，稱其行滿秩；當 rank=n時，稱其列滿秩。
若矩陣秩等於行數，稱爲行滿秩；若矩陣秩等於列數，稱爲列滿秩。既是行滿秩又是列滿秩則爲n階矩陣即n階方陣。行滿秩矩陣就是行向量線性無關，列滿秩矩陣就是列向量線性無關；所以如果是方陣,行滿秩矩陣與列滿秩矩陣是等價的。
對於向量組而言，要考慮向量的維數和個數：
如果向量個數大於其維數（比如說10個三維向量），則該向量組必線性相關，也就是下面左圖情況。也就是說，向量組的秩，不超過 min{向量的個數，向量的維數}。
解釋爲什麼線性代數中向量個數大於向量維數,那麼這幾個向量就線性相關呢??答案就是：判斷向量組的線性相關性就是看方程x1A1+x2A2+...+xkAk=0有沒有非零解.把它展開就是一個線性方程組,係數矩陣有k列,其行數就是向量的維數.若向量的維數小於k（是表示方程組的個數比未知數多嗎？？?）,那麼方程組有非零解（方程個數小於未知量個數時,齊次線性方程組非零解,因爲係數矩陣的秩≤行數＜未知量個數）（向量組線性相關的充分必要條件是它們所拼成的矩陣的秩小於向量的個數。當向量個數大於維數時，矩陣的秩≤行數=向量維數<向量個數，所以向量組一定線性相關。 ）
我印象中好像教科書上沒有介紹非方陣和向量組的滿秩的定義。。所以也可以不用糾結，強調是行滿秩還是列滿秩就行了。我個人認爲，向量組滿秩可以定義爲向量組的秩等於向量的個數，那麼下圖一種情況可能滿秩，上圖一種情況不可能滿秩因爲上圖中響亮的個數爲10，但是向量組的秩不超過3（因爲取最小嘛）。
（2）滿秩矩陣：秩=階數的方陣。滿秩矩陣也可以被稱爲可逆矩陣；（初等矩陣是由單位陣E經過初等變換得到的矩陣，這句話跟此處無關只是看到了記一下）
三個向量行列式爲零,這說明三個向量組成的矩陣不滿秩,也就是說向量組的極大無關組裏,向量的個數小於3,就是說,一定有向量可以由其他向量線性表制示,這就說說明三個向量共面（不共線）。
行列式在數學中，是一個函數，其定義域爲det的矩陣A，取值爲一個標量，寫作det(A)或 | A | 。無論是在線性代數、多項式理論，還是在微積分學中（比如說換元積分法中），行列式作爲基本的數學工具，都有着重要的應用。
行列式可以看做是有向面積或體積的概道念在一般的歐幾里得空間中的推廣。或者說，在 n 維歐幾里得空間中，行列式描述的是一個線性變換對“體積”所造成的影響。
貼一個點法式的平面方程表達形式：
已知三個三維點，求他們的平面方程：
已知三個點座標爲P1(x1,y1,z1), P2(x2,y2,z2), P3(x3,y3,z3)
所以可以設方程爲A(x - x1) + B(y - y1) + C(z - z1) = 0 (點法式) (也可設爲過另外兩個點)
核心代碼：
//在此之前寫好錄入三個三維點的代碼，然後就是處理待定係數，如下：
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)
改天分析代碼吧，先睡覺去了。接着把代碼都註釋了
求點到平面的距離公式：第一種向量法；第二種一般公式。
第一種：向量法（第一種是我這種方式： fabs((x4-x2)*p.a+(y4-y2)*p.b+(z4-z2)*p.c)：表示當前點的向量減去平面上的一個點的向量，然後把他倆相減得到的向量乘以平面法向量（a,b,c）就得到了我的分子。第二種方式就是 fabs(x4*p.a+y4*p.b+z4*p.c+p.d)，兩種方法是等價的。）
（思路：由於原始點雲的數量太多，ransac可能需要迭代很多次才能找到正確的平面，又因爲降採樣不會改變點雲的分佈，因此先降採樣點雲進行一次ransac。
利用降採樣的點雲得到了平面，之後就需要去除地面點了，因此分別對所有點雲計算到平面的距離，並篩選出小於threshold的所有點雲作爲地面點）
頭文件：
//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(){}//構造函數
Eigen::Vector3d calnormal()//Vector3d實質上是Eigen::Matrix<double,3,1>,即三維向量
{
return Eigen::Vector3d(a,b,c);//返回的向量
}
double a;
double b;
double c;
double d;
int num;//在範圍內的點數量
};
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代表所有的點；db表示輸入濾波後的點雲；max_iter最大迭代次數；threshold點距離平面的閾值
srand(time(0));//從0開始的自動隨機種子；//用函數srand()初始化隨機數種子。srand()的參數，用time函數值（即當前時間），因爲兩次調用rand()函數的時間通常是不同的，這樣就可以保證隨機性了
std::vector<int> index_final;
PointXYZ plat_point;
Eigen::Vector3d ABC;//Vector3d表示長度爲3的類型爲double的向量。//VectorXd v(3);//定義維度爲3的列向量v;Vector3d v(1,2,3); 表示"Vector3d"直接定義了一個維度爲3的列向量
index_final.clear();//清除size
while(--max_iter)//設置循環的次數（這個循環次數ransac的迭代次數是爲了多次循環用於找到最大平面）
{
std::vector<int> index;//index是容器
index.clear();
for(int k =0;k<3;++k)
{
index.push_back(rand()%db->size());//隨機選取三個點 db是個點雲指針 //rand()%db->size表示產生從0到db之間的隨機數db->size指的是點雲中包含的點數，rand()對db->size()取餘是爲了防止產生的隨機數超過db->所指向的點雲的點的數量
}
double x1, y1,z1, x2, y2,z2, x3, y3,z3;
//要訪問順序容器和關聯容器中的元素，需要通過“迭代器（iterator）”進行。迭代器是一個變量，相當於容器和操縱容器的算法之間的中介。迭代器可以指向容器中的某個元素，通過迭代器就可以讀寫它指向的元素。從這一點上看，迭代器和指針類似。
//通過迭代器可以讀取它指向的元素，*迭代器名就表示迭代器指向的元素。通過非常量迭代器還能修改其指向的元素。//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;//對正向迭代器進行++操作時，迭代器會指向容器中的後一個元素；
x2 = db->points[*idx].x;
y2 = db->points[*idx].y;
z2 = db->points[*idx].z;
++idx;
x3 = db->points[*idx].x;////這裏idx是迭代器，用於依次取出index容器中的前三個隨機數，然後把隨機數作爲點雲下標，取出三個點賦值（x1, y1,z1）(x2, y2,z2)(x3, y3,z3)
y3 = db->points[*idx].y;
z3 = db->points[*idx].z;//將上述for循環查找到的三個點賦值給（x1, y1,z1）(x2, y2,z2)(x3, y3,z3)//這裏idx是迭代器，用於依次取出index容器中的前三個隨機數，然後把隨機數作爲點雲下標，取出三個點賦值（x1, y1,z1）(x2, y2,z2)(x3, y3,z3)
Platform p;//p是個類 平面的一般公式是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是在算平面點法式的係數
for(auto db_index = db->begin();db_index !=db->end();++db_index)//db是濾波後的點雲
{
double x4 = db_index->x;
double y4 = db_index->y;
double z4 = db_index->z;//（這個x4y4z4是開始遍歷濾波後的所有的點，並計算每個點到以計算出的平面的距離）
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);//點到平面的距離公式有兩種見博客文字解釋此處採用的向量法第一種。
if(dis<0.12)//如果點到平面的距離小於0.12就把他歸爲平面上的點
index.push_back(db_index - db->begin());
}
}
//更新集合
if(index.size()>index_final.size())//對新更新的平面進行索引統計？？？
{
index_final = index;//每次循環會選取隨機三個點，並求出三個點組成的平面，然後計算查詢得到在平面一定範圍內的點的下標，保存進index這個容器中，而index_final是用於記錄包含點最多的那組index，如果這次循環得到在平面一定範圍內點數量比index_final的多，就用index去更新index_final.
plat_point = PointXYZ(x1,y1,z1);//這裏保存了周圍點數量最多的那個平面上的一個點，用於全體點雲計算到平面的距離
ABC = Eigen::Vector3d(p.a,p.b,p.c);//這裏就是平面的法向量，用於之後計算點到平面的距離。
}
}
/*對所有點雲進行計算離平面距離,並提取對應的index*/
std::vector<int> platform_index;//存放平面點index的容器
std::vector<int> unplatform_index;//存放非平面點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;//計算濾波後的點到平面上的一點的距離
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());//就把該點納爲平面內的點
}else
{
unplatform_index.push_back(all_db_index - all_db->begin());//否則作爲非平面內的點
}
}
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 << "最大數量cluster的數量:" << cluster_list[max_index]->size() << endl;
/*再次對最大連通域聚類*/
}
主函數：
#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 << "濾波後的點雲數量：" << 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);//初始化octree
octree.setInputCloud(unplat_points);
octree.addPointsFromInputCloud();
/*dbscan搜索*/
dbscan scan(unplat_points,octree,search_radius,8);//radius與min_sample
scan.run();
/*找出點雲數最多的聚類*/
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]);
}
/*去除噪點*/
cluster_list.erase(cluster_list.begin(),cluster_list.begin()+1);
/*刪除沒識別到的地面*/
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 << "總體用時 = " << time_used5.count() << " ms. " << endl;
/*顯示*/
show_point_cloud(cluster_list,cluster_num,cluster_sort);
return 0;
}