add slam_gmapping

slam_gmapping/openslam_gmapping/gui/CMakeLists.txt

@@ -0,0 +1,38 @@
+find_package(Qt5Widgets)
+if(Qt5Widgets_FOUND)
+else()
+find_package(Qt4)
+include(${QT_USE_FILE})
+endif()
+
+include_directories(../include/gmapping ../ ../include/gmapping/log/)
+
+#add_executable(gfs_nogui gfs_nogui.cpp)
+
+add_executable(gfs_simplegui gfs_simplegui.cpp gsp_thread.cpp)
+if(Qt5Widgets_FOUND)
+target_link_libraries(gfs_simplegui gridfastslam Qt5::Widgets)
+else()
+target_link_libraries(gfs_simplegui gridfastslam ${QT_LIBRARIES})
+endif()
+
+#add_executable(gfs2img gfs2img.cpp gridfastslam)
+
+#-include ../global.mk
+
+#OBJS= gsp_thread.o qparticleviewer.o qgraphpainter.o qmappainter.o 
+
+#APPS= gfs_nogui gfs_simplegui gfs2img
+#LDFLAGS+=  $(QT_LIB) $(KDE_LIB) -lgridfastslam -lscanmatcher -llog -lsensor_range -lsensor_odometry -lsensor_base -lconfigfile -lutils -lpthread
+
+#ifeq ($(CARMENSUPPORT),1)
+#LDFLAGS+= -lcarmenwrapper 
+#endif
+
+#CPPFLAGS+= -I../sensor $(QT_INCLUDE) $(KDE_INCLUDE) -I$(CARMEN_HOME)/include
+
+
+#-include ../build_tools/Makefile.generic-shared-object
+
+
+

slam_gmapping/openslam_gmapping/gui/gfs2img.cpp

@@ -0,0 +1,258 @@
+#include <cstdlib>
+#include <limits.h>
+#include <scanmatcher/scanmatcher.h>
+#include <gridfastslam/gfsreader.h>
+#include <qpixmap.h>
+#include <qpainter.h>
+#include <qimage.h>
+#include <qapplication.h>
+#include <utils/commandline.h>
+
+#define MAX_LASER_BEAMS 1024
+#define MAX_FILENAME 1024
+
+using namespace std;
+using namespace GMapping;
+using namespace GMapping::GFSReader;
+
+inline double min(double a, double b){
+	return (a<b)?a:b;
+}
+
+inline double max(double a, double b){
+	return (a>b)?a:b;
+}
+
+void computeBoundingBox(double& xmin, double& ymin, double& xmax, double& ymax, const LaserRecord& laser, const OrientedPoint& pose, double maxrange){
+	double theta=-M_PI/2+pose.theta;
+	double theta_step=(laser.readings.size()==180||laser.readings.size()==181)?M_PI/180:M_PI/360;
+	for (std::vector<double>::const_iterator it=laser.readings.begin(); it!=laser.readings.end(); it++){
+		if (*it<maxrange){
+			xmin=min(xmin,pose.x+*it*cos(theta));
+			ymin=min(ymin,pose.y+*it*sin(theta));
+			xmax=max(xmax,pose.x+*it*cos(theta));
+			ymax=max(ymax,pose.y+*it*sin(theta));
+		}
+		theta+=theta_step;
+	}
+}
+
+void computeBoundingBox(double& xmin, double& ymin, double& xmax, double& ymax, const RecordList& rl, double maxrange){
+	xmin = ymin = MAXDOUBLE;
+	xmax = ymax =-MAXDOUBLE;
+	const LaserRecord* lastLaser=0;
+	for (RecordList::const_iterator it=rl.begin(); it!=rl.end(); it++){
+		const LaserRecord* lr= dynamic_cast<const LaserRecord*>(*it);
+		if (lr){
+			lastLaser=lr;
+			continue;
+		}
+		const ScanMatchRecord* smr= dynamic_cast<const ScanMatchRecord*>(*it);
+		if (smr && lastLaser){
+			for (std::vector<OrientedPoint>::const_iterator pit=smr->poses.begin(); pit!=smr->poses.end(); pit++){
+				computeBoundingBox(xmin, ymin, xmax, ymax, *lastLaser, *pit, maxrange);
+			}
+		}
+	}
+}
+int main(int argc, char** argv){
+	QApplication app(argc, argv);
+	double maxrange=50;
+	double delta=0.1;
+	int scanSkip=5;
+	const char* filename=0;
+	const char* format="PNG";
+	CMD_PARSE_BEGIN(1, argc)
+		parseDouble("-maxrange", maxrange);
+		parseDouble("-delta", delta);
+		parseInt("-skip", scanSkip);
+		parseString("-filename",filename);
+		parseString("-format",format);
+	CMD_PARSE_END
+	
+	double maxUrange=maxrange;
+	if (! filename){
+		cout << " supply a gfs file, please" << endl;
+		cout << " usage gfs2img [options] -filename <gfs_file>" << endl;
+		cout << " [options]:" << endl;
+		cout << " -maxrange <range>" << endl;
+		cout << " -delta    <map cell size>" << endl;
+		cout << " -skip     <frames to skip among images>" << endl;
+		cout << " -format   <image format in capital letters>" << endl;
+		return -1;
+	}
+	ifstream is(filename);
+	if (!is){
+		cout << " supply an EXISTING gfs file, please" << endl;
+		return -1;
+	}
+	RecordList rl;
+	rl.read(is);
+	
+	int particles=0;
+	int beams=0;
+	for (RecordList::const_iterator it=rl.begin(); it!=rl.end(); it++){
+		const OdometryRecord* odometry=dynamic_cast<const OdometryRecord*>(*it);
+		if (odometry){
+			particles=odometry->dim;
+		}
+		const LaserRecord* s=dynamic_cast<const LaserRecord*>(*it);
+		if (s){
+			beams=s->readings.size();
+		}
+		if (particles && beams)
+			break;
+	}
+	cout << "Particles from gfs=" << particles << endl;
+	if (! particles){
+		cout << "no particles found, terminating" << endl;
+		return -1;
+	}
+	cout << "Laser beams from gfs=" << beams << endl;
+	if (! beams){
+		cout << "0 beams found, terminating" << endl;
+		return -1;
+	}
+	
+	
+	double laserBeamStep=0;
+	if (beams==180||beams==181){
+		laserBeamStep=M_PI/180;
+	} else if (beams==360||beams==361){
+		laserBeamStep=M_PI/360;
+	}
+	cout << "Laser beam step" << laserBeamStep << endl;
+	if (laserBeamStep==0){
+		cout << "Invalid Beam Step, terminating" << endl;
+		return -1;
+	}
+	double laserAngles[MAX_LASER_BEAMS];
+	double theta=-M_PI/2;
+	for (int i=0; i<beams; i++){
+		laserAngles[i]=theta;
+		theta+=laserBeamStep;
+	}
+
+	ScanMatcher matcher;
+	matcher.setLaserParameters(beams, laserAngles, OrientedPoint(0,0,0));
+	matcher.setlaserMaxRange(maxrange);
+	matcher.setusableRange(maxUrange);
+	matcher.setgenerateMap(true);
+	
+	double xmin, ymin, xmax, ymax;
+	cout << "computing bounding box" << endl;
+	computeBoundingBox(xmin, ymin, xmax, ymax, rl, maxrange);
+	cout << "DONE" << endl << "BBOX= " << xmin << " " << ymin << " " << xmax << " " << ymax << endl;
+
+	Point center;
+	center.x=(xmin+xmax)/2.;
+	center.y=(ymin+ymax)/2.;
+		
+	cout << "computing paths" << endl;
+	unsigned int frame=0;
+	int scanCount=0;
+	
+	for (RecordList::const_iterator it=rl.begin(); it!=rl.end(); it++){
+		const ScanMatchRecord* s=dynamic_cast<const ScanMatchRecord*>(*it);
+		if (!s) 
+			continue;
+		scanCount++;
+		if (scanCount%scanSkip)
+			continue;
+		cout << "Frame " << frame << " ";
+		std::vector<RecordList> paths(particles);
+		int bestIdx=0;
+		double bestWeight=-MAXDOUBLE;
+		for (int p=0; p<particles; p++){
+			paths[p]=rl.computePath(p,it);
+			double w=rl.getLogWeight(p,it);
+			if (w>bestWeight){
+				bestWeight=w;
+				bestIdx=p;
+			}
+		}
+		cout << "bestIdx=" << bestIdx << " bestWeight=" << bestWeight << endl;
+		
+		cout << "computing best map" << endl;
+		ScanMatcherMap smap(center, xmin, ymin, xmax, ymax, delta);
+		int count=0;
+		for (RecordList::const_iterator mt=paths[bestIdx].begin(); mt!=paths[bestIdx].end(); mt++){
+			const LaserRecord* s=dynamic_cast<const LaserRecord*>(*mt);
+			if (s){
+				double rawreadings[MAX_LASER_BEAMS];
+				for (uint i=0; i<s->readings.size(); i++)
+					rawreadings[i]=s->readings[i];
+				matcher.invalidateActiveArea();
+				matcher.computeActiveArea(smap, s->pose, rawreadings);
+//				matcher.allocActiveArea(smap, s->pose, rawreadings);
+				matcher.registerScan(smap, s->pose, rawreadings);
+				count++;
+			}
+		}
+		cout << "DONE " << count <<endl;
+
+		QPixmap pixmap(smap.getMapSizeX(), smap.getMapSizeY());
+		pixmap.fill(QColor(200, 200, 255));
+		QPainter painter(&pixmap);
+		for (int x=0; x<smap.getMapSizeX(); x++)
+			for (int y=0; y<smap.getMapSizeY(); y++){
+				double v=smap.cell(x,y);
+				if (v>=0){
+					int grayValue=255-(int)(255.*v);
+					painter.setPen(QColor(grayValue, grayValue, grayValue));
+					painter.drawPoint(x,smap.getMapSizeY()-y-1);
+				}
+			}
+		
+		/*
+		cout << "painting trajectories" << endl;
+		for (int p=0; p<particles; p++){
+			painter.setPen(QColor(Qt::red));
+			if (p==bestIdx)
+				continue;
+			bool first=true;
+			IntPoint oldPoint(0,0);
+			for (RecordList::const_iterator mt=paths[p].begin(); mt!=paths[p].end(); mt++){
+				const LaserRecord* s=dynamic_cast<const LaserRecord*>(*mt);
+				if (s){
+					IntPoint ip=smap.world2map(s->pose);
+					ip.y=smap.getMapSizeY()-ip.y-1;
+					if (!first){
+						painter.drawLine( oldPoint.x, oldPoint.y, ip.x, ip.y);
+					}
+					oldPoint=ip;
+					first=false;
+				}
+			}	
+			paths[p].destroyReferences();;
+		}
+		painter.setPen(QColor(Qt::black));
+		bool first=true;
+		IntPoint oldPoint(0,0);
+		for (RecordList::const_iterator mt=paths[bestIdx].begin(); mt!=paths[bestIdx].end(); mt++){
+			const LaserRecord* s=dynamic_cast<const LaserRecord*>(*mt);
+			if (s){
+				IntPoint ip=smap.world2map(s->pose);
+				ip.y=smap.getMapSizeY()-ip.y-1;
+				if (!first){
+					painter.drawLine( oldPoint.x, oldPoint.y, ip.x, ip.y);
+				} 
+				oldPoint=ip;
+				first=false;
+			}
+		}	
+		paths[bestIdx].destroyReferences();;
+		*/
+		cout << " DONE" << endl;
+		cout << "writing image" << endl;
+		QImage img=pixmap.convertToImage();
+		char ofilename[MAX_FILENAME];
+		sprintf(ofilename,"%s-%.4d.%s",filename, frame, format);
+		cout << ofilename << endl;
+		img.save(QString(ofilename), format,0);
+		frame++;
+		
+	}
+	cout << "For Cyrill: \"The Evil is Outside\"" << endl;
+}
+

