diff --git a/.gitignore b/.gitignore
index ecc6a0f..dc64b9f 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,3 +2,6 @@
 *.o
 bin/
 lib/
+.project
+.cproject
+build/
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 17f2b55..19b98ec 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -13,11 +13,11 @@ include_directories(include)
 add_subdirectory(gridfastslam)
 add_subdirectory(scanmatcher)
 add_subdirectory(sensor)
+add_subdirectory(log)
 add_subdirectory(utils)
-
-if(0)
-  add_subdirectory(gui)
-endif()
+add_subdirectory(configfile)
+add_subdirectory(gui)
+add_subdirectory(particlefilter)
 
 install(DIRECTORY include/
   DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}
diff --git a/configfile/CMakeLists.txt b/configfile/CMakeLists.txt
new file mode 100644
index 0000000..baff890
--- /dev/null
+++ b/configfile/CMakeLists.txt
@@ -0,0 +1,7 @@
+include_directories(../include/gmapping)
+add_library(configfile configfile.cpp)
+install(TARGETS configfile DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
+
+add_executable(configfile_test configfile_test.cpp)
+target_link_libraries(configfile_test configfile)
+install(TARGETS configfile_test DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
diff --git a/configfile/configfile.cpp b/configfile/configfile.cpp
new file mode 100644
index 0000000..687117d
--- /dev/null
+++ b/configfile/configfile.cpp
@@ -0,0 +1,331 @@
+/*****************************************************************
+ *
+ * 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 <configfile/configfile.h>
+#include <fstream>
+#include <iostream>
+#include <string>
+#include <sstream>
+#include <iostream>
+#include <ctype.h>
+
+namespace GMapping
+{
+
+using namespace std;
+
+AutoVal::AutoVal(std::string const& value)
+{
+  m_value = value;
+}
+
+AutoVal::AutoVal(const char* c)
+{
+  m_value = c;
+}
+
+AutoVal::AutoVal(double d)
+{
+  std::stringstream s;
+  s << d;
+  m_value = s.str();
+}
+
+AutoVal::AutoVal(bool d)
+{
+  std::stringstream s;
+  if (d)
+    s << "on";
+  else
+    s << "off";
+  m_value = s.str();
+}
+
+AutoVal::AutoVal(int i)
+{
+  std::stringstream s;
+  s << i;
+  m_value = s.str();
+}
+
+AutoVal::AutoVal(unsigned int i)
+{
+  std::stringstream s;
+  s << i;
+  m_value = s.str();
+}
+
+AutoVal::AutoVal(AutoVal const& other)
+{
+  m_value = other.m_value;
+}
+
+AutoVal& AutoVal::operator=(AutoVal const& other)
+{
+  m_value = other.m_value;
+  return *this;
+}
+
+AutoVal& AutoVal::operator=(double d)
+{
+  std::stringstream s;
+  s << d;
+  m_value = s.str();
+  return *this;
+}
+
+AutoVal& AutoVal::operator=(bool d)
+{
+  std::stringstream s;
+  if (d)
+    s << "on";
+  else
+    s << "off";
+  m_value = s.str();
+  return *this;
+}
+
+AutoVal& AutoVal::operator=(int i)
+{
+  std::stringstream s;
+  s << i;
+  m_value = s.str();
+  return *this;
+}
+
+AutoVal& AutoVal::operator=(unsigned int i)
+{
+  std::stringstream s;
+  s << i;
+  m_value = s.str();
+  return *this;
+}
+
+AutoVal& AutoVal::operator=(std::string const& s)
+{
+  m_value = s;
+  return *this;
+}
+
+AutoVal::operator std::string() const
+{
+  return m_value;
+}
+
+AutoVal::operator double() const
+{
+  return atof(m_value.c_str());
+}
+
+AutoVal::operator int() const
+{
+  return atoi(m_value.c_str());
+}
+
+AutoVal::operator unsigned int() const
+{
+  return (unsigned int) atoi(m_value.c_str());
+}
+
+AutoVal::operator bool() const
+{
+  // stupid c++ does not allow compareNoCase...
+  if (toLower(m_value) == "on" || atoi(m_value.c_str()) == 1)
+    return true;
+  return false;
+}
+
+std::string AutoVal::toLower(std::string const& source) const
+{
+  std::string result(source);
+  char c = '\0';
+  for (unsigned int i = 0; i < result.length(); i++) {
+    c = result[i];
+    c = ::tolower(c);
+    result[i] = c;
+  }
+  return result;
+}
+
+//////////////////////////////////////////////////////////
+
+ConfigFile::ConfigFile()
+{
+}
+
+ConfigFile::ConfigFile(std::string const& configFile)
+{
+  read(configFile);
+}
+
+bool ConfigFile::read(std::string const& configFile)
+{
+  std::ifstream file(configFile.c_str());
+
+  if (!file || file.eof())
+    return false;
+
+  std::string line;
+  std::string name;
+  std::string val;
+  std::string inSection;
+  while (std::getline(file, line)) {
+
+    if (!line.length())
+      continue;
+    // cute the comments
+    if (line[0] == '#')
+      continue;
+    line = truncate(line, "#");
+    line = trim(line);
+    if (!line.length())
+      continue;
+
+    if (line[0] == '[') {
+      inSection = trim(line.substr(1, line.find(']') - 1));
+      continue;
+    }
+
+    istringstream lineStream(line);
+    lineStream >> name;
+    lineStream >> val;
+    insertValue(inSection, name, val);
+  }
+  return true;
+}
+
+void ConfigFile::insertValue(std::string const& section, std::string const& entry, std::string const& thevalue)
+{
+  m_content[toLower(section) + '/' + toLower(entry)] = AutoVal(thevalue);
+}
+
+AutoVal const& ConfigFile::value(std::string const& section, std::string const& entry) const
+{
+
+  std::map<std::string, AutoVal>::const_iterator ci =
+      m_content.find(toLower(section) + '/' + toLower(entry));
+  if (ci == m_content.end())
+    throw "entry does not exist";
+
+  return ci->second;
+}
+
+AutoVal const& ConfigFile::value(std::string const& section, std::string const& entry, double def)
+{
+  try {
+    return value(section, entry);
+  }
+  catch (const char *) {
+    return m_content.insert(std::make_pair(section + '/' + entry, AutoVal(def))).first->second;
+  }
+}
+
+AutoVal const& ConfigFile::value(std::string const& section, std::string const& entry, bool def)
+{
+  try {
+    return value(section, entry);
+  }
+  catch (const char *) {
+    return m_content.insert(std::make_pair(section + '/' + entry, AutoVal(def))).first->second;
+  }
+}
+
+AutoVal const& ConfigFile::value(std::string const& section, std::string const& entry, int def)
+{
+  try {
+    return value(section, entry);
+  }
+  catch (const char *) {
+    return m_content.insert(std::make_pair(section + '/' + entry, AutoVal(def))).first->second;
+  }
+}
+
+AutoVal const& ConfigFile::value(std::string const& section, std::string const& entry, unsigned int def)
+{
+  try {
+    return value(section, entry);
+  }
+  catch (const char *) {
+    return m_content.insert(std::make_pair(section + '/' + entry, AutoVal(def))).first->second;
+  }
+}
+
+AutoVal const& ConfigFile::value(std::string const& section, std::string const& entry, std::string const& def)
+{
+  try {
+    return value(section, entry);
+  }
+  catch (const char *) {
+    return m_content.insert(std::make_pair(section + '/' + entry, AutoVal(def))).first->second;
+  }
+}
+
+void ConfigFile::dumpValues(std::ostream& out)
+{
+
+  for (std::map<std::string, AutoVal>::const_iterator it = m_content.begin();
+      it != m_content.end(); it++) {
+    out << (std::string) it->first << " " << (std::string) it->second << std::endl;
+  }
+}
+
+std::string ConfigFile::trim(std::string const& source, char const* delims) const
+{
+  std::string result(source);
+  std::string::size_type index = result.find_last_not_of(delims);
+  if (index != std::string::npos)
+    result.erase(++index);
+
+  index = result.find_first_not_of(delims);
+  if (index != std::string::npos)
+    result.erase(0, index);
+  else
+    result.erase();
+  return result;
+}
+
+std::string ConfigFile::truncate(std::string const& source, const char* atChar) const
+{
+  std::string::size_type index = source.find_first_of(atChar);
+
+  if (index == 0)
+    return "";
+  else if (index == std::string::npos) {
+    return source;
+  }
+  return
+  source.substr(0, index);
+}
+
+std::string ConfigFile::toLower(std::string const& source) const
+{
+  std::string result(source);
+  char c = '\0';
+  for (unsigned int i = 0; i < result.length(); i++) {
+    c = result[i];
+    c = ::tolower(c);
+    result[i] = c;
+  }
+  return result;
+}
+
+}
diff --git a/configfile/configfile_test.cpp b/configfile/configfile_test.cpp
new file mode 100644
index 0000000..dd7fc68
--- /dev/null
+++ b/configfile/configfile_test.cpp
@@ -0,0 +1,49 @@
+/*****************************************************************
+ *
+ * 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 <configfile/configfile.h>
+
+using namespace std;
+using namespace GMapping;
+
+int main(int argc, char** argv)
+{
+  if (argc != 2) {
+    cerr << "Usage:  " << argv[0] << " [initifle]" << endl;
+    exit(0);
+  }
+
+  ConfigFile cfg;
+  cfg.read(argv[argc - 1]);
+
+  cout << "-- values from configfile --" << endl;
+  cfg.dumpValues(cout);
+
+  cout << "-- adding a value --" << endl;
+  cfg.value("unkown", "unkown", "the new value!");
+
+  cout << "-- values from configfile & added values --" << endl;
+  cfg.dumpValues(cout);
+
+  return 0;
+}
diff --git a/configfile/demo.cfg b/configfile/demo.cfg
new file mode 100644
index 0000000..8c44d1f
--- /dev/null
+++ b/configfile/demo.cfg
@@ -0,0 +1,19 @@
+[mysection]
+
+mykey1    string_value_1  # comment
+mykey2    string value 2
+
+  mykey3  string value 3
+
+#  mykey4  string value 3
+
+   #  mykey5  string value 3
+
+mykey6 1
+mykey7 1.7
+
+mykey8 On
+
+mykey9 off
+
+
diff --git a/configfile/test.ini b/configfile/test.ini
new file mode 100644
index 0000000..5302d3f
--- /dev/null
+++ b/configfile/test.ini
@@ -0,0 +1,10 @@
+
+[test]
+
+particles        30
+
+onLine              on
+generateMap         off
+
+teststr            Hallo
+
diff --git a/gridfastslam/CMakeLists.txt b/gridfastslam/CMakeLists.txt
index b3e7f24..b732054 100644
--- a/gridfastslam/CMakeLists.txt
+++ b/gridfastslam/CMakeLists.txt
@@ -5,5 +5,4 @@ add_library(gridfastslam
     motionmodel.cpp
 )
 target_link_libraries(gridfastslam scanmatcher sensor_range)
-
 install(TARGETS gridfastslam DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
diff --git a/gui/CMakeLists.txt b/gui/CMakeLists.txt
index c8fabda..6feaee8 100644
--- a/gui/CMakeLists.txt
+++ b/gui/CMakeLists.txt
@@ -1,38 +1,47 @@
-find_package(Qt5Widgets)
-if(Qt5Widgets_FOUND)
-else()
-find_package(Qt4)
-include(${QT_USE_FILE})
-endif()
-
-include_directories(../include/gmapping ../ ../include/gmapping/log/)
+include_directories(.. ../include/gmapping ../include/gmapping/log)
+
+set(CMAKE_CXX_FLAGS -fPIC)
+set(CMAKE_EXE_LINKER_FLAGS -fPIE)
+set(CMAKE_AUTOMOC ON)
+
+add_library(gui
+    gsp_thread.cpp
+    qgraphpainter.cpp
+    qmappainter.cpp
+    qnavigatorwidget.cpp
+    qparticleviewer.cpp
+    qpixmapdumper.cpp
+    qslamandnavwidget.cpp
+)
 
-#add_executable(gfs_nogui gfs_nogui.cpp)
-
-add_executable(gfs_simplegui gfs_simplegui.cpp gsp_thread.cpp)
+find_package(Qt5Widgets)
 if(Qt5Widgets_FOUND)
-target_link_libraries(gfs_simplegui gridfastslam Qt5::Widgets)
+  include_directories(${Qt5Widgets_INCLUDE_DIRS})
+  target_link_libraries(gui Qt5::Widgets)
 else()
-target_link_libraries(gfs_simplegui gridfastslam ${QT_LIBRARIES})
+  find_package(Qt4 REQUIRED)
+  include(${QT_USE_FILE})
+  target_link_libraries(gui ${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
-
-
+include_directories(${CMAKE_BINARY_DIR}/gui)
+
+add_executable(gfs_simplegui gfs_simplegui.cpp)
+add_executable(gfs_nogui gfs_nogui.cpp)
+add_executable(gfs2img gfs2img.cpp)
+add_executable(gfs_logplayer gfs_logplayer.cpp)
+
+find_package (Threads REQUIRED)
+target_link_libraries(gfs_simplegui ${CMAKE_THREAD_LIBS_INIT})
+target_link_libraries(gfs_nogui ${CMAKE_THREAD_LIBS_INIT})
+target_link_libraries(gfs2img ${CMAKE_THREAD_LIBS_INIT})
+
+target_link_libraries(gfs_simplegui gui gridfastslam configfile log sensor_odometry)
+target_link_libraries(gfs_nogui gui gridfastslam configfile log sensor_odometry)
+target_link_libraries(gfs2img gui gridfastslam configfile log sensor_odometry)
+target_link_libraries(gfs_logplayer gui gridfastslam configfile log sensor_odometry)
+
+install(TARGETS gfs_simplegui DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+install(TARGETS gfs_nogui DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+install(TARGETS gfs2img DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+install(TARGETS gfs_logplayer DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
 
diff --git a/gui/gfs2img.cpp b/gui/gfs2img.cpp
index cdd654e..9a13fc4 100644
--- a/gui/gfs2img.cpp
+++ b/gui/gfs2img.cpp
@@ -1,5 +1,5 @@
 #include <cstdlib>
-#include <limits.h>
+#include <cfloat>
 #include <scanmatcher/scanmatcher.h>
 #include <gridfastslam/gfsreader.h>
 #include <qpixmap.h>
@@ -15,244 +15,249 @@ using namespace std;
 using namespace GMapping;
 using namespace GMapping::GFSReader;
 
-inline double min(double a, double b){
-	return (a<b)?a:b;
+inline double min(double a, double b)
+{
+  return (a < b) ? a : b;
 }
 
-inline double max(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 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);
-			}
-		}
-	}
+void computeBoundingBox(double& xmin, double& ymin, double& xmax, double& ymax, const RecordList& rl, double maxrange)
+{
+  xmin = ymin = DBL_MAX;
+  xmax = ymax = -DBL_MAX;
+  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;
+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
 
-	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;
+  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);
 
-		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;
-}
+  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 = -DBL_MAX;
+    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.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.toImage();
+    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;
+}
diff --git a/gui/gfs_logplayer.cpp b/gui/gfs_logplayer.cpp
index 674e59c..f71120e 100644
--- a/gui/gfs_logplayer.cpp
+++ b/gui/gfs_logplayer.cpp
@@ -2,40 +2,41 @@
  *
  * This file is part of the GMAPPING project
  *
- * GMAPPING Copyright (c) 2004 Giorgio Grisetti, 
+ * 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, 
+ * 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 
+ * but WITHOUT ANY WARRANTY; without even the implied
  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
- * PURPOSE.  
+ * 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;
-}
+using namespace GMapping;
 
+int main(int argc, char ** argv)
+{
+  QApplication app(argc, argv);
+  QParticleViewer * pviewer = new QParticleViewer(0);
+  app.setActiveWindow(pviewer);
+  pviewer->show();
+  FILE* f = fopen(argv[1], "r");
+  if (!f)
+    return -1;
+  QTextStream is(f);
+  pviewer->tis = &is;
+  pviewer->start(10);
+  return app.exec();
+  std::cout << "DONE: " << argv[1] << endl;
+}
diff --git a/gui/gfs_nogui.cpp b/gui/gfs_nogui.cpp
index d49d7a8..ee53b67 100644
--- a/gui/gfs_nogui.cpp
+++ b/gui/gfs_nogui.cpp
@@ -2,63 +2,62 @@
  *
  * This file is part of the GMAPPING project
  *
- * GMAPPING Copyright (c) 2004 Giorgio Grisetti, 
+ * 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, 
+ * 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 
+ * but WITHOUT ANY WARRANTY; without even the implied
  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
- * PURPOSE.  
+ * 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 ;
+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);
-		}
-	}
+  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);
+    }
+  }
 }
diff --git a/gui/gfs_simplegui.cpp b/gui/gfs_simplegui.cpp
index ba0b1d7..020877b 100644
--- a/gui/gfs_simplegui.cpp
+++ b/gui/gfs_simplegui.cpp
@@ -2,25 +2,24 @@
  *
  * This file is part of the GMAPPING project
  *
- * GMAPPING Copyright (c) 2004 Giorgio Grisetti, 
+ * 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, 
+ * 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 
+ * but WITHOUT ANY WARRANTY; without even the implied
  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
- * PURPOSE.  
+ * PURPOSE.
  *
  *****************************************************************/
 
-
 #include "qparticleviewer.h"
 #include "qgraphpainter.h"
 #include <qapplication.h>
@@ -30,67 +29,68 @@
 #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);
-  }
+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);
 
