Go to the documentation of this file.
32 #define DEBUG cout << __func__
39 std::string configfilename;
40 std::string ebuf=
"not_set";
46 if (configfilename.length()>0){
49 filename = (std::string) cfg.
value(
"gfs",
"filename",filename);
50 outfilename = (std::string) cfg.
value(
"gfs",
"outfilename",outfilename);
51 xmin = cfg.
value(
"gfs",
"xmin", xmin);
52 xmax = cfg.
value(
"gfs",
"xmax",xmax);
53 ymin = cfg.
value(
"gfs",
"ymin",ymin);
54 ymax = cfg.
value(
"gfs",
"ymax",ymax);
57 maxUrange = cfg.
value(
"gfs",
"maxUrange",maxUrange);
58 regscore = cfg.
value(
"gfs",
"regscore",regscore);
59 critscore = cfg.
value(
"gfs",
"critscore",critscore);
60 kernelSize = cfg.
value(
"gfs",
"kernelSize",kernelSize);
61 sigma = cfg.
value(
"gfs",
"sigma",sigma);
62 iterations = cfg.
value(
"gfs",
"iterations",iterations);
63 lstep = cfg.
value(
"gfs",
"lstep",lstep);
64 astep = cfg.
value(
"gfs",
"astep",astep);
65 maxMove = cfg.
value(
"gfs",
"maxMove",maxMove);
66 srr = cfg.
value(
"gfs",
"srr", srr);
67 srt = cfg.
value(
"gfs",
"srt", srt);
68 str = cfg.
value(
"gfs",
"str", str);
69 stt = cfg.
value(
"gfs",
"stt", stt);
70 particles = cfg.
value(
"gfs",
"particles",particles);
71 angularUpdate = cfg.
value(
"gfs",
"angularUpdate", angularUpdate);
72 linearUpdate = cfg.
value(
"gfs",
"linearUpdate", linearUpdate);
73 lsigma = cfg.
value(
"gfs",
"lsigma", lsigma);
74 ogain = cfg.
value(
"gfs",
"lobsGain", ogain);
75 lskip = (int)cfg.
value(
"gfs",
"lskip", lskip);
76 mapUpdateTime = cfg.
value(
"gfs",
"mapUpdate", mapUpdateTime);
77 randseed = cfg.
value(
"gfs",
"randseed", randseed);
78 autosize = cfg.
value(
"gfs",
"autosize", autosize);
79 readFromStdin = cfg.
value(
"gfs",
"stdin", readFromStdin);
80 resampleThreshold = cfg.
value(
"gfs",
"resampleThreshold", resampleThreshold);
81 skipMatching = cfg.
value(
"gfs",
"skipMatching", skipMatching);
82 onLine = cfg.
value(
"gfs",
"onLine", onLine);
83 generateMap = cfg.
value(
"gfs",
"generateMap", generateMap);
84 m_minimumScore = cfg.
value(
"gfs",
"minimumScore", m_minimumScore);
85 llsamplerange = cfg.
value(
"gfs",
"llsamplerange", llsamplerange);
86 lasamplerange = cfg.
value(
"gfs",
"lasamplerange",lasamplerange );
87 llsamplestep = cfg.
value(
"gfs",
"llsamplestep", llsamplestep);
88 lasamplestep = cfg.
value(
"gfs",
"lasamplestep", lasamplestep);
89 linearOdometryReliability = cfg.
value(
"gfs",
"linearOdometryReliability",linearOdometryReliability);
90 angularOdometryReliability = cfg.
value(
"gfs",
"angularOdometryReliability",angularOdometryReliability);
91 ebuf = (std::string) cfg.
value(
"gfs",
"estrategy", ebuf);
92 considerOdometryCovariance = cfg.
value(
"gfs",
"considerOdometryCovariance",considerOdometryCovariance);
126 parseInt(
"-mapUpdate", mapUpdateTime);
130 parseDouble(
"-resampleThreshold", resampleThreshold);
131 parseFlag(
"-skipMatching", skipMatching);
139 parseDouble(
"-linearOdometryReliability",linearOdometryReliability);
140 parseDouble(
"-angularOdometryReliability",angularOdometryReliability);
143 parseFlag(
"-considerOdometryCovariance",considerOdometryCovariance);
146 if (filename.length() <=0){
147 cout <<
"no filename specified" << endl;
155 cout <<
" onLineProcessing" << endl;
162 is.open(filename.c_str());
164 cout <<
"no file found" << endl;
177 if (! readFromStdin){
178 plainStream.open(filename.c_str());
180 cout <<
"Plain Stream opened="<< (bool) plainStream << endl;
183 cout <<
"Plain Stream opened on stdin" << endl;
283 #ifdef CARMEN_SUPPORT
285 cout <<
"starting the process:" << endl;
286 CarmenWrapper::initializeIPC(gpt->
m_argv[0]);
287 CarmenWrapper::start(gpt->
m_argv[0]);
288 cout <<
"Waiting for retrieving the sensor map:" << endl;
289 while (! CarmenWrapper::sensorMapComputed()){
291 cout <<
"." << flush;
293 gpt->
sensorMap=CarmenWrapper::sensorMap();
294 cout <<
"Connected " << endl;
298 cout <<
"FATAL ERROR: cannot run online without the carmen support" << endl;
317 cout <<
"Error, cant autosize form stdin" << endl;
327 gpt->
infoStream() <<
" initialPose=" << initialPose.
x <<
" " << initialPose.
y <<
" " << initialPose.
theta
328 << cout <<
" xmin=" <<
xmin <<
" ymin=" <<
ymin <<
" xmax=" <<
xmax <<
" ymax=" <<
ymax << endl;
339 #define printParam(n)\
340 gpt->outputStream() \
343 << " " << gpt->n << endl
396 cerr <<
"cant open info stream for writing by unuseful debug messages" << endl;
402 #ifdef CARMEN_SUPPORT
403 list<RangeReading*> rrlist;
407 while (CarmenWrapper::getReading(rr)){
409 rrlist.push_back(nr);
415 ofstream rawpath(
"rawpath.dat");
419 (*(gpt->
input)) >> r;
425 assert (rs && rs->
beams().size()==rr->size());
430 cerr <<
"Retrieving state .. ";
432 cerr <<
"Done" << endl;
433 cerr <<
"Deleting Tree state .. ";
434 for (TNodeVector::iterator it=trajetories.begin(); it!=trajetories.end(); it++)
436 cerr <<
"Done" << endl;
458 cerr <<
"WRITING WEIGHTS" << endl;
460 for (TNodeVector::iterator it=trajetories.begin(); it!=trajetories.end(); it++){
462 sprintf(buf,
"w-%03d.dat",pnumber);
463 ofstream weightsStream(buf);
465 double oldWeight=0, oldgWeight=0;
467 double w=
n->weight-oldWeight;
468 double gw=
n->gweight-oldgWeight;
470 oldgWeight=
n->gweight;
471 weightsStream << w <<
" " << gw << endl;
474 weightsStream.close();
481 gpt->
infoStream() <<
"Hallo, I am the gsp thread. I have finished. Do you think it is the case of checking for unlocked mutexes." << endl;
487 std::vector<OrientedPoint> retval(
hypotheses);
494 std::vector<unsigned int> retval(
indexes);
508 cout <<
"PORCO CAZZO" << endl;
541 indexes=GridSlamProcessor::getIndexes();
559 unsigned int bestIdx=0;
560 double bestWeight=-1e1000;
565 if(
part->weightSum>bestWeight){
567 bestWeight=
part->weightSum;
581 event->
index=bestIdx;
std::ostream & infoStream()
double angularOdometryReliability
#define CMD_PARSE_BEGIN_SILENT(i, count)
#define parseString(name, value)
std::vector< OrientedPoint > hypotheses
std::vector< GridSlamProcessor::TNode * > TNodeVector
~GridSlamProcessorThread()
void setMatchingParameters(double urange, double range, double sigma, int kernsize, double lopt, double aopt, int iterations, double likelihoodSigma=1, double likelihoodGain=1, unsigned int likelihoodSkip=0)
OrientedPoint boundingBox(SensorLog *log, double &xmin, double &ymin, double &xmax, double &ymax) const
virtual void onScanmatchUpdate()
int init(int argc, const char *const *argv)
#define parseInt(name, value)
virtual SensorMap computeSensorMap() const
std::ofstream & outputStream()
const char *const * m_argv
virtual void syncOdometryUpdate()
const OrientedPoint & getPose() const
GridSlamProcessorThread()
double UTILS_EXPORT sampleGaussian(double sigma, unsigned long int S=0)
const std::vector< Beam > & beams() const
bool processScan(const RangeReading &reading, int adaptParticles=0)
#define parseDouble(name, value)
std::vector< unsigned int > indexes
unsigned int eventBufferLength
int loadFiles(const char *fn=0)
const char *const *argv double delta
#define CMD_PARSE_END_SILENT
bool considerOdometryCovariance
void setEventBufferSize(unsigned int length)
OrientedPoint boundingBox(double &xmin, double &ymin, double &xmax, double &ymax) const
CMD_PARSE_BEGIN(1, argc-2)
void processTruePos(const OdometryReading &odometry)
virtual void syncResampleUpdate()
void setUpdateDistances(double linear, double angular, double resampleThreshold)
std::istream & load(std::istream &is)
#define parseFlag(name, value)
std::vector< OrientedPoint > getHypotheses()
deque< Event * > EventDeque
const ParticleVector & getParticles() const
const Sensor * getSensor() const
const AutoVal & value(const std::string §ion, const std::string &entry) const
virtual void syncScanmatchUpdate()
static void * fastslamthread(GridSlamProcessorThread *gpt)
void setSensorMap(const SensorMap &smap)
virtual void onResampleUpdate()
Map< PointAccumulator, HierarchicalArray2D< PointAccumulator > > ScanMatcherMap
pthread_mutex_t hist_mutex
double linearOdometryReliability
const OrientedPoint & getPose() const
void setMotionModelParameters(double srr, double srt, double str, double stt)
InputSensorStream * input
pthread_mutex_t ind_mutex
#define parseStringSilent(name, value)
virtual std::istream & load(std::istream &is)
virtual void onOdometryUpdate()
unsigned int mapUpdateTime
std::vector< unsigned int > getIndexes()
std::vector< unsigned int > indexes
TNodeVector getTrajectories() const
std::vector< double > weightSums
openslam_gmapping
Author(s): Cyrill Stachniss, Udo Frese, Giorgio Grisetti, Wolfram Burgard
autogenerated on Thu Oct 19 2023 02:25:51