slam_gmapping/openslam_gmapping/gui/gfs_logplayer.cpp

@@ -0,0 +1,41 @@
+/*****************************************************************
+ *
+ * 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 <qapplication.h>
+#include "qparticleviewer.h"
+
+int  main (int argc, char ** argv){
+	QApplication app(argc, argv);
+	QParticleViewer * pviewer=new QParticleViewer(0);
+	app.setMainWidget(pviewer);
+	pviewer->show();
+	FILE* f=fopen(argv[1], "r");
+	if (!f)
+		return -1;
+	QTextIStream is(f);
+	pviewer->tis=&is;
+	pviewer->start(10);
+	return app.exec();
+	std::cout << "DONE: " << argv[1] <<endl;
+}
+

slam_gmapping/openslam_gmapping/gui/gfs_nogui.cpp

@@ -0,0 +1,64 @@
+/*****************************************************************
+ *
+ * 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 <unistd.h>
+#include "gsp_thread.h"
+
+using namespace GMapping;
+
+int  main (int argc, char ** argv){
+  cerr << "GMAPPING copyright 2004 by Giorgio Grisetti, Cyrill Stachniss," << endl ;
+  cerr << "and Wolfram Burgard. To be published under the CreativeCommons license," << endl ;
+  cerr << "see: http://creativecommons.org/licenses/by-nc-sa/2.0/" << endl << endl;
+
+
+	GridSlamProcessorThread* gsp=  new GridSlamProcessorThread;
+	if (gsp->init(argc, argv)){
+		cout << "GSP INIT ERROR" << endl;
+		return -1;
+	}
+	cout <<"GSP INITIALIZED"<< endl;
+	if (gsp->loadFiles()){
+		cout <<"GSP READFILE ERROR"<< endl;
+		return -2;
+	}
+	cout <<"FILES LOADED"<< endl;
+	gsp->setMapUpdateTime(1000000);
+	gsp->start();
+	cout <<"THREAD STARTED"<< endl;
+	bool done=false;
+	while (!done){
+		GridSlamProcessorThread::EventDeque events=gsp->getEvents();
+		for (GridSlamProcessorThread::EventDeque::iterator it=events.begin(); it!=events.end(); it++){
+			cout << flush;
+			GridSlamProcessorThread::DoneEvent* doneEvent=dynamic_cast<GridSlamProcessorThread::DoneEvent*>(*it);
+			if (doneEvent){
+				done=true;
+				cout <<"DONE!"<< endl;
+				gsp->stop();
+			}
+			if (*it)
+				delete(*it);
+		}
+	}
+}

slam_gmapping/openslam_gmapping/gui/gfs_simplegui.cpp

@@ -0,0 +1,96 @@
+/*****************************************************************
+ *
+ * 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 "qparticleviewer.h"
+#include "qgraphpainter.h"
+#include <qapplication.h>
+#include <qframe.h>
+#include <qlabel.h>
+#include <qlayout.h>
+#include <QVBoxLayout>
+#include <qmainwindow.h>
+
+class GFSMainWindow: public QMainWindow{
+public:
+  GFSMainWindow(GridSlamProcessorThread* t){
+    gsp_thread=t;
+    QVBoxLayout* layout=new QVBoxLayout(this);
+    pviewer=new QParticleViewer(this,0,0,gsp_thread);
+    pviewer->setGeometry(0,0,500,500);
+    pviewer->setFocusPolicy(Qt::ClickFocus);
+    layout->addWidget(pviewer);
+						
+    gpainter=new QGraphPainter(this);
+    gpainter->setFixedHeight(100);
+    layout->addWidget(gpainter);
+    gpainter->setRange(0,1);
+    gpainter->setTitle("Neff");
+		
+    help = new QLabel(QString("+/- - zoom | b - show/hide best path | p - show/hide all paths | c - center robot "),this); 
+    help->setMaximumHeight(30);
+    layout->addWidget(help);
+	
+    QObject::connect( pviewer, SIGNAL(neffChanged(double) ), gpainter, SLOT(valueAdded(double)) );
+    setTabOrder(pviewer, pviewer);
+  }
+		
+  void start(int c){
+    pviewer->start(c);
+    gpainter->start(c);
+  }
+
+protected:
+  GridSlamProcessorThread* gsp_thread;
+  QVBoxLayout* layout;
+  QParticleViewer* pviewer;
+  QGraphPainter* gpainter;
+  QLabel* help;
+};
+
+
+int  main (int argc, char ** argv){
+  cerr << "GMAPPING copyright 2004 by Giorgio Grisetti, Cyrill Stachniss," << endl ;
+  cerr << "and Wolfram Burgard. To be published under the CreativeCommons license," << endl;
+  cerr << "see: http://creativecommons.org/licenses/by-nc-sa/2.0/" << endl << endl;
+
+
+  GridSlamProcessorThread* gsp=  new GridSlamProcessorThread;
+  if (gsp->init(argc, argv)){
+    cerr << "GridFastSlam: Initialization Error!" << endl;
+    cerr << "(Did you specified an input file for reading?)" << endl;
+    return -1;
+  }
+  if (gsp->loadFiles()){
+    cerr <<"Error reading file!"<< endl;
+    return -2;
+  }
+  cerr <<"File successfully loaded!"<< endl;
+  QApplication app(argc, argv);
+  GFSMainWindow* mainWin=new GFSMainWindow(gsp);
+  mainWin->show();
+  gsp->setEventBufferSize(10000);
+  gsp->start();
+  mainWin->start(1000);
+  return app.exec();
+}
+

slam_gmapping/openslam_gmapping/gui/gsp_thread.cpp

@@ -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;
+}

slam_gmapping/openslam_gmapping/gui/gsp_thread.h

@@ -0,0 +1,177 @@
+/*****************************************************************
+ *
+ * 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.  
+ *
+ *****************************************************************/
+
+
+#ifndef GSP_THREAD_H
+#define GSP_THREAD_H
+
+#include <unistd.h>
+#include <pthread.h>
+#include <deque>
+#include <fstream>
+#include <iostream>
+#include <log/carmenconfiguration.h>
+#include <log/sensorstream.h>
+#include <gridfastslam/gridslamprocessor.h>
+
+using namespace std;
+using namespace GMapping;
+
+
+#define MAX_STRING_LENGTH 1024
+
+
+struct GridSlamProcessorThread : public GridSlamProcessor {
+		struct Event{
+			virtual ~Event();
+		};
+		
+		struct ParticleMoveEvent: public Event{
+			bool scanmatched;
+			double neff;
+			std::vector<OrientedPoint> hypotheses;
+			std::vector<double> weightSums;
+		};
+		
+		struct TruePosEvent : public Event{
+			OrientedPoint pose;
+		};
+		
+		struct ResampleEvent: public Event{
+			std::vector<unsigned int> indexes;
+		};
+		
+		struct MapEvent: public Event{
+			ScanMatcherMap* pmap;
+			unsigned int index;
+			OrientedPoint pose;
+			virtual ~MapEvent();
+		};
+		
+		struct DoneEvent: public Event{
+		};
+		
+		typedef deque<Event*> EventDeque;
+		
+		GridSlamProcessorThread();
+		~GridSlamProcessorThread();	
+		int init(int argc, const char * const * argv);
+		int loadFiles(const char * fn=0);
+		static void * fastslamthread(GridSlamProcessorThread* gpt);
+		std::vector<OrientedPoint> getHypotheses();
+		std::vector<unsigned int> getIndexes();
+		
+		EventDeque getEvents();
+		
+		void start();
+		void stop();
+		
+		virtual void onOdometryUpdate();
+		virtual void onResampleUpdate();
+		virtual void onScanmatchUpdate();
+		
+		virtual void syncOdometryUpdate();
+		virtual void syncResampleUpdate();
+		virtual void syncScanmatchUpdate();
+		
+		void setEventBufferSize(unsigned int length);
+		inline void setMapUpdateTime(unsigned int ut) {mapUpdateTime=ut;}
+		inline bool isRunning() const {return running;}
+		OrientedPoint boundingBox(SensorLog* log, double& xmin, double& ymin, double& xmax, double& ymax) const;
+	private:
+		
+		void addEvent(Event *);
+		EventDeque eventBuffer;
+		
+		unsigned int eventBufferLength;
+		unsigned int mapUpdateTime;
+		unsigned int mapTimer;
+		
+		//thread interaction stuff
+		std::vector<OrientedPoint> hypotheses;
+		std::vector<unsigned int> indexes;
+		std::vector<double> weightSums;
+		pthread_mutex_t hp_mutex, ind_mutex, hist_mutex;
+		pthread_t gfs_thread;
+		bool running;
+		
+		//This are the processor parameters
+		std::string filename;
+		std::string outfilename;
+
+		double xmin;
+		double ymin;
+		double xmax;
+		double ymax;
+		bool autosize;
+		double delta;
+		double resampleThreshold;
+		
+		//scan matching parameters
+		double sigma;
+		double maxrange;
+		double maxUrange;
+		double regscore;
+		double lstep;
+		double astep;
+		int kernelSize;
+		int iterations;
+		double critscore;
+		double maxMove;
+		unsigned int lskip;
+
+		//likelihood
+		double lsigma;
+		double ogain;
+		double llsamplerange, lasamplerange;
+		double llsamplestep, lasamplestep;
+		double linearOdometryReliability;
+		double angularOdometryReliability;
+		
+	
+		//motion model parameters
+		double srr, srt, str, stt;
+		//particle parameters
+		int particles;
+		bool skipMatching;
+		
+		//gfs parameters
+		double angularUpdate;
+		double linearUpdate;
+	
+		//robot config
+		SensorMap sensorMap;
+		//input stream
+		InputSensorStream* input;
+		std::ifstream plainStream;
+		bool readFromStdin;
+		bool onLine;
+		bool generateMap;
+		bool considerOdometryCovariance;
+		unsigned int randseed;
+		
+		//dirty carmen interface
+		const char* const * m_argv;
+		unsigned int m_argc;
+		
+};
+#endif

