基於改進形態學濾波的點雲分類演算法------續
在前一篇部落格中《基於改進形態學濾波器的點雲分類演算法》中,介紹了論文中利用改進的形態學方法對點雲進行分類的步驟,這幾天也將該演算法在C++中進行了實現,所以今天將實現過程寫在本文中。
實現過程:
(1)讀入檔案
檔案的讀入分為兩類,分別是ASC型別的PCD和已二進位制格式儲存的PCD;ASC格式PCD的檔案讀入如下所示,關鍵是讀到其中儲存的POINTS 引數,該引數表示該檔案中共有多少個點。讀取的結果存入一個vector中並返回。
如果需要讀入的檔案問已二進位制儲存的PCD檔案,則需要利用到PCL中自帶的讀取方法,再經過一次轉換,將點雲資料仍然存放中一個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; }
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)將地物點寫入檔案
地物點寫入檔案的方法同寫入地面點相同。