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 \n");
 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 \n");
 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\n",leaf_size);
282         fprintf(coord,"%f\n",0.000000);
283         fprintf(coord,"%f\n",0.000000);
284         fprintf(coord,"%f\n",-1.0*leaf_size);
285         fprintf(coord,"%f\n",topleftX);
286         fprintf(coord,"%f\n",topleftY);
287         fclose(coord);
288     }
289     delete[] freespace_evidence;
290     delete[] cellXOYs;
291 }
View Code

 

 

posted @ 2017-10-07 17:12  太一吾鱼水  阅读(310)  评论(0)  编辑  收藏  举报