slam_gmapping/openslam_gmapping/gui/qgraphpainter.cpp

@@ -0,0 +1,142 @@
+/*****************************************************************
+ *
+ * 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 <iostream>
+#include "qgraphpainter.h"
+#include "moc_qgraphpainter.cpp"
+using namespace std;
+
+QGraphPainter::QGraphPainter( QWidget * parent, const char * name, WFlags f):
+	QWidget(parent, name, f|WRepaintNoErase|WResizeNoErase){
+	m_pixmap=new QPixmap(size());
+	m_pixmap->fill(Qt::white);
+	autoscale=false;
+	m_useYReference = false;
+}
+
+void QGraphPainter::resizeEvent(QResizeEvent * sizeev){
+	m_pixmap->resize(sizeev->size());
+}
+
+QGraphPainter::~QGraphPainter(){
+	delete m_pixmap;
+}
+
+void QGraphPainter::clear(){
+	values.clear();
+}
+
+void QGraphPainter::valueAdded(double v){
+	values.push_back(v);
+}
+
+void QGraphPainter::valueAdded(double v, double _min, double _max){
+	setRange(_min, _max);
+	values.push_back(v);
+}
+
+void QGraphPainter::setYReference(double y){
+  m_useYReference = true;
+        reference=y;
+}
+
+void QGraphPainter::disableYReference(){
+  m_useYReference = false;
+}
+
+
+void QGraphPainter::setTitle(const char* t){
+	title=t;
+}
+
+void QGraphPainter::setRange(double _min, double _max){
+	min=_min;
+	max=_max;
+}
+
+void QGraphPainter::setAutoscale(bool a) {
+	autoscale=a;
+}
+
+bool QGraphPainter::getAutoscale() const {
+	return autoscale;
+}
+
+void QGraphPainter::timerEvent(QTimerEvent * te) {
+        if (te->timerId()==timer) 
+		update();
+}
+
+void QGraphPainter::start(int period){
+	timer=startTimer(period);
+}
+
+
+
+void QGraphPainter::paintEvent ( QPaintEvent * ){
+	m_pixmap->fill(Qt::white);
+	QPainter painter(m_pixmap);
+	double _min=MAXDOUBLE, _max=-MAXDOUBLE;
+	if (autoscale){
+		for (unsigned int i=0; i<(unsigned int)width() && i<values.size(); i++){
+			_min=_min<values[i]?_min:values[i];
+			_max=_max>values[i]?_max:values[i];
+		}
+	} else {
+		_min=min;
+		_max=max;
+	}
+	
+
+	painter.setPen(Qt::black);
+	painter.drawRect(0, 0, width(), height());
+	const int boundary=2;
+	int xoffset=40;
+	double scale=((double)height()-2*boundary-2)/(_max-_min);
+
+	if (m_useYReference) {
+	  painter.setPen(Qt::green);
+	  painter.drawLine(xoffset+boundary/2, height()-(int)(scale*(reference-_min)), 
+			   width()-boundary/2, height()-(int)(scale*(reference-_min)));
+	}
+	painter.setPen(Qt::blue);
+	unsigned int start=0;
+	if (values.size()>(unsigned int)width()-2*boundary-xoffset)
+		start=values.size()-width()+2*boundary+xoffset;
+	int oldv=0;
+	if ((unsigned int)width()-2*boundary-xoffset>1 && values.size()>1)
+	  oldv = (int)(scale*(values[1+start]-_min)) + boundary;
+	
+	for (unsigned int i=1; i<(unsigned int)width()-2*boundary-xoffset && i<values.size(); i++){
+		int v=(int)(scale*(values[i+start]-_min)) + boundary;
+		painter.drawLine(i-1+boundary+xoffset, height()-boundary-oldv, 
+				 xoffset+i+boundary, height()-boundary-v);
+		oldv=v;
+	}
+	painter.setPen(Qt::black);
+	painter.drawText( 3, height()/2, title);
+	QFont sansFont( "Helvetica [Cronyx]", 6);	
+	painter.setFont(sansFont);
+	bitBlt(this,0,0,m_pixmap,0,0,m_pixmap->width(),m_pixmap->height(),CopyROP);
+}
+

slam_gmapping/openslam_gmapping/gui/qgraphpainter.h

@@ -0,0 +1,67 @@
+/*****************************************************************
+ *
+ * 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.  
+ *
+ *****************************************************************/
+
+
+#ifndef QGRAPHPAINTER_H
+#define QGRAPHPAINTER_H
+
+#include <qpainter.h>
+#include <qpixmap.h>
+#include <qstring.h>
+#include <qwidget.h>
+#include <qmatrix.h>
+#include <deque>
+#include <utils/gvalues.h>
+
+typedef std::deque<double> DoubleDeque;
+
+class QGraphPainter :  public QWidget{
+	Q_OBJECT
+	public:
+		QGraphPainter( QWidget * parent = 0, const char * name = 0, Qt::WindowFlags f = 0);
+		virtual ~QGraphPainter();
+	public slots:
+		void clear();
+		void valueAdded(double);
+		void valueAdded(double, double, double);
+		void setYReference(double y);
+		void disableYReference();
+		void setRange(double min, double max);
+		void start(int period);
+		void setTitle(const char* title);
+		void setAutoscale(bool a);
+		bool getAutoscale() const;
+	protected:
+		virtual void timerEvent(QTimerEvent * te);
+		virtual void resizeEvent(QResizeEvent *);
+		double min, max, reference;
+		DoubleDeque values;
+		bool autoscale;
+		bool m_useYReference;
+		int timer;
+		virtual void paintEvent ( QPaintEvent *paintevent );
+		QPixmap * m_pixmap;
+		QString title;
+};
+
+#endif
+

