gfsreader.cpp
Go to the documentation of this file.
1 #include <cstring>
2 #include "gmapping/gridfastslam/gfsreader.h"
3 #include <iomanip>
4 #include <limits>
5 
6 namespace GMapping {
7 
8 namespace GFSReader{
9 
10 Record::~Record(){}
11 void Record::write(ostream& os){};
12 
13 void CommentRecord::read(istream& is){
14  char buf[MAX_LINE_LENGHT];
15  memset(buf,0, MAX_LINE_LENGHT*sizeof(char));
16  is.getline(buf, MAX_LINE_LENGHT);
17  text=string(buf);
18 }
19 
20 void CommentRecord::write(ostream& os){
21  os << "#GFS_COMMENT: " << text << endl;
22 }
23 
24 PoseRecord::PoseRecord(bool ideal){
25  truePos=ideal;
26 }
27 void PoseRecord::read(istream& is){
28  is >> pose.x >> pose.y >> pose.theta;
29  time = 0;
30  if (is)
31  is >> time;
32 }
33 void PoseRecord::write(ostream& os){
34  if (truePos)
35  os << "TRUEPOS ";
36  else
37  os << "ODOM ";
38  os << setiosflags(ios::fixed) << setprecision(6);
39  os << pose.x << " " << pose.y << " " << pose.theta << " 0 0 0 ";
40  os << time << " pippo " << time << endl;
41 }
42 
43 void NeffRecord::read(istream& is){
44  is >> neff;
45  time =0;
46  if (is)
47  is >> time;
48 
49 }
50 
51 void NeffRecord::write(ostream& os){
52  os << "NEFF " << neff ;
53  os << setiosflags(ios::fixed) << setprecision(6);
54  os << " " << time << " pippo " << time << endl;
55 }
56 
57 
58 void OdometryRecord::read(istream& is){
59  is >> dim;
60  for (unsigned int i=0; i< dim; i++){
61  OrientedPoint p;
62  double w;
63  is >> p.x;
64  is >> p.y;
65  is >> p.theta;
66  is >> w;
67  poses.push_back(p);
68  }
69  time = 0;
70  if (is)
71  is >> time;
72 }
73 
74 void RawOdometryRecord::read(istream& is){
75  is >> pose.x;
76  is >> pose.y;
77  is >> pose.theta;
78  time = 0;
79  assert(is);
80  is >> time;
81 
82 }
83 
84 
85 void EntropyRecord::read(istream& is){
86  is >> poseEntropy >> trajectoryEntropy >> mapEntropy;
87  time =0;
88  if (is)
89  is >> time;
90 }
91 
92 void EntropyRecord::write(ostream& os){
93  os << setiosflags(ios::fixed) << setprecision(6) << "ENTROPY " << poseEntropy << " " << trajectoryEntropy << " " << mapEntropy;
94  os << " " << time << " pippo " << time << endl;
95 }
96 
97 void ScanMatchRecord::read(istream& is){
98  is >> dim;
99  for (unsigned int i=0; i< dim; i++){
100  OrientedPoint p;
101  double w;
102  is >> p.x;
103  is >> p.y;
104  is >> p.theta;
105  is >> w;
106  poses.push_back(p);
107  weights.push_back(w);
108  }
109 }
110 
111 void LaserRecord::read(istream& is){
112  is >> dim;
113  for (unsigned int i=0; i< dim; i++){
114  double r;
115  is >> r;
116  readings.push_back(r);
117  }
118  is >> pose.x;
119  is >> pose.y;
120  is >> pose.theta;
121  time = 0;
122  if (is)
123  is >> time;
124 }
125 
126 void LaserRecord::write(ostream& os){
127  os << "WEIGHT " << weight << endl;
128  os << "ROBOTLASER1 ";
129 
130 
131  if ((dim == 541)||(dim == 540)) { // S300
132  os <<" 4"; // laser type
133  os <<" -2.351831"; // start_angle
134  os <<" 4.712389"; // fov
135  os <<" 0.008727"; // angular res
136  os <<" 30.0" ; // maxrange
137  }
138  else if ((dim == 180)||(dim == 181)) { // PLS
139  os <<" 0"; // laser type
140  os <<" -1.570796"; // start_angle
141  os <<" 3.141593"; // fov
142  os <<" 0.017453"; // angular res
143  os <<" 81.9" ; // maxrange
144  }
145  else if ((dim == 360)||(dim == 361)) { // LMS
146  os <<" 0"; // laser type
147  os <<" -1.570796"; // start_angle
148  os <<" 3.141593"; // fov
149  os <<" 0.008726"; // angular res
150  os <<" 81.9" ; // maxrange
151  }
152  else if ((dim == 682)||(dim == 683)) { // URG
153  os <<" 0"; // laser type
154  os <<" -2.094395"; // start_angle
155  os <<" 4.1887902"; // fov
156  os << " " << 360.0/1024.0/180.0*M_PI; // angular res
157  os <<" 5.5" ; // maxrange
158  }
159  else { // PLS
160  os <<" 0"; // laser type
161  os <<" -1.570796"; // start_angle
162  os <<" 3.141593"; // fov
163  os <<" 0.017453"; // angular res
164  os <<" 81.9" ; // maxrange
165  }
166  os <<" 0.01"; // accuracy
167  os <<" 0" ; // remission mode
168  os <<" "<< dim; // num readings
169  os << setiosflags(ios::fixed) << setprecision(2);
170  for (unsigned int i=0; i< dim; i++){
171  os <<" "<< readings[i] ;
172  }
173  os << setiosflags(ios::fixed) << setprecision(6);
174  os <<" 0"; // num remession values
175  os <<" "<< pose.x;
176  os <<" "<< pose.y;
177  os <<" "<< pose.theta;
178  os <<" "<< pose.x;
179  os <<" "<< pose.y;
180  os <<" "<< pose.theta;
181  os <<" 0" ; // tv
182  os <<" 0" ; // rv
183  os <<" 0.55" ; // forward_safety_dist
184  os <<" 0.375" ; // sideward_safety_dist
185  os <<" 1000000.0" ; // turn_axis
186  os <<" "<< time << " localhost " << time << endl;
187 };
188 
189 void ResampleRecord::read(istream& is){
190  is >> dim;
191  for (unsigned int i=0; i< dim; i++){
192  unsigned int j;
193  is >> j;
194  indexes.push_back(j);
195  }
196 }
197 
198 istream& RecordList::read(istream& is){
199  while(is){
200  char buf[MAX_LINE_LENGHT];
201  is.getline(buf, MAX_LINE_LENGHT);
202  istringstream lineStream(buf);
203  string recordType;
204  lineStream >> recordType;
205  Record* rec=0;
206  if (recordType=="LASER_READING"){
207  rec=new LaserRecord;
208 // cout << "l" << flush;
209  }
210  else if (recordType=="ODO_UPDATE"){
211  rec=new OdometryRecord;
212 // cout << "o" << flush;
213  }
214  else if (recordType=="ODOM"){
215  rec=new RawOdometryRecord;
216 // cout << "O" << flush;
217  }
218  else if (recordType=="SM_UPDATE"){
219  rec=new ScanMatchRecord;
220 // cout << "m" << flush;
221  }
222  else if (recordType=="SIMULATOR_POS"){
223  rec=new PoseRecord(true);
224 // cout << "t" << flush;
225  }
226  else if (recordType=="RESAMPLE"){
227  rec=new ResampleRecord;
228 // cout << "r" << flush;
229  }
230  else if (recordType=="NEFF"){
231  rec=new NeffRecord;
232 // cout << "n" << flush;
233  }
234  else if (recordType=="COMMENT" || recordType=="#COMMENT"){
235  rec=new CommentRecord;
236 // cout << "c" << flush;
237  }
238  else if (recordType=="ENTROPY"){
239  rec=new EntropyRecord;
240 // cout << "c" << flush;
241  }
242 
243  if (rec){
244  rec->read(lineStream);
245  push_back(rec);
246  }
247  }
248  return is;
249 }
250 
251 double RecordList::getLogWeight(unsigned int i) const{
252  double weight=0;
253  unsigned int currentIndex=i;
254  for(RecordList::const_reverse_iterator it=rbegin(); it!=rend(); it++){
255  ScanMatchRecord* scanmatch=dynamic_cast<ScanMatchRecord*>(*it);
256  if (scanmatch){
257  weight+=scanmatch->weights[currentIndex];
258  }
259  ResampleRecord* resample=dynamic_cast<ResampleRecord*>(*it);
260  if (resample){
261  currentIndex=resample->indexes[currentIndex];
262  }
263  }
264  return weight;
265 }
266 
267 double RecordList::getLogWeight(unsigned int i, RecordList::const_iterator frame) const{
268  double weight=0;
269  unsigned int currentIndex=i;
270  for(RecordList::const_reverse_iterator it(frame); it!=rend(); it++){
271  ScanMatchRecord* scanmatch=dynamic_cast<ScanMatchRecord*>(*it);
272  if (scanmatch){
273  weight+=scanmatch->weights[currentIndex];
274  }
275  ResampleRecord* resample=dynamic_cast<ResampleRecord*>(*it);
276  if (resample){
277  currentIndex=resample->indexes[currentIndex];
278  }
279  }
280  return weight;
281 }
282 
283 unsigned int RecordList::getBestIdx() const {
284  if (empty())
285  return 0;
286  const ScanMatchRecord* scanmatch=0;
287  const_reverse_iterator it=rbegin();
288  while(!scanmatch){
289  scanmatch=dynamic_cast<const ScanMatchRecord*>(*it);
290  it++;
291  }
292  unsigned int dim=scanmatch->dim;
293  sampleSize=(int)dim;
294  double bestw=-std::numeric_limits<double>::max();
295  unsigned int best=scanmatch->dim+1;
296  for (unsigned i=0; i<dim; i++){
297  double w=getLogWeight(i);
298  if (w>bestw){
299  best=i;
300  bestw=w;
301  }
302  }
303  return best;
304 }
305 
306 void RecordList::printLastParticles(ostream& os) const {
307  if (empty())
308  return;
309  const ScanMatchRecord* scanmatch=0;
310  const_reverse_iterator it=rbegin();
311  while(!scanmatch){
312  scanmatch=dynamic_cast<const ScanMatchRecord*>(*it);
313  it++;
314  }
315  if (! scanmatch)
316  return;
317  for (vector<OrientedPoint>::const_iterator it=scanmatch->poses.begin(); it!=scanmatch->poses.end(); it++){
318  os << "MARKER [color=black; circle=" << it->x*100 << "," << it->y*100 << ",10] 0 pippo 0" << endl;
319  }
320 }
321 
322 void RecordList::destroyReferences(){
323  for(RecordList::iterator it=begin(); it!=end(); it++)
324  delete (*it);
325 
326 }
327 
328 RecordList RecordList::computePath(unsigned int i, RecordList::const_iterator frame) const{
329  unsigned int currentIndex=i;
330  OrientedPoint p(0,0,0);
331  RecordList rl;
332 
333  //reconstruct a path
334  bool first=true;
335  for(RecordList::const_reverse_iterator it(frame); it!=rend(); it++){
336  const ScanMatchRecord* scanmatch=dynamic_cast<const ScanMatchRecord*>(*it);
337  if (scanmatch){
338  p=scanmatch->poses[currentIndex];
339  first=false;
340  }
341  const LaserRecord* laser=dynamic_cast<const LaserRecord*>(*it);
342  if (laser && !first){
343  LaserRecord* claser=new LaserRecord(*laser);
344  claser->pose=p;
345  rl.push_front(claser);
346  }
347  const ResampleRecord* resample=dynamic_cast<const ResampleRecord*>(*it);
348  if (resample){
349  currentIndex=resample->indexes[currentIndex];
350  }
351  }
352  return rl;
353 }
354 
355 
356 void RecordList::printPath(ostream& os, unsigned int i, bool err, bool rawodom) const{
357  unsigned int currentIndex=i;
358  OrientedPoint p(0,0,0);
359  RecordList rl;
360  double oldWeight=0;
361  double w=0;
362  //reconstruct a path
363  for(RecordList::const_reverse_iterator it=rbegin(); it!=rend(); it++){
364  const NeffRecord* neff=dynamic_cast<const NeffRecord*>(*it);
365  if (neff){
366  NeffRecord* n=new NeffRecord(*neff);
367  rl.push_front(n);
368  }
369  const EntropyRecord* entropy=dynamic_cast<const EntropyRecord*>(*it);
370  if (entropy){
371  EntropyRecord* n=new EntropyRecord(*entropy);
372  rl.push_front(n);
373  }
374  const ScanMatchRecord* scanmatch=dynamic_cast<const ScanMatchRecord*>(*it);
375  if (scanmatch){
376  PoseRecord* pose=new PoseRecord;
377  pose->dim=0;
378  p=pose->pose=scanmatch->poses[currentIndex];
379  w=scanmatch->weights[currentIndex]-oldWeight;
380  oldWeight=scanmatch->weights[currentIndex];
381 
382  if (!rawodom) {
383  rl.push_front(pose);
384  }
385  }
386  const OdometryRecord* odometry=dynamic_cast<const OdometryRecord*>(*it);
387  if (odometry){
388  PoseRecord* pose=new PoseRecord;
389  pose->dim=0;
390  p=pose->pose=odometry->poses[currentIndex];
391  pose->time=odometry->time;
392  if (!rawodom) {
393  rl.push_front(pose);
394  }
395  }
396  const RawOdometryRecord* rawodometry=dynamic_cast<const RawOdometryRecord*>(*it);
397  if (rawodometry){
398  PoseRecord* pose=new PoseRecord;
399  pose->dim=0;
400  pose->pose=rawodometry->pose;
401  pose->time=rawodometry->time;
402  if (rawodom) {
403  rl.push_front(pose);
404  }
405  }
406  const PoseRecord* tpose=dynamic_cast<const PoseRecord*>(*it);
407  if (tpose){
408  PoseRecord* pose=new PoseRecord(*tpose);
409  rl.push_front(pose);
410  }
411  const LaserRecord* laser=dynamic_cast<const LaserRecord*>(*it);
412  if (laser){
413  LaserRecord* claser=new LaserRecord(*laser);
414  claser->pose=p;
415  claser->weight=w;
416  rl.push_front(claser);
417  }
418  const CommentRecord* comment=dynamic_cast<const CommentRecord*>(*it);
419  if (comment){
420  CommentRecord* ccomment=new CommentRecord(*comment);
421  rl.push_front(ccomment);
422  }
423  const ResampleRecord* resample=dynamic_cast<const ResampleRecord*>(*it);
424  if (resample){
425  rl.push_front(new ResampleRecord(*resample));
426  currentIndex=resample->indexes[currentIndex];
427  }
428 
429  }
430  bool started=false;
431  bool computedTransformation=false;
432  bool truePosFound=false;
433  OrientedPoint truePose;
434  OrientedPoint oldPose;
435  OrientedPoint trueStart, realStart;
436  bool tpf=false;
437  double neff=0;
438  double totalError=0;
439  int count=0;
440  for(RecordList::iterator it=rl.begin(); it!=rl.end(); it++){
441  NeffRecord* neffr=dynamic_cast<NeffRecord*>(*it);
442  if (neffr)
443  neff=neffr->neff/(double)sampleSize;
444  started=started || dynamic_cast<const LaserRecord*>(*it)?true:false;
445  if (started && ! truePosFound){
446  PoseRecord* tpose=dynamic_cast<PoseRecord*>(*it);
447  if (tpose && tpose->truePos){
448  truePosFound=true;
449  tpf=true;
450  truePose=tpose->pose;
451  os << "# ";
452  (*it)->write(os);
453  }
454  }
455  if (started && truePosFound && ! computedTransformation){
456  PoseRecord* pos=dynamic_cast<PoseRecord*>(*it);
457  if (pos && !pos->truePos){
458  trueStart=truePose;
459  realStart=pos->pose;
460  os << "# ";
461  (*it)->write(os);
462  computedTransformation=true;
463  }
464  }
465  if (computedTransformation){
466  os << setiosflags(ios::fixed) << setprecision(6);
467  PoseRecord* pos=dynamic_cast<PoseRecord*>(*it);
468  if (pos){
469  if (pos->truePos){
470  tpf=true;
471  truePose=pos->pose;
472  } else {
473  if (tpf){
474  tpf=false;
475  OrientedPoint realDelta=absoluteDifference(pos->pose,realStart);
476  OrientedPoint trueDelta=absoluteDifference(truePose,trueStart);
477  double ex=realDelta.x-trueDelta.x;
478  double ey=realDelta.y-trueDelta.y;
479  double eth=realDelta.theta-trueDelta.theta;
480  eth=atan2(sin(eth), cos(eth));
481  if (! err)
482  os << "# ERROR ";
483  os << neff << " "
484  << ex << " " << ey << " " << eth
485  << " " << sqrt(ex*ex+ey*ey) << " " << fabs(eth) << endl;
486  totalError+=sqrt(ex*ex+ey*ey);
487  count++;
488  }
489  }
490  }
491 
492  }
493  PoseRecord* pos=dynamic_cast<PoseRecord*>(*it);
494  if (pos)
495  oldPose=pos->pose;
496  if (! err)
497  (*it)->write(os);
498  delete *it;
499  }
500  if (err)
501  cout << "average error" << totalError/count << endl;
502 }
503 
504 }; //gfsreader
505 
506 }; //GMapping;
neff
bool neff
Definition: gfs2stream.cpp:39
GMapping::GFSReader::OdometryRecord::read
virtual void read(istream &is)
Definition: gfsreader.cpp:58
GMapping::GFSReader::RecordList::getBestIdx
unsigned int getBestIdx() const
Definition: gfsreader.cpp:283
GMapping::GFSReader::RecordList::printPath
void printPath(ostream &os, unsigned int i, bool err=false, bool rawodom=false) const
Definition: gfsreader.cpp:356
err
bool err
Definition: gfs2stream.cpp:38
GMapping::GFSReader::PoseRecord::PoseRecord
PoseRecord(bool ideal=false)
Definition: gfsreader.cpp:24
GMapping::GFSReader::Record::read
virtual void read(istream &is)=0
GMapping::GFSReader::RecordList::read
istream & read(istream &is)
Definition: gfsreader.cpp:198
GMapping::GFSReader::Record::write
virtual void write(ostream &os)
Definition: gfsreader.cpp:11
GMapping::GFSReader::PoseRecord::write
virtual void write(ostream &os)
Definition: gfsreader.cpp:33
GMapping::GFSReader::ScanMatchRecord::read
virtual void read(istream &is)
Definition: gfsreader.cpp:97
GMapping::GFSReader::RecordList::printLastParticles
void printLastParticles(ostream &os) const
Definition: gfsreader.cpp:306
GMapping::GFSReader::CommentRecord::write
virtual void write(ostream &os)
Definition: gfsreader.cpp:20
resample
void resample(std::vector< int > &indexes, const WeightVector &weights, unsigned int nparticles=0)
Definition: particlefilter.h:51
GMapping::absoluteDifference
orientedpoint< T, A > absoluteDifference(const orientedpoint< T, A > &p1, const orientedpoint< T, A > &p2)
Definition: point.h:107
GMapping::GFSReader::EntropyRecord::write
virtual void write(ostream &os)
Definition: gfsreader.cpp:92
GMapping::GFSReader::OdometryRecord::poses
vector< OrientedPoint > poses
Definition: gfsreader.h:59
rl
RecordList rl
Definition: gfs2stream.cpp:60
GMapping::GFSReader::LaserRecord::read
virtual void read(istream &is)
Definition: gfsreader.cpp:111
GMapping::GFSReader::ScanMatchRecord::poses
vector< OrientedPoint > poses
Definition: gfsreader.h:69
MAX_LINE_LENGHT
#define MAX_LINE_LENGHT
Definition: gfs2log.cpp:10
GMapping::GFSReader::CommentRecord::read
virtual void read(istream &is)
Definition: gfsreader.cpp:13
GMapping::GFSReader::NeffRecord::write
virtual void write(ostream &os)
Definition: gfsreader.cpp:51
GMapping::GFSReader::ResampleRecord::read
virtual void read(istream &is)
Definition: gfsreader.cpp:189
is
ifstream is(argv[c])
GMapping::GFSReader::LaserRecord::write
virtual void write(ostream &os)
Definition: gfsreader.cpp:126
GMapping::GFSReader::ResampleRecord::indexes
vector< unsigned int > indexes
Definition: gfsreader.h:83
GMapping::GFSReader::RawOdometryRecord::read
virtual void read(istream &is)
Definition: gfsreader.cpp:74
max
double max(double a, double b)
Definition: gfs2img.cpp:22
n
#define n
Definition: eig3.cpp:11
GMapping::GFSReader::RecordList::computePath
RecordList computePath(unsigned int i, RecordList::const_iterator frame) const
Definition: gfsreader.cpp:328
GMapping::GFSReader::RecordList::getLogWeight
double getLogWeight(unsigned int i) const
Definition: gfsreader.cpp:251

openslam_gmapping
Author(s): Cyrill Stachniss, Udo Frese, Giorgio Grisetti, Wolfram Burgard
autogenerated on Mon Feb 28 2022 22:59:20