void
save_map(ScanMatcherMap smap, GridSlamProcessor::Particle pose)
{
static int map_number = 0;
char map_file_name[1000];
FILE *map_file;
sprintf(map_file_name, "map_file%d.pnm", map_number);
map_file = fopen(map_file_name, "w");
IntPoint robot = smap.world2map(pose);
// PNM file header
fprintf(map_file, "P3\n#PNM criado por Alberto\n");
fprintf(map_file, "%d %d\n255\n", smap.getMapSizeX(), smap.getMapSizeY());
for (int y = smap.getMapSizeY()-1; y >= 0; y--)
{
for (int x = 0; x < smap.getMapSizeX(); x++)
{
double v = smap.cell(x,y);
if ( (abs(x - robot.x) <= 2) && (abs(y - robot.y) <= 2) )
fprintf(map_file, "%d\n%d\n%d\n", 255, 0, 0);
else if (v >= 0)
{
int grayValue=255-(int)(255.*v);
fprintf(map_file, "%d\n%d\n%d\n", grayValue, grayValue, grayValue);
}
else
{
fprintf(map_file, "%d\n%d\n%d\n", 120, 120, 255);
}
}
}
fclose(map_file);
map_number++;
}
GMapping::ScanMatcherMap* GridFastSlamMapHandling::loadMap(const std::string& mapName)
{
// Open file:
FILE* mapFile = 0;
mapFile = fopen(mapName.c_str(), "rb" );
if(!mapFile) // In case of error
{
std::cout << mapName << " cannot be opened.\n";
return 0;
}
// Create Map object:
Point center;
double delta;
double xmin, ymin, xmax, ymax;
fread(¢er, sizeof(double), 2, mapFile);
fread(&delta, sizeof(double), 1, mapFile);
fread(&xmin, sizeof(double), 1, mapFile);
fread(&ymin, sizeof(double), 1, mapFile);
fread(&xmax, sizeof(double), 1, mapFile);
fread(&ymax, sizeof(double), 1, mapFile);
printf("center(%g,%g) delta(%g) xmin(%g) xmax(%g) ymin(%g) ymax(%g)\n", center.x, center.y, delta, xmin, xmax, ymin, ymax);
ScanMatcherMap* map = new ScanMatcherMap(center, xmin, ymin, xmax, ymax, delta);
// Read members of HierarchicalArray2D class:
int numberOfActiveAreas;
fread(&numberOfActiveAreas, sizeof(int), 1, mapFile);
HierarchicalArray2D<PointAccumulator>::PointSet activeAreas;
for(int i=0; i<numberOfActiveAreas; i++)
{
point<int> p;
fread(&p, sizeof(int), 2, mapFile);
activeAreas.insert(p);
}
map->storage().setActiveArea(activeAreas);
int numberOfActiveHCells;
fread(&numberOfActiveHCells, sizeof(int), 1, mapFile);
printf("numberOfActiveHCells %d\n", numberOfActiveHCells);
for(int i=0; i<numberOfActiveHCells; i++)
{
//Get position in hierarchical grid and the number of inner cells:
int x,y,numberOfActiveCells;
fread(&x, sizeof(int), 1, mapFile);
fread(&y, sizeof(int), 1, mapFile);
fread(&numberOfActiveCells, sizeof(int), 1, mapFile);
int patchMagnitude = map->storage().getPatchMagnitude();
printf("< %d\n", i);
Array2D<PointAccumulator>* patch = new Array2D<PointAccumulator>(1<<patchMagnitude, 1<<patchMagnitude);
map->storage().m_cells[x][y] = autoptr< Array2D<PointAccumulator> >(patch);
printf(">\n");
autoptr< Array2D<PointAccumulator> >& localArray = map->storage().m_cells[x][y];
for(int j=0; j<numberOfActiveCells; j++)
{
int xx,yy;
fread(&xx, sizeof(int), 1, mapFile);
fread(&yy, sizeof(int), 1, mapFile);
PointAccumulator& pAcc = localArray.m_reference->data->m_cells[xx][yy];
fread(&pAcc, sizeof(PointAccumulator), 1, mapFile);
}
}
// Close file and return map:
fclose(mapFile);
return map;
}
int main (int argc, char** argv)
{
QApplication app (argc, argv);
double maxrange = 50;
double delta = 0.1;
int scanSkip = 5;
const char* filename = 0;
const char* format = "PNG";
CMD_PARSE_BEGIN (1, argc)
parseDouble ("-maxrange", maxrange);
parseDouble ("-delta", delta);
parseInt ("-skip", scanSkip);
parseString ("-filename", filename);
parseString ("-format", format);
CMD_PARSE_END
double maxUrange = maxrange;
if (! filename)
{
cout << " supply a gfs file, please" << endl;
cout << " usage gfs2img [options] -filename <gfs_file>" << endl;
cout << " [options]:" << endl;
cout << " -maxrange <range>" << endl;
cout << " -delta <map cell size>" << endl;
cout << " -skip <frames to skip among images>" << endl;
cout << " -format <image format in capital letters>" << endl;
return -1;
}
ifstream is (filename);
if (!is)
{
cout << " supply an EXISTING gfs file, please" << endl;
return -1;
}
RecordList rl;
rl.read (is);
int particles = 0;
int beams = 0;
for (RecordList::const_iterator it = rl.begin(); it != rl.end(); it++)
{
const OdometryRecord* odometry = dynamic_cast<const OdometryRecord*> (*it);
if (odometry)
{
particles = odometry->dim;
}
const LaserRecord* s = dynamic_cast<const LaserRecord*> (*it);
if (s)
{
beams = s->readings.size();
}
if (particles && beams)
break;
}
cout << "Particles from gfs=" << particles << endl;
if (! particles)
{
cout << "no particles found, terminating" << endl;
return -1;
}
cout << "Laser beams from gfs=" << beams << endl;
if (! beams)
{
cout << "0 beams found, terminating" << endl;
return -1;
}
double laserBeamStep = 0;
if (beams == 180 || beams == 181)
{
laserBeamStep = M_PI / 180;
}
else if (beams == 360 || beams == 361)
{
laserBeamStep = M_PI / 360;
}
cout << "Laser beam step" << laserBeamStep << endl;
if (laserBeamStep == 0)
{
cout << "Invalid Beam Step, terminating" << endl;
return -1;
}
double laserAngles[MAX_LASER_BEAMS];
double theta = -M_PI / 2;
for (int i = 0; i < beams; i++)
{
laserAngles[i] = theta;
theta += laserBeamStep;
}
ScanMatcher matcher;
matcher.setLaserParameters (beams, laserAngles, OrientedPoint (0, 0, 0));
matcher.setlaserMaxRange (maxrange);
matcher.setusableRange (maxUrange);
matcher.setgenerateMap (true);
double xmin, ymin, xmax, ymax;
cout << "computing bounding box" << endl;
computeBoundingBox (xmin, ymin, xmax, ymax, rl, maxrange);
cout << "DONE" << endl << "BBOX= " << xmin << " " << ymin << " " << xmax << " " << ymax << endl;
Point center;
center.x = (xmin + xmax) / 2.;
center.y = (ymin + ymax) / 2.;
cout << "computing paths" << endl;
unsigned int frame = 0;
int scanCount = 0;
for (RecordList::const_iterator it = rl.begin(); it != rl.end(); it++)
{
const ScanMatchRecord* s = dynamic_cast<const ScanMatchRecord*> (*it);
if (!s)
continue;
scanCount++;
if (scanCount % scanSkip)
continue;
cout << "Frame " << frame << " ";
std::vector<RecordList> paths (particles);
int bestIdx = 0;
double bestWeight = -MAXDOUBLE;
for (int p = 0; p < particles; p++)
{
paths[p] = rl.computePath (p, it);
double w = rl.getLogWeight (p, it);
if (w > bestWeight)
{
bestWeight = w;
bestIdx = p;
}
}
cout << "bestIdx=" << bestIdx << " bestWeight=" << bestWeight << endl;
cout << "computing best map" << endl;
ScanMatcherMap smap (center, xmin, ymin, xmax, ymax, delta);
int count = 0;
for (RecordList::const_iterator mt = paths[bestIdx].begin(); mt != paths[bestIdx].end(); mt++)
{
const LaserRecord* s = dynamic_cast<const LaserRecord*> (*mt);
if (s)
{
double rawreadings[MAX_LASER_BEAMS];
for (uint i = 0; i < s->readings.size(); i++)
rawreadings[i] = s->readings[i];
matcher.invalidateActiveArea();
matcher.computeActiveArea (smap, s->pose, rawreadings);
// matcher.allocActiveArea(smap, s->pose, rawreadings);
matcher.registerScan (smap, s->pose, rawreadings);
count++;
}
}
cout << "DONE " << count << endl;
QPixmap pixmap (smap.getMapSizeX(), smap.getMapSizeY());
pixmap.fill (QColor (200, 200, 255));
QPainter painter (&pixmap);
for (int x = 0; x < smap.getMapSizeX(); x++)
for (int y = 0; y < smap.getMapSizeY(); y++)
{
double v = smap.cell (x, y);
if (v >= 0)
{
int grayValue = 255 - (int) (255.*v);
painter.setPen (QColor (grayValue, grayValue, grayValue));
painter.drawPoint (x, smap.getMapSizeY() - y - 1);
}
}
/*
cout << "painting trajectories" << endl;
for (int p=0; p<particles; p++){
painter.setPen(QColor(Qt::red));
if (p==bestIdx)
continue;
bool first=true;
IntPoint oldPoint(0,0);
for (RecordList::const_iterator mt=paths[p].begin(); mt!=paths[p].end(); mt++){
const LaserRecord* s=dynamic_cast<const LaserRecord*>(*mt);
if (s){
IntPoint ip=smap.world2map(s->pose);
ip.y=smap.getMapSizeY()-ip.y-1;
if (!first){
painter.drawLine( oldPoint.x, oldPoint.y, ip.x, ip.y);
}
oldPoint=ip;
first=false;
}
}
paths[p].destroyReferences();;
}
painter.setPen(QColor(Qt::black));
bool first=true;
IntPoint oldPoint(0,0);
for (RecordList::const_iterator mt=paths[bestIdx].begin(); mt!=paths[bestIdx].end(); mt++){
const LaserRecord* s=dynamic_cast<const LaserRecord*>(*mt);
if (s){
IntPoint ip=smap.world2map(s->pose);
ip.y=smap.getMapSizeY()-ip.y-1;
if (!first){
painter.drawLine( oldPoint.x, oldPoint.y, ip.x, ip.y);
}
oldPoint=ip;
first=false;
}
}
paths[bestIdx].destroyReferences();;
*/
cout << " DONE" << endl;
cout << "writing image" << endl;
QImage img = pixmap.convertToImage();
char ofilename[MAX_FILENAME];
sprintf (ofilename, "%s-%.4d.%s", filename, frame, format);
cout << ofilename << endl;
img.save (QString (ofilename), format, 0);
frame++;
}
cout << "For Cyrill: \"The Evil is Outside\"" << endl;
}