slam_gmapping/openslam_gmapping/gui/qmappainter.cpp

@@ -0,0 +1,32 @@
+#include "qmappainter.h"
+#include "moc_qmappainter.cpp"
+
+QMapPainter::QMapPainter( QWidget * parent, const char * name, WFlags f):
+	QWidget(parent, name, f|WRepaintNoErase|WResizeNoErase){
+	m_pixmap=new QPixmap(size());
+	m_pixmap->fill(Qt::white);
+}
+
+void QMapPainter::resizeEvent(QResizeEvent * sizeev){
+	m_pixmap->resize(sizeev->size());
+}
+
+QMapPainter::~QMapPainter(){
+	delete m_pixmap;
+}
+
+
+void QMapPainter::timerEvent(QTimerEvent * te) {
+        if (te->timerId()==timer) 
+		update();
+}
+
+void QMapPainter::start(int period){
+	timer=startTimer(period);
+}
+
+
+void QMapPainter::paintEvent ( QPaintEvent * ){
+	bitBlt(this,0,0,m_pixmap,0,0,m_pixmap->width(),m_pixmap->height(),CopyROP);
+}
+

slam_gmapping/openslam_gmapping/gui/qmappainter.h

@@ -0,0 +1,60 @@
+#ifndef QMAPPAINTER_H
+#define QMAPPAINTER_H
+
+#include <qpainter.h>
+#include <qpixmap.h>
+#include <qstring.h>
+#include <qwidget.h>
+#include <utils/gvalues.h>
+#include <utils/point.h>
+
+class QMapPainter :  public QWidget{
+	public:
+		QMapPainter( QWidget * parent = 0, const char * name = 0, WFlags f = 0);
+		virtual ~QMapPainter();
+	public:
+		template < typename Cell >
+		void setPixmap(unsigned int xsize, unsigned int ysize, Cell** values);
+		template < typename Iterator >
+		void drawPoints(const Iterator& begin, const Iterator& end, unsigned char r,  unsigned char g, unsigned char b);
+		void start(int period);
+	protected:
+		virtual void timerEvent(QTimerEvent * te);
+		virtual void resizeEvent(QResizeEvent *);
+		int timer;
+		virtual void paintEvent ( QPaintEvent *paintevent );
+		QPixmap * m_pixmap;
+};
+
+template <typename Cell>
+void QMapPainter::setPixmap(unsigned int xsize, unsigned int ysize, Cell** values){
+	QSize s(xsize, ysize);
+	m_pixmap->resize(s);
+	m_pixmap->fill(Qt::white);
+	QPainter painter(m_pixmap);
+	for (unsigned int x=0; x<(unsigned int)xsize; x++)
+		for (unsigned int y=0; y<(unsigned int)ysize; y++){
+			double v=(double) values[x][y];
+			
+			if (v>=0){
+				unsigned int grayVal=(unsigned char) (255-(unsigned char)(255*v));
+				painter.setPen(QColor(grayVal, grayVal, grayVal));
+			} else {
+				painter.setPen(QColor(255, 100, 100));
+			}
+			painter.drawPoint(x,ysize-y);
+		}
+}
+
+template < typename Iterator >
+void QMapPainter::drawPoints(const Iterator& begin, const Iterator& end, unsigned char r,  unsigned char g, unsigned char b){
+	QPainter painter(m_pixmap);
+	painter.setPen(QColor(r,g,b));
+	for (Iterator it=begin; it!=end; it++){
+		GMapping::IntPoint p=(GMapping::IntPoint)*it;
+		painter.drawPoint(p.x, height()-p.y);
+	}
+}
+
+#endif
+

slam_gmapping/openslam_gmapping/gui/qnavigatorwidget.cpp

