2. 程式人生 > >基於改進形態學濾波的點雲分類演算法------續

基於改進形態學濾波的點雲分類演算法------續

阿新 • • 發佈：2019-01-25

           在前一篇部落格中《基於改進形態學濾波器的點雲分類演算法》中，介紹了論文中利用改進的形態學方法對點雲進行分類的步驟，這幾天也將該演算法在C++中進行了實現，所以今天將實現過程寫在本文中。

    實現過程：

（1）讀入檔案

  檔案的讀入分為兩類，分別是ASC型別的PCD和已二進位制格式儲存的PCD；ASC格式PCD的檔案讀入如下所示，關鍵是讀到其中儲存的POINTS 引數，該引數表示該檔案中共有多少個點。讀取的結果存入一個vector中並返回。

vector<MyPointXYZ> FileHelper::readAscPcd(const char* pcdFile)
{
	vector<MyPointXYZ> tempPointsVector;//存放結果的vector
	int pointsNum = 0;

	//將 char* 轉換為 wchar_t*，因為_wfopen_s方法不接受char* 型別的引數
	size_t len = strlen(pcdFile) + 1;
	size_t converted = 0;
	wchar_t *WStr;
	WStr = (wchar_t*)malloc(len*sizeof(wchar_t));
	mbstowcs_s(&converted, WStr, len, pcdFile, _TRUNCATE);

	FILE *fp = NULL;
	_wfopen_s(&fp,WStr, L"r+");

	char buf[50] = { 0 };
	for (int i = 0; i < 9; i++)
	{
		fgets(buf, 50, fp);
	}
	fgets(buf,7,fp);//讀取POINTS
	fscanf_s(fp,"%d",&pointsNum);//讀取點個數到變數
	fgets(buf,50,fp);
	fgets(buf,50,fp);
	MyPointXYZ tempPoint;
	for (int i = 0; i < pointsNum; i++)
	{
		fscanf_s(fp, "%f", &tempPoint.x);
		fscanf_s(fp, "%f", &tempPoint.y);
		fscanf_s(fp, "%f", &tempPoint.z);
		tempPoint.initialed = false;

		tempPointsVector.push_back(tempPoint);
	}

	fclose(fp);

	return tempPointsVector;
}
如果需要讀入的檔案問已二進位制儲存的PCD檔案，則需要利用到PCL中自帶的讀取方法，再經過一次轉換，將點雲資料仍然存放中一個vector中。 
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
reader.read<pcl::PointXYZ>("samp11-utm.pcd", *cloud);

//以下方法將cloud中的points存放到一個vector中，方便後續處理
vector<MyPointXYZ> FileHelper::getVectorFromPCL(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
	int pointSize = cloud->points.size();
	vector<MyPointXYZ> pointsVector;
	MyPointXYZ tempPoint;
	for (int i = 0; i < pointSize; i++)
	{
		tempPoint.x = cloud->points[i].x;
		tempPoint.y = cloud->points[i].y;
		tempPoint.z = cloud->points[i].z;
		tempPoint.initialed = false;
		pointsVector.push_back(tempPoint);
	}

	return pointsVector;
}

（2）引數設定

在利用改進的形態學濾波器進行點雲分類時，涉及到很多引數，引數如下所示，具體含義請參見論文。

	//對myFilter進行引數設定
	myFilter.maxWindowSize = 30;
	myFilter.cellSize =1.0f;
	myFilter.initialDistence = 0.5f;
	myFilter.maxDistence = 3.0f;
	myFilter.base = 2.0f;
	myFilter.slop = 1.0f;
	myFilter.exponential = false;
	myFilter.openByRow = false;

（3）計算二維陣列行列值

根據輸入點雲的最大和最小X，Y值以及之前設定的cellSize引數，確定一個二維陣列的行列值分別是多少。

