gsp_thread.cpp

Go to the documentation of this file.

/*****************************************************************
 *
 * This file is part of the GMAPPING project
 *
 * GMAPPING Copyright (c) 2004 Giorgio Grisetti, 
 * Cyrill Stachniss, and Wolfram Burgard
 *
 * This software is licensed under the 3-Clause BSD License
 * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss, 
 * and Wolfram Burgard.
 * 
 * Further information on this license can be found at:
 * https://opensource.org/licenses/BSD-3-Clause
 * 
 * GMAPPING is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied 
 * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
 * PURPOSE.  
 *
 *****************************************************************/


#include "gmapping/gui/gsp_thread.h"
#include <gmapping/utils/commandline.h>
#include <gmapping/utils/stat.h>
#include <gmapping/configfile/configfile.h>

#ifdef CARMEN_SUPPORT
        #include <gmapping/carmenwrapper/carmenwrapper.h>
#endif

#define DEBUG cout << __func__

using namespace std;

int GridSlamProcessorThread::init(int argc, const char * const * argv){
        m_argc=argc;
        m_argv=argv;
        std::string configfilename;
        std::string ebuf="not_set";

        CMD_PARSE_BEGIN_SILENT(1,argc);
                parseStringSilent("-cfg",configfilename);
        CMD_PARSE_END_SILENT;

        if (configfilename.length()>0){
          ConfigFile cfg(configfilename);

          filename = (std::string) cfg.value("gfs","filename",filename);
          outfilename = (std::string) cfg.value("gfs","outfilename",outfilename);
          xmin = cfg.value("gfs","xmin", xmin);
          xmax = cfg.value("gfs","xmax",xmax);
          ymin = cfg.value("gfs","ymin",ymin);
          ymax = cfg.value("gfs","ymax",ymax);
          delta =  cfg.value("gfs","delta",delta);
          maxrange = cfg.value("gfs","maxrange",maxrange);
          maxUrange = cfg.value("gfs","maxUrange",maxUrange);
          regscore = cfg.value("gfs","regscore",regscore);
          critscore = cfg.value("gfs","critscore",critscore);
          kernelSize = cfg.value("gfs","kernelSize",kernelSize);
          sigma = cfg.value("gfs","sigma",sigma);
          iterations = cfg.value("gfs","iterations",iterations);
          lstep = cfg.value("gfs","lstep",lstep);
          astep = cfg.value("gfs","astep",astep);
          maxMove = cfg.value("gfs","maxMove",maxMove);
          srr = cfg.value("gfs","srr", srr);
          srt = cfg.value("gfs","srt", srt);
          str = cfg.value("gfs","str", str);
          stt = cfg.value("gfs","stt", stt);
          particles = cfg.value("gfs","particles",particles);
          angularUpdate = cfg.value("gfs","angularUpdate", angularUpdate);
          linearUpdate = cfg.value("gfs","linearUpdate", linearUpdate);
          lsigma = cfg.value("gfs","lsigma", lsigma);
          ogain = cfg.value("gfs","lobsGain", ogain);
          lskip = (int)cfg.value("gfs","lskip", lskip);
          mapUpdateTime = cfg.value("gfs","mapUpdate", mapUpdateTime);
          randseed = cfg.value("gfs","randseed", randseed);
          autosize = cfg.value("gfs","autosize", autosize);
          readFromStdin = cfg.value("gfs","stdin", readFromStdin);
          resampleThreshold = cfg.value("gfs","resampleThreshold", resampleThreshold);
          skipMatching = cfg.value("gfs","skipMatching", skipMatching);
          onLine = cfg.value("gfs","onLine", onLine);
          generateMap = cfg.value("gfs","generateMap", generateMap);
          m_minimumScore = cfg.value("gfs","minimumScore", m_minimumScore);
          llsamplerange = cfg.value("gfs","llsamplerange", llsamplerange);
          lasamplerange = cfg.value("gfs","lasamplerange",lasamplerange );
          llsamplestep = cfg.value("gfs","llsamplestep", llsamplestep);
          lasamplestep = cfg.value("gfs","lasamplestep", lasamplestep);
          linearOdometryReliability = cfg.value("gfs","linearOdometryReliability",linearOdometryReliability);
          angularOdometryReliability = cfg.value("gfs","angularOdometryReliability",angularOdometryReliability);
          ebuf = (std::string) cfg.value("gfs","estrategy", ebuf);
          considerOdometryCovariance = cfg.value("gfs","considerOdometryCovariance",considerOdometryCovariance);
          
        }


        CMD_PARSE_BEGIN(1,argc);
                parseString("-cfg",configfilename);     /* to avoid the warning*/
                parseString("-filename",filename);
                parseString("-outfilename",outfilename);
                parseDouble("-xmin",xmin);
                parseDouble("-xmax",xmax);
                parseDouble("-ymin",ymin);
                parseDouble("-ymax",ymax);
                parseDouble("-delta",delta);
                parseDouble("-maxrange",maxrange);
                parseDouble("-maxUrange",maxUrange);
                parseDouble("-regscore",regscore);
                parseDouble("-critscore",critscore);
                parseInt("-kernelSize",kernelSize);
                parseDouble("-sigma",sigma);
                parseInt("-iterations",iterations);
                parseDouble("-lstep",lstep);
                parseDouble("-astep",astep);
                parseDouble("-maxMove",maxMove);
                parseDouble("-srr", srr);
                parseDouble("-srt", srt);
                parseDouble("-str", str);
                parseDouble("-stt", stt);
                parseInt("-particles",particles);
                parseDouble("-angularUpdate", angularUpdate);
                parseDouble("-linearUpdate", linearUpdate);
                parseDouble("-lsigma", lsigma);
                parseDouble("-lobsGain", ogain);
                parseInt("-lskip", lskip);
                parseInt("-mapUpdate", mapUpdateTime);
                parseInt("-randseed", randseed);
                parseFlag("-autosize", autosize);
                parseFlag("-stdin", readFromStdin);
                parseDouble("-resampleThreshold", resampleThreshold);
                parseFlag("-skipMatching", skipMatching);
                parseFlag("-onLine", onLine);
                parseFlag("-generateMap", generateMap);
                parseDouble("-minimumScore", m_minimumScore);
                parseDouble("-llsamplerange", llsamplerange);
                parseDouble("-lasamplerange", lasamplerange);
                parseDouble("-llsamplestep", llsamplestep);
                parseDouble("-lasamplestep", lasamplestep);
                parseDouble("-linearOdometryReliability",linearOdometryReliability);
                parseDouble("-angularOdometryReliability",angularOdometryReliability);
                parseString("-estrategy", ebuf);
                
                parseFlag("-considerOdometryCovariance",considerOdometryCovariance);
        CMD_PARSE_END;
        
        if (filename.length() <=0){
                cout << "no filename specified" << endl;
                return -1;
        }
        return 0;
}