@@ -0,0 +1,126 @@
+#include "qnavigatorwidget.h"
+#include <stdio.h>
+using namespace GMapping;
+
+
+QNavigatorWidget::QNavigatorWidget( QWidget * parent, const char * name, WFlags f)
+: QMapPainter(parent, name, f), dumper("navigator", 1){
+	robotPose=IntPoint(0,0);
+	robotHeading=0;
+	confirmLocalization=false;
+	repositionRobot=false;
+	startWalker=false;
+	enableMotion=false;
+	goHome=false;
+	trajectorySent=false;
+	writeImages=false;
+	drawRobot=true;
+	wantsQuit=false;
+}
+
+QNavigatorWidget::~QNavigatorWidget(){}
+
+
+void QNavigatorWidget::mousePressEvent ( QMouseEvent * e ){
+	QPoint p=e->pos();
+	int mx=p.x();
+	int my=height()-p.y();
+	if (!(e->state()&Qt::ShiftButton) && e->button()==Qt::LeftButton) {
+		if (trajectorySent)
+			trajectoryPoints.clear();
+		e->accept();
+		IntPoint p=IntPoint(mx, my);
+		trajectoryPoints.push_back(p);
+		trajectorySent=false;
+	} 
+	if (e->state()&Qt::ControlButton && e->button()==Qt::LeftButton){
+		e->accept();
+		robotPose=IntPoint(mx, my);
+		repositionRobot=true;
+		confirmLocalization=true;
+	}
+	if (e->state()&Qt::ControlButton && e->button()==Qt::RightButton){
+		e->accept();
+		IntPoint p(mx, my);
+		p=p-robotPose;
+		robotHeading=atan2(p.y, p.x);
+		repositionRobot=true;
+		confirmLocalization=true;
+	}
+}
+
+void QNavigatorWidget::keyPressEvent ( QKeyEvent * e ){
+	if (e->key()==Qt::Key_Delete){
+		e->accept();
+		if (!trajectoryPoints.empty())
+			trajectoryPoints.pop_back();
+	}
+	if (e->key()==Qt::Key_S){
+		e->accept();
+		enableMotion=!enableMotion;
+	}
+	if (e->key()==Qt::Key_W){
+		e->accept();
+		startWalker=!startWalker;
+	}
+	if (e->key()==Qt::Key_G){
+		e->accept();
+		startGlobalLocalization=true;
+	}
+	if (e->key()==Qt::Key_T){
+		e->accept();
+		trajectorySent=true;
+	}
+	if (e->key()==Qt::Key_R){
+		e->accept();
+		goHome=true;
+	}	
+	if (e->key()==Qt::Key_C){
+		e->accept();
+		confirmLocalization=true;
+		
+	}
+	if (e->key()==Qt::Key_Q){
+		e->accept();
+		wantsQuit=true;
+		
+	}
+	if (e->key()==Qt::Key_D){
+		e->accept();
+		drawRobot=!drawRobot;;
+		
+	}
+}
+
+void QNavigatorWidget::paintEvent ( QPaintEvent * ){
+	QPixmap pixmap(*m_pixmap);
+	QPainter painter(&pixmap);
+	if (trajectorySent)
+	painter.setPen(Qt::red);
+	bool first=true;
+	int oldx=0, oldy=0;
+	//paint the path
+	for (std::list<IntPoint>::const_iterator it=trajectoryPoints.begin(); it!=trajectoryPoints.end(); it++){
+		int x=it->x;
+		int y=height()-it->y;
+		if (! first)
+			painter.drawLine(oldx, oldy, x,y);
+		oldx=x;
+		oldy=y;
+		first=false;
+	}
+	//paint the robot
+	if (drawRobot){
+		painter.setPen(Qt::black);
+		int rx=robotPose.x;
+		int ry=height()-robotPose.y;
+		int robotSize=6;
+		painter.drawLine(rx, ry, 
+				rx+(int)(robotSize*cos(robotHeading)), ry-(int)(robotSize*sin(robotHeading)));
+		painter.drawEllipse(rx-robotSize, ry-robotSize, 2*robotSize, 2*robotSize);
+	}
+	if (writeImages){
+		dumper.dump(pixmap);
+	}
+	bitBlt(this,0,0,&pixmap,0,0,pixmap.width(),pixmap.height(),CopyROP);
+}

slam_gmapping/openslam_gmapping/gui/qnavigatorwidget.h

@@ -0,0 +1,35 @@
+#ifndef _QNAVIGATOR_WIDGET_H
+#define _QNAVIGATOR_WIDGET_H
+
+#include "qmappainter.h"
+#include "qpixmapdumper.h"
+#include <utils/point.h>
+#include <list>
+
+class QNavigatorWidget :  public QMapPainter{
+	public:
+		QNavigatorWidget( QWidget * parent = 0, const char * name = 0, WFlags f = 0);
+		virtual ~QNavigatorWidget();
+		std::list<GMapping::IntPoint > trajectoryPoints;
+		bool repositionRobot;
+		GMapping::IntPoint robotPose;
+		double robotHeading;
+		bool confirmLocalization;
+		bool enableMotion;
+		bool startWalker;
+		bool startGlobalLocalization;
+		bool trajectorySent;
+		bool goHome;
+		bool wantsQuit;
+		bool writeImages;
+		QPixmapDumper dumper;
+		bool drawRobot;
+
+	protected:
+		virtual void paintEvent ( QPaintEvent *paintevent );
+		virtual void mousePressEvent ( QMouseEvent * e );
+		virtual void keyPressEvent ( QKeyEvent * e );
+};
+
+#endif
+

slam_gmapping/openslam_gmapping/gui/qparticleviewer.cpp

