gfs2img.cpp
Go to the documentation of this file.
00001 #include <cstdlib>
00002 #include <limits.h>
00003 #include <scanmatcher/scanmatcher.h>
00004 #include <gridfastslam/gfsreader.h>
00005 #include <qpixmap.h>
00006 #include <qpainter.h>
00007 #include <qimage.h>
00008 #include <qapplication.h>
00009 #include <utils/commandline.h>
00010 
00011 #define MAX_LASER_BEAMS 1024
00012 #define MAX_FILENAME 1024
00013 
00014 using namespace std;
00015 using namespace GMapping;
00016 using namespace GMapping::GFSReader;
00017 
00018 inline double min(double a, double b){
00019         return (a<b)?a:b;
00020 }
00021 
00022 inline double max(double a, double b){
00023         return (a>b)?a:b;
00024 }
00025 
00026 void computeBoundingBox(double& xmin, double& ymin, double& xmax, double& ymax, const LaserRecord& laser, const OrientedPoint& pose, double maxrange){
00027         double theta=-M_PI/2+pose.theta;
00028         double theta_step=(laser.readings.size()==180||laser.readings.size()==181)?M_PI/180:M_PI/360;
00029         for (std::vector<double>::const_iterator it=laser.readings.begin(); it!=laser.readings.end(); it++){
00030                 if (*it<maxrange){
00031                         xmin=min(xmin,pose.x+*it*cos(theta));
00032                         ymin=min(ymin,pose.y+*it*sin(theta));
00033                         xmax=max(xmax,pose.x+*it*cos(theta));
00034                         ymax=max(ymax,pose.y+*it*sin(theta));
00035                 }
00036                 theta+=theta_step;
00037         }
00038 }
00039 
00040 void computeBoundingBox(double& xmin, double& ymin, double& xmax, double& ymax, const RecordList& rl, double maxrange){
00041         xmin = ymin = MAXDOUBLE;
00042         xmax = ymax =-MAXDOUBLE;
00043         const LaserRecord* lastLaser=0;
00044         for (RecordList::const_iterator it=rl.begin(); it!=rl.end(); it++){
00045                 const LaserRecord* lr= dynamic_cast<const LaserRecord*>(*it);
00046                 if (lr){
00047                         lastLaser=lr;
00048                         continue;
00049                 }
00050                 const ScanMatchRecord* smr= dynamic_cast<const ScanMatchRecord*>(*it);
00051                 if (smr && lastLaser){
00052                         for (std::vector<OrientedPoint>::const_iterator pit=smr->poses.begin(); pit!=smr->poses.end(); pit++){
00053                                 computeBoundingBox(xmin, ymin, xmax, ymax, *lastLaser, *pit, maxrange);
00054                         }
00055                 }
00056         }
00057 }
00058 int main(int argc, char** argv){
00059         QApplication app(argc, argv);
00060         double maxrange=50;
00061         double delta=0.1;
00062         int scanSkip=5;
00063         const char* filename=0;
00064         const char* format="PNG";
00065         CMD_PARSE_BEGIN(1, argc)
00066                 parseDouble("-maxrange", maxrange);
00067                 parseDouble("-delta", delta);
00068                 parseInt("-skip", scanSkip);
00069                 parseString("-filename",filename);
00070                 parseString("-format",format);
00071         CMD_PARSE_END
00072         
00073         double maxUrange=maxrange;
00074         if (! filename){
00075                 cout << " supply a gfs file, please" << endl;
00076                 cout << " usage gfs2img [options] -filename <gfs_file>" << endl;
00077                 cout << " [options]:" << endl;
00078                 cout << " -maxrange <range>" << endl;
00079                 cout << " -delta    <map cell size>" << endl;
00080                 cout << " -skip     <frames to skip among images>" << endl;
00081                 cout << " -format   <image format in capital letters>" << endl;
00082                 return -1;
00083         }
00084         ifstream is(filename);
00085         if (!is){
00086                 cout << " supply an EXISTING gfs file, please" << endl;
00087                 return -1;
00088         }
00089         RecordList rl;
00090         rl.read(is);
00091         
00092         int particles=0;
00093         int beams=0;
00094         for (RecordList::const_iterator it=rl.begin(); it!=rl.end(); it++){
00095                 const OdometryRecord* odometry=dynamic_cast<const OdometryRecord*>(*it);
00096                 if (odometry){
00097                         particles=odometry->dim;
00098                 }
00099                 const LaserRecord* s=dynamic_cast<const LaserRecord*>(*it);
00100                 if (s){
00101                         beams=s->readings.size();
00102                 }
00103                 if (particles && beams)
00104                         break;
00105         }
00106         cout << "Particles from gfs=" << particles << endl;
00107         if (! particles){
00108                 cout << "no particles found, terminating" << endl;
00109                 return -1;
00110         }
00111         cout << "Laser beams from gfs=" << beams << endl;
00112         if (! beams){
00113                 cout << "0 beams found, terminating" << endl;
00114                 return -1;
00115         }
00116         
00117         
00118         double laserBeamStep=0;
00119         if (beams==180||beams==181){
00120                 laserBeamStep=M_PI/180;
00121         } else if (beams==360||beams==361){
00122                 laserBeamStep=M_PI/360;
00123         }
00124         cout << "Laser beam step" << laserBeamStep << endl;
00125         if (laserBeamStep==0){
00126                 cout << "Invalid Beam Step, terminating" << endl;
00127                 return -1;
00128         }
00129         double laserAngles[MAX_LASER_BEAMS];
00130         double theta=-M_PI/2;
00131         for (int i=0; i<beams; i++){
00132                 laserAngles[i]=theta;
00133                 theta+=laserBeamStep;
00134         }
00135 
00136         ScanMatcher matcher;
00137         matcher.setLaserParameters(beams, laserAngles, OrientedPoint(0,0,0));
00138         matcher.setlaserMaxRange(maxrange);
00139         matcher.setusableRange(maxUrange);
00140         matcher.setgenerateMap(true);
00141         
00142         double xmin, ymin, xmax, ymax;
00143         cout << "computing bounding box" << endl;
00144         computeBoundingBox(xmin, ymin, xmax, ymax, rl, maxrange);
00145         cout << "DONE" << endl << "BBOX= " << xmin << " " << ymin << " " << xmax << " " << ymax << endl;
00146 
00147         Point center;
00148         center.x=(xmin+xmax)/2.;
00149         center.y=(ymin+ymax)/2.;
00150                 
00151         cout << "computing paths" << endl;
00152         unsigned int frame=0;
00153         int scanCount=0;
00154         
00155         for (RecordList::const_iterator it=rl.begin(); it!=rl.end(); it++){
00156                 const ScanMatchRecord* s=dynamic_cast<const ScanMatchRecord*>(*it);
00157                 if (!s) 
00158                         continue;
00159                 scanCount++;
00160                 if (scanCount%scanSkip)
00161                         continue;
00162                 cout << "Frame " << frame << " ";
00163                 std::vector<RecordList> paths(particles);
00164                 int bestIdx=0;
00165                 double bestWeight=-MAXDOUBLE;
00166                 for (int p=0; p<particles; p++){
00167                         paths[p]=rl.computePath(p,it);
00168                         double w=rl.getLogWeight(p,it);
00169                         if (w>bestWeight){
00170                                 bestWeight=w;
00171                                 bestIdx=p;
00172                         }
00173                 }
00174                 cout << "bestIdx=" << bestIdx << " bestWeight=" << bestWeight << endl;
00175                 
00176                 cout << "computing best map" << endl;
00177                 ScanMatcherMap smap(center, xmin, ymin, xmax, ymax, delta);
00178                 int count=0;
00179                 for (RecordList::const_iterator mt=paths[bestIdx].begin(); mt!=paths[bestIdx].end(); mt++){
00180                         const LaserRecord* s=dynamic_cast<const LaserRecord*>(*mt);
00181                         if (s){
00182                                 double rawreadings[MAX_LASER_BEAMS];
00183                                 for (uint i=0; i<s->readings.size(); i++)
00184                                         rawreadings[i]=s->readings[i];
00185                                 matcher.invalidateActiveArea();
00186                                 matcher.computeActiveArea(smap, s->pose, rawreadings);
00187 //                              matcher.allocActiveArea(smap, s->pose, rawreadings);
00188                                 matcher.registerScan(smap, s->pose, rawreadings);
00189                                 count++;
00190                         }
00191                 }
00192                 cout << "DONE " << count <<endl;
00193 
00194                 QPixmap pixmap(smap.getMapSizeX(), smap.getMapSizeY());
00195                 pixmap.fill(QColor(200, 200, 255));
00196                 QPainter painter(&pixmap);
00197                 for (int x=0; x<smap.getMapSizeX(); x++)
00198                         for (int y=0; y<smap.getMapSizeY(); y++){
00199                                 double v=smap.cell(x,y);
00200                                 if (v>=0){
00201                                         int grayValue=255-(int)(255.*v);
00202                                         painter.setPen(QColor(grayValue, grayValue, grayValue));
00203                                         painter.drawPoint(x,smap.getMapSizeY()-y-1);
00204                                 }
00205                         }
00206                 
00207                 /*
00208                 cout << "painting trajectories" << endl;
00209                 for (int p=0; p<particles; p++){
00210                         painter.setPen(QColor(Qt::red));
00211                         if (p==bestIdx)
00212                                 continue;
00213                         bool first=true;
00214                         IntPoint oldPoint(0,0);
00215                         for (RecordList::const_iterator mt=paths[p].begin(); mt!=paths[p].end(); mt++){
00216                                 const LaserRecord* s=dynamic_cast<const LaserRecord*>(*mt);
00217                                 if (s){
00218                                         IntPoint ip=smap.world2map(s->pose);
00219                                         ip.y=smap.getMapSizeY()-ip.y-1;
00220                                         if (!first){
00221                                                 painter.drawLine( oldPoint.x, oldPoint.y, ip.x, ip.y);
00222                                         }
00223                                         oldPoint=ip;
00224                                         first=false;
00225                                 }
00226                         }       
00227                         paths[p].destroyReferences();;
00228                 }
00229                 painter.setPen(QColor(Qt::black));
00230                 bool first=true;
00231                 IntPoint oldPoint(0,0);
00232                 for (RecordList::const_iterator mt=paths[bestIdx].begin(); mt!=paths[bestIdx].end(); mt++){
00233                         const LaserRecord* s=dynamic_cast<const LaserRecord*>(*mt);
00234                         if (s){
00235                                 IntPoint ip=smap.world2map(s->pose);
00236                                 ip.y=smap.getMapSizeY()-ip.y-1;
00237                                 if (!first){
00238                                         painter.drawLine( oldPoint.x, oldPoint.y, ip.x, ip.y);
00239                                 } 
00240                                 oldPoint=ip;
00241                                 first=false;
00242                         }
00243                 }       
00244                 paths[bestIdx].destroyReferences();;
00245                 */
00246                 cout << " DONE" << endl;
00247                 cout << "writing image" << endl;
00248                 QImage img=pixmap.convertToImage();
00249                 char ofilename[MAX_FILENAME];
00250                 sprintf(ofilename,"%s-%.4d.%s",filename, frame, format);
00251                 cout << ofilename << endl;
00252                 img.save(QString(ofilename), format,0);
00253                 frame++;
00254                 
00255         }
00256         cout << "For Cyrill: \"The Evil is Outside\"" << endl;
00257 }
00258 


openslam_gmapping
Author(s): Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard
autogenerated on Thu Jun 6 2019 19:25:12