gsp_thread.cpp
Go to the documentation of this file.
00001 /*****************************************************************
00002  *
00003  * This file is part of the GMAPPING project
00004  *
00005  * GMAPPING Copyright (c) 2004 Giorgio Grisetti, 
00006  * Cyrill Stachniss, and Wolfram Burgard
00007  *
00008  * This software is licensed under the "Creative Commons 
00009  * License (Attribution-NonCommercial-ShareAlike 2.0)" 
00010  * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss, 
00011  * and Wolfram Burgard.
00012  * 
00013  * Further information on this license can be found at:
00014  * http://creativecommons.org/licenses/by-nc-sa/2.0/
00015  * 
00016  * GMAPPING is distributed in the hope that it will be useful,
00017  * but WITHOUT ANY WARRANTY; without even the implied 
00018  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
00019  * PURPOSE.  
00020  *
00021  *****************************************************************/
00022 
00023 
00024 #include "gsp_thread.h"
00025 #include <utils/commandline.h>
00026 #include <utils/stat.h>
00027 #include <configfile/configfile.h>
00028 
00029 #ifdef CARMEN_SUPPORT
00030         #include <carmenwrapper/carmenwrapper.h>
00031 #endif
00032 
00033 #define DEBUG cout << __PRETTY_FUNCTION__
00034 
00035 using namespace std;
00036 
00037 int GridSlamProcessorThread::init(int argc, const char * const * argv){
00038         m_argc=argc;
00039         m_argv=argv;
00040         std::string configfilename;
00041         std::string ebuf="not_set";
00042 
00043         CMD_PARSE_BEGIN_SILENT(1,argc);
00044                 parseStringSilent("-cfg",configfilename);
00045         CMD_PARSE_END_SILENT;
00046 
00047         if (configfilename.length()>0){
00048           ConfigFile cfg(configfilename);
00049 
00050           filename = (std::string) cfg.value("gfs","filename",filename);
00051           outfilename = (std::string) cfg.value("gfs","outfilename",outfilename);
00052           xmin = cfg.value("gfs","xmin", xmin);
00053           xmax = cfg.value("gfs","xmax",xmax);
00054           ymin = cfg.value("gfs","ymin",ymin);
00055           ymax = cfg.value("gfs","ymax",ymax);
00056           delta =  cfg.value("gfs","delta",delta);
00057           maxrange = cfg.value("gfs","maxrange",maxrange);
00058           maxUrange = cfg.value("gfs","maxUrange",maxUrange);
00059           regscore = cfg.value("gfs","regscore",regscore);
00060           critscore = cfg.value("gfs","critscore",critscore);
00061           kernelSize = cfg.value("gfs","kernelSize",kernelSize);
00062           sigma = cfg.value("gfs","sigma",sigma);
00063           iterations = cfg.value("gfs","iterations",iterations);
00064           lstep = cfg.value("gfs","lstep",lstep);
00065           astep = cfg.value("gfs","astep",astep);
00066           maxMove = cfg.value("gfs","maxMove",maxMove);
00067           srr = cfg.value("gfs","srr", srr);
00068           srt = cfg.value("gfs","srt", srt);
00069           str = cfg.value("gfs","str", str);
00070           stt = cfg.value("gfs","stt", stt);
00071           particles = cfg.value("gfs","particles",particles);
00072           angularUpdate = cfg.value("gfs","angularUpdate", angularUpdate);
00073           linearUpdate = cfg.value("gfs","linearUpdate", linearUpdate);
00074           lsigma = cfg.value("gfs","lsigma", lsigma);
00075           ogain = cfg.value("gfs","lobsGain", ogain);
00076           lskip = (int)cfg.value("gfs","lskip", lskip);
00077           mapUpdateTime = cfg.value("gfs","mapUpdate", mapUpdateTime);
00078           randseed = cfg.value("gfs","randseed", randseed);
00079           autosize = cfg.value("gfs","autosize", autosize);
00080           readFromStdin = cfg.value("gfs","stdin", readFromStdin);
00081           resampleThreshold = cfg.value("gfs","resampleThreshold", resampleThreshold);
00082           skipMatching = cfg.value("gfs","skipMatching", skipMatching);
00083           onLine = cfg.value("gfs","onLine", onLine);
00084           generateMap = cfg.value("gfs","generateMap", generateMap);
00085           m_minimumScore = cfg.value("gfs","minimumScore", m_minimumScore);
00086           llsamplerange = cfg.value("gfs","llsamplerange", llsamplerange);
00087           lasamplerange = cfg.value("gfs","lasamplerange",lasamplerange );
00088           llsamplestep = cfg.value("gfs","llsamplestep", llsamplestep);
00089           lasamplestep = cfg.value("gfs","lasamplestep", lasamplestep);
00090           linearOdometryReliability = cfg.value("gfs","linearOdometryReliability",linearOdometryReliability);
00091           angularOdometryReliability = cfg.value("gfs","angularOdometryReliability",angularOdometryReliability);
00092           ebuf = (std::string) cfg.value("gfs","estrategy", ebuf);
00093           considerOdometryCovariance = cfg.value("gfs","considerOdometryCovariance",considerOdometryCovariance);
00094           
00095         }
00096 
00097 
00098         CMD_PARSE_BEGIN(1,argc);
00099                 parseString("-cfg",configfilename);     /* to avoid the warning*/
00100                 parseString("-filename",filename);
00101                 parseString("-outfilename",outfilename);
00102                 parseDouble("-xmin",xmin);
00103                 parseDouble("-xmax",xmax);
00104                 parseDouble("-ymin",ymin);
00105                 parseDouble("-ymax",ymax);
00106                 parseDouble("-delta",delta);
00107                 parseDouble("-maxrange",maxrange);
00108                 parseDouble("-maxUrange",maxUrange);
00109                 parseDouble("-regscore",regscore);
00110                 parseDouble("-critscore",critscore);
00111                 parseInt("-kernelSize",kernelSize);
00112                 parseDouble("-sigma",sigma);
00113                 parseInt("-iterations",iterations);
00114                 parseDouble("-lstep",lstep);
00115                 parseDouble("-astep",astep);
00116                 parseDouble("-maxMove",maxMove);
00117                 parseDouble("-srr", srr);
00118                 parseDouble("-srt", srt);
00119                 parseDouble("-str", str);
00120                 parseDouble("-stt", stt);
00121                 parseInt("-particles",particles);
00122                 parseDouble("-angularUpdate", angularUpdate);
00123                 parseDouble("-linearUpdate", linearUpdate);
00124                 parseDouble("-lsigma", lsigma);
00125                 parseDouble("-lobsGain", ogain);
00126                 parseInt("-lskip", lskip);
00127                 parseInt("-mapUpdate", mapUpdateTime);
00128                 parseInt("-randseed", randseed);
00129                 parseFlag("-autosize", autosize);
00130                 parseFlag("-stdin", readFromStdin);
00131                 parseDouble("-resampleThreshold", resampleThreshold);
00132                 parseFlag("-skipMatching", skipMatching);
00133                 parseFlag("-onLine", onLine);
00134                 parseFlag("-generateMap", generateMap);
00135                 parseDouble("-minimumScore", m_minimumScore);
00136                 parseDouble("-llsamplerange", llsamplerange);
00137                 parseDouble("-lasamplerange", lasamplerange);
00138                 parseDouble("-llsamplestep", llsamplestep);
00139                 parseDouble("-lasamplestep", lasamplestep);
00140                 parseDouble("-linearOdometryReliability",linearOdometryReliability);
00141                 parseDouble("-angularOdometryReliability",angularOdometryReliability);
00142                 parseString("-estrategy", ebuf);
00143                 
00144                 parseFlag("-considerOdometryCovariance",considerOdometryCovariance);
00145         CMD_PARSE_END;
00146         
00147         if (filename.length() <=0){
00148                 cout << "no filename specified" << endl;
00149                 return -1;
00150         }
00151         return 0;
00152 }
00153 
00154 int GridSlamProcessorThread::loadFiles(const char * fn){
00155         if (onLine){
00156                 cout << " onLineProcessing" << endl;
00157                 return 0;
00158         }       
00159         ifstream is;
00160         if (fn)
00161           is.open(fn);
00162         else
00163           is.open(filename.c_str());
00164         if (! is){
00165                 cout << "no file found" << endl;
00166                 return -1;
00167         }
00168 
00169         CarmenConfiguration conf;
00170         conf.load(is);
00171         is.close();
00172 
00173         sensorMap=conf.computeSensorMap();
00174         
00175         if (input)
00176                 delete input;
00177         
00178         if (! readFromStdin){
00179                 plainStream.open(filename.c_str());
00180                 input=new InputSensorStream(sensorMap, plainStream);
00181                 cout << "Plain Stream opened="<< (bool) plainStream << endl;
00182         } else {
00183                 input=new InputSensorStream(sensorMap, cin);
00184                 cout << "Plain Stream opened on stdin" << endl;
00185         }
00186         return 0;
00187 }       
00188 
00189 GridSlamProcessorThread::GridSlamProcessorThread(): GridSlamProcessor(cerr){
00190         //This are the processor parameters
00191         filename="";
00192         outfilename="";
00193         xmin=-100.;
00194         ymin=-100.;
00195         xmax=100.;
00196         ymax=100.;
00197         delta=0.05;
00198         
00199         //scan matching parameters
00200         sigma=0.05;
00201         maxrange=80.;
00202         maxUrange=80.;
00203         regscore=1e4;
00204         lstep=.05;
00205         astep=.05;
00206         kernelSize=1;
00207         iterations=5;
00208         critscore=0.;
00209         maxMove=1.;
00210         lsigma=.075;
00211         ogain=3;
00212         lskip=0;
00213         autosize=false;
00214         skipMatching=false;
00215 
00216         //motion model parameters
00217         srr=0.1, srt=0.1, str=0.1, stt=0.1;
00218         //particle parameters
00219         particles=30;
00220         randseed=0;
00221         
00222         //gfs parameters
00223         angularUpdate=0.5;
00224         linearUpdate=1;
00225         resampleThreshold=0.5;
00226         
00227         input=0;
00228         
00229         pthread_mutex_init(&hp_mutex,0);
00230         pthread_mutex_init(&ind_mutex,0);
00231         pthread_mutex_init(&hist_mutex,0);
00232         running=false;
00233         eventBufferLength=0;
00234         mapUpdateTime=5;
00235         mapTimer=0;
00236         readFromStdin=false;
00237         onLine=false;
00238         generateMap=false;
00239 
00240         // This  are the dafault settings for a grid map of 5 cm
00241         llsamplerange=0.01;
00242         llsamplestep=0.01;
00243         lasamplerange=0.005;
00244         lasamplestep=0.005;
00245         linearOdometryReliability=0.;
00246         angularOdometryReliability=0.;
00247         
00248         considerOdometryCovariance=false;
00249 /*      
00250         // This  are the dafault settings for a grid map of 10 cm
00251         m_llsamplerange=0.1;
00252         m_llsamplestep=0.1;
00253         m_lasamplerange=0.02;
00254         m_lasamplestep=0.01;
00255 */      
00256         // This  are the dafault settings for a grid map of 20/25 cm
00257 /*
00258         m_llsamplerange=0.2;
00259         m_llsamplestep=0.1;
00260         m_lasamplerange=0.02;
00261         m_lasamplestep=0.01;
00262         m_generateMap=false;
00263 */
00264 
00265                         
00266 }
00267 
00268 GridSlamProcessorThread::~GridSlamProcessorThread(){
00269         pthread_mutex_destroy(&hp_mutex);
00270         pthread_mutex_destroy(&ind_mutex);
00271         pthread_mutex_destroy(&hist_mutex);
00272         
00273         for (deque<Event*>::const_iterator it=eventBuffer.begin(); it!=eventBuffer.end(); it++)
00274                 delete *it;
00275 }
00276         
00277 
00278 void * GridSlamProcessorThread::fastslamthread(GridSlamProcessorThread* gpt){
00279         if (! gpt->input && ! gpt->onLine)
00280                 return 0;
00281                 
00282         
00283         //if started online retrieve the settings from the connection
00284 #ifdef CARMEN_SUPPORT
00285         if (gpt->onLine){
00286                 cout << "starting the process:" << endl;
00287                 CarmenWrapper::initializeIPC(gpt->m_argv[0]);
00288                 CarmenWrapper::start(gpt->m_argv[0]);
00289                 cout << "Waiting for retrieving the sensor map:" << endl;
00290                 while (! CarmenWrapper::sensorMapComputed()){
00291                         usleep(500000);
00292                         cout << "." << flush;
00293                 }
00294                 gpt->sensorMap=CarmenWrapper::sensorMap();
00295                 cout << "Connected " << endl;
00296         }
00297 #else
00298         if (gpt->onLine){
00299                 cout << "FATAL ERROR: cannot run online without the carmen support" << endl;
00300                 DoneEvent *done=new DoneEvent;
00301                 gpt->addEvent(done);
00302                 return 0;
00303         }
00304 #endif
00305         
00306         gpt->setSensorMap(gpt->sensorMap);
00307         gpt->setMatchingParameters(gpt->maxUrange, gpt->maxrange, gpt->sigma, gpt->kernelSize, gpt->lstep, gpt->astep, gpt->iterations, gpt->lsigma, gpt->ogain, gpt->lskip);
00308         
00309         double xmin=gpt->xmin, 
00310                ymin=gpt->ymin, 
00311                xmax=gpt->xmax, 
00312                ymax=gpt->ymax;
00313         
00314         OrientedPoint initialPose(0,0,0);
00315         
00316         if (gpt->autosize){
00317                 if (gpt->readFromStdin || gpt->onLine)
00318                 cout << "Error, cant autosize form stdin" << endl;
00319                 SensorLog * log=new SensorLog(gpt->sensorMap);
00320                 ifstream is(gpt->filename.c_str());
00321                 log->load(is);
00322                 is.close();
00323                 initialPose=gpt->boundingBox(log, xmin, ymin, xmax, ymax);
00324                 delete log;
00325         }
00326         
00327         if( gpt->infoStream()){
00328                 gpt->infoStream() << " initialPose=" << initialPose.x << " " << initialPose.y << " " << initialPose.theta
00329                                 << cout << " xmin=" << xmin <<" ymin=" << ymin <<" xmax=" << xmax <<" ymax=" << ymax << endl;
00330         }
00331         gpt->setMotionModelParameters(gpt->srr, gpt->srt, gpt->str, gpt->stt);
00332         gpt->setUpdateDistances(gpt->linearUpdate, gpt->angularUpdate, gpt->resampleThreshold);
00333         gpt->setgenerateMap(gpt->generateMap);
00334         gpt->GridSlamProcessor::init(gpt->particles, xmin, ymin, xmax, ymax, gpt->delta, initialPose);
00335         gpt->setllsamplerange(gpt->llsamplerange);
00336         gpt->setllsamplestep(gpt->llsamplestep);
00337         gpt->setlasamplerange(gpt->llsamplerange);
00338         gpt->setlasamplestep(gpt->llsamplestep);
00339         
00340 #define printParam(n)\
00341          gpt->outputStream() \
00342          << "PARAM " << \
00343          #n \
00344          << " " << gpt->n << endl
00345         
00346         if (gpt->outfilename.length()>0 ){
00347                 gpt->outputStream().open(gpt->outfilename.c_str());
00348                 printParam(filename);
00349                 printParam(outfilename);
00350                 printParam(xmin);
00351                 printParam(ymin);
00352                 printParam(xmax);
00353                 printParam(ymax);
00354                 printParam(delta);
00355                 
00356                 //scan matching parameters
00357                 printParam(sigma);
00358                 printParam(maxrange);
00359                 printParam(maxUrange);
00360                 printParam(regscore);
00361                 printParam(lstep);
00362                 printParam(astep);
00363                 printParam(kernelSize);
00364                 printParam(iterations);
00365                 printParam(critscore);
00366                 printParam(maxMove);
00367                 printParam(lsigma);
00368                 printParam(ogain);
00369                 printParam(lskip);
00370                 printParam(autosize);
00371                 printParam(skipMatching);
00372         
00373                 //motion model parameters
00374                 printParam(srr);
00375                 printParam(srt); 
00376                 printParam(str);
00377                 printParam(stt);
00378                 //particle parameters
00379                 printParam(particles);
00380                 printParam(randseed);
00381                 
00382                 //gfs parameters
00383                 printParam(angularUpdate);
00384                 printParam(linearUpdate);
00385                 printParam(resampleThreshold);
00386                 
00387                 printParam(llsamplerange);
00388                 printParam(lasamplerange);
00389                 printParam(llsamplestep);
00390                 printParam(lasamplestep);
00391         }
00392         #undef printParam
00393         
00394         if (gpt->randseed!=0)
00395                 sampleGaussian(1,gpt->randseed);
00396         if (!gpt->infoStream()){
00397                 cerr << "cant open info stream for writing by unuseful debug messages" << endl;
00398         } else {
00399                 gpt->infoStream() << "setting randseed" << gpt->randseed<< endl;
00400         }
00401         
00402         
00403 #ifdef CARMEN_SUPPORT
00404         list<RangeReading*> rrlist;
00405         if (gpt->onLine){
00406                 RangeReading rr(0,0);
00407                 while (1){
00408                         while (CarmenWrapper::getReading(rr)){
00409                                 RangeReading* nr=new RangeReading(rr);
00410                                 rrlist.push_back(nr);
00411                                 gpt->processScan(*nr);
00412                         }
00413                 }
00414         }
00415 #endif
00416         ofstream rawpath("rawpath.dat");
00417         if (!gpt->onLine){
00418                 while(*(gpt->input) && gpt->running){
00419                         const SensorReading* r;
00420                         (*(gpt->input)) >> r;
00421                         if (! r)
00422                                 continue;
00423                         const RangeReading* rr=dynamic_cast<const RangeReading*>(r);
00424                         if (rr && gpt->running){
00425                                 const RangeSensor* rs=dynamic_cast<const RangeSensor*>(rr->getSensor());
00426                                 assert (rs && rs->beams().size()==rr->size());
00427                                 
00428                                 bool processed=gpt->processScan(*rr);
00429                                 rawpath << rr->getPose().x << " " << rr->getPose().y << " " << rr->getPose().theta << endl;
00430                                 if (0 && processed){
00431                                 cerr << "Retrieving state .. ";
00432                                         TNodeVector trajetories=gpt->getTrajectories();
00433                                         cerr << "Done" <<  endl;
00434                                         cerr << "Deleting Tree state .. ";
00435                                         for (TNodeVector::iterator it=trajetories.begin(); it!=trajetories.end(); it++)
00436                                                 delete *it;
00437                                         cerr << "Done" << endl;
00438                                 }
00439 //                              if (0 && processed){
00440 //                                      cerr << "generating copy" << endl;;
00441 //                                      GridSlamProcessor* m_gsp=gpt->clone();
00442 //                                      Map<double, DoubleArray2D, false>*  pmap=m_gsp->getParticles()[0].map.toDoubleMap() ;
00443 //                                      cerr << "deleting" << endl;
00444 //                                      delete m_gsp;
00445 //                                      delete pmap;
00446 //                              }
00447                         }
00448                         const OdometryReading* o=dynamic_cast<const OdometryReading*>(r);
00449                         if (o && gpt->running){
00450                                 gpt->processTruePos(*o);
00451                                 TruePosEvent* truepos=new TruePosEvent;
00452                                 truepos->pose=o->getPose();
00453                         }
00454                 }
00455         }
00456         rawpath.close();
00457 
00458         TNodeVector trajetories=gpt->getTrajectories();
00459         cerr << "WRITING WEIGHTS" << endl;
00460         int pnumber=0;
00461         for (TNodeVector::iterator it=trajetories.begin(); it!=trajetories.end(); it++){
00462                 char buf[10];
00463                 sprintf(buf, "w-%03d.dat",pnumber);
00464                 ofstream weightsStream(buf);
00465                 GridSlamProcessor::TNode* n=*it;
00466                 double oldWeight=0, oldgWeight=0;
00467                 while (n!=0){
00468                         double w=n->weight-oldWeight;
00469                         double gw=n->gweight-oldgWeight;
00470                         oldWeight=n->weight;
00471                         oldgWeight=n->gweight;
00472                         weightsStream << w << " " << gw << endl;
00473                         n=n->parent;
00474                 }
00475                 weightsStream.close();
00476                 pnumber++;
00477                 cerr << buf << endl;
00478         }
00479                 
00480         DoneEvent *done=new DoneEvent;
00481         gpt->addEvent(done);
00482         gpt->infoStream() << "Hallo, I am the gsp thread. I have finished. Do you think it is the case of checking for unlocked mutexes." << endl;
00483         return 0;
00484 }
00485 
00486 std::vector<OrientedPoint> GridSlamProcessorThread::getHypotheses(){
00487         pthread_mutex_lock(&hp_mutex);
00488         std::vector<OrientedPoint> retval(hypotheses);
00489         pthread_mutex_unlock(&hp_mutex);
00490         return retval;
00491 }
00492 
00493 std::vector<unsigned int> GridSlamProcessorThread::getIndexes(){
00494         pthread_mutex_lock(&ind_mutex);
00495         std::vector<unsigned int> retval(indexes);
00496         pthread_mutex_unlock(&ind_mutex);
00497         return retval;
00498 }
00499 
00500 void GridSlamProcessorThread::start(){
00501         if (running)
00502                 return;
00503         running=true;
00504         pthread_create(&gfs_thread, 0, (void * (*)(void *))fastslamthread, (void *) this);
00505 }
00506 
00507 void GridSlamProcessorThread::stop(){
00508         if (! running){
00509                 cout << "PORCO CAZZO" << endl;
00510                 return;
00511         }
00512         running=false;
00513         void * retval;
00514         pthread_join(gfs_thread, &retval);
00515 }
00516 
00517 void GridSlamProcessorThread::onOdometryUpdate(){
00518         pthread_mutex_lock(&hp_mutex);
00519         hypotheses.clear();
00520         weightSums.clear();
00521         for (GridSlamProcessor::ParticleVector::const_iterator part=getParticles().begin(); part!=getParticles().end(); part++ ){
00522                 hypotheses.push_back(part->pose);
00523                 weightSums.push_back(part->weightSum);
00524         }
00525         
00526         ParticleMoveEvent* event=new ParticleMoveEvent;
00527         event->scanmatched=false;
00528         event->hypotheses=hypotheses;
00529         event->weightSums=weightSums;
00530         event->neff=m_neff;
00531         pthread_mutex_unlock(&hp_mutex);
00532         
00533         addEvent(event);
00534         
00535         syncOdometryUpdate();
00536 }
00537 
00538 void GridSlamProcessorThread::onResampleUpdate(){
00539         pthread_mutex_lock(&ind_mutex);
00540         pthread_mutex_lock(&hp_mutex);
00541         
00542         indexes=GridSlamProcessor::getIndexes();
00543         
00544         assert (indexes.size()==getParticles().size());
00545         ResampleEvent* event=new ResampleEvent;
00546         event->indexes=indexes;
00547         
00548         pthread_mutex_unlock(&hp_mutex);
00549         pthread_mutex_unlock(&ind_mutex);
00550         
00551         addEvent(event);
00552         
00553         syncResampleUpdate();
00554 }
00555 
00556 void GridSlamProcessorThread::onScanmatchUpdate(){
00557         pthread_mutex_lock(&hp_mutex);
00558         hypotheses.clear();
00559         weightSums.clear();
00560         unsigned int bestIdx=0;
00561         double bestWeight=-1e1000;
00562         unsigned int idx=0;
00563         for (GridSlamProcessor::ParticleVector::const_iterator part=getParticles().begin(); part!=getParticles().end(); part++ ){
00564                 hypotheses.push_back(part->pose);
00565                 weightSums.push_back(part->weightSum);
00566                 if(part->weightSum>bestWeight){
00567                         bestIdx=idx;
00568                         bestWeight=part->weightSum;
00569                 }
00570                 idx++;
00571         }
00572         
00573         ParticleMoveEvent* event=new ParticleMoveEvent;
00574         event->scanmatched=true;
00575         event->hypotheses=hypotheses;
00576         event->weightSums=weightSums;
00577         event->neff=m_neff;
00578         addEvent(event);
00579         
00580         if (! mapTimer){
00581                 MapEvent* event=new MapEvent;
00582                 event->index=bestIdx;
00583                 event->pmap=new ScanMatcherMap(getParticles()[bestIdx].map);
00584                 event->pose=getParticles()[bestIdx].pose;
00585                 addEvent(event);
00586         }
00587         
00588         mapTimer++;
00589         mapTimer=mapTimer%mapUpdateTime;
00590         
00591         pthread_mutex_unlock(&hp_mutex);
00592 
00593         syncOdometryUpdate();
00594 }
00595 
00596 void GridSlamProcessorThread::syncOdometryUpdate(){
00597 }
00598 
00599 void GridSlamProcessorThread::syncResampleUpdate(){
00600 }
00601 
00602 void GridSlamProcessorThread::syncScanmatchUpdate(){
00603 }
00604 
00605 void GridSlamProcessorThread::addEvent(GridSlamProcessorThread::Event * e){
00606         pthread_mutex_lock(&hist_mutex);
00607         while (eventBuffer.size()>eventBufferLength){
00608                 Event* event=eventBuffer.front();
00609                 delete event;
00610                 eventBuffer.pop_front();
00611         }
00612         eventBuffer.push_back(e);
00613         pthread_mutex_unlock(&hist_mutex);
00614 }
00615 
00616 GridSlamProcessorThread::EventDeque GridSlamProcessorThread::getEvents(){
00617         pthread_mutex_lock(&hist_mutex);
00618         EventDeque copy(eventBuffer);
00619         eventBuffer.clear();
00620         pthread_mutex_unlock(&hist_mutex);
00621         return copy;
00622 }
00623 
00624 GridSlamProcessorThread::Event::~Event(){}
00625 
00626 GridSlamProcessorThread::MapEvent::~MapEvent(){
00627         if (pmap)
00628                 delete pmap;
00629 }
00630 
00631 void GridSlamProcessorThread::setEventBufferSize(unsigned int length){
00632         eventBufferLength=length;
00633 }
00634 
00635 OrientedPoint GridSlamProcessorThread::boundingBox(SensorLog* log, double& xmin, double& ymin, double& xmax, double& ymax) const{
00636         OrientedPoint initialPose(0,0,0);
00637         initialPose=log->boundingBox(xmin, ymin, xmax, ymax);
00638         xmin-=3*maxrange;
00639         ymin-=3*maxrange;
00640         xmax+=3*maxrange;
00641         ymax+=3*maxrange;
00642         return initialPose;
00643 }


openslam_gmapping
Author(s): Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard
autogenerated on Fri Aug 28 2015 11:56:21