@@ -0,0 +1,453 @@
+/*****************************************************************
+ *
+ * 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 "qparticleviewer.h"
+#include "moc_qparticleviewer.cpp"
+#include <qimage.h>
+
+
+using namespace GMapping;
+
+QParticleViewer::QParticleViewer( QWidget * parent, const char * name , WFlags f, GridSlamProcessorThread* thread): QWidget(parent, name, f|WRepaintNoErase|WResizeNoErase){
+  viewCenter=Point(0.,0.);
+  setMinimumSize(500,500);
+  mapscale=10.;
+  m_pixmap=new QPixmap(500,500);
+  m_pixmap->fill(Qt::white);
+  gfs_thread=thread;
+  tis=0;
+  m_particleSize=0;
+  m_refresh=false;
+  bestMap=0;
+  dragging=false;
+  showPaths=0;
+  showBestPath=1;
+  count=0;
+  writeToFile=0;
+}
+
+QParticleViewer::~QParticleViewer(){
+  if (m_pixmap)
+    delete m_pixmap;
+}
+
+void QParticleViewer::paintEvent ( QPaintEvent *paintevent ){
+  if (! m_pixmap)
+    return;
+  bitBlt(this,0,0,m_pixmap,0,0,m_pixmap->width(),m_pixmap->height(),CopyROP);
+}
+
+void QParticleViewer::mousePressEvent ( QMouseEvent *event ){
+  if (event->button()==LeftButton){
+    dragging=true;
+    draggingPos=event->pos();	
+  }
+}
+void QParticleViewer::mouseMoveEvent ( QMouseEvent *event ){
+  if (dragging){
+    QPoint delta=event->pos()-draggingPos;
+    draggingPos=event->pos();
+    viewCenter.x-=delta.x()/mapscale;
+    viewCenter.y+=delta.y()/mapscale;
+    update();
+  }
+}
+
+void QParticleViewer::mouseReleaseEvent ( QMouseEvent *event ){
+  if (event->button()==LeftButton){
+    dragging=false;
+  }
+}
+
+void QParticleViewer::keyPressEvent ( QKeyEvent* e ){
+  switch (e->key()){
+  case Qt::Key_B: showBestPath=!showBestPath; break;
+  case Qt::Key_P: showPaths=!showPaths; break;
+  case Qt::Key_Plus: mapscale *=1.25;  break;
+  case Qt::Key_Minus: mapscale/=1.25;  break;
+  case Qt::Key_C: viewCenter=bestParticlePose; break;
+  default:;
+  }
+}
+
+		
+void QParticleViewer::resizeEvent(QResizeEvent * sizeev){
+  if (!m_pixmap)
+    return;
+  cerr << "QParticleViewer::resizeEvent" <<  sizeev->size().width()<< " " << sizeev->size().height() << endl;
+  m_pixmap->resize(sizeev->size());
+}
+
+void QParticleViewer::drawParticleMove(const QParticleViewer::OrientedPointVector& oldPose, const QParticleViewer::OrientedPointVector& newPose){
+  assert(oldPose.size()==newPose.size());
+  QPainter painter(m_pixmap);
+  painter.setPen(Qt::red);
+  OrientedPointVector::const_iterator nit=newPose.begin();
+  for(OrientedPointVector::const_iterator it=oldPose.begin(); it!=oldPose.end(); it++, nit++){
+    IntPoint p0=map2pic(*it);
+    IntPoint p1=map2pic(*nit);
+    painter.drawLine( 
+		     (int)(p0.x), (int)(p0.y), (int)(p1.x), (int)(p1.y)
+		     );
+  }
+}
+
+void QParticleViewer::drawFromFile(){
+  if(! tis)
+    return;
+  if (tis->atEnd())
+    return;	
+  QTextIStream& is=*tis;
+	
+  string line=is.readLine();
+  istringstream lineStream(line);
+  string recordType;
+  lineStream >> recordType;
+  if (recordType=="LASER_READING"){
+    //do nothing with the laser
+    cout << "l" << flush;
+  }
+  if (recordType=="ODO_UPDATE"){
+    //just move the particles
+    if (m_particleSize)
+      m_refresh=true;
+    m_oldPose=m_newPose;
+    m_newPose.clear();
+    unsigned int size;
+    lineStream >> size;
+    if (!m_particleSize)
+      m_particleSize=size;
+    assert(m_particleSize==size);
+    for (unsigned int i=0; i< size; i++){
+      OrientedPoint p;
+      double w;
+      lineStream >> p.x;
+      lineStream >> p.y;
+      lineStream >> p.theta;
+      lineStream >> w;
+      m_newPose.push_back(p);
+    }
+    cout << "o" << flush;
+  }
+  if (recordType=="SM_UPDATE"){
+    if (m_particleSize)
+      m_refresh=true;
+    m_oldPose=m_newPose;
+    m_newPose.clear();
+    unsigned int size;
+    lineStream >> size;
+    if (!m_particleSize)
+      m_particleSize=size;
+    assert(m_particleSize==size);
+    for (unsigned int i=0; i< size; i++){
+      OrientedPoint p;
+      double w;
+      lineStream >> p.x;
+      lineStream >> p.y;
+      lineStream >> p.theta;
+      lineStream >> w;
+      m_newPose.push_back(p);
+    }
+    cout << "u" << flush;
+  }
+  if (recordType=="RESAMPLE"){
+    unsigned int size;
+    lineStream >> size;
+    if (!m_particleSize)
+      m_particleSize=size;
+    assert(m_particleSize==size);
+    OrientedPointVector temp(size);
+    for (unsigned int i=0; i< size; i++){
+      unsigned int ind;
+      lineStream >> ind;
+      temp[i]=m_newPose[ind];
+    }
+    m_newPose=temp;
+    cout << "r" << flush;
+  }
+  if (m_refresh){
+    drawParticleMove(m_oldPose, m_newPose);
+    m_refresh=false;
+  }
+}
+
+void QParticleViewer::drawMap(const ScanMatcherMap& map){
+  //cout << "Map received" << map.getMapSizeX() << " " << map.getMapSizeY() << endl;
+  QPainter painter(m_pixmap);
+  painter.setPen(Qt::black);
+  m_pixmap->fill(QColor(200,200,255));
+  unsigned int count=0;
+	
+  Point wmin=Point(pic2map(IntPoint(-m_pixmap->width()/2,m_pixmap->height()/2)));
+  Point wmax=Point(pic2map(IntPoint(m_pixmap->width()/2,-m_pixmap->height()/2)));
+  IntPoint imin=map.world2map(wmin);
+  IntPoint imax=map.world2map(wmax);
+  /*	cout << __PRETTY_FUNCTION__ << endl;
+	cout << " viewCenter=" << viewCenter.x << "," << viewCenter.y <<   endl;	
+	cout << " wmin=" << wmin.x << "," << wmin.y <<  " wmax=" << wmax.x << "," << wmax.y << endl;	
+	cout << " imin=" << imin.x << "," << imin.y <<  " imax=" << imax.x << "," << imax.y << endl;
+	cout << " mapSize=" << map.getMapSizeX() << "," << map.getMapSizeY() << endl;*/
+  for(int x=0; x<m_pixmap->width(); x++)
+    for(int y=0; y<m_pixmap->height(); y++){
+      //IntPoint ip=IntPoint(x,y)+imin;
+      //Point p=map.map2world(ip);
+      Point p=pic2map(IntPoint(x-m_pixmap->width()/2,
+			       y-m_pixmap->height()/2));
+
+      //if (map.storage().isInside(map.world2map(p))){
+      double v=map.cell(p);
+      if (v>=0){
+	int grayValue=255-(int)(255.*v);
+	painter.setPen(QColor(grayValue, grayValue, grayValue));
+	painter.drawPoint(x,y);
+	count++;
+      }
+    }
+}
+
+
+void QParticleViewer::drawFromMemory(){
+  if (! gfs_thread)
+    return;
+  m_pixmap->fill(Qt::white);
+  GridSlamProcessorThread::EventDeque events=gfs_thread->getEvents();
+  for (GridSlamProcessorThread::EventDeque::const_iterator it=events.begin(); it!=events.end();it++){
+    GridSlamProcessorThread::MapEvent* mapEvent= dynamic_cast<GridSlamProcessorThread::MapEvent*>(*it);
+    if (mapEvent){
+      //cout << "Map: bestIdx=" << mapEvent->index <<endl;
+      if (bestMap)
+	delete bestMap;
+      else {
+				
+      }
+      bestMap=mapEvent->pmap;
+      mapEvent->pmap=0;
+      bestParticlePose=mapEvent->pose;
+      delete mapEvent;
+    }else{
+      GridSlamProcessorThread::DoneEvent* doneEvent= dynamic_cast<GridSlamProcessorThread::DoneEvent*>(*it);
+      if (doneEvent){
+	gfs_thread->stop();
+	delete doneEvent;
+      } else
+	history.push_back(*it);
+    }	
+			
+  }
+  if (bestMap)
+    drawMap(*bestMap);
+	
+  unsigned int particleSize=0;
+  std::vector<OrientedPoint> oldPose, newPose;
+  vector<unsigned int> indexes;
+	
+  GridSlamProcessorThread::EventDeque::reverse_iterator it=history.rbegin();
+  while (!particleSize && it!=history.rend()){
+    GridSlamProcessorThread::ParticleMoveEvent* move= dynamic_cast<GridSlamProcessorThread::ParticleMoveEvent*>(*it);
+    GridSlamProcessorThread::ResampleEvent* resample= dynamic_cast<GridSlamProcessorThread::ResampleEvent*>(*it);
+    if (move)
+      particleSize=move->hypotheses.size();
+    if (resample)
+      particleSize=resample->indexes.size();
+    it++;
+  }
+	
+  //check for the best index
+  double wmax=-1e2000;
+  unsigned int bestIdx=0;
+  bool emitted=false;
+  for (unsigned int i=0; i<particleSize; i++){
+    unsigned int currentIndex=i;
+    bool done=false;
+    for(GridSlamProcessorThread::EventDeque::reverse_iterator it=history.rbegin(); it!=history.rend()&& !done; it++){
+      GridSlamProcessorThread::ParticleMoveEvent* move= dynamic_cast<GridSlamProcessorThread::ParticleMoveEvent*>(*it);
+      if (move && move->scanmatched){
+	double cw=move->weightSums[currentIndex];
+	if (cw>wmax){
+	  wmax=cw;
+	  bestIdx=currentIndex;
+	} 
+	done=true;
+	if (! emitted){
+	  emit neffChanged(move->neff/particleSize);
+	  emitted=true;
+	}
+      }
+      GridSlamProcessorThread::ResampleEvent* resample= dynamic_cast<GridSlamProcessorThread::ResampleEvent*>(*it);
+      if (resample){
+	currentIndex=resample->indexes[currentIndex];
+      }
+    }
+  }
+  //cout << "bestIdx=" << bestIdx << endl;
+  QPainter painter(m_pixmap);
+	
+  for (unsigned int i=0; i<particleSize+1; i++){
+    painter.setPen(Qt::yellow);
+    unsigned int currentIndex=i;
+    if (i==particleSize && showBestPath){
+      currentIndex=bestIdx;
+      painter.setPen(Qt::red);
+    }
+    bool first=true;
+    OrientedPoint pnew(0,0,0);
+    for(GridSlamProcessorThread::EventDeque::reverse_iterator it=history.rbegin(); it!=history.rend(); it++){
+      GridSlamProcessorThread::ParticleMoveEvent* move= dynamic_cast<GridSlamProcessorThread::ParticleMoveEvent*>(*it);
+      if (move){
+	OrientedPoint pold=move->hypotheses[currentIndex];
+	IntPoint p0=map2pic(pold)+IntPoint(m_pixmap->width()/2,m_pixmap->height()/2);
+	IntPoint p1=map2pic(pnew)+IntPoint(m_pixmap->width()/2,m_pixmap->height()/2);;
+	if (first){
+	  painter.drawPoint(p0.x, p0.y);
+	} else {
+	  painter.drawLine(p0.x, p0.y, p1.x, p1.y);
+	}
+	first=false;
+	if (!(showPaths || showBestPath&&i==particleSize))
+	  break;
+	pnew=pold;
+      }
+      GridSlamProcessorThread::ResampleEvent* resample= dynamic_cast<GridSlamProcessorThread::ResampleEvent*>(*it);
+      if (resample && ! first){
+	currentIndex=resample->indexes[currentIndex];
+      }
+    }
+  }
+  if (writeToFile && bestMap){
+    if (! (count%writeToFile) ){
+      char name[100];
+      sprintf(name,"dump-%05d.png", count/writeToFile);
+      cout << " Writing " << name <<" ..." << flush;
+      QImage image=m_pixmap->convertToImage();
+      bool rv=image.save(name,"PNG");
+      if (rv)
+	cout << " Done";
+      else
+	cout << " ERROR";
+      cout << endl;
+    }
+    count++;
+  }
+}
+
+void QParticleViewer::timerEvent(QTimerEvent * te) {
+  if (te->timerId()==timer) {
+    if ( tis)
+      drawFromFile();
+    else{
+      drawFromMemory();
+      update();
+    }
+  }
+}
+
+
+void QParticleViewer::start(int period){
+  timer=startTimer(period);
+}
+
+void QParticleViewer::refreshParameters(){
+  //scanmatcher
+  matchingParameters.maxrange=gfs_thread->getlaserMaxRange();
+  matchingParameters.urange=gfs_thread->getusableRange();
+  matchingParameters.ssigma=gfs_thread->getgaussianSigma();
+  matchingParameters.sreg=gfs_thread->getregScore();
+  matchingParameters.scrit=gfs_thread->getcritScore();
+  matchingParameters.ksize=gfs_thread->getkernelSize();
+  matchingParameters.lstep=gfs_thread->getoptLinearDelta();
+  matchingParameters.astep=gfs_thread->getoptAngularDelta();
+  matchingParameters.iterations=gfs_thread->getoptRecursiveIterations();
+
+  //start
+  startParameters.srr=gfs_thread->getsrr();
+  startParameters.stt=gfs_thread->getstt();
+  startParameters.str=gfs_thread->getstr();
+  startParameters.srt=gfs_thread->getsrt();
+	
+  startParameters.xmin=gfs_thread->getxmin();
+  startParameters.ymin=gfs_thread->getymin();
+  startParameters.xmax=gfs_thread->getxmax();
+  startParameters.ymax=gfs_thread->getymax();
+  startParameters.delta=gfs_thread->getdelta();
+	
+  startParameters.particles=gfs_thread->getParticles().size();
+  startParameters.resampleThreshold=gfs_thread->getresampleThreshold();
+  startParameters.outFileName=0;
+}
+
+void QParticleViewer::start(){
+  gfs_thread->setMatchingParameters(
+				    matchingParameters.urange, 
+				    matchingParameters.maxrange, 
+				    matchingParameters.ssigma, 
+				    matchingParameters.ksize, 
+				    matchingParameters.lstep, 
+				    matchingParameters.astep, 
+				    matchingParameters.iterations, 
+				    startParameters.lsigma,
+				    startParameters.lgain,
+				    startParameters.lskip);
+  gfs_thread->setMotionModelParameters(
+				       startParameters.srr,
+				       startParameters.srt,
+				       startParameters.srt,
+				       startParameters.stt);
+  gfs_thread->setUpdateDistances(
+				 startParameters.linearUpdate,
+				 startParameters.angularUpdate,
+				 startParameters.resampleThreshold
+				 );
+  ((GridSlamProcessor*)(gfs_thread))->init(
+					   startParameters.particles,
+					   startParameters.xmin, 
+					   startParameters.ymin, 
+					   startParameters.xmax, 
+					   startParameters.ymax, 
+					   startParameters.delta, 
+					   startParameters.initialPose);
+  gfs_thread->start();
+}
+
+void QParticleViewer::setMatchingParameters(const QParticleViewer::MatchingParameters& mp){
+  matchingParameters=mp;
+}
+
+void QParticleViewer::setStartParameters(const QParticleViewer::StartParameters& sp){
+  startParameters=sp;
+}
+
+void QParticleViewer::stop(){
+  gfs_thread->stop();
+}
+
+void QParticleViewer::loadFile(const char * fn){
+  gfs_thread->loadFiles(fn);
+  /*	
+    startParameters.initialPose=
+    gfs_thread->boundingBox(
+    startParameters.xmin, 
+    startParameters.ymin, 
+    startParameters.xmax,
+    startParameters.ymax);
+  */	
+}