int GridSlamProcessorThread::loadFiles(const char * fn){
        if (onLine){
                cout << " onLineProcessing" << endl;
                return 0;
        }       
        ifstream is;
        if (fn)
          is.open(fn);
        else
          is.open(filename.c_str());
        if (! is){
                cout << "no file found" << endl;
                return -1;
        }

        CarmenConfiguration conf;
        conf.load(is);
        is.close();

        sensorMap=conf.computeSensorMap();
        
        if (input)
                delete input;
        
        if (! readFromStdin){
                plainStream.open(filename.c_str());
                input=new InputSensorStream(sensorMap, plainStream);
                cout << "Plain Stream opened="<< (bool) plainStream << endl;
        } else {
                input=new InputSensorStream(sensorMap, cin);
                cout << "Plain Stream opened on stdin" << endl;
        }
        return 0;
}       

GridSlamProcessorThread::GridSlamProcessorThread(): GridSlamProcessor(cerr){
        //This are the processor parameters
        filename="";
        outfilename="";
        xmin=-100.;
        ymin=-100.;
        xmax=100.;
        ymax=100.;
        delta=0.05;
        
        //scan matching parameters
        sigma=0.05;
        maxrange=80.;
        maxUrange=80.;
        regscore=1e4;
        lstep=.05;
        astep=.05;
        kernelSize=1;
        iterations=5;
        critscore=0.;
        maxMove=1.;
        lsigma=.075;
        ogain=3;
        lskip=0;
        autosize=false;
        skipMatching=false;

        //motion model parameters
        srr=0.1, srt=0.1, str=0.1, stt=0.1;
        //particle parameters
        particles=30;
        randseed=0;
        
        //gfs parameters
        angularUpdate=0.5;
        linearUpdate=1;
        resampleThreshold=0.5;
        
        input=0;
        
        pthread_mutex_init(&hp_mutex,0);
        pthread_mutex_init(&ind_mutex,0);
        pthread_mutex_init(&hist_mutex,0);
        running=false;
        eventBufferLength=0;
        mapUpdateTime=5;
        mapTimer=0;
        readFromStdin=false;
        onLine=false;
        generateMap=false;

        // This  are the dafault settings for a grid map of 5 cm
        llsamplerange=0.01;
        llsamplestep=0.01;
        lasamplerange=0.005;
        lasamplestep=0.005;
        linearOdometryReliability=0.;
        angularOdometryReliability=0.;
        
        considerOdometryCovariance=false;
/*      
        // This  are the dafault settings for a grid map of 10 cm
        m_llsamplerange=0.1;
        m_llsamplestep=0.1;
        m_lasamplerange=0.02;
        m_lasamplestep=0.01;
*/      
        // This  are the dafault settings for a grid map of 20/25 cm
/*
        m_llsamplerange=0.2;
        m_llsamplestep=0.1;
        m_lasamplerange=0.02;
        m_lasamplestep=0.01;
        m_generateMap=false;
*/

                        
}

