gfs2img.cpp
Go to the documentation of this file.
1 #include <cstdlib>
2 #include <limits.h>
3 #include <scanmatcher/scanmatcher.h>
4 #include <gridfastslam/gfsreader.h>
5 #include <qpixmap.h>
6 #include <qpainter.h>
7 #include <qimage.h>
8 #include <qapplication.h>
9 #include <utils/commandline.h>
10 
11 #define MAX_LASER_BEAMS 1024
12 #define MAX_FILENAME 1024
13 
14 using namespace std;
15 using namespace GMapping;
16 using namespace GMapping::GFSReader;
17 
18 inline double min(double a, double b){
19  return (a<b)?a:b;
20 }
21 
22 inline double max(double a, double b){
23  return (a>b)?a:b;
24 }
25 
26 void computeBoundingBox(double& xmin, double& ymin, double& xmax, double& ymax, const LaserRecord& laser, const OrientedPoint& pose, double maxrange){
27  double theta=-M_PI/2+pose.theta;
28  double theta_step=(laser.readings.size()==180||laser.readings.size()==181)?M_PI/180:M_PI/360;
29  for (std::vector<double>::const_iterator it=laser.readings.begin(); it!=laser.readings.end(); it++){
30  if (*it<maxrange){
31  xmin=min(xmin,pose.x+*it*cos(theta));
32  ymin=min(ymin,pose.y+*it*sin(theta));
33  xmax=max(xmax,pose.x+*it*cos(theta));
34  ymax=max(ymax,pose.y+*it*sin(theta));
35  }
36  theta+=theta_step;
37  }
38 }
39 
40 void computeBoundingBox(double& xmin, double& ymin, double& xmax, double& ymax, const RecordList& rl, double maxrange){
41  xmin = ymin = MAXDOUBLE;
42  xmax = ymax =-MAXDOUBLE;
43  const LaserRecord* lastLaser=0;
44  for (RecordList::const_iterator it=rl.begin(); it!=rl.end(); it++){
45  const LaserRecord* lr= dynamic_cast<const LaserRecord*>(*it);
46  if (lr){
47  lastLaser=lr;
48  continue;
49  }
50  const ScanMatchRecord* smr= dynamic_cast<const ScanMatchRecord*>(*it);
51  if (smr && lastLaser){
52  for (std::vector<OrientedPoint>::const_iterator pit=smr->poses.begin(); pit!=smr->poses.end(); pit++){
53  computeBoundingBox(xmin, ymin, xmax, ymax, *lastLaser, *pit, maxrange);
54  }
55  }
56  }
57 }
58 int main(int argc, char** argv){
59  QApplication app(argc, argv);
60  double maxrange=50;
61  double delta=0.1;
62  int scanSkip=5;
63  const char* filename=0;
64  const char* format="PNG";
65  CMD_PARSE_BEGIN(1, argc)
66  parseDouble("-maxrange", maxrange);
67  parseDouble("-delta", delta);
68  parseInt("-skip", scanSkip);
69  parseString("-filename",filename);
70  parseString("-format",format);
71  CMD_PARSE_END
72 
73  double maxUrange=maxrange;
74  if (! filename){
75  cout << " supply a gfs file, please" << endl;
76  cout << " usage gfs2img [options] -filename <gfs_file>" << endl;
77  cout << " [options]:" << endl;
78  cout << " -maxrange <range>" << endl;
79  cout << " -delta <map cell size>" << endl;
80  cout << " -skip <frames to skip among images>" << endl;
81  cout << " -format <image format in capital letters>" << endl;
82  return -1;
83  }
84  ifstream is(filename);
85  if (!is){
86  cout << " supply an EXISTING gfs file, please" << endl;
87  return -1;
88  }
89  RecordList rl;
90  rl.read(is);
91 
92  int particles=0;
93  int beams=0;
94  for (RecordList::const_iterator it=rl.begin(); it!=rl.end(); it++){
95  const OdometryRecord* odometry=dynamic_cast<const OdometryRecord*>(*it);
96  if (odometry){
97  particles=odometry->dim;
98  }
99  const LaserRecord* s=dynamic_cast<const LaserRecord*>(*it);
100  if (s){
101  beams=s->readings.size();
102  }
103  if (particles && beams)
104  break;
105  }
106  cout << "Particles from gfs=" << particles << endl;
107  if (! particles){
108  cout << "no particles found, terminating" << endl;
109  return -1;
110  }
111  cout << "Laser beams from gfs=" << beams << endl;
112  if (! beams){
113  cout << "0 beams found, terminating" << endl;
114  return -1;
115  }
116 
117 
118  double laserBeamStep=0;
119  if (beams==180||beams==181){
120  laserBeamStep=M_PI/180;
121  } else if (beams==360||beams==361){
122  laserBeamStep=M_PI/360;
123  }
124  cout << "Laser beam step" << laserBeamStep << endl;
125  if (laserBeamStep==0){
126  cout << "Invalid Beam Step, terminating" << endl;
127  return -1;
128  }
129  double laserAngles[MAX_LASER_BEAMS];
130  double theta=-M_PI/2;
131  for (int i=0; i<beams; i++){
132  laserAngles[i]=theta;
133  theta+=laserBeamStep;
134  }
135 
136  ScanMatcher matcher;
137  matcher.setLaserParameters(beams, laserAngles, OrientedPoint(0,0,0));
138  matcher.setlaserMaxRange(maxrange);
139  matcher.setusableRange(maxUrange);
140  matcher.setgenerateMap(true);
141 
142  double xmin, ymin, xmax, ymax;
143  cout << "computing bounding box" << endl;
144  computeBoundingBox(xmin, ymin, xmax, ymax, rl, maxrange);
145  cout << "DONE" << endl << "BBOX= " << xmin << " " << ymin << " " << xmax << " " << ymax << endl;
146 
147  Point center;
148  center.x=(xmin+xmax)/2.;
149  center.y=(ymin+ymax)/2.;
150 
151  cout << "computing paths" << endl;
152  unsigned int frame=0;
153  int scanCount=0;
154 
155  for (RecordList::const_iterator it=rl.begin(); it!=rl.end(); it++){
156  const ScanMatchRecord* s=dynamic_cast<const ScanMatchRecord*>(*it);
157  if (!s)
158  continue;
159  scanCount++;
160  if (scanCount%scanSkip)
161  continue;
162  cout << "Frame " << frame << " ";
163  std::vector<RecordList> paths(particles);
164  int bestIdx=0;
165  double bestWeight=-MAXDOUBLE;
166  for (int p=0; p<particles; p++){
167  paths[p]=rl.computePath(p,it);
168  double w=rl.getLogWeight(p,it);
169  if (w>bestWeight){
170  bestWeight=w;
171  bestIdx=p;
172  }
173  }
174  cout << "bestIdx=" << bestIdx << " bestWeight=" << bestWeight << endl;
175 
176  cout << "computing best map" << endl;
177  ScanMatcherMap smap(center, xmin, ymin, xmax, ymax, delta);
178  int count=0;
179  for (RecordList::const_iterator mt=paths[bestIdx].begin(); mt!=paths[bestIdx].end(); mt++){
180  const LaserRecord* s=dynamic_cast<const LaserRecord*>(*mt);
181  if (s){
182  double rawreadings[MAX_LASER_BEAMS];
183  for (uint i=0; i<s->readings.size(); i++)
184  rawreadings[i]=s->readings[i];
185  matcher.invalidateActiveArea();
186  matcher.computeActiveArea(smap, s->pose, rawreadings);
187 // matcher.allocActiveArea(smap, s->pose, rawreadings);
188  matcher.registerScan(smap, s->pose, rawreadings);
189  count++;
190  }
191  }
192  cout << "DONE " << count <<endl;
193 
194  QPixmap pixmap(smap.getMapSizeX(), smap.getMapSizeY());
195  pixmap.fill(QColor(200, 200, 255));
196  QPainter painter(&pixmap);
197  for (int x=0; x<smap.getMapSizeX(); x++)
198  for (int y=0; y<smap.getMapSizeY(); y++){
199  double v=smap.cell(x,y);
200  if (v>=0){
201  int grayValue=255-(int)(255.*v);
202  painter.setPen(QColor(grayValue, grayValue, grayValue));
203  painter.drawPoint(x,smap.getMapSizeY()-y-1);
204  }
205  }
206 
207  /*
208  cout << "painting trajectories" << endl;
209  for (int p=0; p<particles; p++){
210  painter.setPen(QColor(Qt::red));
211  if (p==bestIdx)
212  continue;
213  bool first=true;
214  IntPoint oldPoint(0,0);
215  for (RecordList::const_iterator mt=paths[p].begin(); mt!=paths[p].end(); mt++){
216  const LaserRecord* s=dynamic_cast<const LaserRecord*>(*mt);
217  if (s){
218  IntPoint ip=smap.world2map(s->pose);
219  ip.y=smap.getMapSizeY()-ip.y-1;
220  if (!first){
221  painter.drawLine( oldPoint.x, oldPoint.y, ip.x, ip.y);
222  }
223  oldPoint=ip;
224  first=false;
225  }
226  }
227  paths[p].destroyReferences();;
228  }
229  painter.setPen(QColor(Qt::black));
230  bool first=true;
231  IntPoint oldPoint(0,0);
232  for (RecordList::const_iterator mt=paths[bestIdx].begin(); mt!=paths[bestIdx].end(); mt++){
233  const LaserRecord* s=dynamic_cast<const LaserRecord*>(*mt);
234  if (s){
235  IntPoint ip=smap.world2map(s->pose);
236  ip.y=smap.getMapSizeY()-ip.y-1;
237  if (!first){
238  painter.drawLine( oldPoint.x, oldPoint.y, ip.x, ip.y);
239  }
240  oldPoint=ip;
241  first=false;
242  }
243  }
244  paths[bestIdx].destroyReferences();;
245  */
246  cout << " DONE" << endl;
247  cout << "writing image" << endl;
248  QImage img=pixmap.convertToImage();
249  char ofilename[MAX_FILENAME];
250  sprintf(ofilename,"%s-%.4d.%s",filename, frame, format);
251  cout << ofilename << endl;
252  img.save(QString(ofilename), format,0);
253  frame++;
254 
255  }
256  cout << "For Cyrill: \"The Evil is Outside\"" << endl;
257 }
258 
delta
const char *const *argv double delta
Definition: gfs2stream.cpp:19
GMapping::ScanMatcher::registerScan
double registerScan(ScanMatcherMap &map, const OrientedPoint &p, const double *readings)
Definition: scanmatcher.cpp:215
GMapping::Map::cell
Cell & cell(int x, int y)
Definition: map.h:46
MAX_FILENAME
#define MAX_FILENAME
Definition: gfs2img.cpp:12
GMapping::GFSReader::RecordList::read
istream & read(istream &is)
Definition: gfsreader.cpp:198
main
int main(int argc, char **argv)
Definition: gfs2img.cpp:58
CMD_PARSE_END
CMD_PARSE_END
Definition: gfs2stream.cpp:32
parseDouble
#define parseDouble(name, value)
Definition: commandline.h:44
parseInt
#define parseInt(name, value)
Definition: commandline.h:52
maxrange
double maxrange
Definition: gfs2stream.cpp:22
GMapping::ScanMatcher::setLaserParameters
void setLaserParameters(unsigned int beams, double *angles, const OrientedPoint &lpose)
Definition: scanmatcher.cpp:559
computeBoundingBox
void computeBoundingBox(double &xmin, double &ymin, double &xmax, double &ymax, const LaserRecord &laser, const OrientedPoint &pose, double maxrange)
Definition: gfs2img.cpp:26
parseString
#define parseString(name, value)
Definition: commandline.h:35
GMapping::Map::getMapSizeX
int getMapSizeX() const
Definition: map.h:36
rl
RecordList rl
Definition: gfs2stream.cpp:60
GMapping::GFSReader::ScanMatchRecord::poses
vector< OrientedPoint > poses
Definition: gfsreader.h:68
GMapping::GFSReader::RecordList::getLogWeight
double getLogWeight(unsigned int i) const
Definition: gfsreader.cpp:251
GMapping::GFSReader::RecordList::computePath
RecordList computePath(unsigned int i, RecordList::const_iterator frame) const
Definition: gfsreader.cpp:328
min
double min(double a, double b)
Definition: gfs2img.cpp:18
MAXDOUBLE
#define MAXDOUBLE
Definition: gvalues.h:4
GMapping::Map::getMapSizeY
int getMapSizeY() const
Definition: map.h:37
is
ifstream is(argv[c])
max
double max(double a, double b)
Definition: gfs2img.cpp:22
MAX_LASER_BEAMS
#define MAX_LASER_BEAMS
Definition: gfs2img.cpp:11
GMapping::ScanMatcher::computeActiveArea
void computeActiveArea(ScanMatcherMap &map, const OrientedPoint &p, const double *readings)
Definition: scanmatcher.cpp:116
GMapping::OrientedPoint
orientedpoint< double, double > OrientedPoint
Definition: point.h:203
CMD_PARSE_BEGIN
CMD_PARSE_BEGIN(1, argc-2)

openslam_gmapping
Author(s): Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard
autogenerated on Mon Jun 10 2019 14:04:22