add slam_gmapping
This commit is contained in:
X-lanni
@@ -0,0 +1,643 @@
|
||||
/*****************************************************************
|
||||
*
|
||||
* 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 "Creative Commons
|
||||
* License (Attribution-NonCommercial-ShareAlike 2.0)"
|
||||
* and is copyrighted by Giorgio Grisetti, Cyrill Stachniss,
|
||||
* and Wolfram Burgard.
|
||||
*
|
||||
* Further information on this license can be found at:
|
||||
* http://creativecommons.org/licenses/by-nc-sa/2.0/
|
||||
*
|
||||
* 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 "gsp_thread.h"
|
||||
#include <utils/commandline.h>
|
||||
#include <utils/stat.h>
|
||||
#include <configfile/configfile.h>
|
||||
|
||||
#ifdef CARMEN_SUPPORT
|
||||
#include <carmenwrapper/carmenwrapper.h>
|
||||
#endif
|
||||
|
||||
#define DEBUG cout << __PRETTY_FUNCTION__
|
||||
|
||||
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;
|
||||
}
|
||||
Reference in New Issue
Block a user