gfs2rec.cpp
Go to the documentation of this file.
1 #include <cstring>
2 #include <iostream>
3 #include <fstream>
4 #include <sstream>
5 #include <vector>
6 #include <list>
7 #include <gmapping/utils/point.h>
8 
9 #define MAX_LINE_LENGHT (1000000)
10 
11 using namespace GMapping;
12 using namespace std;
13 
14 struct Record{
15  unsigned int dim;
16  double time;
17  virtual ~Record(){}
18  virtual void read(istream& is)=0;
19  virtual void write(ostream& os){};
20 };
21 
22 struct CommentRecord: public Record{
23  string text;
24  virtual void read(istream& is){
25  char buf[MAX_LINE_LENGHT];
26  memset(buf,0, MAX_LINE_LENGHT*sizeof(char));
27  is.getline(buf, MAX_LINE_LENGHT);
28  text=string(buf);
29  }
30  virtual void write(ostream& os){
31  os << "#GFS_COMMENT: " << text << endl;
32  }
33 };
34 
35 struct PoseRecord: public Record{
36  PoseRecord(bool ideal=false){
37  truePos=ideal;
38  }
39  bool truePos;
41  void read(istream& is){
42  is >> pose.x >> pose.y >> pose.theta;
43  time = 0;
44  if (is)
45  is >> time;
46  }
47  virtual void write(ostream& os){
48  if (truePos)
49  os << "POS-CORR";
50  else
51  os << "POS ";
52  // FIXME os << floor(time) << " " << (int) (time-floor(time)*1e6) << ": ";
53  os << "0 0: ";
54  os << pose.x*100 << " " << pose.y*100 << " " << 180/M_PI*pose.theta << endl;
55  }
56 };
57 
58 struct NeffRecord: public Record{
59  double neff;
60  void read(istream& is){
61  is >> neff;
62  }
63  virtual void write(ostream& os){
64  os << "NEFF " << neff << endl;
65  }
66 };
67 
68 
69 struct OdometryRecord: public Record{
70  vector<OrientedPoint> poses;
71  virtual void read(istream& is){
72  is >> dim;
73  for (unsigned int i=0; i< dim; i++){
74  OrientedPoint p;
75  double w;
76  is >> p.x;
77  is >> p.y;
78  is >> p.theta;
79  is >> w;
80  poses.push_back(p);
81  }
82  time = 0;
83  if (is)
84  is >> time;
85  }
86 };
87 
88 
89 struct ScanMatchRecord: public Record{
90  vector<OrientedPoint> poses;
91  vector<double> weights;
92  virtual void read(istream& is){
93  is >> dim;
94  for (unsigned int i=0; i< dim; i++){
95  OrientedPoint p;
96  double w;
97  is >> p.x;
98  is >> p.y;
99  is >> p.theta;
100  is >> w;
101  poses.push_back(p);
102  weights.push_back(w);
103  }
104  }
105 };
106 
107 struct LaserRecord: public Record{
108  vector<double> readings;
110  virtual void read(istream& is){
111  is >> dim;
112  for (unsigned int i=0; i< dim; i++){
113  double r;
114  is >> r;
115  readings.push_back(r);
116  }
117  is >> pose.x;
118  is >> pose.y;
119  is >> pose.theta;
120  time = 0;
121  if (is)
122  is >> time;
123  }
124 
125  // dummy, &sec, &usec, &nLas, &nVal, &range ) == EOF) {
126 
127  virtual void write(ostream& os){
128  os << "POS ";
129  // FIXME os << floor(time) << " " << (int) (time-floor(time)*1e6) << ": ";
130  os << "0 0: ";
131  os << pose.x*100 << " " << pose.y*100 << " " << 180/M_PI*pose.theta << endl;
132 
133  os << "LASER-RANGE ";
134  // FIXME os << floor(time) << " " << (int) (time-floor(time)*1e6) << ": ";
135  os << " 0 0 0 " << dim << " 180. : ";
136  for (unsigned int i=0; i< dim; i++){
137  os <<" "<< readings[i]*100 ;
138  }
139  os << endl;
140  };
141 };
142 
143 struct ResampleRecord: public Record{
144  vector<unsigned int> indexes;
145  virtual void read(istream& is){
146  is >> dim;
147  for (unsigned int i=0; i< dim; i++){
148  unsigned int j;
149  is >> j;
150  indexes.push_back(j);
151  }
152  }
153 };
154 
155 struct RecordList: public list<Record*>{
156  mutable int sampleSize;
157 
158  istream& read(istream& is){
159  while(is){
160  char buf[8192];
161  is.getline(buf, 8192);
162  istringstream lineStream(buf);
163  string recordType;
164  lineStream >> recordType;
165  Record* rec=0;
166  if (recordType=="LASER_READING"){
167  rec=new LaserRecord;
168  cout << "l" << flush;
169  }
170  if (recordType=="ODO_UPDATE"){
171  rec=new OdometryRecord;
172  cout << "o" << flush;
173  }
174  if (recordType=="SM_UPDATE"){
175  rec=new ScanMatchRecord;
176  cout << "m" << flush;
177  }
178  if (recordType=="SIMULATOR_POS"){
179  rec=new PoseRecord(true);
180  cout << "t" << flush;
181  }
182  if (recordType=="RESAMPLE"){
183  rec=new ResampleRecord;
184  cout << "r" << flush;
185  }
186  if (recordType=="NEFF"){
187  rec=new NeffRecord;
188  cout << "n" << flush;
189  }
190  if (recordType=="COMMENT"){
191  rec=new CommentRecord;
192  cout << "c" << flush;
193  }
194  if (rec){
195  rec->read(lineStream);
196  push_back(rec);
197  }
198  }
199  return is;
200  }
201 
202  double getLogWeight(unsigned int i) const{
203  double weight=0;
204  unsigned int currentIndex=i;
205  for(RecordList::const_reverse_iterator it=rbegin(); it!=rend(); it++){
206  ScanMatchRecord* scanmatch=dynamic_cast<ScanMatchRecord*>(*it);
207  if (scanmatch){
208  weight+=scanmatch->weights[currentIndex];
209  }
210  ResampleRecord* resample=dynamic_cast<ResampleRecord*>(*it);
211  if (resample){
212  currentIndex=resample->indexes[currentIndex];
213  }
214  }
215  return weight;
216  }
217  unsigned int getBestIdx() const {
218  if (empty())
219  return 0;
220  const ScanMatchRecord* scanmatch=0;
221  const_reverse_iterator it=rbegin();
222  while(!scanmatch){
223  scanmatch=dynamic_cast<const ScanMatchRecord*>(*it);
224  it++;
225  }
226  unsigned int dim=scanmatch->dim;
227  sampleSize=(int)dim;
228  double bestw=-1e200;
229  unsigned int best=scanmatch->dim+1;
230  for (unsigned i=0; i<dim; i++){
231  double w=getLogWeight(i);
232  if (w>bestw){
233  best=i;
234  bestw=w;
235  }
236  }
237  return best;
238  }
239 
240  void printPath(ostream& os, unsigned int i, bool err=false) const{
241  unsigned int currentIndex=i;
242  OrientedPoint p(0,0,0);
243 
244  RecordList rl;
245 
246  //reconstruct a path
247  for(RecordList::const_reverse_iterator it=rbegin(); it!=rend(); it++){
248  const NeffRecord* neff=dynamic_cast<const NeffRecord*>(*it);
249  if (neff){
250  NeffRecord* n=new NeffRecord(*neff);
251  rl.push_front(n);
252  }
253  const ScanMatchRecord* scanmatch=dynamic_cast<const ScanMatchRecord*>(*it);
254  if (scanmatch){
255  PoseRecord* pose=new PoseRecord;
256  pose->dim=0;
257  p=pose->pose=scanmatch->poses[currentIndex];
258  rl.push_front(pose);
259  }
260  const OdometryRecord* odometry=dynamic_cast<const OdometryRecord*>(*it);
261  if (odometry){
262  PoseRecord* pose=new PoseRecord;
263  pose->dim=0;
264  p=pose->pose=odometry->poses[currentIndex];
265  pose->time=odometry->time;
266  rl.push_front(pose);
267  }
268  const PoseRecord* tpose=dynamic_cast<const PoseRecord*>(*it);
269  if (tpose){
270  PoseRecord* pose=new PoseRecord(*tpose);
271  rl.push_front(pose);
272  }
273  const LaserRecord* laser=dynamic_cast<const LaserRecord*>(*it);
274  if (laser){
275  LaserRecord* claser=new LaserRecord(*laser);
276  claser->pose=p;
277  rl.push_front(claser);
278  }
279  const CommentRecord* comment=dynamic_cast<const CommentRecord*>(*it);
280  if (comment){
281  CommentRecord* ccomment=new CommentRecord(*comment);
282  rl.push_front(ccomment);
283  }
284  const ResampleRecord* resample=dynamic_cast<const ResampleRecord*>(*it);
285  if (resample){
286  currentIndex=resample->indexes[currentIndex];
288  rl.push_front(r);
289  }
290  }
291  bool started=false;
292  double ox=0, oy=0, rxx=0, rxy=0, ryx=0, ryy=0, rth=0;
293  bool computedTransformation=false;
294  bool truePosFound=false;
295  OrientedPoint truePose(0,0,0);
296  OrientedPoint currPose(0,0,0);
297  bool tpf=false;
298  double neff=0;
299  unsigned int count=0;
300  for(RecordList::iterator it=rl.begin(); it!=rl.end(); it++){
301  NeffRecord* neffr=dynamic_cast<NeffRecord*>(*it);
302  if (neffr)
303  neff=neffr->neff/(double)sampleSize;
304  started=started || dynamic_cast<const LaserRecord*>(*it)?true:false;
305  if (started && ! truePosFound){
306  PoseRecord* tpose=dynamic_cast<PoseRecord*>(*it);
307  if (tpose && tpose->truePos){
308  truePosFound=true;
309  tpf=true;
310  truePose=tpose->pose;
311  os << "# ";
312  (*it)->write(os);
313  }
314  }
315  if (started && truePosFound && ! computedTransformation){
316  PoseRecord* pos=dynamic_cast<PoseRecord*>(*it);
317  if (pos && !pos->truePos){
318  OrientedPoint pose=pos->pose;
319  rth=truePose.theta-pose.theta;
320  double s=sin(rth), c=cos(rth);
321  rxx=ryy=c;
322  rxy=-s; ryx=s;
323  ox=truePose.x-(rxx*pose.x+rxy*pose.y);
324  oy=truePose.y-(ryx*pose.x+ryy*pose.y);
325  computedTransformation=true;
326  os << "# ";
327  (*it)->write(os);
328 
329  }
330  }
331  ResampleRecord* resample=dynamic_cast<ResampleRecord*>(*it);
332  if(resample){
333  os << "MARK-POS 0 0: " <<currPose.x*100 << " " << currPose.y*100 << " 0 " << count++ << endl;
334  }
335  if (computedTransformation){
336  PoseRecord* pos=dynamic_cast<PoseRecord*>(*it);
337  if (pos){
338  if (pos->truePos){
339  tpf=true;
340  truePose=pos->pose;
341  } else {
342  if (tpf){
343  tpf=false;
344  OrientedPoint pose=pos->pose;
345  double ex, ey, eth=truePose.theta-pose.theta-rth;
346  ex=truePose.x-(ox+rxx*pose.x+rxy*pose.y);
347  ey=truePose.y-(oy+ryx*pose.x+ryy*pose.y);
348  eth=atan2(sin(eth), cos(eth));
349  if (! err)
350  os << "# ERROR ";
351  os << neff << " "
352  << ex << " " << ey << " " << eth
353  << " " << sqrt(ex*ex+ey*ey) << " " << fabs(eth) << endl;
354  }
355  }
356  }
357 
358  }
359  PoseRecord* pos=dynamic_cast<PoseRecord*>(*it);
360  if (pos)
361  currPose=pos->pose;
362 
363  if (! err)
364  (*it)->write(os);
365  delete *it;
366  }
367  }
368 };
369 
370 
371 
372 int main (int argc, const char * const * argv){
373  if (argc<3){
374  cout << "usage gfs2rec [-err] <infilename> <outfilename>" << endl;
375  return -1;
376  }
377  bool err=0;
378  bool neff=0;
379  unsigned int c=1;
380  if (!strcmp(argv[c],"-err")){
381  err=true;
382  c++;
383  }
384  if (!strcmp(argv[c],"-neff")){
385  neff=true;
386  c++;
387  }
388  ifstream is(argv[c]);
389  if (!is){
390  cout << "could read file "<< endl;
391  return -1;
392  }
393  c++;
394  RecordList rl;
395  rl.read(is);
396  unsigned int bestidx=rl.getBestIdx();
397  cout << endl << "best index = " << bestidx<< endl;
398  ofstream os(argv[c]);
399  if (! os){
400  cout << "could write file "<< endl;
401  return -1;
402  }
403  rl.printPath(os,bestidx,err);
404  os.close();
405  return 0;
406 }
NeffRecord::read
void read(istream &is)
Definition: gfs2rec.cpp:60
RecordList::getBestIdx
unsigned int getBestIdx() const
Definition: gfs2rec.cpp:217
Record::write
virtual void write(ostream &os)
Definition: gfs2rec.cpp:19
NeffRecord::neff
double neff
Definition: gfs2rec.cpp:59
NeffRecord::write
virtual void write(ostream &os)
Definition: gfs2rec.cpp:63
c
unsigned int c
Definition: gfs2stream.cpp:41
ScanMatchRecord::poses
vector< OrientedPoint > poses
Definition: gfs2rec.cpp:90
point.h
ResampleRecord::indexes
vector< unsigned int > indexes
Definition: gfs2rec.cpp:144
Record::~Record
virtual ~Record()
Definition: gfs2rec.cpp:17
RecordList::printPath
void printPath(ostream &os, unsigned int i, bool err=false) const
Definition: gfs2rec.cpp:240
ScanMatchRecord::weights
vector< double > weights
Definition: gfs2rec.cpp:91
LaserRecord
Definition: gfs2rec.cpp:107
GMapping::main
int main(int argc, conat char **argv)
Definition: lumiles.h:45
is
ifstream is(argv[c])
Record::dim
unsigned int dim
Definition: gfs2rec.cpp:15
n
#define n
Definition: eig3.cpp:11
GMapping
Definition: configfile.cpp:34
ResampleRecord::read
virtual void read(istream &is)
Definition: gfs2rec.cpp:145
Record::read
virtual void read(istream &is)=0
rl
RecordList rl
Definition: gfs2stream.cpp:60
read
rl read(is)
PoseRecord::pose
OrientedPoint pose
Definition: gfs2rec.cpp:40
OdometryRecord
Definition: gfs2rec.cpp:69
Record::time
double time
Definition: gfs2rec.cpp:16
ResampleRecord
Definition: gfs2rec.cpp:143
RecordList::sampleSize
int sampleSize
Definition: gfs2rec.cpp:156
LaserRecord::pose
OrientedPoint pose
Definition: gfs2rec.cpp:109
LaserRecord::readings
vector< double > readings
Definition: gfs2rec.cpp:108
PoseRecord
Definition: gfs2rec.cpp:35
GMapping::point::y
T y
Definition: point.h:16
GMapping::orientedpoint::theta
A theta
Definition: point.h:60
CommentRecord
Definition: gfs2rec.cpp:22
RecordList::getLogWeight
double getLogWeight(unsigned int i) const
Definition: gfs2rec.cpp:202
Record
Definition: gfs2rec.cpp:14
PoseRecord::read
void read(istream &is)
Definition: gfs2rec.cpp:41
OdometryRecord::poses
vector< OrientedPoint > poses
Definition: gfs2rec.cpp:70
ScanMatchRecord::read
virtual void read(istream &is)
Definition: gfs2rec.cpp:92
PoseRecord::write
virtual void write(ostream &os)
Definition: gfs2rec.cpp:47
NeffRecord
Definition: gfs2rec.cpp:58
MAX_LINE_LENGHT
#define MAX_LINE_LENGHT
Definition: gfs2rec.cpp:9
neff
bool neff
Definition: gfs2stream.cpp:39
RecordList::read
istream & read(istream &is)
Definition: gfs2rec.cpp:158
ScanMatchRecord
Definition: gfs2rec.cpp:89
CommentRecord::read
virtual void read(istream &is)
Definition: gfs2rec.cpp:24
LaserRecord::write
virtual void write(ostream &os)
Definition: gfs2rec.cpp:127
CommentRecord::write
virtual void write(ostream &os)
Definition: gfs2rec.cpp:30
OdometryRecord::read
virtual void read(istream &is)
Definition: gfs2rec.cpp:71
CommentRecord::text
string text
Definition: gfs2rec.cpp:23
resample
void resample(std::vector< int > &indexes, const WeightVector &weights, unsigned int nparticles=0)
Definition: particlefilter.h:51
err
bool err
Definition: gfs2stream.cpp:38
RecordList
Definition: gfs2rec.cpp:155
GMapping::orientedpoint< double, double >
LaserRecord::read
virtual void read(istream &is)
Definition: gfs2rec.cpp:110
PoseRecord::PoseRecord
PoseRecord(bool ideal=false)
Definition: gfs2rec.cpp:36
bestidx
unsigned int bestidx
Definition: gfs2stream.cpp:62
PoseRecord::truePos
bool truePos
Definition: gfs2rec.cpp:39
GMapping::point::x
T x
Definition: point.h:16


openslam_gmapping
Author(s): Cyrill Stachniss, Udo Frese, Giorgio Grisetti, Wolfram Burgard
autogenerated on Thu Oct 19 2023 02:25:51