void MyMorphologicalFilter::calcuRowCol(vector<MyPointXYZ> pointsVector)
{
	int pointsSize = pointsVector.size();
	minX = pointsVector[0].x;
	minY = pointsVector[0].y;
	maxX = pointsVector[0].x;
	maxY = pointsVector[0].y;
	for (int i = 0; i < pointsSize; i++)
	{
		float tempX = pointsVector[i].x;
		float tempY = pointsVector[i].y;
		if (minX > tempX)
			minX = tempX;
		if (minY > tempY)
			minY = tempY;
		if (maxX < tempX)
			maxX = tempX;
		if (maxY < tempY)
			maxY = tempY;
	}

	//計算二維陣列的行列值
	row = floor((maxY - minY) / cellSize) + 1;
	col = floor((maxX - minX) / cellSize) + 1;
}

（4）將點雲存入二維陣列

這是比較關鍵的一步，因為在這一步中，進行了從三維離散點雲到二維規則網格的對映。對映的規則如一下程式碼所示，根據每一個點的x y計算該點落入的格網座標，如果格網內無點落入，則直接放進去；如果該座標內已經有點存在，則把該點的z值與格網內的點z值進行比較，存放z較小的點。迴圈直到遍歷完所有點

void MyMorphologicalFilter::putVectorPoints2Arr(vector<MyPointXYZ> pointsVector, MyPointXYZ **pointsArr)
{
	//初始化**arrB_originalPoints
	//每個元素賦初值-1
	arrB_originalPoints = new int*[row];
	for (int i = 0; i != row; i++)
	{
		arrB_originalPoints[i] = new int[col];
	}
	for (int i = 0; i < row; i++)
	{
		for (int j = 0; j < col; j++)
			arrB_originalPoints[i][j] = -1;
	}

	MyPointXYZ tempPoint;
	for (int i = 0; i < pointsVector.size(); i++)
	{
		tempPoint.x = pointsVector[i].x;
		tempPoint.y = pointsVector[i].y;
		tempPoint.z = pointsVector[i].z;
		int m = floor((pointsVector[i].y - minY) / cellSize);
		int n = floor((pointsVector[i].x - minX) / cellSize);
		if (pointsArr[m][n].initialed)//該陣列元素已被賦值
		{
			if (pointsArr[m][n].z > tempPoint.z)//當多個點落入同一個元素時，取z最小值的點。
			{                                   //但前一個點被覆蓋
				tempPoint.initialed = true;
				pointsArr[m][n] = tempPoint;
				objectsIndexes.push_back(arrB_originalPoints[m][n]);//取出當前pointsArr[m][n]
																	//中存放的點，找到對應索引，加入地物點索引中
				arrB_originalPoints[m][n] = i;
			}
			//如果落入同一個元素中的Z大於之前的Z，表面該點為地物點，記錄下該索引
			else
			{
				objectsIndexes.push_back(i);
			}
		}
		else//該陣列元素未賦值，直接賦值
		{
			tempPoint.initialed = true;
			pointsArr[m][n] = tempPoint;
			arrB_originalPoints[m][n] = i;
		}

	}
}

（5）計算一系列視窗和閾值

這也是關鍵的一步，涉及到濾波視窗和比較閾值的選擇，每一個視窗都對應一個閾值。計算方法有兩類，一類是指數遞增，一類是線性遞增，指數方式所得視窗個數更少，計算速度更快；線性方式所得視窗更多，所以後續的計算時間更長。

int MyMorphologicalFilter:: getWinSizes()
{
	// Compute the series of window sizes and height thresholds

	int iteration = 0;
	float window_size = 0.0f;
	float height_threshold = 0.0f;

	while (window_size <maxWindowSize)
	{
		// Determine the initial window size.
		if (exponential)
			window_size = cellSize * (2.0f * std::pow(base, iteration) + 1.0f);
		else
			window_size = cellSize * (2.0f * (iteration + 1) * base + 1.0f);

		// Calculate the height threshold to be used in the next iteration.
		if (iteration == 0)
			height_threshold = initialDistence;
		else
			height_threshold = slop * (window_size - window_sizes[iteration - 1]) * cellSize + initialDistence;

		// Enforce max distance on height threshold
		if (height_threshold > maxDistence)
		{
			height_threshold = maxDistence;
		}

		window_sizes.push_back(window_size);
		height_thresholds.push_back(height_threshold);

		iteration++;
	}
	return iteration;
}

