點雲的種子填充算法來聚個類

寫的代碼太多了，我也不記得哪個寫過哪個沒有寫過，好慘，找了半天才找到，既然如此，就記錄一下咯

一樣的先申明一哈哈，ccPointCloud是cloudcompare軟件裏面自帶的類型，就是存所有點的，反正都把它轉成pcl的類型進行處理

//格式轉換：ccPointCloud to pcl:PointCloud
void ccPointCloud2PointCloud(ccPointCloud *input,pcl::PointCloud<pcl::PointXYZ>::Ptr output)
{
    long npts=input->size();
    output->reserve(npts+1);
    for(long i=0;i<npts;i++)
    {
        const CCVector3* P_Residual = input->getPoint(i);
        pcl::PointXYZ *p =new pcl::PointXYZ;
        p->x = P_Residual->x;
        p->y = P_Residual->y;
        p->z = P_Residual->z;
        output->push_back(*p);
        delete []p;
    }
}

//基於種子填充算法進行空間連通性聚類，判斷每個類的密度
void ConnectedComponentAnalysis_filtering(ccPointCloud *input,ccPointCloud *output,ccPointCloud *output_1,double Td,double Th,int k,int longdis,double fitering_density)
{
    //輸入點雲數量
    long npts=input->size();

    //將點雲數據類型轉換爲pcl可處理的pcl::PointCloud<pcl::PointXYZ>::Ptr類型
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_r(new pcl::PointCloud<pcl::PointXYZ>());
    ccPointCloud2PointCloud(input,cloud_r);

    //聲明一個容器，作爲棧存儲滿足條件的點雲
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_l(new pcl::PointCloud<pcl::PointXYZ>());

    //對輸入的點雲數據創建k-d樹
    pcl::KdTreeFLANN<pcl::PointXYZ> tree_b;
    tree_b.setInputCloud(cloud_r);

    //對每個點雲給一個標記lable，初始值爲0
    int *flag=new int[npts+1];
    for(int i=0;i<npts;i++)
        flag[i]=0;

    output->reserve(npts+1);
    output_1->reserve(npts+1);

    int lable=1;//標記值
    double dis,dh;//距離和高差

    for(long i=0;i<npts;i++)
    {
        //對於每一個點，若標記值爲0，則搜索鄰近區域
        if(flag[i]==0)
        {
            //當前點入棧並標記
            pcl::PointXYZ pts_add=cloud_r->at(i);
            cloud_l->push_back(pts_add);
            flag[i]=lable;

            std::vector<int> pointIdxNKNSearch(k);//用來存儲k個點
            std::vector<float> pointNKNSquaredDistance(k);//用來存儲每個點到樣點的距離

            pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_output(new pcl::PointCloud<pcl::PointXYZ>());

            //如果棧不爲空，搜索當前點的k個鄰近點，如果滿足距離和高差均小於閾值且沒有被標記，則進棧並標記
            while(!cloud_l->empty())
            {
                //指向棧底的數據
                pcl::PointCloud<pcl::PointXYZ>::iterator index=cloud_l->begin();
                pcl::PointXYZ pt_now;
                pt_now.x = index->x;
                pt_now.y = index->y;
                pt_now.z = index->z;

                //在數據刪除之前存儲
                cloud_output->push_back(pt_now);
                cloud_l->erase(index);

                if(tree_b.nearestKSearch(pt_now,k, pointIdxNKNSearch, pointNKNSquaredDistance)>0)
                {
                    int closet_point=0;
                    for(long j=0;(j<k);j++)
                    {
                        //對所有鄰近點，判斷如果滿足距離和高差均小於閾值且沒有被標記，則進棧並標記
                        pcl::PointXYZ pp=cloud_r->points[pointIdxNKNSearch[j]];
                        dis=sqrt(pointNKNSquaredDistance[j]);
                        dh=fabs(pt_now.z-pp.z);
                        //這裏聚類的時候我設置了各種五花八門的條件，反正根據自己的要求設置條件就可以了
                        if(dh<Th && dis<Td  && flag[pointIdxNKNSearch[j]]==0 )//&& fabs(pp.z-h_ave)<5
                        {
                            cloud_l->push_back(pp);
                            flag[pointIdxNKNSearch[j]]=lable；
                        }

                    }
                }
            }
            if(cloud_output->size()<10)
                continue;

            ccPointCloud *cc_linepts=new ccPointCloud;
            PointCloud2ccPointCloud(cloud_output,cc_linepts);
          
            //對不同直線賦予不同的顏色屬性
            int col[3];
            col[0] = int(float(255)*float(rand())/float(RAND_MAX));
            col[1] = int(float(255)*float(rand())/float(RAND_MAX));
            col[2] = int(float(255)*float(rand())/float(RAND_MAX));
            cc_linepts->setRGBColor(col[0],col[1],col[2]);
            *output_1+=cc_linepts;
            delete[]cc_linepts;
            cc_linepts=NULL;
            lable++;
        }
    }
    delete[]flag;
}

就是用了一個棧吧，進站出站，原理來說還是很簡單的，儘管當時寫的時候想了好久才搞明白，反正當初師兄跟我也是這麼說的，結果圖：就是這個樣子了，不同的顏色就是不同的類別，只是這個類別是根據連通性來分的