本篇博客将从代码角度以从0到1的方式深度解读这篇paper的算法。paper和开源代码来源：

数据结构

utils.h

首先为了存储点云，在utils.h头文件中定义了PointCloud的结构体。并且在结构体中定义了存储每个点的PtData结构体，以及定义了 std::vector<PtData>pts 以来存出点云中每个点云的(x,y,z)三维坐标值。代码如下：

template <typename T>
struct PointCloud
{
    
    
	struct PtData  //存储每个点
	{
    
    
		T  x,y,z;

		PtData(T xx, T yy, T zz) {
    
     x = xx; y = yy; z = zz;}
		PtData &operator =(const PtData &info)
		{
    
    
			this->x    = info.x;
			this->y    = info.y;
			this->z    = info.z;
			return *this;
		}
	};

	std::vector<PtData>  pts;

	// operator =
	PointCloud &operator =(const PointCloud &info)
	{
    
    
		this->pts    = info.pts;
		return *this;
	}

	// Must return the number of data points
	inline size_t kdtree_get_point_count() const {
    
     return pts.size(); }

	// Returns the dim'th component of the idx'th point in the class:
	// Since this is inlined and the "dim" argument is typically an immediate value, the
	//  "if/else's" are actually solved at compile time.
	inline T kdtree_get_pt(const size_t idx, int dim) const
	{
    
    
		if (dim == 0) return pts[idx].x;
		else if (dim == 1) return pts[idx].y;
		else return pts[idx].z;
	}

	// Optional bounding-box computation: return false to default to a standard bbox computation loop.
	//   Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again.
	//   Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds)
	template <class BBOX>
	bool kdtree_get_bbox(BBOX& /* bb */) const {
    
     return false; }
};

LineDetection3D.h

在LineDetection3D.h中定义了LineDetection3D类和PLANE结构体。LineDetection3D类中的数据定义部分定义了关于整个点云数据的一些参数，如领域点的个数k,数据点的个数pointNum等；方法定义部分则定义了实现该paper算法的主要函数，比如实现整个算法的主函数run()。 PLANE则定义了点云中的某个面的参数，参数包括面的 scale 以及存储该面中所有line segment的 line3d 。LineDetection3D.h中的所有函数的实现全部在LineDetection3D.cpp里，在接下来的讲解会逐一讲解。

struct PLANE   //表达一个面
{
    
    
	double scale;  // 定义面的scale
	std::vector<std::vector<std::vector<cv::Point3d> > > lines3d;  //存储面中所有的line segment
 
	PLANE &operator =(const PLANE &info)
	{
    
    
		this->scale    = info.scale;
		this->lines3d     = info.lines3d;
		return *this;
	}
};

class LineDetection3D 
{
    
    
public:
	LineDetection3D();
	~LineDetection3D();

	void run( PointCloud<double> &data, int k, std::vector<PLANE> &planes, std::vector<std::vector<cv::Point3d> > &lines, std::vector<double> &ts );  //整个算法的主函数

	void pointCloudSegmentation( std::vector<std::vector<int> > &regions );

	void planeBased3DLineDetection( std::vector<std::vector<int> > &regions, std::vector<PLANE> &planes );

	void postProcessing( std::vector<PLANE> &planes, std::vector<std::vector<cv::Point3d> > &lines );

	// 
	void regionGrow( double thAngle, std::vector<std::vector<int> > &regions );

	void regionMerging( double thAngle, std::vector<std::vector<int> > &regions );

	bool maskFromPoint( std::vector<cv::Point2d> &pts2d, double radius, double &xmin, double &ymin, double &xmax, double &ymax, int &margin, cv::Mat &mask );

	void lineFromMask( cv::Mat &mask, int thLineLengthPixel, std::vector<std::vector<std::vector<cv::Point2d> > > &lines );

	void outliersRemoval( std::vector<PLANE> &planes );

	void lineMerging( std::vector<PLANE> &planes, std::vector<std::vector<cv::Point3d> > &lines );

public:  //定义了整个点云数据的某些参数
	int k;
	int pointNum;
	double scale, magnitd;
	std::vector<PCAInfo> pcaInfos;
	PointCloud<double> pointData;
};

CommonFunctions.h

CommonFunctions主要的功能主要是定义了一些公用的函数：比如利用PCA求法向量、从轮廓中提取线段等。CommonFunctions.h里主要定义了LineFunctions类、PCAInfo结构体、PCAFunctions类。PCAInfo类主要在点或者面进行PCA操作时需要用到的一些参数，例如法向量normal、平面的均值点planePt、scale。PCAFunctions类主要是包含了跟PCA操作的几个函数，例如求每个点的法向量的Ori_PCA()、利用PCA拟合平面的PCAsingle()。LineFunctions类主要是包含了对轮廓线进行直线段进行拟合的一些函数，如lineFitting()。CommonFunctions.h中的所有函数的实现全部在CommonFunctions.cpp里，在接下来的讲解会逐一讲解。

#include <opencv2/opencv.hpp>
#include <vector>

#include "nanoflann.hpp"
#include "utils.h"

using namespace cv;
using namespace std;
using namespace nanoflann;

class LineFunctions
{
    
    
public:
	LineFunctions(void){
    
    };
	~LineFunctions(void){
    
    };

public:
	static void lineFitting( int rows, int cols, std::vector<cv::Point> &contour, double thMinimalLineLength, std::vector<std::vector<cv::Point2d> > &lines );

	static void subDivision( std::vector<std::vector<cv::Point> > &straightString, std::vector<cv::Point> &contour, int first_index, int last_index
		, double min_deviation, int min_size  );

	static void lineFittingSVD( cv::Point *points, int length, std::vector<double> &parameters, double &maxDev );
};

struct PCAInfo
{
    
    
	double lambda0, scale;
	cv::Matx31d normal, planePt;
	std::vector<int> idxAll, idxIn;

	PCAInfo &operator =(const PCAInfo &info)
	{
    
    
		this->lambda0 = info.lambda0;
		this->normal = info.normal;
		this->idxIn = info.idxIn;
		this->idxAll = info.idxAll;
		this->scale = scale;
		return *this;
	}
};

class PCAFunctions 
{
    
    
public:
	PCAFunctions(void){
    
    };
	~PCAFunctions(void){
    
    };

	void Ori_PCA( PointCloud<double> &cloud, int k, std::vector<PCAInfo> &pcaInfos, double &scale, double &magnitd );

	void PCASingle( std::vector<std::vector<double> > &pointData, PCAInfo &pcaInfo );

	void MCMD_OutlierRemoval( std::vector<std::vector<double> > &pointData, PCAInfo &pcaInfo );

	double meadian( std::vector<double> dataset );
};

读入数据

在main.cpp 文件中定义readDataFromFile函数来读入数据，首先在main主函数中定义数据路径。

void main() 
{
    
    
	string fileData = "D://Facade//data.txt";
	string fileOut  = "D://Facade//data";

	// read in data
	PointCloud<double> pointData; 
	readDataFromFile( fileData, pointData );
}

然后定义readDataFromFile函数

void readDataFromFile( std::string filepath, PointCloud<double> &cloud )
{
    
    
	cloud.pts.reserve(10000000);  //表明点云数据最大为1000万个点
	cout<<"Reading data ..."<<endl;

	// 1. read in point data
	std::ifstream ptReader( filepath );
	std::vector<cv::Point3d> lidarPoints;
	double x = 0, y = 0, z = 0, color = 0;
	double nx, ny, nz;
	int a = 0, b = 0, c = 0; 
	int labelIdx = 0;
	int count = 0;
	int countTotal = 0;
	if( ptReader.is_open() )
	{
    
    
		while ( !ptReader.eof() ) 
		{
    
    
			//ptReader >> x >> y >> z >> a >> b >> c >> labelIdx;
			//ptReader >> x >> y >> z >> a >> b >> c >> color;
			//ptReader >> x >> y >> z >> color >> a >> b >> c;
			//ptReader >> x >> y >> z >> a >> b >> c ;
			ptReader >> x >> y >> z;
			//ptReader >> x >> y >> z >> color;
			//ptReader >> x >> y >> z >> nx >> ny >> nz;

			cloud.pts.push_back(PointCloud<double>::PtData(x,y,z));

		}
		ptReader.close();
	}

	std::cout << "Total num of points: " << cloud.pts.size() << "\n";
}

算法

算法的主要实现就在** run函数中，首先要在main主函数中引用run(pointData, k, planes, lines, ts)**

