基于改进形态学滤波的点云分类算法------续

           在前一篇博客中《基于改进形态学滤波器的点云分类算法》中，介绍了论文中利用改进的形态学方法对点云进行分类的步骤，这几天也将该算法在C++中进行了实现，所以今天将实现过程写在本文中。

    实现过程：

（1）读入文件

   文件的读入分为两类，分别是ASC类型的PCD和已二进制格式存储的PCD；ASC格式PCD的文件读入如下所示，关键是读到其中存储的POINTS 参数，该参数表示该文件中共有多少个点。读取的结果存入一个vector中并返回。

vector<MyPointXYZ> FileHelper::readAscPcd(const char* pcdFile){vector<MyPointXYZ> tempPointsVector;//存放结果的vectorint 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);//读取POINTSfscanf_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//每个元素赋初值-1arrB_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 thresholdsint 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);elsewindow_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;elseheight_threshold = slop * (window_size - window_sizes[iteration - 1]) * cellSize + initialDistence;// Enforce max distance on height thresholdif (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];}//将数组复制到Bfor (int i = 0; i < row; i++){for (int j = 0; j < col; j++){pointsArrB[i][j] = pointsArr[i][j];}}//pointsArrB = pointsArr;//将数组赋值给BiniFlagArr();//初始化标识数组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）将地物点写入文件

地物点写入文件的方法同写入地面点相同。