GridSlamProcessorThread::~GridSlamProcessorThread(){
        pthread_mutex_destroy(&hp_mutex);
        pthread_mutex_destroy(&ind_mutex);
        pthread_mutex_destroy(&hist_mutex);
        
        for (deque<Event*>::const_iterator it=eventBuffer.begin(); it!=eventBuffer.end(); it++)
                delete *it;
}
        

void * GridSlamProcessorThread::fastslamthread(GridSlamProcessorThread* gpt){
        if (! gpt->input && ! gpt->onLine)
                return 0;
                
        
        //if started online retrieve the settings from the connection
#ifdef CARMEN_SUPPORT
        if (gpt->onLine){
                cout << "starting the process:" << endl;
                CarmenWrapper::initializeIPC(gpt->m_argv[0]);
                CarmenWrapper::start(gpt->m_argv[0]);
                cout << "Waiting for retrieving the sensor map:" << endl;
                while (! CarmenWrapper::sensorMapComputed()){
                        usleep(500000);
                        cout << "." << flush;
                }
                gpt->sensorMap=CarmenWrapper::sensorMap();
                cout << "Connected " << endl;
        }
#else
        if (gpt->onLine){
                cout << "FATAL ERROR: cannot run online without the carmen support" << endl;
                DoneEvent *done=new DoneEvent;
                gpt->addEvent(done);
                return 0;
        }
#endif
        
        gpt->setSensorMap(gpt->sensorMap);
        gpt->setMatchingParameters(gpt->maxUrange, gpt->maxrange, gpt->sigma, gpt->kernelSize, gpt->lstep, gpt->astep, gpt->iterations, gpt->lsigma, gpt->ogain, gpt->lskip);
        
        double xmin=gpt->xmin, 
               ymin=gpt->ymin, 
               xmax=gpt->xmax, 
               ymax=gpt->ymax;
        
        OrientedPoint initialPose(0,0,0);
        
        if (gpt->autosize){
                if (gpt->readFromStdin || gpt->onLine)
                cout << "Error, cant autosize form stdin" << endl;
                SensorLog * log=new SensorLog(gpt->sensorMap);
                ifstream is(gpt->filename.c_str());
                log->load(is);
                is.close();
                initialPose=gpt->boundingBox(log, xmin, ymin, xmax, ymax);
                delete log;
        }
        
        if( gpt->infoStream()){
                gpt->infoStream() << " initialPose=" << initialPose.x << " " << initialPose.y << " " << initialPose.theta
                                << cout << " xmin=" << xmin <<" ymin=" << ymin <<" xmax=" << xmax <<" ymax=" << ymax << endl;
        }
        gpt->setMotionModelParameters(gpt->srr, gpt->srt, gpt->str, gpt->stt);
        gpt->setUpdateDistances(gpt->linearUpdate, gpt->angularUpdate, gpt->resampleThreshold);
        gpt->setgenerateMap(gpt->generateMap);
        gpt->GridSlamProcessor::init(gpt->particles, xmin, ymin, xmax, ymax, gpt->delta, initialPose);
        gpt->setllsamplerange(gpt->llsamplerange);
        gpt->setllsamplestep(gpt->llsamplestep);
        gpt->setlasamplerange(gpt->llsamplerange);
        gpt->setlasamplestep(gpt->llsamplestep);
        
#define printParam(n)\
         gpt->outputStream() \
         << "PARAM " << \
         #n \
         << " " << gpt->n << endl
        
        if (gpt->outfilename.length()>0 ){
                gpt->outputStream().open(gpt->outfilename.c_str());
                printParam(filename);
                printParam(outfilename);
                printParam(xmin);
                printParam(ymin);
                printParam(xmax);
                printParam(ymax);
                printParam(delta);
                
                //scan matching parameters
                printParam(sigma);
                printParam(maxrange);
                printParam(maxUrange);
                printParam(regscore);
                printParam(lstep);
                printParam(astep);
                printParam(kernelSize);
                printParam(iterations);
                printParam(critscore);
                printParam(maxMove);
                printParam(lsigma);
                printParam(ogain);
                printParam(lskip);
                printParam(autosize);
                printParam(skipMatching);
        
                //motion model parameters
                printParam(srr);
                printParam(srt); 
                printParam(str);
                printParam(stt);
                //particle parameters
                printParam(particles);
                printParam(randseed);
                
                //gfs parameters
                printParam(angularUpdate);
                printParam(linearUpdate);
                printParam(resampleThreshold);
                
                printParam(llsamplerange);
                printParam(lasamplerange);
                printParam(llsamplestep);
                printParam(lasamplestep);
        }
        #undef printParam
        
        if (gpt->randseed!=0)
                sampleGaussian(1,gpt->randseed);
        if (!gpt->infoStream()){
                cerr << "cant open info stream for writing by unuseful debug messages" << endl;
        } else {
                gpt->infoStream() << "setting randseed" << gpt->randseed<< endl;
        }
        
        
#ifdef CARMEN_SUPPORT
        list<RangeReading*> rrlist;
        if (gpt->onLine){
                RangeReading rr(0,0);
                while (1){
                        while (CarmenWrapper::getReading(rr)){
                                RangeReading* nr=new RangeReading(rr);
                                rrlist.push_back(nr);
                                gpt->processScan(*nr);
                        }
                }
        }
#endif
        ofstream rawpath("rawpath.dat");
        if (!gpt->onLine){
                while(*(gpt->input) && gpt->running){
                        const SensorReading* r;
                        (*(gpt->input)) >> r;
                        if (! r)
                                continue;
                        const RangeReading* rr=dynamic_cast<const RangeReading*>(r);
                        if (rr && gpt->running){
                                const RangeSensor* rs=dynamic_cast<const RangeSensor*>(rr->getSensor());
                                assert (rs && rs->beams().size()==rr->size());
                                
                                bool processed=gpt->processScan(*rr);
                                rawpath << rr->getPose().x << " " << rr->getPose().y << " " << rr->getPose().theta << endl;
                                if (0 && processed){
                                cerr << "Retrieving state .. ";
                                        TNodeVector trajetories=gpt->getTrajectories();
                                        cerr << "Done" <<  endl;
                                        cerr << "Deleting Tree state .. ";
                                        for (TNodeVector::iterator it=trajetories.begin(); it!=trajetories.end(); it++)
                                                delete *it;
                                        cerr << "Done" << endl;
                                }
//                              if (0 && processed){
//                                      cerr << "generating copy" << endl;;
//                                      GridSlamProcessor* m_gsp=gpt->clone();
//                                      Map<double, DoubleArray2D, false>*  pmap=m_gsp->getParticles()[0].map.toDoubleMap() ;
//                                      cerr << "deleting" << endl;
//                                      delete m_gsp;
//                                      delete pmap;
//                              }
                        }
                        const OdometryReading* o=dynamic_cast<const OdometryReading*>(r);
                        if (o && gpt->running){
                                gpt->processTruePos(*o);
                                TruePosEvent* truepos=new TruePosEvent;
                                truepos->pose=o->getPose();
                        }
                }
        }
        rawpath.close();

        TNodeVector trajetories=gpt->getTrajectories();
        cerr << "WRITING WEIGHTS" << endl;
        int pnumber=0;
        for (TNodeVector::iterator it=trajetories.begin(); it!=trajetories.end(); it++){
                char buf[10];
                sprintf(buf, "w-%03d.dat",pnumber);
                ofstream weightsStream(buf);
                GridSlamProcessor::TNode* n=*it;
                double oldWeight=0, oldgWeight=0;
                while (n!=0){
                        double w=n->weight-oldWeight;
                        double gw=n->gweight-oldgWeight;
                        oldWeight=n->weight;
                        oldgWeight=n->gweight;
                        weightsStream << w << " " << gw << endl;
                        n=n->parent;
                }
                weightsStream.close();
                pnumber++;
                cerr << buf << endl;
        }
                
        DoneEvent *done=new DoneEvent;
        gpt->addEvent(done);
        gpt->infoStream() << "Hallo, I am the gsp thread. I have finished. Do you think it is the case of checking for unlocked mutexes." << endl;
        return 0;
}

