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