add slam_gmapping

slam_gmapping/.gitignore

@@ -0,0 +1,29 @@
+*/build/
+*/build_isolated/
+*/devel/
+*/devel_isolated/
+*/build_isolated/
+*/install_isolated/
+*/logs/
+docs/
+*/.catkin_workspace
+*/DISABLED
+*.bag
+*.pyc
+cmake-build-debug/
+cmake-build-release/
+*.idea
+*.swp
+*.kate-swp
+*.director
+*.DS_STORE
+*.catkin_tools
+*.bag
+*.bag.active
+*.stl
+*.directory
+build
+log
+!openslam_gmapping/include/gmapping/log/
+!openslam_gmapping/log/
+install

slam_gmapping/README.md

@@ -0,0 +1,17 @@
+# SLAM_GMAPPING
+
+SLAM(Simultaneous Localization and Mapping) is the computational problem of constructing or updating a map of an unknown environment while simultaneously keeping track of an agent's location within it.
+
+This contains package ```openslam_gmapping``` and ```slam_gmapping``` which is a ROS2 wrapper for OpenSlam's Gmapping. Using slam_gmapping, you can create a 2-D occupancy grid map (like a building floorplan) from laser and pose data collected by a mobile robot.
+
+## Launch:
+
+```bash
+ros2 launch slam_gmapping slam_gmapping.launch.py
+```
+
+The node slam_gmapping subscribes to sensor_msgs/LaserScan on ros2 topic ``scan``. It also expects appropriate TF to be available.
+
+It publishes the nav_msgs/OccupancyGrid on ``map``. 
+
+Map Meta Data and Entropy is published on ``map_metadata`` and ``entropy`` respectively.

slam_gmapping/openslam_gmapping/CHANGELOG.rst

@@ -0,0 +1,25 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package openslam_gmapping
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.1.2 (2016-04-23)
+------------------
+* better Windows compilation
+  This is taken from `#9 <https://github.com/ros-perception/openslam_gmapping/issues/9>`_ which can now be closed.
+* fix a few more graphics stuff for Qt5
+* get GUI back in shape for those interested
+* use srand instead of srand48
+  srand48 is non-standard and we are using a seed that is an
+  unsigned int so we might as well use srand
+* Contributors: Vincent Rabaud
+
+0.1.1 (2015-06-25)
+------------------
+* fix cppcheck warnings
+* License from BSD to CC
+* Contributors: Isaac IY Saito, Vincent Rabaud
+
+0.1.0 (2013-06-28 17:33:53 -0700)
+---------------------------------
+- Forked from https://openslam.informatik.uni-freiburg.de/data/svn/gmapping/trunk/
+- Catkinized and prepared for release into the ROS ecosystem

slam_gmapping/openslam_gmapping/CMakeLists.txt

@@ -0,0 +1,39 @@
+cmake_minimum_required(VERSION 3.5)
+project(openslam_gmapping)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+    set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+    set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+    add_compile_options(-O3 -funroll-loops)
+endif()
+
+add_compile_options(-fPIC)
+find_package(ament_cmake REQUIRED)
+
+include_directories(include)
+
+add_subdirectory(gridfastslam)
+add_subdirectory(scanmatcher)
+add_subdirectory(sensor)
+add_subdirectory(utils)
+
+ament_export_libraries(gridfastslam)
+ament_export_libraries(scanmatcher)
+ament_export_libraries(sensor_base)
+ament_export_libraries(sensor_odometry)
+ament_export_libraries(sensor_range)
+ament_export_libraries(utils)
+ament_export_include_directories(include)
+
+install(DIRECTORY include/
+        DESTINATION include/)
+
+ament_package()

slam_gmapping/openslam_gmapping/README

@@ -0,0 +1,2 @@
+This is a fork from gmapping at https://openslam.informatik.uni-freiburg.de/data/svn/gmapping/trunk/
+It includes a few patches that could be pushed upstream if needed

slam_gmapping/openslam_gmapping/build_tools/Makefile.app

@@ -0,0 +1,80 @@
+# Makefile generico per applicazione
+#
+# Variabili:
+# APPS lista delle applicazioni
+# OBJS lista degli oggetti
+# QOBJS lista degli oggetti QT
+# LIBS librerie
+#
+# Ogni applicazione viene linkata con tutti gli oggetti
+
+export VERBOSE
+
+ifeq ($(LINUX),1)
+CPPFLAGS+=-DLINUX
+endif
+
+
+APPLICATIONS= $(foreach a, $(APPS),$(BINDIR)/$(a))
+all: $(APPLICATIONS)
+
+PACKAGE=$(notdir $(shell pwd))
+
+.SECONDARY:		$(OBJS) $(QOBJS)
+.PHONY:			all clean copy doc
+
+$(QOBJS): %.o: %.cpp moc_%.cpp
+	@$(MESSAGE) "Compiling (QT) $@"
+	@$(PRETTY) "$(CXX) $(CPPFLAGS) $(QT_INCLUDE) $(CXXFLAGS) -c $< -o $@"
+
+moc_%.cpp: %.h
+	@$(MESSAGE) "Generating MOC $@"
+	@$(PRETTY) "$(MOC) -i $< -o $@"
+
+# Generazione degli oggetti
+%.o: %.cpp
+	@$(MESSAGE) "Compiling $@"
+	@$(PRETTY) "$(CXX) $(CPPFLAGS) $(CXXFLAGS) -c $< -o $@"
+
+# Generazione delle applicazioni
+$(BINDIR)/%: %.cpp $(OBJS) $(QOBJS)
+	@$(MESSAGE) "Linking application `basename $@`"
+	@$(PRETTY) "$(CXX) $(CPPFLAGS) $(CXXFLAGS) $(OBJS) $(QOBJS) $< -L$(LIBDIR) $(LIBS) -o $@"
+
+#Regole per la generazione di tabelle o altri file creati automaticamente
+table_%.cpp: gen_table_%
+	@$(MESSAGE) "Generating $@"
+	@$(PRETTY) "./$< > $@"
+
+gen_table_%: gen_table_%.cpp
+	@$(MESSAGE) "Generating $@"
+	@$(PRETTY) "$(CXX) $(CPPFLAGS) $(CXXFLAGS) $< -o $@"
+
+#Regole per la generazione delle dipendenze
+OBJDEPS=$(foreach module,$(basename $(OBJS) $(QOBJS)),$(module).d)
+
+$(OBJDEPS): %.d: %.cpp # ci va o no? %.h
+	@$(MESSAGE) "Generating dependecies $@"
+	@$(PRETTY) "$(CXX) $(CPPFLAGS) -MM -MG -MF $@ $<"
+
+ifneq ($(MAKECMDGOALS),clean)
+ifneq ($(MAKECMDGOALS),copy)
+-include $(OBJDEPS)
+endif
+endif
+
+doc:
+	rm -rf doc/$(PACKAGE)
+ifeq ($(strip $(DOCTITLE)),)
+	kdoc -L doc -d doc/$(PACKAGE) -n "Package $(PACKAGE) (lib$(PACKAGE).so)" $(HEADERS)
+else
+	kdoc -L doc -d doc/$(PACKAGE) -n "$(DOCTITLE) (lib$(PACKAGE).so)" $(HEADERS)
+endif
+
+clean:
+	@$(MESSAGE) "Cleaning $(PACKAGE)"
+	@$(PRETTY) "rm -f *.d *.o  moc_*.cpp *.d core *~ table_*.cpp  gen_table*[^.][^c][^p][^p] $(APPLICATIONS)"
+	@$(PRETTY) "rm -rf doc/$(PACKAGE)"
+
+copy:	clean
+	tar -C .. -cvzf `date +../$(PACKAGE)-%d%b%y.tgz` $(PACKAGE)

slam_gmapping/openslam_gmapping/build_tools/Makefile.generic-shared-object

@@ -0,0 +1,109 @@
+# Makefile generico per shared object
+export VERBOSE
+export CXX
+
+# Nome del package
+PACKAGE=$(notdir $(shell pwd))
+
+# Libreria da generare:
+# Se non si setta la variabile LIBNAME la libreria si chiama
+# come la directory
+ifndef LIBNAME
+LIBNAME=$(PACKAGE)
+endif
+
+ifeq ($(MACOSX),1)
+SONAME=$(LIBDIR)/lib$(LIBNAME).dylib
+endif
+
+ifeq ($(LINUX),1)
+SONAME=$(LIBDIR)/lib$(LIBNAME).so
+endif
+
+APPLICATIONS= $(foreach a, $(APPS),$(BINDIR)/$(a))
+INSTALL_SCRIPTS=$(foreach a, $(SCRIPTS),$(BINDIR)/$(a))
+
+all: $(SONAME) $(APPLICATIONS) $(INSTALL_SCRIPTS)
+
+.SECONDARY: $(OBJS) $(COBJS)
+.PHONY:     all clean copy doc
+
+# Generazione della libreria
+$(SONAME): $(OBJS) $(COBJS)
+	@$(MESSAGE) "Creating library lib$(LIBNAME).so"
+ifeq ($(MACOSX),1)
+	@$(PRETTY) "$(CXX) $(LDFLAGS)  -dynamiclib $(OBJS) $(COBJS) -L$(LIBDIR) $(LIBS) -install_name $@ -o $@"
+endif
+ifeq ($(LINUX),1)
+ 	@$(PRETTY) "$(CXX) -fPIC -shared $(OBJS) $(COBJS) -L $(LIBDIR) $(LIBS) $(LDFLAGS) -o $@"
+	@if ! $(PRETTY) "$(TESTLIB) $(SONAME)"; then $(MESSAGE) "Testing of $(SONAME) failed."; rm $(SONAME); exit 1; fi;
+endif
+
+# Generazione delle applicazioni
+$(BINDIR)/%: %.o $(SONAME)
+	@$(MESSAGE) "Linking application `basename "$@"`"
+	@$(PRETTY) "$(CXX) $< -l$(LIBNAME) $(LDFLAGS) -L$(LIBDIR) $(LIBS)  -o $@"
+
+#Generazione dei moc files
+moc_%.cpp:              %.h
+	@$(MESSAGE) "Compiling MOC $@"
+	@$(PRETTY) "$(MOC) -i $< -o $@"
+
+# Generazione degli oggetti
+%.o: %.cpp
+	@$(MESSAGE) "Compiling $<"
+	@$(PRETTY) "$(CXX) -fPIC $(CPPFLAGS) $(CXXFLAGS) -c $< -o $@"
+
+%.o: %.c
+	@$(MESSAGE) "Compiling $<"
+	@$(PRETTY) "$(CC) -fPIC $(CPPFLAGS) $(CFLAGS) -c $< -o $@"
+
+#Regole per la generazione delle dipendenze
+OBJDEPS= $(foreach module,$(basename $(OBJS)),$(module).d) $(foreach a, $(APPS),$(a).d)
+COBJDEPS=$(foreach module,$(basename $(COBJS)),$(module).d)
+
+$(OBJDEPS): %.d: %.cpp
+	@$(MESSAGE) "Generating dependencies for $<"
+	@$(PRETTY) "$(CXX) $(CPPFLAGS) -MM -MG $< -MF $@"
+
+$(COBJDEPS): %.d: %.c
+	@$(MESSAGE) "Generating dependencies for $<"
+	@$(PRETTY) "$(CC)  $(CPPFLAGS) -MM -MG $< -MF $@"
+
+#HEADERS=`ls *.h`
+#PRECOMPILED_HEADERS=$(foreach file,$(basename $(HEADERS)), $(file).pch)
+
+ifneq ($(MAKECMDGOALS),clean)
+ifneq ($(MAKECMDGOALS),copy)
+ifneq ($(MAKECMDGOALS),dep)
+-include $(OBJDEPS) $(COBJDEPS)
+endif
+endif
+endif
+
+dep: $(OBJDEPS) $(COBJDEPS)
+
+
+# GLi script vengono semplicemente copiati
+$(BINDIR)/%.sh: %.sh
+	@$(MESSAGE) "Installing script `basename "$@"`"
+	@$(PRETTY) "cp $< $@"
+	@$(PRETTY) "chmod +x $@"
+
+
+#doc:
+#	rm -rf doc/$(PACKAGE)
+#ifeq ($(strip $(DOCTITLE)),)
+#	kdoc -L doc -d doc/$(PACKAGE) -n "Package $(PACKAGE) (lib$(PACKAGE).so)" $(HEADERS)
+#else
+#	kdoc -L doc -d doc/$(PACKAGE) -n "$(DOCTITLE) (lib$(PACKAGE).so)" $(HEADERS)
+#endif
+
+
+clean:
+	@$(MESSAGE) "Cleaning $(PACKAGE)"
+	@$(PRETTY) "rm -f $(SONAME) $(APPLICATIONS)"
+	@$(PRETTY) "rm -f *.o *.d core *~  moc_*.cpp"
+
+copy:	clean
+	tar -C .. -cvzf `date +../$(PACKAGE)-%d%b%y.tgz` $(PACKAGE)

slam_gmapping/openslam_gmapping/build_tools/Makefile.subdirs

@@ -0,0 +1,16 @@
+export VERBOSE
+
+.PHONY: clean, all
+
+ifeq ($(VERBOSE), 0)
+QUIET=--no-print-directory
+endif
+
+all:
+	@for subdir in $(SUBDIRS); do $(MESSAGE) "Entering $$subdir."; if ! $(MAKE) $(QUIET) -C $$subdir; then  $(MESSAGE) "Compilation in $$subdir failed."; exit 1; fi; done
+
+clean:
+	@for subdir in $(SUBDIRS); do $(MESSAGE) "Entering $$subdir."; $(MAKE) $(QUIET) -C $$subdir clean; done
+
+dep:
+	@for subdir in $(SUBDIRS); do $(MESSAGE) "Entering $$subdir."; $(MAKE) $(QUIET) -C $$subdir dep; done

slam_gmapping/openslam_gmapping/build_tools/generate_shared_object

@@ -0,0 +1,16 @@
+#!/bin/tcsh
+
+echo decompressing file $1
+
+set FILELIST=`ar -t $1`
+echo "Object files:"
+foreach i ($FILELIST)
+	echo $i
+end
+
+echo generating $1:r.so
+
+ar -x $1
+ld -shared -o $1:r.so $FILELIST
+
+rm $FILELIST

slam_gmapping/openslam_gmapping/build_tools/message

@@ -0,0 +1,14 @@
+#!/bin/sh
+
+#echo "message: verbose = $VERBOSE"
+
+if ($VERBOSE)
+then
+	exit 0;
+fi
+
+a=$MAKELEVEL
+
+while ((0<$a)); do echo -n "    "; let "a = $a - 1";done
+
+echo $1

slam_gmapping/openslam_gmapping/build_tools/pretty_compiler

@@ -0,0 +1,26 @@
+#!/bin/sh
+
+
+#echo "pretty: verbose = $VERBOSE"
+
+if ($VERBOSE)
+then
+	echo $1;
+	if ! eval $1
+	then
+		echo "Failed command was:"
+		echo $1
+		echo "in directory " `pwd`
+		exit 1
+	fi
+else
+	if ! eval $1
+	then
+		echo "Failed command was:"
+		echo $1
+		echo "in directory " `pwd`
+		exit 1
+	fi
+fi
+
+exit 0

slam_gmapping/openslam_gmapping/build_tools/testlib

@@ -0,0 +1,26 @@
+#!/bin/bash
+if [ -z "$1" ]; then
+	echo "Syntax: rtestlib <library>"
+	exit 1
+fi
+
+exit 0
+
+FNAME=`mktemp rtestlibXXXXXX`
+echo "int main() { return 0; }" > $FNAME.cpp
+
+g++ $1 $FNAME.cpp -o $FNAME
+result=$?
+rm -f $FNAME.cpp $FNAME
+
+exit $result
+
+#if g++ $1 $FNAME.cpp -o $FNAME
+#then#
+#	rm -f $FNAME.cpp $FNAME
+#	exit 1
+#else
+#	rm -f $FNAME.cpp $FNAME
+#	exit 0
+#fi
+

slam_gmapping/openslam_gmapping/carmenwrapper/carmenwrapper.cpp

@@ -0,0 +1,490 @@
+/*****************************************************************
+ *
+ * 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 "carmenwrapper.h"
+
+using namespace GMapping;
+using namespace std;
+
+//static vars for the carmenwrapper
+SensorMap CarmenWrapper::m_sensorMap;
+deque<RangeReading> CarmenWrapper::m_rangeDeque;
+pthread_mutex_t CarmenWrapper::m_mutex;  
+sem_t CarmenWrapper::m_dequeSem;  
+pthread_mutex_t CarmenWrapper::m_lock;  
+pthread_t CarmenWrapper::m_readingThread;
+RangeSensor* CarmenWrapper::m_frontLaser=0;
+RangeSensor* CarmenWrapper::m_rearLaser=0;
+bool CarmenWrapper::m_threadRunning=false;
+OrientedPoint CarmenWrapper::m_truepos;
+bool CarmenWrapper::stopped=true;
+
+
+void CarmenWrapper::initializeIPC(const char* name) {
+  carmen_ipc_initialize(1,(char **)&name);
+}
+
+
+
+
+int CarmenWrapper::registerLocalizationMessages(){
+  lock();
+  IPC_RETURN_TYPE err;
+
+  /* register globalpos message */
+  err = IPC_defineMsg(CARMEN_LOCALIZE_GLOBALPOS_NAME, IPC_VARIABLE_LENGTH, 
+		      CARMEN_LOCALIZE_GLOBALPOS_FMT);
+  carmen_test_ipc_exit(err, "Could not define", CARMEN_LOCALIZE_GLOBALPOS_NAME);
+
+  /* register robot particle message */
+  err = IPC_defineMsg(CARMEN_LOCALIZE_PARTICLE_NAME, IPC_VARIABLE_LENGTH, 
+		      CARMEN_LOCALIZE_PARTICLE_FMT);
+  carmen_test_ipc_exit(err, "Could not define", CARMEN_LOCALIZE_PARTICLE_NAME);
+
+/*
+  carmen_localize_subscribe_initialize_placename_message(NULL, 
+							 (carmen_handler_t)
+							 carmen_localize_initialize_placename_handler,
+							 CARMEN_SUBSCRIBE_LATEST);
+
+  // register map request message 
+  err = IPC_defineMsg(CARMEN_LOCALIZE_MAP_QUERY_NAME, IPC_VARIABLE_LENGTH,
+		      CARMEN_LOCALIZE_MAP_QUERY_FMT);
+  carmen_test_ipc_exit(err, "Could not define", 
+		       CARMEN_LOCALIZE_MAP_QUERY_NAME);
+
+  err = IPC_defineMsg(CARMEN_LOCALIZE_MAP_NAME, IPC_VARIABLE_LENGTH,
+		      CARMEN_LOCALIZE_MAP_FMT);
+  carmen_test_ipc_exit(err, "Could not define", CARMEN_LOCALIZE_MAP_NAME);
+
+  // subscribe to map request message 
+  err = IPC_subscribe(CARMEN_LOCALIZE_MAP_QUERY_NAME, map_query_handler, NULL);
+  carmen_test_ipc(err, "Could not subscribe", CARMEN_LOCALIZE_MAP_QUERY_NAME);
+  IPC_setMsgQueueLength(CARMEN_LOCALIZE_MAP_QUERY_NAME, 1);
+
+
+  // register globalpos request message 
+  err = IPC_defineMsg(CARMEN_LOCALIZE_GLOBALPOS_QUERY_NAME, 
+		      IPC_VARIABLE_LENGTH,
+		      CARMEN_DEFAULT_MESSAGE_FMT);
+  carmen_test_ipc_exit(err, "Could not define", 
+		       CARMEN_LOCALIZE_MAP_QUERY_NAME);
+
+  // subscribe to globalpos request message 
+  err = IPC_subscribe(CARMEN_LOCALIZE_GLOBALPOS_QUERY_NAME, 
+		      globalpos_query_handler, NULL);
+  carmen_test_ipc(err, "Could not subscribe", 
+		  CARMEN_LOCALIZE_GLOBALPOS_QUERY_NAME);
+  IPC_setMsgQueueLength(CARMEN_LOCALIZE_GLOBALPOS_QUERY_NAME, 1);
+*/
+  unlock();
+  return 0;
+}
+
+bool CarmenWrapper::start(const char* name){
+	if (m_threadRunning)
+		return false;
+	carmen_robot_subscribe_frontlaser_message(NULL, (carmen_handler_t)robot_frontlaser_handler, CARMEN_SUBSCRIBE_LATEST);
+	carmen_robot_subscribe_rearlaser_message(NULL, (carmen_handler_t)robot_rearlaser_handler, CARMEN_SUBSCRIBE_LATEST);
+	carmen_simulator_subscribe_truepos_message(NULL,(carmen_handler_t) simulator_truepos_handler, CARMEN_SUBSCRIBE_LATEST);
+
+	IPC_RETURN_TYPE err;
+
+	err = IPC_subscribe(CARMEN_NAVIGATOR_GO_NAME, navigator_go_handler,  NULL);
+	carmen_test_ipc_exit(err, "Could not subscribe", 
+			     CARMEN_NAVIGATOR_GO_NAME);
+	IPC_setMsgQueueLength(CARMEN_NAVIGATOR_GO_NAME, 1);
+
+	err = IPC_subscribe(CARMEN_NAVIGATOR_STOP_NAME, navigator_stop_handler,  NULL);
+	carmen_test_ipc_exit(err, "Could not subscribe", 
+			     CARMEN_NAVIGATOR_STOP_NAME);
+	IPC_setMsgQueueLength(CARMEN_NAVIGATOR_STOP_NAME, 1);
+
+
+
+	signal(SIGINT, shutdown_module);
+	pthread_mutex_init(&m_mutex, 0);
+	pthread_mutex_init(&m_lock, 0);
+	sem_init(&m_dequeSem, 0, 0);
+	m_threadRunning=true;
+	pthread_create (&m_readingThread,0,m_reading_function,0);
+	return true; 
+}
+
+void CarmenWrapper::lock(){
+	//cerr <<"LOCK" << endl;
+	pthread_mutex_lock(&m_lock);
+}
+
+void CarmenWrapper::unlock(){
+	//cerr <<"UNLOCK" << endl;
+	pthread_mutex_unlock(&m_lock);
+}
+
+
+bool CarmenWrapper::sensorMapComputed(){
+	pthread_mutex_lock(&m_mutex);
+	bool smok=m_frontLaser;
+	pthread_mutex_unlock(&m_mutex);
+	return smok;
+}
+
+const SensorMap& CarmenWrapper::sensorMap(){
+	return m_sensorMap;
+}
+ 
+bool CarmenWrapper::isRunning(){
+	return m_threadRunning;
+}
+
+bool CarmenWrapper::isStopped(){
+	return stopped;
+}
+
+int CarmenWrapper::queueLength(){
+	int ql=0;
+	pthread_mutex_lock(&m_mutex);
+	ql=m_rangeDeque.size();
+	pthread_mutex_unlock(&m_mutex);
+	return ql;
+}
+
+OrientedPoint CarmenWrapper::getTruePos(){
+	return m_truepos;
+}
+
+bool CarmenWrapper::getReading(RangeReading& reading){
+	bool present=false;
+	sem_wait(&m_dequeSem);
+	pthread_mutex_lock(&m_mutex);
+	if (!m_rangeDeque.empty()){
+//		cerr << __PRETTY_FUNCTION__ << ": queue size=" <<m_rangeDeque.size() << endl;
+		reading=m_rangeDeque.front();
+		m_rangeDeque.pop_front();
+		present=true;
+	}
+	int sval;
+	sem_getvalue(&m_dequeSem,&sval);
+//	cerr << "fetch. elements= "<< m_rangeDeque.size() << " sval=" << sval <<endl;
+	pthread_mutex_unlock(&m_mutex);
+	return present;
+}
+
+void CarmenWrapper::addReading(RangeReading& reading){
+	pthread_mutex_lock(&m_mutex);
+	m_rangeDeque.push_back(reading);
+	pthread_mutex_unlock(&m_mutex);
+	sem_post(&m_dequeSem);
+	int sval;
+	sem_getvalue(&m_dequeSem,&sval);
+//	cerr << "post. elements= "<< m_rangeDeque.size() << " sval=" << sval <<endl;
+}
+
+
+//RangeSensor::RangeSensor(std::string name, unsigned int beams_num, unsigned int res, const OrientedPoint& position, double span, double maxrange): 
+
+void CarmenWrapper::robot_frontlaser_handler(carmen_robot_laser_message* frontlaser) {
+/*	if (! m_rangeSensor){
+		double res=0;
+		if (frontlaser->num_readings==180 || frontlaser->num_readings==181)
+			res=M_PI/180;
+		if (frontlaser->num_readings==360 || frontlaser->num_readings==361)
+			res=M_PI/360;
+		assert(res>0);
+		m_rangeSensor=new RangeSensor("FLASER",frontlaser->num_readings, res, OrientedPoint(0,0,0), 0, 89.9);
+		m_sensorMap.insert(make_pair(string("FLASER"), m_rangeSensor));
+		
+		cout << __PRETTY_FUNCTION__ 
+		     << ": FrontLaser configured." 
+		     << " Readings " << m_rangeSensor->beams().size() 
+		     << " Resolution " << res << endl;
+	}
+	
+	RangeReading reading(m_rangeSensor, frontlaser->timestamp);
+	reading.resize(m_rangeSensor->beams().size());
+	for (unsigned int i=0; i< (unsigned int)frontlaser->num_readings; i++){
+		reading[i]=(double)frontlaser->range[i];
+	}
+	reading.setPose(OrientedPoint(frontlaser->x, frontlaser->y, frontlaser->theta));
+*/	
+	RangeReading reading=carmen2reading(*frontlaser);
+	addReading(reading);
+}
+
+void CarmenWrapper::robot_rearlaser_handler(carmen_robot_laser_message* rearlaser) {
+/*	if (! m_rangeSensor){
+		double res=0;
+		if (frontlaser->num_readings==180 || frontlaser->num_readings==181)
+			res=M_PI/180;
+		if (frontlaser->num_readings==360 || frontlaser->num_readings==361)
+			res=M_PI/360;
+		assert(res>0);
+		m_rangeSensor=new RangeSensor("FLASER",frontlaser->num_readings, res, OrientedPoint(0,0,0), 0, 89.9);
+		m_sensorMap.insert(make_pair(string("FLASER"), m_rangeSensor));
+		
+		cout << __PRETTY_FUNCTION__ 
+		     << ": FrontLaser configured." 
+		     << " Readings " << m_rangeSensor->beams().size() 
+		     << " Resolution " << res << endl;
+	}
+	
+	RangeReading reading(m_rangeSensor, frontlaser->timestamp);
+	reading.resize(m_rangeSensor->beams().size());
+	for (unsigned int i=0; i< (unsigned int)frontlaser->num_readings; i++){
+		reading[i]=(double)frontlaser->range[i];
+	}
+	reading.setPose(OrientedPoint(frontlaser->x, frontlaser->y, frontlaser->theta));
+*/	
+	RangeReading reading=carmen2reading(*rearlaser);
+	addReading(reading);
+}
+
+
+
+
+void CarmenWrapper:: navigator_go_handler(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void*) {
+  carmen_navigator_go_message msg;
+  FORMATTER_PTR formatter;
+  IPC_RETURN_TYPE err;
+
+  formatter = IPC_msgInstanceFormatter(msgRef);
+  err = IPC_unmarshallData(formatter, callData, &msg,
+			   sizeof(carmen_navigator_go_message));  
+  IPC_freeByteArray(callData);
+
+  carmen_test_ipc_return
+    (err, "Could not unmarshall", IPC_msgInstanceName(msgRef));
+  cerr<<"go"<<endl;
+  stopped=false;
+}
+
+
+void CarmenWrapper:: navigator_stop_handler(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void*) {
+  carmen_navigator_stop_message msg;
+  FORMATTER_PTR formatter;
+  IPC_RETURN_TYPE err;
+
+  formatter = IPC_msgInstanceFormatter(msgRef);
+  err = IPC_unmarshallData(formatter, callData, &msg,
+			   sizeof(carmen_navigator_stop_message));  
+  IPC_freeByteArray(callData);
+
+  carmen_test_ipc_return
+    (err, "Could not unmarshall", IPC_msgInstanceName(msgRef));
+  cerr<<"stop"<<endl;
+  stopped=true;
+}
+
+
+
+void CarmenWrapper::simulator_truepos_handler(carmen_simulator_truepos_message* truepos){
+	m_truepos.x=truepos->truepose.x;
+	m_truepos.y=truepos->truepose.y;
+	m_truepos.theta=truepos->truepose.theta;
+}
+
+RangeReading CarmenWrapper::carmen2reading(const carmen_robot_laser_message& msg){
+	//either front laser or rear laser
+	double dth=msg.laser_pose.theta-msg.robot_pose.theta;
+	dth=atan2(sin(dth), cos(dth));
+
+	if (msg.laser_pose.theta==msg.robot_pose.theta && !m_frontLaser){
+		double res=0;
+		res = msg.config.angular_resolution;
+// 		if (msg.num_readings==180 || msg.num_readings==181)
+// 			res=M_PI/180;
+// 		if (msg.num_readings==360 || msg.num_readings==361)
+// 			res=M_PI/360;
+		assert(res>0);
+		string sensorName="FLASER";
+		OrientedPoint rpose(msg.robot_pose.x, msg.robot_pose.y, msg.robot_pose.theta);
+		OrientedPoint lpose(msg.laser_pose.x, msg.laser_pose.y, msg.laser_pose.theta);
+		OrientedPoint dp=absoluteDifference(lpose, rpose);
+		m_frontLaser=new RangeSensor(sensorName,msg.num_readings, res, OrientedPoint(0,0,msg.laser_pose.theta-msg.robot_pose.theta), 0, 
+					      msg.config.maximum_range);
+		m_frontLaser->updateBeamsLookup();
+		m_sensorMap.insert(make_pair(sensorName, m_frontLaser));
+		
+		cout << __PRETTY_FUNCTION__ 
+		     << ": " << sensorName <<" configured." 
+		     << " Readings " << m_frontLaser->beams().size() 
+		     << " Resolution " << res << endl;
+	}
+	if (msg.laser_pose.theta!=msg.robot_pose.theta && !m_rearLaser){
+		double res=0;
+		res = msg.config.angular_resolution;
+// 		if (msg.num_readings==180 || msg.num_readings==181)
+// 			res=M_PI/180;
+// 		if (msg.num_readings==360 || msg.num_readings==361)
+// 			res=M_PI/360;
+		assert(res>0);
+		OrientedPoint rpose(msg.robot_pose.x, msg.robot_pose.y, msg.robot_pose.theta);
+		OrientedPoint lpose(msg.laser_pose.x, msg.laser_pose.y, msg.laser_pose.theta);
+		OrientedPoint dp=absoluteDifference(lpose, rpose);
+		string sensorName="RLASER";
+		m_rearLaser=new RangeSensor(sensorName,msg.num_readings, res, OrientedPoint(0,0,msg.laser_pose.theta-msg.robot_pose.theta), 0, 
+					      msg.config.maximum_range);
+		m_rearLaser->updateBeamsLookup();
+		m_sensorMap.insert(make_pair(sensorName, m_rearLaser));
+		
+		cout << __PRETTY_FUNCTION__ 
+		     << ": " << sensorName <<" configured." 
+		     << " Readings " << m_rearLaser->beams().size() 
+		     << " Resolution " << res << endl;
+	}	
+
+	const RangeSensor * rs=(msg.laser_pose.theta==msg.robot_pose.theta)?m_frontLaser:m_rearLaser;
+	RangeReading reading(rs, msg.timestamp);
+	reading.resize(rs->beams().size());
+	for (unsigned int i=0; i< (unsigned int)msg.num_readings; i++){
+		reading[i]=(double)msg.range[i];
+	}
+	reading.setPose(OrientedPoint(msg.robot_pose.x, msg.robot_pose.y, msg.robot_pose.theta));
+	return reading;
+}
+
+void CarmenWrapper::publish_globalpos(carmen_localize_summary_p summary)
+{
+  lock();
+  static carmen_localize_globalpos_message globalpos;
+  IPC_RETURN_TYPE err;
+  
+  globalpos.timestamp = carmen_get_time();
+  globalpos.host = carmen_get_host();
+  globalpos.globalpos = summary->mean;
+  globalpos.globalpos_std = summary->std;
+  globalpos.globalpos_xy_cov = summary->xy_cov;
+  globalpos.odometrypos = summary->odometry_pos;
+  globalpos.converged = summary->converged;
+  err = IPC_publishData(CARMEN_LOCALIZE_GLOBALPOS_NAME, &globalpos);
+  carmen_test_ipc_exit(err, "Could not publish", 
+		       CARMEN_LOCALIZE_GLOBALPOS_NAME);  
+  unlock();
+}
+
+/* publish a particle message */
+
+void CarmenWrapper::publish_particles(carmen_localize_particle_filter_p filter, 
+		       carmen_localize_summary_p summary)
+{
+  lock();
+  static carmen_localize_particle_message pmsg;
+  IPC_RETURN_TYPE err;
+
+  pmsg.timestamp = carmen_get_time();
+  pmsg.host = carmen_get_host();
+  pmsg.globalpos = summary->mean;
+  pmsg.globalpos_std = summary->mean;
+  pmsg.num_particles = filter->param->num_particles;
+  pmsg.particles = (carmen_localize_particle_ipc_p)filter->particles;
+  err = IPC_publishData(CARMEN_LOCALIZE_PARTICLE_NAME, &pmsg);
+  carmen_test_ipc_exit(err, "Could not publish", 
+		       CARMEN_LOCALIZE_PARTICLE_NAME);  
+  fprintf(stderr, "P");
+  unlock();
+}
+
+
+
+
+
+void * CarmenWrapper::m_reading_function(void*){
+	while (true) {
+		lock();
+		IPC_listen(100);
+		unlock();
+		usleep(20000);
+	}    
+	return 0;
+}
+
+void CarmenWrapper::shutdown_module(int sig){
+  if(sig == SIGINT) {
+      carmen_ipc_disconnect();
+
+      fprintf(stderr, "\nDisconnecting (shutdown_module(%d) called).\n",sig);
+      exit(0);
+  }
+}
+/*
+typedef struct {
+  int num_readings;
+  float *range;
+  char *tooclose;
+  double x, y, theta;//position of the laser on the robot
+  double odom_x, odom_y, odom_theta; //position of the center of the robot
+  double tv, rv;
+  double forward_safety_dist, side_safety_dist;
+  double turn_axis;
+  double timestamp;
+  char host[10];
+} carmen_robot_laser_message;
+*/
+
+carmen_robot_laser_message CarmenWrapper::reading2carmen(const RangeReading& reading){
+	carmen_robot_laser_message frontlaser;
+	frontlaser.num_readings=reading.size();
+	frontlaser.range = new float[frontlaser.num_readings];
+	frontlaser.tooclose=0;
+	frontlaser.laser_pose.x=frontlaser.robot_pose.x=reading.getPose().x;
+	frontlaser.laser_pose.y=frontlaser.robot_pose.y=reading.getPose().y;
+	frontlaser.laser_pose.theta=frontlaser.robot_pose.theta=reading.getPose().theta;
+	frontlaser.tv=frontlaser.rv=0;
+	frontlaser.forward_safety_dist=frontlaser.side_safety_dist=0;
+	frontlaser.turn_axis=0;
+	frontlaser.timestamp=reading.getTime();
+	for (unsigned int i=0; i< reading.size(); i++){
+		frontlaser.range[i]=(float)reading[i];
+	}
+	return frontlaser;
+}
+
+carmen_point_t CarmenWrapper::point2carmen (const OrientedPoint& p){
+	return (carmen_point_t){p.x,p.y,p.theta};
+}
+
+OrientedPoint CarmenWrapper::carmen2point (const carmen_point_t& p){
+	return OrientedPoint(p.x, p.y, p.theta);
+}
+
+
+/*
+int main (int argc, char** argv) {
+	CarmenWrapper::start(argc, argv);
+	while(1){
+		sleep(2);
+		RangeReading reading(0,0);
+		while(CarmenWrapper::getReading(reading)){
+			cout << "FLASER " <<  reading.size();
+			for (int i=0; i<reading.size(); i++)
+				cout << " " << reading[i];
+			cout << reading.getPose().x << " "
+			     << reading.getPose().y << " "
+			     << reading.getPose().theta << " 0 cazzo 0" << endl;
+		}
+		cout << endl;
+	}
+	return 1;
+}
+*/
+

slam_gmapping/openslam_gmapping/carmenwrapper/carmenwrapper.h

@@ -0,0 +1,108 @@
+/*****************************************************************
+ *
+ * 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 CARMENWRAPPER_H
+#define CARMENWRAPPER_H
+
+#include <iostream>
+#include <deque>
+#include <pthread.h>
+#include <semaphore.h>
+#include <carmen/carmen.h>
+#include <carmen/global.h>
+#include <sensor/sensor_base/sensor.h>
+#include <log/carmenconfiguration.h>
+#include <log/sensorstream.h>
+#include <log/sensorlog.h>
+#include <sensor/sensor_range/rangesensor.h>
+#include <sensor/sensor_range/rangereading.h>
+
+namespace GMapping{
+
+class CarmenWrapper {
+public:
+  static void initializeIPC(const char* name);
+  static bool start(const char* name);
+  static bool isRunning();
+  static void lock();
+  static void unlock();
+  static int registerLocalizationMessages();
+  
+  static int queueLength();
+  static OrientedPoint getTruePos();
+  static bool getReading(RangeReading& reading);
+  static void addReading(RangeReading& reading);
+  static const SensorMap& sensorMap();
+  static bool sensorMapComputed();
+  static bool isStopped();
+  
+// conversion function  
+  static carmen_robot_laser_message reading2carmen(const RangeReading& reading);
+  static RangeReading carmen2reading(const carmen_robot_laser_message& msg);
+  static carmen_point_t point2carmen (const OrientedPoint& p);
+  static OrientedPoint carmen2point (const carmen_point_t& p);
+  
+    
+// carmen interaction
+  static void robot_frontlaser_handler(carmen_robot_laser_message* frontlaser);
+  static void robot_rearlaser_handler(carmen_robot_laser_message* frontlaser);
+  static void simulator_truepos_handler(carmen_simulator_truepos_message* truepos);
+  //babsi:
+  static void navigator_go_handler(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void*) ;
+  static void navigator_stop_handler(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void*) ;
+
+  //babsi:
+  static void publish_globalpos(carmen_localize_summary_p summary);
+  static void publish_particles(carmen_localize_particle_filter_p filter, 
+				carmen_localize_summary_p summary);
+
+  static void shutdown_module(int sig);
+  
+  private:
+  static std::deque<RangeReading> m_rangeDeque;
+  static sem_t m_dequeSem;
+  static pthread_mutex_t m_mutex, m_lock;  
+  static pthread_t m_readingThread;
+  static void * m_reading_function(void*);
+  static bool m_threadRunning;
+  static SensorMap m_sensorMap;
+  static RangeSensor* m_frontLaser, *m_rearLaser;
+  static OrientedPoint m_truepos;
+  static bool stopped;
+};
+
+} //end namespace
+
+
+
+#endif
+/*
+int main (int argc, char** argv) {
+
+	CarmenWrapper::init_carmen(argc, argv);
+	while (true) {
+		IPC_listenWait(100);
+	}    
+	return 1;
+}
+*/

slam_gmapping/openslam_gmapping/gfs-carmen/Makefile

@@ -0,0 +1,21 @@
+OBJS= 
+APPS= gfs-carmen
+
+LIBS+= -lcarmenwrapper -lgridfastslam -lconfigfile
+CPPFLAGS+= -I ../sensor -I$(CARMEN_HOME)/include 
+
+-include ../global.mk
+ifeq ($(CARMENSUPPORT), 0)
+APPS=
+.PHONY: clean all
+
+all:
+
+clean:
+
+else
+	-include ../build_tools/Makefile.app
+endif
+
+
+

slam_gmapping/openslam_gmapping/gfs-carmen/gfs-carmen.cpp

@@ -0,0 +1,243 @@
+/*****************************************************************
+ *
+ * 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 <utils/commandline.h>
+#include <carmenwrapper/carmenwrapper.h>
+#include <gridfastslam/gridslamprocessor.h>
+#include <utils/orientedboundingbox.h>
+#include <configfile/configfile.h>
+
+#define DEBUG cout << __PRETTY_FUNCTION__
+
+/*
+Example file for interfacing carmen, and gfs.
+
+if you want to look for a specific topic search for one of the following keywords in the file comments
+
+KEYWORDS:
+	CREATION
+	INITIALIZATION
+	SENSOR MAP
+	BEST PARTICLE INDEX
+	PARTICLE VECTOR
+	PARTICLE TRAJECTORIES
+	BEST MAP
+ 	BOUNDING BOX
+*/
+
+using namespace GMapping;
+using namespace std;
+
+int main(int argc, const char * const * argv){
+	
+	std::string outfilename="";
+	double xmin=-100.;
+	double ymin=-100.;
+	double xmax=100.;
+	double ymax=100.;
+	double delta=0.05;
+	
+	//scan matching parameters
+	double sigma=0.05;
+	double maxrange=80.;
+	double maxUrange=80.;
+	double regscore=1e4;
+	double lstep=.05;
+	double astep=.05;
+	int kernelSize=1;
+	int iterations=5;
+	double critscore=0.;
+	double maxMove=1.;
+	double lsigma=.075;
+	double ogain=3;
+	int lskip=0;
+
+	//motion model parameters
+	double srr=0.01, srt=0.01, str=0.01, stt=0.01;
+	//particle parameters
+	int particles=30;
+	
+	
+	//gfs parameters
+	double angularUpdate=0.5;
+	double linearUpdate=1;
+	double resampleThreshold=0.5;
+	bool generateMap=true;
+	
+	std::string configfilename = "";
+
+	CMD_PARSE_BEGIN_SILENT(1,argc);
+		parseStringSilent("-cfg",configfilename);
+	CMD_PARSE_END_SILENT;
+
+	if (configfilename.length()>0){
+	  ConfigFile cfg(configfilename);
+	  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);
+	  //	  randseed = cfg.value("gfs","randseed", randseed);
+	  resampleThreshold = cfg.value("gfs","resampleThreshold", resampleThreshold);
+	  generateMap = cfg.value("gfs","generateMap", generateMap);
+	}
+
+	
+	CMD_PARSE_BEGIN(1,argc);
+		parseString("-cfg",configfilename);
+		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);
+		parseDouble("-resampleThreshold", resampleThreshold);
+		parseFlag("-generateMap", generateMap);
+	CMD_PARSE_END;
+
+	cerr << "Parameter parsed, connecting to Carmen!";
+
+	CarmenWrapper::initializeIPC(argv[0]);
+	CarmenWrapper::start(argv[0]);
+
+	while (! CarmenWrapper::sensorMapComputed()){
+		usleep(500000);
+		cerr << "." << flush;
+	}
+
+	//CREATION
+	
+	GridSlamProcessor* processor=new GridSlamProcessor;
+		
+	//SENSOR MAP 
+	//loads from the carmen wrapper the laser and robot settings
+	SensorMap sensorMap=CarmenWrapper::sensorMap();
+	cerr << "Connected " << endl;
+	processor->setSensorMap(sensorMap);
+
+	//set the command line parameters
+	processor->setMatchingParameters(maxUrange, maxrange, sigma, kernelSize, lstep, astep, iterations, lsigma, ogain, lskip);
+	processor->setMotionModelParameters(srr, srt, str, stt);
+	processor->setUpdateDistances(linearUpdate, angularUpdate, resampleThreshold);
+	processor->setgenerateMap(generateMap);
+	OrientedPoint initialPose(xmin+xmax/2, ymin+ymax/2, 0);
+	
+	
+	//INITIALIZATION
+	processor->init(particles, xmin, ymin, xmax, ymax, delta, initialPose);
+	if (outfilename.length()>0)
+		processor->outputStream().open(outfilename.c_str());
+	
+	bool running=true;
+	
+	GridSlamProcessor* ap, *copy=processor->clone();
+	ap=processor; processor=copy; copy=ap;
+
+	//this is the CORE LOOP;	
+	RangeReading rr(0,0);
+	while (running){
+		while (CarmenWrapper::getReading(rr)){
+
+		  		  
+			bool processed=processor->processScan(rr);
+			
+			//this returns true when the algorithm effectively processes (the traveled path since the last processing is over a given threshold)
+			if (processed){
+				cerr << "PROCESSED" << endl;
+				//for searching for the BEST PARTICLE INDEX
+				//				unsigned int best_idx=processor->getBestParticleIndex();
+				
+				//if you want to access to the PARTICLE VECTOR
+				const GridSlamProcessor::ParticleVector& particles = processor->getParticles(); 
+				//remember to use a const reference, otherwise it copys the whole particles and maps
+				
+				//this is for recovering the tree of PARTICLE TRAJECTORIES (obtaining the ancestor of each particle)
+				cerr << "Particle reproduction story begin" << endl;
+				for (unsigned int i=0; i<particles.size(); i++){
+					cerr << particles[i].previousIndex << "->"  << i << " ";
+				}
+				cerr << "Particle reproduction story end" << endl;
+/*				
+				//then if you want to access the BEST MAP,
+				//of course by copying it in a plain structure 
+				Map<double, DoubleArray2D, false>* mymap = processor->getParticles()[best_idx].map.toDoubleMap();
+				//at this point mymap is yours. Can do what you want.
+												
+				double best_weight=particles[best_idx].weightSum;
+				cerr << "Best Particle is " << best_idx << " with weight " << best_weight << endl;
+				
+*/				
+				cerr << __PRETTY_FUNCTION__  << "CLONING... " << endl;
+				GridSlamProcessor* newProcessor=processor->clone();
+				cerr << "DONE" << endl;
+				cerr << __PRETTY_FUNCTION__  << "DELETING... " << endl;
+				delete processor;
+				cerr << "DONE" << endl;
+				processor=newProcessor;
+			} 
+		}
+	}
+	return 0;
+}
+

slam_gmapping/openslam_gmapping/grid/Makefile

@@ -0,0 +1,9 @@
+OBJS= 
+APPS= map_test
+
+LDFLAGS+= 
+CPPFLAGS+= -DNDEBUG 
+
+-include ../global.mk
+-include ../build_tools/Makefile.app
+

slam_gmapping/openslam_gmapping/grid/graphmap.cpp

@@ -0,0 +1,59 @@
+#ifndef GRAPHMAP_H
+#define GRAPHMAP_H
+#include <list>
+#include <utils/point.h>
+#include <utils/graph.h>
+#include <grid/map.h>
+
+namespace GMapping {
+
+class RasterMap;
+
+struct GraphMapPatch{
+	typedef typename std::list<IntPoint> PointList; 
+	/**Renders the map relatively to the center of the patch*/
+	//void render(RenderMap rmap);
+	/**returns the lower left corner of the patch, relative to the center*/
+	//Point minBoundary() const; 
+	/**returns the upper right corner of the patch, relative to the center*/
+	//Point maxBoundary() const; // 
+
+	OrientedPoint center;
+	PointList m_points;
+};
+
+struct Covariance3{
+	double sxx, sxy, sxt, syy, syt ,stt;
+};
+
+struct GraphMapEdge{
+	Covariance3 covariance;
+	GraphMapPatch* first, *second;
+	inline operator double() const{
+		return sqrt((first->center-second->center)*(first->center-second->center));
+	}
+};
+
+
+struct GraphPatchGraph: public Graph<GraphMapPatch, Covariance3>{
+	void addEdge(Vertex* v1, Vertex* v2, const Covariance3& covariance);
+};
+
+void GraphPatchGraph::addEdge(GraphPatchGraph::Vertex* v1, GraphPatchGraph::VertexVertex* v2, 
+	const Covariance3& cov){
+	GraphMapEdge gme;
+	gme.covariance=cov;
+	gme.first=v1;
+	gme.second=v2;
+	return Graph<GraphMapPatch, Covariance3>::addEdge(v1,v2,gme);
+}
+
+struct GraphPatchDirectoryCell: public std::set<GraphMapPatch::Vertex*> {
+	GraphPatchDirectoryCell(double);
+};
+
+typedef Map<GraphPatchDirectoryCell>, Array2D::set<GraphPatchDirectoryCell> >
+
+};
+
+#endif

slam_gmapping/openslam_gmapping/grid/map_test.cpp

@@ -0,0 +1,62 @@
+#include <iostream>
+#include "map.h"
+#include "harray2d.h"
+
+using namespace std;
+using namespace GMapping;
+
+struct SimpleCell{
+	int value;
+	SimpleCell(int v=0){value=v;}
+	static const SimpleCell& Unknown();
+	static SimpleCell* address;
+};
+
+SimpleCell* SimpleCell::address=0;
+
+const SimpleCell& SimpleCell::Unknown(){
+	if (address)
+		return *address;
+	address=new SimpleCell(-1);
+	return *address;
+}
+
+typedef Map< SimpleCell, HierarchicalArray2D<SimpleCell> > CGrid;
+
+int main (int argc, char ** argv){
+	CGrid g1(Point(0.,0.), 200, 200, 0.1);
+	CGrid g2(Point(10.,10.), 200, 200, 0.1);
+	{
+		HierarchicalArray2D<SimpleCell>::PointSet ps;
+		IntPoint pp=g1.world2map(Point(5.1,5.1));
+		cout << pp.x << " " << pp.y << endl;
+		ps.insert(pp);
+		g1.storage().setActiveArea(ps,false);
+		g1.storage().allocActiveArea();
+		g1.cell(Point(5.1,5.1)).value=5;
+		cout << "cell value" << (int) g1.cell(Point(5.1,5.1)).value << endl;
+		g1.resize(-150, -150, 150, 150);
+		cout << "cell value" << (int) g1.cell(Point(5.1,5.1)).value << endl;
+		CGrid g3(g1);
+		g1=g2;
+	}
+	cerr << "copy and modify test" << endl;
+	CGrid *ap,* gp1=new CGrid(Point(0,0), 200, 200, 0.1);
+        CGrid* gp0=new CGrid(*gp1);
+	for (int i=1; i<10; i++){
+	  ap=new CGrid(*gp1);
+	  delete gp1;
+	  gp1=gp0;
+	  gp0=ap;
+	  IntPoint pp=gp0->world2map(Point(5.1,5.1));
+	  HierarchicalArray2D<SimpleCell>::PointSet ps;
+	  ps.insert(pp);
+          gp1->storage().setActiveArea(ps,false);	
+          gp1->storage().allocActiveArea();
+	  gp1->cell(Point(5.1,5.1)).value=i;
+          cout << "cell value" << (int) gp1->cell(Point(5.1,5.1)).value << endl;
+	}
+	delete gp0;
+	delete gp1;
+	return 0;
+}

slam_gmapping/openslam_gmapping/gridfastslam/CMakeLists.txt

@@ -0,0 +1,11 @@
+add_library(gridfastslam
+        gfsreader.cpp
+        gridslamprocessor.cpp
+        gridslamprocessor_tree.cpp
+        motionmodel.cpp
+        )
+target_link_libraries(gridfastslam scanmatcher sensor_range)
+
+install(TARGETS gridfastslam DESTINATION lib)
+
+#ament_export_libraries(gridfastslam)

slam_gmapping/openslam_gmapping/gridfastslam/gfs2log.cpp

@@ -0,0 +1,64 @@
+#include <cstring>
+#include <iostream>
+#include <fstream>
+#include <sstream>
+#include <vector>
+#include <list>
+#include <gmapping/utils/point.h>
+#include "gfsreader.h"
+
+#define MAX_LINE_LENGHT (1000000)
+
+using namespace std;
+using namespace GMapping;
+using namespace GMapping::GFSReader;
+
+int main (int argc, const char * const * argv){
+	if (argc<3){
+		cout << "usage gfs2log [-err] [-neff] [-part] [-odom] <infilename> <outfilename>" << endl;
+		cout << "  -odom : dump raw odometry in ODOM message instead of inpolated corrected one" << endl;
+		return -1;
+	}
+	bool err=0;
+	bool neff=0;
+	bool part=0;
+	bool odom=0;
+	//	int particle_num;
+	unsigned int  c=1;
+	if (!strcmp(argv[c],"-err")){
+		err=true;
+		c++;
+	}
+	if (!strcmp(argv[c],"-neff")){
+		neff=true;
+		c++;
+	}
+	if (!strcmp(argv[c],"-part")){
+		part=true;
+		c++;
+	}
+	if (!strcmp(argv[c],"-odom")){
+		odom=true;
+		c++;
+	}
+	ifstream is(argv[c]);
+	if (!is){
+		cout << "could read file "<< endl;
+		return -1;
+	}
+		c++;
+	RecordList rl;
+	rl.read(is);
+	unsigned int bestidx=rl.getBestIdx();
+	cout << endl << "best index = " <<  bestidx<< endl;
+	ofstream os(argv[c]);
+	if (! os){
+		cout << "could write file "<< endl;
+		return -1;
+	}
+	rl.printPath(os,bestidx,err,odom);
+	if(part)
+	  rl.printLastParticles(os);
+	os.close();
+	return 0;
+}

slam_gmapping/openslam_gmapping/gridfastslam/gfs2neff.cpp

@@ -0,0 +1,40 @@
+#include <iostream>
+#include <fstream>
+#include <sstream>
+#include <cstring>
+
+using namespace std;
+
+int main(int argc, char**argv){
+	if (argc<3){
+		cout << "usage gfs2neff <infilename> <nefffilename>" << endl;
+		return -1;
+	}
+	ifstream is(argv[1]);
+	if (!is){
+		cout << "could read file "<< endl;
+		return -1;
+	}
+	ofstream os(argv[2]);
+	if (! os){
+		cout << "could write file "<< endl;
+		return -1;
+	}
+	unsigned int frame=0;
+	double neff=0;
+	while(is){
+			char buf[8192];
+			is.getline(buf, 8192);
+			istringstream lineStream(buf);
+			string recordType;
+			lineStream >> recordType;
+			if (recordType=="FRAME"){
+				lineStream>> frame;
+			}
+			if (recordType=="NEFF"){
+				lineStream>> neff;
+				os << frame << " " << neff << endl;
+			}
+	}
+	os.close();
+}

slam_gmapping/openslam_gmapping/gridfastslam/gfs2rec.cpp

@@ -0,0 +1,406 @@
+#include <cstring>
+#include <iostream>
+#include <fstream>
+#include <sstream>
+#include <vector>
+#include <list>
+#include <gmapping/utils/point.h>
+
+#define MAX_LINE_LENGHT (1000000)
+
+using namespace GMapping;
+using namespace std;
+
+struct Record{
+	unsigned int dim;
+	double time;
+	virtual ~Record(){}
+	virtual void read(istream& is)=0;
+	virtual void write(ostream& os){};
+};
+
+struct CommentRecord: public Record{
+	string text;
+	virtual void read(istream& is){
+		char buf[MAX_LINE_LENGHT];
+		memset(buf,0, MAX_LINE_LENGHT*sizeof(char));
+		is.getline(buf, MAX_LINE_LENGHT);
+		text=string(buf);
+	}
+	virtual void write(ostream& os){
+		os << "#GFS_COMMENT: " << text << endl;
+	}
+};
+
+struct PoseRecord: public Record{
+	PoseRecord(bool ideal=false){
+		truePos=ideal;
+	}
+	bool truePos;
+	OrientedPoint pose;
+	void read(istream& is){
+		is >> pose.x >> pose.y >> pose.theta;
+		time = 0;
+		if (is)
+			is >> time;
+	}
+	virtual void write(ostream& os){
+		if (truePos) 
+			os << "POS-CORR";
+		else
+			os << "POS ";
+		// FIXME os << floor(time) << " " << (int) (time-floor(time)*1e6) << ": ";
+		os << "0 0: ";
+		os << pose.x*100 << " " << pose.y*100 << " " << 180/M_PI*pose.theta <<  endl;
+	}
+};
+
+struct NeffRecord: public Record{
+	double neff;
+	void read(istream& is){
+		is >> neff;
+	}
+	virtual void write(ostream& os){
+		os << "NEFF " << neff << endl;
+	}
+};
+
+
+struct OdometryRecord: public Record{
+	vector<OrientedPoint> poses;
+	virtual void read(istream& is){
+		is >> dim;
+		for (unsigned int i=0; i< dim; i++){
+			OrientedPoint p;
+			double w;
+			is >> p.x;
+			is >> p.y;
+			is >> p.theta;
+			is >> w;
+			poses.push_back(p);
+		}
+		time = 0;
+		if (is)
+			is >> time;
+	}
+};
+
+
+struct ScanMatchRecord: public Record{
+	vector<OrientedPoint> poses;
+	vector<double> weights;
+	virtual void read(istream& is){
+		is >> dim;
+		for (unsigned int i=0; i< dim; i++){
+			OrientedPoint p;
+			double w;
+			is >> p.x;
+			is >> p.y;
+			is >> p.theta;
+			is >> w;
+			poses.push_back(p);
+			weights.push_back(w);
+		}
+	}
+};
+
+struct LaserRecord: public Record{
+	vector<double> readings;
+	OrientedPoint pose;
+	virtual void read(istream& is){
+		is >> dim;
+		for (unsigned int i=0; i< dim; i++){
+			double r;
+			is >> r;
+			readings.push_back(r);
+		}
+		is >> pose.x;
+		is >> pose.y;
+		is >> pose.theta;
+		time = 0;
+		if (is)
+			is >> time;
+	}
+	
+	//	dummy, &sec, &usec, &nLas, &nVal, &range ) == EOF) {
+
+	virtual void write(ostream& os){
+		os << "POS ";
+		// FIXME os << floor(time) << " " << (int) (time-floor(time)*1e6) << ": ";
+		os << "0 0: ";
+		os << pose.x*100 << " " << pose.y*100 << " " << 180/M_PI*pose.theta <<  endl;
+		
+		os << "LASER-RANGE ";
+		// FIXME os << floor(time) << " " << (int) (time-floor(time)*1e6) << ": ";
+		os << " 0 0 0 " << dim << " 180. : ";
+		for (unsigned int i=0; i< dim; i++){
+			os <<" "<< readings[i]*100 ;
+		}
+		os << endl;
+	};
+};
+
+struct ResampleRecord: public Record{
+	vector<unsigned int> indexes;
+	virtual void read(istream& is){
+		is >> dim;
+		for (unsigned int i=0; i< dim; i++){
+			unsigned int j;
+			is >> j;
+			indexes.push_back(j);
+		}
+	}
+};
+
+struct RecordList: public list<Record*>{
+	mutable int sampleSize;
+	
+	istream& read(istream& is){
+		while(is){
+			char buf[8192];
+			is.getline(buf, 8192);
+			istringstream lineStream(buf);
+			string recordType;
+			lineStream >> recordType;
+			Record* rec=0;
+			if (recordType=="LASER_READING"){
+				rec=new LaserRecord;
+				cout << "l" << flush;
+			}
+			if (recordType=="ODO_UPDATE"){
+				rec=new OdometryRecord;
+				cout << "o" << flush;
+			}
+			if (recordType=="SM_UPDATE"){
+				rec=new ScanMatchRecord;
+				cout << "m" << flush;
+			}
+			if (recordType=="SIMULATOR_POS"){
+				rec=new PoseRecord(true);
+				cout << "t" << flush;
+			}
+			if (recordType=="RESAMPLE"){
+				rec=new ResampleRecord;
+				cout << "r" << flush;
+			}
+			if (recordType=="NEFF"){
+				rec=new NeffRecord;
+				cout << "n" << flush;
+			}
+			if (recordType=="COMMENT"){
+				rec=new CommentRecord;
+				cout << "c" << flush;
+			}
+			if (rec){
+				rec->read(lineStream);
+				push_back(rec);
+			}
+		}
+		return is;
+	}
+	
+	double getLogWeight(unsigned int i) const{
+		double weight=0;
+		unsigned int currentIndex=i;
+		for(RecordList::const_reverse_iterator it=rbegin(); it!=rend(); it++){
+			ScanMatchRecord* scanmatch=dynamic_cast<ScanMatchRecord*>(*it); 
+			if (scanmatch){
+				weight+=scanmatch->weights[currentIndex];
+			}
+			ResampleRecord* resample=dynamic_cast<ResampleRecord*>(*it); 
+			if (resample){
+				currentIndex=resample->indexes[currentIndex];
+			}
+		}
+		return weight;
+	}
+	unsigned int getBestIdx() const {
+		if (empty())
+			return 0;
+		const ScanMatchRecord* scanmatch=0;
+		const_reverse_iterator it=rbegin();
+		while(!scanmatch){
+			scanmatch=dynamic_cast<const ScanMatchRecord*>(*it); 
+			it++;
+		}
+		unsigned int dim=scanmatch->dim;
+		sampleSize=(int)dim;
+		double bestw=-1e200;
+		unsigned int best=scanmatch->dim+1;
+		for (unsigned i=0; i<dim; i++){
+			double w=getLogWeight(i);
+			if (w>bestw){
+				best=i;
+				bestw=w;
+			}
+		}
+		return best;
+	}
+	
+	void printPath(ostream& os, unsigned int i, bool err=false) const{
+		unsigned int currentIndex=i;
+		OrientedPoint p(0,0,0);
+	
+		RecordList rl;
+		
+		//reconstruct a  path
+		for(RecordList::const_reverse_iterator it=rbegin(); it!=rend(); it++){
+			const NeffRecord* neff=dynamic_cast<const NeffRecord*>(*it);
+			if (neff){
+				NeffRecord* n=new NeffRecord(*neff);
+				rl.push_front(n);
+			}
+			const ScanMatchRecord* scanmatch=dynamic_cast<const ScanMatchRecord*>(*it); 
+			if (scanmatch){
+				PoseRecord* pose=new PoseRecord;
+				pose->dim=0;
+				p=pose->pose=scanmatch->poses[currentIndex];
+				rl.push_front(pose);
+			}
+			const OdometryRecord* odometry=dynamic_cast<const OdometryRecord*>(*it);
+			if (odometry){
+				PoseRecord* pose=new PoseRecord;
+				pose->dim=0;
+				p=pose->pose=odometry->poses[currentIndex];
+				pose->time=odometry->time;
+				rl.push_front(pose);
+			}
+			const PoseRecord* tpose=dynamic_cast<const PoseRecord*>(*it);
+			if (tpose){
+				PoseRecord* pose=new PoseRecord(*tpose);
+				rl.push_front(pose);
+			}
+			const LaserRecord* laser=dynamic_cast<const LaserRecord*>(*it); 
+			if (laser){
+				LaserRecord* claser=new LaserRecord(*laser);
+				claser->pose=p;
+				rl.push_front(claser);
+			}
+			const CommentRecord* comment=dynamic_cast<const CommentRecord*>(*it); 
+			if (comment){
+				CommentRecord* ccomment=new CommentRecord(*comment);
+				rl.push_front(ccomment);
+			}
+			const ResampleRecord* resample=dynamic_cast<const ResampleRecord*>(*it); 
+			if (resample){
+				currentIndex=resample->indexes[currentIndex];
+				ResampleRecord* r= new ResampleRecord(*resample);
+				rl.push_front(r);
+			}
+		}
+		bool started=false;
+		double ox=0, oy=0, rxx=0, rxy=0, ryx=0, ryy=0, rth=0;
+		bool computedTransformation=false;
+		bool truePosFound=false;
+		OrientedPoint truePose(0,0,0);
+		OrientedPoint currPose(0,0,0);
+		bool tpf=false;
+		double neff=0;
+		unsigned int count=0;
+		for(RecordList::iterator it=rl.begin(); it!=rl.end(); it++){
+			NeffRecord* neffr=dynamic_cast<NeffRecord*>(*it);
+			if (neffr)
+				neff=neffr->neff/(double)sampleSize;
+			started=started || dynamic_cast<const LaserRecord*>(*it)?true:false;
+			if (started && ! truePosFound){
+				PoseRecord* tpose=dynamic_cast<PoseRecord*>(*it);
+				if (tpose && tpose->truePos){
+					truePosFound=true;
+					tpf=true;
+					truePose=tpose->pose;
+					os << "# ";
+					(*it)->write(os);
+				}
+			}
+			if (started && truePosFound && ! computedTransformation){
+				PoseRecord* pos=dynamic_cast<PoseRecord*>(*it);
+				if (pos && !pos->truePos){
+					OrientedPoint pose=pos->pose;
+					rth=truePose.theta-pose.theta;
+					double s=sin(rth), c=cos(rth);
+					rxx=ryy=c;
+					rxy=-s; ryx=s;
+					ox=truePose.x-(rxx*pose.x+rxy*pose.y);
+					oy=truePose.y-(ryx*pose.x+ryy*pose.y);
+					computedTransformation=true;
+					os << "# ";
+					(*it)->write(os);
+					
+				}
+			}
+			ResampleRecord* resample=dynamic_cast<ResampleRecord*>(*it);
+			if(resample){
+				os << "MARK-POS 0 0: " <<currPose.x*100 << " " << currPose.y*100 << " 0 " << count++ << endl;
+			} 
+			if (computedTransformation){
+				PoseRecord* pos=dynamic_cast<PoseRecord*>(*it);
+				if (pos){
+					if (pos->truePos){
+						tpf=true;
+						truePose=pos->pose;
+					} else {
+						if (tpf){
+							tpf=false;
+							OrientedPoint pose=pos->pose;
+							double ex, ey, eth=truePose.theta-pose.theta-rth;
+							ex=truePose.x-(ox+rxx*pose.x+rxy*pose.y);
+							ey=truePose.y-(oy+ryx*pose.x+ryy*pose.y);
+							eth=atan2(sin(eth), cos(eth));
+							if (! err)
+								os << "# ERROR ";
+							os << neff << " "
+							   << ex << " " << ey << " " << eth 
+							   << " " << sqrt(ex*ex+ey*ey) << " " << fabs(eth) << endl;
+						}
+					}
+				}
+				
+			}
+			PoseRecord* pos=dynamic_cast<PoseRecord*>(*it);
+			if (pos)
+				currPose=pos->pose;
+			
+			if (! err)
+				(*it)->write(os);
+			delete *it;
+		}
+	}
+};
+
+
+
+int main (int argc, const char * const * argv){
+	if (argc<3){
+		cout << "usage gfs2rec [-err] <infilename> <outfilename>" << endl;
+		return -1;
+	}
+	bool err=0;
+	bool neff=0;
+	unsigned int  c=1;
+	if (!strcmp(argv[c],"-err")){
+		err=true;
+		c++;
+	}
+	if (!strcmp(argv[c],"-neff")){
+		neff=true;
+		c++;
+	}
+	ifstream is(argv[c]);
+	if (!is){
+		cout << "could read file "<< endl;
+		return -1;
+	}
+		c++;
+	RecordList rl;
+	rl.read(is);
+	unsigned int bestidx=rl.getBestIdx();
+	cout << endl << "best index = " <<  bestidx<< endl;
+	ofstream os(argv[c]);
+	if (! os){
+		cout << "could write file "<< endl;
+		return -1;
+	}
+	rl.printPath(os,bestidx,err);
+	os.close();
+	return 0;
+}

slam_gmapping/openslam_gmapping/gridfastslam/gfs2stat.cpp

@@ -0,0 +1,58 @@
+#include <gmapping/utils/stat.h>
+#include <gmapping/particlefilter/particlefilter.h>
+#include <iostream>
+#include <fstream>
+#include "gfsreader.h"
+
+using namespace std;
+using namespace GMapping;
+using namespace GMapping::GFSReader;
+
+
+int main(int argc, char ** argv){
+	if (argc<2){
+		cout << "usage gfs2stat <infilename> <outfilename>" << endl;
+		return 0;
+	}
+	ifstream is(argv[1]);
+	if (!is){
+		cout << "no file found: " << argv[1] << endl;
+		return 0;
+	}
+	ofstream os(argv[2]);
+	if (!os){
+		cout << "cannot open file: " << argv[1] << endl;
+		return 0;
+	}
+	cout << "loading... "<< flush;
+	RecordList rl;
+	rl.read(is);
+	cout << " done" << endl;
+	int count=-1;
+	for (RecordList::const_iterator it=rl.begin(); it!=rl.end(); it++){
+		
+		count++;
+		const ScanMatchRecord* rec=dynamic_cast<const ScanMatchRecord*>(*it);
+		if (!rec)
+			continue;
+		Gaussian3 gaussian;
+		/*	
+		vector<double> nweights;
+		cout << "N"<< flush;
+		back_insert_iterator< vector<double> > out(nweights);
+		toNormalForm(out,rec->weights.begin(), rec->weights.end());
+		cout << "G"<< flush;
+		gaussian.computeFromSamples(rec->poses, nweights);
+		*/
+		gaussian.computeFromSamples(rec->poses);
+		cout << "E"<< flush;
+		os << count <<" ";
+		os << gaussian.mean.x <<" ";
+		os << gaussian.mean.y <<" ";
+		os << gaussian.mean.theta <<" ";
+		os << gaussian.covariance.eval[0] <<" ";
+		os << gaussian.covariance.eval[1] <<" ";
+		os << gaussian.covariance.eval[2] <<endl;
+	}
+	os.close();
+}

slam_gmapping/openslam_gmapping/gridfastslam/gfs2stream.cpp

@@ -0,0 +1,74 @@
+#include <iostream>
+#include <fstream>
+#include <sstream>
+#include <vector>
+#include <list>
+#include <gmapping/utils/point.h>
+#include <gmapping/utils/commandline.h>
+#include "gfsreader.h"
+#define MAX_LINE_LENGHT (1000000)
+
+using namespace std;
+using namespace GMapping;
+using namespace GMapping::GFSReader;
+
+computeBoundingBox()
+
+
+int main (unsigned int argc, const char * const * argv)
+	double delta = 0.1;
+	double skip  = 2;
+	double rotate = 0;
+	double maxrange = 0;
+	
+	if (argc<3){
+		cout << "usage gfs2stream [-step Number]  <outfilename>" << endl;
+		return -1;
+	}
+	
+	
+	CMD_PARSE_BEGIN(1,argc-2);
+	
+	CMD_PARSE_END;
+
+	if (argc<3){
+		cout << "usage gfs2stream [-step Number]  <outfilename>" << endl;
+		return -1;
+	}
+	bool err=0;
+	bool neff=0;
+	bool part=0;
+	unsigned int  c=1;
+	if (!strcmp(argv[c],"-err")){
+		err=true;
+		c++;
+	}
+	if (!strcmp(argv[c],"-neff")){
+		neff=true;
+		c++;
+	}
+	if (!strcmp(argv[c],"-part")){
+		part=true;
+		c++;
+	}
+	ifstream is(argv[c]);
+	if (!is){
+		cout << "could read file "<< endl;
+		return -1;
+	}
+		c++;
+	RecordList rl;
+	rl.read(is);
+	unsigned int bestidx=rl.getBestIdx();
+	cout << endl << "best index = " <<  bestidx<< endl;
+	ofstream os(argv[c]);
+	if (! os){
+		cout << "could write file "<< endl;
+		return -1;
+	}
+	rl.printPath(os,bestidx,err);
+	if(part)
+		rl.printLastParticles(os);
+	os.close();
+	return 0;
+}

slam_gmapping/openslam_gmapping/gridfastslam/gfsreader.cpp

@@ -0,0 +1,506 @@
+#include <cstring>
+#include "gfsreader.h"
+#include <iomanip>
+#include <limits>
+
+namespace  GMapping { 
+
+namespace GFSReader{
+
+Record::~Record(){}
+void Record::write(ostream& os){};
+
+void CommentRecord::read(istream& is){
+	char buf[MAX_LINE_LENGHT];
+	memset(buf,0, MAX_LINE_LENGHT*sizeof(char));
+	is.getline(buf, MAX_LINE_LENGHT);
+	text=string(buf);
+}
+
+void CommentRecord::write(ostream& os){
+	os << "#GFS_COMMENT: " << text << endl;
+}
+
+PoseRecord::PoseRecord(bool ideal){
+	truePos=ideal;
+}
+void PoseRecord::read(istream& is){
+	is >> pose.x >> pose.y >> pose.theta;
+	time = 0;
+	if (is)
+		is >> time;
+}
+void PoseRecord::write(ostream& os){
+	if (truePos)
+		os << "TRUEPOS ";
+	else
+		os << "ODOM ";
+	os << setiosflags(ios::fixed) << setprecision(6);
+	os << pose.x << " " << pose.y << " " << pose.theta << " 0 0 0 ";
+	os << time << " pippo " << time << endl;
+}
+
+void NeffRecord::read(istream& is){
+        is >> neff;
+        time =0;
+	if (is)
+	  is >> time;
+
+}
+
+void NeffRecord::write(ostream& os){
+	os << "NEFF " << neff ;
+	os << setiosflags(ios::fixed) << setprecision(6);
+	os << " " << time << " pippo " << time << endl;
+}
+
+
+void OdometryRecord::read(istream& is){
+	is >> dim;
+	for (unsigned int i=0; i< dim; i++){
+		OrientedPoint p;
+		double w;
+		is >> p.x;
+		is >> p.y;
+		is >> p.theta;
+		is >> w;
+		poses.push_back(p);
+	}
+	time = 0;
+	if (is)
+		is >> time;
+}
+
+void RawOdometryRecord::read(istream& is){
+  is >> pose.x;
+  is >> pose.y;
+  is >> pose.theta;
+  time = 0;
+  assert(is);
+    is >> time;
+ 
+}
+
+
+void EntropyRecord::read(istream& is){
+  is >> poseEntropy >> trajectoryEntropy >> mapEntropy;
+  time =0;
+  if (is)
+    is >> time;
+}
+
+void EntropyRecord::write(ostream& os){
+  os << setiosflags(ios::fixed) << setprecision(6) << "ENTROPY " << poseEntropy << " " << trajectoryEntropy << " " << mapEntropy;
+  os << " " << time << " pippo " << time << endl;
+}
+
+void ScanMatchRecord::read(istream& is){
+	is >> dim;
+	for (unsigned int i=0; i< dim; i++){
+		OrientedPoint p;
+		double w;
+		is >> p.x;
+		is >> p.y;
+		is >> p.theta;
+		is >> w;
+		poses.push_back(p);
+		weights.push_back(w);
+	}
+}
+
+void LaserRecord::read(istream& is){
+	is >> dim;
+	for (unsigned int i=0; i< dim; i++){
+		double r;
+		is >> r;
+		readings.push_back(r);
+	}
+	is >> pose.x;
+	is >> pose.y;
+	is >> pose.theta;
+	time = 0;
+	if (is)
+		is >> time;
+}
+
+void LaserRecord::write(ostream& os){
+	os << "WEIGHT " << weight << endl;
+	os << "ROBOTLASER1 ";
+	
+        
+        if ((dim == 541)||(dim == 540)) { // S300
+          os <<" 4";  // laser type
+          os <<" -2.351831";  // start_angle
+          os <<" 4.712389";  // fov
+          os <<" 0.008727";  // angular res
+          os <<" 30.0" ;  // maxrange
+        }
+        else if ((dim == 180)||(dim == 181)) { // PLS
+          os <<" 0";  // laser type
+          os <<" -1.570796";  // start_angle
+          os <<" 3.141593";  // fov
+          os <<" 0.017453";  // angular res
+          os <<" 81.9" ;  // maxrange
+        }
+        else if ((dim == 360)||(dim == 361)) { // LMS
+          os <<" 0";  // laser type
+          os <<" -1.570796";  // start_angle
+          os <<" 3.141593";  // fov
+          os <<" 0.008726";  // angular res
+          os <<" 81.9" ;  // maxrange
+        }
+        else if ((dim == 682)||(dim == 683)) { // URG
+          os <<" 0";  // laser type
+          os <<" -2.094395";  // start_angle
+          os <<" 4.1887902";  // fov
+          os << " " <<   360.0/1024.0/180.0*M_PI;  // angular res
+          os <<" 5.5" ;  // maxrange
+        }
+        else {     // PLS
+          os <<" 0";  // laser type
+          os <<" -1.570796";  // start_angle
+          os <<" 3.141593";  // fov
+          os <<" 0.017453";  // angular res
+          os <<" 81.9" ;  // maxrange
+        }
+	os <<" 0.01"; // accuracy	
+	os <<" 0" ;  // remission mode
+	os <<" "<< dim; // num readings
+        os << setiosflags(ios::fixed) << setprecision(2);
+	for (unsigned int i=0; i< dim; i++){
+		os <<" "<< readings[i] ;
+	}
+	os << setiosflags(ios::fixed) << setprecision(6);
+	os <<" 0"; // num remession values
+	os <<" "<< pose.x;
+	os <<" "<< pose.y;
+	os <<" "<< pose.theta;
+	os <<" "<< pose.x;
+	os <<" "<< pose.y;
+	os <<" "<< pose.theta;
+	os <<" 0" ;  // tv
+	os <<" 0" ;  // rv
+	os <<" 0.55" ;  // forward_safety_dist
+	os <<" 0.375" ;     // sideward_safety_dist
+	os <<" 1000000.0" ; // turn_axis
+	os <<" "<< time <<  " localhost " << time << endl;
+};
+
+void ResampleRecord::read(istream& is){
+	is >> dim;
+	for (unsigned int i=0; i< dim; i++){
+		unsigned int j;
+		is >> j;
+		indexes.push_back(j);
+	}
+}
+
+istream& RecordList::read(istream& is){
+	while(is){
+		char buf[MAX_LINE_LENGHT];
+		is.getline(buf, MAX_LINE_LENGHT);
+		istringstream lineStream(buf);
+		string recordType;
+		lineStream >> recordType;
+		Record* rec=0;
+		if (recordType=="LASER_READING"){
+			rec=new LaserRecord;
+//			cout << "l" << flush;
+		}
+		else if (recordType=="ODO_UPDATE"){
+			rec=new OdometryRecord;
+//			cout << "o" << flush;
+		}
+		else if (recordType=="ODOM"){
+			rec=new RawOdometryRecord;
+//			cout << "O" << flush;
+		}
+		else if (recordType=="SM_UPDATE"){
+			rec=new ScanMatchRecord;
+//			cout << "m" << flush;
+		}
+		else if (recordType=="SIMULATOR_POS"){
+			rec=new PoseRecord(true);
+//			cout << "t" << flush;
+		}
+		else if (recordType=="RESAMPLE"){
+			rec=new ResampleRecord;
+//			cout << "r" << flush;
+		}
+		else if (recordType=="NEFF"){
+			rec=new NeffRecord;
+//			cout << "n" << flush;
+		}
+		else if (recordType=="COMMENT" || recordType=="#COMMENT"){
+			rec=new CommentRecord;
+//			cout << "c" << flush;
+		}
+		else if (recordType=="ENTROPY"){
+			rec=new EntropyRecord;
+//			cout << "c" << flush;
+		}
+		
+		if (rec){
+			rec->read(lineStream);
+			push_back(rec);
+		}
+	}
+	return is;
+}
+
+double RecordList::getLogWeight(unsigned int i) const{
+	double weight=0;
+	unsigned int currentIndex=i;
+	for(RecordList::const_reverse_iterator it=rbegin(); it!=rend(); it++){
+		ScanMatchRecord* scanmatch=dynamic_cast<ScanMatchRecord*>(*it); 
+		if (scanmatch){
+			weight+=scanmatch->weights[currentIndex];
+		}
+		ResampleRecord* resample=dynamic_cast<ResampleRecord*>(*it); 
+		if (resample){
+			currentIndex=resample->indexes[currentIndex];
+		}
+	}
+	return weight;
+}
+
+double RecordList::getLogWeight(unsigned int i, RecordList::const_iterator frame) const{
+	double weight=0;
+	unsigned int currentIndex=i;
+	for(RecordList::const_reverse_iterator it(frame); it!=rend(); it++){
+		ScanMatchRecord* scanmatch=dynamic_cast<ScanMatchRecord*>(*it); 
+		if (scanmatch){
+			weight+=scanmatch->weights[currentIndex];
+		}
+		ResampleRecord* resample=dynamic_cast<ResampleRecord*>(*it); 
+		if (resample){
+			currentIndex=resample->indexes[currentIndex];
+		}
+	}
+	return weight;
+}
+
+unsigned int RecordList::getBestIdx() const {
+	if (empty())
+		return 0;
+	const ScanMatchRecord* scanmatch=0;
+	const_reverse_iterator it=rbegin();
+	while(!scanmatch){
+		scanmatch=dynamic_cast<const ScanMatchRecord*>(*it); 
+		it++;
+	}
+	unsigned int dim=scanmatch->dim;
+	sampleSize=(int)dim;
+	double bestw=-std::numeric_limits<double>::max();
+	unsigned int best=scanmatch->dim+1;
+	for (unsigned i=0; i<dim; i++){
+		double w=getLogWeight(i);
+		if (w>bestw){
+			best=i;
+			bestw=w;
+		}
+	}
+	return best;
+}
+
+void RecordList::printLastParticles(ostream& os) const {
+	if (empty())
+		return;
+	const ScanMatchRecord* scanmatch=0;
+	const_reverse_iterator it=rbegin();
+	while(!scanmatch){
+		scanmatch=dynamic_cast<const ScanMatchRecord*>(*it); 
+		it++;
+	}
+	if (! scanmatch)
+		return;
+	for (vector<OrientedPoint>::const_iterator it=scanmatch->poses.begin(); it!=scanmatch->poses.end(); it++){
+		os << "MARKER [color=black; circle=" << it->x*100 << "," << it->y*100 << ",10] 0 pippo 0" << endl;
+	}
+}
+
+void RecordList::destroyReferences(){
+	for(RecordList::iterator it=begin(); it!=end(); it++)
+		delete (*it);
+	
+}
+
+RecordList RecordList::computePath(unsigned int i, RecordList::const_iterator frame) const{
+	unsigned int currentIndex=i;
+	OrientedPoint p(0,0,0);
+	RecordList rl;
+	
+	//reconstruct a  path
+	bool first=true;
+	for(RecordList::const_reverse_iterator it(frame); it!=rend(); it++){
+		const ScanMatchRecord* scanmatch=dynamic_cast<const ScanMatchRecord*>(*it); 
+		if (scanmatch){
+			p=scanmatch->poses[currentIndex];
+			first=false;
+		}
+		const LaserRecord* laser=dynamic_cast<const LaserRecord*>(*it); 
+		if (laser && !first){
+			LaserRecord* claser=new LaserRecord(*laser);
+			claser->pose=p;
+			rl.push_front(claser);
+		}
+		const ResampleRecord* resample=dynamic_cast<const ResampleRecord*>(*it); 
+		if (resample){
+			currentIndex=resample->indexes[currentIndex];
+		}
+	}
+	return rl;
+}
+
+	
+void RecordList::printPath(ostream& os, unsigned int i, bool err, bool rawodom) const{
+	unsigned int currentIndex=i;
+	OrientedPoint p(0,0,0);
+	RecordList rl;
+	double oldWeight=0;
+	double w=0;
+	//reconstruct a  path
+	for(RecordList::const_reverse_iterator it=rbegin(); it!=rend(); it++){
+		const NeffRecord* neff=dynamic_cast<const NeffRecord*>(*it);
+		if (neff){
+			NeffRecord* n=new NeffRecord(*neff);
+			rl.push_front(n);
+		}
+		const EntropyRecord* entropy=dynamic_cast<const EntropyRecord*>(*it);
+		if (entropy){
+		    EntropyRecord* n=new EntropyRecord(*entropy);
+		    rl.push_front(n);
+		}
+		const ScanMatchRecord* scanmatch=dynamic_cast<const ScanMatchRecord*>(*it); 
+		if (scanmatch){
+			PoseRecord* pose=new PoseRecord;
+			pose->dim=0;
+			p=pose->pose=scanmatch->poses[currentIndex];
+			w=scanmatch->weights[currentIndex]-oldWeight;
+			oldWeight=scanmatch->weights[currentIndex];
+			
+			if (!rawodom) {
+			  rl.push_front(pose);
+			}
+		}
+		const OdometryRecord* odometry=dynamic_cast<const OdometryRecord*>(*it);
+		if (odometry){
+		  PoseRecord* pose=new PoseRecord;
+		  pose->dim=0;
+		  p=pose->pose=odometry->poses[currentIndex];
+		  pose->time=odometry->time;
+		  if (!rawodom) {
+		    rl.push_front(pose);
+		  }
+		}
+		const RawOdometryRecord* rawodometry=dynamic_cast<const RawOdometryRecord*>(*it);
+		if (rawodometry){
+		  PoseRecord* pose=new PoseRecord;
+		  pose->dim=0;
+		  pose->pose=rawodometry->pose;
+		  pose->time=rawodometry->time;
+		  if (rawodom) {
+		    rl.push_front(pose);
+		  }
+ 		}
+		const PoseRecord* tpose=dynamic_cast<const PoseRecord*>(*it);
+		if (tpose){
+			PoseRecord* pose=new PoseRecord(*tpose);
+			rl.push_front(pose);
+		}
+		const LaserRecord* laser=dynamic_cast<const LaserRecord*>(*it); 
+		if (laser){
+			LaserRecord* claser=new LaserRecord(*laser);
+			claser->pose=p;
+			claser->weight=w;
+			rl.push_front(claser);
+		}
+		const CommentRecord* comment=dynamic_cast<const CommentRecord*>(*it); 
+		if (comment){
+			CommentRecord* ccomment=new CommentRecord(*comment);
+			rl.push_front(ccomment);
+		}
+		const ResampleRecord* resample=dynamic_cast<const ResampleRecord*>(*it); 
+		if (resample){
+			rl.push_front(new ResampleRecord(*resample));
+			currentIndex=resample->indexes[currentIndex];
+		}
+		
+	}
+	bool started=false;
+	bool computedTransformation=false;
+	bool truePosFound=false;
+	OrientedPoint truePose;
+	OrientedPoint oldPose;
+	OrientedPoint trueStart, realStart;
+	bool tpf=false;
+	double neff=0;
+	double totalError=0;
+	int count=0;
+	for(RecordList::iterator it=rl.begin(); it!=rl.end(); it++){
+		NeffRecord* neffr=dynamic_cast<NeffRecord*>(*it);
+		if (neffr)
+			neff=neffr->neff/(double)sampleSize;
+		started=started || dynamic_cast<const LaserRecord*>(*it)?true:false;
+		if (started && ! truePosFound){
+			PoseRecord* tpose=dynamic_cast<PoseRecord*>(*it);
+			if (tpose && tpose->truePos){
+				truePosFound=true;
+				tpf=true;
+				truePose=tpose->pose;
+				os << "# ";
+				(*it)->write(os);
+			}
+		}
+		if (started && truePosFound && ! computedTransformation){
+			PoseRecord* pos=dynamic_cast<PoseRecord*>(*it);
+			if (pos && !pos->truePos){
+				trueStart=truePose;
+				realStart=pos->pose;
+				os << "# ";
+				(*it)->write(os);
+				computedTransformation=true;
+			}
+		}
+		if (computedTransformation){
+		  os << setiosflags(ios::fixed) << setprecision(6);
+			PoseRecord* pos=dynamic_cast<PoseRecord*>(*it);
+			if (pos){
+				if (pos->truePos){
+					tpf=true;
+					truePose=pos->pose;
+				} else {
+					if (tpf){
+						tpf=false;
+						OrientedPoint realDelta=absoluteDifference(pos->pose,realStart);
+						OrientedPoint trueDelta=absoluteDifference(truePose,trueStart);
+						double ex=realDelta.x-trueDelta.x;
+						double ey=realDelta.y-trueDelta.y;
+						double eth=realDelta.theta-trueDelta.theta;
+						eth=atan2(sin(eth), cos(eth));
+						if (! err)
+							os << "# ERROR ";
+						os << neff << " "
+							<< ex << " " << ey << " " << eth 
+							<< " " << sqrt(ex*ex+ey*ey) << " " << fabs(eth) << endl;
+						totalError+=sqrt(ex*ex+ey*ey);
+						count++;
+					}
+				}
+			}
+			
+		}
+		PoseRecord* pos=dynamic_cast<PoseRecord*>(*it);
+		if (pos)
+			oldPose=pos->pose;
+		if (! err)
+			(*it)->write(os);
+		delete *it;
+	}
+	if (err)
+		cout << "average error" << totalError/count << endl;
+}
+
+}; //gfsreader
+
+}; //GMapping;

slam_gmapping/openslam_gmapping/gridfastslam/gfsreader.h

@@ -0,0 +1,101 @@
+#ifndef GFSREADER_H
+#define GFSREADER_H
+
+#include <iostream>
+#include <fstream>
+#include <sstream>
+#include <vector>
+#include <list>
+#include <gmapping/utils/point.h>
+
+#define MAX_LINE_LENGHT (1000000)
+
+namespace GMapping{
+
+namespace GFSReader{
+	
+using namespace std;
+
+struct Record{
+	unsigned int dim;
+	double time;
+	virtual ~Record();
+	virtual void read(istream& is)=0;
+	virtual void write(ostream& os);
+};
+
+struct CommentRecord: public Record{
+	string text;
+	virtual void read(istream& is);
+	virtual void write(ostream& os);
+};
+
+struct PoseRecord: public Record{
+	PoseRecord(bool ideal=false);
+	void read(istream& is);
+	virtual void write(ostream& os);
+	bool truePos;
+	OrientedPoint pose;
+};
+
+struct NeffRecord: public Record{
+	void read(istream& is);
+	virtual void write(ostream& os);
+	double neff;
+};
+
+struct EntropyRecord: public Record{
+	void read(istream& is);
+	virtual void write(ostream& os);
+	double poseEntropy;
+	double trajectoryEntropy;
+	double mapEntropy;
+};
+
+
+struct OdometryRecord: public Record{
+	virtual void read(istream& is);
+	vector<OrientedPoint> poses;
+};
+
+struct RawOdometryRecord: public Record{
+	virtual void read(istream& is);
+	OrientedPoint pose;
+};
+
+struct ScanMatchRecord: public Record{
+	virtual void read(istream& is);
+	vector<OrientedPoint> poses;
+	vector<double> weights;
+};
+
+struct LaserRecord: public Record{
+	virtual void read(istream& is);
+	virtual void write(ostream& os);
+	vector<double> readings;
+	OrientedPoint pose;
+	double weight;
+};
+
+struct ResampleRecord: public Record{
+	virtual void read(istream& is);
+	vector<unsigned int> indexes;
+};
+
+struct RecordList: public list<Record*>{
+	mutable int sampleSize;
+	istream& read(istream& is);
+	double getLogWeight(unsigned int i) const;
+	double getLogWeight(unsigned int i, RecordList::const_iterator frame) const;
+	unsigned int getBestIdx() const ;
+	void printLastParticles(ostream& os) const ;
+	void printPath(ostream& os, unsigned int i, bool err=false, bool rawodom=false) const;
+	RecordList computePath(unsigned int i, RecordList::const_iterator frame) const;
+	void destroyReferences();
+};
+
+}; //end namespace GFSReader
+
+}; //end namespace GMapping
+
+#endif

slam_gmapping/openslam_gmapping/gridfastslam/gridslamprocessor.cpp

@@ -0,0 +1,518 @@
+#include <string>
+#include <deque>
+#include <list>
+#include <map>
+#include <set>
+#include <fstream>
+#include <iomanip>
+#include <gmapping/utils/stat.h>
+#include <gmapping/gridfastslam/gridslamprocessor.h>
+
+//#define MAP_CONSISTENCY_CHECK
+//#define GENERATE_TRAJECTORIES
+
+namespace GMapping {
+
+const double m_distanceThresholdCheck = 20;
+ 
+using namespace std;
+
+  GridSlamProcessor::GridSlamProcessor(): m_infoStream(cout){
+    
+    period_ = 5.0;
+    m_obsSigmaGain=1;
+    m_resampleThreshold=0.5;
+    m_minimumScore=0.;
+  }
+  
+  GridSlamProcessor::GridSlamProcessor(const GridSlamProcessor& gsp) 
+    :last_update_time_(0.0), m_particles(gsp.m_particles), m_infoStream(cout){
+    
+    period_ = 5.0;
+
+    m_obsSigmaGain=gsp.m_obsSigmaGain;
+    m_resampleThreshold=gsp.m_resampleThreshold;
+    m_minimumScore=gsp.m_minimumScore;
+    
+    m_beams=gsp.m_beams;
+    m_indexes=gsp.m_indexes;
+    m_motionModel=gsp.m_motionModel;
+    m_resampleThreshold=gsp.m_resampleThreshold;
+    m_matcher=gsp.m_matcher;
+    
+    m_count=gsp.m_count;
+    m_readingCount=gsp.m_readingCount;
+    m_lastPartPose=gsp.m_lastPartPose;
+    m_pose=gsp.m_pose;
+    m_odoPose=gsp.m_odoPose;
+    m_linearDistance=gsp.m_linearDistance;
+    m_angularDistance=gsp.m_angularDistance;
+    m_neff=gsp.m_neff;
+	
+    cerr << "FILTER COPY CONSTRUCTOR" << endl;
+    cerr << "m_odoPose=" << m_odoPose.x << " " <<m_odoPose.y << " " << m_odoPose.theta << endl;
+    cerr << "m_lastPartPose=" << m_lastPartPose.x << " " <<m_lastPartPose.y << " " << m_lastPartPose.theta << endl;
+    cerr << "m_linearDistance=" << m_linearDistance << endl;
+    cerr << "m_angularDistance=" << m_linearDistance << endl;
+    
+		
+    m_xmin=gsp.m_xmin;
+    m_ymin=gsp.m_ymin;
+    m_xmax=gsp.m_xmax;
+    m_ymax=gsp.m_ymax;
+    m_delta=gsp.m_delta;
+    
+    m_regScore=gsp.m_regScore;
+    m_critScore=gsp.m_critScore;
+    m_maxMove=gsp.m_maxMove;
+    
+    m_linearThresholdDistance=gsp.m_linearThresholdDistance;
+    m_angularThresholdDistance=gsp.m_angularThresholdDistance;
+    m_obsSigmaGain=gsp.m_obsSigmaGain;
+    
+#ifdef MAP_CONSISTENCY_CHECK
+    cerr << __PRETTY_FUNCTION__ <<  ": trajectories copy.... ";
+#endif
+    TNodeVector v=gsp.getTrajectories();
+    for (unsigned int i=0; i<v.size(); i++){
+		m_particles[i].node=v[i];
+    }
+#ifdef MAP_CONSISTENCY_CHECK
+    cerr <<  "end" << endl;
+#endif
+
+
+    cerr  << "Tree: normalizing, resetting and propagating weights within copy construction/cloneing ..." ;
+    updateTreeWeights(false);
+    cerr  << ".done!" <<endl;
+  }
+  
+  GridSlamProcessor::GridSlamProcessor(std::ostream& infoS): m_infoStream(infoS){
+    period_ = 5.0;
+    m_obsSigmaGain=1;
+    m_resampleThreshold=0.5;
+    m_minimumScore=0.;
+	
+  }
+
+  GridSlamProcessor* GridSlamProcessor::clone() const {
+# ifdef MAP_CONSISTENCY_CHECK
+    cerr << __PRETTY_FUNCTION__ << ": performing preclone_fit_test" << endl;
+    typedef std::map<autoptr< Array2D<PointAccumulator> >::reference* const, int> PointerMap;
+    PointerMap pmap;
+	for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){
+	  const ScanMatcherMap& m1(it->map);
+	  const HierarchicalArray2D<PointAccumulator>& h1(m1.storage());
+ 	  for (int x=0; x<h1.getXSize(); x++){
+	    for (int y=0; y<h1.getYSize(); y++){
+	      const autoptr< Array2D<PointAccumulator> >& a1(h1.m_cells[x][y]);
+	      if (a1.m_reference){
+		PointerMap::iterator f=pmap.find(a1.m_reference);
+		if (f==pmap.end())
+		  pmap.insert(make_pair(a1.m_reference, 1));
+		else
+		  f->second++;
+	      }
+	    }
+	  }
+	}
+	cerr << __PRETTY_FUNCTION__ <<  ": Number of allocated chunks" << pmap.size() << endl;
+	for(PointerMap::const_iterator it=pmap.begin(); it!=pmap.end(); it++)
+	  assert(it->first->shares==(unsigned int)it->second);
+
+	cerr << __PRETTY_FUNCTION__ <<  ": SUCCESS, the error is somewhere else" << endl;
+# endif
+	GridSlamProcessor* cloned=new GridSlamProcessor(*this);
+	
+# ifdef MAP_CONSISTENCY_CHECK
+	cerr << __PRETTY_FUNCTION__ <<  ": trajectories end" << endl;
+	cerr << __PRETTY_FUNCTION__ << ": performing afterclone_fit_test" << endl;
+	ParticleVector::const_iterator jt=cloned->m_particles.begin();
+	for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){
+	  const ScanMatcherMap& m1(it->map);
+	  const ScanMatcherMap& m2(jt->map);
+	  const HierarchicalArray2D<PointAccumulator>& h1(m1.storage());
+	  const HierarchicalArray2D<PointAccumulator>& h2(m2.storage());
+	  jt++;
+ 	  for (int x=0; x<h1.getXSize(); x++){
+	    for (int y=0; y<h1.getYSize(); y++){
+	      const autoptr< Array2D<PointAccumulator> >& a1(h1.m_cells[x][y]);
+	      const autoptr< Array2D<PointAccumulator> >& a2(h2.m_cells[x][y]);
+	      assert(a1.m_reference==a2.m_reference);
+	      assert((!a1.m_reference) || !(a1.m_reference->shares%2));
+	    }
+	  }
+	}
+	cerr << __PRETTY_FUNCTION__ <<  ": SUCCESS, the error is somewhere else" << endl;
+# endif
+	return cloned;
+}
+  
+  GridSlamProcessor::~GridSlamProcessor(){
+    cerr << __PRETTY_FUNCTION__ << ": Start" << endl;
+    cerr << __PRETTY_FUNCTION__ << ": Deleting tree" << endl;
+    for (std::vector<Particle>::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
+#ifdef TREE_CONSISTENCY_CHECK		
+      TNode* node=it->node;
+      while(node)
+	node=node->parent;
+      cerr << "@" << endl;
+#endif
+      if (it->node)
+	delete it->node;
+      //cout << "l=" << it->weight<< endl;
+    }
+    
+# ifdef MAP_CONSISTENCY_CHECK
+    cerr << __PRETTY_FUNCTION__ << ": performing predestruction_fit_test" << endl;
+    typedef std::map<autoptr< Array2D<PointAccumulator> >::reference* const, int> PointerMap;
+    PointerMap pmap;
+    for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){
+      const ScanMatcherMap& m1(it->map);
+      const HierarchicalArray2D<PointAccumulator>& h1(m1.storage());
+      for (int x=0; x<h1.getXSize(); x++){
+	for (int y=0; y<h1.getYSize(); y++){
+	  const autoptr< Array2D<PointAccumulator> >& a1(h1.m_cells[x][y]);
+	  if (a1.m_reference){
+	    PointerMap::iterator f=pmap.find(a1.m_reference);
+	    if (f==pmap.end())
+	      pmap.insert(make_pair(a1.m_reference, 1));
+	    else
+	      f->second++;
+	  }
+	}
+      }
+    }
+    cerr << __PRETTY_FUNCTION__ << ": Number of allocated chunks" << pmap.size() << endl;
+    for(PointerMap::const_iterator it=pmap.begin(); it!=pmap.end(); it++)
+      assert(it->first->shares>=(unsigned int)it->second);
+    cerr << __PRETTY_FUNCTION__ << ": SUCCESS, the error is somewhere else" << endl;
+# endif
+  }
+
+
+		
+  void GridSlamProcessor::setMatchingParameters (double urange, double range, double sigma, int kernsize, double lopt, double aopt, 
+						 int iterations, double likelihoodSigma, double likelihoodGain, unsigned int likelihoodSkip){
+    m_obsSigmaGain=likelihoodGain;
+    m_matcher.setMatchingParameters(urange, range, sigma, kernsize, lopt, aopt, iterations, likelihoodSigma, likelihoodSkip);
+    if (m_infoStream)
+      m_infoStream << " -maxUrange "<< urange
+		   << " -maxUrange "<< range
+		   << " -sigma     "<< sigma
+		   << " -kernelSize "<< kernsize
+		   << " -lstep "    << lopt
+		   << " -lobsGain " << m_obsSigmaGain
+		   << " -astep "    << aopt << endl;
+    
+    
+  }
+  
+void GridSlamProcessor::setMotionModelParameters
+(double srr, double srt, double str, double stt){
+  m_motionModel.srr=srr;
+  m_motionModel.srt=srt;
+  m_motionModel.str=str;
+  m_motionModel.stt=stt;	
+  
+  if (m_infoStream)
+    m_infoStream << " -srr "<< srr 	<< " -srt "<< srt  
+		 << " -str "<< str 	<< " -stt "<< stt << endl;
+  
+}
+  
+  void GridSlamProcessor::setUpdateDistances(double linear, double angular, double resampleThreshold){
+    m_linearThresholdDistance=linear; 
+    m_angularThresholdDistance=angular;
+    m_resampleThreshold=resampleThreshold;	
+    if (m_infoStream)
+      m_infoStream << " -linearUpdate " << linear
+		   << " -angularUpdate "<< angular
+		   << " -resampleThreshold " << m_resampleThreshold << endl;
+  }
+  
+  //HERE STARTS THE BEEF
+
+  GridSlamProcessor::Particle::Particle(const ScanMatcherMap& m):
+    map(m), pose(0,0,0), weight(0), weightSum(0), gweight(0), previousIndex(0){
+    node=0;
+  }
+  
+  
+  void GridSlamProcessor::setSensorMap(const SensorMap& smap){
+    
+    /*
+      Construct the angle table for the sensor
+      
+      FIXME For now detect the readings of only the front laser, and assume its pose is in the center of the robot 
+    */
+    
+    SensorMap::const_iterator laser_it=smap.find(std::string("FLASER"));
+    if (laser_it==smap.end()){
+      cerr << "Attempting to load the new carmen log format" << endl;
+      laser_it=smap.find(std::string("ROBOTLASER1"));
+      assert(laser_it!=smap.end());
+    }
+    const RangeSensor* rangeSensor=dynamic_cast<const RangeSensor*>((laser_it->second));
+    assert(rangeSensor && rangeSensor->beams().size());
+    
+    m_beams=static_cast<unsigned int>(rangeSensor->beams().size());
+    double* angles=new double[rangeSensor->beams().size()];
+    for (unsigned int i=0; i<m_beams; i++){
+      angles[i]=rangeSensor->beams()[i].pose.theta;
+    }
+    m_matcher.setLaserParameters(m_beams, angles, rangeSensor->getPose());
+    delete [] angles;
+  }
+  
+  void GridSlamProcessor::init(unsigned int size, double xmin, double ymin, double xmax, double ymax, double delta, OrientedPoint initialPose){
+    m_xmin=xmin;
+    m_ymin=ymin;
+    m_xmax=xmax;
+    m_ymax=ymax;
+    m_delta=delta;
+    if (m_infoStream)
+      m_infoStream 
+	<< " -xmin "<< m_xmin
+	<< " -xmax "<< m_xmax
+	<< " -ymin "<< m_ymin
+	<< " -ymax "<< m_ymax
+	<< " -delta "<< m_delta
+	<< " -particles "<< size << endl;
+    
+
+    m_particles.clear();
+    TNode* node=new TNode(initialPose, 0, 0, 0);
+    ScanMatcherMap lmap(Point(xmin+xmax, ymin+ymax)*.5, xmax-xmin, ymax-ymin, delta);
+    for (unsigned int i=0; i<size; i++){
+      m_particles.push_back(Particle(lmap));
+      m_particles.back().pose=initialPose;
+      m_particles.back().previousPose=initialPose;
+      m_particles.back().setWeight(0);
+      m_particles.back().previousIndex=0;
+      
+		// this is not needed
+		//		m_particles.back().node=new TNode(initialPose, 0, node, 0);
+
+		// we use the root directly
+		m_particles.back().node= node;
+    }
+    m_neff=(double)size;
+    m_count=0;
+    m_readingCount=0;
+    m_linearDistance=m_angularDistance=0;
+  }
+
+  void GridSlamProcessor::processTruePos(const OdometryReading& o){
+    const OdometrySensor* os=dynamic_cast<const OdometrySensor*>(o.getSensor());
+    if (os && os->isIdeal() && m_outputStream){
+      m_outputStream << setiosflags(ios::fixed) << setprecision(3);
+      m_outputStream <<  "SIMULATOR_POS " <<  o.getPose().x << " " << o.getPose().y << " " ;
+      m_outputStream << setiosflags(ios::fixed) << setprecision(6) << o.getPose().theta << " " <<  o.getTime() << endl;
+    }
+  }
+  
+  
+  bool GridSlamProcessor::processScan(const RangeReading & reading, int adaptParticles){
+    
+    /**retireve the position from the reading, and compute the odometry*/
+    OrientedPoint relPose=reading.getPose();
+    if (!m_count){
+      m_lastPartPose=m_odoPose=relPose;
+    }
+    
+    //write the state of the reading and update all the particles using the motion model
+    for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
+      OrientedPoint& pose(it->pose);
+      pose=m_motionModel.drawFromMotion(it->pose, relPose, m_odoPose);
+    }
+
+    // update the output file
+    if (m_outputStream.is_open()){
+      m_outputStream << setiosflags(ios::fixed) << setprecision(6);
+      m_outputStream << "ODOM ";
+      m_outputStream << setiosflags(ios::fixed) << setprecision(3) << m_odoPose.x << " " << m_odoPose.y << " ";
+      m_outputStream << setiosflags(ios::fixed) << setprecision(6) << m_odoPose.theta << " ";
+      m_outputStream << reading.getTime();
+      m_outputStream << endl;
+    }
+    if (m_outputStream.is_open()){
+      m_outputStream << setiosflags(ios::fixed) << setprecision(6);
+      m_outputStream << "ODO_UPDATE "<< m_particles.size() << " ";
+      for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
+	OrientedPoint& pose(it->pose);
+	m_outputStream << setiosflags(ios::fixed) << setprecision(3) << pose.x << " " << pose.y << " ";
+	m_outputStream << setiosflags(ios::fixed) << setprecision(6) << pose.theta << " " << it-> weight << " ";
+      }
+      m_outputStream << reading.getTime();
+      m_outputStream << endl;
+    }
+    
+    //invoke the callback
+    onOdometryUpdate();
+    
+
+    // accumulate the robot translation and rotation
+    OrientedPoint move=relPose-m_odoPose;
+    move.theta=atan2(sin(move.theta), cos(move.theta));
+    m_linearDistance+=sqrt(move*move);
+    m_angularDistance+=fabs(move.theta);
+    
+    // if the robot jumps throw a warning
+    if (m_linearDistance>m_distanceThresholdCheck){
+      cerr << "***********************************************************************" << endl;
+      cerr << "********** Error: m_distanceThresholdCheck overridden!!!! *************" << endl;
+      cerr << "m_distanceThresholdCheck=" << m_distanceThresholdCheck << endl;
+      cerr << "Old Odometry Pose= " << m_odoPose.x << " " << m_odoPose.y 
+	   << " " <<m_odoPose.theta << endl;
+      cerr << "New Odometry Pose (reported from observation)= " << relPose.x << " " << relPose.y 
+	   << " " <<relPose.theta << endl;
+      cerr << "***********************************************************************" << endl;
+      cerr << "** The Odometry has a big jump here. This is probably a bug in the   **" << endl;
+      cerr << "** odometry/laser input. We continue now, but the result is probably **" << endl;
+      cerr << "** crap or can lead to a core dump since the map doesn't fit.... C&G **" << endl;
+      cerr << "***********************************************************************" << endl;
+    }
+    
+    m_odoPose=relPose;
+    
+    bool processed=false;
+
+    // process a scan only if the robot has traveled a given distance or a certain amount of time has elapsed
+    if (! m_count 
+	|| m_linearDistance>=m_linearThresholdDistance 
+	|| m_angularDistance>=m_angularThresholdDistance
+    || (period_ >= 0.0 && (reading.getTime() - last_update_time_) > period_)){
+      last_update_time_ = reading.getTime();      
+
+      if (m_outputStream.is_open()){
+	m_outputStream << setiosflags(ios::fixed) << setprecision(6);
+	m_outputStream << "FRAME " <<  m_readingCount;
+	m_outputStream << " " << m_linearDistance;
+	m_outputStream << " " << m_angularDistance << endl;
+      }
+      
+      if (m_infoStream)
+	m_infoStream << "update frame " <<  m_readingCount << endl
+		     << "update ld=" << m_linearDistance << " ad=" << m_angularDistance << endl;
+      
+      
+      cerr << "Laser Pose= " << reading.getPose().x << " " << reading.getPose().y 
+	   << " " << reading.getPose().theta << endl;
+      
+      
+      //this is for converting the reading in a scan-matcher feedable form
+      assert(reading.size()==m_beams);
+      double * plainReading = new double[m_beams];
+      for(unsigned int i=0; i<m_beams; i++){
+	plainReading[i]=reading[i];
+      }
+      m_infoStream << "m_count " << m_count << endl;
+
+      RangeReading* reading_copy = 
+              new RangeReading(reading.size(),
+                               &(reading[0]),
+                               static_cast<const RangeSensor*>(reading.getSensor()),
+                               reading.getTime());
+
+      if (m_count>0){
+	scanMatch(plainReading);
+	if (m_outputStream.is_open()){
+	  m_outputStream << "LASER_READING "<< reading.size() << " ";
+	  m_outputStream << setiosflags(ios::fixed) << setprecision(2);
+	  for (RangeReading::const_iterator b=reading.begin(); b!=reading.end(); b++){
+	    m_outputStream << *b << " ";
+	  }
+	  OrientedPoint p=reading.getPose();
+	  m_outputStream << setiosflags(ios::fixed) << setprecision(6);
+	  m_outputStream << p.x << " " << p.y << " " << p.theta << " " << reading.getTime()<< endl;
+	  m_outputStream << "SM_UPDATE "<< m_particles.size() << " ";
+	  for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){
+	    const OrientedPoint& pose=it->pose;
+	    m_outputStream << setiosflags(ios::fixed) << setprecision(3) <<  pose.x << " " << pose.y << " ";
+	    m_outputStream << setiosflags(ios::fixed) << setprecision(6) <<  pose.theta << " " << it-> weight << " ";
+	  }
+	  m_outputStream << endl;
+	}
+	onScanmatchUpdate();
+	
+	updateTreeWeights(false);
+				
+	if (m_infoStream){
+	  m_infoStream << "neff= " << m_neff  << endl;
+	}
+	if (m_outputStream.is_open()){
+	  m_outputStream << setiosflags(ios::fixed) << setprecision(6);
+	  m_outputStream << "NEFF " << m_neff << endl;
+	}
+ 	resample(plainReading, adaptParticles, reading_copy);
+	
+      } else {
+	m_infoStream << "Registering First Scan"<< endl;
+	for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){	
+	  m_matcher.invalidateActiveArea();
+	  m_matcher.computeActiveArea(it->map, it->pose, plainReading);
+	  m_matcher.registerScan(it->map, it->pose, plainReading);
+	  
+	  // cyr: not needed anymore, particles refer to the root in the beginning!
+	  TNode* node=new	TNode(it->pose, 0., it->node,  0);
+	  //node->reading=0;
+      node->reading = reading_copy;
+	  it->node=node;
+	  
+	}
+      }
+      //		cerr  << "Tree: normalizing, resetting and propagating weights at the end..." ;
+      updateTreeWeights(false);
+      //		cerr  << ".done!" <<endl;
+      
+      delete [] plainReading;
+      m_lastPartPose=m_odoPose; //update the past pose for the next iteration
+      m_linearDistance=0;
+      m_angularDistance=0;
+      m_count++;
+      processed=true;
+      
+      //keep ready for the next step
+      for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
+	it->previousPose=it->pose;
+      }
+      
+    }
+    if (m_outputStream.is_open())
+      m_outputStream << flush;
+    m_readingCount++;
+    return processed;
+  }
+  
+  
+  std::ofstream& GridSlamProcessor::outputStream(){
+    return m_outputStream;
+  }
+  
+  std::ostream& GridSlamProcessor::infoStream(){
+    return m_infoStream;
+  }
+  
+  
+  int GridSlamProcessor::getBestParticleIndex() const{
+    unsigned int bi=0;
+    double bw=-std::numeric_limits<double>::max();
+    for (unsigned int i=0; i<m_particles.size(); i++)
+      if (bw<m_particles[i].weightSum){
+	bw=m_particles[i].weightSum;
+	bi=i;
+      }
+    return (int) bi;
+  }
+
+  void GridSlamProcessor::onScanmatchUpdate(){}
+  void GridSlamProcessor::onResampleUpdate(){}
+  void GridSlamProcessor::onOdometryUpdate(){}
+
+  
+};// end namespace
+
+
+
+

slam_gmapping/openslam_gmapping/gridfastslam/gridslamprocessor_tree.cpp

@@ -0,0 +1,263 @@
+#include <string>
+#include <deque>
+#include <list>
+#include <map>
+#include <set>
+#include <fstream>
+//#include <gsl/gsl_blas.h>
+
+#include <gmapping/utils/stat.h>
+#include <gmapping/gridfastslam/gridslamprocessor.h>
+
+namespace GMapping {
+
+using namespace std;
+
+GridSlamProcessor::TNode::TNode(const OrientedPoint& p, double w, TNode* n, unsigned int c){
+	pose=p;
+	weight=w;
+	childs=c;
+	parent=n;
+	reading=0;
+	gweight=0;
+	if (n){
+		n->childs++;
+	}
+	flag=0;
+	accWeight=0;
+}
+
+
+GridSlamProcessor::TNode::~TNode(){
+	if (parent && (--parent->childs)<=0)
+		delete parent;
+	assert(!childs);
+}
+
+
+//BEGIN State Save/Restore
+
+GridSlamProcessor::TNodeVector GridSlamProcessor::getTrajectories() const{
+  TNodeVector v;
+  TNodeMultimap parentCache;
+  TNodeDeque border;
+	
+	for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){
+		TNode* node=it->node;
+		while(node){
+			node->flag=false;
+			node=node->parent;
+		}
+	}
+	
+	for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){
+		TNode* newnode=new TNode(* (it->node) );
+		
+		v.push_back(newnode);
+		assert(newnode->childs==0);
+		if (newnode->parent){
+			parentCache.insert(make_pair(newnode->parent, newnode));
+			//cerr << __PRETTY_FUNCTION__ << ": node " << newnode->parent << " flag=" << newnode->parent->flag<< endl;
+			if (! newnode->parent->flag){
+				//cerr << __PRETTY_FUNCTION__ << ": node " << newnode->parent << " flag=" << newnode->parent->flag<< endl;
+				newnode->parent->flag=true;
+				border.push_back(newnode->parent);
+			}
+		}
+	}
+	
+	//cerr << __PRETTY_FUNCTION__ << ": border.size(INITIAL)=" << border.size() << endl;
+	//cerr << __PRETTY_FUNCTION__ << ": parentCache.size()=" << parentCache.size() << endl;
+	while (! border.empty()){
+		//cerr << __PRETTY_FUNCTION__ << ": border.size(PREPROCESS)=" << border.size() << endl;
+		//cerr << __PRETTY_FUNCTION__ << ": parentCache.size(PREPROCESS)=" << parentCache.size() << endl;
+		const TNode* node=border.front();
+		//cerr << __PRETTY_FUNCTION__ << ": node " << node << endl;
+		border.pop_front();
+		if (! node)
+			continue;
+			
+		TNode* newnode=new TNode(*node);
+		node->flag=false;
+		
+		//update the parent of all of the referring childs 
+		pair<TNodeMultimap::iterator, TNodeMultimap::iterator> p=parentCache.equal_range(node);
+		double childs=0;
+		for (TNodeMultimap::iterator it=p.first; it!=p.second; it++){
+			assert(it->second->parent==it->first);
+			(it->second)->parent=newnode;
+			//cerr << "PS(" << it->first << ", "<< it->second << ")";
+			childs++;
+		}
+		////cerr << endl;
+		parentCache.erase(p.first, p.second);
+		//cerr << __PRETTY_FUNCTION__ << ": parentCache.size(POSTERASE)=" << parentCache.size() << endl;
+		assert(childs==newnode->childs);
+		
+		//unmark the node
+		if ( node->parent ){
+			parentCache.insert(make_pair(node->parent, newnode));
+			if(! node->parent->flag){
+				border.push_back(node->parent);
+				node->parent->flag=true;
+			}	
+		}
+		//insert the parent in the cache
+	}
+	//cerr << __PRETTY_FUNCTION__ << " : checking cloned trajectories" << endl;
+	for (unsigned int i=0; i<v.size(); i++){
+		TNode* node= v[i];
+		while (node){
+			//cerr <<".";
+			node=node->parent;
+		}
+		//cerr << endl;
+	}	
+	
+	return v;
+
+}
+
+void GridSlamProcessor::integrateScanSequence(GridSlamProcessor::TNode* node){
+	//reverse the list 
+	TNode* aux=node;
+	TNode* reversed=0;
+	double count=0;
+	while(aux!=0){
+		TNode * newnode=new TNode(*aux);
+		newnode->parent=reversed;
+		reversed=newnode;
+		aux=aux->parent;
+		count++;
+	}
+	
+	//attach the path to each particle and compute the map;
+	if (m_infoStream )
+		m_infoStream << "Restoring State Nodes=" <<count << endl;
+		
+		
+	aux=reversed;
+	bool first=true;
+	double oldWeight=0;
+	OrientedPoint oldPose;
+	while (aux!=0){
+		if (first){
+			oldPose=aux->pose;
+			first=false;
+			oldWeight=aux->weight;
+		}
+		
+		OrientedPoint dp=aux->pose-oldPose;
+		double dw=aux->weight-oldWeight;
+		oldPose=aux->pose;
+		
+		
+		double * plainReading = new double[m_beams];
+		for(unsigned int i=0; i<m_beams; i++)
+			plainReading[i]=(*(aux->reading))[i];
+		
+		for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
+			//compute the position relative to the path;
+			double s=sin(oldPose.theta-it->pose.theta),
+			       c=cos(oldPose.theta-it->pose.theta);
+			
+			it->pose.x+=c*dp.x-s*dp.y;
+			it->pose.y+=s*dp.x+c*dp.y;
+			it->pose.theta+=dp.theta;
+			it->pose.theta=atan2(sin(it->pose.theta), cos(it->pose.theta));
+			
+			//register the scan
+			m_matcher.invalidateActiveArea();
+			m_matcher.computeActiveArea(it->map, it->pose, plainReading);
+			it->weight+=dw;
+			it->weightSum+=dw;
+
+			// this should not work, since it->weight is not the correct weight!
+			//			it->node=new TNode(it->pose, it->weight, it->node);
+			it->node=new TNode(it->pose, 0.0, it->node);
+			//update the weight
+		}
+		
+		delete [] plainReading;
+		aux=aux->parent;
+	}
+	
+	//destroy the path
+	aux=reversed;
+	while (reversed){
+		aux=reversed;
+		reversed=reversed->parent;
+		delete aux;
+	}
+}
+
+//END State Save/Restore
+
+//BEGIN
+
+void  GridSlamProcessor::updateTreeWeights(bool weightsAlreadyNormalized){
+
+  if (!weightsAlreadyNormalized) {
+    normalize();
+  }
+  resetTree();
+  propagateWeights();
+}
+
+void GridSlamProcessor::resetTree(){
+  // don't calls this function directly, use updateTreeWeights(..) !
+
+	for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
+		TNode* n=it->node;
+		while (n){
+			n->accWeight=0;
+			n->visitCounter=0;
+			n=n->parent;
+		}
+	}
+}
+
+double propagateWeight(GridSlamProcessor::TNode* n, double weight){
+	if (!n)
+		return weight;
+	double w=0;
+	n->visitCounter++;
+	n->accWeight+=weight;
+	if (n->visitCounter==n->childs){
+		w=propagateWeight(n->parent,n->accWeight);
+	}
+	assert(n->visitCounter<=n->childs);
+	return w;
+}
+
+double GridSlamProcessor::propagateWeights(){
+  // don't calls this function directly, use updateTreeWeights(..) !
+
+        // all nodes must be resetted to zero and weights normalized
+
+        // the accumulated weight of the root
+	double lastNodeWeight=0;
+	// sum of the weights in the leafs
+	double aw=0;                   
+
+	std::vector<double>::iterator w=m_weights.begin();
+	for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
+		double weight=*w;
+		aw+=weight;
+		TNode * n=it->node;
+		n->accWeight=weight;
+		lastNodeWeight+=propagateWeight(n->parent,n->accWeight);
+		w++;
+	}
+	
+	if (fabs(aw-1.0) > 0.0001 || fabs(lastNodeWeight-1.0) > 0.0001) {
+	  cerr << "ERROR: ";
+	  cerr << "root->accWeight=" << lastNodeWeight << "    sum_leaf_weights=" << aw << endl;
+	  assert(0);         
+	}
+	return lastNodeWeight;
+}
+
+};
+
+//END

slam_gmapping/openslam_gmapping/gridfastslam/motionmodel.cpp

@@ -0,0 +1,80 @@
+#include <gmapping/gridfastslam/motionmodel.h>
+#include <gmapping/utils/stat.h>
+#include <iostream>
+
+#define MotionModelConditioningLinearCovariance 0.01
+#define MotionModelConditioningAngularCovariance 0.001
+
+namespace GMapping {
+
+
+
+OrientedPoint 
+MotionModel::drawFromMotion (const OrientedPoint& p, double linearMove, double angularMove) const{
+	OrientedPoint n(p);
+	double lm=linearMove  + fabs( linearMove ) * sampleGaussian( srr ) + fabs( angularMove ) * sampleGaussian( str );
+	double am=angularMove + fabs( linearMove ) * sampleGaussian( srt ) + fabs( angularMove ) * sampleGaussian( stt );
+	n.x+=lm*cos(n.theta+.5*am);
+	n.y+=lm*sin(n.theta+.5*am);
+	n.theta+=am;
+	n.theta=atan2(sin(n.theta), cos(n.theta));
+	return n;
+}
+
+OrientedPoint 
+MotionModel::drawFromMotion(const OrientedPoint& p, const OrientedPoint& pnew, const OrientedPoint& pold) const{
+	double sxy=0.3*srr;
+	OrientedPoint delta=absoluteDifference(pnew, pold);
+	OrientedPoint noisypoint(delta);
+	noisypoint.x+=sampleGaussian(srr*fabs(delta.x)+str*fabs(delta.theta)+sxy*fabs(delta.y));
+	noisypoint.y+=sampleGaussian(srr*fabs(delta.y)+str*fabs(delta.theta)+sxy*fabs(delta.x));
+	noisypoint.theta+=sampleGaussian(stt*fabs(delta.theta)+srt*sqrt(delta.x*delta.x+delta.y*delta.y));
+	noisypoint.theta=fmod(noisypoint.theta, 2*M_PI);
+	if (noisypoint.theta>M_PI)
+		noisypoint.theta-=2*M_PI;
+	return absoluteSum(p,noisypoint);
+}
+
+
+/*
+OrientedPoint 
+MotionModel::drawFromMotion(const OrientedPoint& p, const OrientedPoint& pnew, const OrientedPoint& pold) const{
+	
+	//compute the three stps needed for perfectly matching the two poses if the noise is absent
+	
+	OrientedPoint delta=pnew-pold;
+	double aoffset=atan2(delta.y, delta.x);
+	double alpha1=aoffset-pold.theta;
+	alpha1=atan2(sin(alpha1), cos(alpha1));
+	double rho=sqrt(delta*delta);
+	double alpha2=pnew.theta-aoffset;
+	alpha2=atan2(sin(alpha2), cos(alpha2));
+	
+	OrientedPoint pret=drawFromMotion(p, 0, alpha1);
+	pret=drawFromMotion(pret, rho, 0);
+	pret=drawFromMotion(pret, 0, alpha2);
+	return pret;
+}
+*/
+
+
+Covariance3 MotionModel::gaussianApproximation(const OrientedPoint& pnew, const OrientedPoint& pold) const{
+	OrientedPoint delta=absoluteDifference(pnew,pold);
+	double linearMove=sqrt(delta.x*delta.x+delta.y*delta.y);
+	double angularMove=fabs(delta.x);
+	double s11=srr*srr*linearMove*linearMove;
+	double s22=stt*stt*angularMove*angularMove;
+	double s12=str*angularMove*srt*linearMove;
+	Covariance3 cov;
+	double s=sin(pold.theta),c=cos(pold.theta);
+	cov.xx=c*c*s11+MotionModelConditioningLinearCovariance;
+	cov.yy=s*s*s11+MotionModelConditioningLinearCovariance;
+	cov.tt=s22+MotionModelConditioningAngularCovariance;
+	cov.xy=s*c*s11;
+	cov.xt=c*s12;
+	cov.yt=s*s12;
+	return cov;
+}
+
+};
+

slam_gmapping/openslam_gmapping/gui/CMakeLists.txt

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

slam_gmapping/openslam_gmapping/gui/gfs2img.cpp

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

slam_gmapping/openslam_gmapping/gui/gfs_logplayer.cpp

@@ -0,0 +1,41 @@
+/*****************************************************************
+ *
+ * This file is part of the GMAPPING project
+ *
+ * GMAPPING Copyright (c) 2004 Giorgio Grisetti, 
+ * Cyrill Stachniss, and Wolfram Burgard
+ *
+ * This software is licensed under the "Creative Commons 
+ * License (Attribution-NonCommercial-ShareAlike 2.0)" 
+ * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss, 
+ * and Wolfram Burgard.
+ * 
+ * Further information on this license can be found at:
+ * http://creativecommons.org/licenses/by-nc-sa/2.0/
+ * 
+ * GMAPPING is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied 
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
+ * PURPOSE.  
+ *
+ *****************************************************************/
+
+
+#include <qapplication.h>
+#include "qparticleviewer.h"
+
+int  main (int argc, char ** argv){
+	QApplication app(argc, argv);
+	QParticleViewer * pviewer=new QParticleViewer(0);
+	app.setMainWidget(pviewer);
+	pviewer->show();
+	FILE* f=fopen(argv[1], "r");
+	if (!f)
+		return -1;
+	QTextIStream is(f);
+	pviewer->tis=&is;
+	pviewer->start(10);
+	return app.exec();
+	std::cout << "DONE: " << argv[1] <<endl;
+}
+

slam_gmapping/openslam_gmapping/gui/gfs_nogui.cpp

@@ -0,0 +1,64 @@
+/*****************************************************************
+ *
+ * This file is part of the GMAPPING project
+ *
+ * GMAPPING Copyright (c) 2004 Giorgio Grisetti, 
+ * Cyrill Stachniss, and Wolfram Burgard
+ *
+ * This software is licensed under the "Creative Commons 
+ * License (Attribution-NonCommercial-ShareAlike 2.0)" 
+ * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss, 
+ * and Wolfram Burgard.
+ * 
+ * Further information on this license can be found at:
+ * http://creativecommons.org/licenses/by-nc-sa/2.0/
+ * 
+ * GMAPPING is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied 
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
+ * PURPOSE.  
+ *
+ *****************************************************************/
+
+
+#include <unistd.h>
+#include "gsp_thread.h"
+
+using namespace GMapping;
+
+int  main (int argc, char ** argv){
+  cerr << "GMAPPING copyright 2004 by Giorgio Grisetti, Cyrill Stachniss," << endl ;
+  cerr << "and Wolfram Burgard. To be published under the CreativeCommons license," << endl ;
+  cerr << "see: http://creativecommons.org/licenses/by-nc-sa/2.0/" << endl << endl;
+
+
+	GridSlamProcessorThread* gsp=  new GridSlamProcessorThread;
+	if (gsp->init(argc, argv)){
+		cout << "GSP INIT ERROR" << endl;
+		return -1;
+	}
+	cout <<"GSP INITIALIZED"<< endl;
+	if (gsp->loadFiles()){
+		cout <<"GSP READFILE ERROR"<< endl;
+		return -2;
+	}
+	cout <<"FILES LOADED"<< endl;
+	gsp->setMapUpdateTime(1000000);
+	gsp->start();
+	cout <<"THREAD STARTED"<< endl;
+	bool done=false;
+	while (!done){
+		GridSlamProcessorThread::EventDeque events=gsp->getEvents();
+		for (GridSlamProcessorThread::EventDeque::iterator it=events.begin(); it!=events.end(); it++){
+			cout << flush;
+			GridSlamProcessorThread::DoneEvent* doneEvent=dynamic_cast<GridSlamProcessorThread::DoneEvent*>(*it);
+			if (doneEvent){
+				done=true;
+				cout <<"DONE!"<< endl;
+				gsp->stop();
+			}
+			if (*it)
+				delete(*it);
+		}
+	}
+}

slam_gmapping/openslam_gmapping/gui/gfs_simplegui.cpp

@@ -0,0 +1,96 @@
+/*****************************************************************
+ *
+ * This file is part of the GMAPPING project
+ *
+ * GMAPPING Copyright (c) 2004 Giorgio Grisetti, 
+ * Cyrill Stachniss, and Wolfram Burgard
+ *
+ * This software is licensed under the "Creative Commons 
+ * License (Attribution-NonCommercial-ShareAlike 2.0)" 
+ * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss, 
+ * and Wolfram Burgard.
+ * 
+ * Further information on this license can be found at:
+ * http://creativecommons.org/licenses/by-nc-sa/2.0/
+ * 
+ * GMAPPING is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied 
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
+ * PURPOSE.  
+ *
+ *****************************************************************/
+
+
+#include "qparticleviewer.h"
+#include "qgraphpainter.h"
+#include <qapplication.h>
+#include <qframe.h>
+#include <qlabel.h>
+#include <qlayout.h>
+#include <QVBoxLayout>
+#include <qmainwindow.h>
+
+class GFSMainWindow: public QMainWindow{
+public:
+  GFSMainWindow(GridSlamProcessorThread* t){
+    gsp_thread=t;
+    QVBoxLayout* layout=new QVBoxLayout(this);
+    pviewer=new QParticleViewer(this,0,0,gsp_thread);
+    pviewer->setGeometry(0,0,500,500);
+    pviewer->setFocusPolicy(Qt::ClickFocus);
+    layout->addWidget(pviewer);
+						
+    gpainter=new QGraphPainter(this);
+    gpainter->setFixedHeight(100);
+    layout->addWidget(gpainter);
+    gpainter->setRange(0,1);
+    gpainter->setTitle("Neff");
+		
+    help = new QLabel(QString("+/- - zoom | b - show/hide best path | p - show/hide all paths | c - center robot "),this); 
+    help->setMaximumHeight(30);
+    layout->addWidget(help);
+	
+    QObject::connect( pviewer, SIGNAL(neffChanged(double) ), gpainter, SLOT(valueAdded(double)) );
+    setTabOrder(pviewer, pviewer);
+  }
+		
+  void start(int c){
+    pviewer->start(c);
+    gpainter->start(c);
+  }
+
+protected:
+  GridSlamProcessorThread* gsp_thread;
+  QVBoxLayout* layout;
+  QParticleViewer* pviewer;
+  QGraphPainter* gpainter;
+  QLabel* help;
+};
+
+
+int  main (int argc, char ** argv){
+  cerr << "GMAPPING copyright 2004 by Giorgio Grisetti, Cyrill Stachniss," << endl ;
+  cerr << "and Wolfram Burgard. To be published under the CreativeCommons license," << endl;
+  cerr << "see: http://creativecommons.org/licenses/by-nc-sa/2.0/" << endl << endl;
+
+
+  GridSlamProcessorThread* gsp=  new GridSlamProcessorThread;
+  if (gsp->init(argc, argv)){
+    cerr << "GridFastSlam: Initialization Error!" << endl;
+    cerr << "(Did you specified an input file for reading?)" << endl;
+    return -1;
+  }
+  if (gsp->loadFiles()){
+    cerr <<"Error reading file!"<< endl;
+    return -2;
+  }
+  cerr <<"File successfully loaded!"<< endl;
+  QApplication app(argc, argv);
+  GFSMainWindow* mainWin=new GFSMainWindow(gsp);
+  mainWin->show();
+  gsp->setEventBufferSize(10000);
+  gsp->start();
+  mainWin->start(1000);
+  return app.exec();
+}
+

slam_gmapping/openslam_gmapping/gui/gsp_thread.cpp

@@ -0,0 +1,643 @@
+/*****************************************************************
+ *
+ * This file is part of the GMAPPING project
+ *
+ * GMAPPING Copyright (c) 2004 Giorgio Grisetti, 
+ * Cyrill Stachniss, and Wolfram Burgard
+ *
+ * This software is licensed under the "Creative Commons 
+ * License (Attribution-NonCommercial-ShareAlike 2.0)" 
+ * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss, 
+ * and Wolfram Burgard.
+ * 
+ * Further information on this license can be found at:
+ * http://creativecommons.org/licenses/by-nc-sa/2.0/
+ * 
+ * GMAPPING is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied 
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
+ * PURPOSE.  
+ *
+ *****************************************************************/
+
+
+#include "gsp_thread.h"
+#include <utils/commandline.h>
+#include <utils/stat.h>
+#include <configfile/configfile.h>
+
+#ifdef CARMEN_SUPPORT
+	#include <carmenwrapper/carmenwrapper.h>
+#endif
+
+#define DEBUG cout << __PRETTY_FUNCTION__
+
+using namespace std;
+
+int GridSlamProcessorThread::init(int argc, const char * const * argv){
+	m_argc=argc;
+	m_argv=argv;
+	std::string configfilename;
+	std::string ebuf="not_set";
+
+	CMD_PARSE_BEGIN_SILENT(1,argc);
+		parseStringSilent("-cfg",configfilename);
+	CMD_PARSE_END_SILENT;
+
+	if (configfilename.length()>0){
+	  ConfigFile cfg(configfilename);
+
+	  filename = (std::string) cfg.value("gfs","filename",filename);
+	  outfilename = (std::string) cfg.value("gfs","outfilename",outfilename);
+	  xmin = cfg.value("gfs","xmin", xmin);
+	  xmax = cfg.value("gfs","xmax",xmax);
+	  ymin = cfg.value("gfs","ymin",ymin);
+	  ymax = cfg.value("gfs","ymax",ymax);
+	  delta =  cfg.value("gfs","delta",delta);
+	  maxrange = cfg.value("gfs","maxrange",maxrange);
+	  maxUrange = cfg.value("gfs","maxUrange",maxUrange);
+	  regscore = cfg.value("gfs","regscore",regscore);
+	  critscore = cfg.value("gfs","critscore",critscore);
+	  kernelSize = cfg.value("gfs","kernelSize",kernelSize);
+	  sigma = cfg.value("gfs","sigma",sigma);
+	  iterations = cfg.value("gfs","iterations",iterations);
+	  lstep = cfg.value("gfs","lstep",lstep);
+	  astep = cfg.value("gfs","astep",astep);
+	  maxMove = cfg.value("gfs","maxMove",maxMove);
+	  srr = cfg.value("gfs","srr", srr);
+	  srt = cfg.value("gfs","srt", srt);
+	  str = cfg.value("gfs","str", str);
+	  stt = cfg.value("gfs","stt", stt);
+	  particles = cfg.value("gfs","particles",particles);
+	  angularUpdate = cfg.value("gfs","angularUpdate", angularUpdate);
+	  linearUpdate = cfg.value("gfs","linearUpdate", linearUpdate);
+	  lsigma = cfg.value("gfs","lsigma", lsigma);
+	  ogain = cfg.value("gfs","lobsGain", ogain);
+	  lskip = (int)cfg.value("gfs","lskip", lskip);
+	  mapUpdateTime = cfg.value("gfs","mapUpdate", mapUpdateTime);
+	  randseed = cfg.value("gfs","randseed", randseed);
+	  autosize = cfg.value("gfs","autosize", autosize);
+	  readFromStdin = cfg.value("gfs","stdin", readFromStdin);
+	  resampleThreshold = cfg.value("gfs","resampleThreshold", resampleThreshold);
+	  skipMatching = cfg.value("gfs","skipMatching", skipMatching);
+	  onLine = cfg.value("gfs","onLine", onLine);
+	  generateMap = cfg.value("gfs","generateMap", generateMap);
+	  m_minimumScore = cfg.value("gfs","minimumScore", m_minimumScore);
+	  llsamplerange = cfg.value("gfs","llsamplerange", llsamplerange);
+	  lasamplerange = cfg.value("gfs","lasamplerange",lasamplerange );
+	  llsamplestep = cfg.value("gfs","llsamplestep", llsamplestep);
+	  lasamplestep = cfg.value("gfs","lasamplestep", lasamplestep);
+	  linearOdometryReliability = cfg.value("gfs","linearOdometryReliability",linearOdometryReliability);
+	  angularOdometryReliability = cfg.value("gfs","angularOdometryReliability",angularOdometryReliability);
+	  ebuf = (std::string) cfg.value("gfs","estrategy", ebuf);
+	  considerOdometryCovariance = cfg.value("gfs","considerOdometryCovariance",considerOdometryCovariance);
+	  
+	}
+
+
+	CMD_PARSE_BEGIN(1,argc);
+          	parseString("-cfg",configfilename);     /* to avoid the warning*/
+		parseString("-filename",filename);
+		parseString("-outfilename",outfilename);
+		parseDouble("-xmin",xmin);
+		parseDouble("-xmax",xmax);
+		parseDouble("-ymin",ymin);
+		parseDouble("-ymax",ymax);
+		parseDouble("-delta",delta);
+		parseDouble("-maxrange",maxrange);
+		parseDouble("-maxUrange",maxUrange);
+		parseDouble("-regscore",regscore);
+		parseDouble("-critscore",critscore);
+		parseInt("-kernelSize",kernelSize);
+		parseDouble("-sigma",sigma);
+		parseInt("-iterations",iterations);
+		parseDouble("-lstep",lstep);
+		parseDouble("-astep",astep);
+		parseDouble("-maxMove",maxMove);
+		parseDouble("-srr", srr);
+		parseDouble("-srt", srt);
+		parseDouble("-str", str);
+		parseDouble("-stt", stt);
+		parseInt("-particles",particles);
+		parseDouble("-angularUpdate", angularUpdate);
+		parseDouble("-linearUpdate", linearUpdate);
+		parseDouble("-lsigma", lsigma);
+		parseDouble("-lobsGain", ogain);
+		parseInt("-lskip", lskip);
+		parseInt("-mapUpdate", mapUpdateTime);
+		parseInt("-randseed", randseed);
+		parseFlag("-autosize", autosize);
+		parseFlag("-stdin", readFromStdin);
+		parseDouble("-resampleThreshold", resampleThreshold);
+		parseFlag("-skipMatching", skipMatching);
+		parseFlag("-onLine", onLine);
+		parseFlag("-generateMap", generateMap);
+		parseDouble("-minimumScore", m_minimumScore);
+		parseDouble("-llsamplerange", llsamplerange);
+		parseDouble("-lasamplerange", lasamplerange);
+		parseDouble("-llsamplestep", llsamplestep);
+		parseDouble("-lasamplestep", lasamplestep);
+		parseDouble("-linearOdometryReliability",linearOdometryReliability);
+		parseDouble("-angularOdometryReliability",angularOdometryReliability);
+		parseString("-estrategy", ebuf);
+		
+		parseFlag("-considerOdometryCovariance",considerOdometryCovariance);
+	CMD_PARSE_END;
+	
+	if (filename.length() <=0){
+		cout << "no filename specified" << endl;
+		return -1;
+	}
+	return 0;
+}
+
+int GridSlamProcessorThread::loadFiles(const char * fn){
+	if (onLine){
+		cout << " onLineProcessing" << endl;
+		return 0;
+	}	
+	ifstream is;
+	if (fn)
+	  is.open(fn);
+	else
+	  is.open(filename.c_str());
+	if (! is){
+		cout << "no file found" << endl;
+		return -1;
+	}
+
+	CarmenConfiguration conf;
+	conf.load(is);
+	is.close();
+
+	sensorMap=conf.computeSensorMap();
+	
+	if (input)
+		delete input;
+	
+	if (! readFromStdin){
+		plainStream.open(filename.c_str());
+		input=new InputSensorStream(sensorMap, plainStream);
+		cout << "Plain Stream opened="<< (bool) plainStream << endl;
+	} else {
+		input=new InputSensorStream(sensorMap, cin);
+		cout << "Plain Stream opened on stdin" << endl;
+	}
+	return 0;
+}	
+
+GridSlamProcessorThread::GridSlamProcessorThread(): GridSlamProcessor(cerr){
+	//This are the processor parameters
+	filename="";
+	outfilename="";
+	xmin=-100.;
+	ymin=-100.;
+	xmax=100.;
+	ymax=100.;
+	delta=0.05;
+	
+	//scan matching parameters
+	sigma=0.05;
+	maxrange=80.;
+	maxUrange=80.;
+	regscore=1e4;
+	lstep=.05;
+	astep=.05;
+	kernelSize=1;
+	iterations=5;
+	critscore=0.;
+	maxMove=1.;
+	lsigma=.075;
+	ogain=3;
+	lskip=0;
+	autosize=false;
+	skipMatching=false;
+
+	//motion model parameters
+	srr=0.1, srt=0.1, str=0.1, stt=0.1;
+	//particle parameters
+	particles=30;
+	randseed=0;
+	
+	//gfs parameters
+	angularUpdate=0.5;
+	linearUpdate=1;
+	resampleThreshold=0.5;
+	
+	input=0;
+	
+	pthread_mutex_init(&hp_mutex,0);
+	pthread_mutex_init(&ind_mutex,0);
+	pthread_mutex_init(&hist_mutex,0);
+	running=false;
+	eventBufferLength=0;
+	mapUpdateTime=5;
+	mapTimer=0;
+	readFromStdin=false;
+	onLine=false;
+	generateMap=false;
+
+	// This  are the dafault settings for a grid map of 5 cm
+	llsamplerange=0.01;
+	llsamplestep=0.01;
+	lasamplerange=0.005;
+	lasamplestep=0.005;
+	linearOdometryReliability=0.;
+	angularOdometryReliability=0.;
+	
+	considerOdometryCovariance=false;
+/*	
+	// This  are the dafault settings for a grid map of 10 cm
+	m_llsamplerange=0.1;
+	m_llsamplestep=0.1;
+	m_lasamplerange=0.02;
+	m_lasamplestep=0.01;
+*/	
+	// This  are the dafault settings for a grid map of 20/25 cm
+/*
+	m_llsamplerange=0.2;
+	m_llsamplestep=0.1;
+	m_lasamplerange=0.02;
+	m_lasamplestep=0.01;
+	m_generateMap=false;
+*/
+
+			
+}
+
+GridSlamProcessorThread::~GridSlamProcessorThread(){
+	pthread_mutex_destroy(&hp_mutex);
+	pthread_mutex_destroy(&ind_mutex);
+	pthread_mutex_destroy(&hist_mutex);
+	
+	for (deque<Event*>::const_iterator it=eventBuffer.begin(); it!=eventBuffer.end(); it++)
+		delete *it;
+}
+	
+
+void * GridSlamProcessorThread::fastslamthread(GridSlamProcessorThread* gpt){
+	if (! gpt->input && ! gpt->onLine)
+		return 0;
+		
+	
+	//if started online retrieve the settings from the connection
+#ifdef CARMEN_SUPPORT
+	if (gpt->onLine){
+		cout << "starting the process:" << endl;
+		CarmenWrapper::initializeIPC(gpt->m_argv[0]);
+		CarmenWrapper::start(gpt->m_argv[0]);
+		cout << "Waiting for retrieving the sensor map:" << endl;
+		while (! CarmenWrapper::sensorMapComputed()){
+			usleep(500000);
+			cout << "." << flush;
+		}
+		gpt->sensorMap=CarmenWrapper::sensorMap();
+		cout << "Connected " << endl;
+	}
+#else
+	if (gpt->onLine){
+		cout << "FATAL ERROR: cannot run online without the carmen support" << endl;
+		DoneEvent *done=new DoneEvent;
+		gpt->addEvent(done);
+		return 0;
+	}
+#endif
+	
+	gpt->setSensorMap(gpt->sensorMap);
+	gpt->setMatchingParameters(gpt->maxUrange, gpt->maxrange, gpt->sigma, gpt->kernelSize, gpt->lstep, gpt->astep, gpt->iterations, gpt->lsigma, gpt->ogain, gpt->lskip);
+	
+	double xmin=gpt->xmin, 
+	       ymin=gpt->ymin, 
+	       xmax=gpt->xmax, 
+	       ymax=gpt->ymax;
+	
+	OrientedPoint initialPose(0,0,0);
+	
+	if (gpt->autosize){
+		if (gpt->readFromStdin || gpt->onLine)
+		cout << "Error, cant autosize form stdin" << endl;
+		SensorLog * log=new SensorLog(gpt->sensorMap);
+		ifstream is(gpt->filename.c_str());
+		log->load(is);
+		is.close();
+		initialPose=gpt->boundingBox(log, xmin, ymin, xmax, ymax);
+		delete log;
+	}
+	
+	if( gpt->infoStream()){
+		gpt->infoStream() << " initialPose=" << initialPose.x << " " << initialPose.y << " " << initialPose.theta
+				<< cout << " xmin=" << xmin <<" ymin=" << ymin <<" xmax=" << xmax <<" ymax=" << ymax << endl;
+	}
+	gpt->setMotionModelParameters(gpt->srr, gpt->srt, gpt->str, gpt->stt);
+	gpt->setUpdateDistances(gpt->linearUpdate, gpt->angularUpdate, gpt->resampleThreshold);
+	gpt->setgenerateMap(gpt->generateMap);
+	gpt->GridSlamProcessor::init(gpt->particles, xmin, ymin, xmax, ymax, gpt->delta, initialPose);
+	gpt->setllsamplerange(gpt->llsamplerange);
+	gpt->setllsamplestep(gpt->llsamplestep);
+	gpt->setlasamplerange(gpt->llsamplerange);
+	gpt->setlasamplestep(gpt->llsamplestep);
+	
+#define printParam(n)\
+	 gpt->outputStream() \
+	 << "PARAM " << \
+	 #n \
+	 << " " << gpt->n << endl
+	
+	if (gpt->outfilename.length()>0 ){
+		gpt->outputStream().open(gpt->outfilename.c_str());
+		printParam(filename);
+		printParam(outfilename);
+		printParam(xmin);
+		printParam(ymin);
+		printParam(xmax);
+		printParam(ymax);
+		printParam(delta);
+		
+		//scan matching parameters
+		printParam(sigma);
+		printParam(maxrange);
+		printParam(maxUrange);
+		printParam(regscore);
+		printParam(lstep);
+		printParam(astep);
+		printParam(kernelSize);
+		printParam(iterations);
+		printParam(critscore);
+		printParam(maxMove);
+		printParam(lsigma);
+		printParam(ogain);
+		printParam(lskip);
+		printParam(autosize);
+		printParam(skipMatching);
+	
+		//motion model parameters
+		printParam(srr);
+		printParam(srt); 
+		printParam(str);
+		printParam(stt);
+		//particle parameters
+		printParam(particles);
+		printParam(randseed);
+		
+		//gfs parameters
+		printParam(angularUpdate);
+		printParam(linearUpdate);
+		printParam(resampleThreshold);
+		
+		printParam(llsamplerange);
+		printParam(lasamplerange);
+		printParam(llsamplestep);
+		printParam(lasamplestep);
+	}
+	#undef printParam
+	
+	if (gpt->randseed!=0)
+		sampleGaussian(1,gpt->randseed);
+	if (!gpt->infoStream()){
+		cerr << "cant open info stream for writing by unuseful debug messages" << endl;
+	} else {
+		gpt->infoStream() << "setting randseed" << gpt->randseed<< endl;
+	}
+	
+	
+#ifdef CARMEN_SUPPORT
+	list<RangeReading*> rrlist;
+	if (gpt->onLine){
+		RangeReading rr(0,0);
+		while (1){
+			while (CarmenWrapper::getReading(rr)){
+				RangeReading* nr=new RangeReading(rr);
+				rrlist.push_back(nr);
+				gpt->processScan(*nr);
+			}
+		}
+	}
+#endif
+	ofstream rawpath("rawpath.dat");
+	if (!gpt->onLine){
+		while(*(gpt->input) && gpt->running){
+			const SensorReading* r;
+			(*(gpt->input)) >> r;
+			if (! r)
+				continue;
+			const RangeReading* rr=dynamic_cast<const RangeReading*>(r);
+			if (rr && gpt->running){
+				const RangeSensor* rs=dynamic_cast<const RangeSensor*>(rr->getSensor());
+				assert (rs && rs->beams().size()==rr->size());
+				
+				bool processed=gpt->processScan(*rr);
+				rawpath << rr->getPose().x << " " << rr->getPose().y << " " << rr->getPose().theta << endl;
+				if (0 && processed){
+				cerr << "Retrieving state .. ";
+					TNodeVector trajetories=gpt->getTrajectories();
+					cerr << "Done" <<  endl;
+					cerr << "Deleting Tree state .. ";
+					for (TNodeVector::iterator it=trajetories.begin(); it!=trajetories.end(); it++)
+						delete *it;
+					cerr << "Done" << endl;
+				}
+// 				if (0 && processed){
+// 					cerr << "generating copy" << endl;;
+// 					GridSlamProcessor* m_gsp=gpt->clone();
+// 					Map<double, DoubleArray2D, false>*  pmap=m_gsp->getParticles()[0].map.toDoubleMap() ;
+// 					cerr << "deleting" << endl;
+// 					delete m_gsp;
+// 					delete pmap;
+// 				}
+			}
+			const OdometryReading* o=dynamic_cast<const OdometryReading*>(r);
+			if (o && gpt->running){
+				gpt->processTruePos(*o);
+				TruePosEvent* truepos=new TruePosEvent;
+				truepos->pose=o->getPose();
+			}
+		}
+	}
+	rawpath.close();
+
+	TNodeVector trajetories=gpt->getTrajectories();
+	cerr << "WRITING WEIGHTS" << endl;
+	int pnumber=0;
+	for (TNodeVector::iterator it=trajetories.begin(); it!=trajetories.end(); it++){
+		char buf[10];
+		sprintf(buf, "w-%03d.dat",pnumber);
+		ofstream weightsStream(buf);
+		GridSlamProcessor::TNode* n=*it;
+		double oldWeight=0, oldgWeight=0;
+		while (n!=0){
+			double w=n->weight-oldWeight;
+			double gw=n->gweight-oldgWeight;
+			oldWeight=n->weight;
+			oldgWeight=n->gweight;
+			weightsStream << w << " " << gw << endl;
+			n=n->parent;
+		}
+		weightsStream.close();
+		pnumber++;
+		cerr << buf << endl;
+	}
+		
+	DoneEvent *done=new DoneEvent;
+	gpt->addEvent(done);
+	gpt->infoStream() << "Hallo, I am the gsp thread. I have finished. Do you think it is the case of checking for unlocked mutexes." << endl;
+	return 0;
+}
+
+std::vector<OrientedPoint> GridSlamProcessorThread::getHypotheses(){
+	pthread_mutex_lock(&hp_mutex);
+	std::vector<OrientedPoint> retval(hypotheses);
+	pthread_mutex_unlock(&hp_mutex);
+	return retval;
+}
+
+std::vector<unsigned int> GridSlamProcessorThread::getIndexes(){
+	pthread_mutex_lock(&ind_mutex);
+	std::vector<unsigned int> retval(indexes);
+	pthread_mutex_unlock(&ind_mutex);
+	return retval;
+}
+
+void GridSlamProcessorThread::start(){
+	if (running)
+		return;
+	running=true;
+	pthread_create(&gfs_thread, 0, (void * (*)(void *))fastslamthread, (void *) this);
+}
+
+void GridSlamProcessorThread::stop(){
+	if (! running){
+		cout << "PORCO CAZZO" << endl;
+		return;
+	}
+	running=false;
+	void * retval;
+	pthread_join(gfs_thread, &retval);
+}
+
+void GridSlamProcessorThread::onOdometryUpdate(){
+	pthread_mutex_lock(&hp_mutex);
+	hypotheses.clear();
+	weightSums.clear();
+	for (GridSlamProcessor::ParticleVector::const_iterator part=getParticles().begin(); part!=getParticles().end(); part++ ){
+		hypotheses.push_back(part->pose);
+		weightSums.push_back(part->weightSum);
+	}
+	
+	ParticleMoveEvent* event=new ParticleMoveEvent;
+	event->scanmatched=false;
+	event->hypotheses=hypotheses;
+	event->weightSums=weightSums;
+	event->neff=m_neff;
+	pthread_mutex_unlock(&hp_mutex);
+	
+	addEvent(event);
+	
+	syncOdometryUpdate();
+}
+
+void GridSlamProcessorThread::onResampleUpdate(){
+	pthread_mutex_lock(&ind_mutex);
+	pthread_mutex_lock(&hp_mutex);
+	
+	indexes=GridSlamProcessor::getIndexes();
+	
+	assert (indexes.size()==getParticles().size());
+	ResampleEvent* event=new ResampleEvent;
+	event->indexes=indexes;
+	
+	pthread_mutex_unlock(&hp_mutex);
+	pthread_mutex_unlock(&ind_mutex);
+	
+	addEvent(event);
+	
+	syncResampleUpdate();
+}
+
+void GridSlamProcessorThread::onScanmatchUpdate(){
+	pthread_mutex_lock(&hp_mutex);
+	hypotheses.clear();
+	weightSums.clear();
+	unsigned int bestIdx=0;
+	double bestWeight=-1e1000;
+	unsigned int idx=0;
+	for (GridSlamProcessor::ParticleVector::const_iterator part=getParticles().begin(); part!=getParticles().end(); part++ ){
+		hypotheses.push_back(part->pose);
+		weightSums.push_back(part->weightSum);
+		if(part->weightSum>bestWeight){
+			bestIdx=idx;
+			bestWeight=part->weightSum;
+		}
+		idx++;
+	}
+	
+	ParticleMoveEvent* event=new ParticleMoveEvent;
+	event->scanmatched=true;
+	event->hypotheses=hypotheses;
+	event->weightSums=weightSums;
+	event->neff=m_neff;
+	addEvent(event);
+	
+	if (! mapTimer){
+		MapEvent* event=new MapEvent;
+		event->index=bestIdx;
+		event->pmap=new ScanMatcherMap(getParticles()[bestIdx].map);
+		event->pose=getParticles()[bestIdx].pose;
+		addEvent(event);
+	}
+	
+	mapTimer++;
+	mapTimer=mapTimer%mapUpdateTime;
+	
+	pthread_mutex_unlock(&hp_mutex);
+
+	syncOdometryUpdate();
+}
+
+void GridSlamProcessorThread::syncOdometryUpdate(){
+}
+
+void GridSlamProcessorThread::syncResampleUpdate(){
+}
+
+void GridSlamProcessorThread::syncScanmatchUpdate(){
+}
+
+void GridSlamProcessorThread::addEvent(GridSlamProcessorThread::Event * e){
+	pthread_mutex_lock(&hist_mutex);
+	while (eventBuffer.size()>eventBufferLength){
+		Event* event=eventBuffer.front();
+		delete event;
+		eventBuffer.pop_front();
+	}
+	eventBuffer.push_back(e);
+	pthread_mutex_unlock(&hist_mutex);
+}
+
+GridSlamProcessorThread::EventDeque GridSlamProcessorThread::getEvents(){
+	pthread_mutex_lock(&hist_mutex);
+	EventDeque copy(eventBuffer);
+	eventBuffer.clear();
+	pthread_mutex_unlock(&hist_mutex);
+	return copy;
+}
+
+GridSlamProcessorThread::Event::~Event(){}
+
+GridSlamProcessorThread::MapEvent::~MapEvent(){
+	if (pmap)
+		delete pmap;
+}
+
+void GridSlamProcessorThread::setEventBufferSize(unsigned int length){
+	eventBufferLength=length;
+}
+
+OrientedPoint GridSlamProcessorThread::boundingBox(SensorLog* log, double& xmin, double& ymin, double& xmax, double& ymax) const{
+	OrientedPoint initialPose(0,0,0);
+	initialPose=log->boundingBox(xmin, ymin, xmax, ymax);
+	xmin-=3*maxrange;
+	ymin-=3*maxrange;
+	xmax+=3*maxrange;
+	ymax+=3*maxrange;
+	return initialPose;
+}

slam_gmapping/openslam_gmapping/gui/gsp_thread.h

@@ -0,0 +1,177 @@
+/*****************************************************************
+ *
+ * This file is part of the GMAPPING project
+ *
+ * GMAPPING Copyright (c) 2004 Giorgio Grisetti, 
+ * Cyrill Stachniss, and Wolfram Burgard
+ *
+ * This software is licensed under the "Creative Commons 
+ * License (Attribution-NonCommercial-ShareAlike 2.0)" 
+ * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss, 
+ * and Wolfram Burgard.
+ * 
+ * Further information on this license can be found at:
+ * http://creativecommons.org/licenses/by-nc-sa/2.0/
+ * 
+ * GMAPPING is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied 
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
+ * PURPOSE.  
+ *
+ *****************************************************************/
+
+
+#ifndef GSP_THREAD_H
+#define GSP_THREAD_H
+
+#include <unistd.h>
+#include <pthread.h>
+#include <deque>
+#include <fstream>
+#include <iostream>
+#include <log/carmenconfiguration.h>
+#include <log/sensorstream.h>
+#include <gridfastslam/gridslamprocessor.h>
+
+using namespace std;
+using namespace GMapping;
+
+
+#define MAX_STRING_LENGTH 1024
+
+
+struct GridSlamProcessorThread : public GridSlamProcessor {
+		struct Event{
+			virtual ~Event();
+		};
+		
+		struct ParticleMoveEvent: public Event{
+			bool scanmatched;
+			double neff;
+			std::vector<OrientedPoint> hypotheses;
+			std::vector<double> weightSums;
+		};
+		
+		struct TruePosEvent : public Event{
+			OrientedPoint pose;
+		};
+		
+		struct ResampleEvent: public Event{
+			std::vector<unsigned int> indexes;
+		};
+		
+		struct MapEvent: public Event{
+			ScanMatcherMap* pmap;
+			unsigned int index;
+			OrientedPoint pose;
+			virtual ~MapEvent();
+		};
+		
+		struct DoneEvent: public Event{
+		};
+		
+		typedef deque<Event*> EventDeque;
+		
+		GridSlamProcessorThread();
+		~GridSlamProcessorThread();	
+		int init(int argc, const char * const * argv);
+		int loadFiles(const char * fn=0);
+		static void * fastslamthread(GridSlamProcessorThread* gpt);
+		std::vector<OrientedPoint> getHypotheses();
+		std::vector<unsigned int> getIndexes();
+		
+		EventDeque getEvents();
+		
+		void start();
+		void stop();
+		
+		virtual void onOdometryUpdate();
+		virtual void onResampleUpdate();
+		virtual void onScanmatchUpdate();
+		
+		virtual void syncOdometryUpdate();
+		virtual void syncResampleUpdate();
+		virtual void syncScanmatchUpdate();
+		
+		void setEventBufferSize(unsigned int length);
+		inline void setMapUpdateTime(unsigned int ut) {mapUpdateTime=ut;}
+		inline bool isRunning() const {return running;}
+		OrientedPoint boundingBox(SensorLog* log, double& xmin, double& ymin, double& xmax, double& ymax) const;
+	private:
+		
+		void addEvent(Event *);
+		EventDeque eventBuffer;
+		
+		unsigned int eventBufferLength;
+		unsigned int mapUpdateTime;
+		unsigned int mapTimer;
+		
+		//thread interaction stuff
+		std::vector<OrientedPoint> hypotheses;
+		std::vector<unsigned int> indexes;
+		std::vector<double> weightSums;
+		pthread_mutex_t hp_mutex, ind_mutex, hist_mutex;
+		pthread_t gfs_thread;
+		bool running;
+		
+		//This are the processor parameters
+		std::string filename;
+		std::string outfilename;
+
+		double xmin;
+		double ymin;
+		double xmax;
+		double ymax;
+		bool autosize;
+		double delta;
+		double resampleThreshold;
+		
+		//scan matching parameters
+		double sigma;
+		double maxrange;
+		double maxUrange;
+		double regscore;
+		double lstep;
+		double astep;
+		int kernelSize;
+		int iterations;
+		double critscore;
+		double maxMove;
+		unsigned int lskip;
+
+		//likelihood
+		double lsigma;
+		double ogain;
+		double llsamplerange, lasamplerange;
+		double llsamplestep, lasamplestep;
+		double linearOdometryReliability;
+		double angularOdometryReliability;
+		
+	
+		//motion model parameters
+		double srr, srt, str, stt;
+		//particle parameters
+		int particles;
+		bool skipMatching;
+		
+		//gfs parameters
+		double angularUpdate;
+		double linearUpdate;
+	
+		//robot config
+		SensorMap sensorMap;
+		//input stream
+		InputSensorStream* input;
+		std::ifstream plainStream;
+		bool readFromStdin;
+		bool onLine;
+		bool generateMap;
+		bool considerOdometryCovariance;
+		unsigned int randseed;
+		
+		//dirty carmen interface
+		const char* const * m_argv;
+		unsigned int m_argc;
+		
+};
+#endif

slam_gmapping/openslam_gmapping/gui/qgraphpainter.cpp

@@ -0,0 +1,142 @@
+/*****************************************************************
+ *
+ * This file is part of the GMAPPING project
+ *
+ * GMAPPING Copyright (c) 2004 Giorgio Grisetti, 
+ * Cyrill Stachniss, and Wolfram Burgard
+ *
+ * This software is licensed under the "Creative Commons 
+ * License (Attribution-NonCommercial-ShareAlike 2.0)" 
+ * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss, 
+ * and Wolfram Burgard.
+ * 
+ * Further information on this license can be found at:
+ * http://creativecommons.org/licenses/by-nc-sa/2.0/
+ * 
+ * GMAPPING is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied 
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
+ * PURPOSE.  
+ *
+ *****************************************************************/
+
+
+#include <iostream>
+#include "qgraphpainter.h"
+#include "moc_qgraphpainter.cpp"
+using namespace std;
+
+QGraphPainter::QGraphPainter( QWidget * parent, const char * name, WFlags f):
+	QWidget(parent, name, f|WRepaintNoErase|WResizeNoErase){
+	m_pixmap=new QPixmap(size());
+	m_pixmap->fill(Qt::white);
+	autoscale=false;
+	m_useYReference = false;
+}
+
+void QGraphPainter::resizeEvent(QResizeEvent * sizeev){
+	m_pixmap->resize(sizeev->size());
+}
+
+QGraphPainter::~QGraphPainter(){
+	delete m_pixmap;
+}
+
+void QGraphPainter::clear(){
+	values.clear();
+}
+
+void QGraphPainter::valueAdded(double v){
+	values.push_back(v);
+}
+
+void QGraphPainter::valueAdded(double v, double _min, double _max){
+	setRange(_min, _max);
+	values.push_back(v);
+}
+
+void QGraphPainter::setYReference(double y){
+  m_useYReference = true;
+        reference=y;
+}
+
+void QGraphPainter::disableYReference(){
+  m_useYReference = false;
+}
+
+
+void QGraphPainter::setTitle(const char* t){
+	title=t;
+}
+
+void QGraphPainter::setRange(double _min, double _max){
+	min=_min;
+	max=_max;
+}
+
+void QGraphPainter::setAutoscale(bool a) {
+	autoscale=a;
+}
+
+bool QGraphPainter::getAutoscale() const {
+	return autoscale;
+}
+
+void QGraphPainter::timerEvent(QTimerEvent * te) {
+        if (te->timerId()==timer) 
+		update();
+}
+
+void QGraphPainter::start(int period){
+	timer=startTimer(period);
+}
+
+
+
+void QGraphPainter::paintEvent ( QPaintEvent * ){
+	m_pixmap->fill(Qt::white);
+	QPainter painter(m_pixmap);
+	double _min=MAXDOUBLE, _max=-MAXDOUBLE;
+	if (autoscale){
+		for (unsigned int i=0; i<(unsigned int)width() && i<values.size(); i++){
+			_min=_min<values[i]?_min:values[i];
+			_max=_max>values[i]?_max:values[i];
+		}
+	} else {
+		_min=min;
+		_max=max;
+	}
+	
+
+	painter.setPen(Qt::black);
+	painter.drawRect(0, 0, width(), height());
+	const int boundary=2;
+	int xoffset=40;
+	double scale=((double)height()-2*boundary-2)/(_max-_min);
+
+	if (m_useYReference) {
+	  painter.setPen(Qt::green);
+	  painter.drawLine(xoffset+boundary/2, height()-(int)(scale*(reference-_min)), 
+			   width()-boundary/2, height()-(int)(scale*(reference-_min)));
+	}
+	painter.setPen(Qt::blue);
+	unsigned int start=0;
+	if (values.size()>(unsigned int)width()-2*boundary-xoffset)
+		start=values.size()-width()+2*boundary+xoffset;
+	int oldv=0;
+	if ((unsigned int)width()-2*boundary-xoffset>1 && values.size()>1)
+	  oldv = (int)(scale*(values[1+start]-_min)) + boundary;
+	
+	for (unsigned int i=1; i<(unsigned int)width()-2*boundary-xoffset && i<values.size(); i++){
+		int v=(int)(scale*(values[i+start]-_min)) + boundary;
+		painter.drawLine(i-1+boundary+xoffset, height()-boundary-oldv, 
+				 xoffset+i+boundary, height()-boundary-v);
+		oldv=v;
+	}
+	painter.setPen(Qt::black);
+	painter.drawText( 3, height()/2, title);
+	QFont sansFont( "Helvetica [Cronyx]", 6);	
+	painter.setFont(sansFont);
+	bitBlt(this,0,0,m_pixmap,0,0,m_pixmap->width(),m_pixmap->height(),CopyROP);
+}
+

slam_gmapping/openslam_gmapping/gui/qgraphpainter.h

@@ -0,0 +1,67 @@
+/*****************************************************************
+ *
+ * This file is part of the GMAPPING project
+ *
+ * GMAPPING Copyright (c) 2004 Giorgio Grisetti, 
+ * Cyrill Stachniss, and Wolfram Burgard
+ *
+ * This software is licensed under the "Creative Commons 
+ * License (Attribution-NonCommercial-ShareAlike 2.0)" 
+ * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss, 
+ * and Wolfram Burgard.
+ * 
+ * Further information on this license can be found at:
+ * http://creativecommons.org/licenses/by-nc-sa/2.0/
+ * 
+ * GMAPPING is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied 
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
+ * PURPOSE.  
+ *
+ *****************************************************************/
+
+
+#ifndef QGRAPHPAINTER_H
+#define QGRAPHPAINTER_H
+
+#include <qpainter.h>
+#include <qpixmap.h>
+#include <qstring.h>
+#include <qwidget.h>
+#include <qmatrix.h>
+#include <deque>
+#include <utils/gvalues.h>
+
+typedef std::deque<double> DoubleDeque;
+
+class QGraphPainter :  public QWidget{
+	Q_OBJECT
+	public:
+		QGraphPainter( QWidget * parent = 0, const char * name = 0, Qt::WindowFlags f = 0);
+		virtual ~QGraphPainter();
+	public slots:
+		void clear();
+		void valueAdded(double);
+		void valueAdded(double, double, double);
+		void setYReference(double y);
+		void disableYReference();
+		void setRange(double min, double max);
+		void start(int period);
+		void setTitle(const char* title);
+		void setAutoscale(bool a);
+		bool getAutoscale() const;
+	protected:
+		virtual void timerEvent(QTimerEvent * te);
+		virtual void resizeEvent(QResizeEvent *);
+		double min, max, reference;
+		DoubleDeque values;
+		bool autoscale;
+		bool m_useYReference;
+		int timer;
+		virtual void paintEvent ( QPaintEvent *paintevent );
+		QPixmap * m_pixmap;
+		QString title;
+};
+
+#endif
+

slam_gmapping/openslam_gmapping/gui/qmappainter.cpp

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

slam_gmapping/openslam_gmapping/gui/qmappainter.h

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

slam_gmapping/openslam_gmapping/gui/qnavigatorwidget.cpp

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

slam_gmapping/openslam_gmapping/gui/qnavigatorwidget.h

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

slam_gmapping/openslam_gmapping/gui/qparticleviewer.cpp

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

slam_gmapping/openslam_gmapping/gui/qparticleviewer.h

@@ -0,0 +1,161 @@
+/*****************************************************************
+ *
+ * This file is part of the GMAPPING project
+ *
+ * GMAPPING Copyright (c) 2004 Giorgio Grisetti, 
+ * Cyrill Stachniss, and Wolfram Burgard
+ *
+ * This software is licensed under the "Creative Commons 
+ * License (Attribution-NonCommercial-ShareAlike 2.0)" 
+ * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss, 
+ * and Wolfram Burgard.
+ * 
+ * Further information on this license can be found at:
+ * http://creativecommons.org/licenses/by-nc-sa/2.0/
+ * 
+ * GMAPPING is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied 
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
+ * PURPOSE.  
+ *
+ *****************************************************************/
+
+
+#ifndef QPARTICLEVIEWER_H
+#define QPARTICLEVIEWER_H
+
+#include <qpainter.h>
+#include <qpixmap.h>
+#include <qwidget.h>
+#include <qmatrix.h>
+#include <qtextstream.h>
+#include <vector>
+#include <assert.h>
+#include <sstream>
+#include <iostream>
+#include <qimage.h>
+#include <QTextStream>
+
+#include <utils/point.h>
+#include "gsp_thread.h"
+
+namespace GMapping {
+
+class QParticleViewer :  public QWidget{
+	Q_OBJECT
+	public:
+		struct StartParameters{
+			//motionmodel
+			double srr, srt, str, stt;
+			//map
+			double xmin, ymin, xmax, ymax, delta;
+			OrientedPoint initialPose;
+			//likelihood
+			double lsigma, lgain;
+			unsigned int lskip;
+			//update
+			double linearUpdate, angularUpdate;
+			//filter
+			unsigned int particles;
+			double resampleThreshold;
+			//mode
+			bool drawFromObservation;
+			//output
+			const char * outFileName;
+		};
+		
+		struct MatchingParameters{
+			//ranges
+			double maxrange, urange;
+			//score
+			double ssigma, sreg, scrit;
+			unsigned int ksize;
+			//search
+			double lstep, astep;
+			unsigned int iterations;
+		};
+
+		void refreshParameters(); //reads the parameters from the thread
+		inline void setGSP( GridSlamProcessorThread* thread){gfs_thread=thread;}
+		
+		
+		typedef std::vector<OrientedPoint> OrientedPointVector;
+		QParticleViewer( QWidget * parent = 0, const char * name = 0, Qt::WindowFlags f = 0, GridSlamProcessorThread* thread=0 );
+		virtual ~QParticleViewer();
+		virtual void timerEvent(QTimerEvent * te);
+		virtual void resizeEvent(QResizeEvent *);
+		
+		void drawFromFile();
+		void drawFromMemory();
+		void drawMap(const ScanMatcherMap& map);
+		void start(int period);
+		QTextStream* tis;
+		
+		MatchingParameters matchingParameters;
+		StartParameters startParameters;
+		
+		int writeToFile;
+	public slots:
+		void setMatchingParameters(const MatchingParameters& mp);
+		void setStartParameters(const StartParameters& mp);
+		void start();
+		void stop();
+		void loadFile(const char *);
+	signals:
+		void neffChanged(double);
+		void poseEntropyChanged(double, double, double);
+		void trajectoryEntropyChanged(double, double, double);
+		void mapsEntropyChanged(double);
+		void mapsIGainChanged(double);
+		
+	protected:
+		ifstream inputStream;
+		ofstream outputStream;
+		
+			
+	protected:
+		inline Point pic2map(const IntPoint& p) 
+			{return viewCenter+Point(p.x/mapscale, -p.y/mapscale); }
+		inline IntPoint map2pic(const Point& p)
+			{return IntPoint((int)((p.x-viewCenter.x)*mapscale),(int)((viewCenter.y-p.y)*mapscale)); }
+		
+		int timer;
+		virtual void paintEvent ( QPaintEvent *paintevent );
+		void drawParticleMove(const OrientedPointVector& start, const OrientedPointVector& end); 
+		QPixmap* m_pixmap;
+		
+		//thread interaction
+		GridSlamProcessorThread* gfs_thread;
+		GridSlamProcessorThread::EventDeque history;
+		
+		//mouse movement
+		virtual void mousePressEvent(QMouseEvent*);
+		virtual void mouseReleaseEvent(QMouseEvent*);
+		virtual void mouseMoveEvent(QMouseEvent*);
+		QPoint draggingPos;
+		bool dragging;
+		
+		//particle plotting
+		virtual void keyPressEvent ( QKeyEvent* e );
+		
+		//map painting
+		double mapscale;
+		Point viewCenter;
+		Point bestParticlePose;
+		ScanMatcherMap * bestMap;
+		
+		// view mode
+		bool showPaths;
+		bool showBestPath;
+		
+		// file plotting
+		QParticleViewer::OrientedPointVector m_oldPose, m_newPose;
+		unsigned int m_particleSize;
+		bool m_refresh;
+		int count;
+};
+
+};
+
+#endif
+

slam_gmapping/openslam_gmapping/gui/qpixmapdumper.cpp

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

slam_gmapping/openslam_gmapping/gui/qpixmapdumper.h

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

slam_gmapping/openslam_gmapping/gui/qslamandnavwidget.cpp

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

slam_gmapping/openslam_gmapping/gui/qslamandnavwidget.h

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

slam_gmapping/openslam_gmapping/include/gmapping/grid/accessstate.h

@@ -0,0 +1,9 @@
+#ifndef ACCESSTATE_H
+#define ACCESSTATE_H
+
+namespace GMapping {
+enum AccessibilityState{Outside=0x0, Inside=0x1, Allocated=0x2};
+};
+
+#endif
+

slam_gmapping/openslam_gmapping/include/gmapping/grid/array2d.h

@@ -0,0 +1,188 @@
+#ifndef ARRAY2D_H
+#define ARRAY2D_H
+
+#include <assert.h>
+#include <gmapping/utils/point.h>
+#include "accessstate.h"
+
+#include <iostream>
+
+#ifndef __PRETTY_FUNCTION__
+#define __FUNCDNAME__
+#endif
+
+namespace GMapping {
+
+template<class Cell, const bool debug=false> class Array2D{
+	public:
+		Array2D(int xsize=0, int ysize=0);
+		Array2D& operator=(const Array2D &);
+		Array2D(const Array2D<Cell,debug> &);
+		~Array2D();
+		void clear();
+		void resize(int xmin, int ymin, int xmax, int ymax);
+		
+		
+		inline bool isInside(int x, int y) const;
+		inline const Cell& cell(int x, int y) const;
+		inline Cell& cell(int x, int y);
+		inline AccessibilityState cellState(int x, int y) const { return (AccessibilityState) (isInside(x,y)?(Inside|Allocated):Outside);}
+		
+		inline bool isInside(const IntPoint& p) const { return isInside(p.x, p.y);}
+		inline const Cell& cell(const IntPoint& p) const {return cell(p.x,p.y);}
+		inline Cell& cell(const IntPoint& p) {return cell(p.x,p.y);}
+		inline AccessibilityState cellState(const IntPoint& p) const { return cellState(p.x, p.y);}
+		
+		inline int getPatchSize() const{return 0;}
+		inline int getPatchMagnitude() const{return 0;}
+		inline int getXSize() const {return m_xsize;}
+		inline int getYSize() const {return m_ysize;}
+		inline Cell** cells() {return m_cells;}
+		Cell ** m_cells;
+	protected:
+		int m_xsize, m_ysize;
+};
+
+
+template <class Cell, const bool debug>
+Array2D<Cell,debug>::Array2D(int xsize, int ysize){
+//	assert(xsize>0);
+//	assert(ysize>0);
+	m_xsize=xsize;
+	m_ysize=ysize;
+	if (m_xsize>0 && m_ysize>0){
+		m_cells=new Cell*[m_xsize];
+		for (int i=0; i<m_xsize; i++)
+			m_cells[i]=new Cell[m_ysize];
+	}
+	else{
+		m_xsize=m_ysize=0;
+		m_cells=0;
+	}
+	if (debug){
+		std::cerr << __PRETTY_FUNCTION__ << std::endl;
+		std::cerr << "m_xsize= " << m_xsize<< std::endl;
+		std::cerr << "m_ysize= " << m_ysize<< std::endl;
+	}
+}
+
+template <class Cell, const bool debug>
+Array2D<Cell,debug> & Array2D<Cell,debug>::operator=(const Array2D<Cell,debug> & g){
+	if (debug || m_xsize!=g.m_xsize || m_ysize!=g.m_ysize){
+		for (int i=0; i<m_xsize; i++)
+			delete [] m_cells[i];
+		delete [] m_cells;
+		m_xsize=g.m_xsize;
+		m_ysize=g.m_ysize;
+		m_cells=new Cell*[m_xsize];
+		for (int i=0; i<m_xsize; i++)
+			m_cells[i]=new Cell[m_ysize];
+	}
+	for (int x=0; x<m_xsize; x++)
+		for (int y=0; y<m_ysize; y++)
+			m_cells[x][y]=g.m_cells[x][y];
+	
+	if (debug){
+		std::cerr << __PRETTY_FUNCTION__ << std::endl;
+		std::cerr << "m_xsize= " << m_xsize<< std::endl;
+		std::cerr << "m_ysize= " << m_ysize<< std::endl;
+	}
+	return *this;
+}
+
+template <class Cell, const bool debug>
+Array2D<Cell,debug>::Array2D(const Array2D<Cell,debug> & g){
+	m_xsize=g.m_xsize;
+	m_ysize=g.m_ysize;
+	m_cells=new Cell*[m_xsize];
+	for (int x=0; x<m_xsize; x++){
+		m_cells[x]=new Cell[m_ysize];
+		for (int y=0; y<m_ysize; y++)
+			m_cells[x][y]=g.m_cells[x][y];
+	}
+	if (debug){
+		std::cerr << __PRETTY_FUNCTION__ << std::endl;
+		std::cerr << "m_xsize= " << m_xsize<< std::endl;
+		std::cerr << "m_ysize= " << m_ysize<< std::endl;
+	}
+}
+
+template <class Cell, const bool debug>
+Array2D<Cell,debug>::~Array2D(){
+  if (debug){
+  	std::cerr << __PRETTY_FUNCTION__ << std::endl;
+	std::cerr << "m_xsize= " << m_xsize<< std::endl;
+	std::cerr << "m_ysize= " << m_ysize<< std::endl;
+  }
+  for (int i=0; i<m_xsize; i++){
+    delete [] m_cells[i];
+    m_cells[i]=0;
+  }
+  delete [] m_cells;
+  m_cells=0;
+}
+
+template <class Cell, const bool debug>
+void Array2D<Cell,debug>::clear(){
+  if (debug){
+  	std::cerr << __PRETTY_FUNCTION__ << std::endl;
+	std::cerr << "m_xsize= " << m_xsize<< std::endl;
+	std::cerr << "m_ysize= " << m_ysize<< std::endl;
+  }
+  for (int i=0; i<m_xsize; i++){
+    delete [] m_cells[i];
+    m_cells[i]=0;
+  }
+  delete [] m_cells;
+  m_cells=0;
+  m_xsize=0;
+  m_ysize=0;
+}
+
+
+template <class Cell, const bool debug>
+void Array2D<Cell,debug>::resize(int xmin, int ymin, int xmax, int ymax){
+	int xsize=xmax-xmin;
+	int ysize=ymax-ymin;
+	Cell ** newcells=new Cell *[xsize];
+	for (int x=0; x<xsize; x++){
+		newcells[x]=new Cell[ysize];
+	}
+	int dx= xmin < 0 ? 0 : xmin;
+	int dy= ymin < 0 ? 0 : ymin;
+	int Dx=xmax<this->m_xsize?xmax:this->m_xsize;
+	int Dy=ymax<this->m_ysize?ymax:this->m_ysize;
+	for (int x=dx; x<Dx; x++){
+		for (int y=dy; y<Dy; y++){
+			newcells[x-xmin][y-ymin]=this->m_cells[x][y];
+		}
+		delete [] this->m_cells[x];
+	}
+	delete [] this->m_cells;
+	this->m_cells=newcells;
+	this->m_xsize=xsize;
+	this->m_ysize=ysize; 
+}
+
+template <class Cell, const bool debug>
+inline bool Array2D<Cell,debug>::isInside(int x, int y) const{
+	return x>=0 && y>=0 && x<m_xsize && y<m_ysize; 
+}
+
+template <class Cell, const bool debug>
+inline const Cell& Array2D<Cell,debug>::cell(int x, int y) const{
+	assert(isInside(x,y));
+	return m_cells[x][y];
+}
+
+
+template <class Cell, const bool debug>
+inline Cell& Array2D<Cell,debug>::cell(int x, int y){
+	assert(isInside(x,y));
+	return m_cells[x][y];
+}
+
+};
+
+#endif
+

slam_gmapping/openslam_gmapping/include/gmapping/grid/harray2d.h

@@ -0,0 +1,200 @@
+#ifndef HARRAY2D_H
+#define HARRAY2D_H
+#include <set>
+#include <gmapping/utils/point.h>
+#include <gmapping/utils/autoptr.h>
+#include "array2d.h"
+
+namespace GMapping {
+
+template <class Cell>
+class HierarchicalArray2D: public Array2D<autoptr< Array2D<Cell> > >{
+	public:
+		typedef std::set< point<int>, pointcomparator<int> > PointSet;
+		HierarchicalArray2D(int xsize, int ysize, int patchMagnitude=5);
+		HierarchicalArray2D(const HierarchicalArray2D& hg);
+		HierarchicalArray2D& operator=(const HierarchicalArray2D& hg);
+		virtual ~HierarchicalArray2D(){}
+		void resize(int ixmin, int iymin, int ixmax, int iymax);
+		inline int getPatchSize() const {return m_patchMagnitude;}
+		inline int getPatchMagnitude() const {return m_patchMagnitude;}
+		
+		inline const Cell& cell(int x, int y) const;
+		inline Cell& cell(int x, int y);
+		inline bool isAllocated(int x, int y) const;
+		inline AccessibilityState cellState(int x, int y) const ;
+		inline IntPoint patchIndexes(int x, int y) const;
+		
+		inline const Cell& cell(const IntPoint& p) const { return cell(p.x,p.y); }
+		inline Cell& cell(const IntPoint& p) { return cell(p.x,p.y); }
+		inline bool isAllocated(const IntPoint& p) const { return isAllocated(p.x,p.y);}
+		inline AccessibilityState cellState(const IntPoint& p) const { return cellState(p.x,p.y); }
+		inline IntPoint patchIndexes(const IntPoint& p) const { return patchIndexes(p.x,p.y);}
+		
+		inline void setActiveArea(const PointSet&, bool patchCoords=false);
+		const PointSet& getActiveArea() const {return m_activeArea; }
+		inline void allocActiveArea();
+	protected:
+		virtual Array2D<Cell> * createPatch(const IntPoint& p) const;
+		PointSet m_activeArea;
+		int m_patchMagnitude;
+		int m_patchSize;
+};
+
+template <class Cell>
+HierarchicalArray2D<Cell>::HierarchicalArray2D(int xsize, int ysize, int patchMagnitude) 
+  :Array2D<autoptr< Array2D<Cell> > >::Array2D((xsize>>patchMagnitude), (ysize>>patchMagnitude)){
+	m_patchMagnitude=patchMagnitude;
+	m_patchSize=1<<m_patchMagnitude;
+}
+
+template <class Cell>
+HierarchicalArray2D<Cell>::HierarchicalArray2D(const HierarchicalArray2D& hg)
+  :Array2D<autoptr< Array2D<Cell> > >::Array2D((hg.m_xsize>>hg.m_patchMagnitude), (hg.m_ysize>>hg.m_patchMagnitude))  // added by cyrill: if you have a resize error, check this again
+{
+	this->m_xsize=hg.m_xsize;
+	this->m_ysize=hg.m_ysize;
+	this->m_cells=new autoptr< Array2D<Cell> >*[this->m_xsize];
+	for (int x=0; x<this->m_xsize; x++){
+		this->m_cells[x]=new autoptr< Array2D<Cell> >[this->m_ysize];
+		for (int y=0; y<this->m_ysize; y++)
+			this->m_cells[x][y]=hg.m_cells[x][y];
+	}
+	this->m_patchMagnitude=hg.m_patchMagnitude;
+	this->m_patchSize=hg.m_patchSize;
+}
+
+template <class Cell>
+void HierarchicalArray2D<Cell>::resize(int xmin, int ymin, int xmax, int ymax){
+	int xsize=xmax-xmin;
+	int ysize=ymax-ymin;
+	autoptr< Array2D<Cell> > ** newcells=new autoptr< Array2D<Cell> > *[xsize];
+	for (int x=0; x<xsize; x++){
+		newcells[x]=new autoptr< Array2D<Cell> >[ysize];
+		for (int y=0; y<ysize; y++){
+			newcells[x][y]=autoptr< Array2D<Cell> >(0);
+		}
+	}
+	int dx= xmin < 0 ? 0 : xmin;
+	int dy= ymin < 0 ? 0 : ymin;
+	int Dx=xmax<this->m_xsize?xmax:this->m_xsize;
+	int Dy=ymax<this->m_ysize?ymax:this->m_ysize;
+	for (int x=dx; x<Dx; x++){
+		for (int y=dy; y<Dy; y++){
+			newcells[x-xmin][y-ymin]=this->m_cells[x][y];
+		}
+		delete [] this->m_cells[x];
+	}
+	delete [] this->m_cells;
+	this->m_cells=newcells;
+	this->m_xsize=xsize;
+	this->m_ysize=ysize; 
+}
+
+template <class Cell>
+HierarchicalArray2D<Cell>& HierarchicalArray2D<Cell>::operator=(const HierarchicalArray2D& hg){
+//	Array2D<autoptr< Array2D<Cell> > >::operator=(hg);
+	if (this->m_xsize!=hg.m_xsize || this->m_ysize!=hg.m_ysize){
+		for (int i=0; i<this->m_xsize; i++)
+			delete [] this->m_cells[i];
+		delete [] this->m_cells;
+		this->m_xsize=hg.m_xsize;
+		this->m_ysize=hg.m_ysize;
+		this->m_cells=new autoptr< Array2D<Cell> >*[this->m_xsize];
+		for (int i=0; i<this->m_xsize; i++)
+			this->m_cells[i]=new autoptr< Array2D<Cell> > [this->m_ysize];
+	}
+	for (int x=0; x<this->m_xsize; x++)
+		for (int y=0; y<this->m_ysize; y++)
+			this->m_cells[x][y]=hg.m_cells[x][y];
+	
+	m_activeArea.clear();
+	m_patchMagnitude=hg.m_patchMagnitude;
+	m_patchSize=hg.m_patchSize;
+	return *this;
+}
+
+
+template <class Cell>
+void HierarchicalArray2D<Cell>::setActiveArea(const typename HierarchicalArray2D<Cell>::PointSet& aa, bool patchCoords){
+	m_activeArea.clear();
+	for (PointSet::const_iterator it= aa.begin(); it!=aa.end(); ++it) {
+		IntPoint p;
+		if (patchCoords)
+			p=*it;
+		else
+			p=patchIndexes(*it);
+		m_activeArea.insert(p);
+	}
+}
+
+template <class Cell>
+Array2D<Cell>* HierarchicalArray2D<Cell>::createPatch(const IntPoint& ) const{
+	return new Array2D<Cell>(1<<m_patchMagnitude, 1<<m_patchMagnitude);
+}
+
+
+template <class Cell>
+AccessibilityState  HierarchicalArray2D<Cell>::cellState(int x, int y) const {
+	if (this->isInside(patchIndexes(x,y))) {
+		if(isAllocated(x,y))
+			return (AccessibilityState)((int)Inside|(int)Allocated);
+		else
+			return Inside;
+	}
+	return Outside;
+}
+
+template <class Cell>
+void HierarchicalArray2D<Cell>::allocActiveArea(){
+	for (PointSet::const_iterator it= m_activeArea.begin(); it!=m_activeArea.end(); ++it){
+		const autoptr< Array2D<Cell> >& ptr=this->m_cells[it->x][it->y];
+		Array2D<Cell>* patch=0;
+		if (!ptr){
+			patch=createPatch(*it);
+		} else{	
+			patch=new Array2D<Cell>(*ptr);
+		}
+		this->m_cells[it->x][it->y]=autoptr< Array2D<Cell> >(patch);
+	}
+}
+
+template <class Cell>
+bool HierarchicalArray2D<Cell>::isAllocated(int x, int y) const{
+	IntPoint c=patchIndexes(x,y);
+	autoptr< Array2D<Cell> >& ptr=this->m_cells[c.x][c.y];
+	return (ptr != 0);
+}
+
+template <class Cell>
+IntPoint HierarchicalArray2D<Cell>::patchIndexes(int x, int y) const{
+	if (x>=0 && y>=0)
+		return IntPoint(x>>m_patchMagnitude, y>>m_patchMagnitude);
+	return IntPoint(-1, -1);
+}
+
+template <class Cell>
+Cell& HierarchicalArray2D<Cell>::cell(int x, int y){
+	IntPoint c=patchIndexes(x,y);
+	assert(this->isInside(c.x, c.y));
+	if (!this->m_cells[c.x][c.y]){
+		Array2D<Cell>* patch=createPatch(IntPoint(x,y));
+		this->m_cells[c.x][c.y]=autoptr< Array2D<Cell> >(patch);
+		//cerr << "!!! FATAL: your dick is going to fall down" << endl;
+	}
+	autoptr< Array2D<Cell> >& ptr=this->m_cells[c.x][c.y];
+	return (*ptr).cell(IntPoint(x-(c.x<<m_patchMagnitude),y-(c.y<<m_patchMagnitude)));
+}
+
+template <class Cell>
+const Cell& HierarchicalArray2D<Cell>::cell(int x, int y) const{
+	assert(isAllocated(x,y));
+	IntPoint c=patchIndexes(x,y);
+	const autoptr< Array2D<Cell> >& ptr=this->m_cells[c.x][c.y];
+	return (*ptr).cell(IntPoint(x-(c.x<<m_patchMagnitude),y-(c.y<<m_patchMagnitude)));
+}
+
+};
+
+#endif
+

slam_gmapping/openslam_gmapping/include/gmapping/grid/map.h

@@ -0,0 +1,265 @@
+#ifndef MAP_H
+#define MAP_H
+#include <gmapping/utils/point.h>
+#include <assert.h>
+#include "accessstate.h"
+#include "array2d.h"
+
+namespace GMapping {
+/**
+The cells have to define the special value Cell::Unknown to handle with the unallocated areas.
+The cells have to define (int) constructor;
+*/
+typedef Array2D<double> DoubleArray2D;
+
+template <class Cell, class Storage, const bool isClass=true> 
+class Map{
+	public:
+		Map(int mapSizeX, int mapSizeY, double delta);
+		Map(const Point& center, double worldSizeX, double worldSizeY, double delta);
+		Map(const Point& center, double xmin, double ymin, double xmax, double ymax, double delta);
+		/* the standard implementation works filen in this case*/
+		//Map(const Map& g);
+		//Map& operator =(const Map& g);
+		void resize(double xmin, double ymin, double xmax, double ymax);
+		void grow(double xmin, double ymin, double xmax, double ymax);
+		inline IntPoint world2map(const Point& p) const;
+		inline Point map2world(const IntPoint& p) const;
+		inline IntPoint world2map(double x, double y) const 
+		  { return world2map(Point(x,y)); }
+		inline Point map2world(int x, int y) const 
+		  { return map2world(IntPoint(x,y)); }
+
+		inline Point getCenter() const {return m_center;}	
+		inline double getWorldSizeX() const {return m_worldSizeX;}
+		inline double getWorldSizeY() const {return m_worldSizeY;}
+		inline int getMapSizeX() const {return m_mapSizeX;}
+		inline int getMapSizeY() const {return m_mapSizeY;}
+		inline double getDelta() const { return m_delta;}
+		inline double getMapResolution() const { return m_delta;}
+		inline double getResolution() const { return m_delta;}
+		inline void getSize(double & xmin, double& ymin, double& xmax, double& ymax) const {
+			Point min=map2world(0,0), max=map2world(IntPoint(m_mapSizeX-1, m_mapSizeY-1)); 
+			xmin=min.x, ymin=min.y,  xmax=max.x, ymax=max.y; 
+		}
+		
+		inline Cell& cell(int x, int y) {
+		  return cell(IntPoint(x, y));
+		}
+		inline Cell& cell(const IntPoint& p);
+
+		inline const Cell& cell(int x, int y) const  {
+		  return cell(IntPoint(x, y));
+		}
+		inline const Cell& cell(const IntPoint& p) const;
+
+		inline Cell& cell(double x, double y) {
+		  return cell(Point(x, y));
+		}
+		inline Cell& cell(const Point& p);
+
+		inline const Cell& cell(double x, double y) const {
+		  return cell(Point(x, y));
+		}
+
+		inline bool isInside(int x, int y) const {
+		  return m_storage.cellState(IntPoint(x,y))&Inside;
+		}
+		inline bool isInside(const IntPoint& p) const {
+		  return m_storage.cellState(p)&Inside;
+		}
+
+		inline bool isInside(double x, double y) const {
+		  return m_storage.cellState(world2map(x,y))&Inside;
+		}
+		inline bool isInside(const Point& p) const {
+		  return m_storage.cellState(world2map(p))&Inside;
+		}
+
+		inline const Cell& cell(const Point& p) const;
+
+		inline Storage& storage() { return m_storage; }
+		inline const Storage& storage() const { return m_storage; }
+		DoubleArray2D* toDoubleArray() const;
+	        Map<double, DoubleArray2D, false>* toDoubleMap() const;
+		
+	protected:
+		Point m_center;
+		double m_worldSizeX, m_worldSizeY, m_delta;
+		Storage m_storage;
+		int m_mapSizeX, m_mapSizeY;
+		int m_sizeX2, m_sizeY2;
+	static const Cell m_unknown;
+};
+
+typedef Map<double, DoubleArray2D, false> DoubleMap;
+
+template <class Cell, class Storage, const bool isClass>
+  const Cell  Map<Cell,Storage,isClass>::m_unknown = Cell(-1);
+
+template <class Cell, class Storage, const bool isClass>
+Map<Cell,Storage,isClass>::Map(int mapSizeX, int mapSizeY, double delta):
+	m_storage(mapSizeX, mapSizeY){
+	m_worldSizeX=mapSizeX * delta;
+	m_worldSizeY=mapSizeY * delta;
+	m_delta=delta;
+	m_center=Point(0.5*m_worldSizeX, 0.5*m_worldSizeY);
+	m_sizeX2=m_mapSizeX>>1;
+	m_sizeY2=m_mapSizeY>>1;
+}
+
+template <class Cell, class Storage, const bool isClass>
+Map<Cell,Storage,isClass>::Map(const Point& center, double worldSizeX, double worldSizeY, double delta):
+	m_storage((int)ceil(worldSizeX/delta), (int)ceil(worldSizeY/delta)){
+	m_center=center;
+	m_worldSizeX=worldSizeX;
+	m_worldSizeY=worldSizeY;
+	m_delta=delta;
+	m_mapSizeX=m_storage.getXSize()<<m_storage.getPatchSize();
+	m_mapSizeY=m_storage.getYSize()<<m_storage.getPatchSize();
+	m_sizeX2=m_mapSizeX>>1;
+	m_sizeY2=m_mapSizeY>>1;
+}
+
+template <class Cell, class Storage, const bool isClass>
+Map<Cell,Storage,isClass>::Map(const Point& center, double xmin, double ymin, double xmax, double ymax, double delta):
+	m_storage((int)ceil((xmax-xmin)/delta), (int)ceil((ymax-ymin)/delta)){
+	m_center=center;
+	m_worldSizeX=xmax-xmin;
+	m_worldSizeY=ymax-ymin;
+	m_delta=delta;
+	m_mapSizeX=m_storage.getXSize()<<m_storage.getPatchSize();
+	m_mapSizeY=m_storage.getYSize()<<m_storage.getPatchSize();
+	m_sizeX2=(int)round((m_center.x-xmin)/m_delta);
+	m_sizeY2=(int)round((m_center.y-ymin)/m_delta);
+}
+		
+template <class Cell, class Storage, const bool isClass>
+void Map<Cell,Storage,isClass>::resize(double xmin, double ymin, double xmax, double ymax){
+	IntPoint imin=world2map(xmin, ymin);
+	IntPoint imax=world2map(xmax, ymax);
+	int pxmin, pymin, pxmax, pymax;
+	pxmin=(int)floor((float)imin.x/(1<<m_storage.getPatchMagnitude()));
+	pxmax=(int)ceil((float)imax.x/(1<<m_storage.getPatchMagnitude()));
+	pymin=(int)floor((float)imin.y/(1<<m_storage.getPatchMagnitude()));
+	pymax=(int)ceil((float)imax.y/(1<<m_storage.getPatchMagnitude()));
+	m_storage.resize(pxmin, pymin, pxmax, pymax);
+	m_mapSizeX=m_storage.getXSize()<<m_storage.getPatchSize();
+	m_mapSizeY=m_storage.getYSize()<<m_storage.getPatchSize();
+	m_worldSizeX=xmax-xmin;
+	m_worldSizeY=ymax-ymin;
+	m_sizeX2-=pxmin*(1<<m_storage.getPatchMagnitude());
+	m_sizeY2-=pymin*(1<<m_storage.getPatchMagnitude());
+}
+
+template <class Cell, class Storage, const bool isClass>
+void Map<Cell,Storage,isClass>::grow(double xmin, double ymin, double xmax, double ymax){
+	IntPoint imin=world2map(xmin, ymin);
+	IntPoint imax=world2map(xmax, ymax);
+	if (isInside(imin) && isInside(imax))
+		return;
+	imin=min(imin, IntPoint(0,0));
+	imax=max(imax, IntPoint(m_mapSizeX-1,m_mapSizeY-1));
+	int pxmin, pymin, pxmax, pymax;
+	pxmin=(int)floor((float)imin.x/(1<<m_storage.getPatchMagnitude()));
+	pxmax=(int)ceil((float)imax.x/(1<<m_storage.getPatchMagnitude()));
+	pymin=(int)floor((float)imin.y/(1<<m_storage.getPatchMagnitude()));
+	pymax=(int)ceil((float)imax.y/(1<<m_storage.getPatchMagnitude()));
+	m_storage.resize(pxmin, pymin, pxmax, pymax);
+	m_mapSizeX=m_storage.getXSize()<<m_storage.getPatchSize();
+	m_mapSizeY=m_storage.getYSize()<<m_storage.getPatchSize();
+	m_worldSizeX=xmax-xmin;
+	m_worldSizeY=ymax-ymin;
+	m_sizeX2-=pxmin*(1<<m_storage.getPatchMagnitude());
+	m_sizeY2-=pymin*(1<<m_storage.getPatchMagnitude());
+}
+
+
+template <class Cell, class Storage, const bool isClass>
+IntPoint Map<Cell,Storage,isClass>::world2map(const Point& p) const{
+	return IntPoint( (int)round((p.x-m_center.x)/m_delta)+m_sizeX2, (int)round((p.y-m_center.y)/m_delta)+m_sizeY2);
+}
+
+template <class Cell, class Storage, const bool isClass>
+Point Map<Cell,Storage,isClass>::map2world(const IntPoint& p) const{
+	return Point( (p.x-m_sizeX2)*m_delta,
+		      (p.y-m_sizeY2)*m_delta)+m_center;
+}
+
+
+template <class Cell, class Storage, const bool isClass>
+Cell& Map<Cell,Storage,isClass>::cell(const IntPoint& p) {
+	AccessibilityState s=m_storage.cellState(p);
+	if (! (s&Inside))
+		assert(0);
+	//if (s&Allocated) return m_storage.cell(p); assert(0);
+
+	// this will never happend. Just to satify the compiler..
+	return m_storage.cell(p);
+
+}
+
+template <class Cell, class Storage, const bool isClass>
+Cell& Map<Cell,Storage,isClass>::cell(const Point& p) {
+	IntPoint ip=world2map(p);
+	AccessibilityState s=m_storage.cellState(ip);
+	if (! (s&Inside))
+		assert(0);
+	//if (s&Allocated) return m_storage.cell(ip); assert(0);
+
+	// this will never happend. Just to satify the compiler..
+	return m_storage.cell(ip);
+}
+
+template <class Cell, class Storage, const bool isClass>
+  const Cell& Map<Cell,Storage,isClass>::cell(const IntPoint& p) const {
+  AccessibilityState s=m_storage.cellState(p);
+  //if (! s&Inside) assert(0);
+  if (s&Allocated)	 
+    return m_storage.cell(p);
+  return m_unknown;
+}
+
+template <class Cell, class Storage, const bool isClass>
+const  Cell& Map<Cell,Storage,isClass>::cell(const Point& p) const {
+  IntPoint ip=world2map(p);
+  AccessibilityState s=m_storage.cellState(ip);
+  //if (! s&Inside) assert(0);
+  if (s&Allocated)	 
+    return m_storage.cell(ip);
+  return  m_unknown;
+}
+
+
+//FIXME check why the last line of the map is corrupted.
+template <class Cell, class Storage, const bool isClass>
+DoubleArray2D* Map<Cell,Storage,isClass>::toDoubleArray() const{
+        DoubleArray2D* darr=new DoubleArray2D(getMapSizeX()-1, getMapSizeY()-1);
+	for(int x=0; x<getMapSizeX()-1; x++)
+		for(int y=0; y<getMapSizeY()-1; y++){
+			IntPoint p(x,y);
+			darr->cell(p)=cell(p);
+		}
+	return darr;
+}
+
+
+template <class Cell, class Storage, const bool isClass>
+Map<double, DoubleArray2D, false>* Map<Cell,Storage,isClass>::toDoubleMap() const{
+//FIXME size the map so that m_center will be setted accordingly
+        Point pmin=map2world(IntPoint(0,0));
+	Point pmax=map2world(getMapSizeX()-1,getMapSizeY()-1);
+	Point center=(pmax+pmin)*0.5;
+	Map<double, DoubleArray2D, false>*  plainMap=new Map<double, DoubleArray2D, false>(center, (pmax-pmin).x, (pmax-pmin).y, getDelta());
+	for(int x=0; x<getMapSizeX()-1; x++)
+		for(int y=0; y<getMapSizeY()-1; y++){
+			IntPoint p(x,y);
+			plainMap->cell(p)=cell(p);
+		}
+	return plainMap;
+}
+
+};
+
+#endif
+

slam_gmapping/openslam_gmapping/include/gmapping/gridfastslam/gridslamprocessor.h

@@ -0,0 +1,337 @@
+#ifndef GRIDSLAMPROCESSOR_H
+#define GRIDSLAMPROCESSOR_H
+
+#include <climits>
+#include <limits>
+#include <fstream>
+#include <vector>
+#include <deque>
+#include <gmapping/particlefilter/particlefilter.h>
+#include <gmapping/utils/point.h>
+#include <gmapping/utils/macro_params.h>
+#include <gmapping/log/sensorlog.h>
+#include <gmapping/sensor/sensor_range/rangesensor.h>
+#include <gmapping/sensor/sensor_range/rangereading.h>
+#include <gmapping/scanmatcher/scanmatcher.h>
+#include "motionmodel.h"
+
+
+namespace GMapping {
+
+  /**This class defines the basic GridFastSLAM algorithm.  It
+     implements a rao blackwellized particle filter. Each particle
+     has its own map and robot pose.<br> This implementation works
+     as follows: each time a new pair odometry/laser reading is
+     received, the particle's robot pose is updated according to the
+     motion model.  This pose is subsequently used for initalizing a
+     scan matching algorithm.  The scanmatcher performs a local
+     optimization for each particle.  It is initialized with the
+     pose drawn from the motion model, and the pose is corrected
+     according to the each particle map.<br>
+     In order to avoid unnecessary computation the filter state is updated 
+     only when the robot moves more than a given threshold.
+  */
+  class GridSlamProcessor{
+  public:
+
+    
+    /**This class defines the the node of reversed tree in which the trajectories are stored.
+       Each node of a tree has a pointer to its parent and a counter indicating the number of childs of a node.
+       The tree is updated in a way consistent with the operation performed on the particles.
+   */
+    struct TNode{
+      /**Constructs a node of the trajectory tree.
+       @param pose:      the pose of the robot in the trajectory
+       @param weight:    the weight of the particle at that point in the trajectory
+       @param accWeight: the cumulative weight of the particle
+       @param parent:    the parent node in the tree
+       @param childs:    the number of childs
+      */
+      TNode(const OrientedPoint& pose, double weight, TNode* parent=0, unsigned int childs=0);
+
+      /**Destroys a tree node, and consistently updates the tree. If a node whose parent has only one child is deleted,
+       also the parent node is deleted. This because the parent will not be reacheable anymore in the trajectory tree.*/
+      ~TNode();
+
+      /**The pose of the robot*/
+      OrientedPoint pose; 
+      
+      /**The weight of the particle*/
+      double weight;
+
+      /**The sum of all the particle weights in the previous part of the trajectory*/
+      double accWeight;
+
+      double gweight;
+
+
+      /**The parent*/
+      TNode* parent;
+
+      /**The range reading to which this node is associated*/
+      const RangeReading* reading;
+
+      /**The number of childs*/
+      unsigned int childs;
+
+      /**counter in visiting the node (internally used)*/
+      mutable unsigned int visitCounter;
+
+      /**visit flag (internally used)*/
+      mutable bool flag;
+    };
+    
+    typedef std::vector<GridSlamProcessor::TNode*> TNodeVector;
+    typedef std::deque<GridSlamProcessor::TNode*> TNodeDeque;
+    
+    /**This class defines a particle of the filter. Each particle has a map, a pose, a weight and retains the current node in the trajectory tree*/
+    struct Particle{
+      /**constructs a particle, given a map
+	 @param map: the particle map
+      */
+      Particle(const ScanMatcherMap& map);
+
+      /** @returns the weight of a particle */
+      inline operator double() const {return weight;}
+      /** @returns the pose of a particle */
+      inline operator OrientedPoint() const {return pose;}
+      /** sets the weight of a particle
+	  @param w the weight
+      */
+      inline void setWeight(double w) {weight=w;}
+      /** The map */
+      ScanMatcherMap map;
+      /** The pose of the robot */
+      OrientedPoint pose;
+
+      /** The pose of the robot at the previous time frame (used for computing thr odometry displacements) */
+      OrientedPoint previousPose;
+
+      /** The weight of the particle */
+      double weight;
+
+      /** The cumulative weight of the particle */
+      double weightSum;
+
+      double gweight;
+
+      /** The index of the previous particle in the trajectory tree */
+      int previousIndex;
+
+      /** Entry to the trajectory tree */
+      TNode* node; 
+    };
+	
+    
+    typedef std::vector<Particle> ParticleVector;
+    
+    /** Constructs a GridSlamProcessor, initialized with the default parameters */
+    GridSlamProcessor();
+
+    /** Constructs a GridSlamProcessor, whose output is routed to a stream.
+     @param infoStr: the output stream
+    */
+    GridSlamProcessor(std::ostream& infoStr);
+    
+    /** @returns  a deep copy of the grid slam processor with all the internal structures.
+    */
+    GridSlamProcessor* clone() const;
+    
+    /**Deleted the gridslamprocessor*/
+    virtual ~GridSlamProcessor();
+    
+    //methods for accessing the parameters
+    void setSensorMap(const SensorMap& smap);
+    void init(unsigned int size, double xmin, double ymin, double xmax, double ymax, double delta, 
+	      OrientedPoint initialPose=OrientedPoint(0,0,0));
+    void setMatchingParameters(double urange, double range, double sigma, int kernsize, double lopt, double aopt, 
+			       int iterations, double likelihoodSigma=1, double likelihoodGain=1, unsigned int likelihoodSkip=0);
+    void setMotionModelParameters(double srr, double srt, double str, double stt);
+    void setUpdateDistances(double linear, double angular, double resampleThreshold);
+    void setUpdatePeriod(double p) {period_=p;}
+    
+    //the "core" algorithm
+    void processTruePos(const OdometryReading& odometry);
+    bool processScan(const RangeReading & reading, int adaptParticles=0);
+    
+    /**This method copies the state of the filter in a tree.
+     The tree is represented through reversed pointers (each node has a pointer to its parent).
+     The leafs are stored in a vector, whose size is the same as the number of particles.
+     @returns the leafs of the tree
+    */
+    TNodeVector getTrajectories() const;
+    void integrateScanSequence(TNode* node);
+    
+    /**the scanmatcher algorithm*/
+    ScanMatcher m_matcher;
+    /**the stream used for writing the output of the algorithm*/
+    std::ofstream& outputStream();
+    /**the stream used for writing the info/debug messages*/
+    std::ostream& infoStream();
+    /**@returns the particles*/
+    inline const ParticleVector& getParticles() const {return m_particles; }
+    
+    inline const std::vector<unsigned int>& getIndexes() const{return m_indexes; }
+    int getBestParticleIndex() const;
+    //callbacks
+    virtual void onOdometryUpdate();
+    virtual void onResampleUpdate();
+    virtual void onScanmatchUpdate();
+	
+    //accessor methods
+    /**the maxrange of the laser to consider */
+    MEMBER_PARAM_SET_GET(m_matcher, double, laserMaxRange, protected, public, public);
+
+    /**the maximum usable range of the laser. A beam is cropped to this value. [scanmatcher]*/
+    MEMBER_PARAM_SET_GET(m_matcher, double, usableRange, protected, public, public);
+
+    /**The sigma used by the greedy endpoint matching. [scanmatcher]*/
+    MEMBER_PARAM_SET_GET(m_matcher,double, gaussianSigma, protected, public, public);
+
+    /**The sigma  of a beam used for likelihood computation [scanmatcher]*/
+    MEMBER_PARAM_SET_GET(m_matcher,double, likelihoodSigma, protected, public, public);
+
+    /**The kernel in which to look for a correspondence[scanmatcher]*/
+    MEMBER_PARAM_SET_GET(m_matcher, int,    kernelSize, protected, public, public);
+
+    /**The optimization step in rotation [scanmatcher]*/
+    MEMBER_PARAM_SET_GET(m_matcher, double, optAngularDelta, protected, public, public);
+
+    /**The optimization step in translation [scanmatcher]*/
+    MEMBER_PARAM_SET_GET(m_matcher, double, optLinearDelta, protected, public, public);
+
+    /**The number of iterations of the scanmatcher [scanmatcher]*/
+    MEMBER_PARAM_SET_GET(m_matcher, unsigned int, optRecursiveIterations, protected, public, public);
+
+    /**the beams to skip for computing the likelihood (consider a beam every likelihoodSkip) [scanmatcher]*/
+    MEMBER_PARAM_SET_GET(m_matcher, unsigned int, likelihoodSkip, protected, public, public);
+
+    /**translational sampling range for the likelihood [scanmatcher]*/
+    MEMBER_PARAM_SET_GET(m_matcher, double, llsamplerange, protected, public, public);
+
+    /**angular sampling range for the likelihood [scanmatcher]*/
+    MEMBER_PARAM_SET_GET(m_matcher, double, lasamplerange, protected, public, public);
+
+    /**translational sampling range for the likelihood [scanmatcher]*/
+    MEMBER_PARAM_SET_GET(m_matcher, double, llsamplestep, protected, public, public);
+
+    /**angular sampling step for the likelihood [scanmatcher]*/
+    MEMBER_PARAM_SET_GET(m_matcher, double, lasamplestep, protected, public, public);
+
+    /**generate an accupancy grid map [scanmatcher]*/
+    MEMBER_PARAM_SET_GET(m_matcher, bool, generateMap, protected, public, public);
+
+    /**enlarge the map when the robot goes out of the boundaries [scanmatcher]*/
+    MEMBER_PARAM_SET_GET(m_matcher, bool, enlargeStep, protected, public, public);
+
+    /**pose of the laser wrt the robot [scanmatcher]*/
+    MEMBER_PARAM_SET_GET(m_matcher, OrientedPoint, laserPose, protected, public, public);
+
+
+    /**odometry error in translation as a function of translation (rho/rho) [motionmodel]*/
+    STRUCT_PARAM_SET_GET(m_motionModel, double, srr, protected, public, public);
+
+    /**odometry error in translation as a function of rotation (rho/theta) [motionmodel]*/
+    STRUCT_PARAM_SET_GET(m_motionModel, double, srt, protected, public, public);
+
+    /**odometry error in rotation as a function of translation (theta/rho) [motionmodel]*/
+    STRUCT_PARAM_SET_GET(m_motionModel, double, str, protected, public, public);
+
+    /**odometry error in  rotation as a function of rotation (theta/theta) [motionmodel]*/
+    STRUCT_PARAM_SET_GET(m_motionModel, double, stt, protected, public, public);
+		
+    /**minimum score for considering the outcome of the scanmatching good*/
+    PARAM_SET_GET(double, minimumScore, protected, public, public);
+
+  protected:
+    /**Copy constructor*/
+    GridSlamProcessor(const GridSlamProcessor& gsp);
+ 
+    /**the laser beams*/
+    unsigned int m_beams;
+    double last_update_time_;
+    double period_;
+    
+    /**the particles*/
+    ParticleVector m_particles;
+
+    /**the particle indexes after resampling (internally used)*/
+    std::vector<unsigned int> m_indexes;
+
+    /**the particle weights (internally used)*/
+    std::vector<double> m_weights;
+    
+    /**the motion model*/
+    MotionModel m_motionModel;
+
+    /**this sets the neff based resampling threshold*/
+    PARAM_SET_GET(double, resampleThreshold, protected, public, public);
+      
+    //state
+    int  m_count, m_readingCount;
+    OrientedPoint m_lastPartPose;
+    OrientedPoint m_odoPose;
+    OrientedPoint m_pose;
+    double m_linearDistance, m_angularDistance;
+    PARAM_GET(double, neff, protected, public);
+      
+    //processing parameters (size of the map)
+    PARAM_GET(double, xmin, protected, public);
+    PARAM_GET(double, ymin, protected, public);
+    PARAM_GET(double, xmax, protected, public);
+    PARAM_GET(double, ymax, protected, public);
+    //processing parameters (resolution of the map)
+    PARAM_GET(double, delta, protected, public);
+	
+    //registration score (if a scan score is above this threshold it is registered in the map)
+    PARAM_SET_GET(double, regScore, protected, public, public);
+    //registration score (if a scan score is below this threshold a scan matching failure is reported)
+    PARAM_SET_GET(double, critScore, protected, public, public);
+    //registration score maximum move allowed between consecutive scans
+    PARAM_SET_GET(double, maxMove, protected, public, public);
+	
+    //process a scan each time the robot translates of linearThresholdDistance
+    PARAM_SET_GET(double, linearThresholdDistance, protected, public, public);
+
+    //process a scan each time the robot rotates more than angularThresholdDistance
+    PARAM_SET_GET(double, angularThresholdDistance, protected, public, public);
+    
+    //smoothing factor for the likelihood
+    PARAM_SET_GET(double, obsSigmaGain, protected, public, public);
+	
+    //stream in which to write the gfs file
+    std::ofstream m_outputStream;
+
+    // stream in which to write the messages
+    std::ostream& m_infoStream;
+    
+    
+    // the functions below performs side effect on the internal structure,
+    //should be called only inside the processScan method
+  private:
+    
+    /**scanmatches all the particles*/
+    inline void scanMatch(const double *plainReading);
+    /**normalizes the particle weights*/
+    inline void normalize();
+    
+    // return if a resampling occured or not
+    inline bool resample(const double* plainReading, int adaptParticles, 
+			 const RangeReading* rr=0);
+    
+    //tree utilities
+    
+    void updateTreeWeights(bool weightsAlreadyNormalized = false);
+    void resetTree();
+    double propagateWeights();
+    
+  };
+
+typedef std::multimap<const GridSlamProcessor::TNode*, GridSlamProcessor::TNode*> TNodeMultimap;
+
+
+#include "gridslamprocessor.hxx"
+
+};
+
+#endif

slam_gmapping/openslam_gmapping/include/gmapping/gridfastslam/gridslamprocessor.hxx

@@ -0,0 +1,177 @@
+
+#ifdef MACOSX
+// This is to overcome a possible bug in Apple's GCC.
+#define isnan(x) (x==FP_NAN)
+#endif
+
+/**Just scan match every single particle.
+If the scan matching fails, the particle gets a default likelihood.*/
+inline void GridSlamProcessor::scanMatch(const double* plainReading){
+  // sample a new pose from each scan in the reference
+  
+  double sumScore=0;
+  for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
+    OrientedPoint corrected;
+    double score, l, s;
+    score=m_matcher.optimize(corrected, it->map, it->pose, plainReading);
+    //    it->pose=corrected;
+    if (score>m_minimumScore){
+      it->pose=corrected;
+    } else {
+	if (m_infoStream){
+	  m_infoStream << "Scan Matching Failed, using odometry. Likelihood=" << l <<std::endl;
+	  m_infoStream << "lp:" << m_lastPartPose.x << " "  << m_lastPartPose.y << " "<< m_lastPartPose.theta <<std::endl;
+	  m_infoStream << "op:" << m_odoPose.x << " " << m_odoPose.y << " "<< m_odoPose.theta <<std::endl;
+	}
+    }
+
+    m_matcher.likelihoodAndScore(s, l, it->map, it->pose, plainReading);
+    sumScore+=score;
+    it->weight+=l;
+    it->weightSum+=l;
+
+    //set up the selective copy of the active area
+    //by detaching the areas that will be updated
+    m_matcher.invalidateActiveArea();
+    m_matcher.computeActiveArea(it->map, it->pose, plainReading);
+  }
+  if (m_infoStream)
+    m_infoStream << "Average Scan Matching Score=" << sumScore/m_particles.size() << std::endl;	
+}
+
+inline void GridSlamProcessor::normalize(){
+  //normalize the log m_weights
+  double gain=1./(m_obsSigmaGain*m_particles.size());
+  double lmax= -std::numeric_limits<double>::max();
+  for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
+    lmax=it->weight>lmax?it->weight:lmax;
+  }
+  //cout << "!!!!!!!!!!! maxwaight= "<< lmax << endl;
+  
+  m_weights.clear();
+  double wcum=0;
+  m_neff=0;
+  for (std::vector<Particle>::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
+    m_weights.push_back(exp(gain*(it->weight-lmax)));
+    wcum+=m_weights.back();
+    //cout << "l=" << it->weight<< endl;
+  }
+  
+  m_neff=0;
+  for (std::vector<double>::iterator it=m_weights.begin(); it!=m_weights.end(); it++){
+    *it=*it/wcum;
+    double w=*it;
+    m_neff+=w*w;
+  }
+  m_neff=1./m_neff;
+  
+}
+
+inline bool GridSlamProcessor::resample(const double* plainReading, int adaptSize, const RangeReading* reading){
+  
+  bool hasResampled = false;
+  
+  TNodeVector oldGeneration;
+  for (unsigned int i=0; i<m_particles.size(); i++){
+    oldGeneration.push_back(m_particles[i].node);
+  }
+  
+  if (m_neff<m_resampleThreshold*m_particles.size()){		
+    
+    if (m_infoStream)
+      m_infoStream  << "*************RESAMPLE***************" << std::endl;
+    
+    uniform_resampler<double, double> resampler;
+    m_indexes=resampler.resampleIndexes(m_weights, adaptSize);
+    
+    if (m_outputStream.is_open()){
+      m_outputStream << "RESAMPLE "<< m_indexes.size() << " ";
+      for (std::vector<unsigned int>::const_iterator it=m_indexes.begin(); it!=m_indexes.end(); it++){
+	m_outputStream << *it <<  " ";
+      }
+      m_outputStream << std::endl;
+    }
+    
+    onResampleUpdate();
+    //BEGIN: BUILDING TREE
+    ParticleVector temp;
+    unsigned int j=0;
+    std::vector<unsigned int> deletedParticles;  		//this is for deleteing the particles which have been resampled away.
+    
+    //		cerr << "Existing Nodes:" ;
+    for (unsigned int i=0; i<m_indexes.size(); i++){
+      //			cerr << " " << m_indexes[i];
+      while(j<m_indexes[i]){
+	deletedParticles.push_back(j);
+	j++;
+			}
+      if (j==m_indexes[i])
+	j++;
+      Particle & p=m_particles[m_indexes[i]];
+      TNode* node=0;
+      TNode* oldNode=oldGeneration[m_indexes[i]];
+      //			cerr << i << "->" << m_indexes[i] << "B("<<oldNode->childs <<") ";
+      node=new	TNode(p.pose, 0, oldNode, 0);
+      //node->reading=0;
+      node->reading=reading;
+      //			cerr << "A("<<node->parent->childs <<") " <<endl;
+      
+      temp.push_back(p);
+      temp.back().node=node;
+      temp.back().previousIndex=m_indexes[i];
+    }
+    while(j<m_indexes.size()){
+      deletedParticles.push_back(j);
+      j++;
+    }
+    //		cerr << endl;
+    std::cerr <<  "Deleting Nodes:";
+    for (unsigned int i=0; i<deletedParticles.size(); i++){
+      std::cerr <<" " << deletedParticles[i];
+      delete m_particles[deletedParticles[i]].node;
+      m_particles[deletedParticles[i]].node=0;
+    }
+    std::cerr  << " Done" <<std::endl;
+    
+    //END: BUILDING TREE
+    std::cerr << "Deleting old particles..." ;
+    m_particles.clear();
+    std::cerr << "Done" << std::endl;
+    std::cerr << "Copying Particles and  Registering  scans...";
+    for (ParticleVector::iterator it=temp.begin(); it!=temp.end(); it++){
+      it->setWeight(0);
+      m_matcher.invalidateActiveArea();
+      m_matcher.registerScan(it->map, it->pose, plainReading);
+      m_particles.push_back(*it);
+    }
+    std::cerr  << " Done" <<std::endl;
+    hasResampled = true;
+  } else {
+    int index=0;
+    std::cerr << "Registering Scans:";
+    TNodeVector::iterator node_it=oldGeneration.begin();
+    for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
+      //create a new node in the particle tree and add it to the old tree
+      //BEGIN: BUILDING TREE  
+      TNode* node=0;
+      node=new TNode(it->pose, 0.0, *node_it, 0);
+      
+      //node->reading=0;
+      node->reading=reading;
+      it->node=node;
+
+      //END: BUILDING TREE
+      m_matcher.invalidateActiveArea();
+      m_matcher.registerScan(it->map, it->pose, plainReading);
+      it->previousIndex=index;
+      index++;
+      node_it++;
+      
+    }
+    std::cerr  << "Done" <<std::endl;
+    
+  }
+  //END: BUILDING TREE
+  
+  return hasResampled;
+}

slam_gmapping/openslam_gmapping/include/gmapping/gridfastslam/motionmodel.h

@@ -0,0 +1,19 @@
+#ifndef MOTIONMODEL_H
+#define MOTIONMODEL_H
+
+#include <gmapping/utils/point.h>
+#include <gmapping/utils/stat.h>
+#include <gmapping/utils/macro_params.h>
+
+namespace  GMapping { 
+
+struct MotionModel{
+	OrientedPoint drawFromMotion(const OrientedPoint& p, double linearMove, double angularMove) const;
+	OrientedPoint drawFromMotion(const OrientedPoint& p, const OrientedPoint& pnew, const OrientedPoint& pold) const;
+	Covariance3 gaussianApproximation(const OrientedPoint& pnew, const OrientedPoint& pold) const;
+	double srr, str, srt, stt;
+};
+
+};
+
+#endif

slam_gmapping/openslam_gmapping/include/gmapping/log/configuration.h

@@ -0,0 +1,17 @@
+#ifndef CONFIGURATION_H
+#define CONFIGURATION_H
+
+#include <istream>
+#include <gmapping/sensor/sensor_base/sensor.h>
+
+namespace GMapping {
+
+class Configuration{
+	public:
+		virtual ~Configuration();
+		virtual SensorMap computeSensorMap() const=0;
+};
+
+};
+#endif
+

slam_gmapping/openslam_gmapping/include/gmapping/log/sensorlog.h

@@ -0,0 +1,29 @@
+#ifndef SENSORLOG_H
+#define SENSORLOG_H
+
+#include <list>
+#include <istream>
+#include <gmapping/sensor/sensor_base/sensorreading.h>
+#include <gmapping/sensor/sensor_odometry/odometrysensor.h>
+#include <gmapping/sensor/sensor_range/rangesensor.h>
+#include <gmapping/sensor/sensor_odometry/odometryreading.h>
+#include <gmapping/sensor/sensor_range/rangereading.h>
+#include "configuration.h"
+
+namespace GMapping {
+
+class SensorLog : public std::list<SensorReading*>{
+	public:
+		SensorLog(const SensorMap&);
+		~SensorLog();
+		std::istream& load(std::istream& is);
+		OrientedPoint boundingBox(double& xmin, double& ymin, double& xmax, double& ymax) const;
+	protected:
+		const SensorMap& m_sensorMap;
+		OdometryReading* parseOdometry(std::istream& is, const OdometrySensor* ) const;
+		RangeReading* parseRange(std::istream& is, const RangeSensor* ) const;
+};
+
+};
+
+#endif

slam_gmapping/openslam_gmapping/include/gmapping/particlefilter/particlefilter.h

@@ -0,0 +1,328 @@
+#ifndef PARTICLEFILTER_H
+#define PARTICLEFILTER_H
+#include <stdlib.h>
+#include <sys/types.h>
+#include <vector>
+#include <utility>
+#include <cmath>
+#include <limits>
+#include <gmapping/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 = -std::numeric_limits<double>::max();
+	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 Vector>
+void repeatIndexes(Vector& dest, const std::vector<int>& indexes, const Vector& particles){
+	assert(indexes.size()==particles.size());
+	dest.resize(particles.size());
+	unsigned int i=0;
+	for (std::vector<int>::const_iterator it=indexes.begin(); it!=indexes.end(); ++it){
+		dest[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 Iterator>
+void normalize(const Iterator& begin, const Iterator& end){
+	double sum=0;
+	for (Iterator it=begin; it!=end; ++it){
+		sum+=*it;
+	}
+	for (Iterator it=begin; it!=end; ++it){
+		*it=*it/sum;
+	}
+}
+
+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++;
+}
+
+//BEGIN legacy
+template <class Particle, class Numeric>
+struct uniform_resampler{
+	std::vector<unsigned int> resampleIndexes(const std::vector<Particle> & particles, int nparticles=0) const;
+	std::vector<Particle> resample(const std::vector<Particle> & particles, int nparticles=0) const;
+	Numeric neff(const std::vector<Particle> & particles) const;
+};
+
+/*Implementation of the above stuff*/
+template <class Particle, class Numeric>
+std::vector<unsigned int> uniform_resampler<Particle, Numeric>:: resampleIndexes(const std::vector<Particle>& particles, int nparticles) const{
+	Numeric cweight=0;
+
+	//compute the cumulative weights
+	unsigned int n=0;
+	for (typename std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it){
+		cweight+=(Numeric)*it;
+		n++;
+	}
+
+	if (nparticles>0)
+		n=nparticles;
+	
+	//compute the interval
+	Numeric interval=cweight/n;
+
+	//compute the initial target weight
+	Numeric target=interval*::drand48();
+	//compute the resampled indexes
+
+	cweight=0;
+	std::vector<unsigned int> indexes(n);
+	n=0;
+	unsigned int i=0;
+	for (typename std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it, ++i){
+		cweight+=(Numeric)* it;
+		while(cweight>target){
+			indexes[n++]=i;
+			target+=interval;
+		}
+	}
+	return indexes;
+}
+
+template <class Particle, class Numeric>
+std::vector<Particle> uniform_resampler<Particle,Numeric>::resample
+  (const typename std::vector<Particle>& particles, int nparticles) const{
+	Numeric cweight=0;
+
+	//compute the cumulative weights
+	unsigned int n=0;
+	for (typename std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it){
+		cweight+=(Numeric)*it;
+		n++;
+	}
+
+	if (nparticles>0)
+		n=nparticles;
+	
+	//weight of the particles after resampling
+	double uw=1./n;
+
+	//compute the interval
+	Numeric interval=cweight/n;
+
+	//compute the initial target weight
+	Numeric target=interval*::drand48();
+	//compute the resampled indexes
+
+	cweight=0;
+	std::vector<Particle> resampled;
+	n=0;
+	unsigned int i=0;
+	for (typename std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it, ++i){
+		cweight+=(Numeric)*it;
+		while(cweight>target){
+			resampled.push_back(*it);
+			resampled.back().setWeight(uw);
+			target+=interval;
+		}
+	}
+	return resampled;
+}
+
+template <class Particle, class Numeric>
+Numeric uniform_resampler<Particle,Numeric>::neff(const std::vector<Particle> & particles) const{
+	double cum=0;
+	double sum=0;
+	for (typename std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it){
+		Numeric w=(Numeric)*it;
+		cum+=w*w;
+		sum+=w;
+	}
+	return sum*sum/cum;
+}
+
+
+/*
+
+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 Particle, class EvolutionModel>
+struct evolver{
+	EvolutionModel evolutionModel;
+	void evolve(std::vector<Particle>& particles);
+	void evolve(std::vector<Particle>& dest, const std::vector<Particle>& src);
+};
+
+template <class Particle, class EvolutionModel>
+void evolver<Particle, EvolutionModel>::evolve(std::vector<Particle>& particles){
+	for (typename std::vector<Particle>::iterator it=particles.begin(); it!=particles.end(); ++it){
+		*it=evolutionModel.evolve(*it);
+	}
+}
+
+template <class Particle, class EvolutionModel>
+void evolver<Particle, EvolutionModel>::evolve(std::vector<Particle>& dest, const std::vector<Particle>& src){
+	dest.clear();
+	for (typename std::vector<Particle>::const_iterator it=src.begin(); it!=src.end(); ++it)
+		dest.push_back(evolutionModel.evolve(*it));
+}
+
+
+template <class Particle, class Numeric, class QualificationModel, class EvolutionModel, class LikelyhoodModel>
+struct auxiliary_evolver{
+	EvolutionModel evolutionModel;
+	QualificationModel qualificationModel;
+	LikelyhoodModel likelyhoodModel;
+	void evolve(std::vector<Particle>& particles);
+	void evolve(std::vector<Particle>& dest, const std::vector<Particle>& src);
+};
+
+template <class Particle, class Numeric, class QualificationModel, class EvolutionModel, class LikelyhoodModel>
+void auxiliary_evolver<Particle, Numeric, QualificationModel, EvolutionModel, LikelyhoodModel>::evolve
+  (std::vector<Particle>&particles){
+	std::vector<Numeric> observationWeights(particles.size());
+	unsigned int i=0;
+	for (typename std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it, i++){
+		observationWeights[i]=likelyhoodModel.likelyhood(qualificationModel.evolve(*it));
+	}
+	uniform_resampler<Numeric, Numeric> resampler;
+	std::vector<unsigned int> indexes(resampler.resampleIndexes(observationWeights));
+	for (typename std::vector<unsigned int>::const_iterator it=indexes.begin(); it!=indexes.end(); ++it){
+		Particle & particle=particles[*it];
+		particle=evolutionModel.evolve(particle);
+		particle.setWeight(likelyhoodModel.likelyhood(particle)/observationWeights[*it]);
+	}
+}
+
+template <class Particle, class Numeric, class QualificationModel, class EvolutionModel, class LikelyhoodModel>
+void auxiliary_evolver<Particle, Numeric, QualificationModel, EvolutionModel, LikelyhoodModel>::evolve
+  (std::vector<Particle>& dest, const std::vector<Particle>& src){
+	dest.clear();
+	std::vector<Numeric> observationWeights(src.size());
+	unsigned int i=0;
+	for (typename std::vector<Particle>::const_iterator it=src.begin(); it!=src.end(); ++it, i++){
+		observationWeights[i]=likelyhoodModel.likelyhood(qualificationModel.evolve(*it));
+	}
+	uniform_resampler<Numeric, Numeric> resampler;
+	std::vector<unsigned int> indexes(resampler.resampleIndexes(observationWeights));
+	for (typename 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.likelyhood(particle)/observationWeights[*it];
+	}
+}
+//END legacy
+
+#endif

slam_gmapping/openslam_gmapping/include/gmapping/scanmatcher/icp.h

@@ -0,0 +1,85 @@
+#ifndef _ICP_H_
+#define _ICP_H_
+
+#include <gmapping/utils/point.h>
+#include <utility>
+#include <list>
+#include <vector>
+
+namespace GMapping{
+typedef std::pair<Point,Point> PointPair;
+
+template <typename PointPairContainer>
+double icpStep(OrientedPoint & retval, const PointPairContainer& container){
+	typedef typename PointPairContainer::const_iterator ContainerIterator;
+	PointPair mean=std::make_pair(Point(0.,0.), Point(0.,0.));
+	int size=0;
+	for (ContainerIterator it=container.begin(); it!=container.end(); it++){
+		mean.first=mean.first+it->first;
+		mean.second=mean.second+it->second;
+		size++;
+	}
+	mean.first=mean.first*(1./size);
+	mean.second=mean.second*(1./size);
+	double sxx=0, sxy=0, syx=0, syy=0;
+	
+	for (ContainerIterator it=container.begin(); it!=container.end(); it++){
+		PointPair mf=std::make_pair(it->first-mean.first, it->second-mean.second);
+		sxx+=mf.first.x*mf.second.x;
+		sxy+=mf.first.x*mf.second.y;
+		syx+=mf.first.y*mf.second.x;
+		syy+=mf.first.y*mf.second.y;
+	}
+	retval.theta=atan2(sxy-syx, sxx+sxy);
+	double s=sin(retval.theta), c=cos(retval.theta);
+	retval.x=mean.second.x-(c*mean.first.x-s*mean.first.y);
+	retval.y=mean.second.y-(s*mean.first.x+c*mean.first.y);
+	
+	double error=0;
+	for (ContainerIterator it=container.begin(); it!=container.end(); it++){
+		Point delta(
+			c*it->first.x-s*it->first.y+retval.x-it->second.x, s*it->first.x+c*it->first.y+retval.y-it->second.y);
+		error+=delta*delta;
+	}
+	return error;	
+}
+
+template <typename PointPairContainer>
+double icpNonlinearStep(OrientedPoint & retval, const PointPairContainer& container){
+	typedef typename PointPairContainer::const_iterator ContainerIterator;
+	PointPair mean=std::make_pair(Point(0.,0.), Point(0.,0.));
+	int size=0;
+	for (ContainerIterator it=container.begin(); it!=container.end(); it++){
+		mean.first=mean.first+it->first;
+		mean.second=mean.second+it->second;
+		size++;
+	}
+	
+	mean.first=mean.first*(1./size);
+	mean.second=mean.second*(1./size);
+	
+	double ms=0,mc=0;
+	for (ContainerIterator it=container.begin(); it!=container.end(); it++){
+		PointPair mf=std::make_pair(it->first-mean.first, it->second-mean.second);
+		double  dalpha=atan2(mf.second.y, mf.second.x) - atan2(mf.first.y, mf.first.x);
+		double gain=sqrt(mean.first*mean.first);
+		ms+=gain*sin(dalpha);
+		mc+=gain*cos(dalpha);
+	}
+	retval.theta=atan2(ms, mc);
+	double s=sin(retval.theta), c=cos(retval.theta);
+	retval.x=mean.second.x-(c*mean.first.x-s*mean.first.y);
+	retval.y=mean.second.y-(s*mean.first.x+c*mean.first.y);
+	
+	double error=0;
+	for (ContainerIterator it=container.begin(); it!=container.end(); it++){
+		Point delta(
+			c*it->first.x-s*it->first.y+retval.x-it->second.x, s*it->first.x+c*it->first.y+retval.y-it->second.y);
+		error+=delta*delta;
+	}
+	return error;	
+}
+
+}//end namespace
+
+#endif

slam_gmapping/openslam_gmapping/include/gmapping/scanmatcher/scanmatcher.h

@@ -0,0 +1,252 @@
+#ifndef SCANMATCHER_H
+#define SCANMATCHER_H
+
+#include "icp.h"
+#include "smmap.h"
+#include <gmapping/utils/macro_params.h>
+#include <gmapping/utils/stat.h>
+#include <iostream>
+#include <gmapping/utils/gvalues.h>
+#define LASER_MAXBEAMS 2048
+
+namespace GMapping {
+
+class ScanMatcher{
+	public:
+		typedef Covariance3 CovarianceMatrix;
+		
+		ScanMatcher();
+		~ScanMatcher();
+		double icpOptimize(OrientedPoint& pnew, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const;
+		double optimize(OrientedPoint& pnew, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const;
+		double optimize(OrientedPoint& mean, CovarianceMatrix& cov, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const;
+		
+		double   registerScan(ScanMatcherMap& map, const OrientedPoint& p, const double* readings);
+		void setLaserParameters
+			(unsigned int beams, double* angles, const OrientedPoint& lpose);
+		void setMatchingParameters
+			(double urange, double range, double sigma, int kernsize, double lopt, double aopt, int iterations, double likelihoodSigma=1, unsigned int likelihoodSkip=0 );
+		void invalidateActiveArea();
+		void computeActiveArea(ScanMatcherMap& map, const OrientedPoint& p, const double* readings);
+
+		inline double icpStep(OrientedPoint & pret, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const;
+		inline double score(const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const;
+		inline unsigned int likelihoodAndScore(double& s, double& l, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const;
+		double likelihood(double& lmax, OrientedPoint& mean, CovarianceMatrix& cov, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings);
+		double likelihood(double& _lmax, OrientedPoint& _mean, CovarianceMatrix& _cov, const ScanMatcherMap& map, const OrientedPoint& p, Gaussian3& odometry, const double* readings, double gain=180.);
+		inline const double* laserAngles() const { return m_laserAngles; }
+		inline unsigned int laserBeams() const { return m_laserBeams; }
+		
+		static const double nullLikelihood;
+	protected:
+		//state of the matcher
+		bool m_activeAreaComputed;
+		
+		/**laser parameters*/
+		unsigned int m_laserBeams;
+		double       m_laserAngles[LASER_MAXBEAMS];
+		//OrientedPoint m_laserPose;
+		PARAM_SET_GET(OrientedPoint, laserPose, protected, public, public)
+		PARAM_SET_GET(double, laserMaxRange, protected, public, public)
+		/**scan_matcher parameters*/
+		PARAM_SET_GET(double, usableRange, protected, public, public)
+		PARAM_SET_GET(double, gaussianSigma, protected, public, public)
+		PARAM_SET_GET(double, likelihoodSigma, protected, public, public)
+		PARAM_SET_GET(int,    kernelSize, protected, public, public)
+		PARAM_SET_GET(double, optAngularDelta, protected, public, public)
+		PARAM_SET_GET(double, optLinearDelta, protected, public, public)
+		PARAM_SET_GET(unsigned int, optRecursiveIterations, protected, public, public)
+		PARAM_SET_GET(unsigned int, likelihoodSkip, protected, public, public)
+		PARAM_SET_GET(double, llsamplerange, protected, public, public)
+		PARAM_SET_GET(double, llsamplestep, protected, public, public)
+		PARAM_SET_GET(double, lasamplerange, protected, public, public)
+		PARAM_SET_GET(double, lasamplestep, protected, public, public)
+		PARAM_SET_GET(bool, generateMap, protected, public, public)
+		PARAM_SET_GET(double, enlargeStep, protected, public, public)
+		PARAM_SET_GET(double, fullnessThreshold, protected, public, public)
+		PARAM_SET_GET(double, angularOdometryReliability, protected, public, public)
+		PARAM_SET_GET(double, linearOdometryReliability, protected, public, public)
+		PARAM_SET_GET(double, freeCellRatio, protected, public, public)
+		PARAM_SET_GET(unsigned int, initialBeamsSkip, protected, public, public)
+
+		// allocate this large array only once
+		IntPoint* m_linePoints;
+};
+
+inline double ScanMatcher::icpStep(OrientedPoint & pret, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const{
+	const double * angle=m_laserAngles+m_initialBeamsSkip;
+	OrientedPoint lp=p;
+	lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
+	lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
+	lp.theta+=m_laserPose.theta;
+	unsigned int skip=0;
+	double freeDelta=map.getDelta()*m_freeCellRatio;
+	std::list<PointPair> pairs;
+	
+	for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++){
+		skip++;
+		skip=skip>m_likelihoodSkip?0:skip;
+		if (*r>m_usableRange||*r==0.0) continue;
+		if (skip) continue;
+		Point phit=lp;
+		phit.x+=*r*cos(lp.theta+*angle);
+		phit.y+=*r*sin(lp.theta+*angle);
+		IntPoint iphit=map.world2map(phit);
+		Point pfree=lp;
+		pfree.x+=(*r-map.getDelta()*freeDelta)*cos(lp.theta+*angle);
+		pfree.y+=(*r-map.getDelta()*freeDelta)*sin(lp.theta+*angle);
+ 		pfree=pfree-phit;
+		IntPoint ipfree=map.world2map(pfree);
+		bool found=false;
+		Point bestMu(0.,0.);
+		Point bestCell(0.,0.);
+		for (int xx=-m_kernelSize; xx<=m_kernelSize; xx++)
+		for (int yy=-m_kernelSize; yy<=m_kernelSize; yy++){
+			IntPoint pr=iphit+IntPoint(xx,yy);
+			IntPoint pf=pr+ipfree;
+			//AccessibilityState s=map.storage().cellState(pr);
+			//if (s&Inside && s&Allocated){
+				const PointAccumulator& cell=map.cell(pr);
+				const PointAccumulator& fcell=map.cell(pf);
+				if (((double)cell )> m_fullnessThreshold && ((double)fcell )<m_fullnessThreshold){
+					Point mu=phit-cell.mean();
+					if (!found){
+						bestMu=mu;
+						bestCell=cell.mean();
+						found=true;
+					}else
+						if((mu*mu)<(bestMu*bestMu)){
+							bestMu=mu;
+							bestCell=cell.mean();
+						} 
+						
+				}
+			//}
+		}
+		if (found){
+			pairs.push_back(std::make_pair(phit, bestCell));
+			//std::cerr << "(" << phit.x-bestCell.x << "," << phit.y-bestCell.y << ") ";
+		}
+		//std::cerr << std::endl;
+	}
+	
+	OrientedPoint result(0,0,0);
+	//double icpError=icpNonlinearStep(result,pairs);
+	std::cerr << "result(" << pairs.size() << ")=" << result.x << " " << result.y << " " << result.theta << std::endl;
+	pret.x=p.x+result.x;
+	pret.y=p.y+result.y;
+	pret.theta=p.theta+result.theta;
+	pret.theta=atan2(sin(pret.theta), cos(pret.theta));
+	return score(map, p, readings);
+}
+
+inline double ScanMatcher::score(const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const{
+	double s=0;
+	const double * angle=m_laserAngles+m_initialBeamsSkip;
+	OrientedPoint lp=p;
+	lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
+	lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
+	lp.theta+=m_laserPose.theta;
+	unsigned int skip=0;
+	double freeDelta=map.getDelta()*m_freeCellRatio;
+	for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++){
+		skip++;
+		skip=skip>m_likelihoodSkip?0:skip;
+		if (skip||*r>m_usableRange||*r==0.0) continue;
+		Point phit=lp;
+		phit.x+=*r*cos(lp.theta+*angle);
+		phit.y+=*r*sin(lp.theta+*angle);
+		IntPoint iphit=map.world2map(phit);
+		Point pfree=lp;
+		pfree.x+=(*r-map.getDelta()*freeDelta)*cos(lp.theta+*angle);
+		pfree.y+=(*r-map.getDelta()*freeDelta)*sin(lp.theta+*angle);
+ 		pfree=pfree-phit;
+		IntPoint ipfree=map.world2map(pfree);
+		bool found=false;
+		Point bestMu(0.,0.);
+		for (int xx=-m_kernelSize; xx<=m_kernelSize; xx++)
+		for (int yy=-m_kernelSize; yy<=m_kernelSize; yy++){
+			IntPoint pr=iphit+IntPoint(xx,yy);
+			IntPoint pf=pr+ipfree;
+			//AccessibilityState s=map.storage().cellState(pr);
+			//if (s&Inside && s&Allocated){
+				const PointAccumulator& cell=map.cell(pr);
+				const PointAccumulator& fcell=map.cell(pf);
+				if (((double)cell )> m_fullnessThreshold && ((double)fcell )<m_fullnessThreshold){
+					Point mu=phit-cell.mean();
+					if (!found){
+						bestMu=mu;
+						found=true;
+					}else
+						bestMu=(mu*mu)<(bestMu*bestMu)?mu:bestMu;
+				}
+			//}
+		}
+		if (found)
+			s+=exp(-1./m_gaussianSigma*bestMu*bestMu);
+	}
+	return s;
+}
+
+inline unsigned int ScanMatcher::likelihoodAndScore(double& s, double& l, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const{
+	using namespace std;
+	l=0;
+	s=0;
+	const double * angle=m_laserAngles+m_initialBeamsSkip;
+	OrientedPoint lp=p;
+	lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
+	lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
+	lp.theta+=m_laserPose.theta;
+	double noHit=nullLikelihood/(m_likelihoodSigma);
+	unsigned int skip=0;
+	unsigned int c=0;
+	double freeDelta=map.getDelta()*m_freeCellRatio;
+	for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++){
+		skip++;
+		skip=skip>m_likelihoodSkip?0:skip;
+		if (*r>m_usableRange) continue;
+		if (skip) continue;
+		Point phit=lp;
+		phit.x+=*r*cos(lp.theta+*angle);
+		phit.y+=*r*sin(lp.theta+*angle);
+		IntPoint iphit=map.world2map(phit);
+		Point pfree=lp;
+		pfree.x+=(*r-freeDelta)*cos(lp.theta+*angle);
+		pfree.y+=(*r-freeDelta)*sin(lp.theta+*angle);
+		pfree=pfree-phit;
+		IntPoint ipfree=map.world2map(pfree);
+		bool found=false;
+		Point bestMu(0.,0.);
+		for (int xx=-m_kernelSize; xx<=m_kernelSize; xx++)
+		for (int yy=-m_kernelSize; yy<=m_kernelSize; yy++){
+			IntPoint pr=iphit+IntPoint(xx,yy);
+			IntPoint pf=pr+ipfree;
+			//AccessibilityState s=map.storage().cellState(pr);
+			//if (s&Inside && s&Allocated){
+				const PointAccumulator& cell=map.cell(pr);
+				const PointAccumulator& fcell=map.cell(pf);
+				if (((double)cell )>m_fullnessThreshold && ((double)fcell )<m_fullnessThreshold){
+					Point mu=phit-cell.mean();
+					if (!found){
+						bestMu=mu;
+						found=true;
+					}else
+						bestMu=(mu*mu)<(bestMu*bestMu)?mu:bestMu;
+				}
+			//}	
+		}
+		if (found){
+			s+=exp(-1./m_gaussianSigma*bestMu*bestMu);
+			c++;
+		}
+		if (!skip){
+			double f=(-1./m_likelihoodSigma)*(bestMu*bestMu);
+			l+=(found)?f:noHit;
+		}
+	}
+	return c;
+}
+
+};
+
+#endif

slam_gmapping/openslam_gmapping/include/gmapping/scanmatcher/smmap.h

@@ -0,0 +1,54 @@
+#ifndef SMMAP_H
+#define SMMAP_H
+#include <gmapping/grid/map.h>
+#include <gmapping/grid/harray2d.h>
+#include <gmapping/utils/point.h>
+#define SIGHT_INC 1
+
+namespace GMapping {
+
+struct PointAccumulator{
+	typedef point<float> FloatPoint;
+	/* before 
+	PointAccumulator(int i=-1): acc(0,0), n(0), visits(0){assert(i==-1);}
+	*/
+	/*after begin*/
+	PointAccumulator(): acc(0,0), n(0), visits(0){}
+	PointAccumulator(int i): acc(0,0), n(0), visits(0){assert(i==-1);}
+	/*after end*/
+        inline void update(bool value, const Point& p=Point(0,0));
+	inline Point mean() const {return 1./n*Point(acc.x, acc.y);}
+	inline operator double() const { return visits?(double)n*SIGHT_INC/(double)visits:-1; }
+	inline void add(const PointAccumulator& p) {acc=acc+p.acc; n+=p.n; visits+=p.visits; }
+	static const PointAccumulator& Unknown();
+	static PointAccumulator* unknown_ptr;
+	FloatPoint acc;
+	int n, visits;
+	inline double entropy() const;
+};
+
+void PointAccumulator::update(bool value, const Point& p){
+	if (value) {
+		acc.x+= static_cast<float>(p.x);
+		acc.y+= static_cast<float>(p.y); 
+		n++; 
+		visits+=SIGHT_INC;
+	} else
+		visits++;
+}
+
+double PointAccumulator::entropy() const{
+	if (!visits)
+		return -log(.5);
+	if (n==visits || n==0)
+		return 0;
+	double x=(double)n*SIGHT_INC/(double)visits;
+	return -( x*log(x)+ (1-x)*log(1-x) );
+}
+
+
+typedef Map<PointAccumulator,HierarchicalArray2D<PointAccumulator> > ScanMatcherMap;
+
+};
+
+#endif 

slam_gmapping/openslam_gmapping/include/gmapping/sensor/sensor_base/sensor.h

@@ -0,0 +1,24 @@
+#ifndef SENSOR_H
+#define SENSOR_H
+
+#include <string>
+#include <map>
+
+namespace GMapping{
+
+class Sensor{
+	public:
+		Sensor(const std::string& name="");
+		virtual ~Sensor();
+		inline std::string getName() const {return m_name;}
+		inline void setName(const std::string& name) {m_name=name;}
+	protected:
+		std::string m_name;
+};
+
+typedef std::map<std::string, Sensor*> SensorMap;
+
+}; //end namespace
+
+#endif
+

slam_gmapping/openslam_gmapping/include/gmapping/sensor/sensor_base/sensorreading.h

@@ -0,0 +1,26 @@
+#ifndef SENSORREADING_H
+#define SENSORREADING_H
+
+#include "sensor.h"
+namespace GMapping{
+
+class SensorReading{
+	public:
+		SensorReading(const Sensor* s, double time){
+			m_sensor=s;
+			m_time=time;
+		};
+		~SensorReading(){};
+		inline double getTime() const {return m_time;}
+		inline void setTime(double t) {m_time=t;}
+		inline const Sensor* getSensor() const {return m_sensor;}
+	protected:
+		double m_time;
+		const Sensor* m_sensor;
+
+};
+
+}; //end namespace
+#endif
+
+

slam_gmapping/openslam_gmapping/include/gmapping/sensor/sensor_odometry/odometryreading.h

@@ -0,0 +1,29 @@
+#ifndef ODOMETRYREADING_H
+#define ODOMETRYREADING_H
+
+#include <string.h>
+#include <gmapping/sensor/sensor_base/sensorreading.h>
+#include <gmapping/utils/point.h>
+#include "odometrysensor.h"
+
+namespace GMapping{
+
+class OdometryReading: public SensorReading{
+	public:
+		OdometryReading(const OdometrySensor* odo, double time=0);
+		inline const OrientedPoint& getPose() const {return m_pose;}
+		inline const OrientedPoint& getSpeed() const {return m_speed;}
+		inline const OrientedPoint& getAcceleration() const {return m_acceleration;}
+		inline void setPose(const OrientedPoint& pose) {m_pose=pose;}
+		inline void setSpeed(const OrientedPoint& speed) {m_speed=speed;}
+		inline void setAcceleration(const OrientedPoint& acceleration) {m_acceleration=acceleration;}
+		
+	protected:
+		OrientedPoint m_pose;
+		OrientedPoint m_speed;
+		OrientedPoint m_acceleration;
+};
+
+};
+#endif
+

slam_gmapping/openslam_gmapping/include/gmapping/sensor/sensor_odometry/odometrysensor.h

@@ -0,0 +1,20 @@
+#ifndef ODOMETRYSENSOR_H
+#define ODOMETRYSENSOR_H
+
+#include <string>
+#include <gmapping/sensor/sensor_base/sensor.h>
+
+namespace GMapping{
+
+class OdometrySensor: public Sensor{
+	public:
+		OdometrySensor(const std::string& name, bool ideal=false);
+		inline bool isIdeal() const { return m_ideal; }
+	protected:
+		bool m_ideal;	
+};
+
+};
+
+#endif
+

slam_gmapping/openslam_gmapping/include/gmapping/sensor/sensor_range/rangereading.h

@@ -0,0 +1,26 @@
+#ifndef RANGEREADING_H
+#define RANGEREADING_H
+
+#include <vector>
+#include <gmapping/sensor/sensor_base/sensorreading.h>
+#include <gmapping/sensor/sensor_range/rangesensor.h>
+
+namespace GMapping{
+
+class RangeReading: public SensorReading, public std::vector<double>{
+	public:
+		RangeReading(const RangeSensor* rs, double time=0);
+		RangeReading(unsigned int n_beams, const double* d, const RangeSensor* rs, double time=0);
+		virtual ~RangeReading();
+		inline const OrientedPoint& getPose() const {return m_pose;}
+		inline void setPose(const OrientedPoint& pose) {m_pose=pose;}
+		unsigned int rawView(double* v, double density=0.) const;
+		std::vector<Point> cartesianForm(double maxRange=1e6) const;
+		unsigned int activeBeams(double density=0.) const;
+	protected:
+		OrientedPoint m_pose;
+};
+
+};
+
+#endif

slam_gmapping/openslam_gmapping/include/gmapping/sensor/sensor_range/rangesensor.h

@@ -0,0 +1,35 @@
+#ifndef RANGESENSOR_H
+#define RANGESENSOR_H
+
+#include <vector>
+#include <gmapping/sensor/sensor_base/sensor.h>
+#include <gmapping/utils/point.h>
+
+namespace GMapping{
+
+class RangeSensor: public Sensor{
+	friend class Configuration;
+	friend class CarmenConfiguration;
+	friend class CarmenWrapper;
+	public:
+		struct Beam{
+			OrientedPoint pose;	//pose relative to the center of the sensor
+			double span;	//spam=0 indicates a line-like beam
+			double maxRange;	//maximum range of the sensor
+			double s,c;		//sinus and cosinus of the beam (optimization);
+		};	
+		RangeSensor(std::string name);
+		RangeSensor(std::string name, unsigned int beams, double res, const OrientedPoint& position=OrientedPoint(0,0,0), double span=0, double maxrange=89.0);
+		inline const std::vector<Beam>& beams() const {return m_beams;}
+		inline std::vector<Beam>& beams() {return m_beams;}
+		inline OrientedPoint getPose() const {return m_pose;}
+		void updateBeamsLookup();
+		bool newFormat;
+	protected:
+		OrientedPoint m_pose;
+		std::vector<Beam> m_beams;
+};
+
+};
+
+#endif

slam_gmapping/openslam_gmapping/include/gmapping/utils/autoptr.h

@@ -0,0 +1,97 @@
+#ifndef AUTOPTR_H
+#define AUTOPTR_H
+#include <assert.h>
+
+namespace GMapping{
+
+template <class X>
+class autoptr{
+	protected:
+	
+	public:
+	struct reference{
+		X* data;
+		unsigned int shares;
+	};
+		inline autoptr(X* p=(X*)(0));
+		inline autoptr(const autoptr<X>& ap);
+		inline autoptr& operator=(const autoptr<X>& ap);
+		inline ~autoptr();
+		inline operator int() const;
+		inline X& operator*();
+		inline const X& operator*() const;
+		//p	
+		reference * m_reference;
+	protected:
+};
+
+template <class X>
+autoptr<X>::autoptr(X* p){
+	m_reference=0;
+	if (p){
+		m_reference=new reference;
+		m_reference->data=p;
+		m_reference->shares=1;
+	}
+}
+
+template <class X>
+autoptr<X>::autoptr(const autoptr<X>& ap){
+	m_reference=0;
+	reference* ref=ap.m_reference;
+	if (ap.m_reference){
+		m_reference=ref;
+		m_reference->shares++;
+	}
+}
+
+template <class X>
+autoptr<X>& autoptr<X>::operator=(const autoptr<X>& ap){
+	reference* ref=ap.m_reference;
+	if (m_reference==ref){
+		return *this;
+	}
+	if (m_reference && !(--m_reference->shares)){
+		delete m_reference->data;
+		delete m_reference;
+		m_reference=0;
+	}	
+	if (ref){
+		m_reference=ref;
+		m_reference->shares++;
+	} 
+//20050802 nasty changes begin
+	else
+		m_reference=0;
+//20050802 nasty changes end
+	return *this;
+}
+
+template <class X>
+autoptr<X>::~autoptr(){
+	if (m_reference && !(--m_reference->shares)){
+		delete m_reference->data;
+		delete m_reference;
+		m_reference=0;
+	}	
+}
+
+template <class X>
+autoptr<X>::operator int() const{
+	return m_reference && m_reference->shares && m_reference->data;
+}
+
+template <class X>
+X& autoptr<X>::operator*(){
+	assert(m_reference && m_reference->shares && m_reference->data);
+	return *(m_reference->data);
+}
+
+template <class X>
+const X& autoptr<X>::operator*() const{
+	assert(m_reference && m_reference->shares && m_reference->data);
+	return *(m_reference->data);
+}
+
+};
+#endif

slam_gmapping/openslam_gmapping/include/gmapping/utils/commandline.h

@@ -0,0 +1,115 @@
+/*****************************************************************
+ *
+ * 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 COMMANDLINE_H
+#define COMMANDLINE_H
+
+	
+#define parseFlag(name,value)\
+if (!strcmp(argv[c],name)){\
+	value=true;\
+	cout << name << " on"<< endl;\
+	recognized=true;\
+}\
+
+#define parseString(name,value)\
+if (!strcmp(argv[c],name) && c<argc-1){\
+	c++;\
+	value=argv[c];\
+	cout << name << "=" << value << endl;\
+	recognized=true;\
+}\
+
+
+#define parseDouble(name,value)\
+if (!strcmp(argv[c],name) && c<argc-1){\
+	c++;\
+	value=atof(argv[c]);\
+	cout << name << "=" << value << endl;\
+	recognized=true;\
+}\
+
+#define parseInt(name,value)\
+if (!strcmp(argv[c],name) && c<argc-1){\
+	c++;\
+	value=atoi(argv[c]);\
+	cout << name << "=" << value << endl;\
+	recognized=true;\
+}\
+
+#define CMD_PARSE_BEGIN(i, count)\
+{\
+	int c=i;\
+	while (c<count){\
+		bool recognized=false;
+	
+#define CMD_PARSE_END\
+		if (!recognized)\
+			cout << "COMMAND LINE: parameter " << argv[c] << " not recognized" << endl;\
+		c++;\
+	}\
+}
+
+#define CMD_PARSE_BEGIN_SILENT(i, count)\
+{\
+	int c=i;\
+	while (c<count){\
+		bool recognized=false;
+	
+#define CMD_PARSE_END_SILENT\
+		c++;\
+	}\
+}
+
+	
+#define parseFlagSilent(name,value)\
+if (!strcmp(argv[c],name)){\
+	value=true;\
+	recognized=true;\
+}\
+
+#define parseStringSilent(name,value)\
+if (!strcmp(argv[c],name) && c<argc-1){\
+	c++;\
+	value=argv[c];\
+	recognized=true;\
+}\
+
+
+#define parseDoubleSilent(name,value)\
+if (!strcmp(argv[c],name) && c<argc-1){\
+	c++;\
+	value=atof(argv[c]);\
+	recognized=true;\
+}\
+
+#define parseIntSilent(name,value)\
+if (!strcmp(argv[c],name) && c<argc-1){\
+	c++;\
+	value=atoi(argv[c]);\
+	recognized=true;\
+}\
+
+
+#endif
+

slam_gmapping/openslam_gmapping/include/gmapping/utils/gvalues.h

@@ -0,0 +1,28 @@
+#ifndef _GVALUES_H_
+#define _GVALUES_H_
+
+#define MAXDOUBLE 1e1000
+#ifdef LINUX
+	#include <values.h>
+#endif
+#ifdef MACOSX
+	#include <limits.h>
+	#include <math.h>
+	//#define isnan(x) (x==FP_NAN)
+#endif
+#ifdef _WIN32
+  #include <limits>
+  #ifndef __DRAND48_DEFINED__
+     #define __DRAND48_DEFINED__
+     inline double drand48() { return double(rand()) / RAND_MAX;}
+  #endif
+  #ifndef M_PI
+    #define M_PI 3.1415926535897932384626433832795
+  #endif
+  #define round(d) (floor((d) + 0.5))
+  typedef unsigned int uint;
+  #define isnan(x) (_isnan(x))
+#endif
+
+#endif 
+

slam_gmapping/openslam_gmapping/include/gmapping/utils/macro_params.h

@@ -0,0 +1,38 @@
+#ifndef MACRO_PARAMS_H
+#define MACRO_PARAMS_H
+
+#define PARAM_SET_GET(type, name, qualifier, setqualifier, getqualifier)\
+qualifier: type m_##name;\
+getqualifier: inline type get##name() const {return m_##name;}\
+setqualifier: inline void set##name(type name) {m_##name=name;}
+
+#define PARAM_SET(type, name, qualifier, setqualifier)\
+qualifier: type m_##name;\
+setqualifier: inline void set##name(type name) {m_##name=name;}
+
+#define PARAM_GET(type, name, qualifier, getqualifier)\
+qualifier: type m_##name;\
+getqualifier: inline type get##name() const {return m_##name;}
+
+#define MEMBER_PARAM_SET_GET(member, type, name, qualifier, setqualifier, getqualifier)\
+getqualifier: inline type get##name() const {return member.get##name();}\
+setqualifier: inline void set##name(type name) { member.set##name(name);}
+
+#define MEMBER_PARAM_SET(member, type, name, qualifier, setqualifier, getqualifier)\
+setqualifier: inline void set##name(type name) { member.set##name(name);}
+
+#define MEMBER_PARAM_GET(member, type, name, qualifier, setqualifier, getqualifier)\
+getqualifier: inline type get##name() const {return member.get##name();}
+
+#define STRUCT_PARAM_SET_GET(member, type, name, qualifier, setqualifier, getqualifier)\
+getqualifier: inline type get##name() const {return member.name;}\
+setqualifier: inline void set##name(type name) {member.name=name;}
+
+#define STRUCT_PARAM_SET(member, type, name, qualifier, setqualifier, getqualifier)\
+setqualifier: inline void set##name(type name) {member.name=name;}
+
+#define STRUCT_PARAM_GET(member, type, name, qualifier, setqualifier, getqualifier)\
+getqualifier: inline type get##name() const {return member.name;}\
+
+#define convertStringArgument(var,val,buf) if (!strcmp(buf,#val)) var=val
+#endif

slam_gmapping/openslam_gmapping/include/gmapping/utils/point.h

@@ -0,0 +1,207 @@
+#ifndef _POINT_H_
+#define _POINT_H_
+#include <assert.h>
+#include <math.h>
+#include <iostream>
+#include "gvalues.h"
+
+#define DEBUG_STREAM cerr << __PRETTY_FUNCTION__ << ":" //FIXME
+
+namespace GMapping {
+
+template <class T>
+struct point{
+	inline point():x(0),y(0) {}
+	inline point(T _x, T _y):x(_x),y(_y){}
+	T x, y;
+};
+
+template <class T>
+inline point<T> operator+(const point<T>& p1, const point<T>& p2){
+	return point<T>(p1.x+p2.x, p1.y+p2.y);
+}
+
+template <class T>
+inline point<T> operator - (const point<T> & p1, const point<T> & p2){
+	return point<T>(p1.x-p2.x, p1.y-p2.y);
+}
+
+template <class T>
+inline point<T> operator * (const point<T>& p, const T& v){
+	return point<T>(p.x*v, p.y*v);
+}
+
+template <class T>
+inline point<T> operator * (const T& v, const point<T>& p){
+	return point<T>(p.x*v, p.y*v);
+}
+
+template <class T>
+inline T operator * (const point<T>& p1, const point<T>& p2){
+	return p1.x*p2.x+p1.y*p2.y;
+}
+
+
+template <class T, class A>
+struct orientedpoint: public point<T>{
+  inline orientedpoint() : point<T>(0,0), theta(0) {};
+	inline orientedpoint(const point<T>& p);
+	inline orientedpoint(T x, T y, A _theta): point<T>(x,y), theta(_theta){}
+        inline void normalize();
+	inline orientedpoint<T,A> rotate(A alpha){
+		T s=sin(alpha), c=cos(alpha);
+		A a=alpha+theta;
+		a=atan2(sin(a),cos(a));
+		return orientedpoint(
+			c*this->x-s*this->y,
+			s*this->x+c*this->y, 
+			a);
+	}
+	A theta;
+};
+
+
+template <class T, class A>
+void orientedpoint<T,A>::normalize() {
+  if (theta >= -M_PI && theta < M_PI)
+    return;
+  
+  int multiplier = (int)(theta / (2*M_PI));
+  theta = theta - multiplier*2*M_PI;
+  if (theta >= M_PI)
+    theta -= 2*M_PI;
+  if (theta < -M_PI)
+    theta += 2*M_PI;
+}
+
+
+template <class T, class A>
+orientedpoint<T,A>::orientedpoint(const point<T>& p){
+	this->x=p.x;
+	this->y=p.y;
+	this->theta=0.;
+}
+
+
+template <class T, class A>
+orientedpoint<T,A> operator+(const orientedpoint<T,A>& p1, const orientedpoint<T,A>& p2){
+	return orientedpoint<T,A>(p1.x+p2.x, p1.y+p2.y, p1.theta+p2.theta);
+}
+
+template <class T, class A>
+orientedpoint<T,A> operator - (const orientedpoint<T,A> & p1, const orientedpoint<T,A> & p2){
+	return orientedpoint<T,A>(p1.x-p2.x, p1.y-p2.y, p1.theta-p2.theta);
+}
+
+template <class T, class A>
+orientedpoint<T,A> operator * (const orientedpoint<T,A>& p, const T& v){
+	return orientedpoint<T,A>(p.x*v, p.y*v, p.theta*v);
+}
+
+template <class T, class A>
+orientedpoint<T,A> operator * (const T& v, const orientedpoint<T,A>& p){
+	return orientedpoint<T,A>(p.x*v, p.y*v, p.theta*v);
+}
+
+template <class T, class A>
+orientedpoint<T,A> absoluteDifference(const orientedpoint<T,A>& p1,const orientedpoint<T,A>& p2){
+	orientedpoint<T,A> delta=p1-p2;
+	delta.theta=atan2(sin(delta.theta), cos(delta.theta));
+	double s=sin(p2.theta), c=cos(p2.theta);
+	return orientedpoint<T,A>(c*delta.x+s*delta.y, 
+	                         -s*delta.x+c*delta.y, delta.theta);
+}
+
+template <class T, class A>
+orientedpoint<T,A> absoluteSum(const orientedpoint<T,A>& p1,const orientedpoint<T,A>& p2){
+	double s=sin(p1.theta), c=cos(p1.theta);
+	return orientedpoint<T,A>(c*p2.x-s*p2.y,
+				  s*p2.x+c*p2.y, p2.theta) + p1;
+}
+
+template <class T, class A>
+point<T> absoluteSum(const orientedpoint<T,A>& p1,const point<T>& p2){
+	double s=sin(p1.theta), c=cos(p1.theta);
+	return point<T>(c*p2.x-s*p2.y, s*p2.x+c*p2.y) + (point<T>) p1;
+}
+
+template <class T>
+struct pointcomparator{
+	bool operator ()(const point<T>& a, const point<T>& b) const {
+		return a.x<b.x || (a.x==b.x && a.y<b.y);
+	}	
+};
+
+template <class T>
+struct pointradialcomparator{
+	point<T> origin;
+	bool operator ()(const point<T>& a, const point<T>& b) const {
+		point<T> delta1=a-origin;
+		point<T> delta2=b-origin;
+		return (atan2(delta1.y,delta1.x)<atan2(delta2.y,delta2.x));
+	}	
+};
+
+template <class T>
+inline point<T> max(const point<T>& p1, const point<T>& p2){
+	point<T> p=p1;
+	p.x=p.x>p2.x?p.x:p2.x;
+	p.y=p.y>p2.y?p.y:p2.y;
+	return p;
+}
+
+template <class T>
+inline point<T> min(const point<T>& p1, const point<T>& p2){
+	point<T> p=p1;
+	p.x=p.x<p2.x?p.x:p2.x;
+	p.y=p.y<p2.y?p.y:p2.y;
+	return p;
+}
+
+template <class T, class F>
+inline point<T> interpolate(const point<T>& p1,  const F& t1, const point<T>& p2, const F& t2, const F& t3){
+	F gain=(t3-t1)/(t2-t1);
+	point<T> p=p1+(p2-p1)*gain;
+	return p;
+}
+
+template <class T, class A, class F>
+inline orientedpoint<T,A> 
+interpolate(const orientedpoint<T,A>& p1,  const F& t1, const orientedpoint<T,A>& p2, const F& t2, const F& t3){
+	F gain=(t3-t1)/(t2-t1);
+	orientedpoint<T,A> p;
+	p.x=p1.x+(p2.x-p1.x)*gain;
+	p.y=p1.y+(p2.y-p1.y)*gain;
+	double  s=sin(p1.theta)+sin(p2.theta)*gain,
+		c=cos(p1.theta)+cos(p2.theta)*gain;
+	p.theta=atan2(s,c);
+	return p;
+}
+
+
+template <class T>
+inline double euclidianDist(const point<T>& p1, const point<T>& p2){
+  return hypot(p1.x-p2.x, p1.y-p2.y);
+}
+template <class T, class A>
+inline double euclidianDist(const orientedpoint<T,A>& p1, const orientedpoint<T,A>& p2){
+  return hypot(p1.x-p2.x, p1.y-p2.y);
+}
+template <class T, class A>
+inline double euclidianDist(const orientedpoint<T,A>& p1, const point<T>& p2){
+  return hypot(p1.x-p2.x, p1.y-p2.y);
+}
+template <class T, class A>
+inline double euclidianDist(const point<T>& p1, const orientedpoint<T,A>& p2 ){
+  return hypot(p1.x-p2.x, p1.y-p2.y);
+}
+
+
+
+typedef point<int> IntPoint;
+typedef point<double> Point;
+typedef orientedpoint<double, double> OrientedPoint;
+
+}; //end namespace
+
+#endif

slam_gmapping/openslam_gmapping/include/gmapping/utils/stat.h

@@ -0,0 +1,147 @@
+#ifndef STAT_H
+#define STAT_H
+#include "point.h"
+#include <vector>
+#include "gvalues.h"
+
+namespace GMapping {
+
+/**stupid utility function for drawing particles form a zero mean, sigma variance normal distribution
+probably it should not go there*/
+double sampleGaussian(double sigma,unsigned int S=0);
+
+double evalGaussian(double sigmaSquare, double delta);
+double evalLogGaussian(double sigmaSquare, double delta);
+int sampleUniformInt(int max);
+double sampleUniformDouble(double min, double max);
+
+struct Covariance3{
+	Covariance3 operator + (const Covariance3 & cov) const;
+	static Covariance3 zero;
+	double xx, yy, tt, xy, xt, yt;
+};
+
+struct EigenCovariance3{
+	EigenCovariance3();
+	EigenCovariance3(const Covariance3& c);
+	EigenCovariance3 rotate(double angle) const;
+	OrientedPoint sample() const;
+	double eval[3];
+	double evec[3][3];
+};
+
+struct Gaussian3{
+	OrientedPoint mean;
+	EigenCovariance3 covariance;
+	Covariance3 cov;
+	double eval(const OrientedPoint& p) const;
+	void computeFromSamples(const std::vector<OrientedPoint> & poses);
+	void computeFromSamples(const std::vector<OrientedPoint> & poses, const std::vector<double>& weights );
+};
+
+template<typename PointIterator, typename WeightIterator>
+Gaussian3 computeGaussianFromSamples(PointIterator& pointBegin, PointIterator& pointEnd, WeightIterator& weightBegin, WeightIterator& weightEnd){
+	Gaussian3 gaussian;
+	OrientedPoint mean=OrientedPoint(0,0,0);
+	double wcum=0;
+	double s=0, c=0;
+	WeightIterator wt=weightBegin;
+	double *w=new double();
+	OrientedPoint *p=new OrientedPoint();
+	for (PointIterator pt=pointBegin; pt!=pointEnd; pt++){
+		*w=*wt;
+		*p=*pt;
+		s+=*w*sin(p->theta);
+		c+=*w*cos(p->theta);
+		mean.x+=*w*p->x;
+		mean.y+=*w*p->y;
+		wcum+=*w;
+		wt++;
+	}
+	mean.x/=wcum;
+	mean.y/=wcum;
+	s/=wcum;
+	c/=wcum;
+	mean.theta=atan2(s,c);
+
+	Covariance3 cov=Covariance3::zero;
+	wt=weightBegin;
+	for (PointIterator pt=pointBegin; pt!=pointEnd; pt++){
+		*w=*wt;
+		*p=*pt;
+		OrientedPoint delta=(*p)-mean;
+		delta.theta=atan2(sin(delta.theta),cos(delta.theta));
+		cov.xx+=*w*delta.x*delta.x;
+		cov.yy+=*w*delta.y*delta.y;
+		cov.tt+=*w*delta.theta*delta.theta;
+		cov.xy+=*w*delta.x*delta.y;
+		cov.yt+=*w*delta.y*delta.theta;
+		cov.xt+=*w*delta.x*delta.theta;
+		wt++;
+	}
+	cov.xx/=wcum;
+	cov.yy/=wcum;
+	cov.tt/=wcum;
+	cov.xy/=wcum;
+	cov.yt/=wcum;
+	cov.xt/=wcum;
+	EigenCovariance3 ecov(cov);
+	gaussian.mean=mean;
+	gaussian.covariance=ecov;
+	gaussian.cov=cov;
+	delete w;
+	delete p;
+	return gaussian;
+}
+
+template<typename PointIterator>
+Gaussian3 computeGaussianFromSamples(PointIterator& pointBegin, PointIterator& pointEnd){
+	Gaussian3 gaussian;
+	OrientedPoint mean=OrientedPoint(0,0,0);
+	double wcum=1;
+	double s=0, c=0;
+	OrientedPoint *p=new OrientedPoint();
+	for (PointIterator pt=pointBegin; pt!=pointEnd; pt++){
+		*p=*pt;
+		s+=sin(p->theta);
+		c+=cos(p->theta);
+		mean.x+=p->x;
+		mean.y+=p->y;
+		wcum+=1.;
+	}
+	mean.x/=wcum;
+	mean.y/=wcum;
+	s/=wcum;
+	c/=wcum;
+	mean.theta=atan2(s,c);
+
+	Covariance3 cov=Covariance3::zero;
+	for (PointIterator pt=pointBegin; pt!=pointEnd; pt++){
+		*p=*pt;
+		OrientedPoint delta=(*p)-mean;
+		delta.theta=atan2(sin(delta.theta),cos(delta.theta));
+		cov.xx+=delta.x*delta.x;
+		cov.yy+=delta.y*delta.y;
+		cov.tt+=delta.theta*delta.theta;
+		cov.xy+=delta.x*delta.y;
+		cov.yt+=delta.y*delta.theta;
+		cov.xt+=delta.x*delta.theta;
+	}
+	cov.xx/=wcum;
+	cov.yy/=wcum;
+	cov.tt/=wcum;
+	cov.xy/=wcum;
+	cov.yt/=wcum;
+	cov.xt/=wcum;
+	EigenCovariance3 ecov(cov);
+	gaussian.mean=mean;
+	gaussian.covariance=ecov;
+	gaussian.cov=cov;
+	delete p;
+	return gaussian;
+}
+
+
+}; //end namespace
+#endif
+

slam_gmapping/openslam_gmapping/ini/gfs-LMS-10cm.ini

@@ -0,0 +1,78 @@
+### gfs dummy config file
+
+## WARNING: Changing these parameters, can
+## increase of decrese the performance of the
+## mapper!
+
+
+[gfs]
+
+#################################################
+##
+## These are probably the most improtant parameters
+##
+
+## gfs - number of particles
+particles        30
+
+## gfs measurement integration
+angularUpdate       0.5
+linearUpdate        1
+
+## map resolution
+delta            0.1
+
+## scan matcher
+maxrange         81.0     #   (maximum valid) for SICK LMS, 81m max, SICK PLS 50m
+maxUrange        80.0     #   (use up to)
+sigma            0.05     # scan matcher cell sigma, for the greedy search
+regscore         0.0004   # minimum score for regsistering a scan 
+iterations       5        # iterations 
+critscore        0.0      # critical score (leave this)
+maxMove          1.0      # maximum move among two scans. This detects some corrupted logs
+autosize         off	  # determine te map size by pre readoing the log
+
+
+## default settings for a 0.1 m map cell
+lstep            0.1       # linear search step (choose delta)
+astep            0.05      # angular search step, this is fine, depending on the odometry error and the update interval
+lsigma           0.075       # sigma likelihood of 1 beam
+lskip            0	   # beams to skip in the likelihood computation
+skipMatching     off	   # do not perform scan matching before computing the statistics
+
+kernelSize       1        # the higher the value the slower the filter
+                          # the better it can deal with noise, but the less precise and slower
+ogain            3	  # gain for smoothing the likelihood
+resampleThreshold   0.5   # when neff  is below this value a resampling occurs
+randseed            0	  # this is for the repeated experiments
+
+## likelihood sampling
+llsamplerange              0.1	# linear range
+llsamplestep               0.1  # linear step
+lasamplerange              0.05 # angular range
+lasamplestep               0.05 # angular step
+
+## motion model parameters
+srr      0.1              # translation as a function of translation
+srt      0.1              # translation as a function of rotation
+str      0.1              # rotation as a function of translation
+stt      0.1              # rotation as a function of rotation
+
+## odometry integration in proposal
+linearOdometryReliability  0.0
+angularOdometryReliability 0.0
+considerOdometryCovariance   off
+
+
+## inital map params
+xmin            -100.0
+ymin            -100.0
+xmax            100.0
+ymax            100.0
+
+## file parameters
+
+readFromStdin       off
+onLine              off
+generateMap         off
+

slam_gmapping/openslam_gmapping/ini/gfs-LMS-20cm.ini

@@ -0,0 +1,76 @@
+### gfs dummy config file
+
+## WARNING: Changing these parameters, can
+## increase of decrese the performance of the
+## mapper!
+
+
+[gfs]
+
+#################################################
+##
+## These are probably the most improtant parameters
+##
+
+## gfs - number of particles
+particles        30
+
+## gfs measurement integration
+angularUpdate	0.5
+linearUpdate	1
+
+## map resolution
+delta            0.2
+## scan matcher
+maxrange         80     #   (maximum valid) for SICK LMS, 81m max, SICK PLS 50m
+maxUrange        80     #   (use up to)
+sigma            0.05     # scan matcher cell sigma, for the greedy search
+regscore         10000    # minimum score for regsistering a scan 
+iterations       5        # iterations 
+critscore        0.0      # critical score (leave this)
+maxMove          1.0      # maximum move among two scans. This detects some corrupted logs
+autosize         off	  # determine te map size by pre readoing the log
+
+
+lstep            0.2      # linear search step (choose delta)
+astep            0.05      # angular search step, this is fine, depending on the odometry error and the update interval
+lsigma           0.2       # sigma likelihood of 1 beam
+lskip            1.	   # beams to skip in the likelihood computation
+skipMatching     off	   # do not perform scan matching before computing the statistics
+
+kernelSize       1        # the higher the value the slower the filter
+                          # the better it can deal with noise, but the less precise and slower
+ogain            3	  # gain for smoothing the likelihood
+resampleThreshold   0.5   # when neff  is below this value a resampling occurs
+randseed            0	  # this is for the repeated experiments
+
+## likelihood sampling
+llsamplerange              0.2	 # linear range
+llsamplestep               0.2  # linear step
+lasamplerange              0.05 # angular range
+lasamplestep               0.05 # angular step
+
+## motion model parameters
+srr      0.1              # translation as a function of translation
+srt      0.1              # translation as a function of rotation
+str      0.1              # rotation as a function of translation
+stt      0.1              # rotation as a function of rotation
+
+## odometry integration in proposal
+linearOdometryReliability  0.0
+angularOdometryReliability 0.0
+considerOdometryCovariance   off
+
+
+## inital map params
+xmin            -150.0
+ymin            -100.0
+xmax            100.0
+ymax            100.0
+
+## file parameters
+
+readFromStdin       off
+onLine              off
+generateMap         off
+

slam_gmapping/openslam_gmapping/ini/gfs-LMS-5cm.ini

@@ -0,0 +1,78 @@
+### gfs dummy config file
+
+## WARNING: Changing these parameters, can
+## increase of decrese the performance of the
+## mapper!
+
+
+[gfs]
+
+#################################################
+##
+## These are probably the most improtant parameters
+##
+
+## gfs - number of particles
+particles        30
+
+## gfs measurement integration
+angularUpdate       0.5
+linearUpdate        1
+
+## map resolution
+delta            0.05
+
+## scan matcher
+maxrange         81.0     #   (maximum valid) for SICK LMS, 81m max, SICK PLS 50m
+maxUrange        80.0     #   (use up to)
+sigma            0.05     # scan matcher cell sigma, for the greedy search
+regscore         0.0004   # minimum score for regsistering a scan 
+iterations       5        # iterations 
+critscore        0.0      # critical score (leave this)
+maxMove          1.0      # maximum move among two scans. This detects some corrupted logs
+autosize         off	  # determine te map size by pre readoing the log
+
+
+## default settings for a 0.1 m map cell
+lstep            0.05       # linear search step (choose delta)
+astep            0.05      # angular search step, this is fine, depending on the odometry error and the update interval
+lsigma           0.05       # sigma likelihood of 1 beam
+lskip            0	   # beams to skip in the likelihood computation
+skipMatching     off	   # do not perform scan matching before computing the statistics
+
+kernelSize       1        # the higher the value the slower the filter
+                          # the better it can deal with noise, but the less precise and slower
+ogain            3	  # gain for smoothing the likelihood
+resampleThreshold   0.5   # when neff  is below this value a resampling occurs
+randseed            0	  # this is for the repeated experiments
+
+## likelihood sampling
+llsamplerange              0.05	# linear range
+llsamplestep               0.05  # linear step
+lasamplerange              0.05 # angular range
+lasamplestep               0.05 # angular step
+
+## motion model parameters
+srr      0.1              # translation as a function of translation
+srt      0.1              # translation as a function of rotation
+str      0.1              # rotation as a function of translation
+stt      0.1              # rotation as a function of rotation
+
+## odometry integration in proposal
+linearOdometryReliability  0.0
+angularOdometryReliability 0.0
+considerOdometryCovariance   off
+
+
+## inital map params
+xmin            -100.0
+ymin            -100.0
+xmax            100.0
+ymax            100.0
+
+## file parameters
+
+readFromStdin       off
+onLine              off
+generateMap         off
+

slam_gmapping/openslam_gmapping/ini/gfs-PLS-10cm.ini

@@ -0,0 +1,78 @@
+### gfs dummy config file
+
+## WARNING: Changing these parameters, can
+## increase of decrese the performance of the
+## mapper!
+
+
+[gfs]
+
+#################################################
+##
+## These are probably the most improtant parameters
+##
+
+## gfs - number of particles
+particles        30
+
+## gfs measurement integration
+angularUpdate       0.5
+linearUpdate        1
+
+## map resolution
+delta            0.1
+
+## scan matcher
+maxrange         50.0     #   (maximum valid) for SICK LMS, 81m max, SICK PLS 50m
+maxUrange        50.0     #   (use up to)
+sigma            0.075     # scan matcher cell sigma, for the greedy search
+regscore         0.0004   # minimum score for regsistering a scan 
+iterations       5        # iterations 
+critscore        0.0      # critical score (leave this)
+maxMove          1.0      # maximum move among two scans. This detects some corrupted logs
+autosize         off	  # determine te map size by pre readoing the log
+
+
+## default settings for a 0.1 m map cell
+lstep            0.1       # linear search step (choose delta)
+astep            0.05      # angular search step, this is fine, depending on the odometry error and the update interval
+lsigma           0.1       # sigma likelihood of 1 beam
+lskip            0	   # beams to skip in the likelihood computation
+skipMatching     off	   # do not perform scan matching before computing the statistics
+
+kernelSize       1        # the higher the value the slower the filter
+                          # the better it can deal with noise, but the less precise and slower
+ogain            3	  # gain for smoothing the likelihood
+resampleThreshold   0.5   # when neff  is below this value a resampling occurs
+randseed            0	  # this is for the repeated experiments
+
+## likelihood sampling
+llsamplerange              0.1	# linear range
+llsamplestep               0.1  # linear step
+lasamplerange              0.05 # angular range
+lasamplestep               0.05 # angular step
+
+## motion model parameters
+srr      0.1              # translation as a function of translation
+srt      0.1              # translation as a function of rotation
+str      0.1              # rotation as a function of translation
+stt      0.1              # rotation as a function of rotation
+
+## odometry integration in proposal
+linearOdometryReliability  0.0 # condition the scan matcher with odometry while serching for max [translation component]
+angularOdometryReliability 0.0 # condition the scan matcher with odometry while serching for max [rotation component]
+considerOdometryCovariance   off
+
+
+## inital map params
+xmin            -100.0
+ymin            -100.0
+xmax            100.0
+ymax            100.0
+
+## file parameters
+
+readFromStdin       off
+onLine              off
+generateMap         off
+

slam_gmapping/openslam_gmapping/ini/gfs-PLS-5cm.ini

@@ -0,0 +1,78 @@
+### gfs dummy config file
+
+## WARNING: Changing these parameters, can
+## increase of decrese the performance of the
+## mapper!
+
+
+[gfs]
+
+#################################################
+##
+## These are probably the most improtant parameters
+##
+
+## gfs - number of particles
+particles        30
+
+## gfs measurement integration
+angularUpdate       0.5
+linearUpdate        1
+
+## map resolution
+delta            0.05
+
+## scan matcher
+maxrange         50.0     #   (maximum valid) for SICK LMS, 81m max, SICK PLS 50m
+maxUrange        50.0     #   (use up to)
+sigma            0.07     # scan matcher cell sigma, for the greedy search
+regscore         0.0004   # minimum score for regsistering a scan 
+iterations       5        # iterations 
+critscore        0.0      # critical score (leave this)
+maxMove          1.0      # maximum move among two scans. This detects some corrupted logs
+autosize         off	  # determine te map size by pre readoing the log
+
+
+## default settings for a 0.1 m map cell
+lstep            0.05       # linear search step (choose delta)
+astep            0.05      # angular search step, this is fine, depending on the odometry error and the update interval
+lsigma           0.05       # sigma likelihood of 1 beam
+lskip            0	   # beams to skip in the likelihood computation
+skipMatching     off	   # do not perform scan matching before computing the statistics
+
+kernelSize       1        # the higher the value the slower the filter
+                          # the better it can deal with noise, but the less precise and slower
+ogain            3	  # gain for smoothing the likelihood
+resampleThreshold   0.5   # when neff  is below this value a resampling occurs
+randseed            0	  # this is for the repeated experiments
+
+## likelihood sampling
+llsamplerange              0.05	# linear range
+llsamplestep               0.05  # linear step
+lasamplerange              0.05 # angular range
+lasamplestep               0.05 # angular step
+
+## motion model parameters
+srr      0.1              # translation as a function of translation
+srt      0.1              # translation as a function of rotation
+str      0.1              # rotation as a function of translation
+stt      0.1              # rotation as a function of rotation
+
+## odometry integration in proposal
+linearOdometryReliability  0.0
+angularOdometryReliability 0.0
+considerOdometryCovariance   off
+
+
+## inital map params
+xmin            -100.0
+ymin            -100.0
+xmax            100.0
+ymax            100.0
+
+## file parameters
+
+readFromStdin       off
+onLine              off
+generateMap         off
+

slam_gmapping/openslam_gmapping/log/Makefile

@@ -0,0 +1,9 @@
+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
+

slam_gmapping/openslam_gmapping/log/carmenconfiguration.cpp

@@ -0,0 +1,463 @@
+#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>
+
+
+#define LINEBUFFER_SIZE 10000
+
+namespace GMapping {
+
+using namespace std;
+
+istream& CarmenConfiguration::load(istream& is){
+	clear();
+	char buf[LINEBUFFER_SIZE];
+	bool laseron=false;
+	bool rlaseron=false;
+	bool rlaser1=false;
+	bool rlaser2=false;
+
+	string beams;
+	string rbeams;
+
+	while (is){
+		is.getline(buf, LINEBUFFER_SIZE);
+		istringstream lis(buf);
+		
+		string qualifier;
+		string name;
+		
+		if (lis) 
+			lis >> qualifier; 
+		else 
+			continue;
+		//this is a workaround for carmen log files
+		//the number lf laser beams should be specofoed in the config
+		//part of the log
+		if (qualifier=="FLASER"){
+			laseron=true;
+			lis >> beams;
+		}
+		if (qualifier=="RLASER"){
+			rlaseron=true;
+			lis >> rbeams;
+		}
+		if (qualifier=="ROBOTLASER1"){
+			string laser_type, start_angle, field_of_view, angular_resolution, maximum_range, accuracy, remission_mode;
+			lis >> laser_type>> start_angle>> field_of_view>> angular_resolution>> maximum_range>> accuracy>> remission_mode>> beams;
+			rlaser1=true;
+		}
+		if (qualifier=="ROBOTLASER2"){
+			string laser_type, start_angle, field_of_view, angular_resolution, maximum_range, accuracy, remission_mode;
+			lis >> laser_type>> start_angle>> field_of_view>> angular_resolution>> maximum_range>> accuracy>> remission_mode>> rbeams;
+			rlaser2=true;
+		}
+		if (qualifier!="PARAM") 
+			continue;
+		if (lis) 
+			lis >> name; 
+		else continue;
+		
+
+		vector<string> v;
+		while (lis){
+			string cparm;
+			lis >> cparm;
+			if (lis)
+				v.push_back(cparm);
+		}
+		insert(make_pair(name, v));
+	}
+	if (laseron || rlaser1){
+		vector<string> v;
+		v.push_back(beams);
+		insert(make_pair("laser_beams", v));
+		cerr << "FRONT LASER BEAMS FROM LOG: " << beams << endl;
+		v.clear();
+		v.push_back("on");
+		insert(make_pair("robot_use_laser", v));
+	}
+	if (rlaseron || rlaser2){
+		vector<string> v;
+		v.push_back(rbeams);
+		insert(make_pair("rear_laser_beams", v));
+		cerr << "REAR LASER BEAMS FROM LOG: " << beams << endl;
+		v.clear();
+		v.push_back("on");
+		insert(make_pair("robot_use_rear_laser", v));
+	}
+	return is;
+}
+
+SensorMap CarmenConfiguration::computeSensorMap() const{
+	//this boring stuff is for retrieving the parameters from the loaded tokens
+	
+	SensorMap smap;
+	//odometry
+	OdometrySensor* odometry=new OdometrySensor("ODOM");
+	OdometrySensor* truepos=new OdometrySensor("TRUEPOS", true);
+	
+	smap.insert(make_pair(odometry->getName(), odometry));
+	smap.insert(make_pair(truepos->getName(), truepos));
+	//sonars
+	const_iterator key=find("robot_use_sonar");
+	if (key!=end() && key->second.front()=="on"){
+		RangeSensor* sonar=new RangeSensor("SONAR");
+
+		//the center of the sonar is the center of the base
+		sonar->m_pose.x=sonar->m_pose.y=sonar->m_pose.theta=0;
+
+		double maxrange=10.;
+		key=find("robot_max_sonar");
+		if (key!=end()){
+			maxrange=atof(key->second.front().c_str());
+			cerr << "max sonar:" << maxrange << endl;
+		}
+
+		unsigned int sonar_num=0;
+		key=find("robot_num_sonars");
+		if (key!=end()){
+			sonar_num=atoi(key->second.front().c_str());
+			cerr << "robot_num_sonars" << sonar_num << endl;
+		}
+
+		key=find("robot_sonar_offsets");
+		if (key!=end()){
+			const vector<string> & soff=key->second;
+
+			if( (soff.size()/3<sonar_num)){
+				cerr << __PRETTY_FUNCTION__ << ": Error " << soff.size()
+				<< " parameters for defining the sonar offsets"
+				<< " while the specified number of sonars requires "
+				<< sonar_num*3 << " parameters at least" << endl;
+			} else {
+				cerr << __PRETTY_FUNCTION__ << ": Ok " << soff.size() << " parameters for defining the sonar offsets of " << sonar_num << " devices" << endl;
+			}
+
+
+			RangeSensor::Beam beam;
+
+			for (unsigned int i=0; i<sonar_num*3; i+=3){
+				beam.span=M_PI/180.*7.5;
+				beam.pose.x=atof(soff[i].c_str());
+				beam.pose.y=atof(soff[i+1].c_str());
+				beam.pose.theta=atof(soff[i+2].c_str());
+				beam.maxRange=maxrange;
+				sonar->m_beams.push_back(beam);
+				cerr << "beam_x" << beam.pose.x;
+				cerr << " beam_y" << beam.pose.y;
+				cerr << " beam_theta" << beam.pose.theta << endl;;
+			}
+		}
+		sonar->updateBeamsLookup();
+		smap.insert(make_pair(sonar->getName(), sonar));
+	}
+	
+	//laser
+	key=find("robot_use_laser");
+
+	if (key!=end() && key->second.front()=="on"){
+		RangeSensor* laser=new RangeSensor("FLASER");
+		laser->newFormat=false;
+		//by default the center of the robot is the center of the laser
+		laser->m_pose.x=laser->m_pose.y=laser->m_pose.theta=0;
+		key=find("robot_frontlaser_offset");
+		if (key!=end()){
+			laser->m_pose.x=atof(key->second.front().c_str());
+			cerr << "FRONT OFFSET= " << laser->m_pose.x << endl;
+		}
+		
+		
+		
+		RangeSensor::Beam beam;
+			
+		//double angle=-.5*M_PI;
+		unsigned int beam_no=180;
+	
+		key=find("laser_beams");
+		if (key!=end()){
+			beam_no=atoi(key->second.front().c_str());
+			cerr << "FRONT BEAMS="<< beam_no << endl;
+		}
+		
+		double maxrange=50;
+		double resolution=1.;
+				
+		
+		if (beam_no==180 || beam_no==181)
+		  resolution =1.;
+		else if (beam_no==360 || beam_no==361)
+		  resolution =.5;
+		else if (beam_no==540 || beam_no==541)
+		  resolution =.5;
+		else if (beam_no==769) {
+		  resolution =360./1024.;
+		  maxrange = 4.1;
+		}
+		else if (beam_no==682) {
+		  resolution =360./1024.;
+		  maxrange = 4.1;
+		}
+		else if (beam_no==683) {
+		  resolution =360./1024.;
+		  maxrange = 5.5;
+		}
+		else {
+			key=find("laser_front_laser_resolution");
+			if (key!=end()){
+				resolution=atof(key->second.front().c_str());
+				cerr << "FRONT RES " << resolution << endl;
+			}
+		}
+		
+		laser->m_beams.resize(beam_no);
+		double center_beam=(double)beam_no/2.;
+		uint low_index=(uint)floor(center_beam);
+		uint up_index=(uint)ceil(center_beam);
+		double step=resolution*M_PI/180.;
+		double angle=beam_no%2?0:step;
+		unsigned int i=beam_no%2?0:1;
+		for (; i<low_index+1; i++, angle+=step){
+			beam.span=0;
+			beam.pose.x=0;
+			beam.pose.y=0;
+			beam.s = 1;
+			beam.c = 1;
+			beam.pose.theta=-angle;
+			beam.maxRange=maxrange;
+			laser->m_beams[low_index-i]=beam;
+			beam.pose.theta=angle;
+			laser->m_beams[up_index+i-1]=beam;
+		}
+		laser->updateBeamsLookup();
+		smap.insert(make_pair(laser->getName(), laser));
+		cerr << "front beams " << beam_no << endl;
+		cerr << "maxrange " << maxrange << endl;
+	}
+
+
+	key=find("robot_use_laser");
+	if (key!=end() && key->second.front()=="on"){
+		RangeSensor* laser=new RangeSensor("ROBOTLASER1");
+		laser->newFormat=true;
+		cerr << "ROBOTLASER1 inserted" << endl; 
+		//by default the center of the robot is the center of the laser
+		laser->m_pose.x=laser->m_pose.y=laser->m_pose.theta=0;
+		key=find("robot_frontlaser_offset");
+		if (key!=end()){
+			laser->m_pose.x=atof(key->second.front().c_str());
+			cerr << "FRONT OFFSET=" << laser->m_pose.x << endl;
+		}
+		
+		RangeSensor::Beam beam;
+			
+		//double angle=-.5*M_PI;
+		unsigned int beam_no=180;
+	
+		key=find("laser_beams");
+		if (key!=end()){
+			beam_no=atoi(key->second.front().c_str());
+			cerr << "FRONT BEAMS="<< beam_no << endl;
+		}
+		
+		double maxrange=50;
+		double resolution=1.;
+				
+		
+		if (beam_no==180 || beam_no==181)
+		  resolution =1.;
+		else if (beam_no==360 || beam_no==361)
+		  resolution =.5;
+		else if (beam_no==540 || beam_no==541)
+		  resolution =.5;
+		else if (beam_no==769)
+		  resolution =360./1024.;
+		else if (beam_no==683) {
+		  resolution =360./1024.;
+		   maxrange=5.50;
+		}
+		else {
+			key=find("laser_front_laser_resolution");
+			if (key!=end()){
+				resolution=atof(key->second.front().c_str());
+				cerr << "FRONT RES" << resolution << endl;
+			}
+		}
+		
+		laser->m_beams.resize(beam_no);
+		double center_beam=(double)beam_no/2.;
+		uint low_index=(uint)floor(center_beam);
+		uint up_index=(uint)ceil(center_beam);
+		double step=resolution*M_PI/180.;
+		double angle=beam_no%2?0:step;
+		unsigned int i=beam_no%2?0:1;
+		for (; i<low_index+1; i++, angle+=step){
+			beam.span=0;
+			beam.pose.x=0;
+			beam.pose.y=0;
+			beam.s=0;
+			beam.c=1;
+			beam.pose.theta=-angle;
+			beam.maxRange=maxrange;
+			laser->m_beams[low_index-i]=beam;
+			beam.pose.theta=angle;
+			laser->m_beams[up_index+i-1]=beam;
+		}
+		laser->updateBeamsLookup();
+		smap.insert(make_pair(laser->getName(), laser));
+		cerr << "front beams" << beam_no << endl;
+	}
+
+	
+	//vertical laser
+	key=find("robot_use_rear_laser");
+
+	if (key!=end() && key->second.front()=="on"){
+		RangeSensor* laser=new RangeSensor("RLASER");
+		
+		//by default the center of the robot is the center of the laser
+		laser->m_pose.x=laser->m_pose.y=laser->m_pose.theta=0;
+		laser->m_pose.theta=M_PI;
+		key=find("robot_rearlaser_offset");
+		if (key!=end()){
+			laser->m_pose.x=atof(key->second.front().c_str());
+			cerr << "REAR OFFSET = " << laser->m_pose.x << endl;
+		}
+		
+		
+		
+		RangeSensor::Beam beam;
+			
+		//double angle=-.5*M_PI;
+		unsigned int beam_no=180;
+	
+		key=find("rear_laser_beams");
+		if (key!=end()){
+			beam_no=atoi(key->second.front().c_str());
+			cerr << "REAR BEAMS="<< beam_no << endl;
+		}
+		
+		double maxrange=89;
+		double resolution=1.;
+				
+		
+		if (beam_no==180 || beam_no==181)
+			resolution =1.;
+		else if (beam_no==360 || beam_no==361)
+			resolution =.5;
+		else if (beam_no==540 || beam_no==541)
+		  resolution =.5;
+		else if (beam_no==769)
+			resolution =360./1024.;
+		else {
+			key=find("laser_rear_laser_resolution");
+			if (key!=end()){
+				resolution=atof(key->second.front().c_str());
+				cerr << "REAR RES" << resolution << endl;
+			}
+		}
+		
+		laser->m_beams.resize(beam_no);
+		double center_beam=(double)beam_no/2.;
+		uint low_index=(uint)floor(center_beam);
+		uint up_index=(uint)ceil(center_beam);
+		double step=resolution*M_PI/180.;
+		double angle=beam_no%2?0:step;
+		unsigned int i=beam_no%2?0:1;
+		for (; i<low_index+1; i++, angle+=step){
+			beam.span=0;
+			beam.pose.x=0;
+			beam.pose.y=0;
+			beam.s=0;
+			beam.c=1;
+			beam.pose.theta=-angle;
+			beam.maxRange=maxrange;
+			laser->m_beams[low_index-i]=beam;
+			beam.pose.theta=angle;
+			laser->m_beams[up_index+i-1]=beam;
+		}
+		laser->updateBeamsLookup();
+		smap.insert(make_pair(laser->getName(), laser));
+		cerr<< "rear beams" << beam_no << endl;
+	}
+
+	key=find("robot_use_rear_laser");
+	if (key!=end() && key->second.front()=="on"){
+		RangeSensor* laser=new RangeSensor("ROBOTLASER2");
+		laser->newFormat=true;
+		cerr << "ROBOTLASER2 inserted" << endl; 
+		//by default the center of the robot is the center of the laser
+		laser->m_pose.x=laser->m_pose.y=0;
+		laser->m_pose.theta=M_PI;
+		key=find("robot_rearlaser_offset");
+		if (key!=end()){
+			// laser->m_pose.x==atof(key->second.front().c_str());
+			cerr << "REAR OFFSET not used" << laser->m_pose.x << endl;
+		}
+		
+		RangeSensor::Beam beam;
+			
+		//double angle=-.5*M_PI;
+		unsigned int beam_no=180;
+	
+		key=find("rear_laser_beams");
+		if (key!=end()){
+			beam_no=atoi(key->second.front().c_str());
+			cerr << "REAR BEAMS="<< beam_no << endl;
+		}
+		
+		double maxrange=50;
+		double resolution=1.;
+				
+		
+		if (beam_no==180 || beam_no==181)
+			resolution =1.;
+		else if (beam_no==360 || beam_no==361)
+			resolution =.5;
+		else if (beam_no==540 || beam_no==541)
+		  resolution =.5;
+		else if (beam_no==769)
+			resolution =360./1024.;
+		else {
+			key=find("laser_rear_laser_resolution");
+			if (key!=end()){
+				resolution=atof(key->second.front().c_str());
+				cerr << "REAR RES" << resolution << endl;
+			}
+		}
+		
+		laser->m_beams.resize(beam_no);
+		double center_beam=(double)beam_no/2.;
+		uint low_index=(uint)floor(center_beam);
+		uint up_index=(uint)ceil(center_beam);
+		double step=resolution*M_PI/180.;
+		double angle=beam_no%2?0:step;
+		unsigned int i=beam_no%2?0:1;
+		for (; i<low_index+1; i++, angle+=step){
+			beam.span=0;
+			beam.s=0;
+			beam.c=1;
+			beam.pose.x=0;
+			beam.pose.y=0;
+			beam.pose.theta=-angle;
+			beam.maxRange=maxrange;
+			laser->m_beams[low_index-i]=beam;
+			beam.pose.theta=angle;
+			laser->m_beams[up_index+i-1]=beam;
+		}
+		laser->updateBeamsLookup();
+		smap.insert(make_pair(laser->getName(), laser));
+		cerr << "rear beams" << beam_no << endl;
+	}
+
+
+	return smap;
+}
+
+};
+

slam_gmapping/openslam_gmapping/log/carmenconfiguration.h

@@ -0,0 +1,22 @@
+#ifndef CARMENCONFIGURATION_H
+#define CARMENCONFIGURATION_H
+
+#include <string>
+#include <map>
+#include <vector>
+#include <istream>
+#include <sensor/sensor_base/sensor.h>
+#include "configuration.h"
+
+namespace GMapping {
+
+class CarmenConfiguration: public Configuration, public std::map<std::string, std::vector<std::string> >{
+	public:
+		virtual std::istream& load(std::istream& is);
+		virtual SensorMap computeSensorMap() const;
+};
+
+};
+
+#endif
+

slam_gmapping/openslam_gmapping/log/configuration.cpp

@@ -0,0 +1,8 @@
+#include "configuration.h"
+
+namespace GMapping {
+
+Configuration::~Configuration(){
+}
+
+};

slam_gmapping/openslam_gmapping/log/configuration.h

@@ -0,0 +1,17 @@
+#ifndef CONFIGURATION_H
+#define CONFIGURATION_H
+
+#include <istream>
+#include <sensor/sensor_base/sensor.h>
+
+namespace GMapping {
+
+class Configuration{
+	public:
+		virtual ~Configuration();
+		virtual SensorMap computeSensorMap() const=0;
+};
+
+};
+#endif
+

slam_gmapping/openslam_gmapping/log/log_plot.cpp

@@ -0,0 +1,71 @@
+#include <cstdlib>
+#include <fstream>
+#include <iostream>
+#include <sys/types.h>
+#include <log/carmenconfiguration.h>
+#include <log/sensorlog.h>
+
+
+using namespace std;
+using namespace GMapping;
+
+int main(int argc, char ** argv){
+  double maxrange=2.;
+	if (argc<2){
+		cout << "usage log_plot <filename> | gnuplot" << endl;
+		exit (-1);
+	}
+	ifstream is(argv[1]);
+	if (! is){
+		cout << "no file " << argv[1] << " found" << endl;
+		exit (-1);
+	}
+	CarmenConfiguration conf;
+	conf.load(is);
+	
+	SensorMap m=conf.computeSensorMap();
+	
+	//for (SensorMap::const_iterator it=m.begin(); it!=m.end(); it++)
+	//	cout << it->first << " " << it->second->getName() << endl;
+	
+	SensorLog log(m);
+	is.close();
+	
+	ifstream ls(argv[1]);
+	log.load(ls);
+	ls.close();
+	int count=0;
+	int frame=0;
+	cerr << "log size" << log.size() << endl;
+	for (SensorLog::iterator it=log.begin(); it!=log.end(); it++){
+		RangeReading* rr=dynamic_cast<RangeReading*>(*it);
+		if (rr){
+		  count++;
+		  if (count%3)
+		    continue;
+		  std::vector<Point> points(rr->size());
+		  uint j=0;
+		  for (uint i=0; i<rr->size(); i++){
+		    const RangeSensor * rs=dynamic_cast<const RangeSensor*>(rr->getSensor());
+		    double c=rs->beams()[i].c, s=rs->beams()[i].s;
+		    double r=(*rr)[i];
+		    if (r>maxrange)
+		      continue;
+		    points[j++]=Point(r*c,r*s);
+		  }
+		  if (j){
+		    char buf[1024];
+		    sprintf(buf,"frame-%05d.gif",frame);
+		    frame++;
+		    cout << "set terminal gif" << endl;
+		    cout << "set output \"" << buf << "\"" << endl;
+		    cout << "set size ratio -1" << endl;
+		    cout << "plot [-3:3][0:3] '-' w p ps 1" << endl;
+		    for (uint i=0; i<j; i++){
+		      cout << points[i].y << " " << points[i].x << endl;
+		  }
+		  cout << "e" << endl;
+		  }
+		}
+	}
+}

slam_gmapping/openslam_gmapping/log/log_test.cpp

@@ -0,0 +1,47 @@
+#include <cstdlib>
+#include <fstream>
+#include <iostream>
+#include <log/carmenconfiguration.h>
+#include <log/sensorlog.h>
+
+
+using namespace std;
+using namespace GMapping;
+
+int main(int argc, char ** argv){
+	if (argc<2){
+		cout << "usage log_test <filename>" << endl;
+		exit (-1);
+	}
+	ifstream is(argv[1]);
+	if (! is){
+		cout << "no file " << argv[1] << " found" << endl;
+		exit (-1);
+	}
+	CarmenConfiguration conf;
+	conf.load(is);
+	
+	SensorMap m=conf.computeSensorMap();
+	
+	//for (SensorMap::const_iterator it=m.begin(); it!=m.end(); it++)
+	//	cout << it->first << " " << it->second->getName() << endl;
+	
+	SensorLog log(m);
+	is.close();
+	
+	ifstream ls(argv[1]);
+	log.load(ls);
+	ls.close();
+	cerr << "log size" << log.size() << endl;
+	for (SensorLog::iterator it=log.begin(); it!=log.end(); it++){
+		RangeReading* rr=dynamic_cast<RangeReading*>(*it);
+		if (rr){
+			//cerr << rr->getSensor()->getName() << " ";
+			//cerr << rr->size()<< " ";
+			//for (RangeReading::const_iterator it=rr->begin(); it!=rr->end(); it++){
+			//	cerr << *it << " ";
+			//}
+			cout<< rr->getPose().x << " " << rr->getPose().y << " " << rr->getPose().theta << " " << rr->getTime()  << endl;
+		}
+	}
+}

slam_gmapping/openslam_gmapping/log/rdk2carmen.cpp

@@ -0,0 +1,58 @@
+#include <cstdlib>
+#include <fstream>
+#include <iostream>
+#include <log/carmenconfiguration.h>
+#include <log/sensorlog.h>
+
+
+using namespace std;
+using namespace GMapping;
+
+int main(int argc, char ** argv){
+	if (argc<2){
+		cerr << "usage "<<argv[0]<<" <filename> <outfilename>" << endl;
+		cerr << "or "<<argv[0]<<" <filename> for standard output" << endl;
+		exit (-1);
+	}
+	ifstream is(argv[1]);
+	if (! is){
+		cerr << "no file " << argv[1] << " found" << endl;
+		exit (-1);
+	}
+	ostream *os;
+	if (argc<3) 
+		os=&cout;
+	else{
+		os=new ofstream(argv[2]);
+		if (! os){
+			cerr << "no file " << argv[1] << " found" << endl;
+			exit (-1);
+		}
+	}
+	CarmenConfiguration conf;
+	conf.load(is);
+	
+	SensorMap m=conf.computeSensorMap();
+	
+	//for (SensorMap::const_iterator it=m.begin(); it!=m.end(); it++)
+	//	cout << it->first << " " << it->second->getName() << endl;
+	
+	SensorLog log(m);
+	is.close();
+	
+	ifstream ls(argv[1]);
+	log.load(ls);
+	ls.close();
+	cerr << "log size" << log.size() << endl;
+	for (SensorLog::iterator it=log.begin(); it!=log.end(); it++){
+		RangeReading* rr=dynamic_cast<RangeReading*>(*it);
+		if (rr){
+			*os << rr->getSensor()->getName() << " ";
+			*os << rr->size()<< " ";
+			for (RangeReading::const_iterator it=rr->begin(); it!=rr->end(); it++){
+				*os << (*it)*0.001 << " ";
+			}
+			*os<< rr->getPose().x*0.001 << " " << rr->getPose().y*0.001 << " " << rr->getPose().theta << endl;
+		}
+	}
+}

slam_gmapping/openslam_gmapping/log/scanstudio2carmen.cpp

@@ -0,0 +1,66 @@
+#include <cstdlib>
+#include <iostream>
+#include <fstream>
+#include <sstream>
+#include <assert.h>
+#include <utils/point.h>
+
+#define MAXLINELENGHT (10240)
+#define MAXREADINGS   (10240)
+
+using namespace std;
+using namespace GMapping;
+
+int  main (int argc, char** argv){
+	if (argc<3){
+		cout << "usage scanstudio2carmen scanfilename carmenfilename" << endl;
+		exit(1);
+	}
+	ifstream is(argv[1]);
+	if (!is){
+		cout << "cannopt open file" << argv[1] << endl;
+		exit(1);
+	}
+	
+	ofstream os(argv[2]);
+	
+	double readings[MAXREADINGS];
+	OrientedPoint pose;
+	int nbeams;
+	while (is){
+		char buf[MAXLINELENGHT];
+		is.getline(buf,MAXLINELENGHT);
+		istringstream st(buf);
+		string token;
+		st>>token;
+		if (token=="RobotPos:"){
+			st >> pose.x >> pose.y >> pose.theta;
+			pose.x/=1000;
+			pose.y/=1000;
+		} else
+		if (token=="NumPoints:"){
+			st >> nbeams;
+			assert(nbeams<MAXREADINGS);
+		} else
+		if (token=="DATA"){
+			int c=0;
+			while (c<nbeams && is){
+				double angle;
+				is >> angle;
+				is >> readings[c];
+				readings[c]/=1000;
+				c++;
+			}
+			if (c==nbeams)
+				os << "FLASER " << nbeams << " ";
+			c=0;
+			while (c<nbeams){
+				os << readings[c] << " ";
+				c++;
+			}	
+			os << pose.x << " " << pose.y << " " << pose.theta << "0 0 0 0 pippo 0" << endl;
+		}
+		
+	}
+	os.close();
+}

slam_gmapping/openslam_gmapping/log/sensorlog.cpp

@@ -0,0 +1,153 @@
+#include "sensorlog.h"
+
+#include <iostream>
+#include <sstream>
+#include <assert.h>
+#include <sensor_odometry/odometrysensor.h>
+#include <sensor_range/rangesensor.h>
+
+#define LINEBUFFER_SIZE 100000
+
+namespace GMapping {
+
+using namespace std;
+
+SensorLog::SensorLog(const SensorMap& sm): m_sensorMap(sm){
+}
+
+SensorLog::~SensorLog(){
+	for (iterator it=begin(); it!=end(); it++)
+		if (*it) delete (*it);
+}
+
+istream& SensorLog::load(istream& is){
+	for (iterator it=begin(); it!=end(); it++)
+		if (*it) delete (*it);
+	clear();
+	
+	char buf[LINEBUFFER_SIZE];
+	while (is){
+		is.getline(buf, LINEBUFFER_SIZE);
+		istringstream lis(buf);
+		
+		string sensorname;
+		
+		if (lis) 
+			lis >>sensorname; 
+		else 
+			continue;
+		
+		
+			
+		SensorMap::const_iterator it=m_sensorMap.find(sensorname);
+		if (it==m_sensorMap.end()){
+			continue;
+		}
+		
+		Sensor* sensor=it->second;
+		
+		SensorReading* reading=0;
+		OdometrySensor* odometry=dynamic_cast<OdometrySensor*>(sensor);
+		if (odometry)
+			reading=parseOdometry(lis, odometry);
+		
+		RangeSensor* range=dynamic_cast<RangeSensor*>(sensor);
+		if (range)
+			reading=parseRange(lis, range);
+		if (reading)
+			push_back(reading);
+	}
+	return is;
+	
+}
+
+OdometryReading* SensorLog::parseOdometry(istream& is, const OdometrySensor* osen) const{
+	OdometryReading* reading=new OdometryReading(osen);
+	OrientedPoint pose;
+	OrientedPoint speed;
+	OrientedPoint accel;
+	is >> pose.x >> pose.y >> pose.theta;
+	is >> speed.x >>speed.theta;
+	speed.y=0;
+	is >> accel.x;
+	accel.y=accel.theta=0;
+	reading->setPose(pose); reading->setSpeed(speed); reading->setAcceleration(accel);
+	return reading;
+}
+
+RangeReading* SensorLog::parseRange(istream& is, const RangeSensor* rs) const{
+	if(rs->newFormat){
+		string laser_type, start_angle, field_of_view, angular_resolution, maximum_range, accuracy, remission_mode;
+		is >> laser_type>> start_angle>> field_of_view>> angular_resolution>> maximum_range>> accuracy >> remission_mode;
+	}
+	
+	unsigned int size;
+	is >> size;
+	assert(size==rs->beams().size());
+	
+	RangeReading* reading=new RangeReading(rs);
+	//cerr << "#R=" << size << endl;
+	reading->resize(size);
+	for (unsigned int i=0; i<size; i++){
+		is >> (*reading)[i];
+	}
+	if (rs->newFormat){
+		int reflectionBeams;
+		is >> reflectionBeams;
+		double reflection;
+		for (int i=0; i<reflectionBeams; i++)
+			is >> reflection;
+	}
+	//FIXME XXX
+	OrientedPoint laserPose;
+	is >> laserPose.x >> laserPose.y >> laserPose.theta;
+	OrientedPoint pose;
+	is >> pose.x >> pose.y >> pose.theta; 
+	reading->setPose(pose);
+	double a,b,c;
+	if (rs->newFormat){
+		string laser_tv, laser_rv, forward_safety_dist, side_safty_dist, turn_axis;
+		is >> laser_tv >> laser_rv >> forward_safety_dist >> side_safty_dist >> turn_axis;
+	} else {
+		is >> a >> b >> c;
+	}
+	string s;
+	is >> a >> s;
+	is >> a;
+	reading->setTime(a);
+	return reading;
+}
+
+OrientedPoint SensorLog::boundingBox(double& xmin, double& ymin, double& xmax, double& ymax) const {
+	xmin=ymin=1e6;
+	xmax=ymax=-1e6;
+	bool first=true;
+	OrientedPoint start;
+	for (const_iterator it=begin(); it!=end(); it++){
+		double lxmin=0., lxmax=0., lymin=0., lymax=0.;
+		const SensorReading* reading=*it;
+		const OdometryReading* odometry=dynamic_cast<const OdometryReading*> (reading);
+		if (odometry){
+			lxmin=lxmax=odometry->getPose().x;
+			lymin=lymax=odometry->getPose().y;
+		}
+		
+		const RangeReading* rangeReading=dynamic_cast<const RangeReading*> (reading);
+		if (rangeReading){
+			lxmin=lxmax=rangeReading->getPose().x;
+			lymin=lymax=rangeReading->getPose().y;
+			if (first){
+				first=false;
+				start=rangeReading->getPose();	
+			}
+		}
+		xmin=xmin<lxmin?xmin:lxmin;
+		xmax=xmax>lxmax?xmax:lxmax;
+		ymin=ymin<lymin?lymin:lymin;
+		ymax=ymax>lymax?ymax:lymax;
+	}
+	return start;
+}
+
+};
+

slam_gmapping/openslam_gmapping/log/sensorlog.h

@@ -0,0 +1,29 @@
+#ifndef SENSORLOG_H
+#define SENSORLOG_H
+
+#include <list>
+#include <istream>
+#include <sensor/sensor_base/sensorreading.h>
+#include <sensor/sensor_odometry/odometrysensor.h>
+#include <sensor/sensor_range/rangesensor.h>
+#include <sensor/sensor_odometry/odometryreading.h>
+#include <sensor/sensor_range/rangereading.h>
+#include "configuration.h"
+
+namespace GMapping {
+
+class SensorLog : public std::list<SensorReading*>{
+	public:
+		SensorLog(const SensorMap&);
+		~SensorLog();
+		std::istream& load(std::istream& is);
+		OrientedPoint boundingBox(double& xmin, double& ymin, double& xmax, double& ymax) const;
+	protected:
+		const SensorMap& m_sensorMap;
+		OdometryReading* parseOdometry(std::istream& is, const OdometrySensor* ) const;
+		RangeReading* parseRange(std::istream& is, const RangeSensor* ) const;
+};
+
+};
+
+#endif

slam_gmapping/openslam_gmapping/log/sensorstream.cpp

@@ -0,0 +1,155 @@
+#include <assert.h>
+#include <sstream>
+#include "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.
+#define LINEBUFFER_SIZE 8192
+
+namespace GMapping {
+
+using namespace std;
+
+//SensorStream
+SensorStream::SensorStream(const SensorMap& sensorMap) :m_sensorMap(sensorMap){}
+
+SensorStream::~SensorStream(){}
+
+SensorReading* SensorStream::parseReading(std::istream& is, const SensorMap& smap){
+	SensorReading* reading=0;
+	if (is){
+		char buf[LINEBUFFER_SIZE];
+		is.getline(buf, LINEBUFFER_SIZE);
+		istringstream lis(buf);
+		
+		string sensorname;
+		
+		if (lis){
+			lis >>sensorname; 
+		} else 
+			return 0;
+		
+		SensorMap::const_iterator it=smap.find(sensorname);
+		if (it==smap.end()){
+			return 0;
+		}
+		
+		Sensor* sensor=it->second;
+		
+		OdometrySensor* odometry=dynamic_cast<OdometrySensor*>(sensor);
+		if (odometry)
+			reading=parseOdometry(lis, odometry);
+		
+		RangeSensor* range=dynamic_cast<RangeSensor*>(sensor);
+		if (range)
+			reading=parseRange(lis, range);
+	}
+	return reading;
+}
+
+OdometryReading* SensorStream::parseOdometry(std::istream& is, const OdometrySensor* osen ){
+	OdometryReading* reading=new OdometryReading(osen);
+	OrientedPoint pose;
+	OrientedPoint speed;
+	OrientedPoint accel;
+	is >> pose.x >> pose.y >> pose.theta;
+	is >> speed.x >>speed.theta;
+	speed.y=0;
+	is >> accel.x;
+	accel.y=accel.theta=0;
+	reading->setPose(pose); reading->setSpeed(speed); reading->setAcceleration(accel);
+	double timestamp, reltimestamp;
+	string s;
+	is >> timestamp >>s >> reltimestamp;
+	reading->setTime(timestamp);
+	return reading;
+}
+
+RangeReading* SensorStream::parseRange(std::istream& is, const RangeSensor* rs){
+	//cerr << __PRETTY_FUNCTION__ << endl;
+	if(rs->newFormat){
+		string laser_type, start_angle, field_of_view, angular_resolution, maximum_range, accuracy, remission_mode;
+		is >> laser_type>> start_angle>> field_of_view>> angular_resolution>> maximum_range>> accuracy>> remission_mode;
+		//cerr << " New format laser msg" << endl;
+	}
+	unsigned int size;
+	is >> size;
+	assert(size==rs->beams().size());
+	RangeReading* reading=new RangeReading(rs);
+	reading->resize(size);
+	for (unsigned int i=0; i<size; i++){
+		is >> (*reading)[i];
+	}
+	if (rs->newFormat){
+		int reflectionBeams;
+		is >> reflectionBeams;
+		double reflection;
+		for (int i=0; i<reflectionBeams; i++)
+			is >> reflection;
+	}
+	OrientedPoint laserPose;
+	is >> laserPose.x >> laserPose.y >> laserPose.theta;
+	OrientedPoint pose;
+	is >> pose.x >> pose.y >> pose.theta;
+	reading->setPose(pose);
+
+	if (rs->newFormat){
+		string laser_tv, laser_rv, forward_safety_dist, side_safty_dist, turn_axis;
+		is >> laser_tv >> laser_rv >> forward_safety_dist >> side_safty_dist >> turn_axis;
+	} 
+// 	else {
+// 	  double a,b,c;
+// 		is >> a >> b >> c;
+// 	}
+	double timestamp, reltimestamp;
+	string s;
+	is >> timestamp >>s >> reltimestamp;
+	reading->setTime(timestamp);
+	return reading;
+
+}
+
+//LogSensorStream
+LogSensorStream::LogSensorStream(const SensorMap& sensorMap, const SensorLog* log):
+	SensorStream(sensorMap){
+	m_log=log;
+	assert(m_log);
+	m_cursor=log->begin();
+}
+
+LogSensorStream::operator bool() const{
+	return m_cursor==m_log->end();
+}
+
+bool LogSensorStream::rewind(){
+	m_cursor=m_log->begin();
+	return true;
+}
+
+SensorStream& LogSensorStream::operator >>(const SensorReading*& rd){
+	rd=*m_cursor;
+	m_cursor++;
+	return *this;
+}
+
+//InputSensorStream
+InputSensorStream::InputSensorStream(const SensorMap& sensorMap, std::istream& is):
+	SensorStream(sensorMap), m_inputStream(is){
+}
+
+InputSensorStream::operator bool() const{
+	return (bool) m_inputStream;
+}
+
+bool InputSensorStream::rewind(){
+	//m_inputStream.rewind();
+	return false;
+}
+
+SensorStream& InputSensorStream::operator >>(const SensorReading*& reading){
+	reading=parseReading(m_inputStream, m_sensorMap);
+	return *this;
+}
+
+};
+

slam_gmapping/openslam_gmapping/log/sensorstream.h

@@ -0,0 +1,47 @@
+#ifndef SENSORSTREAM_H
+#define SENSORSTREAM_H
+
+#include <istream>
+#include "sensorlog.h"
+
+namespace GMapping {
+class SensorStream{
+	public:
+		SensorStream(const SensorMap& sensorMap);
+		virtual ~SensorStream();
+		virtual operator bool() const=0;
+		virtual bool rewind() = 0 ;
+		virtual SensorStream& operator >>(const SensorReading*&) = 0;
+		inline const SensorMap& getSensorMap() const {return m_sensorMap; }
+	protected:
+		const SensorMap& m_sensorMap;
+		static SensorReading* parseReading(std::istream& is, const SensorMap& smap);
+		static OdometryReading* parseOdometry(std::istream& is, const OdometrySensor* );
+		static RangeReading* parseRange(std::istream& is, const RangeSensor* );
+};
+
+class InputSensorStream: public SensorStream{
+	public:
+		InputSensorStream(const SensorMap& sensorMap, std::istream& is);
+		virtual operator bool() const;
+		virtual bool rewind();
+		virtual SensorStream& operator >>(const SensorReading*&);
+		
+		//virtual SensorStream& operator >>(SensorLog*& log);
+	protected:
+		std::istream& m_inputStream;
+};
+
+class LogSensorStream: public SensorStream{
+	public:
+		LogSensorStream(const SensorMap& sensorMap, const SensorLog* log);
+		virtual operator bool() const;
+		virtual bool rewind();
+		virtual SensorStream& operator >>(const SensorReading*&);
+	protected:
+		const SensorLog* m_log;
+		SensorLog::const_iterator m_cursor;
+};
+
+};
+#endif

slam_gmapping/openslam_gmapping/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="2">
+  <name>openslam_gmapping</name>
+  <version>0.1.2</version>
+  <description>ROS-ified version of gmapping SLAM. Forked from https://openslam.informatik.uni-freiburg.de/data/svn/gmapping/trunk/</description>
+  <maintainer email="vincent.rabaud@gmail.com">Vincent Rabaud</maintainer>
+  <license>CreativeCommons-by-nc-sa-2.0</license>
+
+  <url type="website">http://openslam.org/gmapping</url>
+  <url type="repository">https://github.com/ros-perception/openslam_gmapping</url>
+  <url type="bugtracker">https://github.com/ros-perception/openslam_gmapping/issues</url>
+
+  <author>Giorgio Grisetti</author>
+  <author>Cyrill Stachniss</author>
+  <author>Wolfram Burgard</author>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+
+</package>

slam_gmapping/openslam_gmapping/particlefilter/particlefilter.cpp

@@ -0,0 +1,133 @@
+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();
+}

slam_gmapping/openslam_gmapping/particlefilter/particlefilter_test.cpp

@@ -0,0 +1,98 @@
+#include <stdlib.h>
+#include <iostream>
+#include <fstream>
+#include "particlefilter.h"
+
+using namespace std;
+
+#define test(s) {cout << s <<  " " << flush;}
+#define testOk() {cout << "OK" << endl;}
+
+struct Particle{
+	double p;
+	double w;
+	inline operator double() const {return w;}
+	inline void setWeight(double _w) {w=_w;}
+};
+
+ostream& printParticles(ostream& os, const vector<Particle>& p)
+{
+	for (vector<Particle>::const_iterator it=p.begin(); it!=p.end(); ++it) {
+		os << it->p<< " " << (double)*it << endl;
+	}
+	return os;
+}
+
+struct EvolutionModel{
+	Particle evolve(const Particle& p){
+		Particle pn(p);
+		pn.p+=.5*(drand48()-.5);
+		return pn;
+	}
+};
+
+struct QualificationModel{
+	Particle evolve(const Particle& p){
+		return p;
+	}
+};
+
+struct LikelyhoodModel{
+	double likelyhood(const Particle& p) const{
+		double v = 1./(0.1+10*(p.p-2)*(p.p-2))+0.5/(0.1+10*(p.p-8)*(p.p-8));
+		return v;
+	}
+};
+
+int main (unsigned int argc, const char * const * argv){
+	int nparticles=100;
+	if (argc>1)
+		nparticles=atoi(argv[1]);
+	vector<Particle> particles(nparticles);
+	LikelyhoodModel likelyhoodModel;
+	uniform_resampler<Particle, double> resampler;
+	auxiliary_evolver <Particle, double, QualificationModel, EvolutionModel, LikelyhoodModel> auxevolver;
+	evolver <Particle, EvolutionModel> evolver;
+
+	for (vector<Particle>::iterator it=particles.begin(); it!=particles.end(); it++){
+		it->w=1;
+		it->p=10*(drand48());
+	}
+
+	vector<Particle> sirparticles(particles);
+	vector<Particle> auxparticles(particles);
+
+	/*sir step*/
+	while (1){
+		char buf[2];
+		cin.getline(buf,2);
+		vector<Particle> newgeneration;
+
+		cout << "# SIR step" << endl;
+		evolver.evolve(sirparticles);
+		for (vector<Particle>::iterator it=sirparticles.begin(); it!=sirparticles.end(); it++){
+			it->setWeight(likelyhoodModel.likelyhood(*it));
+		}
+		ofstream os("sir.dat");
+		printParticles(os, sirparticles);
+		os.close();
+		newgeneration=resampler.resample(sirparticles);
+		sirparticles=newgeneration;
+
+		cout << "# AUX step" << endl;
+		auxevolver.evolve(auxparticles);
+		for (vector<Particle>::iterator it=auxparticles.begin(); it!=auxparticles.end(); it++){
+			it->setWeight(likelyhoodModel.likelyhood(*it));
+		}
+		os.open("aux.dat");
+		printParticles(os, auxparticles);
+		os.close();
+		newgeneration=resampler.resample(auxparticles);
+		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;
+	}
+}
+

slam_gmapping/openslam_gmapping/particlefilter/pf.h

@@ -0,0 +1,175 @@
+#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
+

slam_gmapping/openslam_gmapping/particlefilter/range_bearing.cpp

@@ -0,0 +1,99 @@
+#include <stdlib.h>
+#include <iostream>
+#include <fstream>
+#include <utils/point.h>
+#include "particlefilter.h"
+
+using namespace std;
+using namespace GMapping;
+
+#define test(s) {cout << s <<  " " << flush;}
+#define testOk() {cout << "OK" << endl;}
+
+struct Particle{
+	Particle(): p(0,0), w(0){}
+	Point p;
+	double w;
+	operator double() const {return w; }
+	void setWeight(double _w) {w=_w;}
+};
+
+ostream& printParticles(ostream& os, const vector<Particle>& p)
+{
+	for (vector<Particle>::const_iterator it=p.begin(); it!=p.end(); ++it) {
+		os << it->p.x << " " << it->p.y << endl;
+	}
+	return os;
+}
+
+struct EvolutionModel{
+	Particle evolve(const Particle& p){
+		Particle pn(p);
+		pn.p.x+=10*(drand48()-.5);
+		pn.p.y+=10*(drand48()-.5);
+		return pn;
+	}
+};
+
+
+struct LikelyhoodModel{
+	std::vector<Point> observerVector;
+	std::vector<double> observations;
+	double sigma;
+	double likelyhood(const Particle& p) const{
+		double v=1;
+		std::vector<double>::const_iterator oit=observations.begin();
+		for (std::vector<Point>::const_iterator it=observerVector.begin(); it!=observerVector.end();it++){
+			v*=exp(-pow(((p.p-*it)*(p.p-*it)-*oit*(*oit))/sigma, 2));
+			oit++;
+		}
+		cout << "#v=" << v << endl;
+		return v;
+	}
+};
+
+int main (unsigned int argc, const char * const * argv){
+	vector<Particle> particles(1000);
+	LikelyhoodModel likelyhoodModel;
+	uniform_resampler<Particle, double> resampler;
+	evolver <Particle, EvolutionModel> evolver;
+
+	for (vector<Particle>::iterator it=particles.begin(); it!=particles.end(); it++){
+		it->w=1;
+		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];
+		cin.getline(buf,2);
+		vector<Particle> newgeneration;
+
+		cout << "# SIR step" << endl;
+		evolver.evolve(particles);
+		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();
+		vector<Particle> newpart=resampler.resample(particles);
+		particles=newpart;
+
+		cout << "plot [-200:200][-200:200]\"sir.dat\" w p" << endl;
+	}
+}
+

slam_gmapping/openslam_gmapping/scanmatcher/CMakeLists.txt

@@ -0,0 +1,4 @@
+add_library(scanmatcher STATIC eig3.cpp scanmatcher.cpp scanmatcherprocessor.cpp smmap.cpp)
+target_link_libraries(scanmatcher sensor_range utils)
+
+install(TARGETS scanmatcher DESTINATION lib)

slam_gmapping/openslam_gmapping/scanmatcher/eig3.cpp

@@ -0,0 +1,270 @@
+
+/* Eigen decomposition code for symmetric 3x3 matrices, copied from the public
+   domain Java Matrix library JAMA. */
+
+#include <math.h>
+
+#ifndef MAX
+#define MAX(a, b) ((a)>(b)?(a):(b))
+#endif
+
+#define n 3
+
+static double hypot2(double x, double y) {
+  return sqrt(x*x+y*y);
+}
+
+// Symmetric Householder reduction to tridiagonal form.
+
+static void tred2(double V[n][n], double d[n], double e[n]) {
+
+//  This is derived from the Algol procedures tred2 by
+//  Bowdler, Martin, Reinsch, and Wilkinson, Handbook for
+//  Auto. Comp., Vol.ii-Linear Algebra, and the corresponding
+//  Fortran subroutine in EISPACK.
+
+  int i,j,k;
+  double f,g,h,hh;
+  for (j = 0; j < n; j++) {
+    d[j] = V[n-1][j];
+  }
+
+  // Householder reduction to tridiagonal form.
+
+  for (i = n-1; i > 0; i--) {
+
+    // Scale to avoid under/overflow.
+
+    double scale = 0.0;
+    double h = 0.0;
+    for (k = 0; k < i; k++) {
+      scale = scale + fabs(d[k]);
+    }
+    if (scale == 0.0) {
+      e[i] = d[i-1];
+      for (j = 0; j < i; j++) {
+        d[j] = V[i-1][j];
+        V[i][j] = 0.0;
+        V[j][i] = 0.0;
+      }
+    } else {
+
+      // Generate Householder vector.
+
+      for (k = 0; k < i; k++) {
+        d[k] /= scale;
+        h += d[k] * d[k];
+      }
+      f = d[i-1];
+      g = sqrt(h);
+      if (f > 0) {
+        g = -g;
+      }
+      e[i] = scale * g;
+      h = h - f * g;
+      d[i-1] = f - g;
+      for (j = 0; j < i; j++) {
+        e[j] = 0.0;
+      }
+
+      // Apply similarity transformation to remaining columns.
+
+      for (j = 0; j < i; j++) {
+        f = d[j];
+        V[j][i] = f;
+        g = e[j] + V[j][j] * f;
+        for (k = j+1; k <= i-1; k++) {
+          g += V[k][j] * d[k];
+          e[k] += V[k][j] * f;
+        }
+        e[j] = g;
+      }
+      f = 0.0;
+      for (j = 0; j < i; j++) {
+        e[j] /= h;
+        f += e[j] * d[j];
+      }
+      hh = f / (h + h);
+      for (j = 0; j < i; j++) {
+        e[j] -= hh * d[j];
+      }
+      for (j = 0; j < i; j++) {
+        f = d[j];
+        g = e[j];
+        for (k = j; k <= i-1; k++) {
+          V[k][j] -= (f * e[k] + g * d[k]);
+        }
+        d[j] = V[i-1][j];
+        V[i][j] = 0.0;
+      }
+    }
+    d[i] = h;
+  }
+
+  // Accumulate transformations.
+
+  for (i = 0; i < n-1; i++) {
+    V[n-1][i] = V[i][i];
+    V[i][i] = 1.0;
+    h = d[i+1];
+    if (h != 0.0) {
+      for (k = 0; k <= i; k++) {
+        d[k] = V[k][i+1] / h;
+      }
+      for (j = 0; j <= i; j++) {
+        g = 0.0;
+        for (k = 0; k <= i; k++) {
+          g += V[k][i+1] * V[k][j];
+        }
+        for (k = 0; k <= i; k++) {
+          V[k][j] -= g * d[k];
+        }
+      }
+    }
+    for (k = 0; k <= i; k++) {
+      V[k][i+1] = 0.0;
+    }
+  }
+  for (j = 0; j < n; j++) {
+    d[j] = V[n-1][j];
+    V[n-1][j] = 0.0;
+  }
+  V[n-1][n-1] = 1.0;
+  e[0] = 0.0;
+} 
+
+// Symmetric tridiagonal QL algorithm.
+
+static void tql2(double V[n][n], double d[n], double e[n]) {
+
+//  This is derived from the Algol procedures tql2, by
+//  Bowdler, Martin, Reinsch, and Wilkinson, Handbook for
+//  Auto. Comp., Vol.ii-Linear Algebra, and the corresponding
+//  Fortran subroutine in EISPACK.
+
+  int i,j,m,l,k;
+  double g,p,r,dl1,h,f,tst1,eps;
+  double c,c2,c3,el1,s,s2;
+
+  for (i = 1; i < n; i++) {
+    e[i-1] = e[i];
+  }
+  e[n-1] = 0.0;
+
+  f = 0.0;
+  tst1 = 0.0;
+  eps = pow(2.0,-52.0);
+  for (l = 0; l < n; l++) {
+
+    // Find small subdiagonal element
+
+    tst1 = MAX(tst1,fabs(d[l]) + fabs(e[l]));
+    m = l;
+    while (m < n) {
+      if (fabs(e[m]) <= eps*tst1) {
+        break;
+      }
+      m++;
+    }
+
+    // If m == l, d[l] is an eigenvalue,
+    // otherwise, iterate.
+
+    if (m > l) {
+      int iter = 0;
+      do {
+        iter = iter + 1;  // (Could check iteration count here.)
+
+        // Compute implicit shift
+
+        g = d[l];
+        p = (d[l+1] - g) / (2.0 * e[l]);
+        r = hypot2(p,1.0);
+        if (p < 0) {
+          r = -r;
+        }
+        d[l] = e[l] / (p + r);
+        d[l+1] = e[l] * (p + r);
+        dl1 = d[l+1];
+        h = g - d[l];
+        for (i = l+2; i < n; i++) {
+          d[i] -= h;
+        }
+        f = f + h;
+
+        // Implicit QL transformation.
+
+        p = d[m];
+        c = 1.0;
+        c2 = c;
+        c3 = c;
+        el1 = e[l+1];
+        s = 0.0;
+        s2 = 0.0;
+        for (i = m-1; i >= l; i--) {
+          c3 = c2;
+          c2 = c;
+          s2 = s;
+          g = c * e[i];
+          h = c * p;
+          r = hypot2(p,e[i]);
+          e[i+1] = s * r;
+          s = e[i] / r;
+          c = p / r;
+          p = c * d[i] - s * g;
+          d[i+1] = h + s * (c * g + s * d[i]);
+
+          // Accumulate transformation.
+
+          for (k = 0; k < n; k++) {
+            h = V[k][i+1];
+            V[k][i+1] = s * V[k][i] + c * h;
+            V[k][i] = c * V[k][i] - s * h;
+          }
+        }
+        p = -s * s2 * c3 * el1 * e[l] / dl1;
+        e[l] = s * p;
+        d[l] = c * p;
+
+        // Check for convergence.
+
+      } while (fabs(e[l]) > eps*tst1);
+    }
+    d[l] = d[l] + f;
+    e[l] = 0.0;
+  }
+  
+  // Sort eigenvalues and corresponding vectors.
+
+  for (i = 0; i < n-1; i++) {
+    k = i;
+    p = d[i];
+    for (j = i+1; j < n; j++) {
+      if (d[j] < p) {
+        k = j;
+        p = d[j];
+      }
+    }
+    if (k != i) {
+      d[k] = d[i];
+      d[i] = p;
+      for (j = 0; j < n; j++) {
+        p = V[j][i];
+        V[j][i] = V[j][k];
+        V[j][k] = p;
+      }
+    }
+  }
+}
+
+void eigen_decomposition(double A[n][n], double V[n][n], double d[n]) {
+  int i,j;
+  double e[n];
+  for (i = 0; i < n; i++) {
+    for (j = 0; j < n; j++) {
+      V[i][j] = A[i][j];
+    }
+  }
+  tred2(V, d, e);
+  tql2(V, d, e);
+}

slam_gmapping/openslam_gmapping/scanmatcher/eig3.h

@@ -0,0 +1,11 @@
+
+/* Eigen-decomposition for symmetric 3x3 real matrices.
+   Public domain, copied from the public domain Java library JAMA. */
+
+#ifndef _eig_h
+
+/* Symmetric matrix A => eigenvectors in columns of V, corresponding
+   eigenvalues in d. */
+void eigen_decomposition(double A[3][3], double V[3][3], double d[3]);
+
+#endif