std::vector<OrientedPoint> GridSlamProcessorThread::getHypotheses(){
        pthread_mutex_lock(&hp_mutex);
        std::vector<OrientedPoint> retval(hypotheses);
        pthread_mutex_unlock(&hp_mutex);
        return retval;
}

std::vector<unsigned int> GridSlamProcessorThread::getIndexes(){
        pthread_mutex_lock(&ind_mutex);
        std::vector<unsigned int> retval(indexes);
        pthread_mutex_unlock(&ind_mutex);
        return retval;
}

void GridSlamProcessorThread::start(){
        if (running)
                return;
        running=true;
        pthread_create(&gfs_thread, 0, (void * (*)(void *))fastslamthread, (void *) this);
}

void GridSlamProcessorThread::stop(){
        if (! running){
                cout << "PORCO CAZZO" << endl;
                return;
        }
        running=false;
        void * retval;
        pthread_join(gfs_thread, &retval);
}

void GridSlamProcessorThread::onOdometryUpdate(){
        pthread_mutex_lock(&hp_mutex);
        hypotheses.clear();
        weightSums.clear();
        for (GridSlamProcessor::ParticleVector::const_iterator part=getParticles().begin(); part!=getParticles().end(); part++ ){
                hypotheses.push_back(part->pose);
                weightSums.push_back(part->weightSum);
        }
        
        ParticleMoveEvent* event=new ParticleMoveEvent;
        event->scanmatched=false;
        event->hypotheses=hypotheses;
        event->weightSums=weightSums;
        event->neff=m_neff;
        pthread_mutex_unlock(&hp_mutex);
        
        addEvent(event);
        
        syncOdometryUpdate();
}

