zoukankan      html  css  js  c++  java
  • 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

      1 //计算自由空间栅格,依赖视点
      2 void qMIMSPlugin::doRasterFreeSpace()
      3 {
      4     //选择文件夹,设置平面点云提取参数
      5     ccNDTFuisonDlg dlg;
      6     dlg.cellRaidiusSpinBox->setValue(.5f);//注意此处设置计算法向量的参数
      7     dlg.cellepsilonDoubleSpinBox->setValue(.075);
      8     dlg.epsilonDoubleSpinBox->setValue(.05);        // set distance threshold to 0.5% of bounding box width
      9 
     10     if (!dlg.exec())
     11         return;
     12     //获取参数
     13     double leaf_size=dlg.cellRaidiusSpinBox->value();
     14     QString mFolderPath=dlg.txtPath->text();
     15     int startIdx=dlg.spinBox->value();
     16     int endIdx=dlg.spinBox_2->value();
     17     double ratio=1.0;
     18     double isPlyformat=0;
     19     Eigen::Vector3d bbMin;
     20     Eigen::Vector3d bbMax;
     21     std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> m_PointClouds;
     22     //循环读取点云数据和相机(视点信息)
     23     for (int idx=startIdx;idx<endIdx;idx++)
     24     {
     25         //读取点云数据
     26         pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
     27         //读取每一帧点云
     28         char a[10],b[10];
     29         sprintf(a, "%03d",idx);
     30         if (isPlyformat)
     31         {
     32             string filename=mFolderPath.toStdString() + "\scan" + a + ".ply";
     33             pcl::PLYReader reader;
     34             if (reader.read(filename,*cloud) == -1)
     35             {
     36 
     37                 PCL_ERROR ("Couldn't read file *.pcd 
    ");
     38                 m_app->dispToConsole("Read PCD file failed!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
     39                 break;
     40             }
     41         }
     42         else
     43         {
     44             string filename=mFolderPath.toStdString() + "\scan" + a + ".pcd";
     45             if (pcl::io::loadPCDFile<pcl::PointXYZ> (filename, *cloud) == -1)
     46             {
     47                 PCL_ERROR ("Couldn't read file *.pcd 
    ");
     48                 m_app->dispToConsole("Read PCD file failed!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
     49                 break;
     50             }
     51         }
     52         //包围盒叠加,存储极值的两个点
     53         pcl::PointXYZ minPt, maxPt;
     54         //获取坐标极值
     55         pcl::getMinMax3D(*cloud, minPt, maxPt);
     56 
     57         bbMin[0]=std::min(bbMin[0],(double)minPt.x);
     58         bbMin[1]=std::min(bbMin[1],(double)minPt.y);
     59         bbMin[2]=std::min(bbMin[2],(double)minPt.z);
     60         bbMax[0]=std::max(bbMax[0],(double)maxPt.x);
     61         bbMax[1]=std::max(bbMax[1],(double)maxPt.y);
     62         bbMax[2]=std::max(bbMax[2],(double)maxPt.z);
     63         //视点信息
     64         Eigen::Vector4f origin_=cloud->sensor_origin_;
     65         m_PointClouds.push_back(cloud);
     66     }
     67     //根据包围盒计算栅格数据的长宽高
     68     Eigen::Vector3d diff = bbMax - bbMin;
     69     double scale=std::max(std::max(diff[0], diff[1]), diff[2]);
     70     Eigen::Vector3d b0 =bbMin -0.05*diff;
     71     double xmax=bbMax[0];    double ymax=bbMax[1];    double zmax=bbMax[2];
     72     double xmin=bbMin[0];    double ymin=bbMin[1];    double zmin=bbMin[2];
     73     unsigned gNumX = ceil(1.10*(xmax-xmin)/leaf_size);
     74     unsigned gNumY = ceil(1.10*(ymax-ymin)/leaf_size);
     75     unsigned gNumZ = ceil(1.10*(zmax-zmin)/leaf_size);
     76 
     77     double *freespace_evidence = new double[gNumX*gNumY*gNumZ];
     78     for (int idt=0;idt<gNumX*gNumY*gNumZ;idt++)
     79     {
     80         freespace_evidence[idt]=0;
     81     }
     82     //遍历每一个点云
     83     for (int idx=0;idx<m_PointClouds.size();idx++)
     84     {
     85         pcl::PointCloud<pcl::PointXYZ>::Ptr cloud=m_PointClouds[idx];
     86         Eigen::Vector4f origin_=cloud->sensor_origin_;    
     87         int pointCount=cloud->size();
     88         //视点信息
     89         Eigen::Vector4f viewPoint=cloud->sensor_origin_;
     90 
     91         Eigen::Vector3d p0=Eigen::Vector3d::Zero();
     92         p0[0] = viewPoint[0];
     93         p0[1] = viewPoint[1];
     94         p0[2] = viewPoint[2];
     95         
     96         //遍历每一个点
     97         for (int jdx=0;jdx<pointCount;jdx++)
     98         {
     99             Eigen::Vector3d p1=Eigen::Vector3d::Zero();
    100             p1[0] = cloud->points[jdx].x;
    101             p1[1] = cloud->points[jdx].y;
    102             p1[2] = cloud->points[jdx].z;
    103 
    104             Eigen::Vector3d X=Eigen::Vector3d::Zero();
    105             X[0] = floor((p0[0]-b0[0])/leaf_size);
    106             X[1] = floor((p0[1]-b0[1])/leaf_size);
    107             X[2] = floor((p0[2]-b0[2])/leaf_size);//相机在栅格中的位置
    108 
    109             Eigen::Vector3d Y=Eigen::Vector3d::Zero();
    110             Y[0] = floor((p1[0]-b0[0])/leaf_size);
    111             Y[1] = floor((p1[1]-b0[1])/leaf_size);                               
    112             Y[2] = floor((p1[2]-b0[2])/leaf_size);//点在栅格中的位置
    113 
    114             Eigen::Vector3d v=Eigen::Vector3d::Zero();
    115             v[0] = p1[0] - p0[0];
    116             v[1] = p1[1] - p0[1];
    117             v[2] = p1[2] - p0[2];
    118 
    119             Eigen::Vector3d step=Eigen::Vector3d::Zero();
    120             step[0] = sign(v[0]);
    121             step[1] = sign(v[1]);
    122             step[2] = sign(v[2]);
    123 
    124             Eigen::Vector3d tDelta=Eigen::Vector3d::Zero();
    125             tDelta[0] = abs(leaf_size/v[0]);
    126             tDelta[1] = abs(leaf_size/v[1]);
    127             tDelta[2] = abs(leaf_size/v[2]);
    128 
    129             Eigen::Vector3d tMax=Eigen::Vector3d::Zero();
    130             tMax[0] = abs((0.5*(1-step[0])*leaf_size-(b0[0] + leaf_size*(X[0]+1) - p0[0]))/v[0]);
    131             tMax[1] = abs((0.5*(1-step[1])*leaf_size-(b0[1] + leaf_size*(X[1]+1) - p0[1]))/v[1]);
    132             tMax[2] = abs((0.5*(1-step[2])*leaf_size-(b0[2] + leaf_size*(X[2]+1) - p0[2]))/v[2]);
    133 
    134             int count = 0;
    135             unsigned long long index;
    136             vector<int> xlist;
    137             vector<int> ylist;
    138             vector<int> zlist;
    139             while(true)
    140             {
    141                 //计算视点和点之间的关系
    142                 if (X[0] == Y[0] && X[1] == Y[1] && X[2] == Y[2]) break;
    143 
    144                 if (X[0] < 0 || X[0] >= gNumX || X[1] < 0 || X[1] >= gNumY || X[2] < 0 || X[2] >= gNumZ) break;
    145 
    146                 if (tMax[0] < tMax[1])
    147                 {
    148                     if (tMax[0] < tMax[2])
    149                     {
    150                         tMax[0] = tMax[0] + tDelta[0];
    151                         X[0] = X[0] + step[0];
    152                         index = X[2]*gNumX*gNumY+X[0]*gNumY+X[1];
    153                         if(X[0]>=0 && X[0]<gNumX && X[1]>=0 && X[1]<gNumY && X[2]>=0 && X[2]<gNumZ)
    154                         {
    155                             xlist.push_back(X[0]);
    156                             ylist.push_back(X[1]);
    157                             zlist.push_back(X[2]);
    158                         }
    159 
    160                     }
    161                     else
    162                     {
    163                         tMax[2] = tMax[2] + tDelta[2];
    164                         X[2] = X[2] + step[2];
    165                         index = X[2]*gNumX*gNumY+X[0]*gNumY+X[1];
    166                         if(X[0]>=0 && X[0]<gNumX && X[1]>=0 && X[1]<gNumY && X[2]>=0 && X[2]<gNumZ)
    167                         {
    168                             xlist.push_back(X[0]);
    169                             ylist.push_back(X[1]);
    170                             zlist.push_back(X[2]);
    171                         }
    172 
    173                     }
    174                 }
    175                 else
    176                 {
    177                     if (tMax[1] < tMax[2])
    178                     {
    179                         tMax[1] = tMax[1] + tDelta[1];
    180                         X[1] = X[1] + step[1];
    181                         index = X[2]*gNumX*gNumY+X[0]*gNumY+X[1];
    182                         if(X[0]>=0 && X[0]<gNumX && X[1]>=0 && X[1]<gNumY && X[2]>=0 && X[2]<gNumZ)
    183                         {
    184                             xlist.push_back(X[0]);
    185                             ylist.push_back(X[1]);
    186                             zlist.push_back(X[2]);
    187                         }
    188                     }
    189                     else
    190                     {
    191                         tMax[2] = tMax[2] + tDelta[2];
    192                         X[2] = X[2] + step[2];
    193                         index = X[2]*gNumX*gNumY+X[0]*gNumY+X[1];
    194                         if(X[0]>=0 && X[0]<gNumX && X[1]>=0 && X[1]<gNumY && X[2]>=0 && X[2]<gNumZ)
    195                         {
    196                             xlist.push_back(X[0]);
    197                             ylist.push_back(X[1]);
    198                             zlist.push_back(X[2]);                    
    199                         }
    200                     }
    201 
    202                 }
    203             }
    204 
    205             int size_list = xlist.size();
    206 
    207             for(int j=0;j<ratio*xlist.size();j++)
    208             {
    209                 index = zlist[j]*gNumX*gNumY+xlist[j]*gNumY+ylist[j];
    210                 *(freespace_evidence+index) = *(freespace_evidence+index) + 1;
    211             }
    212         }        
    213     }
    214     //按照Z方向层数,计算累加的栅格概率
    215 
    216     double *cellXOYs = new double[gNumX*gNumY];
    217     for (int idt=0;idt<gNumX*gNumY;idt++)
    218     {
    219         cellXOYs[idt]=0;
    220     }
    221     for (int idx=0;idx<gNumX;idx++)
    222     {
    223         for (int idy=0;idy<gNumY;idy++)
    224         {
    225             for (int k=0;k<gNumZ;k++)
    226             {
    227                 //注意这个数组是以左下角点为原点的
    228                 unsigned long long idxImage = idx*gNumY+idy;
    229                 unsigned long long index = k*gNumX*gNumY+idx*gNumY+idy;
    230                 *(cellXOYs+idxImage)+=*(freespace_evidence+index);    
    231             }
    232         }
    233     }
    234 
    235     //完成投影2D栅格,保存
    236     cv::Mat rgb1(gNumY,gNumX, CV_8UC1);
    237     double dmax=0;double dmin=0;
    238     for (int idx=0;idx<gNumX;idx++)
    239     {
    240         for (int idy=0;idy<gNumY;idy++)
    241         {
    242             unsigned long long idxImage = idx*gNumY+idy;//每个2D栅格中的值
    243             double acc=*(cellXOYs+idxImage);
    244             if (acc>dmax)
    245             {
    246                 dmax=acc;
    247             }
    248         }
    249     }
    250     for (int row=0;row<gNumY;row++)
    251     {
    252         for (int col=0;col<gNumX;col++)
    253         {
    254             unsigned long long idxImage = col*gNumY+row;//每个2D栅格中的值,左下角点为坐标原点
    255             double acc=*(cellXOYs+idxImage);
    256             //rgb1.data[idx*gNumY+idy]=255*acc/(dmax-dmin);
    257             if (acc>=1)
    258             {
    259                 //图像为左上角点为坐标原点,同时行列顺序也变量
    260                 rgb1.data[(gNumY-row-1)*gNumX+col]=255;
    261             }
    262             else
    263             {
    264                 rgb1.data[(gNumY-row-1)*gNumX+col]=acc;
    265             }
    266 
    267         }
    268     }
    269     //cv::normalize(rgb1,rgb1,1.0,0.0,cv::NORM_MINMAX);
    270     cv::Mat elementdilate = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(1, 1));  
    271     cv::Mat outdilate;  
    272     //进行膨胀操作  
    273     cv::dilate(rgb1, outdilate, elementdilate); 
    274     cv::imwrite( "D:\freesapce.png", outdilate);
    275     //计算左上角点坐标
    276     double topleftX=b0[0];
    277     double topleftY=b0[1]+gNumY*leaf_size;
    278     FILE * coord=fopen("D:\freesapce.pgw","w");
    279     if (coord)
    280     {
    281         fprintf(coord,"%f
    ",leaf_size);
    282         fprintf(coord,"%f
    ",0.000000);
    283         fprintf(coord,"%f
    ",0.000000);
    284         fprintf(coord,"%f
    ",-1.0*leaf_size);
    285         fprintf(coord,"%f
    ",topleftX);
    286         fprintf(coord,"%f
    ",topleftY);
    287         fclose(coord);
    288     }
    289     delete[] freespace_evidence;
    290     delete[] cellXOYs;
    291 }
    View Code

     

  • 相关阅读:
    python基础(十七)
    Python基础(十六)
    Python基础(十五)
    Python基础(十四)
    Python基础(十三)
    Python基础(十二)
    项目分享:模拟博客园登录
    项目分享:模拟购物车
    Python基础(十一)
    课件站
  • 原文地址:https://www.cnblogs.com/yhlx125/p/7635184.html
Copyright © 2011-2022 走看看