（6）對陣列中的空白元素進行插值

經過第四步之後，二維陣列中已經存放了z值較低的點。但是仍然有很多陣列元素未被賦值，為空。因此利用插值方法，填補這些點。此處採用了最簡單的插值方法，取一為空元素周圍8個鄰居然後去均值，就是該點的高程值；至於x y 值都賦為0，以和原始資料中的點進行區分。

插值完成之後，將陣列pointsArr賦值給另一個數組pointsArrB，因為後續的計算基於pointsArr，會改變其中的元素值，利用pointsArrB儲存原始的資訊，最後的地面點也是從其中取出。

//對陣列進行插值，插值方法為取某元素相鄰8個元素的均值
void MyMorphologicalFilter::interpolateArr(MyPointXYZ **pointsArr,int row, int col)
{
	int nullElementOfArr = 0;//採用的插值方法仍未被插值的元素個數
	int interNum = 0;//成功插值元素個數
	int iniNum = 0;
	float sumZ = 0;
	for (int i=0; i < row; i++)
	{
		for (int j = 0; j < col; j++)
		{
			if (!pointsArr[i][j].initialed)//如果該元素為空，使用最近鄰插值
			{
				int maxN = Min(j + 1, col - 1);
				int maxM = Min(i+1,row-1);
				for (int m = Max(i - 1, 0); m <= maxM; m++)
				{
					for (int n = Max(j - 1, 0); n <= maxN; n++)
					{
						if (pointsArr[m][n].initialed)
						{
							iniNum++;
							sumZ += pointsArr[m][n].z;
						}
					}
				}
				if (iniNum)
				{
					pointsArr[i][j].z = sumZ / iniNum;
					pointsArr[i][j].initialed = true;
					pointsArr[i][j].x = 0;
					pointsArr[i][j].y = 0;
					iniNum = 0;
					sumZ = 0;
					interNum++;
				}
				else{//插值失敗
					nullElementOfArr++;
					iniNum = 0;
					sumZ = 0;
				}
			}
		}
	}
	int notNullElements = row*col - nullElementOfArr;
	pointsArrB = new MyPointXYZ*[row];
	for (int i = 0; i != row; i++)
	{
		pointsArrB[i] = new MyPointXYZ[col];
	}
	//將陣列複製到B
	for (int i = 0; i < row; i++)
	{
		for (int j = 0; j < col; j++)
		{
			pointsArrB[i][j] = pointsArr[i][j];
		}
	}
	//pointsArrB = pointsArr;//將陣列賦值給B

	iniFlagArr();//初始化標識陣列flag

}

（7）對陣列中的元素進行形態學“開”運算

對陣列中的元素先進行“腐蝕”，再“膨脹”，構成“開”運算，運算後的結果z值與原始z值進行差運算，再與閾值進行比較，如果小於閾值，表明該點為地面點，相反為地物點。

		int erosionGood = 0;
		for (int win_size_loop = 0; win_size_loop < window_sizes.size(); win_size_loop++)
		{
			for (int row_loop = 0; row_loop < row; row_loop++)
			{
				MyPointXYZ* Pi = pointsArr[row_loop];
				MyPointXYZ* Z = Pi;
				MyPointXYZ* Zf = new MyPointXYZ[col];
				Zf = erosion(Z, col, window_sizes[win_size_loop]);//腐蝕運算
				Zf = dilation(Zf, col, window_sizes[win_size_loop]);//膨脹運算
				Pi = Zf;
				pointsArr[row_loop] = Pi;
				for (int col_loop = 0; col_loop < col; col_loop++)
				{
					float tempZkZ = Z[col_loop].z;
					float tempZfZ = Zf[col_loop].z;
					if (Z[col_loop].z - Zf[col_loop].z > height_thresholds[win_size_loop])//原始高程-開運算後的高程>高度閾值
					{
						flag[row_loop][col_loop] = window_sizes[win_size_loop];//首先要進行初始化
						biggerThanThre++;
					}

				}
			}
		}