void GridSlamProcessorThread::onResampleUpdate(){
        pthread_mutex_lock(&ind_mutex);
        pthread_mutex_lock(&hp_mutex);
        
        indexes=GridSlamProcessor::getIndexes();
        
        assert (indexes.size()==getParticles().size());
        ResampleEvent* event=new ResampleEvent;
        event->indexes=indexes;
        
        pthread_mutex_unlock(&hp_mutex);
        pthread_mutex_unlock(&ind_mutex);
        
        addEvent(event);
        
        syncResampleUpdate();
}

void GridSlamProcessorThread::onScanmatchUpdate(){
        pthread_mutex_lock(&hp_mutex);
        hypotheses.clear();
        weightSums.clear();
        unsigned int bestIdx=0;
        double bestWeight=-1e1000;
        unsigned int idx=0;
        for (GridSlamProcessor::ParticleVector::const_iterator part=getParticles().begin(); part!=getParticles().end(); part++ ){
                hypotheses.push_back(part->pose);
                weightSums.push_back(part->weightSum);
                if(part->weightSum>bestWeight){
                        bestIdx=idx;
                        bestWeight=part->weightSum;
                }
                idx++;
        }
        
        ParticleMoveEvent* event=new ParticleMoveEvent;
        event->scanmatched=true;
        event->hypotheses=hypotheses;
        event->weightSums=weightSums;
        event->neff=m_neff;
        addEvent(event);
        
        if (! mapTimer){
                MapEvent* event=new MapEvent;
                event->index=bestIdx;
                event->pmap=new ScanMatcherMap(getParticles()[bestIdx].map);
                event->pose=getParticles()[bestIdx].pose;
                addEvent(event);
        }
        
        mapTimer++;
        mapTimer=mapTimer%mapUpdateTime;
        
        pthread_mutex_unlock(&hp_mutex);

        syncOdometryUpdate();
}

