freespace_evidence

根据视点计算点云的freespace_evidence

参考资料：

Bresenham's line algorithm：https://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm

Bresenham in 3D algorithm: http://members.chello.at/~easyfilter/bresenham.html

https://www.cnblogs.com/wlzy/p/8695226.html

//计算自由空间栅格，依赖视点
void qMIMSPlugin::doRasterFreeSpace()
{
    //选择文件夹，设置平面点云提取参数
    ccNDTFuisonDlg dlg;
    dlg.cellRaidiusSpinBox->setValue(.5f);//注意此处设置计算法向量的参数
    dlg.cellepsilonDoubleSpinBox->setValue(.075);
    dlg.epsilonDoubleSpinBox->setValue(.05);        // set distance threshold to 0.5% of bounding box width

    if (!dlg.exec())
        return;
    //获取参数
    double leaf_size=dlg.cellRaidiusSpinBox->value();
    QString mFolderPath=dlg.txtPath->text();
    int startIdx=dlg.spinBox->value();
    int endIdx=dlg.spinBox_2->value();
    double ratio=1.0;
    double isPlyformat=0;
    Eigen::Vector3d bbMin;
    Eigen::Vector3d bbMax;
    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> m_PointClouds;
    //循环读取点云数据和相机（视点信息）
    for (int idx=startIdx;idx<endIdx;idx++)
    {
        //读取点云数据
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
        //读取每一帧点云
        char a[10],b[10];
        sprintf(a, "%03d",idx);
        if (isPlyformat)
        {
            string filename=mFolderPath.toStdString() + "\scan" + a + ".ply";
            pcl::PLYReader reader;
            if (reader.read(filename,*cloud) == -1)
            {

                PCL_ERROR ("Couldn't read file *.pcd 
");
                m_app->dispToConsole("Read PCD file failed!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
                break;
            }
        }
        else
        {
            string filename=mFolderPath.toStdString() + "\scan" + a + ".pcd";
            if (pcl::io::loadPCDFile<pcl::PointXYZ> (filename, *cloud) == -1)
            {
                PCL_ERROR ("Couldn't read file *.pcd 
");
                m_app->dispToConsole("Read PCD file failed!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
                break;
            }
        }
        //包围盒叠加,存储极值的两个点
        pcl::PointXYZ minPt, maxPt;
        //获取坐标极值
        pcl::getMinMax3D(*cloud, minPt, maxPt);

        bbMin[0]=std::min(bbMin[0],(double)minPt.x);
        bbMin[1]=std::min(bbMin[1],(double)minPt.y);
        bbMin[2]=std::min(bbMin[2],(double)minPt.z);
        bbMax[0]=std::max(bbMax[0],(double)maxPt.x);
        bbMax[1]=std::max(bbMax[1],(double)maxPt.y);
        bbMax[2]=std::max(bbMax[2],(double)maxPt.z);
        //视点信息
        Eigen::Vector4f origin_=cloud->sensor_origin_;
        m_PointClouds.push_back(cloud);
    }
    //根据包围盒计算栅格数据的长宽高
    Eigen::Vector3d diff = bbMax - bbMin;
    double scale=std::max(std::max(diff[0], diff[1]), diff[2]);
    Eigen::Vector3d b0 =bbMin -0.05*diff;
    double xmax=bbMax[0];    double ymax=bbMax[1];    double zmax=bbMax[2];
    double xmin=bbMin[0];    double ymin=bbMin[1];    double zmin=bbMin[2];
    unsigned gNumX = ceil(1.10*(xmax-xmin)/leaf_size);
    unsigned gNumY = ceil(1.10*(ymax-ymin)/leaf_size);
    unsigned gNumZ = ceil(1.10*(zmax-zmin)/leaf_size);

    double *freespace_evidence = new double[gNumX*gNumY*gNumZ];
    for (int idt=0;idt<gNumX*gNumY*gNumZ;idt++)
    {
        freespace_evidence[idt]=0;
    }
    //遍历每一个点云
    for (int idx=0;idx<m_PointClouds.size();idx++)
    {
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud=m_PointClouds[idx];
        Eigen::Vector4f origin_=cloud->sensor_origin_;    
        int pointCount=cloud->size();
        //视点信息
        Eigen::Vector4f viewPoint=cloud->sensor_origin_;

        Eigen::Vector3d p0=Eigen::Vector3d::Zero();
        p0[0] = viewPoint[0];
        p0[1] = viewPoint[1];
        p0[2] = viewPoint[2];
        
        //遍历每一个点
        for (int jdx=0;jdx<pointCount;jdx++)
        {
            Eigen::Vector3d p1=Eigen::Vector3d::Zero();
            p1[0] = cloud->points[jdx].x;
            p1[1] = cloud->points[jdx].y;
            p1[2] = cloud->points[jdx].z;

            Eigen::Vector3d X=Eigen::Vector3d::Zero();
            X[0] = floor((p0[0]-b0[0])/leaf_size);
            X[1] = floor((p0[1]-b0[1])/leaf_size);
            X[2] = floor((p0[2]-b0[2])/leaf_size);//相机在栅格中的位置

            Eigen::Vector3d Y=Eigen::Vector3d::Zero();
            Y[0] = floor((p1[0]-b0[0])/leaf_size);
            Y[1] = floor((p1[1]-b0[1])/leaf_size);                               
            Y[2] = floor((p1[2]-b0[2])/leaf_size);//点在栅格中的位置

            Eigen::Vector3d v=Eigen::Vector3d::Zero();
            v[0] = p1[0] - p0[0];
            v[1] = p1[1] - p0[1];
            v[2] = p1[2] - p0[2];

            Eigen::Vector3d step=Eigen::Vector3d::Zero();
            step[0] = sign(v[0]);
            step[1] = sign(v[1]);
            step[2] = sign(v[2]);

            Eigen::Vector3d tDelta=Eigen::Vector3d::Zero();
            tDelta[0] = abs(leaf_size/v[0]);
            tDelta[1] = abs(leaf_size/v[1]);
            tDelta[2] = abs(leaf_size/v[2]);

            Eigen::Vector3d tMax=Eigen::Vector3d::Zero();
            tMax[0] = abs((0.5*(1-step[0])*leaf_size-(b0[0] + leaf_size*(X[0]+1) - p0[0]))/v[0]);
            tMax[1] = abs((0.5*(1-step[1])*leaf_size-(b0[1] + leaf_size*(X[1]+1) - p0[1]))/v[1]);
            tMax[2] = abs((0.5*(1-step[2])*leaf_size-(b0[2] + leaf_size*(X[2]+1) - p0[2]))/v[2]);

            int count = 0;
            unsigned long long index;
            vector<int> xlist;
            vector<int> ylist;
            vector<int> zlist;
            while(true)
            {
                //计算视点和点之间的关系
                if (X[0] == Y[0] && X[1] == Y[1] && X[2] == Y[2]) break;

                if (X[0] < 0 || X[0] >= gNumX || X[1] < 0 || X[1] >= gNumY || X[2] < 0 || X[2] >= gNumZ) break;

                if (tMax[0] < tMax[1])
                {
                    if (tMax[0] < tMax[2])
                    {
                        tMax[0] = tMax[0] + tDelta[0];
                        X[0] = X[0] + step[0];
                        index = X[2]*gNumX*gNumY+X[0]*gNumY+X[1];
                        if(X[0]>=0 && X[0]<gNumX && X[1]>=0 && X[1]<gNumY && X[2]>=0 && X[2]<gNumZ)
                        {
                            xlist.push_back(X[0]);
                            ylist.push_back(X[1]);
                            zlist.push_back(X[2]);
                        }

                    }
                    else
                    {
                        tMax[2] = tMax[2] + tDelta[2];
                        X[2] = X[2] + step[2];
                        index = X[2]*gNumX*gNumY+X[0]*gNumY+X[1];
                        if(X[0]>=0 && X[0]<gNumX && X[1]>=0 && X[1]<gNumY && X[2]>=0 && X[2]<gNumZ)
                        {
                            xlist.push_back(X[0]);
                            ylist.push_back(X[1]);
                            zlist.push_back(X[2]);
                        }

                    }
                }
                else
                {
                    if (tMax[1] < tMax[2])
                    {
                        tMax[1] = tMax[1] + tDelta[1];
                        X[1] = X[1] + step[1];
                        index = X[2]*gNumX*gNumY+X[0]*gNumY+X[1];
                        if(X[0]>=0 && X[0]<gNumX && X[1]>=0 && X[1]<gNumY && X[2]>=0 && X[2]<gNumZ)
                        {
                            xlist.push_back(X[0]);
                            ylist.push_back(X[1]);
                            zlist.push_back(X[2]);
                        }
                    }
                    else
                    {
                        tMax[2] = tMax[2] + tDelta[2];
                        X[2] = X[2] + step[2];
                        index = X[2]*gNumX*gNumY+X[0]*gNumY+X[1];
                        if(X[0]>=0 && X[0]<gNumX && X[1]>=0 && X[1]<gNumY && X[2]>=0 && X[2]<gNumZ)
                        {
                            xlist.push_back(X[0]);
                            ylist.push_back(X[1]);
                            zlist.push_back(X[2]);                    
                        }
                    }

                }
            }

            int size_list = xlist.size();

            for(int j=0;j<ratio*xlist.size();j++)
            {
                index = zlist[j]*gNumX*gNumY+xlist[j]*gNumY+ylist[j];
                *(freespace_evidence+index) = *(freespace_evidence+index) + 1;
            }
        }        
    }
    //按照Z方向层数，计算累加的栅格概率

    double *cellXOYs = new double[gNumX*gNumY];
    for (int idt=0;idt<gNumX*gNumY;idt++)
    {
        cellXOYs[idt]=0;
    }
    for (int idx=0;idx<gNumX;idx++)
    {
        for (int idy=0;idy<gNumY;idy++)
        {
            for (int k=0;k<gNumZ;k++)
            {
                //注意这个数组是以左下角点为原点的
                unsigned long long idxImage = idx*gNumY+idy;
                unsigned long long index = k*gNumX*gNumY+idx*gNumY+idy;
                *(cellXOYs+idxImage)+=*(freespace_evidence+index);    
            }
        }
    }

    //完成投影2D栅格，保存
    cv::Mat rgb1(gNumY,gNumX, CV_8UC1);
    double dmax=0;double dmin=0;
    for (int idx=0;idx<gNumX;idx++)
    {
        for (int idy=0;idy<gNumY;idy++)
        {
            unsigned long long idxImage = idx*gNumY+idy;//每个2D栅格中的值
            double acc=*(cellXOYs+idxImage);
            if (acc>dmax)
            {
                dmax=acc;
            }
        }
    }
    for (int row=0;row<gNumY;row++)
    {
        for (int col=0;col<gNumX;col++)
        {
            unsigned long long idxImage = col*gNumY+row;//每个2D栅格中的值，左下角点为坐标原点
            double acc=*(cellXOYs+idxImage);
            //rgb1.data[idx*gNumY+idy]=255*acc/(dmax-dmin);
            if (acc>=1)
            {
                //图像为左上角点为坐标原点,同时行列顺序也变量
                rgb1.data[(gNumY-row-1)*gNumX+col]=255;
            }
            else
            {
                rgb1.data[(gNumY-row-1)*gNumX+col]=acc;
            }

        }
    }
    //cv::normalize(rgb1,rgb1,1.0,0.0,cv::NORM_MINMAX);
    cv::Mat elementdilate = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(1, 1));  
    cv::Mat outdilate;  
    //进行膨胀操作  
    cv::dilate(rgb1, outdilate, elementdilate); 
    cv::imwrite( "D:\freesapce.png", outdilate);
    //计算左上角点坐标
    double topleftX=b0[0];
    double topleftY=b0[1]+gNumY*leaf_size;
    FILE * coord=fopen("D:\freesapce.pgw","w");
    if (coord)
    {
        fprintf(coord,"%f
",leaf_size);
        fprintf(coord,"%f
",0.000000);
        fprintf(coord,"%f
",0.000000);
        fprintf(coord,"%f
",-1.0*leaf_size);
        fprintf(coord,"%f
",topleftX);
        fprintf(coord,"%f
",topleftY);
        fclose(coord);
    }
    delete[] freespace_evidence;
    delete[] cellXOYs;
}