scanmatch_test.cpp
Go to the documentation of this file.
1 #include <cstdlib>
2 #include <fstream>
3 #include <iostream>
5 #include <log/sensorlog.h>
6 #include <unistd.h>
7 #include <utils/commandline.h>
8 #include <log/sensorstream.h>
9 #include "scanmatcherprocessor.h"
10 
11 using namespace std;
12 using namespace GMapping;
13 
14 #define DEBUG cout << __PRETTY_FUNCTION__
15 #define MAX_STRING_LENGTH 1024
16 
17 int main(int argc, const char * const * argv){
18  string filename;
19  string outfilename;
20  double xmin=-100.;
21  double ymin=-100.;
22  double xmax=100.;
23  double ymax=100.;
24  double delta=1.;
25  double patchDelta=0.1;
26  double sigma=0.02;
27  double maxrange=81.9;
28  double maxUrange=81.9;
29  double regscore=1e4;
30  double lstep=.05;
31  double astep=.05;
32  int kernelSize=0;
33  int iterations=4;
34  double critscore=0.;
35  double maxMove=1.;
36  bool computeCovariance=false;
37  bool readFromStdin=false;
38  bool useICP=false;
39  double laserx=.0,lasery=.0,lasertheta=.0;
40 // bool headingOnly=false;
41 
42 
43  if (argc<2){
44  cout << "usage main {arglist}" << endl;
45  cout << "where the arguments are: " << endl;
46  cout << "\t -xmin <value>" << endl;
47  cout << "\t -xmax <value>" << endl;
48  cout << "\t -ymin <value>" << endl;
49  cout << "\t -ymax <value>" << endl;
50  cout << "\t -maxrange <value> : maxmimum preception range" << endl;
51  cout << "\t -delta <value> : patch size" << endl;
52  cout << "\t -patchDelta <value> : patch cell size" << endl;
53  cout << "\t -lstep <value> : linear serach step" << endl;
54  cout << "\t -astep <value> : ├Čangular search step" << endl;
55  cout << "\t -regscore <value> : registration scan score" << endl;
56  cout << "\t -filename <value> : log filename in carmen format" << endl;
57  cout << "\t -sigma <value> : convolution kernel size" << endl;
58  cout << "Look the code for discovering another thousand of unuseful parameters" << endl;
59  return -1;
60  }
61 
62  CMD_PARSE_BEGIN(1,argc);
63  parseString("-filename",filename);
64  parseString("-outfilename",outfilename);
65  parseDouble("-xmin",xmin);
66  parseDouble("-xmax",xmax);
67  parseDouble("-ymin",ymin);
68  parseDouble("-ymax",ymax);
69  parseDouble("-delta",delta);
70  parseDouble("-patchDelta",patchDelta);
71  parseDouble("-maxrange",maxrange);
72  parseDouble("-maxUrange",maxUrange);
73  parseDouble("-regscore",regscore);
74  parseDouble("-critscore",critscore);
75  parseInt("-kernelSize",kernelSize);
76  parseDouble("-sigma",sigma);
77  parseInt("-iterations",iterations);
78  parseDouble("-lstep",lstep);
79  parseDouble("-astep",astep);
80  parseDouble("-maxMove",maxMove);
81  parseFlag("-computeCovariance",computeCovariance);
82  parseFlag("-stdin", readFromStdin);
83  parseFlag("-useICP", useICP);
84  parseDouble("-laserx",laserx);
85  parseDouble("-lasery",lasery);
86  parseDouble("-lasertheta",lasertheta);
88 
89  if (!filename.size()){
90  cout << "no filename specified" << endl;
91  return -1;
92  }
93 
94  ifstream is;
95  is.open(filename.c_str());
96  if (! is){
97  cout << "no file found" << endl;
98  return -1;
99  }
100 
101 
102  DEBUG << "scanmatcher processor construction" << endl;
103  ScanMatcherProcessor scanmatcher(xmin, ymin, xmax, ymax, delta, patchDelta);
104 
105  //double range, double sigma, int kernsize, double lopt, double aopt, int iterations
106  scanmatcher.setMatchingParameters(maxUrange, maxrange, sigma, kernelSize, lstep, astep, iterations, computeCovariance);
107  scanmatcher.setRegistrationParameters(regscore, critscore);
108  scanmatcher.setmaxMove(maxMove);
109  scanmatcher.useICP=useICP;
110  scanmatcher.matcher().setlaserPose(OrientedPoint(laserx,lasery,lasertheta));
111 
112  CarmenConfiguration conf;
113  conf.load(is);
114  is.close();
115 
116  SensorMap sensorMap=conf.computeSensorMap();
117  scanmatcher.setSensorMap(sensorMap);
118 
119  InputSensorStream* input=0;
120 
121  ifstream plainStream;
122  if (! readFromStdin){
123  plainStream.open(filename.c_str());
124  input=new InputSensorStream(sensorMap, plainStream);
125  cout << "Plain Stream opened="<< (bool) plainStream << endl;
126  } else {
127  input=new InputSensorStream(sensorMap, cin);
128  cout << "Plain Stream opened on stdin" << endl;
129  }
130 
131 /*
132  SensorLog log(sensorMap);
133  ifstream logstream(filename);
134  log.load(logstream);
135  logstream.close();
136  cout << "Log loaded " << log.size() << " records" << endl;
137 */
138  ostream* output;
139  ofstream poseStream;
140  if (! readFromStdin){
141  if (! outfilename.size()){
142  outfilename=string("scanmatched")+filename;
143  }
144  poseStream.open(outfilename.c_str());
145  output=&poseStream;
146  } else {
147  output=&cout;
148  }
149  scanmatcher.init();
150  ofstream odopathStream("odopath.dat");
151  while (*input){
152  const SensorReading* r;
153  (*input) >> r;
154  if (! r)
155  continue;
156  const RangeReading* rr=dynamic_cast<const RangeReading*>(r);
157  if (rr){
158  const RangeSensor* s=dynamic_cast<const RangeSensor*>(r->getSensor());
159  bool isFront= s->getPose().theta==0;
160 
161  if (! readFromStdin){
162  cout << "." << flush;
163  }
164  const RangeSensor* rs=dynamic_cast<const RangeSensor*>(rr->getSensor());
165  assert (rs && rs->beams().size()==rr->size());
166  odopathStream << rr->getPose().x << " " << rr->getPose().y << endl;
167  scanmatcher.processScan(*rr);
168  OrientedPoint p=scanmatcher.getPose();
169  if (isFront)
170  *output << "FLASER "<< rr->size() << " ";
171  else
172  *output << "RLASER "<< rr->size() << " ";
173  for (RangeReading::const_iterator b=rr->begin(); b!=rr->end(); b++){
174  *output << *b << " ";
175  }
176  *output << p.x << " " << p.y << " " << p.theta << " ";
177  //p=rr->getPose();
178  double t=rr->getTime(); //FIXME
179  *output << p.x << " " << p.y << " " << p.theta << " ";
180  *output << t << " nohost " << t << endl;
181  }
182  }
183  if (! readFromStdin){
184  poseStream.close();
185  }
186 }
const char *const *argv double delta
Definition: gfs2stream.cpp:19
#define parseFlag(name, value)
Definition: commandline.h:28
const Sensor * getSensor() const
Definition: sensorreading.h:13
void setSensorMap(const SensorMap &smap, std::string sensorName="FLASER")
virtual SensorMap computeSensorMap() const
void setRegistrationParameters(double regScore, double critScore)
void setMatchingParameters(double urange, double range, double sigma, int kernsize, double lopt, double aopt, int iterations, bool computeCovariance=false)
virtual std::istream & load(std::istream &is)
CMD_PARSE_END
Definition: gfs2stream.cpp:32
#define parseDouble(name, value)
Definition: commandline.h:44
const OrientedPoint & getPose() const
Definition: rangereading.h:15
#define parseInt(name, value)
Definition: commandline.h:52
double maxrange
Definition: gfs2stream.cpp:22
#define parseString(name, value)
Definition: commandline.h:35
std::map< std::string, Sensor * > SensorMap
Definition: sensor.h:19
virtual void processScan(const RangeReading &reading)
int main(int argc, const char *const *argv)
double getTime() const
Definition: sensorreading.h:11
ifstream is(argv[c])
OrientedPoint getPose() const
Definition: rangesensor.h:25
#define DEBUG
const std::vector< Beam > & beams() const
Definition: rangesensor.h:23
orientedpoint< double, double > OrientedPoint
Definition: point.h:203
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