void GridSlamProcessorThread::syncOdometryUpdate(){
}

void GridSlamProcessorThread::syncResampleUpdate(){
}

void GridSlamProcessorThread::syncScanmatchUpdate(){
}

void GridSlamProcessorThread::addEvent(GridSlamProcessorThread::Event * e){
        pthread_mutex_lock(&hist_mutex);
        while (eventBuffer.size()>eventBufferLength){
                Event* event=eventBuffer.front();
                delete event;
                eventBuffer.pop_front();
        }
        eventBuffer.push_back(e);
        pthread_mutex_unlock(&hist_mutex);
}

GridSlamProcessorThread::EventDeque GridSlamProcessorThread::getEvents(){
        pthread_mutex_lock(&hist_mutex);
        EventDeque copy(eventBuffer);
        eventBuffer.clear();
        pthread_mutex_unlock(&hist_mutex);
        return copy;
}

GridSlamProcessorThread::Event::~Event(){}

GridSlamProcessorThread::MapEvent::~MapEvent(){
        if (pmap)
                delete pmap;
}

void GridSlamProcessorThread::setEventBufferSize(unsigned int length){
        eventBufferLength=length;
}

OrientedPoint GridSlamProcessorThread::boundingBox(SensorLog* log, double& xmin, double& ymin, double& xmax, double& ymax) const{
        OrientedPoint initialPose(0,0,0);
        initialPose=log->boundingBox(xmin, ymin, xmax, ymax);
        xmin-=3*maxrange;
        ymin-=3*maxrange;
        xmax+=3*maxrange;
        ymax+=3*maxrange;
        return initialPose;
}