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(.);
     dlg.epsilonDoubleSpinBox->setValue(.);        // 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=;
     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[],b[];
         sprintf(a, "%03d",idx);
         if (isPlyformat)
         {
             string filename=mFolderPath.toStdString() + "\\scan" + a + ".ply";
             pcl::PLYReader reader;
             if (reader.read(filename,*cloud) == -)
             {

                 PCL_ERROR ("Couldn't read file *.pcd \n");
                 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) == -)
             {
                 PCL_ERROR ("Couldn't read file *.pcd \n");
                 m_app->dispToConsole("Read PCD file failed!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
                 break;
             }
         }
         //包围盒叠加,存储极值的两个点
         pcl::PointXYZ minPt, maxPt;
         //获取坐标极值
         pcl::getMinMax3D(*cloud, minPt, maxPt);

         bbMin[]=std::min(bbMin[],(double)minPt.x);
         bbMin[]=std::min(bbMin[],(double)minPt.y);
         bbMin[]=std::min(bbMin[],(double)minPt.z);
         bbMax[]=std::max(bbMax[],(double)maxPt.x);
         bbMax[]=std::max(bbMax[],(double)maxPt.y);
         bbMax[]=std::max(bbMax[],(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[], diff[]), diff[]);
     Eigen::Vector3d b0 =bbMin -0.05*diff;
     double xmax=bbMax[];    double ymax=bbMax[];    double zmax=bbMax[];
     double xmin=bbMin[];    double ymin=bbMin[];    double zmin=bbMin[];
     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=;idt<gNumX*gNumY*gNumZ;idt++)
     {
         freespace_evidence[idt]=;
     }
     //遍历每一个点云
     for (int idx=;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[] = viewPoint[];
         p0[] = viewPoint[];
         p0[] = viewPoint[];

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

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

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

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

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

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

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

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

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

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

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

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

                 }
             }

             int size_list = xlist.size();

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

     double *cellXOYs = new double[gNumX*gNumY];
     for (int idt=;idt<gNumX*gNumY;idt++)
     {
         cellXOYs[idt]=;
     }
     for (int idx=;idx<gNumX;idx++)
     {
         for (int idy=;idy<gNumY;idy++)
         {
             for (int k=;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=;double dmin=;
     for (int idx=;idx<gNumX;idx++)
     {
         for (int idy=;idy<gNumY;idy++)
         {
             unsigned long long idxImage = idx*gNumY+idy;//每个2D栅格中的值
             double acc=*(cellXOYs+idxImage);
             if (acc>dmax)
             {
                 dmax=acc;
             }
         }
     }
     for (int row=;row<gNumY;row++)
     {
         for (int col=;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>=)
             {
                 //图像为左上角点为坐标原点,同时行列顺序也变量
                 rgb1.data[(gNumY-row-)*gNumX+col]=;
             }
             else
             {
                 rgb1.data[(gNumY-row-)*gNumX+col]=acc;
             }

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