slam_gmapping/openslam_gmapping/gui/qparticleviewer.h

@@ -0,0 +1,161 @@
+/*****************************************************************
+ *
+ * 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.  
+ *
+ *****************************************************************/
+
+
+#ifndef QPARTICLEVIEWER_H
+#define QPARTICLEVIEWER_H
+
+#include <qpainter.h>
+#include <qpixmap.h>
+#include <qwidget.h>
+#include <qmatrix.h>
+#include <qtextstream.h>
+#include <vector>
+#include <assert.h>
+#include <sstream>
+#include <iostream>
+#include <qimage.h>
+#include <QTextStream>
+
+#include <utils/point.h>
+#include "gsp_thread.h"
+
+namespace GMapping {
+
+class QParticleViewer :  public QWidget{
+	Q_OBJECT
+	public:
+		struct StartParameters{
+			//motionmodel
+			double srr, srt, str, stt;
+			//map
+			double xmin, ymin, xmax, ymax, delta;
+			OrientedPoint initialPose;
+			//likelihood
+			double lsigma, lgain;
+			unsigned int lskip;
+			//update
+			double linearUpdate, angularUpdate;
+			//filter
+			unsigned int particles;
+			double resampleThreshold;
+			//mode
+			bool drawFromObservation;
+			//output
+			const char * outFileName;
+		};
+		
+		struct MatchingParameters{
+			//ranges
+			double maxrange, urange;
+			//score
+			double ssigma, sreg, scrit;
+			unsigned int ksize;
+			//search
+			double lstep, astep;
+			unsigned int iterations;
+		};
+
+		void refreshParameters(); //reads the parameters from the thread
+		inline void setGSP( GridSlamProcessorThread* thread){gfs_thread=thread;}
+		
+		
+		typedef std::vector<OrientedPoint> OrientedPointVector;
+		QParticleViewer( QWidget * parent = 0, const char * name = 0, Qt::WindowFlags f = 0, GridSlamProcessorThread* thread=0 );
+		virtual ~QParticleViewer();
+		virtual void timerEvent(QTimerEvent * te);
+		virtual void resizeEvent(QResizeEvent *);
+		
+		void drawFromFile();
+		void drawFromMemory();
+		void drawMap(const ScanMatcherMap& map);
+		void start(int period);
+		QTextStream* tis;
+		
+		MatchingParameters matchingParameters;
+		StartParameters startParameters;
+		
+		int writeToFile;
+	public slots:
+		void setMatchingParameters(const MatchingParameters& mp);
+		void setStartParameters(const StartParameters& mp);
+		void start();
+		void stop();
+		void loadFile(const char *);
+	signals:
+		void neffChanged(double);
+		void poseEntropyChanged(double, double, double);
+		void trajectoryEntropyChanged(double, double, double);
+		void mapsEntropyChanged(double);
+		void mapsIGainChanged(double);
+		
+	protected:
+		ifstream inputStream;
+		ofstream outputStream;
+		
+			
+	protected:
+		inline Point pic2map(const IntPoint& p) 
+			{return viewCenter+Point(p.x/mapscale, -p.y/mapscale); }
+		inline IntPoint map2pic(const Point& p)
+			{return IntPoint((int)((p.x-viewCenter.x)*mapscale),(int)((viewCenter.y-p.y)*mapscale)); }
+		
+		int timer;
+		virtual void paintEvent ( QPaintEvent *paintevent );
+		void drawParticleMove(const OrientedPointVector& start, const OrientedPointVector& end); 
+		QPixmap* m_pixmap;
+		
+		//thread interaction
+		GridSlamProcessorThread* gfs_thread;
+		GridSlamProcessorThread::EventDeque history;
+		
+		//mouse movement
+		virtual void mousePressEvent(QMouseEvent*);
+		virtual void mouseReleaseEvent(QMouseEvent*);
+		virtual void mouseMoveEvent(QMouseEvent*);
+		QPoint draggingPos;
+		bool dragging;
+		
+		//particle plotting
+		virtual void keyPressEvent ( QKeyEvent* e );
+		
+		//map painting
+		double mapscale;
+		Point viewCenter;
+		Point bestParticlePose;
+		ScanMatcherMap * bestMap;
+		
+		// view mode
+		bool showPaths;
+		bool showBestPath;
+		
+		// file plotting
+		QParticleViewer::OrientedPointVector m_oldPose, m_newPose;
+		unsigned int m_particleSize;
+		bool m_refresh;
+		int count;
+};
+
+};
+
+#endif
+