-protected:
-  GridSlamProcessorThread* gsp_thread;
-  QVBoxLayout* layout;
-  QParticleViewer* pviewer;
-  QGraphPainter* gpainter;
-  QLabel* help;
-};
+      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);
 
-int  main (int argc, char ** argv){
-  cerr << "GMAPPING copyright 2004 by Giorgio Grisetti, Cyrill Stachniss," << endl ;
+      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)){
+  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;
+  if (gsp->loadFiles()) {
+    cerr << "Error reading file!" << endl;
     return -2;
   }
-  cerr <<"File successfully loaded!"<< endl;
+  cerr << "File successfully loaded!" << endl;
   QApplication app(argc, argv);
-  GFSMainWindow* mainWin=new GFSMainWindow(gsp);
+  GFSMainWindow* mainWin = new GFSMainWindow(gsp);
   mainWin->show();
   gsp->setEventBufferSize(10000);
   gsp->start();
   mainWin->start(1000);
   return app.exec();
 }
-
diff --git a/gui/gsp_thread.cpp b/gui/gsp_thread.cpp
index 5d0022f..5239385 100644
--- a/gui/gsp_thread.cpp
+++ b/gui/gsp_thread.cpp
@@ -2,440 +2,452 @@
  *
  * This file is part of the GMAPPING project
  *
