double MapImage::getExploredFraction(std::vector<CvPoint> & pathHistory) { CvScalar p; // then also find trapped gray parts, make them zero too IplImage* im2 = cvCreateImage(cvSize(img->width,img->height), IPL_DEPTH_8U, 1); im2->widthStep = img->widthStep; cvCopy(img, im2); // consider mapped obstacles (180-255) as zeros, so all explored cells are represented // in a single continous interval cvThreshold(img, img, 180, 254, CV_THRESH_TOZERO_INV); // truncate values to get only obstacles cvThreshold(im2, im2, 180, 254, CV_THRESH_BINARY); cvDilate(im2, im2); bool found = false; CvPoint point; unsigned int i = 0; for(i = 0; i < pathHistory.size()-1; i++) { p = cvGet2D(im2, pathHistory[i].y, pathHistory[i].x); if( ((unsigned int)p.val[0]) < 73 ) { point.x = pathHistory[i].x; point.y = pathHistory[i].y; found = true; break; } } if( !found && i == 0 ) { point.x = pathHistory[0].x; point.y = pathHistory[0].y; found = true; } if( !found ) { std::string msg("error in finding a seed point to make map qt ready"); std::cout << msg << std::endl; throw MapException(msg.c_str()); } cvFloodFill(im2, point, cvScalar(127)); std::vector<int> dx; std::vector<int> dy; std::vector<CvPoint> neighbors; PointSet pointSet; for(int i = 0; i < img->height; i++) { dy.push_back(0); if(i == 0) dy.push_back(1); else if(i == img->height-1) dy.push_back(-1); else { dy.push_back(-1); dy.push_back(1); } for(int j = 0; j < img->width; j++) { // p = cvGet2D(im4,i,j); dx.push_back(0); if( j == 0 ) dx.push_back(1); else if(j == img->width-1) dx.push_back(-1); else { dx.push_back(-1); dx.push_back(1); } p = cvGet2D(im2,i,j); if( (unsigned int)p.val[0] == 0) // trapped gray cells { //cvSet2D(img, i, j, p); // set its neighbors too for(unsigned int k = 0; k < dy.size(); k++) { for(unsigned int l = 0; l < dx.size(); l++) { //cvSet2D(img, i+dy[k], j+dx[l], p); //neighbors.push_back(cvPoint(j+dx[l],i+dy[k])); pointSet.addPoint(cvPoint(j+dx[l],i+dy[k])); } } } dx.clear(); } dy.clear(); } pointSet.getPointsList(&neighbors); std::cout << "updating trapped pixels : " << neighbors.size() << std::endl; for(unsigned int i = 0; i < neighbors.size(); i++) { CvPoint pp = neighbors[i]; cvSet2D(img, pp.y, pp.x, cvScalar(0)); } if( neighbors.size() == (unsigned int)(img->widthStep*img->height)*0.5 ) // if more than 50% of the area is found as trapped, then something is wrong { std::cout << point.x << "," << point.y << " - " << (int)im2->imageData[point.y*im2->widthStep + point.x] << std::endl; cvNamedWindow("make1"); cvNamedWindow("make2"); cvShowImage("make1", im2); cvWaitKey(0); cvShowImage("make2", im2); cvWaitKey(0); // cvNamedWindow("make3"); // cvShowImage("make3", img); // cvWaitKey(0); cvDestroyWindow("make1"); cvDestroyWindow("make2"); // cvDestroyWindow("make3"); } // little hack to rectify mis-mapping of top most row and right most column for(int j = 0; j < img->width-1; j++) { cvSet2D(img, 0, j, cvGet2D(img, 1, j)); } for(int i = 0; i < img->height; i++) { cvSet2D(img, i, img->width-1, cvGet2D(img, i, img->width-2)); } int count = 0; for(int i = 0; i < img->height; i++) { for(int j = 0; j < img->widthStep; j++) { if( (unsigned int)img->imageData[i*img->widthStep + j] < (unsigned int)50 ) count++; } } cvReleaseImage(&im2); double totalCells = img->height*img->widthStep; return ((double)count/totalCells); }