RANSAC算法（2）：（拟合平面）本文以地面为基础以及源码分布解读

本章代码是本人根据一个未曾谋面的好人学习的（要怀抱希望，世界上好人真的是很多的，我要做一个去给别人带去正能量积极态度的人，加油喽），如需转载学习请注明。谢谢

---------------基于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();//清除sizewhile(--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);//初始化octreeoctree.setInputCloud(unplat_points);octree.addPointsFromInputCloud();/*dbscan搜索*/dbscan scan(unplat_points,octree,search_radius,8);//radius与min_samplescan.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;
}

下面是：PCL 实现最小二乘平面拟合（参考博客：https://blog.csdn.net/qq_36686437/article/details/109137124）

一、算法原理

参考文献：[1]曹诗卉,亓迎川.基于最小二乘法平面拟合的点云法矢算法[J].空军预警学院学报,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：返回参数A B C D */
/* point：存放输入点 */
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;}//  ----------------可视化，左窗口为原始点云，右窗口为根据点云拟合出来的平面--------------
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);// ---------------------点云按照z字段进行渲染--------------------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)
{// 加载点云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 << "拟合系数:" << "\n A=" << Coff[0] << "\n B=" << Coff[1] << "\n C=" << Coff[2] << "\n D=" << Coff[3] << endl;visualize_pcd(cloud, plane_coeff);return (0);
}

结果显示：

四、拟合显示：addPlane()

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