在这里插入代码片
```void main() 
{
    
    
	string fileData = "D://Facade//data.txt";
	string fileOut  = "D://Facade//data";

	// read in data
	PointCloud<double> pointData; 
	readDataFromFile( fileData, pointData );

	int k = 20;
	LineDetection3D detector;
	std::vector<PLANE> planes;
	std::vector<std::vector<cv::Point3d> > lines;
	std::vector<double> ts;
	detector.run( pointData, k, planes, lines, ts );
}

run函数的定义在LineDetection.cpp 里，主要包含三个函数：pointCloudSegmentaion(regions) 、 planeBased3DLineDetection(regions,planes)、postProcessing(planes,lines) 。三个函数分别实现平面分割、3D线段提取、后处理。

void LineDetection3D::run( PointCloud<double> &data, int k, std::vector<PLANE> &planes, std::vector<std::vector<cv::Point3d> > &lines, std::vector<double> &ts  )
{
    
    
	this->pointData = data;
	this->pointNum = data.pts.size();
	this->k = k;

	// step1: point cloud segmentation
	double totalTime = 0.0;
	CTimer timer;
	char msg[1024];

	timer.Start();
	cout<<endl<<endl;
	cout<<"Step1: Point Cloud Segmentation ..."<<endl;
	std::vector<std::vector<int> > regions;
	pointCloudSegmentation( regions );  //算法第一步：平面分割
	timer.Stop();
	totalTime += timer.GetElapsedSeconds();
	timer.PrintElapsedTimeMsg(msg);
	printf("  Point Cloud Segmentation Time: %s.\n\n", msg);
	ts.push_back(timer.GetElapsedSeconds());

	// step2: plane based 3D line detection
	timer.Start();
	cout<<"Step2: Plane Based 3D LineDetection ..."<<endl;
	planeBased3DLineDetection( regions, planes ); //算法第二步：3D线段提取
	timer.Stop();
	totalTime += timer.GetElapsedSeconds();
	timer.PrintElapsedTimeMsg(msg);
	printf("  Plane Based 3D LineDetection Time: %s.\n\n", msg);
	ts.push_back(timer.GetElapsedSeconds());

	// step3: post processing
	timer.Start();
	cout<<"Step3: Post Processing ..."<<endl;
	postProcessing( planes, lines );  //算法第三步：后处理
	timer.Stop();
	totalTime += timer.GetElapsedSeconds();
	timer.PrintElapsedTimeMsg(msg);
	printf("  Post Processing Time: %s.\n\n", msg);
	ts.push_back(timer.GetElapsedSeconds());

	printf("Total Time: %lf.\n\n", totalTime);
}

算法第一步：平面分割

平面分割（实现函数：pointCloudSegmentation(））主要分为三部分：（1）Normal Calculation：计算每个点的法向量以及获得每个点的scale及其它参数（实现函数：pcaer.Ori_PCA() ）；（2）Region Growing：对每个点进行平面增长，得到一个个平面；（实现函数：regionGrow()）; (3)Region Merging: 对第二步得到的平面进行合并，得到更大一点的平面（实现函数：regionMerging()）。上述除掉Ori_PCA()定义在CommonFunction里，其它都定义在LineDetection3D里。

void LineDetection3D::pointCloudSegmentation( std::vector<std::vector<int> > &regions )
{
    
    
	cout<<"----- Normal Calculation ..."<<endl;
	PCAFunctions pcaer;
	pcaer.Ori_PCA( this->pointData, this->k, this->pcaInfos, this->scale, this->magnitd );
	
	cout<<"----- Region Growing ..."<<endl;
	double thAngle = 15.0/180.0*CV_PI;
	regionGrow( thAngle, regions );

	// step3: region merging
	cout<<"----- Region Merging ..."<<endl;
	double thAnglePatch = thAngle;
	regionMerging( thAnglePatch, regions );
}

Normal Calculation

这一步主要是对每个点进行法向量的计算，求每个点的法向量的步骤有以下几步：
（1）对整个数据点建立kd-tree；
（2）循环整个点云中每个点，根据建好的索引找出每个点的最近邻的20(k=20)的点；
（3）根据最近邻点计算协防差矩阵，随即可以求得该协方差矩阵的特征值和对应的特征向量；
（4）上一步最小的特征值对应的特征向量就是该点的法向量， $\lambda = \frac {\lambda_3}{\lambda_1+\lambda_2+\lambda_3},\lambda_1,\lambda_2,\lambda_3$ 分别表示由大到小排列的特征值，该点的scale=表示第i点到这点第三近邻点的距离。
该算法的代码实现是在PCAFunctions类中的**Ori_PCA()**函数中，这个函数的参数包含以下：

para1： PointCloud<double> &cloud ： this->pointData;(其中的this是LineDetection3D类的一个实例)
para2:  int k :20;
para3:  std::vector<PCAInfo> &pcaInfos:this->pcaInfos; 
para4:  doubel& scale: this->scale;   //每个点的scale
parta5: doubel &magnitd: this->magnitd  //存储整个数据第一个点到坐标原点的距离

**Ori_PCA()**的实现代码如下,其中主要的步骤如下：

void PCAFunctions::Ori_PCA( PointCloud<double> &cloud, int k, std::vector<PCAInfo> &pcaInfos, double &scale, double &magnitd )
{
    
    
	double MINVALUE = 1e-7;
	int pointNum = cloud.pts.size();  //点云的数量

	// 1. build kd-tree
	typedef KDTreeSingleIndexAdaptor< L2_Simple_Adaptor<double, PointCloud<double> >, PointCloud<double>, 3/*dim*/ > my_kd_tree_t;   //利用FLANN库构建kd-tree
	my_kd_tree_t index(3 /*dim*/, cloud, KDTreeSingleIndexAdaptorParams(10 /* max leaf */) );
	index.buildIndex();

	// 2. knn search
	size_t *out_ks = new size_t[pointNum];  //out_ks[i] 存储着第i个点的近邻点的个数
	size_t **out_indices = new size_t *[pointNum];  // out_indices[i]存储着第i个点所有近邻点的序号，例如out_indices[i][k]就是第i个点第k个近邻点的索引号
#pragma omp parallel for
	for (int i=0; i<pointNum; ++i)
	{
    
    
		double *query_pt = new double[3];
		query_pt[0] = cloud.pts[i].x;  query_pt[1] = cloud.pts[i].y;  query_pt[2] = cloud.pts[i].z;
		double *dis_temp = new double[k];
		out_indices[i]= new size_t[k];

		nanoflann::KNNResultSet<double> resultSet(k);
		resultSet.init(out_indices[i], dis_temp );
		index.findNeighbors(resultSet, &query_pt[0], nanoflann::SearchParams(10));
		out_ks[i] = resultSet.size();

		delete query_pt;
		delete dis_temp;
	}
	index.freeIndex(index);

	// 3. PCA normal estimation
	scale = 0.0;
	pcaInfos.resize( pointNum );
#pragma omp parallel for
	for ( int i = 0; i < pointNum; ++i ) //循环每个点
	{
    
    
		// 
		int ki = out_ks[i];

		double h_mean_x = 0.0, h_mean_y = 0.0, h_mean_z = 0.0;
		for( int j = 0; j < ki; ++j )
		{
    
    
			int idx = out_indices[i][j];
			h_mean_x += cloud.pts[idx].x;
			h_mean_y += cloud.pts[idx].y;
			h_mean_z += cloud.pts[idx].z;
		}
		h_mean_x *= 1.0/ki;  h_mean_y *= 1.0/ki; h_mean_z *= 1.0/ki;  //求某个点所有近邻点的均值向量

		double h_cov_1 = 0.0, h_cov_2 = 0.0, h_cov_3 = 0.0;
		double h_cov_5 = 0.0, h_cov_6 = 0.0;
		double h_cov_9 = 0.0;  
		double dx = 0.0, dy = 0.0, dz = 0.0;
		for( int j = 0; j < k; ++j )  //求解协方差矩阵，k=20，为近邻点的个数，我认为为ki更合适
		{
    
    
			int idx = out_indices[i][j];
			dx = cloud.pts[idx].x - h_mean_x;
			dy = cloud.pts[idx].y - h_mean_y;
			dz = cloud.pts[idx].z - h_mean_z;

			h_cov_1 += dx*dx; h_cov_2 += dx*dy; h_cov_3 += dx*dz;
			h_cov_5 += dy*dy; h_cov_6 += dy*dz;
			h_cov_9 += dz*dz;
		}
		cv::Matx33d h_cov(  //协方差矩阵
			h_cov_1, h_cov_2, h_cov_3, 
			h_cov_2, h_cov_5, h_cov_6, 
			h_cov_3, h_cov_6, h_cov_9);
		h_cov *= 1.0/ki;

		// eigenvector
		cv::Matx33d h_cov_evectors; //特征向量
		cv::Matx31d h_cov_evals;  //特征值
		cv::eigen( h_cov, h_cov_evals, h_cov_evectors );  //求解这个点的协方差举证的特征向量和特征值

		// 
		pcaInfos[i].idxAll.resize( ki );  //pcaInfos[i]即为PCAInfos结构体，里面的参数表示第i个点进行PCA时用到的一些参数
		for ( int j =0; j<ki; ++j )  //pacaInfos[i].idxAll :存储着第i点所有近邻点的序号
		{
    
    
			int idx = out_indices[i][j];
			pcaInfos[i].idxAll[j] = idx;
		}

		int idx = out_indices[i][3]; //表示第i点第三近邻点的索引号
		dx = cloud.pts[idx].x - cloud.pts[i].x;
		dy = cloud.pts[idx].y - cloud.pts[i].y;
		dz = cloud.pts[idx].z - cloud.pts[i].z;
		double scaleTemp = sqrt(dx*dx + dy*dy + dz*dz);
		pcaInfos[i].scale = scaleTemp; //pcaInfos[i].scale存储着第i点的scale，这个scale表示第i点到这点第三近邻点的距离
		scale += scaleTemp;

		//pcaInfos[i].lambda0 = h_cov_evals.row(2).val[0];
		double t = h_cov_evals.row(0).val[0] + h_cov_evals.row(1).val[0] + h_cov_evals.row(2).val[0] + ( rand()%10 + 1 ) * MINVALUE;
		pcaInfos[i].lambda0 = h_cov_evals.row(2).val[0] / t; //pcaInfos[i].lambda0存储着第i点的lambda0 = lambda3/(lambda1+lambda2+lambda2),对应着曲率
		pcaInfos[i].normal = h_cov_evectors.row(2).t();  //pcaInfos[i].normal存储着第i点的法向量，即为协方差矩阵的第3个特征向量(对应特征值最小的那个)

		// outliers removal via MCMD
		pcaInfos[i].idxIn = pcaInfos[i].idxAll;

		delete out_indices[i]; //删除二维指针的第二维
	}
	delete []out_indices;  //删除二维指针的第一维
	delete out_ks;  //删除一维指针

	scale /= pointNum; //scale为LineDetection3D类的成员，表示整个数据的scale，即每个点的scale的总和/所有点的个数
	magnitd = sqrt(cloud.pts[0].x*cloud.pts[0].x + cloud.pts[0].y*cloud.pts[0].y + cloud.pts[0].z*cloud.pts[0].z);//magnitd为LineDetection3D类的成员，即为第一个点到坐标原点的距离
}

region growing

在本篇paper中，region growing的主要算法步骤如下：
（1）对每个点，按照 $\lambda_0$ 的大小进行升序排列。
（2）从排序后unprocessed的第一个点 $P_s$ 开始，我们令 $\tau$ list存储与 $P_s$ 共面的所有点，将 $P_s$ 点加入到 $\tau$ ，成为第一个点。
（3）对于 $\tau$ 一个未处理的点 $P_i$ , 遍历 $P_i$ 所有的近邻点 $P_j$ ,如果 $P_j$ 满足下面三个条件，则将 $P_j$ 加入到 $\tau$ 中。遍历完 $P_i$ 所有的近邻点之后，我们就把这个 $P_i$ 点定义为processed。
$\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ arccos(|n_{P_s}^T \bullet n_{P_j}|) <\theta \\ \ \ \ \ \ \ \ |n_{P_s}^T \bullet \overrightarrow {P_sP_j}| <th_o \\ ||\overrightarrow {P_sP_j}|| < th_p$
其中参数为 $\theta = 15^。，th_o = S_{P_s}，th_p = 50S_{P_s}，S_{P_s}为P_s的scale$
(4)重复第（2）、（3）步，直至所有点都是processed
该算法的代码实现是在LineDetection3D类中的regionGrow函数中，这个函数的参数包含以下：

void LineDetection3D::regionGrow( double thAngle, std::vector<std::vector<int> > &regions )
{
    
    
	double thNormal = cos(thAngle);  //阈值大小

	// sort according to the curvature of points
	std::vector<std::pair<int,double> > idxSorted( this->pointNum );
	for ( int i=0; i<this->pointNum; ++i )
	{
    
    
		idxSorted[i].first = i;
		idxSorted[i].second = pcaInfos[i].lambda0;
	}
	std::sort( idxSorted.begin(), idxSorted.end(), [](const std::pair<int,double>& lhs, const std::pair<int,double>& rhs) {
    
     return lhs.second < rhs.second; } );

	// get the initial clusters
	double percent = 0.9;
	int idx = int(this->pointNum*percent);
	std::vector<int> isUsed( this->pointNum, 0 );
	for ( int i=0; i<idx; ++i )  //循环每个点
	{
    
    
		int idxStrater = idxSorted[i].first; //对应着Ps点
		if ( isUsed[idxStrater] ) {
    
     continue; }
		cv::Matx31d normalStarter = pcaInfos[idxStrater].normal;
		double xStrater = pointData.pts[idxStrater].x, yStrater = pointData.pts[idxStrater].y, zStrater = pointData.pts[idxStrater].z;
		double thRadius2 = pow(50*pcaInfos[idxStrater].scale, 2); //阈值th_p
        double thOrtho = pcaInfos[idxSeed].scale;//阈值th_o
        
		std::vector<int> clusterTemp; //对应着list T
		clusterTemp.reserve(10000);
		clusterTemp.push_back( idxStrater );
		int count = 0;
		while( count < clusterTemp.size() )  //循环list T中未处理的点
		{
    
    
			int idxSeed = clusterTemp[count]; //对应着list T中未处理的点Pi
			cv::Matx31d normalSeed = pcaInfos[idxSeed].normal;
			//double thOrtho = pcaInfos[idxSeed].scale;

			// point cloud collection
			int num = pcaInfos[idxSeed].idxAll.size();
			for( int j = 0; j < num; ++j )  //循环点Pi所有近邻点Pj
			{
    
    
				int idxCur = pcaInfos[idxSeed].idxAll[j]; //对应着点Pj
				if (isUsed[idxCur])
				{
    
    
					continue;
				}

				// judgement1: normal deviation
				cv::Matx31d normalCur = pcaInfos[idxCur].normal;
				double normalDev = abs(normalCur.val[0] * normalStarter.val[0] + normalCur.val[1] * normalStarter.val[1] + normalCur.val[2] * normalStarter.val[2]);
				//double normalDev = abs(normalCur.val[0] * normalSeed.val[0] + normalCur.val[1] * normalSeed.val[1] + normalCur.val[2] * normalSeed.val[2]);
				if (normalDev < thNormal)
				{
    
    
					continue;
				}

				// judgement2: orthogonal distance
				double dx = pointData.pts[idxCur].x - xStrater;
				double dy = pointData.pts[idxCur].y - yStrater;
				double dz = pointData.pts[idxCur].z - zStrater;
				double dOrtho = abs(dx * normalCur.val[0] + dy * normalCur.val[1] + dz * normalCur.val[2]);
				if (dOrtho > thOrtho)
				{
    
    
					continue;
				}

				// judgement3: parallel distance
				double dPara = dx*dx + dy*dy + dz*dz;
				if (dPara > thRadius2)
				{
    
    
					continue;
				}

				clusterTemp.push_back( idxCur );
				isUsed[idxCur] = 1;
			}
			count ++;
		}

		if ( clusterTemp.size() > 30 )  //如果list T中的点的个数大于30，则保存
		{
    
    
			regions.push_back( clusterTemp );
		}
		else
		{
    
    
			for (int j=0; j<clusterTemp.size(); ++j)
			{
    
    
				isUsed[clusterTemp[j]] = 0;
			}
		}
	}
}

region merging

本文中的平面融合的算法主要包括以下几步：
（1）首先将region growing得到的regions的每个region进行PCA拟合，得到每个region的法向量、曲率( $\lambda_0$ )、scale。
（2）给每个region编号，然后根据其以下规则，找出每个region对应的相邻的region。规则：region $R_i$ ,对于该region上的每个点 $P_m$ , 然后遍历 $P_m$ 的领域 $I_{P_m}$ 中所有的点 $P_n$ , 若 $P_n$ 点所在的region不是 $R_i$ ,则 $P_n$ 点所在的region就是 $R_i$ 的相邻的region(adjacent region)。
（3）对于第一个unprocessed region $R_i$ ，我们令 $\Tau$ 为准备和 $R_i$ 合并的region的集合。该集合中unprocessed的region $R_m$ ，遍历该 $R_m$ 所有的adjacent region $R_n$ ，然后根据以下两个公式判断是否将该adjacent region $R_n$ 合并到该region $R_i$ 。遍历完之后，这个region $R_n$ 改为processed。
（4）重复步骤(3),直至所有的region都processed。
$\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ arccos(|n_{R_i}^T \bullet n_{R_n}|) <\theta \\ \ \ \ \ \ \ \ |n_{R_i}^T \bullet \overrightarrow {P_rP_n}| <th_o \\$
其中， $n_{R_i},n_{R_n}$ 分别为region ${R_i}、{R_n}$ 的法向量， $\overrightarrow {P_rP_n}$ 表示region ${R_i}$ 均值点到region $R_n$ 均值点的矢量， $\theta = 15^。，th_o = S_{R_i}$

该算法主要由LineDetection3D类中regionMerging函数实现，其中，拟合每个平面由PCAFunctions类中PCASingle函数实现。

void LineDetection3D::regionMerging( double thAngle, std::vector<std::vector<int> > &regions )
{
    
    
	double thRegionSize = 600000;  //每个region合并之后的点个数的最大数

	// step1: plane fitting via PCA for each region
	std::vector<PCAInfo> patches;
	patches.resize( regions.size() );

#pragma omp parallel for
	for ( int i=0; i<regions.size(); ++i )  //循环每个region，对每个region进行拟合
	{
    
    
		int pointNumCur = regions[i].size();
		std::vector<std::vector<double> > pointDataCur(pointNumCur);
		for ( int j=0; j<pointNumCur; ++j )
		{
    
    
			pointDataCur[j].resize(3);
			pointDataCur[j][0] = this->pointData.pts[regions[i][j]].x;
			pointDataCur[j][1] = this->pointData.pts[regions[i][j]].y;
			pointDataCur[j][2] = this->pointData.pts[regions[i][j]].z;
		}

		PCAFunctions pcaer;
		pcaer.PCASingle( pointDataCur, patches[i] );

		patches[i].idxAll = regions[i];
		double scaleAvg = 0.0;
		for ( int j=0; j<patches[i].idxIn.size(); ++j )
		{
    
    
			int idx = regions[i][patches[i].idxIn[j]];
			patches[i].idxIn[j] = idx;
			scaleAvg += pcaInfos[idx].scale;
		}
		scaleAvg /= patches[i].idxIn.size();
		patches[i].scale = 5.0 * scaleAvg;
	}

	// get the patch label of each point
	std::vector<int> label( this->pointNum, -1 );
#pragma omp parallel for
	for ( int i=0; i<regions.size(); ++i )
	{
    
    
		for ( int j=0; j<regions[i].size(); ++j )
		{
    
    
			int id = regions[i][j];
			label[id] = i;
		}
	}

	// step2: find adjacent patches
	std::vector<std::vector<int> > patchAdjacent( patches.size() );
#pragma omp parallel for
	for ( int i=0; i<patches.size(); ++i )
	{
    
    
		std::vector<int> patchAdjacentTemp; //存放这个region的adjacent pathces的序号
		std::vector<std::vector<int> > pointAdjacentTemp; //pointAdjacentTemp[i]存放region[i]判断为adjacent patches的近邻点Pn，即边界点boundary point。
		for ( int j=0; j<patches[i].idxIn.size(); ++j ) //遍历这个patch(region)中的每个点
		{
    
    
			int id = patches[i].idxIn[j]; //这个patch中的第id个点
			for ( int m=0; m<pcaInfos[id].idxIn.size(); ++m ) //循环第id点所有的邻近点
			{
    
    
				int idPoint = pcaInfos[id].idxIn[m];  //第id点的邻近点idPoint
				int labelPatch = label[idPoint];
				if ( labelPatch == i || labelPatch < 0 )
				{
    
    
					continue;
				}
     
                //判断id点和idPoint点（boundary point）是否为neighbor，如果idPoint的领域中也由id点，则这两点就为neighbour
				bool isNeighbor = false;
				for ( int n=0; n<pcaInfos[idPoint].idxIn.size(); ++n )
				{
    
    
					if ( pcaInfos[idPoint].idxIn[n] == id )
					{
    
    
						isNeighbor = true;
					}
				}
				if ( ! isNeighbor )
				{
    
    
					continue;
				}

				// accept the patch as a neighbor
				bool isIn = false;
				int n = 0;
				for ( n=0; n<patchAdjacentTemp.size(); ++n )
				{
    
    
					if (patchAdjacentTemp[n] == labelPatch )
					{
    
    
						isIn = true;
						break;
					}
				}

				if ( isIn )
				{
    
    
					pointAdjacentTemp[n].push_back( idPoint );
				}
				else
				{
    
    
					patchAdjacentTemp.push_back( labelPatch );

					std::vector<int> temp;
					temp.push_back( idPoint );
					pointAdjacentTemp.push_back( temp );
				}
			}
		}

		// repetition removal  去除使region成为adjacent patch的重复的boundary点
		for ( int j=0; j<pointAdjacentTemp.size(); ++j )
		{
    
    
			std::sort(pointAdjacentTemp[j].begin(), pointAdjacentTemp[j].end());  
			vector<int>::iterator new_end = unique(pointAdjacentTemp[j].begin(), pointAdjacentTemp[j].end());
			pointAdjacentTemp[j].erase(new_end, pointAdjacentTemp[j].end());

			if ( pointAdjacentTemp[j].size() >= 3 ) //如果使region成为adjacent patch的boundary点的个数小于3，则该region没有adjacent region
			{
    
    
				patchAdjacent[i].push_back( patchAdjacentTemp[j] );
			}
		}
	}

	// try to merge adjacent patch
	regions.clear();
	std::vector<int> mergedIndex( patches.size(), 0 );
	for ( int i=0; i<patches.size(); ++i ) //遍历每个region
	{
    
    
		if ( !mergedIndex[i] )
		{
    
    
			int idxStarter = i; //region Ri
			cv::Matx31d normalStarter = patches[idxStarter].normal;
			cv::Matx31d ptStarter = patches[idxStarter].planePt;

			std::vector<int> patchIdx; //集合T
			patchIdx.push_back( idxStarter );

			int count = 0;
			int totalPoints = 0;
			bool isEnough = false;
			while ( count < patchIdx.size() ) 
			{
    
    
				int idxSeed = patchIdx[count]; //region Rm
				cv::Matx31d normalSeed = patches[idxSeed].normal;
				cv::Matx31d ptSeed = patches[idxSeed].planePt;
				//double thOrtho = patches[idxSeed].scale;
				double thOrtho = patches[idxStarter].scale;

				for ( int j=0; j<patchAdjacent[idxSeed].size(); ++j ) 
				{
    
    
					int idxCur = patchAdjacent[idxSeed][j]; //region R_n

					if ( mergedIndex[idxCur] )
					{
    
    
						continue;
					}

					cv::Matx31d normalCur = patches[idxCur].normal;
					cv::Matx31d ptCur = patches[idxCur].planePt;

					// plane angle deviation and distance
					double devAngle = 0.0;
					double devDis = 0.0;
					double thDev = 0.0;

					cv::Matx31d ptVector1 = ptCur - ptStarter;
					//cv::Matx31d ptVector2 = ptCur - ptSeed;
					devAngle = acos( normalStarter.val[0] * normalCur.val[0] + normalStarter.val[1] * normalCur.val[1] + normalStarter.val[2] * normalCur.val[2] ); //theta
					//devDis = abs( normalSeed.val[0] * ptVector2.val[0] + normalSeed.val[1] * ptVector2.val[1] + normalSeed.val[2] * ptVector2.val[2] );
					devDis = abs( normalStarter.val[0] * ptVector1.val[0] + normalStarter.val[1] * ptVector1.val[1] + normalStarter.val[2] * ptVector1.val[2] ); //tho

					if ( min( devAngle, fabs( CV_PI - devAngle ) ) < thAngle && devDis < thOrtho )
					{
    
    
						patchIdx.push_back( idxCur );
						mergedIndex[idxCur] = 1;

						totalPoints += patches[idxCur].idxAll.size();
						if ( totalPoints > thRegionSize )
						{
    
    
							isEnough = true;
							break;
						}
					}
				}

				if ( isEnough )
				{
    
    
					break;
				}
				count ++;
			}

			// create a new cluster
			std::vector<int> patchNewCur; //为合并之后region中所有点的索引号
			for ( int j=0; j<patchIdx.size(); ++j )
			{
    
    
				int idx = patchIdx[j];

				for ( int m=0; m<patches[idx].idxAll.size(); ++m )
				{
    
    
					patchNewCur.push_back( patches[idx].idxAll[m] );
				}
			}

			// 
			if (patchNewCur.size() > 100)  //如果合并之后的region中的point的个数大于100
			{
    
    
				regions.push_back( patchNewCur );
			}
		}
	}
}

void PCAFunctions::PCASingle( std::vector<std::vector<double> > &pointData, PCAInfo &pcaInfo )
{
    
    
	int i, j;
	int k = pointData.size();
	double a = 1.4826;
	double thRz = 2.5;

	// 
	pcaInfo.idxIn.resize( k );
	cv::Matx31d h_mean( 0, 0, 0 );
	for( i = 0; i < k; ++i )
	{
    
    
		pcaInfo.idxIn[i] = i;
		h_mean += cv::Matx31d( pointData[i][0], pointData[i][1], pointData[i][2] );
	}
	h_mean *= ( 1.0 / k );

	cv::Matx33d h_cov( 0, 0, 0, 0, 0, 0, 0, 0, 0 );
	for( i = 0; i < k; ++i )
	{
    
    
		cv::Matx31d hi = cv::Matx31d( pointData[i][0], pointData[i][1], pointData[i][2] );
		h_cov += ( hi - h_mean ) * ( hi - h_mean ).t();
	}
	h_cov *=( 1.0 / k );

	// eigenvector
	cv::Matx33d h_cov_evectors;
	cv::Matx31d h_cov_evals;
	cv::eigen( h_cov, h_cov_evals, h_cov_evectors );

	// 
	pcaInfo.idxAll = pcaInfo.idxIn;
	//pcaInfo.lambda0 = h_cov_evals.row(2).val[0];
	pcaInfo.lambda0 = h_cov_evals.row(2).val[0] / ( h_cov_evals.row(0).val[0] + h_cov_evals.row(1).val[0] + h_cov_evals.row(2).val[0] );
	pcaInfo.normal  = h_cov_evectors.row(2).t();
	pcaInfo.planePt = h_mean;

	// outliers removal via MCMD
	MCMD_OutlierRemoval( pointData, pcaInfo );	
}

算法第二步：Plane Based 3D Line Detection

第二步算法的主要有以下几步：
（1）对上述最后得到的合并之后的region通过PCA拟合成新的region；
（2）对每个合并之后的region(3D) 将它投影到 2D形成灰度图像；
（3）在灰度图像上利用findContour函数提取所有轮廓，然后将提取到的contours分割成一个个line segment，最后用PCA拟合。
（4）将第三步的提取到的line segment灰度图反投影到3D中，从而得到三维中的line segment
这个算法的具体实现是在LineDetection3D中的**planeBased3DLineDetection()**函数中

void LineDetection3D::planeBased3DLineDetection( std::vector<std::vector<int> > &regions, std::vector<PLANE> &planes )
{
    
    
	double thAngle = 10.0/180.0*CV_PI;
	double thLineLength = 8*this->scale;
	int numPatches = regions.size();

    // step1: fitting 3D plane via PCA
	std::vector<PCAInfo> patches(numPatches);
#pragma omp parallel for
	for ( int i=0; i<numPatches; ++i )
	{
    
    
		int pointNumCur = regions[i].size();
		std::vector<std::vector<double> > pointDataCur(pointNumCur);
		for ( int j=0; j<pointNumCur; ++j )
		{
    
    
			pointDataCur[j].resize(3);
			pointDataCur[j][0] = this->pointData.pts[regions[i][j]].x;
			pointDataCur[j][1] = this->pointData.pts[regions[i][j]].y;
			pointDataCur[j][2] = this->pointData.pts[regions[i][j]].z;
		}

		PCAFunctions pcaer;
		pcaer.PCASingle( pointDataCur, patches[i] );

		patches[i].idxAll = regions[i];
		for ( int j=0; j<patches[i].idxIn.size(); ++j )
		{
    
    
			int idx = patches[i].idxIn[j];
			patches[i].idxIn[j] = regions[i][idx];
		}
	}

	// step2: 3D line detection
	planes.resize(patches.size()); //planes存储最后处理的有线结构的三维plane
#pragma omp parallel for
	for(int i=0; i<patches.size(); ++i)  //循环遍历每个region
	{
    
    
		// A. 3D-2D Projection: project the 3d point onto the plane coordinate
		std::vector<cv::Point2d> pts2d;  //存储由3D点投影到2D点的坐标
		std::vector<double> ptScales;    //存储每个点的scale
		
		bool initialized = false;
		cv::Mat_<double> vX, vY;
		cv::Mat_<double> planePt = (cv::Mat_<double>(3,1) << patches[i].planePt.val[0], patches[i].planePt.val[1], patches[i].planePt.val[2]);  //patch的均值点Pc
		cv::Mat_<double> normal  = (cv::Mat_<double>(3,1) << patches[i].normal.val[0], patches[i].normal.val[1], patches[i].normal.val[2]);   //patch的法向量

		for(int j=0; j<patches[i].idxAll.size(); ++j)  //循环遍历patch中的每个点
		{
    
    
			int id = patches[i].idxAll[j];  //patch中的点序号
			cv::Mat_<double> pt3d = (cv::Mat_<double>(3,1) << pointData.pts[id].x, pointData.pts[id].y, pointData.pts[id].z );  //patch中点的三维坐标

			cv::Mat_<double> v3d = pt3d - planePt;  //向量Pc-Pi
			cv::Mat_<double> vOrtho = v3d.dot(normal) * normal; //
			cv::Mat_<double> vPlane = v3d - vOrtho; //向量Pc-Pi‘
			cv::Mat_<double> ptPlane = planePt + vPlane;   

			if(!initialized)  //根据第一点求vX,vY
			{
    
    
				vX = vPlane * 1.0/(cv::norm(vPlane));
				vY = vX.cross(normal);
				vY = vY * 1.0/cv::norm(vY);
				initialized = true;
			}
			if( initialized ) //将三维点转化为二维点，并将二维坐标(x,y)存储在pts2d中
			{
    
    
				double x = vPlane.dot(vX);
				double y = vPlane.dot(vY);
				pts2d.push_back(cv::Point2d(x,y));
				ptScales.push_back(pcaInfos[id].scale);
			}
		}

		// A. 3D-2D Projection: get the side length of the grid cell
		double gridSideLength = 0;
		std::sort( ptScales.begin(), ptScales.end(), [](const double& lhs, const double& rhs) {
    
     return lhs < rhs; } );
		int idxNinety = min( int(double(ptScales.size()) * 0.9), int(ptScales.size()-1) );
		gridSideLength = ptScales[idxNinety] * 0.75;  //the side length of the grid cell，即像素的大小

		// A. 3D-2D Projection: get the binary image of the plane
		double xmin, ymin, xmax, ymax;
		int margin = 0;
		cv::Mat mask;//存储2维点转化成的灰度图
		bool isok = maskFromPoint( pts2d, gridSideLength, xmin, ymin, xmax, ymax, margin, mask );
		if ( !isok )
		{
    
    
			continue;
		}

		// B. 2D Line Detection
		int thLineLengthPixel = max(thLineLength/gridSideLength,10.0);
		std::vector<std::vector<std::vector<cv::Point2d> > > lines2d;
		lineFromMask( mask, thLineLengthPixel, lines2d );
		if (!lines2d.size())
		{
    
    
			continue;
		}

		// C. 2D-3D Projection
		planes[i].scale = gridSideLength;
		for ( int m=0; m<lines2d.size(); ++m ) 
		{
    
    
			std::vector<std::vector<cv::Point3d> > temp;
			for (int n=0; n<lines2d[m].size(); ++n)
			{
    
    
				double length = abs(lines2d[m][n][1].x-lines2d[m][n][0].x) + abs(lines2d[m][n][1].y-lines2d[m][n][0].y);
				if ( length < thLineLengthPixel )
				{
    
    
					continue;
				}

				lines2d[m][n][0].x = (lines2d[m][n][0].x - margin) * gridSideLength + xmin;
				lines2d[m][n][0].y = (lines2d[m][n][0].y - margin) * gridSideLength + ymin;

				lines2d[m][n][1].x = (lines2d[m][n][1].x - margin) * gridSideLength + xmin;
				lines2d[m][n][1].y = (lines2d[m][n][1].y - margin) * gridSideLength + ymin;

				cv::Mat_<double> xs = lines2d[m][n][0].x * vX;
				cv::Mat_<double> ys = lines2d[m][n][0].y * vY;
				cv::Mat_<double> pts = planePt + xs + ys;

				cv::Mat_<double> xe = lines2d[m][n][1].x * vX;
				cv::Mat_<double> ye = lines2d[m][n][1].y * vY;
				cv::Mat_<double> pte = planePt + xe + ye;

				std::vector<cv::Point3d> line3dTemp(2);
				line3dTemp[0] = cv::Point3d(pts(0), pts(1), pts(2));
				line3dTemp[1] = cv::Point3d(pte(0), pte(1), pte(2));

				temp.push_back( line3dTemp );
			}
			if (temp.size())
			{
    
    
				planes[i].lines3d.push_back(temp);
			}
		}
	}
}

bool LineDetection3D::maskFromPoint( std::vector<cv::Point2d> &pts2d, double radius, double &xmin, double &ymin, double &xmax, double &ymax, int &margin, cv::Mat &mask )
{
    
    
	xmin=10000000, ymin = 10000000;
	xmax=-xmin;
	ymax=-ymin;
	for (int i=0; i<pts2d.size(); ++i)
	{
    
    
		if(pts2d[i].x < xmin) {
    
     xmin = pts2d[i].x; }
		if(pts2d[i].x > xmax) {
    
     xmax = pts2d[i].x; }

		if(pts2d[i].y < ymin) {
    
     ymin = pts2d[i].y; }
		if(pts2d[i].y > ymax) {
    
     ymax = pts2d[i].y; }
	}

	margin = 4;
	int cols = (xmax-xmin) / radius + 2*margin;
	int rows = (ymax-ymin) / radius + 2*margin;
	if ( cols < 10 || rows < 10 )
	{
    
    
		return false;
	}

	mask = cv::Mat(rows, cols, CV_8UC1, cv::Scalar(0));
	for (int i=0; i<pts2d.size(); ++i)
	{
    
    
		int xInt = int((pts2d[i].x-xmin)/radius+0.5+margin);
		int yInt = int((pts2d[i].y-ymin)/radius+0.5+margin);
		mask.at<uchar>(yInt,xInt) = 255;
	}
	return true;
}

void LineDetection3D::lineFromMask( cv::Mat &mask, int thLineLengthPixel, std::vector<std::vector<std::vector<cv::Point2d> > > &lines )
{
    
    
	lines.clear();

	// get mask image via dilate and erode
	cv::Mat mask2;
	cv::dilate(mask, mask2, cv::Mat());
	cv::erode(mask2, mask2, cv::Mat());

	// A. contours
	double thLength = thLineLengthPixel;
	
	std::vector<std::vector<cv::Point> > contours;  
	std::vector<cv::Vec4i> hierarchy;  
	cv::findContours(mask2, contours, hierarchy, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);

	// B. line fitting from the contours
	for ( int i=0; i<contours.size(); ++i )
	{
    
    
		if ( contours[i].size() < 4*thLength  )
		{
    
    
			continue;
		}

		std::vector<std::vector<cv::Point2d> > lineTemp;
		LineFunctions::lineFitting( mask2.rows, mask2.cols, contours[i], thLength, lineTemp );
		lines.push_back(lineTemp);
	}
}

3D-2D projection

怎么将一个三维的点云平面plane投影到二维中，并生成二维图像？这篇paper给出的算法步骤是：
（1）将一个三维的plane $\Pi$ 上的每个point投影到二维中，三维坐标 $x_i,y_i,z_i)$ 转化成二维坐标 $x_{i2},y_{i2})$ 。转化步骤如下：

首先求出 $\Pi$ 对应的二维平面的原点、x轴、y轴。原点就是三维region的中心点即均值点，然后x轴 $v X$ 和y轴 $v Y$ 就是根据下面公式计算出来的。下面公式 $P_c、P_0、n_{\Pi}$ 分别指三维plane $\Pi$ 的均值点、第一个点和法向量。
$\overrightarrow{P_cP_0^{'}} = \overrightarrow{P_cP_0} -(\overrightarrow{P_cP_0^{'}} \bullet n_{\Pi})n_{\Pi} \\[2ex] vX = \frac {\overrightarrow{P_cP_0^{'}} } {||\overrightarrow{P_cP_0^{'}}||} \\[2ex] vY = vX \times n_{\Pi}$
三维plane中的每个点 $P_i$ $x_i,y_i,z_i)$ 按下面公式(3)求得二维坐标 $x_{i2},y_{i2})$
$\overrightarrow{P_cP_i} = P_i-P_c \\[2ex] \overrightarrow{P_cP_i^{'}} = \overrightarrow{P_cP_i} -(\overrightarrow{P_cP_i^{'}} \bullet n_{\Pi})n_{\Pi} \\[2ex] x_{i2} = \overrightarrow{P_cP_i^{'}} \bullet vX \\[2ex] y_{i2} = \overrightarrow{P_cP_i^{'}} \bullet vY$
代码实现如下：

for(int j=0; j<patches[i].idxAll.size(); ++j)  //循环遍历patch中的每个点
		{
    
    
			int id = patches[i].idxAll[j];  //patch中的点序号
			cv::Mat_<double> pt3d = (cv::Mat_<double>(3,1) << pointData.pts[id].x, pointData.pts[id].y, pointData.pts[id].z );  //patch中点的三维坐标

			cv::Mat_<double> v3d = pt3d - planePt;  //向量Pc-Pi
			cv::Mat_<double> vOrtho = v3d.dot(normal) * normal; //
			cv::Mat_<double> vPlane = v3d - vOrtho; //向量Pc-Pi‘
			cv::Mat_<double> ptPlane = planePt + vPlane;   

			if(!initialized)  //根据第一点求vX,vY
			{
    
    
				vX = vPlane * 1.0/(cv::norm(vPlane));
				vY = vX.cross(normal);
				vY = vY * 1.0/cv::norm(vY);
				initialized = true;
			}
			if( initialized ) //将三维点转化为二维点，并将二维坐标(x,y)存储在pts2d中
			{
    
    
				double x = vPlane.dot(vX);
				double y = vPlane.dot(vY);
				pts2d.push_back(cv::Point2d(x,y));
				ptScales.push_back(pcaInfos[id].scale);
			}
		}

然后计算灰度图像像素的大小 $s_{\Pi}$ ，计算公式： $0.75\{s_p\}_{0.9}$ , ${s_p\}_{0.9}$ 表示的意思是plane中每个point的scale按照升序排列时排在第90%的point的scale，再将这个值乘以0.75。

         double gridSideLength = 0;
         std::sort( ptScales.begin(), ptScales.end(), [](const double& lhs, const double& rhs) {
    
     return lhs < rhs; } );
		 int idxNinety = min( int(double(ptScales.size()) * 0.9),int(ptScales.size()-1) );            
		 gridSideLength = ptScales[idxNinety] * 0.75;  //the side length of the grid cell，即像素的大小

计算二维plane的 $x_{min},y_{min}$ ,然后计算每个点 $x_{i2},y_{i2})$ 的像素坐标 $u_i,v_i)$ ，计算公式(5)如下:
$u_i = [(x_{i2}-x_{min})/s_{\Pi}] \\[2ex] v_i = [(y_{i2}-y_{min})/s_{\Pi}] \\[2ex]$
代码的实现主要在LineDetection3D中的**maskFromPoint()**函数中

bool LineDetection3D::maskFromPoint( std::vector<cv::Point2d> &pts2d, double radius, double &xmin, double &ymin, double &xmax, double &ymax, int &margin, cv::Mat &mask )
{
    
      //pts2d存储着2D plane里所有点的坐标
	xmin=10000000, ymin = 10000000;
	xmax=-xmin;
	ymax=-ymin;
	for (int i=0; i<pts2d.size(); ++i)
	{
    
    
		if(pts2d[i].x < xmin) {
    
     xmin = pts2d[i].x; }
		if(pts2d[i].x > xmax) {
    
     xmax = pts2d[i].x; }

		if(pts2d[i].y < ymin) {
    
     ymin = pts2d[i].y; }
		if(pts2d[i].y > ymax) {
    
     ymax = pts2d[i].y; }
	}

	margin = 4; // 灰度图的边缘，左右各为2个像素，
	int cols = (xmax-xmin) / radius + 2*margin;  //灰度图的列数(因为灰度图的坐标原点在左上角)
	int rows = (ymax-ymin) / radius + 2*margin;  //灰度图的行数
	if ( cols < 10 || rows < 10 )
	{
    
    
		return false;
	}

	mask = cv::Mat(rows, cols, CV_8UC1, cv::Scalar(0));
	for (int i=0; i<pts2d.size(); ++i) //计算每个点在灰度图中的像素坐标(yInt,xInt)
	{
    
    
		int xInt = int((pts2d[i].x-xmin)/radius+0.5+margin); // 计算哪一列
		int yInt = int((pts2d[i].y-ymin)/radius+0.5+margin); //计算哪一行
		mask.at<uchar>(yInt,xInt) = 255;
	}
	return true;
}

2D Line Detection

本篇paper提取二维图像中的直线段，步骤如下：

首先利用open CV中 dilate()和erode() 分别进行膨胀和腐蚀，然后利用findContous() 提取灰度图中所有的轮廓
然后利用作者在论文“CANNYLINES: A PARAMETER-FREE LINE SEGMENT DETECTOR”中将提取到的轮廓分割成直线段
最后用最小二乘法拟合上述分割得到的直线段
上述的算法具体实现在LineDetection3D中的**lineFromMask()**函数中,第二步和第三步的算法实现在LineFunctions类中的 lineFitting() 中。如下：

//输入参数
cv::Mat mask, 每个三维plane转化成的灰度图
int thLineLengthPixel: max(thLineLength/gridSideLength,10.0),其中thLineLength=8*this->scale,gridSideLength:mask的像素大小
std::vector<std::vector<std::vector<cv::Point2d> > > &lines: 存储mask中的灰度图所有的line segment，
如：< <<Point2d1,Point2d2>,<Point2d3,Point4 >,... > ，<<Point2d21,Point2d21>,<Point2d23,Point2d24 > ,...>,... >, 
最内一层vector<Point2d1,Point2d2>里存储着两个Point2d，分别为一条line segment的两端点;
中间一层vector<<Point2d1,Point2d2>,<Point2d3,Point4 >,... > 存储着每条contour的line segment;
最外一层vector<<<>,<>...>,<>...>存储着每个plane的contours

//想得到得就是lines(形参），在原代码中，对应的实参是line2d

planes的lines3d

void LineDetection3D::lineFromMask( cv::Mat &mask, int thLineLengthPixel, std::vector<std::vector<std::vector<cv::Point2d> > > &lines )
{
    
    
	lines.clear();

	// get mask image via dilate and erode
	cv::Mat mask2;
	cv::dilate(mask, mask2, cv::Mat());  //膨胀
	cv::erode(mask2, mask2, cv::Mat());  //腐蚀

	// A. contours
	double thLength = thLineLengthPixel;
	
	std::vector<std::vector<cv::Point> > contours;  
	std::vector<cv::Vec4i> hierarchy;  
	cv::findContours(mask2, contours, hierarchy, CV_RETR_LIST, CV_CHAIN_APPROX_NONE); 

	// B. line fitting from the contours
	for ( int i=0; i<contours.size(); ++i )
	{
    
    
		if ( contours[i].size() < 4*thLength  )
		{
    
    
			continue;
		}

		std::vector<std::vector<cv::Point2d> > lineTemp;
		LineFunctions::lineFitting( mask2.rows, mask2.cols, contours[i], thLength, lineTemp );
		lines.push_back(lineTemp);
	}
}

/************************************************************************/
/*                           Line Functions                             */
/************************************************************************/

void LineFunctions::lineFitting( int rows, int cols, std::vector<cv::Point> &contour, double thMinimalLineLength, std::vector<std::vector<cv::Point2d> > &lines )
{
    
    
	// get straight strings from the contour
	double minDeviation = 6.0;
	std::vector<std::vector<cv::Point> > straightString;
	subDivision(straightString, contour, 0, contour.size()-1, minDeviation, int(thMinimalLineLength));
	if ( !straightString.size() )
	{
    
    
		return;
	}
	for ( int i=0; i<straightString.size(); ++i )
	{
    
    
		if ( straightString[i].size() < thMinimalLineLength )
		{
    
    
			continue;
		}

		std::vector<double> parameters( 4 );
		double maxDev = 0.0;
		//bool isOK = lineFittingLS( straightString[i], parameters, maxDev );
		lineFittingSVD(&straightString[i][0], straightString[i].size(), parameters, maxDev);
		//if ( isOK )
		{
    
    
			double k = parameters[1];
			double b = parameters[2];
			int lineLen = straightString[i].size();

			double xs = 0, ys = 0, xe = 0, ye = 0;
			if ( ! parameters[0] )  // horizontal
			{
    
    
				xs = straightString[i][0].x;
				ys = k * xs + b;
				xe = straightString[i][lineLen-1].x;
				ye = k * xe + b;
			}
			else   // vertical
			{
    
    
				ys = straightString[i][0].y;
				xs = k * ys + b;
				ye = straightString[i][lineLen-1].y;
				xe = k * ye + b;
			}

			if ( !( xs==xe && ys==ye ) )
			{
    
    
				std::vector<cv::Point2d> lineCur(2);
				lineCur[0] = cv::Point2d(xs, ys);
				lineCur[1] = cv::Point2d(xe, ye);

				lines.push_back( lineCur );
			}
		}
	}
}

void LineFunctions::subDivision( std::vector<std::vector<cv::Point> > &straightString, std::vector<cv::Point> &contour, int first_index, int last_index
	, double min_deviation, int min_size )
{
    
    
	int clusters_count = straightString.size();

	cv::Point first = contour[first_index];
	cv::Point last = contour[last_index];

	// Compute the length of the straight line segment defined by the endpoints of the cluster.
	int x = first.x - last.x;
	int y = first.y - last.y;
	double length = sqrt( static_cast<double>( (x * x) + (y * y) ) );

	// Find the pixels with maximum deviation from the line segment in order to subdivide the cluster.
	int max_pixel_index = 0;
	double max_deviation = -1.0;

	for (int i=first_index, count=contour.size(); i!=last_index; i=(i+1)%count)
	{
    
    
		cv::Point current = contour[i];

		double deviation = static_cast<double>( abs( ((current.x - first.x) * (first.y - last.y)) + ((current.y - first.y) * (last.x - first.x)) ) );

		if (deviation > max_deviation)
		{
    
    
			max_pixel_index = i;
			max_deviation = deviation;
		}
	}
	max_deviation /= length;

	// 
	// 	// Compute the ratio between the length of the segment and the maximum deviation.
	// 	float ratio = length / std::max( max_deviation, min_deviation );

	// Test the number of pixels of the sub-clusters.
	int half_min_size=min_size/2;
	if ((max_deviation>=min_deviation) && ((max_pixel_index - first_index + 1) >= half_min_size) && ((last_index - max_pixel_index + 1) >= half_min_size))
	{
    
    
		subDivision( straightString, contour, first_index, max_pixel_index, min_deviation, min_size );
		subDivision( straightString, contour, max_pixel_index, last_index, min_deviation, min_size );
	}
	else
	{
    
    
		// 
		if ( last_index - first_index > min_size )
		{
    
    
			std::vector<cv::Point> straightStringCur;
			for ( int i=first_index; i<last_index; ++i )
			{
    
    
				straightStringCur.push_back(contour[i]);
			}
			straightString.push_back(straightStringCur);
			//terminalIds.push_back(std::pair<int,int>(first_index, last_index));
		}
	}
}

void LineFunctions::lineFittingSVD(cv::Point *points, int length, std::vector<double> &parameters, double &maxDev)
{
    
    
	// 
	cv::Matx21d h_mean( 0, 0 );
	for( int i = 0; i < length; ++i )
	{
    
    
		h_mean += cv::Matx21d( points[i].x, points[i].y );
	}
	h_mean *= ( 1.0 / length );

	cv::Matx22d h_cov( 0, 0, 0, 0 );
	for( int i = 0; i < length; ++i )
	{
    
    
		cv::Matx21d hi = cv::Matx21d( points[i].x, points[i].y );
		h_cov += ( hi - h_mean ) * ( hi - h_mean ).t();
	}
	h_cov *=( 1.0 / length );

	// eigenvector
	cv::Matx22d h_cov_evectors;
	cv::Matx21d h_cov_evals;
	cv::eigen( h_cov, h_cov_evals, h_cov_evectors );

	cv::Matx21d normal = h_cov_evectors.row(1).t();

	// 
	if ( abs(normal.val[0]) < abs(normal.val[1]) )  // horizontal
	{
    
    
		parameters[0] = 0;
		parameters[1] = - normal.val[0] / normal.val[1];
		parameters[2] = h_mean.val[1] - parameters[1] * h_mean.val[0];
	}
	else  // vertical
	{
    
    
		parameters[0] = 1;
		parameters[1] = - normal.val[1] / normal.val[0];
		parameters[2] = h_mean.val[0] - parameters[1] * h_mean.val[1];
	}

	// maximal deviation
	maxDev = 0;
	for( int i = 0; i < length; ++i )
	{
    
    
		cv::Matx21d hi = cv::Matx21d( points[i].x, points[i].y );
		cv::Matx21d v = hi - h_mean;
		double dis2 = v.dot(v);
		double disNormal = v.dot(normal);
		double disOrtho = sqrt(dis2 - disNormal*disNormal);
		if ( disOrtho > maxDev )
		{
    
    
			maxDev = disOrtho;
		}
	}
}

2D-3D Re-projection

灰度图像转化为三维plane的步骤，本篇paper讲的步骤如下：

根据公式(5)的反过程，将二维灰度图像的坐标 $u_i,v_i)$ 转化成2D plane里的坐标 $x_i,y_i)$ 。
根据公式(4)的反过程，将2D Plane的坐标 $x_i,y_i)$ 转化成三维坐标值。

// C. 2D-3D Projection
		planes[i].scale = gridSideLength;   //设置每个面的scale为灰度图的像素大小
		for ( int m=0; m<lines2d.size(); ++m )   //循环点云中的每个patch
		{
    
    
			std::vector<std::vector<cv::Point3d> > temp;
			for (int n=0; n<lines2d[m].size(); ++n)  //循环patch中的每个contour
			{
    
    
				double length = abs(lines2d[m][n][1].x-lines2d[m][n][0].x) + abs(lines2d[m][n][1].y-lines2d[m][n][0].y);  //line segment两个端点的横、纵坐标相减的绝对值相加
				if ( length < thLineLengthPixel )
				{
    
    
					continue;
				}

				lines2d[m][n][0].x = (lines2d[m][n][0].x - margin) * gridSideLength + xmin; //line segment第一个start端点在2D Plane的x坐标
				lines2d[m][n][0].y = (lines2d[m][n][0].y - margin) * gridSideLength + ymin;  //line segment第一个（start）端点在2D Plane的y坐标

				lines2d[m][n][1].x = (lines2d[m][n][1].x - margin) * gridSideLength + xmin;   //line segment第二个（end)端点在2D Plane的x坐标
				lines2d[m][n][1].y = (lines2d[m][n][1].y - margin) * gridSideLength + ymin;   //line segment第二个（end)端点在2D Plane的y坐标

				cv::Mat_<double> xs = lines2d[m][n][0].x * vX;//这个是2D Plane中以Pc为原点的坐标系下的x坐标
				cv::Mat_<double> ys = lines2d[m][n][0].y * vY; 这个是2D Plane中以Pc为原点的坐标系下的y坐标
				cv::Mat_<double> pts = planePt + xs + ys;  //这个是从2D Plane中line segment的start点转化到3D Plane中的点的三维坐标

				cv::Mat_<double> xe = lines2d[m][n][1].x * vX;
				cv::Mat_<double> ye = lines2d[m][n][1].y * vY;
				cv::Mat_<double> pte = planePt + xe + ye;  //这个是从2D Plane中line segment的end点转化到3D Plane中的点的三维坐标

				std::vector<cv::Point3d> line3dTemp(2); //line3dTemp存储3D Plane中contour每个line segment两个端点的三维坐标
				line3dTemp[0] = cv::Point3d(pts(0), pts(1), pts(2));
				line3dTemp[1] = cv::Point3d(pte(0), pte(1), pte(2));

				temp.push_back( line3dTemp );  //temp存储3D Plane中的contour每个line segment的坐标
			}
			if (temp.size())
			{
    
    
				planes[i].lines3d.push_back(temp); //lines3d存储着3D Plane中每个contour的坐标(line3d 是LineDetection3D类中的plane的成员变量)
			} 
		}

算法第三步: 后处理

后处理(post processing) 的过程主要分为两步：（1）Outliers Removal (2)Line Merging

void LineDetection3D::postProcessing( std::vector<PLANE> &planes, std::vector<std::vector<cv::Point3d> > &lines )
{
    
    
	// step1: plane line regularization
	outliersRemoval( planes );

	// step2: line merging
	lineMerging( planes, lines );
}

Outliers removal

（1）remove outlier 3D planes
（2）remove outlier 3D line segments

void LineDetection3D::outliersRemoval( std::vector<PLANE> &planes )
{
    
    
	double thCosAngleIN = cos(12.5/180.0*CV_PI);
	double thCosAngleNEW = cos(30.0/180.0*CV_PI);
	double thNonStructPlaneRatio = 0.3;
	double thAngle = 12.5;
	double thCosAngleParal = cos(thAngle/180.0*CV_PI);
	double thCosAngleOrtho = cos((90.0-thAngle)/180.0*CV_PI);
	double thNonStructLineRatio = 10;
	double thStructPlane = 60*this->scale;

	std::vector<int> isPlaneGood(planes.size(), 0);
#pragma omp parallel for
	for (int i=0; i<planes.size(); ++i)
	{
    
    
		if (!planes[i].lines3d.size())
		{
    
    
			continue;
		}

		// step1: remove non-structural planes
		std::vector<double> lengthsAll;
		std::vector<cv::Mat> orientsAll;
		std::vector<std::pair<int, double> > lineInfos;
		std::vector<std::vector<double> > lengths(planes[i].lines3d.size());
		std::vector<std::vector<cv::Mat> > orients(planes[i].lines3d.size());

		double totalLength = 0.0;
		int count = 0;
		for (int m=0; m<planes[i].lines3d.size(); ++m)
		{
    
    
			lengths[m].resize(planes[i].lines3d[m].size());
			orients[m].resize(planes[i].lines3d[m].size());
			for (int n=0; n<planes[i].lines3d[m].size(); ++n)
			{
    
    
				cv::Mat orientTemp = cv::Mat(planes[i].lines3d[m][n][1] - planes[i].lines3d[m][n][0]);
				double lengthTemp = cv::norm(orientTemp);
				lengthsAll.push_back(lengthTemp);
				lengths[m][n] = lengthTemp;

				orientTemp *= 1.0/lengthTemp;
				orientsAll.push_back(orientTemp);
				orients[m][n] = orientTemp;

				std::pair<int, double> lineInfoTemp(count, lengthTemp);
				lineInfos.push_back(lineInfoTemp);

				totalLength += lengthTemp;
				count ++;
			}
		}
		std::sort( lineInfos.begin(), lineInfos.end(), [](const std::pair<int, double>& lhs, const std::pair<int, double>& rhs) {
    
     return lhs.second > rhs.second; } );

		std::vector<cv::Mat> clusterOrient;
		std::vector<std::pair<int, double> > clusterInfos;
		for (int j=0; j<lineInfos.size(); ++j)
		{
    
    
			int id = lineInfos[j].first;
			double length = lineInfos[j].second;

			if (!clusterInfos.size())
			{
    
    
				clusterInfos.push_back(std::pair<int, double>(clusterInfos.size(), length));
				clusterOrient.push_back(orientsAll[id]);
				continue;
			}

			bool isIn = false;
			double cosValueMin = 100;
			for (int m=0; m<clusterInfos.size(); ++m)
			{
    
    
				double cosValue = abs(orientsAll[id].dot(clusterOrient[m]));
				if ( cosValue < cosValueMin )
				{
    
    
					cosValueMin =  cosValue;
				}
				if (cosValue > thCosAngleIN)
				{
    
    
					clusterInfos[m].second += length;
					isIn = true;
					break;
				}
			}

			if (!isIn && cosValueMin < thCosAngleNEW)
			{
    
    
				clusterInfos.push_back(std::pair<int, double>(clusterInfos.size(), length));
				clusterOrient.push_back(orientsAll[id]);
				continue;
			}
		}

		double scaleCur = max(this->scale,planes[i].scale);
		if ( clusterInfos.size() > 1)
		{
    
    
			double LStruct =  clusterInfos[0].second + clusterInfos[1].second;
			if( LStruct < thNonStructPlaneRatio*totalLength || LStruct < thStructPlane ) 
			{
    
    
				continue;
			}
		}

		// step2: remove non-structural lines
		PLANE planeNew;
		planeNew.scale = planes[i].scale;
		//double scaleCur = planes[i].scale;
		double thNonStructLineLength = scaleCur*thNonStructLineRatio;
		for (int m=0; m<planes[i].lines3d.size(); ++m)
		{
    
    
			int numLines = planes[i].lines3d[m].size();

			double lengthTotal = 0.0;
			for (int n=0; n<numLines; ++n)
			{
    
    
				lengthTotal += lengths[m][n];
			}

			double ratioStruct = 0.0;
			double lengthStruct = 0.0;
			std::vector<int> isStruct(numLines, 0);
			if (numLines > 1)
			{
    
    
				// judge if the contour is structural
				std::vector<int> idxOrthoPara;
				for (int n=0; n<numLines-1; ++n)
				{
    
    
					int id1 = n;
					int id2 = (n+1)%numLines;

					double cosAngle = abs(orients[m][id1].dot(orients[m][id2]));
					if (cosAngle > thCosAngleParal || cosAngle < thCosAngleOrtho)
					{
    
    
						idxOrthoPara.push_back(id1);
						idxOrthoPara.push_back(id2);
					}
				}

				if (idxOrthoPara.size())
				{
    
    
					// structural ratio
					std::sort( idxOrthoPara.begin(), idxOrthoPara.end(), [](const int& lhs, const int& rhs) {
    
     return lhs > rhs; } );

					int idTemp = idxOrthoPara[0];
					isStruct[idTemp] = 1;
					lengthStruct = lengths[m][idTemp];
					for (int n=0; n<idxOrthoPara.size(); ++n)
					{
    
    
						if (idxOrthoPara[n] != idTemp)
						{
    
    
							lengthStruct += lengths[m][idxOrthoPara[n]];
							idTemp = idxOrthoPara[n];
							isStruct[idTemp] = 1;
						}
					}

					ratioStruct = lengthStruct/lengthTotal;
				}
			}

			std::vector<std::vector<cv::Point3d> > contourTemp;
			for (int n=0; n<numLines; ++n)
			{
    
    
				double thLengthTemp = 0.0;
				if (isStruct[n])
				{
    
    
					if(ratioStruct>=0.75) 
					{
    
    
						thLengthTemp = thNonStructLineLength;
					}
					else if (ratioStruct>=0.5) 
					{
    
    
						thLengthTemp = 2*thNonStructLineLength;
					}
					else 
					{
    
    
						thLengthTemp = 4*thNonStructLineLength;
					}
				}
				else
				{
    
    
					thLengthTemp = 4*thNonStructLineLength;
				}

				if (lengths[m][n] > thLengthTemp)
				{
    
    
					contourTemp.push_back(planes[i].lines3d[m][n]);
				}
			}
			if (contourTemp.size())
			{
    
    
				planeNew.lines3d.push_back(contourTemp);
			}
		}

		if (planeNew.lines3d.size())
		{
    
    
			planes[i] = planeNew;
			isPlaneGood[i] = 1;
		}
	}

	//
	std::vector<PLANE> planesNew;
	for (int i=0; i<isPlaneGood.size(); ++i)
	{
    
    
		if (isPlaneGood[i])
		{
    
    
			planesNew.push_back(planes[i]);
		}
	}
	planes = planesNew;
}

line Merging

主要合并相近和相交的line segment,该篇paper中这个主要分为两部分：

识别每个线段与之相近的所有线段，这些相近的线段即为合并候选的线段
1.1 首先计算每个线段的latitude，即为 $a s i n (z)$ , 计算公式如下:
$asin(\frac {|z_2-z_1|} {\sqrt{(x_2-x_1)^2+(y_2-y_1)^2+(z_2-z_1)^2}})$
其中， $x_1,y_1,z_1) (x_2,y_2,z_2)$ 为线段的两个端点的坐标
1.2 以6度为一个单位，将90度划分为15个区间。每个线段根据 $a s i n (z)$ 值，分别被划分到对应的区间中，对每个区间的线段按照它们的长度进行降序排列。
1.3 遍历每一条直线 $L_i$ ，判断这条线段的右区间中每一条线段 $L_j$ ，如果满足下面公式，则认为 $L_j$ 为 $L_i$ 的候选合并线段。
$|d_{L_i} - d_{L_j} | / mag < 0.1 \\[2ex] d_{L_i} = cv::norm(v.cross(ptmid)) \\[1ex] v = lines[i][1] - lines[i][0] \\[1ex] ptmid = (lines[i][1] + lines[i][0]) * 0.5 \\[1ex] lines[i][0] 和 lines[i][1]为L_i的两个端点 \\[2ex] mag = \sqrt {x_1^2+y_1^2+ z_1^2} \\[1ex] (x_1,y_1,z_1)为整个点云中第一个点的坐标$
根据第一步得出得每个直线 $L_i$ 的所有候选合并线段，然后有以下两步
2.1 遍历所有候选合并线段 $L_j$ , 如果 $L_j$ 的两个端点到 $L_i$ 的垂直距离任何一个大于 $4S_{\pi}$ ,则从候选合并线段丢弃 $L_j$
2.2 如果满足上述条件，则更新 $L_i$ 的端点。将 $L_j$ 的两个端点投影到 $L_i$ 中，再加上 $L_i$ 的两个端点，一共有四个点。则 $L_i$ 的两个新端点为这四个点最远的两个。同时标记 $L_j$ 为已合并线段，不需要再合并。

void LineDetection3D::lineMerging( std::vector<PLANE> &planes, std::vector<std::vector<cv::Point3d> > &lines )
{
    
    
	double thGapRatio = 20;
	double thMergeRatio = 6;
	double thDisHyps = 0.1;

	// get all the lines
	std::vector<double> lineScales;
	for (int i=0; i<planes.size(); ++i)
	{
    
    
		for (int m=0; m<planes[i].lines3d.size(); ++m)
		{
    
    
			for (int n=0; n<planes[i].lines3d[m].size(); ++n)
			{
    
    
				lines.push_back(planes[i].lines3d[m][n]);
				lineScales.push_back(planes[i].scale);
			}
		}
	}

	// get the parameters of each 3d line
	std::vector<std::vector<double> > lineParas(lines.size()) ;
	std::vector<std::pair<int, double> > lineInfos(lines.size());
	for ( int i=0; i<lines.size(); ++i )
	{
    
    
		cv::Mat v(lines[i][1]-lines[i][0]);
		double length = cv::norm(v);
		v *= 1.0/length;

		cv::Mat ptmid((lines[i][1]+lines[i][0])*0.5);
		cv::Mat d = v.cross(ptmid)*(1.0/this->magnitd);

		// get the latitude of the line, longitude is not stable
		double latitude = asin(abs(v.at<double>(2)));

		// the length of the line
		lineParas[i].resize(6);
		lineParas[i][0] = v.at<double>(0);       lineParas[i][1] = v.at<double>(1);       lineParas[i][2] = v.at<double>(2);
		lineParas[i][3] = latitude;   
		lineParas[i][4] = cv::norm(d); 
		lineParas[i][5] = length; 

		lineInfos[i] = std::pair<int,double>(i, length);
	}
	std::sort( lineInfos.begin(), lineInfos.end(), [](const std::pair<int,double>& lhs, const std::pair<int,double>& rhs) {
    
     return lhs.second > rhs.second; } );

	// build grid with latitude
	double precision = 6.0/180.0*CV_PI;
	int laSize = CV_PI/2.0/precision;
	std::vector<std::vector<int > > grid(laSize);
	std::vector<int> gridIndex(lineParas.size());
	for ( int i=0; i<lineParas.size(); ++i )
	{
    
    
		int la = lineParas[i][3]/precision;
		grid[la].push_back(i);
		gridIndex[i] = la;
	}

	// line merging
	std::vector<bool> isUsed(lines.size(), 0);
	std::vector<std::vector<cv::Point3d> > linesNew;
	for ( int i=0; i<lineInfos.size(); ++i )
	{
    
    
		int id0 = lineInfos[i].first;
		if ( isUsed[id0] )
		{
    
    
			continue;
		}
		isUsed[id0] = 1;

		double lineScale = max(lineScales[id0], this->scale);
		double vx0 = lineParas[id0][0], vy0 = lineParas[id0][1], vz0 = lineParas[id0][2];
		double d0 = lineParas[id0][4], length0 = lineParas[id0][5];
		cv::Point3d pts0 = lines[id0][0], pte0 = lines[id0][1];

		// get the merging hypotheses
		std::vector<int> idHyps;
		for (int j=-1; j<=1; ++j)
		{
    
    
			int latemp = gridIndex[id0]+j;
			int la = (latemp+laSize)%laSize;
			for ( int m=0; m<grid[la].size(); ++m )
			{
    
    
				int idTemp = grid[la][m];
				if (abs(lineParas[idTemp][4]-d0) < thDisHyps)
				{
    
    
					idHyps.push_back(idTemp);
				}
			}
		}

		// try merging
		for (int j=0; j<idHyps.size(); ++j)
		{
    
    
			int id1 = idHyps[j];
			if ( isUsed[id1] )
			{
    
    
				continue;
			}

			cv::Point3d pts1 = lines[id1][0], pte1 = lines[id1][1];
			double length1 = lineParas[id1][5];

			// judge the distance between two line
			cv::Point3d v1 = pts0 - pts1;
			double disNormal1 = v1.x*vx0 + v1.y*vy0 + v1.z*vz0;
			cv::Point3d vOrtho1 = v1 - disNormal1*cv::Point3d(vx0, vy0, vz0);
			double disOrtho1 = sqrt(vOrtho1.x*vOrtho1.x + vOrtho1.y*vOrtho1.y + vOrtho1.z*vOrtho1.z);

			cv::Point3d v2 = pts0 - pte1;
			double disNormal2 = v2.x*vx0 + v2.y*vy0 + v2.z*vz0;
			cv::Point3d vOrtho2 = v2 - disNormal2*cv::Point3d(vx0, vy0, vz0);
			double disOrtho2 = sqrt(vOrtho2.x*vOrtho2.x + vOrtho2.y*vOrtho2.y + vOrtho2.z*vOrtho2.z);

			if ( disOrtho1 > thMergeRatio*lineScale || disOrtho2 > thMergeRatio*lineScale )
			{
    
    
				continue;
			}

			// judge the overlapping ratio of two line
			cv::Point3d d1 = pts0 - pts1, d2 = pts0 - pte1, d3 = pte0 - pts1, d4 = pte0 - pte1;
			double dis1 = sqrt(d1.x*d1.x + d1.y*d1.y + d1.z*d1.z);
			double dis2 = sqrt(d2.x*d2.x + d2.y*d2.y + d2.z*d2.z);
			double dis3 = sqrt(d3.x*d3.x + d3.y*d3.y + d3.z*d3.z);
			double dis4 = sqrt(d4.x*d4.x + d4.y*d4.y + d4.z*d4.z);
			double disMerge = max( max(dis1, dis2), max(dis3, dis4) );

			double gapLength = disMerge - length0 - length1;
			double gapRatio = gapLength / length0;
			if ( gapRatio < 0.1 && gapLength < thGapRatio*lineScale )
			{
    
    
				// update line id0
				if (gapRatio > 0)
				{
    
    
					if (dis1 == disMerge)
					{
    
    
						double disNormal = d1.x*vx0 + d1.y*vy0 + d1.z*vz0;
						lines[id0][1] = pts0 - disNormal*cv::Point3d(vx0, vy0, vz0);
					}
					else if (dis2 == disMerge)
					{
    
    
						double disNormal = d2.x*vx0 + d2.y*vy0 + d2.z*vz0;
						lines[id0][1] = pts0 - disNormal*cv::Point3d(vx0, vy0, vz0);
					}
					else if (dis3 == disMerge)
					{
    
    
						double disNormal = d3.x*vx0 + d3.y*vy0 + d3.z*vz0;
						lines[id0][0] = pte0 - disNormal*cv::Point3d(vx0, vy0, vz0);
					}
					else
					{
    
    
						double disNormal = d4.x*vx0 + d4.y*vy0 + d4.z*vz0;
						lines[id0][0] = pte0 - disNormal*cv::Point3d(vx0, vy0, vz0);
					}
				}

				isUsed[id1] = 1;
			}
		}

		linesNew.push_back(lines[id0]);
	}

	lines = linesNew;
}

“Fast 3D Line Segment Detection From Unorganized Point Cloud“ ---深度解读