<pre name="code" class="cpp">
//腐蝕
MyPointXYZ* MyMorphologicalFilter::erosion(MyPointXYZ* Z,int n, float win_Size)
{
	MyPointXYZ* tempZ = new MyPointXYZ[n];
	for (int i = 0; i < n; i++)
	{
		tempZ[i] = Z[i];
	}
	int half_win = floor(win_Size / 2);
	for (int i = 0; i < n; i++)
	{
		float tempMinZ = Z[i].z;
		for (int j = (i - half_win)>0?(i-half_win):0 ; j <( (i + half_win+1)>n?n:(i+half_win+1)); j++)
		{
			//tempMinZ = tempMinZ>Z[j].z ? Z[j].z : tempMinZ;
			float tempZ_z = Z[j].z;
			if (tempMinZ > tempZ_z)
				tempMinZ = tempZ_z;
		}
		tempZ[i].z = tempMinZ;
	}
	return tempZ;
}

//膨脹
MyPointXYZ* MyMorphologicalFilter::dilation(MyPointXYZ* Z,int n, float win_Size)
{
	MyPointXYZ* tempZ = new MyPointXYZ[n];
	for (int i = 0; i < n; i++)
	{
		tempZ[i] = Z[i];
	}
	int half_win = floor(win_Size / 2);
	for (int i = 0; i < n; i++)
	{
		float tempMaxZ = tempZ[i].z;
		for (int j = (i - half_win)>0 ? (i - half_win) : 0; j <((i + half_win+1)>n ? n : (i + half_win+1)); j++)
		{
			tempMaxZ = tempMaxZ>tempZ[j].z ? tempMaxZ :tempZ[j].z;
		}
		Z[i].z = tempMaxZ;
	}

	return Z;
}
此處的“腐蝕”和“膨脹”選取的都是一維的結構元素，考慮改進的話可以嘗試二維結構元素或者三維結構元素，但是必須指出，維數越高，計算複雜度也越高。

（8）確定地面點和地物點的索引

利用pointsArrB以及標識陣列flag ，確定地面點，並存入一個vector中返回，同時，取出poitnsArrB中的地物點索引

std::vector<MyPointXYZ> MyMorphologicalFilter::getGroundPoints(int row, int col)
{
	std::vector<MyPointXYZ> tempGroundPoints;
	int originalPoints = 0;
	for (int i = 0; i < row; i++)
	{
		for (int j = 0; j < col; j++)
		{
			float tempX = pointsArrB[i][j].x;
			float tempY = pointsArrB[i][j].y;
			if (pointsArrB[i][j].x!=0 && pointsArrB[i][j].y !=0)//該元素為原始資料，非插值生成
			{
				originalPoints++;
				if (flag[i][j] == 0.0f)//該點為地面點,
				{                     //將改點存入地面點陣列中，並存儲該點對應索引
					tempGroundPoints.push_back(pointsArrB[i][j]);
					groundsIndexes.push_back(arrB_originalPoints[i][j]);
				}
				else{//該點為地物點，記錄下該索引
					objectsIndexes.push_back(arrB_originalPoints[i][j]);//objectsIndexes中存放了地物點的索引
				}
			}
		}
	}

	return tempGroundPoints;
}


（9）將地面點寫入檔案

地面點寫入檔案時需要兩個引數，檔名以及存放地面點的vector。寫入時先寫入標準PCD標頭檔案格式

void MyMorphologicalFilter::writePoints2AscPcd(const char* fileName, std::vector<MyPointXYZ> points)
{
	std::ofstream ofile;
	ofile.open(fileName);
	ofile << "# .PCD v0.7 - Point Cloud Data file format" << endl;
	ofile << "VERSION 0.7" << endl;
	ofile << "FIELDS x y z" << endl;
	ofile << "SIZE 4 4 4" << endl;
	ofile << "TYPE F F F" << endl;
	ofile << "COUNT 1 1 1" << endl;
	ofile << "WIDTH " <<points.size()<< endl;
	ofile << "HEIGHT 1" << endl;
	ofile << "VIEWPOINT 0 0 0 1 0 0 0" << endl;
	ofile <<"POINTS "<<points.size()<< std::endl;
	ofile << "DATA ascii" << endl;
	for (int i = 0; i < points.size(); i++)
	{
		float tempX = points[i].x;
		float tempY = points[i].y;
		float tempZ = points[i].z;
		char strX[50];
		_gcvt_s(strX,50,tempX,20);
		char strY[50];
		_gcvt_s(strY,50,tempY,20);
		char strZ[50];
		_gcvt_s(strZ,50,tempZ,10);
		ofile <<strX<<" "<<strY<<" "<<strZ<< std::endl;
	}

	ofile.close();
}

