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);
}