- * GMAPPING Copyright (c) 2004 Giorgio Grisetti, 
+ * 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, 
+ * 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 
+ * but WITHOUT ANY WARRANTY; without even the implied
  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
- * PURPOSE.  
+ * PURPOSE.
  *
  *****************************************************************/
 
-
 #include "gsp_thread.h"
+#include <utils/gvalues.h>
 #include <utils/commandline.h>
 #include <utils/stat.h>
 #include <configfile/configfile.h>
 
 #ifdef CARMEN_SUPPORT
-	#include <carmenwrapper/carmenwrapper.h>
+#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::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;
-*/
-
-			
+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(){
-	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;
+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;
+   */
 }
-	
-
-void * GridSlamProcessorThread::fastslamthread(GridSlamProcessorThread* gpt){
-	if (! gpt->input && ! gpt->onLine)
-		return 0;
-		
-	
-	//if started online retrieve the settings from the connection
+
+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;
-	}
+  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;
-	}
+  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);
-	
+
+  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
+        << " 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;
-	}
-	
-	
+
+  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);
-			}
-		}
-	}
+  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;
-				}
+
+  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();
@@ -444,200 +456,220 @@ void * GridSlamProcessorThread::fastslamthread(GridSlamProcessorThread* gpt){
 // 					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;
+      }
+      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<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;
+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::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::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::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::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::onScanmatchUpdate()
+{
+  pthread_mutex_lock(&hp_mutex);
+  hypotheses.clear();
+  weightSums.clear();
+  unsigned int bestIdx = 0;
+  double bestWeight = -DBL_MAX;
+  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::syncOdometryUpdate()
+{
 }
 
-void GridSlamProcessorThread::syncResampleUpdate(){
+void GridSlamProcessorThread::syncResampleUpdate()
+{
 }
 
-void GridSlamProcessorThread::syncScanmatchUpdate(){
+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);
+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::EventDeque GridSlamProcessorThread::getEvents()
+{
+  pthread_mutex_lock(&hist_mutex);
+  EventDeque copy(eventBuffer);
+  eventBuffer.clear();
+  pthread_mutex_unlock(&hist_mutex);
+  return copy;
 }
 
-GridSlamProcessorThread::Event::~Event(){}
+GridSlamProcessorThread::Event::~Event()
+{
+}
 
-GridSlamProcessorThread::MapEvent::~MapEvent(){
-	if (pmap)
-		delete pmap;
+GridSlamProcessorThread::MapEvent::~MapEvent()
+{
+  if (pmap)
+    delete pmap;
 }
 
-void GridSlamProcessorThread::setEventBufferSize(unsigned int length){
-	eventBufferLength=length;
+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;
+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;
 }
diff --git a/gui/gsp_thread.h b/gui/gsp_thread.h
index 51c0dc9..e583103 100644
--- a/gui/gsp_thread.h
+++ b/gui/gsp_thread.h
@@ -2,25 +2,24 @@
  *
  * This file is part of the GMAPPING project
  *
- * GMAPPING Copyright (c) 2004 Giorgio Grisetti, 
+ * 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, 
+ * 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 
+ * but WITHOUT ANY WARRANTY; without even the implied
  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
- * PURPOSE.  
+ * PURPOSE.
  *
  *****************************************************************/
 
-
 #ifndef GSP_THREAD_H
 #define GSP_THREAD_H
 
@@ -36,142 +35,155 @@
 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();
 
-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;
-		
+    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
diff --git a/gui/qgraphpainter.cpp b/gui/qgraphpainter.cpp
index 744e23b..bfecfc7 100644
--- a/gui/qgraphpainter.cpp
+++ b/gui/qgraphpainter.cpp
@@ -2,141 +2,161 @@
  *
  * This file is part of the GMAPPING project
  *
- * GMAPPING Copyright (c) 2004 Giorgio Grisetti, 
+ * 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, 
+ * 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 
+ * but WITHOUT ANY WARRANTY; without even the implied
  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
- * PURPOSE.  
+ * PURPOSE.
  *
  *****************************************************************/
 
-
-#include <iostream>
 #include "qgraphpainter.h"
-#include "moc_qgraphpainter.cpp"
+#include <utils/gvalues.h>
+#include <iostream>
+#include <qevent.h>
+
 using namespace std;
+using namespace GMapping;
 
-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;
+// moc_qgraphpainter.cpp is generated by moc at compile time
+#include "moc_qgraphpainter.cpp"
+
+QGraphPainter::QGraphPainter(QWidget * parent, const char * name, Qt::WindowFlags f) :
+    QWidget(parent, f)
+{
+  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());
+void QGraphPainter::resizeEvent(QResizeEvent * sizeev)
+{
+  m_pixmap->scaled(sizeev->size());
 }
 
-QGraphPainter::~QGraphPainter(){
-	delete m_pixmap;
+QGraphPainter::~QGraphPainter()
+{
+  delete m_pixmap;
 }
 
-void QGraphPainter::clear(){
-	values.clear();
+void QGraphPainter::clear()
+{
+  values.clear();
 }
 
-void QGraphPainter::valueAdded(double v){
-	values.push_back(v);
+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::valueAdded(double v, double _min, double _max)
+{
+  setRange(_min, _max);
+  values.push_back(v);
 }
 
-void QGraphPainter::setYReference(double y){
+void QGraphPainter::setYReference(double y)
+{
   m_useYReference = true;
-        reference=y;
+  reference = y;
 }
 
-void QGraphPainter::disableYReference(){
+void QGraphPainter::disableYReference()
+{
   m_useYReference = false;
 }
 
-
-void QGraphPainter::setTitle(const char* t){
-	title=t;
+void QGraphPainter::setTitle(const char* t)
+{
+  title = t;
 }
 
-void QGraphPainter::setRange(double _min, double _max){
-	min=_min;
-	max=_max;
+void QGraphPainter::setRange(double _min, double _max)
+{
+  min = _min;
+  max = _max;
 }
 
-void QGraphPainter::setAutoscale(bool a) {
-	autoscale=a;
+void QGraphPainter::setAutoscale(bool a)
+{
+  autoscale = a;
 }
 
-bool QGraphPainter::getAutoscale() const {
-	return autoscale;
+bool QGraphPainter::getAutoscale() const
+{
+  return autoscale;
 }
 
-void QGraphPainter::timerEvent(QTimerEvent * te) {
-        if (te->timerId()==timer) 
-		update();
+void QGraphPainter::timerEvent(QTimerEvent * te)
+{
+  if (te->timerId() == timer)
+    update();
 }
 
-void QGraphPainter::start(int period){
-	timer=startTimer(period);
+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);
+void QGraphPainter::paintEvent(QPaintEvent *)
+{
+  m_pixmap->fill(Qt::white);
+  QPainter painter(m_pixmap);
+  double _min = DBL_MAX, _max = -DBL_MAX;
+  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);
+
+  QPainter widgetPainter(this);
+  widgetPainter.drawPixmap(0, 0, *m_pixmap, 0, 0, m_pixmap->width(), m_pixmap->height());
 }
-
diff --git a/gui/qgraphpainter.h b/gui/qgraphpainter.h
index feb633b..798ff0a 100644
--- a/gui/qgraphpainter.h
+++ b/gui/qgraphpainter.h
@@ -2,25 +2,24 @@
  *
  * This file is part of the GMAPPING project
  *
- * GMAPPING Copyright (c) 2004 Giorgio Grisetti, 
+ * 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, 
+ * 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 
+ * but WITHOUT ANY WARRANTY; without even the implied
  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
- * PURPOSE.  
+ * PURPOSE.
  *
  *****************************************************************/
 
-
 #ifndef QGRAPHPAINTER_H
 #define QGRAPHPAINTER_H
 
@@ -32,36 +31,45 @@
 #include <deque>
 #include <utils/gvalues.h>
 
+namespace GMapping
+{
+
 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;
+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 *);
+    virtual void paintEvent(QPaintEvent *paintevent);
+
+    double min, max, reference;
+    DoubleDeque values;
+    bool autoscale;
+    bool m_useYReference;
+    int timer;
+    QPixmap * m_pixmap;
+    QString title;
 };
 
-#endif
+}
 
+#endif
diff --git a/gui/qmappainter.cpp b/gui/qmappainter.cpp
index 3aa2dfb..6c9663c 100644
--- a/gui/qmappainter.cpp
+++ b/gui/qmappainter.cpp
@@ -1,32 +1,38 @@
 #include "qmappainter.h"
-#include "moc_qmappainter.cpp"
+#include <qevent.h>
 
-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);
-}
+using namespace GMapping;
 
-void QMapPainter::resizeEvent(QResizeEvent * sizeev){
-	m_pixmap->resize(sizeev->size());
+QMapPainter::QMapPainter(QWidget * parent, const char * name, Qt::WindowFlags f) :
+    QWidget(parent, f)
+{
+  m_pixmap = new QPixmap(size());
+  m_pixmap->fill(Qt::white);
 }
 
-QMapPainter::~QMapPainter(){
-	delete m_pixmap;
+void QMapPainter::resizeEvent(QResizeEvent * sizeev)
+{
+  m_pixmap->scaled(sizeev->size());
 }
 
-
-void QMapPainter::timerEvent(QTimerEvent * te) {
-        if (te->timerId()==timer) 
-		update();
+QMapPainter::~QMapPainter()
+{
+  delete m_pixmap;
 }
 
-void QMapPainter::start(int period){
-	timer=startTimer(period);
+void QMapPainter::timerEvent(QTimerEvent * te)
+{
+  if (te->timerId() == timer)
+    update();
 }
 
-
-void QMapPainter::paintEvent ( QPaintEvent * ){
-	bitBlt(this,0,0,m_pixmap,0,0,m_pixmap->width(),m_pixmap->height(),CopyROP);
+void QMapPainter::start(int period)
+{
+  timer = startTimer(period);
 }
 
+void QMapPainter::paintEvent(QPaintEvent *)
+{
+  QPainter painter(this);
+  painter.drawPixmap(0, 0, *m_pixmap, 0, 0, m_pixmap->width(), m_pixmap->height());
+}
diff --git a/gui/qmappainter.h b/gui/qmappainter.h
index d121d1b..2cc2296 100644
--- a/gui/qmappainter.h
+++ b/gui/qmappainter.h
@@ -8,53 +8,63 @@
 #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;
+namespace GMapping
+{
+
+class QMapPainter: public QWidget
+{
+  public:
+    QMapPainter(QWidget * parent = 0, const char * name = 0, Qt::WindowFlags f = 0);
+    virtual ~QMapPainter();
+
+    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 *);
+    virtual void paintEvent(QPaintEvent *paintevent);
+    int timer;
+    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 Cell>
+void QMapPainter::setPixmap(unsigned int xsize, unsigned int ysize, Cell** values)
+{
+  QSize s(xsize, ysize);
+  m_pixmap->scaled(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);
-	}
+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
+}
 
+#endif
diff --git a/gui/qnavigatorwidget.cpp b/gui/qnavigatorwidget.cpp
index d818554..97c72cd 100644
--- a/gui/qnavigatorwidget.cpp
+++ b/gui/qnavigatorwidget.cpp
@@ -1,126 +1,130 @@
 #include "qnavigatorwidget.h"
 #include <stdio.h>
-using namespace GMapping;
+#include <qevent.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(QWidget * parent, const char * name, Qt::WindowFlags 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(){}
-
+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::mousePressEvent(QMouseEvent * e)
+{
+  QPoint p = e->pos();
+  int mx = p.x();
+  int my = height() - p.y();
+  if (!(e->modifiers() & Qt::ShiftModifier) && e->button() == Qt::LeftButton) {
+    if (trajectorySent)
+      trajectoryPoints.clear();
+    e->accept();
+    IntPoint p = IntPoint(mx, my);
+    trajectoryPoints.push_back(p);
+    trajectorySent = false;
+  }
+  if ((e->modifiers() & Qt::ControlModifier) && e->button() == Qt::LeftButton) {
+    e->accept();
+    robotPose = IntPoint(mx, my);
+    repositionRobot = true;
+    confirmLocalization = true;
+  }
+  if ((e->modifiers() & Qt::ControlModifier) && 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::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);
+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);
+  }
+  QPainter widgetPainter(this);
+  widgetPainter.drawPixmap(0, 0, pixmap, 0, 0, pixmap.width(), pixmap.height());
 }
diff --git a/gui/qnavigatorwidget.h b/gui/qnavigatorwidget.h
index 9766b9d..47c1e29 100644
--- a/gui/qnavigatorwidget.h
+++ b/gui/qnavigatorwidget.h
@@ -6,30 +6,36 @@
 #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;
+namespace GMapping
+{
 
-	protected:
-		virtual void paintEvent ( QPaintEvent *paintevent );
-		virtual void mousePressEvent ( QMouseEvent * e );
-		virtual void keyPressEvent ( QKeyEvent * e );
+class QNavigatorWidget: public QMapPainter
+{
+  public:
+    QNavigatorWidget(QWidget * parent = 0, const char * name = 0, Qt::WindowFlags 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
+}
 
+#endif
diff --git a/gui/qparticleviewer.cpp b/gui/qparticleviewer.cpp
index 633aee5..b717e50 100644
--- a/gui/qparticleviewer.cpp
+++ b/gui/qparticleviewer.cpp
@@ -2,143 +2,166 @@
  *
  * This file is part of the GMAPPING project
  *
- * GMAPPING Copyright (c) 2004 Giorgio Grisetti, 
+ * 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, 
+ * 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 
+ * but WITHOUT ANY WARRANTY; without even the implied
  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
- * PURPOSE.  
+ * PURPOSE.
  *
  *****************************************************************/
 
-
 #include "qparticleviewer.h"
-#include "moc_qparticleviewer.cpp"
 #include <qimage.h>
-
+#include <qevent.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);
+// moc_qparticleviewer.cpp is generated by moc at compile time
+#include "moc_qparticleviewer.cpp"
+
+QParticleViewer::QParticleViewer(QWidget * parent, const char * name, Qt::WindowFlags f, GridSlamProcessorThread* thread) :
+    QWidget(parent, f)
+{
+  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;
+  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(){
+QParticleViewer::~QParticleViewer()
+{
   if (m_pixmap)
     delete m_pixmap;
 }
 
-void QParticleViewer::paintEvent ( QPaintEvent *paintevent ){
-  if (! 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);
+  QPainter painter(this);
+  painter.drawPixmap(0, 0, *m_pixmap, 0, 0, m_pixmap->width(), m_pixmap->height());
 }
 
-void QParticleViewer::mousePressEvent ( QMouseEvent *event ){
-  if (event->button()==LeftButton){
-    dragging=true;
-    draggingPos=event->pos();	
+void QParticleViewer::mousePressEvent(QMouseEvent *event)
+{
+  if (event->button() == Qt::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;
+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::mouseReleaseEvent(QMouseEvent *event)
+{
+  if (event->button() == Qt::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::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){
+void QParticleViewer::resizeEvent(QResizeEvent * sizeev)
+{
   if (!m_pixmap)
     return;
-  cerr << "QParticleViewer::resizeEvent" <<  sizeev->size().width()<< " " << sizeev->size().height() << endl;
-  m_pixmap->resize(sizeev->size());
+  cerr << "QParticleViewer::resizeEvent" << sizeev->size().width() << " " << sizeev->size().height() << endl;
+  m_pixmap->scaled(sizeev->size());
 }
 
-void QParticleViewer::drawParticleMove(const QParticleViewer::OrientedPointVector& oldPose, const QParticleViewer::OrientedPointVector& newPose){
-  assert(oldPose.size()==newPose.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)
-		     );
+  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)
+void QParticleViewer::drawFromFile()
+{
+  if (!tis)
     return;
   if (tis->atEnd())
-    return;	
-  QTextIStream& is=*tis;
-	
-  string line=is.readLine();
+    return;
+  QTextStream& is = *tis;
+
+  string line = is.readLine().toUtf8().constData();
   istringstream lineStream(line);
   string recordType;
   lineStream >> recordType;
-  if (recordType=="LASER_READING"){
+  if (recordType == "LASER_READING") {
     //do nothing with the laser
     cout << "l" << flush;
   }
-  if (recordType=="ODO_UPDATE"){
+  if (recordType == "ODO_UPDATE") {
     //just move the particles
     if (m_particleSize)
-      m_refresh=true;
-    m_oldPose=m_newPose;
+      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++){
+      m_particleSize = size;
+    assert(m_particleSize == size);
+    for (unsigned int i = 0; i < size; i++) {
       OrientedPoint p;
       double w;
       lineStream >> p.x;
@@ -149,17 +172,17 @@ void QParticleViewer::drawFromFile(){
     }
     cout << "o" << flush;
   }
-  if (recordType=="SM_UPDATE"){
+  if (recordType == "SM_UPDATE") {
     if (m_particleSize)
-      m_refresh=true;
-    m_oldPose=m_newPose;
+      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++){
+      m_particleSize = size;
+    assert(m_particleSize == size);
+    for (unsigned int i = 0; i < size; i++) {
       OrientedPoint p;
       double w;
       lineStream >> p.x;
@@ -170,284 +193,297 @@ void QParticleViewer::drawFromFile(){
     }
     cout << "u" << flush;
   }
-  if (recordType=="RESAMPLE"){
+  if (recordType == "RESAMPLE") {
     unsigned int size;
     lineStream >> size;
     if (!m_particleSize)
-      m_particleSize=size;
-    assert(m_particleSize==size);
+      m_particleSize = size;
+    assert(m_particleSize == size);
     OrientedPointVector temp(size);
-    for (unsigned int i=0; i< size; i++){
+    for (unsigned int i = 0; i < size; i++) {
       unsigned int ind;
       lineStream >> ind;
-      temp[i]=m_newPose[ind];
+      temp[i] = m_newPose[ind];
     }
-    m_newPose=temp;
+    m_newPose = temp;
     cout << "r" << flush;
   }
-  if (m_refresh){
+  if (m_refresh) {
     drawParticleMove(m_oldPose, m_newPose);
-    m_refresh=false;
+    m_refresh = false;
   }
 }
 
-void QParticleViewer::drawMap(const ScanMatcherMap& map){
+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);
+  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++){
+   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));
+      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++;
+      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)
+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){
+  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;
+        delete bestMap;
       else {
-				
+
       }
-      bestMap=mapEvent->pmap;
-      mapEvent->pmap=0;
-      bestParticlePose=mapEvent->pose;
+      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);
-    }	
-			
+    }
+    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;
+
+  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);
+
+  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();
+      particleSize = move->hypotheses.size();
     if (resample)
-      particleSize=resample->indexes.size();
+      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;
-	}
+  double wmax = -DBL_MAX;
+  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];
+      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++){
+
+  for (unsigned int i = 0; i < particleSize + 1; i++) {
     painter.setPen(Qt::yellow);
-    unsigned int currentIndex=i;
-    if (i==particleSize && showBestPath){
-      currentIndex=bestIdx;
+    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;
+    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];
+      GridSlamProcessorThread::ResampleEvent* resample = dynamic_cast<GridSlamProcessorThread::ResampleEvent*>(*it);
+      if (resample && !first) {
+        currentIndex = resample->indexes[currentIndex];
       }
     }
   }
-  if (writeToFile && bestMap){
-    if (! (count%writeToFile) ){
+  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");
+      sprintf(name, "dump-%05d.png", count / writeToFile);
+      cout << " Writing " << name << " ..." << flush;
+      QImage image = m_pixmap->toImage();
+      bool rv = image.save(name, "PNG");
       if (rv)
-	cout << " Done";
+        cout << " Done";
       else
-	cout << " ERROR";
+        cout << " ERROR";
       cout << endl;
     }
     count++;
   }
 }
 
-void QParticleViewer::timerEvent(QTimerEvent * te) {
-  if (te->timerId()==timer) {
-    if ( tis)
+void QParticleViewer::timerEvent(QTimerEvent * te)
+{
+  if (te->timerId() == timer) {
+    if (tis)
       drawFromFile();
-    else{
+    else {
       drawFromMemory();
       update();
     }
   }
 }
 
-
-void QParticleViewer::start(int period){
-  timer=startTimer(period);
+void QParticleViewer::start(int period)
+{
+  timer = startTimer(period);
 }
 
-void QParticleViewer::refreshParameters(){
+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();
+  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;
+  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(){
+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);
+      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);
+      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);
+      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::setMatchingParameters(const QParticleViewer::MatchingParameters& mp)
+{
+  matchingParameters = mp;
 }
 
-void QParticleViewer::setStartParameters(const QParticleViewer::StartParameters& sp){
-  startParameters=sp;
+void QParticleViewer::setStartParameters(const QParticleViewer::StartParameters& sp)
+{
+  startParameters = sp;
 }
 
-void QParticleViewer::stop(){
+void QParticleViewer::stop()
+{
   gfs_thread->stop();
 }
 
-void QParticleViewer::loadFile(const char * fn){
+void QParticleViewer::loadFile(const char * fn)
+{
   gfs_thread->loadFiles(fn);
-  /*	
-    startParameters.initialPose=
-    gfs_thread->boundingBox(
-    startParameters.xmin, 
-    startParameters.ymin, 
-    startParameters.xmax,
-    startParameters.ymax);
-  */	
+  /*
+   startParameters.initialPose=
+     gfs_thread->boundingBox(
+       startParameters.xmin,
+       startParameters.ymin,
+       startParameters.xmax,
+       startParameters.ymax);
+   */
 }
diff --git a/gui/qparticleviewer.h b/gui/qparticleviewer.h
index 63ff4fd..4132134 100644
--- a/gui/qparticleviewer.h
+++ b/gui/qparticleviewer.h
@@ -2,25 +2,24 @@
  *
  * This file is part of the GMAPPING project
  *
- * GMAPPING Copyright (c) 2004 Giorgio Grisetti, 
+ * 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, 
+ * 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 
+ * but WITHOUT ANY WARRANTY; without even the implied
  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
- * PURPOSE.  
+ * PURPOSE.
  *
  *****************************************************************/
 
-
 #ifndef QPARTICLEVIEWER_H
 #define QPARTICLEVIEWER_H
 
@@ -35,127 +34,136 @@
 #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;
-};
+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;
+
+    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
 
diff --git a/gui/qpixmapdumper.cpp b/gui/qpixmapdumper.cpp
index 239b209..2ce19b3 100644
--- a/gui/qpixmapdumper.cpp
+++ b/gui/qpixmapdumper.cpp
@@ -1,33 +1,37 @@
 #include "qpixmapdumper.h"
 #include <cstdio>
 #include <cstring>
+#include <qevent.h>
 
-QPixmapDumper::QPixmapDumper(std::string p, int c){
-	format="PNG";
-	prefix=p;
-	reset();
-	cycles=c;
+using namespace GMapping;
+
+QPixmapDumper::QPixmapDumper(std::string p, int c)
+{
+  format = "PNG";
+  prefix = p;
+  reset();
+  cycles = c;
 }
 
-void QPixmapDumper::reset(){
-	cycles=0;
-	frame=0;
-	counter=0;
+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;
+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.toImage();
+    image.save(QString(buf), format.c_str(), 0);
+    frame++;
+  }
+  counter++;
+  return processed;
 }
-
-
diff --git a/gui/qpixmapdumper.h b/gui/qpixmapdumper.h
index 7193d12..ff221f2 100644
--- a/gui/qpixmapdumper.h
+++ b/gui/qpixmapdumper.h
@@ -4,16 +4,21 @@
 #include <qimage.h>
 #include <string>
 
+namespace GMapping
+{
 
-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;
+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
diff --git a/gui/qslamandnavwidget.cpp b/gui/qslamandnavwidget.cpp
index 9c81264..9136098 100644
--- a/gui/qslamandnavwidget.cpp
+++ b/gui/qslamandnavwidget.cpp
@@ -1,128 +1,135 @@
 #include "qslamandnavwidget.h"
 #include <stdio.h>
+#include <qevent.h>
+
 using namespace GMapping;
 
+QSLAMandNavWidget::QSLAMandNavWidget(QWidget * parent, const char * name, Qt::WindowFlags 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( 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()
+{
 }
 
-QSLAMandNavWidget::~QSLAMandNavWidget(){}
+void QSLAMandNavWidget::mousePressEvent(QMouseEvent * e)
+{
+  QPoint p = e->pos();
+  int mx = p.x();
+  int my = height() - p.y();
+  if ((e->modifiers() & Qt::ShiftModifier) && 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;
 
-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;
-	} 
-}
+  }
+  if (e->key() == Qt::Key_Q) {
+    e->accept();
+    wantsQuit = true;
 
-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
-	}
+  }
+  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;
-	}
+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);
+  //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);
+  }
+  QPainter widgetPainter(this);
+  widgetPainter.drawPixmap(0, 0, pixmap, 0, 0, pixmap.width(), pixmap.height());
 }
diff --git a/gui/qslamandnavwidget.h b/gui/qslamandnavwidget.h
index e33ca3f..66979cd 100644
--- a/gui/qslamandnavwidget.h
+++ b/gui/qslamandnavwidget.h
@@ -6,33 +6,38 @@
 #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 );
+namespace GMapping
+{
+
+class QSLAMandNavWidget: public QMapPainter
+{
+  public:
+    QSLAMandNavWidget(QWidget * parent = 0, const char * name = 0, Qt::WindowFlags 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
 
diff --git a/include/gmapping/configfile/configfile.h b/include/gmapping/configfile/configfile.h
new file mode 100644
index 0000000..e6fc856
--- /dev/null
+++ b/include/gmapping/configfile/configfile.h
@@ -0,0 +1,94 @@
+/*****************************************************************
+ *
+ * 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 CONFIGFILE_H
+#define CONFIGFILE_H
+
+#include <iostream>
+#include <string>
+#include <map>
+
+namespace GMapping
+{
+
+class AutoVal
+{
+  public:
+    AutoVal() {}
+    explicit AutoVal(const std::string&);
+    explicit AutoVal(double);
+    explicit AutoVal(int);
+    explicit AutoVal(unsigned int);
+    explicit AutoVal(bool);
+    explicit AutoVal(const char*);
+
+    AutoVal(const AutoVal&);
+    AutoVal& operator=(AutoVal const&);
+
+    AutoVal& operator=(double);
+    AutoVal& operator=(int);
+    AutoVal& operator=(unsigned int);
+    AutoVal& operator=(bool);
+    AutoVal& operator=(std::string const&);
+
+    operator std::string() const;
+    operator double() const;
+    operator int() const;
+    operator unsigned int() const;
+    operator bool() const;
+
+  protected:
+    std::string toLower(std::string const& source) const;
+
+  private:
+    std::string m_value;
+};
+
+class ConfigFile
+{
+    std::map<std::string, AutoVal> m_content;
+
+  public:
+    ConfigFile();
+    ConfigFile(std::string const& configFile);
+
+    bool read(std::string const& configFile);
+
+    AutoVal const& value(std::string const& section, std::string const& entry) const;
+    AutoVal const& value(std::string const& section, std::string const& entry, double def);
+    AutoVal const& value(std::string const& section, std::string const& entry, bool def);
+    AutoVal const& value(std::string const& section, std::string const& entry, int def);
+    AutoVal const& value(std::string const& section, std::string const& entry, unsigned int def);
+    AutoVal const& value(std::string const& section, std::string const& entry, std::string const& def);
+
+    void dumpValues(std::ostream& out);
+
+  protected:
+    std::string trim(std::string const& source, char const* delims = " \t\r\n") const;
+    std::string truncate(std::string const& source, const char* atChar) const;
+    std::string toLower(std::string const& source) const;
+    void insertValue(std::string const& section, std::string const& entry, std::string const& thevalue);
+};
+
+}
+
+#endif
diff --git a/log/carmenconfiguration.h b/include/gmapping/log/carmenconfiguration.h
similarity index 100%
rename from log/carmenconfiguration.h
rename to include/gmapping/log/carmenconfiguration.h
diff --git a/log/sensorstream.h b/include/gmapping/log/sensorstream.h
similarity index 100%
rename from log/sensorstream.h
rename to include/gmapping/log/sensorstream.h
diff --git a/include/gmapping/utils/gvalues.h b/include/gmapping/utils/gvalues.h
index 3c192e1..9a0e396 100644
--- a/include/gmapping/utils/gvalues.h
+++ b/include/gmapping/utils/gvalues.h
@@ -1,16 +1,13 @@
 #ifndef _GVALUES_H_
 #define _GVALUES_H_
 
-#define MAXDOUBLE 1e1000
-#ifdef LINUX
-	#include <values.h>
-#endif
-#ifdef MACOSX
-	#include <limits.h>
+#if __linux__ or __APPLE__
+	#include <climits>
+  #include <cfloat>
 	#include <math.h>
-	//#define isnan(x) (x==FP_NAN)
 #endif
-#ifdef _WIN32
+
+#if _WIN32
   #include <limits>
   #ifndef __DRAND48_DEFINED__
      #define __DRAND48_DEFINED__
@@ -24,5 +21,4 @@
   #define isnan(x) (_isnan(x))
 #endif
 
-#endif 
-
+#endif
diff --git a/log/CMakeLists.txt b/log/CMakeLists.txt
new file mode 100644
index 0000000..6b32fdc
--- /dev/null
+++ b/log/CMakeLists.txt
@@ -0,0 +1,24 @@
+include_directories(../include/gmapping)
+add_library(log
+    carmenconfiguration.cpp
+    configuration.cpp
+    log_plot.cpp
+    sensorlog.cpp
+    sensorstream.cpp
+)
+install(TARGETS log DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
+
+add_executable(rdk2carmen rdk2carmen.cpp)
+add_executable(scanstudio2carmen scanstudio2carmen.cpp)
+add_executable(log_plot log_plot.cpp)
+add_executable(log_test log_test.cpp)
+
+target_link_libraries(rdk2carmen log sensor_odometry sensor_range)
+target_link_libraries(scanstudio2carmen log sensor_odometry sensor_range)
+target_link_libraries(log_plot log sensor_odometry sensor_range)
+target_link_libraries(log_test log sensor_odometry sensor_range)
+
+install(TARGETS rdk2carmen DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+install(TARGETS scanstudio2carmen DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+install(TARGETS log_plot DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+install(TARGETS log_test DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
diff --git a/log/Makefile b/log/Makefile
deleted file mode 100644
index 02a0515..0000000
--- a/log/Makefile
+++ /dev/null
@@ -1,9 +0,0 @@
-OBJS= configuration.o carmenconfiguration.o sensorlog.o sensorstream.o
-APPS= log_test log_plot scanstudio2carmen rdk2carmen
-
-LDFLAGS+=  -lsensor_range -lsensor_odometry -lsensor_base 
-CPPFLAGS+= -I../sensor 
-
--include ../global.mk
--include ../build_tools/Makefile.generic-shared-object
-
diff --git a/log/carmenconfiguration.cpp b/log/carmenconfiguration.cpp
index 13784e3..b852ca7 100644
--- a/log/carmenconfiguration.cpp
+++ b/log/carmenconfiguration.cpp
@@ -1,11 +1,11 @@
 #include <cstdlib>
-#include "carmenconfiguration.h"
 #include <iostream>
 #include <sstream>
 #include <assert.h>
 #include <sys/types.h>
-#include <sensor_odometry/odometrysensor.h>
-#include <sensor_range/rangesensor.h>
+#include <log/carmenconfiguration.h>
+#include <sensor/sensor_odometry/odometrysensor.h>
+#include <sensor/sensor_range/rangesensor.h>
 
 
 #define LINEBUFFER_SIZE 10000
diff --git a/log/configuration.cpp b/log/configuration.cpp
index 9c81c5e..0ed9061 100644
--- a/log/configuration.cpp
+++ b/log/configuration.cpp
@@ -1,4 +1,4 @@
-#include "configuration.h"
+#include <log/configuration.h>
 
 namespace GMapping {
 
diff --git a/log/sensorlog.cpp b/log/sensorlog.cpp
index ca5892d..ae52ad1 100644
--- a/log/sensorlog.cpp
+++ b/log/sensorlog.cpp
@@ -1,10 +1,9 @@
-#include "sensorlog.h"
-
 #include <iostream>
 #include <sstream>
 #include <assert.h>
-#include <sensor_odometry/odometrysensor.h>
-#include <sensor_range/rangesensor.h>
+#include <log/sensorlog.h>
+#include <sensor/sensor_odometry/odometrysensor.h>
+#include <sensor/sensor_range/rangesensor.h>
 
 #define LINEBUFFER_SIZE 100000
 
diff --git a/log/sensorstream.cpp b/log/sensorstream.cpp
index 3c27b04..2405f22 100644
--- a/log/sensorstream.cpp
+++ b/log/sensorstream.cpp
@@ -1,6 +1,6 @@
 #include <assert.h>
 #include <sstream>
-#include "sensorstream.h"
+#include <log/sensorstream.h>
 //#define LINEBUFFER_SIZE 1000000 //for not Cyrill to unbless me, it is better to exagerate :-))
 // Can't declare a buffer that big on the stack.  So we'll risk Cyrill's
 // unblessing, and make it smaller.
diff --git a/particlefilter/CMakeLists.txt b/particlefilter/CMakeLists.txt
new file mode 100644
index 0000000..72d1318
--- /dev/null
+++ b/particlefilter/CMakeLists.txt
@@ -0,0 +1,7 @@
+include_directories(../include/gmapping)
+
+add_executable(particlefilter_test particlefilter_test.cpp)
+add_executable(range_bearing range_bearing.cpp)
+
+install(TARGETS particlefilter_test DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+install(TARGETS range_bearing DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
diff --git a/particlefilter/particlefilter.cpp b/particlefilter/particlefilter.cpp
deleted file mode 100644
index e49999a..0000000
--- a/particlefilter/particlefilter.cpp
+++ /dev/null
@@ -1,133 +0,0 @@
-std::vector<unsigned int> sistematicResampler<State,Numeric>::resample(const vector<Particle>& particles) const{
-	Numeric cweight=0;
-
-	//compute the cumulative weights
-	unsigned int n=0;
-	for (vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it){
-		cweight+=it->weight;
-		n++;
-	}
-
-	//compute the interval
-	Numeric interval=cweight/n;
-
-	//compute the initial target weight
-	Numeric target=
-	//compute the resampled indexes
-
-	cweight=0;
-	std::vector<int> indexes(n);
-	n=0;
-	unsigned int i=0;
-	for (vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it, ++i){
-		cweight+=it->weight;
-		while(cweight>target){
-			indexes[n++]=i;
-			target+=interval;
-		}
-	}
-	return indexes;
-	}
-
-template <class Numeric>
-std::vector<unsigned int> indexResampler<Numeric>::resample(const vector<Numeric> >& weights) const{
-	Numeric cweight=0;
-
-	//compute the cumulative weights
-	unsigned int n=0;
-	for (vector<Numeric>::const_iterator it=weights.begin(); it!=weights.end(); ++it){
-		cweight+=*it;
-		n++;
-	}
-
-	//compute the interval
-	Numeric interval=cweight/n;
-
-	//compute the initial target weight
-	Numeric target=
-	//compute the resampled indexes
-
-	cweight=0;
-	std::vector<int> indexes(n);
-	n=0;
-	unsigned int i=0;
-	for (vector<Numeric>::const_iterator it=weights.begin(); it!=weights.end(); ++it, ++i){
-		cweight+=it->weight;
-		while(cweight>target){
-			indexes[n++]=i;
-			target+=interval;
-		}
-	}
-	return indexes;
-}
-
-/*
-
-The following are patterns for the evolution and the observation classes
-The user should implement classes having the specified meaning
-
-template <class State, class Numeric, class Observation>
-struct observer{
-	Observation& observation
-	Numeric observe(const class State&) const;
-};
-
-template <class State, class Numeric, class Input>
-struct evolver{
-	Input& input;
-	State& evolve(const State& s);
-};
-*/
-
-template <class State, class Numeric, class EvolutionModel>
-void evolver<State, Numeric, EvolutionModel>::evolve(std::vector<evolver::Particle>& particles) const{
-	for (std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it)
-		*it=evolutionModel.evolve(*it);
-}
-
-void evolver<State, Numeric, EvolutionModel>::evolve(std::vector<evolver::Particle>& dest, const std::vector<evolver::Particle>& src) const{
-	dest.clear();
-	for (std::vector<Particle>::const_iterator it=src.begin(); it!=src.end(); ++it)
-		dest.push_back(evolutionModel.evolve(*it));
-}
-
-template <class State, class Numeric, class QualificationModel, class EvolutionModel, class LikelyhoodModel>
-struct auxiliaryEvolver{
-	typedef particle<State, Numeric> Particle;
-
-	EvolutionModel evolutionModel;
-	QualificationModel qualificationModel;
-	LikelyhoodModel likelyhoodModel;
-	indexResampler<Numeric> resampler;
-
-void auxiliaryEvolver<State, Numeric, QualificationModel, EvolutionModel, LikelyhoodModel>::evolve
-  (std::vector<auxiliaryEvolver::Particle>&particles){
-	std::vector<Numeric> observationWeights(particles.size());
-	unsigned int i=0;
-	for (std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it, i++){
-		observationWeights[i]=likelyhoodModel.likelyhood(qualificationModel.evolve(*it));
-	}
-	std::vector<unsigned int> indexes(indexResampler.resample(observationWeights));
-	for (std::vector<unsigned int>::const_iterator it=indexes.begin(); it!=indexes.end(); it++){
-		Particle & particle=particles[*it];
-		particle=evolutionModel.evolve(particle);
-		particle.weight*=lykelyhoodModel.lykelyhood(particle)/observationWeights[*it];
-	}
-}
-
-void auxiliaryEvolver<State, Numeric, QualificationModel, EvolutionModel, LikelyhoodModel>::evolve
-  (std::vector<Particle>& dest, const std::vector<Particle>& src){
-	dest.clear();
-	std::vector<Numeric> observationWeights(particles.size());
-	unsigned int i=0;
-	for (std::vector<Particle>::const_iterator it=src.begin(); it!=src.end(); ++it, i++){
-		observationWeights[i]=likelyhoodModel.likelyhood(qualificationModel.evolve(*it));
-	}
-	std::vector<unsigned int> indexes(indexResampler.resample(observationWeights));
-	for (std::vector<unsigned int>::const_iterator it=indexes.begin(); it!=indexes.end(); it++){
-		Particle & particle=src[*it];
-		dest.push_back(evolutionModel.evolve(particle));
-		dest.back().weight*=likelyhoodModel.lykelyhood(particle)/observationWeights[*it];
-	}
-	return dest();
-}
diff --git a/particlefilter/particlefilter_test.cpp b/particlefilter/particlefilter_test.cpp
index 28defed..c8af3df 100644
--- a/particlefilter/particlefilter_test.cpp
+++ b/particlefilter/particlefilter_test.cpp
@@ -1,7 +1,7 @@
 #include <stdlib.h>
 #include <iostream>
 #include <fstream>
-#include "particlefilter.h"
+#include <particlefilter/particlefilter.h>
 
 using namespace std;
 
@@ -44,7 +44,7 @@ struct LikelyhoodModel{
 	}
 };
 
-int main (unsigned int argc, const char * const * argv){
+int main (int argc, const char * const * argv){
 	int nparticles=100;
 	if (argc>1)
 		nparticles=atoi(argv[1]);
@@ -91,7 +91,7 @@ int main (unsigned int argc, const char * const * argv){
 		auxparticles=newgeneration;
 		cout << "plot [0:10][0:10]\"sir.dat\" w impulses" << endl;
 		cout << "replot 1./(0.1+10*(x-2)*(x-2))+0.5/(0.1+10*(x-8)*(x-8))" << endl;
-		
+
 //		cout << "replot \"aux.dat\" w p" << endl;
 	}
 }
diff --git a/particlefilter/pf.h b/particlefilter/pf.h
deleted file mode 100644
index 374b0d8..0000000
--- a/particlefilter/pf.h
+++ /dev/null
@@ -1,175 +0,0 @@
-#ifndef PARTICLEFILTER_H
-#define PARTICLEFILTER_H
-#include <stdlib.h>
-#include<vector>
-#include<utility>
-#include<utils/gvalues.h>
-
-
-
-/**
-the particle class has to be convertible into numeric data type;
-That means that a particle must define the Numeric conversion operator;
-	operator Numeric() const.
-that returns the weight, and the method
-	setWeight(Numeric)
-that sets the weight.
-
-*/
-
-typedef std::pair<uint,uint> UIntPair;
-
-template <class OutputIterator, class Iterator>
-double toNormalForm(OutputIterator& out, const Iterator & begin, const Iterator & end){
-	//determine the maximum
-	double lmax=-MAXDOUBLE;
-	for (Iterator it=begin; it!=end; it++){
-		lmax=lmax>((double)(*it))? lmax: (double)(*it);
-	}
-	//convert to raw form
-	for (Iterator it=begin; it!=end; it++){
-		*out=exp((double)(*it)-lmax);
-		out++;
-	}
-	return lmax;
-}
-
-template <class OutputIterator, class Iterator,	class Numeric>
-void toLogForm(OutputIterator& out, const Iterator & begin, const Iterator & end, Numeric lmax){
-	//determine the maximum
-	for (Iterator it=begin; it!=end; it++){
-		*out=log((Numeric)(*it))-lmax;
-		out++;
-	}
-	return lmax;
-}
-
-template <class WeightVector>
-void resample(std::vector<int>& indexes, const WeightVector& weights, unsigned int nparticles=0){
-	double cweight=0;
-	
-	//compute the cumulative weights
-	unsigned int n=0;
-	for (typename WeightVector::const_iterator it=weights.begin(); it!=weights.end(); ++it){
-		cweight+=(double)*it;
-		n++;
-	}
-
-	if (nparticles>0)
-		n=nparticles;
-	
-	//compute the interval
-	double interval=cweight/n;
-
-	//compute the initial target weight
-	double target=interval*::drand48();
-	//compute the resampled indexes
-
-	cweight=0;
-	indexes.resize(n);
-	
-	n=0;
-	unsigned int i=0;
-	for (typename WeightVector::const_iterator it=weights.begin(); it!=weights.end(); ++it, ++i){
-		cweight+=(double)* it;
-		while(cweight>target){
-			indexes[n++]=i;
-			target+=interval;
-		}
-	}
-}
-
-template <typename WeightVector>
-void normalizeWeights(WeightVector& weights, unsigned int size, double minWeight){
-	double wmin=MAXDOUBLE;
-	double wmax=-MAXDOUBLE;
-	for (uint i=0; i<size; i++){
-		wmin=wmin<weights[i]?wmin:weights[i];
-		wmax=wmax>weights[i]?wmax:weights[i];
-	}
-	double min_normalized_value=log(minWeight);
-	double max_normalized_value=log(1.);
-	double dn=max_normalized_value-min_normalized_value;
-	double dw=wmax-wmin;
-	if (dw==0) dw=1;
-	double scale=dn/dw;
-	double offset=-wmax*scale;
-	for (uint i=0; i<size; i++){
-		double w=weights[i];
-		w=scale*w+offset;
-		weights[i]=exp(w);
-	}
-}
-
-template <typename Vector>
-void repeatIndexes(Vector& dest, const std::vector<int>& indexes, const Vector& particles){
-/*<<<<<<< .mine
-	assert(indexes.size()==particles.size());
-	if (dest.size()!=particles.size())
-		dest.resize(particles.size());
-=======*/
-	//assert(indexes.size()==particles.size()); //DIEGO non ne vedo il senso, anzi è sbagliata
-	//dest.resize(particles.size());	    //      è sbagliato anche questo
-	dest.resize(indexes.size());
-// >>>>>>> .r2534
-	unsigned int i=0;
-	for (std::vector<int>::const_iterator it=indexes.begin(); it!=indexes.end(); ++it){
-		dest[i]=particles[*it];
-		i++;
-	}
-}
-
-template <typename Vector>
-void repeatIndexes(Vector& dest, const std::vector<int>& indexes2, const Vector& particles, const std::vector<int>& indexes){
-  //	assert(indexes.size()==indexes2.size());
-	dest=particles;
-	unsigned int i=0;
-	for (std::vector<int>::const_iterator it=indexes2.begin(); it!=indexes2.end(); ++it){
-		dest[indexes[i]]=particles[*it];
-		i++;
-	}
-}
-
-
-template <class Iterator>
-double neff(const Iterator& begin, const Iterator& end){
-	double sum=0;
-	for (Iterator it=begin; it!=end; ++it){
-		sum+=*it;
-	}
-	double cum=0;
-	for (Iterator it=begin; it!=end; ++it){
-		double w=*it/sum;
-		cum+=w*w;
-	}
-	return 1./cum;
-}
-
-
-
-template <class OutputIterator, class Iterator>
-void rle(OutputIterator& out, const Iterator & begin, const Iterator & end){
-	unsigned int current=0;
-	unsigned int count=0;
-	for (Iterator it=begin; it!=end; it++){
-		if (it==begin){
-			current=*it;
-			count=1;
-			continue;
-		}
-		if (((uint)*it) ==current)
-			count++;
-		if (((uint)*it)!=current){
-			*out=std::make_pair(current,count);
-			out++;
-			current=*it;
-			count=1;
-		}
-	}
-	if (count>0)
-		*out=std::make_pair(current,count);
-		out++;
-}
-
-#endif
-
diff --git a/particlefilter/range_bearing.cpp b/particlefilter/range_bearing.cpp
index 44f99e0..15b1432 100644
--- a/particlefilter/range_bearing.cpp
+++ b/particlefilter/range_bearing.cpp
@@ -2,7 +2,7 @@
 #include <iostream>
 #include <fstream>
 #include <utils/point.h>
-#include "particlefilter.h"
+#include <particlefilter/particlefilter.h>
 
 using namespace std;
 using namespace GMapping;
@@ -52,7 +52,7 @@ struct LikelyhoodModel{
 	}
 };
 
-int main (unsigned int argc, const char * const * argv){
+int main (int argc, const char * const * argv){
 	vector<Particle> particles(1000);
 	LikelyhoodModel likelyhoodModel;
 	uniform_resampler<Particle, double> resampler;
@@ -63,18 +63,18 @@ int main (unsigned int argc, const char * const * argv){
 		it->p.x=400*(drand48()-.5);
 		it->p.y=400*(drand48()-.5);
 	}
-	
+
 	vector<Point> sensors;
-	
+
 	sensors.push_back(Point(-50,0));
 	sensors.push_back(Point(50,0));
 	sensors.push_back(Point(0,100));
-	
+
 	likelyhoodModel.sigma=1000;
 	likelyhoodModel.observations.push_back(70);
 	likelyhoodModel.observations.push_back(70);
 	likelyhoodModel.observations.push_back(70);
-	
+
 	likelyhoodModel.observerVector=sensors;
 	while (1){
 		char buf[2];
@@ -86,7 +86,7 @@ int main (unsigned int argc, const char * const * argv){
 		for (vector<Particle>::iterator it=particles.begin(); it!=particles.end(); it++){
 			it->w*=likelyhoodModel.likelyhood(*it);
 		}
-		
+
 		ofstream os("sir.dat");
 		printParticles(os, particles);
 		os.close();
diff --git a/scanmatcher/CMakeLists.txt b/scanmatcher/CMakeLists.txt
index 64c1d2f..ca20a37 100644
--- a/scanmatcher/CMakeLists.txt
+++ b/scanmatcher/CMakeLists.txt
@@ -1,4 +1,3 @@
 add_library(scanmatcher eig3.cpp scanmatcher.cpp scanmatcherprocessor.cpp smmap.cpp)
 target_link_libraries(scanmatcher sensor_range utils)
-
 install(TARGETS scanmatcher DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
diff --git a/sensor/Makefile b/sensor/Makefile
deleted file mode 100644
index 6b9f784..0000000
--- a/sensor/Makefile
+++ /dev/null
@@ -1,5 +0,0 @@
--include ../global.mk
-
-SUBDIRS=sensor_base sensor_odometry sensor_range
-
--include ../build_tools/Makefile.subdirs
diff --git a/sensor/sensor_base/CMakeLists.txt b/sensor/sensor_base/CMakeLists.txt
index e0317d8..7bb883b 100644
--- a/sensor/sensor_base/CMakeLists.txt
+++ b/sensor/sensor_base/CMakeLists.txt
@@ -1,3 +1,2 @@
-include_directories(./)
 add_library(sensor_base sensor.cpp sensorreading.cpp)
 install(TARGETS sensor_base DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
diff --git a/sensor/sensor_odometry/CMakeLists.txt b/sensor/sensor_odometry/CMakeLists.txt
index 55adc26..b59641c 100644
--- a/sensor/sensor_odometry/CMakeLists.txt
+++ b/sensor/sensor_odometry/CMakeLists.txt
@@ -1,4 +1,3 @@
 add_library(sensor_odometry odometryreading.cpp odometrysensor.cpp)
 target_link_libraries(sensor_odometry sensor_base)
-
 install(TARGETS sensor_odometry DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
diff --git a/sensor/sensor_range/CMakeLists.txt b/sensor/sensor_range/CMakeLists.txt
index 95444bb..6cd4ca3 100644
--- a/sensor/sensor_range/CMakeLists.txt
+++ b/sensor/sensor_range/CMakeLists.txt
@@ -1,4 +1,3 @@
 add_library(sensor_range rangereading.cpp rangesensor.cpp)
 target_link_libraries(sensor_range sensor_base)
-
 install(TARGETS sensor_range DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
diff --git a/sensor/sensor_range/rangereading.cpp b/sensor/sensor_range/rangereading.cpp
index 6bb8946..4d8b5d1 100644
--- a/sensor/sensor_range/rangereading.cpp
+++ b/sensor/sensor_range/rangereading.cpp
@@ -40,7 +40,7 @@ unsigned int RangeReading::rawView(double* v, double density) const{
 			Point dp=lastPoint-lp;
 			double distance=sqrt(dp*dp);
 			if (distance<density){
-			  //				v[i]=MAXDOUBLE;
+			  //				v[i]=DBL_MAX;
 				v[i]=std::numeric_limits<double>::max();
 				suppressed++;
 			}
diff --git a/utils/datasmoother.h b/utils/datasmoother.h
index ddd8553..58ba771 100644
--- a/utils/datasmoother.h
+++ b/utils/datasmoother.h
@@ -34,8 +34,8 @@ class DataSmoother {
     m_cummulated.clear();
     m_int=-1; 
     m_parzenWindow = parzenWindow;
-    m_from = MAXDOUBLE;
-    m_to = -MAXDOUBLE;
+    m_from = DBL_MAX;
+    m_to = -DBL_MAX;
     m_lastStep = 0.001;
   };
 
@@ -46,7 +46,7 @@ class DataSmoother {
 
 
   void setMinToZero() {
-    double minval=MAXDOUBLE;
+    double minval=DBL_MAX;
 
     for (Data::const_iterator it = m_data.begin(); it != m_data.end(); it++) {
       const DataPoint& d = *it;
@@ -324,8 +324,8 @@ class DataSmoother {
 /*   DataSmoother(double parzenWindow) { */
 /*     m_int=-1;  */
 /*     m_parzenWindow = parzenWindow; */
-/*     m_from = InputPoint(MAXDOUBLE,MAXDOUBLE,MAXDOUBLE); */
-/*     m_to = InputPoint(-MAXDOUBLE,-MAXDOUBLE,-MAXDOUBLE); */
+/*     m_from = InputPoint(DBL_MAX,DBL_MAX,DBL_MAX); */
+/*     m_to = InputPoint(-DBL_MAX,-DBL_MAX,-DBL_MAX); */
 /*   }; */
 
 /*   virtual ~DataSmoother() { */
@@ -338,7 +338,7 @@ class DataSmoother {
 
 
 /*   void setMinToZero() { */
-/*     double minval=MAXDOUBLE; */
+/*     double minval=DBL_MAX; */
 /*     for (Data::const_iterator it = m_data.begin(); it != m_data.end(); it++) { */
 /*       const DataPoint& d = *it; */
 /*       if (minval > d.val) */