（10）利用地物點索引取出地物點 
//根據原始點雲和點雲索引查詢索引對應點並返回
std::vector<MyPointXYZ> MyMorphologicalFilter::getPointsByIndex(std::vector<MyPointXYZ> oriPoints, std::vector<int> pointsIndexs_)
{
	vector<MyPointXYZ> pointsVector;
	for (int index = 0; index < pointsIndexs_.size(); index++)
	{
		int indexValue = pointsIndexs_[index];
		pointsVector.push_back(oriPoints[indexValue]);
	}

	return pointsVector;
}

（11）將地物點寫入檔案

地物點寫入檔案的方法同寫入地面點相同。

相關推薦

基於改進形態學濾波的點雲分類演算法------續

           在前一篇部落格中《基於改進形態學濾波器的點雲分類演算法》中，介紹了論文中利用改進的形態學方法對點雲進行分類的步驟，這幾天也將該演算法在C++中進行了實現，所以今天將實現過程寫在本

基於深度學習的三維點雲分類的介紹

轉自博主 particle，該文章翻譯自http://www.itzikbs.com/3d-point-cloud-classification-using-deep-learning    在過去的這些年裡，對二維影象已經有了大量深入的研究，並且有著長足的發

基於深度學習的三維點雲分類和分割(找了幾篇文章)

https://blog.csdn.net/bflong/article/details/79655200http://www.sohu.com/a/162727821_715754https://www.cnblogs.com/li-yao7758258/p/8182846

Pytorch實現PointNet中的點雲分類網路。

下面是PointNet論文中分類模型的結構： 但是對於模型的細節，PointNet論文中並沒有詳細的解釋，尤其是T-Net，可以參考PointNet的supplemental部分。如果找不到，可以留言找我要。 話不多說，下面是程式碼，基本上完全還原了論文中的PointNet分類模型

基於ML-KNN的多標籤分類演算法

     最近有一個專案需要用多標籤分類思想來建模，之前對這塊不是太瞭解，查了一些論文，發現目前主流的演算法包括ML-KNN、ML-DT、Rank-SVM、CML等，其中ML-KNN演算法思想最簡單，結合原始論文，本文大概介紹下演算法思想和程式碼實現。      ML-KNN

基於QGLViewer實現的點雲顯示、框選（例項）

生活中處處面臨著狗血的選擇，如果你是個很吸引人的青年，你會發現身邊有很多選擇，但做出選擇往往是很難的。         而且大家有時候只能看,不能選，因為不會選，選錯了你就完蛋了。         如果這時候你會選了，選對了，就不用一個人過節了。         好，那麼今天

PCL庫學習（3）----基於平面模型的點雲分割（地面點雲分割）（Plane Model Segmentation）

執行環境： VS2013，PCL1.8.0 程式碼參考：        最近做的專案需要對採集到的點雲場景進行去除地面的操作。在參考了CSDN博主：有夢想的田園犬對於PCL官方几種例程中的點雲分割方法的實驗後，考慮到系統的實時性要求，選擇基於平面模型的地面點雲去噪方

雙邊濾波演算法在點雲資料處理時的應用

雙邊濾波演算法在點雲資料處理時的應用 簡介（摘自百科） 雙邊濾波（Bilateral filter）是一種非線性的濾波方法，是結合影象的空間鄰近度和畫素值相似度的一種折中處理，同時考慮空域資訊和灰度相似性，達到保邊去噪的目的。具有簡單、非迭代、區域性的特點 。雙邊濾波器的好處是可以做

[PCL]點雲漸進形態學濾波

