根据视点计算点云的freespace_evidence
参考资料：
Bresenham's line algorithm：
Bresenham in 3D algorithm:
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 ::Ptr> m_PointClouds; 22 //循环读取点云数据和相机（视点信息） 23 for (int idx=startIdx;idx ::Ptr cloud(new pcl::PointCloud ); 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 (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 ::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 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 xlist;137 vector ylist;138 vector 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] =0 && X[1] =0 && X[2] =0 && X[0] =0 && X[1] =0 && X[2] =0 && X[0] =0 && X[1] =0 && X[2] =0 && X[0] =0 && X[1] =0 && X[2] dmax)245 {246 dmax=acc;247 }248 }249 }250 for (int row=0;row =1)258 {259 //图像为左上角点为坐标原点,同时行列顺序也变量260 rgb1.data[(gNumY-row-1)*gNumX+col]=255;261 }262 else263 {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 } 