slam_gmapping/openslam_gmapping/gui/qpixmapdumper.cpp

@@ -0,0 +1,33 @@
+#include "qpixmapdumper.h"
+#include <cstdio>
+#include <cstring>
+
+QPixmapDumper::QPixmapDumper(std::string p, int c){
+	format="PNG";
+	prefix=p;
+	reset();
+	cycles=c;
+}
+
+void QPixmapDumper::reset(){
+	cycles=0;
+	frame=0;
+	counter=0;
+}
+
+#define filename_bufsize 1024
+
+bool QPixmapDumper::dump(const QPixmap& pixmap){
+	bool processed=false;
+	if (!(counter%cycles)){
+		char buf[filename_bufsize];
+		sprintf(buf,"%s-%05d.%s",prefix.c_str(), frame, format.c_str());
+		QImage image=pixmap.convertToImage();
+		image.save(QString(buf), format.c_str(),0);
+		frame ++;
+	}
+	counter++;
+	return processed;
+}
+
+

slam_gmapping/openslam_gmapping/gui/qpixmapdumper.h

@@ -0,0 +1,19 @@
+#ifndef _QPIXMAPDUMPER_H_
+#define _QPIXMAPDUMPER_H_
+#include <qpixmap.h>
+#include <qimage.h>
+#include <string>
+
+
+struct QPixmapDumper{
+	QPixmapDumper(std::string prefix, int cycles);
+	void reset();
+	std::string prefix;
+	std::string format;	
+	bool dump(const QPixmap& pixmap);
+	int counter;
+	int cycles;
+	int frame;
+};
+
+#endif

slam_gmapping/openslam_gmapping/gui/qslamandnavwidget.cpp

@@ -0,0 +1,128 @@
+#include "qslamandnavwidget.h"
+#include <stdio.h>
+using namespace GMapping;
+
+
+QSLAMandNavWidget::QSLAMandNavWidget( QWidget * parent, const char * name, WFlags f)
+: QMapPainter(parent, name, f), dumper("slamandnav", 1){
+	robotPose=IntPoint(0,0);
+	robotHeading=0;
+	slamRestart=false;
+	slamFinished=false;
+	enableMotion=false;
+	startWalker=false;
+	trajectorySent=false;
+	goHome=false;
+	wantsQuit=false;
+	printHelp=false;
+	saveGoalPoints=false;
+	writeImages=false;
+	drawRobot=true;
+}
+
+QSLAMandNavWidget::~QSLAMandNavWidget(){}
+
+
+void QSLAMandNavWidget::mousePressEvent ( QMouseEvent * e ){
+	QPoint p=e->pos();
+	int mx=p.x();
+	int my=height()-p.y();
+	if ( e->state()&Qt::ShiftButton && e->button()==Qt::LeftButton) {
+		if (trajectorySent)
+			trajectoryPoints.clear();
+		e->accept();
+		IntPoint p=IntPoint(mx, my);
+		trajectoryPoints.push_back(p);
+		trajectorySent=false;
+	} 
+}
+
+void QSLAMandNavWidget::keyPressEvent ( QKeyEvent * e ){
+	if (e->key()==Qt::Key_Delete){
+		e->accept();
+		if (!trajectoryPoints.empty())
+			trajectoryPoints.pop_back();
+	}
+	if (e->key()==Qt::Key_S){
+		e->accept();
+		enableMotion=!enableMotion;
+	}
+	if (e->key()==Qt::Key_W){
+		e->accept();
+		startWalker=!startWalker;
+	}
+	if (e->key()==Qt::Key_G){
+		e->accept();
+		slamRestart=true;
+	}
+	if (e->key()==Qt::Key_T){
+		e->accept();
+		trajectorySent=true;
+	}
+	if (e->key()==Qt::Key_R){
+		e->accept();
+		goHome=true;
+	}	
+	if (e->key()==Qt::Key_C){
+		e->accept();
+		slamFinished=true;
+		
+	}
+	if (e->key()==Qt::Key_Q){
+		e->accept();
+		wantsQuit=true;
+		
+	}
+	if (e->key()==Qt::Key_H){
+		e->accept();
+		printHelp=true;
+		//BABSI
+		//insert the help here
+	}
+	if (e->key()==Qt::Key_Y){
+		e->accept();
+		saveGoalPoints=true;
+		//BABSI
+		//insert the help here
+	}
+	if (e->key()==Qt::Key_D){
+		e->accept();
+		drawRobot=!drawRobot;
+		//BABSI
+		//insert the help here
+	}
+}
+
+void QSLAMandNavWidget::paintEvent ( QPaintEvent * ){
+	QPixmap pixmap(*m_pixmap);
+	QPainter painter(&pixmap);
+	if (trajectorySent)
+	painter.setPen(Qt::red);
+	bool first=true;
+	int oldx=0, oldy=0;
+	//paint the path
+	for (std::list<IntPoint>::const_iterator it=trajectoryPoints.begin(); it!=trajectoryPoints.end(); it++){
+		int x=it->x;
+		int y=height()-it->y;
+		if (! first)
+			painter.drawLine(oldx, oldy, x,y);
+		oldx=x;
+		oldy=y;
+		first=false;
+	}
+
+	//paint the robot
+	if (drawRobot){
+		painter.setPen(Qt::black);
+		int rx=robotPose.x;
+		int ry=height()-robotPose.y;
+		int robotSize=6;
+		painter.drawLine(rx, ry, 
+				rx+(int)(robotSize*cos(robotHeading)), ry-(int)(robotSize*sin(robotHeading)));
+		painter.drawEllipse(rx-robotSize, ry-robotSize, 2*robotSize, 2*robotSize);
+	}
+	if (writeImages){
+		dumper.dump(pixmap);
+	}
+	bitBlt(this,0,0,&pixmap,0,0,pixmap.width(),pixmap.height(),CopyROP);
+}

slam_gmapping/openslam_gmapping/gui/qslamandnavwidget.h

@@ -0,0 +1,38 @@
+#ifndef _QSLAMANDNAV_WIDGET_H
+#define _QSLAMANDNAV_WIDGET_H
+
+#include "qmappainter.h"
+#include "qpixmapdumper.h"
+#include <utils/point.h>
+#include <list>
+
+class QSLAMandNavWidget :  public QMapPainter{
+	public:
+		QSLAMandNavWidget( QWidget * parent = 0, const char * name = 0, WFlags f = 0);
+		virtual ~QSLAMandNavWidget();
+		std::list<GMapping::IntPoint > trajectoryPoints;
+		GMapping::IntPoint robotPose;
+		double robotHeading;
+		
+		bool slamRestart;
+		bool slamFinished;
+		bool enableMotion;
+		bool startWalker;
+		bool trajectorySent;
+		bool goHome;
+		bool wantsQuit;
+		bool printHelp;
+		bool saveGoalPoints;
+		bool writeImages;
+		bool drawRobot;
+		QPixmapDumper dumper;
+
+
+	protected:
+		virtual void paintEvent ( QPaintEvent *paintevent );
+		virtual void mousePressEvent ( QMouseEvent * e );
+		virtual void keyPressEvent ( QKeyEvent * e );
+};
+
+#endif
+