PCL支援點雲的形態學濾波，四種操作：侵蝕、膨脹、開（先侵蝕後膨脹）、閉（先膨脹後侵蝕） 關於漸進的策略，在初始cell_size_ 的基礎上逐漸變大。滿足如下公式： $$window\_size =cell\_size *(2*base^{k}+1)$$ $$window\_size =cell\_s

點雲分割之基於邊界的分割演算法（一）

基於邊界的點雲分割演算法通過檢測區域邊界得到分割塊。主要演算法有： 一、通過點雲強度的劇烈變化得到點雲邊界； 二、計算邊界梯度，計算點雲表面的法矢方向梯度變化以及3D線匹配； 三、對於距離影象的掃描線分割演算法，但不適用於密度不均勻的點雲資料。 四、通過

基於PCL的三維重建——點雲的濾波處理

在獲取點雲資料時 ，由於裝置精度，操作者經驗環境因素帶來的影響，以及電磁波的衍射特性，被測物體表面性質變化和資料拼接配準操作過程的影響，點雲資料中講不可避免的出現一些噪聲。在點雲處理流程中濾波處理作為預處理的第一步，對後續的影響比較大，只有在濾波預處理中將噪聲點 ，離群點，

基於八叉樹的區域增長點雲分割演算法

該篇文章出自2015年ICIP會議，主要介紹影象分割領域一種新的區域生長演算法  [TOC] 參考：https://blog.csdn.net/cjx2lxj/article/details/50529618  提出的問題 鐳射雷達探測到城市環境的物體表面構成三維幾何

資料探勘——基於sklearn包的分類演算法小結

　　         目錄 一、分類演算法簡介 二、KNN演算法 三、貝葉斯分類演算法 四、決策樹演算法 五、隨機森林演算法 六、SVM演算法      一、分類演算法簡介 1、概念 　　1.1 監督學習（Super

基於協同訓練的半監督文字分類演算法

標籤： 半監督學習，文字分類 作者：煉己者 --- 本部落格所有內容以學習、研究和分享為主，如需轉載，請聯絡本人，標明作者和出處，並且是非商業用途，謝謝！ 如果大家覺得格式看著不舒服，也歡迎大家去看我的簡書 半監督學習文字分類系列 用半監督演算法做文字分類(sklearn) sklearn半監督學習(

基於機器學習的文字分類演算法的研究

1. 簡述 文字分類的方法屬於有監督的學習方法，分類過程包括文字預處理、特徵抽取、降維、分類和模型評價。本文首先研究了文字分類的背景，中文分詞演算法。然後是對各種各樣的特徵抽取進行研究，包括詞項頻率-逆文件頻率和word2vec，降維方法有主成分分析法和潛在索引分析，最後是對分類演算法進行研究，

3D點雲地圖地面去除（1）：基於隨機一致性取樣（RANSAC），未做RANSAC後續處理

       點雲地圖中包括了大量的地面點，不僅讓整個地圖顯得很雜亂，而且對後續障礙物點雲的分類、識別和跟蹤帶來麻煩，所以需要首先去除。首先我有兩個思路： 在雷達的原始資料上把地面濾除，這樣在後續的建圖就不會有地面資訊。 在建圖結束後檢測地

PCL點雲庫：ICP演算法

轉自：https://www.cnblogs.com/21207-iHome/p/6034462.html 　ICP（Iterative Closest Point迭代最近點）演算法是一種點集對點集配準方法。在VTK、PCL、MRPT、MeshLab等C++庫或軟體中都有實現，可以參見維基百科中

基於KNN分類演算法手寫數字識別的實現（二）——構建KD樹

上一篇已經簡單粗暴的建立了一個KNN模型對手寫圖片進行了識別，所以本篇文章採用構造KD樹的方法實現手寫數字的識別。 （一）構造KD樹 構造KD樹的基本原理網上都有介紹，所以廢話不多說，直接上程式碼。 #Knn KD_Tree演算法 import math from

VoxelNet: 基於點雲的三維空間資訊逐層次學習網路

                                         

VoxelNet：基於點雲的三維物體檢測的端到端學習

https://arxiv.org/pdf/1711.06396.pdf