log_plot.cpp
Go to the documentation of this file.
00001 #include <cstdlib>
00002 #include <fstream>
00003 #include <iostream>
00004 #include <sys/types.h>
00005 #include <log/carmenconfiguration.h>
00006 #include <log/sensorlog.h>
00007 
00008 
00009 using namespace std;
00010 using namespace GMapping;
00011 
00012 int main(int argc, char ** argv){
00013   double maxrange=2.;
00014         if (argc<2){
00015                 cout << "usage log_plot <filename> | gnuplot" << endl;
00016                 exit (-1);
00017         }
00018         ifstream is(argv[1]);
00019         if (! is){
00020                 cout << "no file " << argv[1] << " found" << endl;
00021                 exit (-1);
00022         }
00023         CarmenConfiguration conf;
00024         conf.load(is);
00025         
00026         SensorMap m=conf.computeSensorMap();
00027         
00028         //for (SensorMap::const_iterator it=m.begin(); it!=m.end(); it++)
00029         //      cout << it->first << " " << it->second->getName() << endl;
00030         
00031         SensorLog log(m);
00032         is.close();
00033         
00034         ifstream ls(argv[1]);
00035         log.load(ls);
00036         ls.close();
00037         int count=0;
00038         int frame=0;
00039         cerr << "log size" << log.size() << endl;
00040         for (SensorLog::iterator it=log.begin(); it!=log.end(); it++){
00041                 RangeReading* rr=dynamic_cast<RangeReading*>(*it);
00042                 if (rr){
00043                   count++;
00044                   if (count%3)
00045                     continue;
00046                   std::vector<Point> points(rr->size());
00047                   uint j=0;
00048                   for (uint i=0; i<rr->size(); i++){
00049                     const RangeSensor * rs=dynamic_cast<const RangeSensor*>(rr->getSensor());
00050                     double c=rs->beams()[i].c, s=rs->beams()[i].s;
00051                     double r=(*rr)[i];
00052                     if (r>maxrange)
00053                       continue;
00054                     points[j++]=Point(r*c,r*s);
00055                   }
00056                   if (j){
00057                     char buf[1024];
00058                     sprintf(buf,"frame-%05d.gif",frame);
00059                     frame++;
00060                     cout << "set terminal gif" << endl;
00061                     cout << "set output \"" << buf << "\"" << endl;
00062                     cout << "set size ratio -1" << endl;
00063                     cout << "plot [-3:3][0:3] '-' w p ps 1" << endl;
00064                     for (uint i=0; i<j; i++){
00065                       cout << points[i].y << " " << points[i].x << endl;
00066                   }
00067                   cout << "e" << endl;
00068                   }
00069                 }
00070         }
00071 }


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