diff --git a/CMakeLists.txt b/CMakeLists.txt
new file mode 100644
index 0000000..afb5a18
--- /dev/null
+++ b/CMakeLists.txt
@@ -0,0 +1,179 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(openslam_gmapping)
+
+## Find catkin macros and libraries
+find_package(catkin REQUIRED)
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES utils sensor_base sensor_odometry sensor_range log configfile scanmatcher gridfastslam
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(include)
+
+#SUBDIRS=utils sensor log configfile scanmatcher gridfastslam gui
+
+# utils/
+# OBJS= stat.o  movement.o
+# APPS= autoptr_test #stat_test
+# CPPFLAGS+= -DFSLINE
+add_library(utils
+  utils/stat.cpp
+  utils/movement.cpp)
+add_executable(autoptr_test
+  utils/autoptr_test.cpp)
+
+# sensor/
+# SUBDIRS=sensor_base sensor_odometry sensor_range
+
+# sensor/sensor_base/
+# OBJS= sensor.o sensorreading.o
+add_library(sensor_base
+  sensor/sensor_base/sensor.cpp
+  sensor/sensor_base/sensorreading.cpp)
+
+# sensor/sensor_odometry/
+# LDFLAGS+= -lsensor_base
+# OBJS= odometrysensor.o odometryreading.o
+add_library(sensor_odometry
+  sensor/sensor_odometry/odometrysensor.cpp
+  sensor/sensor_odometry/odometryreading.cpp)
+target_link_libraries(sensor_odometry
+  sensor_base)
+
+# sensor/sensor_range/
+# CPPFLAGS+= -I../
+# LDFLAGS+= -lsensor_base
+# OBJS= rangesensor.o rangereading.o 
+add_library(sensor_range
+  sensor/sensor_range/rangesensor.cpp
+  sensor/sensor_range/rangereading.cpp)
+target_link_libraries(sensor_range
+  sensor_base)
+
+# log/
+# CPPFLAGS+= -I../sensor
+# OBJS= configuration.o carmenconfiguration.o sensorlog.o sensorstream.o
+# APPS= log_test log_plot scanstudio2carmen rdk2carmen
+# LDFLAGS+=  -lsensor_range -lsensor_odometry -lsensor_base 
+add_library(log
+  log/configuration.cpp
+  log/carmenconfiguration.cpp
+  log/sensorlog.cpp
+  log/sensorstream.cpp)
+add_executable(log_test
+  log/log_test.cpp)
+add_executable(log_plot
+  log/log_plot.cpp)
+add_executable(scanstudio2carmen
+  log/scanstudio2carmen.cpp)
+add_executable(rdk2carmen
+  log/rdk2carmen.cpp)
+target_link_libraries(log_test log)
+target_link_libraries(log_plot log)
+target_link_libraries(scanstudio2carmen log)
+target_link_libraries(rdk2carmen log)
+target_link_libraries(log
+  sensor_range sensor_odometry sensor_base)
+
+# configfile/
+# OBJS= configfile.o 
+# APPS= configfile_test 
+add_library(configfile
+  configfile/configfile.cpp)
+add_executable(configfile_test
+  configfile/configfile_test.cpp)
+target_link_libraries(configfile_test configfile)
+
+# scanmatcher/
+# CPPFLAGS+=-I../sensor
+# OBJS= smmap.o scanmatcher.o scanmatcherprocessor.o eig3.o
+# APPS= scanmatch_test icptest
+# LDFLAGS+= -llog -lsensor_range -lsensor_odometry -lsensor_base -lutils
+add_library(scanmatcher
+  scanmatcher/smmap.cpp
+  scanmatcher/scanmatcher.cpp
+  scanmatcher/scanmatcherprocessor.cpp
+  scanmatcher/eig3.cpp)
+add_executable(scanmatch_test
+  scanmatcher/scanmatch_test.cpp)
+add_executable(icptest
+  scanmatcher/icptest.cpp)
+target_link_libraries(scanmatch_test scanmatcher)
+target_link_libraries(icptest scanmatcher)
+target_link_libraries(scanmatcher
+  log sensor_range sensor_odometry sensor_base utils)
+
+# gridfastslam/
+# CPPFLAGS+=-I../sensor
+# OBJS= gridslamprocessor_tree.o motionmodel.o gridslamprocessor.o gfsreader.o
+# APPS= gfs2log gfs2rec gfs2neff #gfs2stat
+# LDFLAGS+=  -lscanmatcher -llog -lsensor_range -lsensor_odometry -lsensor_base -lutils
+add_library(gridfastslam
+  gridfastslam/gridslamprocessor_tree.cpp
+  gridfastslam/motionmodel.cpp
+  gridfastslam/gridslamprocessor.cpp
+  gridfastslam/gfsreader.cpp)
+add_executable(gfs2log
+  gridfastslam/gfs2log.cpp)
+add_executable(gfs2rec
+  gridfastslam/gfs2rec.cpp)
+add_executable(gfs2neff
+  gridfastslam/gfs2neff.cpp)
+target_link_libraries(gfs2log gridfastslam)
+target_link_libraries(gfs2rec gridfastslam)
+target_link_libraries(gfs2neff gridfastslam)
+target_link_libraries(gridfastslam
+  scanmatcher log sensor_range sensor_odometry sensor_base utils)
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.h"
+  PATTERN ".svn" EXCLUDE
+)
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_openslam_gmapping.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
diff --git a/build_tools/Makefile.generic-shared-object b/build_tools/Makefile.generic-shared-object
index 49aba3d..8bb7ec6 100644
--- a/build_tools/Makefile.generic-shared-object
+++ b/build_tools/Makefile.generic-shared-object
@@ -35,7 +35,7 @@ ifeq ($(MACOSX),1)
 	@$(PRETTY) "$(CXX) $(LDFLAGS)  -dynamiclib $(OBJS) $(COBJS) -L$(LIBDIR) $(LIBS) -install_name $@ -o $@"
 endif
 ifeq ($(LINUX),1)
-	@$(PRETTY) "$(CXX) $(LDFLAGS) -fPIC -shared $(OBJS) $(COBJS) -L $(LIBDIR) $(LIBS) -o $@"
+ 	@$(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
 
diff --git a/carmenwrapper/carmenwrapper.cpp b/carmenwrapper/carmenwrapper.cpp
index 9c193e7..d51d753 100644
--- a/carmenwrapper/carmenwrapper.cpp
+++ b/carmenwrapper/carmenwrapper.cpp
@@ -21,7 +21,7 @@
  *****************************************************************/
 
 
-#include "carmenwrapper.h"
+#include "gmapping/carmenwrapper/carmenwrapper.h"
 
 using namespace GMapping;
 using namespace std;
diff --git a/configfile/configfile.cpp b/configfile/configfile.cpp
index dc584be..817b98d 100644
--- a/configfile/configfile.cpp
+++ b/configfile/configfile.cpp
@@ -22,7 +22,7 @@
 
 
 #include <cstdlib>
-#include "configfile.h"
+#include "gmapping/configfile/configfile.h"
 
 #include <fstream>
 #include <iostream>
diff --git a/configfile/configfile_test.cpp b/configfile/configfile_test.cpp
index 1258ff3..8f7a356 100644
--- a/configfile/configfile_test.cpp
+++ b/configfile/configfile_test.cpp
@@ -23,7 +23,7 @@
 
 #include <cstdlib>
 #include <iostream>
-#include "configfile.h"
+#include "gmapping/configfile/configfile.h"
 
 using namespace std;
 using namespace GMapping;
diff --git a/gfs-carmen/gfs-carmen.cpp b/gfs-carmen/gfs-carmen.cpp
index 14deee4..7c19e65 100644
--- a/gfs-carmen/gfs-carmen.cpp
+++ b/gfs-carmen/gfs-carmen.cpp
@@ -21,11 +21,11 @@
  *****************************************************************/
 
 
-#include <utils/commandline.h>
-#include <carmenwrapper/carmenwrapper.h>
-#include <gridfastslam/gridslamprocessor.h>
-#include <utils/orientedboundingbox.h>
-#include <configfile/configfile.h>
+#include <gmapping/utils/commandline.h>
+#include <gmapping/carmenwrapper/carmenwrapper.h>
+#include <gmapping/gridfastslam/gridslamprocessor.h>
+#include <gmapping/utils/orientedboundingbox.h>
+#include <gmapping/configfile/configfile.h>
 
 #define DEBUG cout << __PRETTY_FUNCTION__
 
diff --git a/grid/graphmap.cpp b/grid/graphmap.cpp
index a262d4c..a56253e 100644
--- a/grid/graphmap.cpp
+++ b/grid/graphmap.cpp
@@ -1,9 +1,9 @@
 #ifndef GRAPHMAP_H
 #define GRAPHMAP_H
 #include <list>
-#include <utils/point.h>
+#include <gmapping/utils/point.h>
 #include <utils/graph.h>
-#include <grid/map.h>
+#include <gmapping/grid/map.h>
 
 namespace GMapping {
 
diff --git a/grid/map_test.cpp b/grid/map_test.cpp
index 41dbf5a..3b7157c 100644
--- a/grid/map_test.cpp
+++ b/grid/map_test.cpp
@@ -1,6 +1,6 @@
 #include <iostream>
-#include "map.h"
-#include "harray2d.h"
+#include "gmapping/grid/map.h"
+#include "gmapping/grid/harray2d.h"
 
 using namespace std;
 using namespace GMapping;
diff --git a/gridfastslam/gfs2log.cpp b/gridfastslam/gfs2log.cpp
index d89185f..e688e77 100644
--- a/gridfastslam/gfs2log.cpp
+++ b/gridfastslam/gfs2log.cpp
@@ -4,8 +4,8 @@
 #include <sstream>
 #include <vector>
 #include <list>
-#include <utils/point.h>
-#include "gfsreader.h"
+#include <gmapping/utils/point.h>
+#include "gmapping/gridfastslam/gfsreader.h"
 
 #define MAX_LINE_LENGHT (1000000)
 
diff --git a/gridfastslam/gfs2neff.cpp b/gridfastslam/gfs2neff.cpp
index 106512f..5faad99 100644
--- a/gridfastslam/gfs2neff.cpp
+++ b/gridfastslam/gfs2neff.cpp
@@ -1,6 +1,7 @@
 #include <iostream>
 #include <fstream>
 #include <sstream>
+#include <cstring>
 
 using namespace std;
 
diff --git a/gridfastslam/gfs2rec.cpp b/gridfastslam/gfs2rec.cpp
index 35a396d..5eb65fa 100644
--- a/gridfastslam/gfs2rec.cpp
+++ b/gridfastslam/gfs2rec.cpp
@@ -4,7 +4,7 @@
 #include <sstream>
 #include <vector>
 #include <list>
-#include <utils/point.h>
+#include <gmapping/utils/point.h>
 
 #define MAX_LINE_LENGHT (1000000)
 
@@ -145,9 +145,9 @@ struct ResampleRecord: public Record{
 	virtual void read(istream& is){
 		is >> dim;
 		for (unsigned int i=0; i< dim; i++){
-			unsigned int i;
-			is >> i;
-			indexes.push_back(i);
+			unsigned int j;
+			is >> j;
+			indexes.push_back(j);
 		}
 	}
 };
diff --git a/gridfastslam/gfs2stat.cpp b/gridfastslam/gfs2stat.cpp
index 9e7446c..107c19a 100644
--- a/gridfastslam/gfs2stat.cpp
+++ b/gridfastslam/gfs2stat.cpp
@@ -1,8 +1,8 @@
-#include <utils/stat.h>
-#include <particlefilter/particlefilter.h>
+#include <gmapping/utils/stat.h>
+#include <gmapping/particlefilter/particlefilter.h>
 #include <iostream>
 #include <fstream>
-#include "gfsreader.h"
+#include "gmapping/gridfastslam/gfsreader.h"
 
 using namespace std;
 using namespace GMapping;
diff --git a/gridfastslam/gfs2stream.cpp b/gridfastslam/gfs2stream.cpp
index 80cc18f..c639947 100644
--- a/gridfastslam/gfs2stream.cpp
+++ b/gridfastslam/gfs2stream.cpp
@@ -3,9 +3,9 @@
 #include <sstream>
 #include <vector>
 #include <list>
-#include <utils/point.h>
-#include <utils/commandline.h>
-#include "gfsreader.h"
+#include <gmapping/utils/point.h>
+#include <gmapping/utils/commandline.h>
+#include "gmapping/gridfastslam/gfsreader.h"
 #define MAX_LINE_LENGHT (1000000)
 
 using namespace std;
diff --git a/gridfastslam/gfsreader.cpp b/gridfastslam/gfsreader.cpp
index 8b16427..ba892e5 100644
--- a/gridfastslam/gfsreader.cpp
+++ b/gridfastslam/gfsreader.cpp
@@ -1,5 +1,5 @@
 #include <cstring>
-#include "gfsreader.h"
+#include "gmapping/gridfastslam/gfsreader.h"
 #include <iomanip>
 #include <limits>
 
diff --git a/gridfastslam/gridslamprocessor.cpp b/gridfastslam/gridslamprocessor.cpp
index 90aebb8..3466746 100644
--- a/gridfastslam/gridslamprocessor.cpp
+++ b/gridfastslam/gridslamprocessor.cpp
@@ -5,8 +5,8 @@
 #include <set>
 #include <fstream>
 #include <iomanip>
-#include <utils/stat.h>
-#include "gridslamprocessor.h"
+#include <gmapping/utils/stat.h>
+#include "gmapping/gridfastslam/gridslamprocessor.h"
 
 //#define MAP_CONSISTENCY_CHECK
 //#define GENERATE_TRAJECTORIES
@@ -19,14 +19,17 @@ 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) 
-    :m_particles(gsp.m_particles), m_infoStream(cout){
+    :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;
@@ -85,6 +88,7 @@ using namespace std;
   }
   
   GridSlamProcessor::GridSlamProcessor(std::ostream& infoS): m_infoStream(infoS){
+    period_ = 5.0;
     m_obsSigmaGain=1;
     m_resampleThreshold=0.5;
     m_minimumScore=0.;
@@ -146,7 +150,7 @@ using namespace std;
   
   GridSlamProcessor::~GridSlamProcessor(){
     cerr << __PRETTY_FUNCTION__ << ": Start" << endl;
-    cerr << __PRETTY_FUNCTION__ << ": Deeting tree" << 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;
@@ -374,11 +378,13 @@ void GridSlamProcessor::setMotionModelParameters
     
     bool processed=false;
 
-    // process a scan only if the robot has traveled a given distance
+    // 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){
-      
+	|| 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;
@@ -402,6 +408,13 @@ void GridSlamProcessor::setMotionModelParameters
 	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()){
@@ -432,7 +445,7 @@ void GridSlamProcessor::setMotionModelParameters
 	  m_outputStream << setiosflags(ios::fixed) << setprecision(6);
 	  m_outputStream << "NEFF " << m_neff << endl;
 	}
-	resample(plainReading, adaptParticles);
+ 	resample(plainReading, adaptParticles, reading_copy);
 	
       } else {
 	m_infoStream << "Registering First Scan"<< endl;
@@ -443,7 +456,8 @@ void GridSlamProcessor::setMotionModelParameters
 	  
 	  // 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=0;
+      node->reading = reading_copy;
 	  it->node=node;
 	  
 	}
diff --git a/gridfastslam/gridslamprocessor_tree.cpp b/gridfastslam/gridslamprocessor_tree.cpp
index 9ecd216..41685c6 100644
--- a/gridfastslam/gridslamprocessor_tree.cpp
+++ b/gridfastslam/gridslamprocessor_tree.cpp
@@ -6,8 +6,8 @@
 #include <fstream>
 //#include <gsl/gsl_blas.h>
 
-#include <utils/stat.h>
-#include "gridslamprocessor.h"
+#include <gmapping/utils/stat.h>
+#include "gmapping/gridfastslam/gridslamprocessor.h"
 
 namespace GMapping {
 
diff --git a/gridfastslam/motionmodel.cpp b/gridfastslam/motionmodel.cpp
index 7bcb727..7f0d1fc 100644
--- a/gridfastslam/motionmodel.cpp
+++ b/gridfastslam/motionmodel.cpp
@@ -1,5 +1,5 @@
-#include "motionmodel.h"
-#include <utils/stat.h>
+#include "gmapping/gridfastslam/motionmodel.h"
+#include <gmapping/utils/stat.h>
 #include <iostream>
 
 #define MotionModelConditioningLinearCovariance 0.01
diff --git a/gui/gfs2img.cpp b/gui/gfs2img.cpp
index cdd654e..826dc28 100644
--- a/gui/gfs2img.cpp
+++ b/gui/gfs2img.cpp
@@ -1,12 +1,12 @@
 #include <cstdlib>
 #include <limits.h>
-#include <scanmatcher/scanmatcher.h>
-#include <gridfastslam/gfsreader.h>
+#include <gmapping/scanmatcher/scanmatcher.h>
+#include <gmapping/gridfastslam/gfsreader.h>
 #include <qpixmap.h>
 #include <qpainter.h>
 #include <qimage.h>
 #include <qapplication.h>
-#include <utils/commandline.h>
+#include <gmapping/utils/commandline.h>
 
 #define MAX_LASER_BEAMS 1024
 #define MAX_FILENAME 1024
diff --git a/gui/gfs_logplayer.cpp b/gui/gfs_logplayer.cpp
index 674e59c..4bf7b57 100644
--- a/gui/gfs_logplayer.cpp
+++ b/gui/gfs_logplayer.cpp
@@ -22,7 +22,7 @@
 
 
 #include <qapplication.h>
-#include "qparticleviewer.h"
+#include "gmapping/gui/qparticleviewer.h"
 
 int  main (int argc, char ** argv){
 	QApplication app(argc, argv);
diff --git a/gui/gfs_nogui.cpp b/gui/gfs_nogui.cpp
index d49d7a8..53c7834 100644
--- a/gui/gfs_nogui.cpp
+++ b/gui/gfs_nogui.cpp
@@ -22,7 +22,7 @@
 
 
 #include <unistd.h>
-#include "gsp_thread.h"
+#include "gmapping/gui/gsp_thread.h"
 
 using namespace GMapping;
 
diff --git a/gui/gfs_simplegui.cpp b/gui/gfs_simplegui.cpp
index e8b4461..4de4d40 100644
--- a/gui/gfs_simplegui.cpp
+++ b/gui/gfs_simplegui.cpp
@@ -21,8 +21,8 @@
  *****************************************************************/
 
 
-#include "qparticleviewer.h"
-#include "qgraphpainter.h"
+#include "gmapping/gui/qparticleviewer.h"
+#include "gmapping/gui/qgraphpainter.h"
 #include <qapplication.h>
 #include <qframe.h>
 #include <qlabel.h>
diff --git a/gui/gsp_thread.cpp b/gui/gsp_thread.cpp
index 5d0022f..e6fb208 100644
--- a/gui/gsp_thread.cpp
+++ b/gui/gsp_thread.cpp
@@ -21,13 +21,13 @@
  *****************************************************************/
 
 
-#include "gsp_thread.h"
-#include <utils/commandline.h>
-#include <utils/stat.h>
-#include <configfile/configfile.h>
+#include "gmapping/gui/gsp_thread.h"
+#include <gmapping/utils/commandline.h>
+#include <gmapping/utils/stat.h>
+#include <gmapping/configfile/configfile.h>
 
 #ifdef CARMEN_SUPPORT
-	#include <carmenwrapper/carmenwrapper.h>
+	#include <gmapping/carmenwrapper/carmenwrapper.h>
 #endif
 
 #define DEBUG cout << __PRETTY_FUNCTION__
diff --git a/gui/qgraphpainter.cpp b/gui/qgraphpainter.cpp
index 744e23b..7204eaa 100644
--- a/gui/qgraphpainter.cpp
+++ b/gui/qgraphpainter.cpp
@@ -22,7 +22,7 @@
 
 
 #include <iostream>
-#include "qgraphpainter.h"
+#include "gmapping/gui/qgraphpainter.h"
 #include "moc_qgraphpainter.cpp"
 using namespace std;
 
diff --git a/gui/qmappainter.cpp b/gui/qmappainter.cpp
index 3aa2dfb..b88512e 100644
--- a/gui/qmappainter.cpp
+++ b/gui/qmappainter.cpp
@@ -1,4 +1,4 @@
-#include "qmappainter.h"
+#include "gmapping/gui/qmappainter.h"
 #include "moc_qmappainter.cpp"
 
 QMapPainter::QMapPainter( QWidget * parent, const char * name, WFlags f):
diff --git a/gui/qnavigatorwidget.cpp b/gui/qnavigatorwidget.cpp
index d818554..9410002 100644
--- a/gui/qnavigatorwidget.cpp
+++ b/gui/qnavigatorwidget.cpp
@@ -1,4 +1,4 @@
-#include "qnavigatorwidget.h"
+#include "gmapping/gui/qnavigatorwidget.h"
 #include <stdio.h>
 using namespace GMapping;
 
diff --git a/gui/qparticleviewer.cpp b/gui/qparticleviewer.cpp
index 633aee5..0a39761 100644
--- a/gui/qparticleviewer.cpp
+++ b/gui/qparticleviewer.cpp
@@ -21,7 +21,7 @@
  *****************************************************************/
 
 
-#include "qparticleviewer.h"
+#include "gmapping/gui/qparticleviewer.h"
 #include "moc_qparticleviewer.cpp"
 #include <qimage.h>
 
diff --git a/gui/qpixmapdumper.cpp b/gui/qpixmapdumper.cpp
index 239b209..abf0140 100644
--- a/gui/qpixmapdumper.cpp
+++ b/gui/qpixmapdumper.cpp
@@ -1,4 +1,4 @@
-#include "qpixmapdumper.h"
+#include "gmapping/gui/qpixmapdumper.h"
 #include <cstdio>
 #include <cstring>
 
diff --git a/gui/qslamandnavwidget.cpp b/gui/qslamandnavwidget.cpp
index 9c81264..3dc1ff4 100644
--- a/gui/qslamandnavwidget.cpp
+++ b/gui/qslamandnavwidget.cpp
@@ -1,4 +1,4 @@
-#include "qslamandnavwidget.h"
+#include "gmapping/gui/qslamandnavwidget.h"
 #include <stdio.h>
 using namespace GMapping;
 
diff --git a/carmenwrapper/carmenwrapper.h b/include/gmapping/carmenwrapper/carmenwrapper.h
similarity index 91%
rename from carmenwrapper/carmenwrapper.h
rename to include/gmapping/carmenwrapper/carmenwrapper.h
index 423fcef..80a6d87 100644
--- a/carmenwrapper/carmenwrapper.h
+++ b/include/gmapping/carmenwrapper/carmenwrapper.h
@@ -30,12 +30,12 @@
 #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>
+#include <gmapping/sensor/sensor_base/sensor.h>
+#include <gmapping/log/carmenconfiguration.h>
+#include <gmapping/log/sensorstream.h>
+#include <gmapping/log/sensorlog.h>
+#include <gmapping/sensor/sensor_range/rangesensor.h>
+#include <gmapping/sensor/sensor_range/rangereading.h>
 
 namespace GMapping{
 
diff --git a/configfile/configfile.h b/include/gmapping/configfile/configfile.h
similarity index 100%
rename from configfile/configfile.h
rename to include/gmapping/configfile/configfile.h
diff --git a/grid/accessstate.h b/include/gmapping/grid/accessstate.h
similarity index 100%
rename from grid/accessstate.h
rename to include/gmapping/grid/accessstate.h
diff --git a/grid/array2d.h b/include/gmapping/grid/array2d.h
similarity index 98%
rename from grid/array2d.h
rename to include/gmapping/grid/array2d.h
index fbe98e5..2494350 100644
--- a/grid/array2d.h
+++ b/include/gmapping/grid/array2d.h
@@ -2,8 +2,8 @@
 #define ARRAY2D_H
 
 #include <assert.h>
-#include <utils/point.h>
-#include "accessstate.h"
+#include <gmapping/utils/point.h>
+#include "gmapping/grid/accessstate.h"
 
 #include <iostream>
 
diff --git a/grid/harray2d.h b/include/gmapping/grid/harray2d.h
similarity index 98%
rename from grid/harray2d.h
rename to include/gmapping/grid/harray2d.h
index 2fee799..23004ab 100644
--- a/grid/harray2d.h
+++ b/include/gmapping/grid/harray2d.h
@@ -1,9 +1,9 @@
 #ifndef HARRAY2D_H
 #define HARRAY2D_H
 #include <set>
-#include <utils/point.h>
-#include <utils/autoptr.h>
-#include "array2d.h"
+#include <gmapping/utils/point.h>
+#include <gmapping/utils/autoptr.h>
+#include "gmapping/grid/array2d.h"
 
 namespace GMapping {
 
diff --git a/grid/map.h b/include/gmapping/grid/map.h
similarity index 98%
rename from grid/map.h
rename to include/gmapping/grid/map.h
index ea9963a..1d91248 100644
--- a/grid/map.h
+++ b/include/gmapping/grid/map.h
@@ -1,9 +1,9 @@
 #ifndef MAP_H
 #define MAP_H
-#include <utils/point.h>
+#include <gmapping/utils/point.h>
 #include <assert.h>
-#include "accessstate.h"
-#include "array2d.h"
+#include "gmapping/grid/accessstate.h"
+#include "gmapping/grid/array2d.h"
 
 namespace GMapping {
 /**
diff --git a/gridfastslam/gfsreader.h b/include/gmapping/gridfastslam/gfsreader.h
similarity index 98%
rename from gridfastslam/gfsreader.h
rename to include/gmapping/gridfastslam/gfsreader.h
index 3ad34ac..16041f3 100644
--- a/gridfastslam/gfsreader.h
+++ b/include/gmapping/gridfastslam/gfsreader.h
@@ -6,7 +6,7 @@
 #include <sstream>
 #include <vector>
 #include <list>
-#include <utils/point.h>
+#include <gmapping/utils/point.h>
 
 #define MAX_LINE_LENGHT (1000000)
 
diff --git a/gridfastslam/gridslamprocessor.h b/include/gmapping/gridfastslam/gridslamprocessor.h
similarity index 96%
rename from gridfastslam/gridslamprocessor.h
rename to include/gmapping/gridfastslam/gridslamprocessor.h
index 54a76d2..3b7f650 100644
--- a/gridfastslam/gridslamprocessor.h
+++ b/include/gmapping/gridfastslam/gridslamprocessor.h
@@ -6,14 +6,14 @@
 #include <fstream>
 #include <vector>
 #include <deque>
-#include <particlefilter/particlefilter.h>
-#include <utils/point.h>
-#include <utils/macro_params.h>
-#include <log/sensorlog.h>
-#include <sensor/sensor_range/rangesensor.h>
-#include <sensor/sensor_range/rangereading.h>
-#include <scanmatcher/scanmatcher.h>
-#include "motionmodel.h"
+#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 "gmapping/gridfastslam/motionmodel.h"
 
 
 namespace GMapping {
@@ -148,6 +148,7 @@ namespace GMapping {
 			       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);
@@ -248,7 +249,8 @@ namespace GMapping {
  
     /**the laser beams*/
     unsigned int m_beams;
-    
+    double last_update_time_;
+    double period_;
     
     /**the particles*/
     ParticleVector m_particles;
@@ -328,7 +330,7 @@ namespace GMapping {
 typedef std::multimap<const GridSlamProcessor::TNode*, GridSlamProcessor::TNode*> TNodeMultimap;
 
 
-#include "gridslamprocessor.hxx"
+#include "gmapping/gridfastslam/gridslamprocessor.hxx"
 
 };
 
diff --git a/gridfastslam/gridslamprocessor.hxx b/include/gmapping/gridfastslam/gridslamprocessor.hxx
similarity index 97%
rename from gridfastslam/gridslamprocessor.hxx
rename to include/gmapping/gridfastslam/gridslamprocessor.hxx
index e280464..8a8b7a4 100644
--- a/gridfastslam/gridslamprocessor.hxx
+++ b/include/gmapping/gridfastslam/gridslamprocessor.hxx
@@ -67,7 +67,7 @@ inline void GridSlamProcessor::normalize(){
   
 }
 
-inline bool GridSlamProcessor::resample(const double* plainReading, int adaptSize, const RangeReading* ){
+inline bool GridSlamProcessor::resample(const double* plainReading, int adaptSize, const RangeReading* reading){
   
   bool hasResampled = false;
   
@@ -112,7 +112,8 @@ inline bool GridSlamProcessor::resample(const double* plainReading, int adaptSiz
       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=0;
+      node->reading=reading;
       //			cerr << "A("<<node->parent->childs <<") " <<endl;
       
       temp.push_back(p);
@@ -155,7 +156,8 @@ inline bool GridSlamProcessor::resample(const double* plainReading, int adaptSiz
       TNode* node=0;
       node=new TNode(it->pose, 0.0, *node_it, 0);
       
-      node->reading=0;
+      //node->reading=0;
+      node->reading=reading;
       it->node=node;
 
       //END: BUILDING TREE
diff --git a/gridfastslam/motionmodel.h b/include/gmapping/gridfastslam/motionmodel.h
similarity index 80%
rename from gridfastslam/motionmodel.h
rename to include/gmapping/gridfastslam/motionmodel.h
index 6f5535d..d983901 100644
--- a/gridfastslam/motionmodel.h
+++ b/include/gmapping/gridfastslam/motionmodel.h
@@ -1,9 +1,9 @@
 #ifndef MOTIONMODEL_H
 #define MOTIONMODEL_H
 
-#include <utils/point.h>
-#include <utils/stat.h>
-#include <utils/macro_params.h>
+#include <gmapping/utils/point.h>
+#include <gmapping/utils/stat.h>
+#include <gmapping/utils/macro_params.h>
 
 namespace  GMapping { 
 
diff --git a/gui/gsp_thread.h b/include/gmapping/gui/gsp_thread.h
similarity index 96%
rename from gui/gsp_thread.h
rename to include/gmapping/gui/gsp_thread.h
index 51c0dc9..4bcd425 100644
--- a/gui/gsp_thread.h
+++ b/include/gmapping/gui/gsp_thread.h
@@ -29,9 +29,9 @@
 #include <deque>
 #include <fstream>
 #include <iostream>
-#include <log/carmenconfiguration.h>
-#include <log/sensorstream.h>
-#include <gridfastslam/gridslamprocessor.h>
+#include <gmapping/log/carmenconfiguration.h>
+#include <gmapping/log/sensorstream.h>
+#include <gmapping/gridfastslam/gridslamprocessor.h>
 
 using namespace std;
 using namespace GMapping;
diff --git a/gui/qgraphpainter.h b/include/gmapping/gui/qgraphpainter.h
similarity index 98%
rename from gui/qgraphpainter.h
rename to include/gmapping/gui/qgraphpainter.h
index 42a6bba..cd8376f 100644
--- a/gui/qgraphpainter.h
+++ b/include/gmapping/gui/qgraphpainter.h
@@ -30,7 +30,7 @@
 #include <qwidget.h>
 #include <qwmatrix.h>
 #include <deque>
-#include <utils/gvalues.h>
+#include <gmapping/utils/gvalues.h>
 
 typedef std::deque<double> DoubleDeque;
 
diff --git a/gui/qmappainter.h b/include/gmapping/gui/qmappainter.h
similarity index 96%
rename from gui/qmappainter.h
rename to include/gmapping/gui/qmappainter.h
index d121d1b..0606c2b 100644
--- a/gui/qmappainter.h
+++ b/include/gmapping/gui/qmappainter.h
@@ -5,8 +5,8 @@
 #include <qpixmap.h>
 #include <qstring.h>
 #include <qwidget.h>
-#include <utils/gvalues.h>
-#include <utils/point.h>
+#include <gmapping/utils/gvalues.h>
+#include <gmapping/utils/point.h>
 
 class QMapPainter :  public QWidget{
 	public:
diff --git a/gui/qnavigatorwidget.h b/include/gmapping/gui/qnavigatorwidget.h
similarity index 87%
rename from gui/qnavigatorwidget.h
rename to include/gmapping/gui/qnavigatorwidget.h
index 9766b9d..19757fe 100644
--- a/gui/qnavigatorwidget.h
+++ b/include/gmapping/gui/qnavigatorwidget.h
@@ -1,9 +1,9 @@
 #ifndef _QNAVIGATOR_WIDGET_H
 #define _QNAVIGATOR_WIDGET_H
 
-#include "qmappainter.h"
-#include "qpixmapdumper.h"
-#include <utils/point.h>
+#include "gmapping/gui/qmappainter.h"
+#include "gmapping/gui/qpixmapdumper.h"
+#include <gmapping/utils/point.h>
 #include <list>
 
 class QNavigatorWidget :  public QMapPainter{
diff --git a/gui/qparticleviewer.h b/include/gmapping/gui/qparticleviewer.h
similarity index 98%
rename from gui/qparticleviewer.h
rename to include/gmapping/gui/qparticleviewer.h
index 7189f77..7f33ff8 100644
--- a/gui/qparticleviewer.h
+++ b/include/gmapping/gui/qparticleviewer.h
@@ -35,8 +35,8 @@
 #include <iostream>
 #include <qimage.h>
 
-#include <utils/point.h>
-#include "gsp_thread.h"
+#include <gmapping/utils/point.h>
+#include "gmapping/gui/gsp_thread.h"
 
 namespace GMapping {
 
diff --git a/gui/qpixmapdumper.h b/include/gmapping/gui/qpixmapdumper.h
similarity index 100%
rename from gui/qpixmapdumper.h
rename to include/gmapping/gui/qpixmapdumper.h
diff --git a/gui/qslamandnavwidget.h b/include/gmapping/gui/qslamandnavwidget.h
similarity index 87%
rename from gui/qslamandnavwidget.h
rename to include/gmapping/gui/qslamandnavwidget.h
index e33ca3f..e42ed55 100644
--- a/gui/qslamandnavwidget.h
+++ b/include/gmapping/gui/qslamandnavwidget.h
@@ -1,9 +1,9 @@
 #ifndef _QSLAMANDNAV_WIDGET_H
 #define _QSLAMANDNAV_WIDGET_H
 
-#include "qmappainter.h"
-#include "qpixmapdumper.h"
-#include <utils/point.h>
+#include "gmapping/gui/qmappainter.h"
+#include "gmapping/gui/qpixmapdumper.h"
+#include <gmapping/utils/point.h>
 #include <list>
 
 class QSLAMandNavWidget :  public QMapPainter{
diff --git a/log/carmenconfiguration.h b/include/gmapping/log/carmenconfiguration.h
similarity index 81%
rename from log/carmenconfiguration.h
rename to include/gmapping/log/carmenconfiguration.h
index 4cf9bfb..ee5188f 100644
--- a/log/carmenconfiguration.h
+++ b/include/gmapping/log/carmenconfiguration.h
@@ -5,8 +5,8 @@
 #include <map>
 #include <vector>
 #include <istream>
-#include <sensor/sensor_base/sensor.h>
-#include "configuration.h"
+#include <gmapping/sensor/sensor_base/sensor.h>
+#include "gmapping/log/configuration.h"
 
 namespace GMapping {
 
diff --git a/log/configuration.h b/include/gmapping/log/configuration.h
similarity index 81%
rename from log/configuration.h
rename to include/gmapping/log/configuration.h
index f02d17f..ce1c81c 100644
--- a/log/configuration.h
+++ b/include/gmapping/log/configuration.h
@@ -2,7 +2,7 @@
 #define CONFIGURATION_H
 
 #include <istream>
-#include <sensor/sensor_base/sensor.h>
+#include <gmapping/sensor/sensor_base/sensor.h>
 
 namespace GMapping {
 
diff --git a/log/sensorlog.h b/include/gmapping/log/sensorlog.h
similarity index 62%
rename from log/sensorlog.h
rename to include/gmapping/log/sensorlog.h
index c6b380f..d380a3f 100644
--- a/log/sensorlog.h
+++ b/include/gmapping/log/sensorlog.h
@@ -3,12 +3,12 @@
 
 #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"
+#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 "gmapping/log/configuration.h"
 
 namespace GMapping {
 
diff --git a/log/sensorstream.h b/include/gmapping/log/sensorstream.h
similarity index 97%
rename from log/sensorstream.h
rename to include/gmapping/log/sensorstream.h
index 5e25507..2551ba8 100644
--- a/log/sensorstream.h
+++ b/include/gmapping/log/sensorstream.h
@@ -2,7 +2,7 @@
 #define SENSORSTREAM_H
 
 #include <istream>
-#include "sensorlog.h"
+#include "gmapping/log/sensorlog.h"
 
 namespace GMapping {
 class SensorStream{
diff --git a/particlefilter/particlefilter.h b/include/gmapping/particlefilter/particlefilter.h
similarity index 98%
rename from particlefilter/particlefilter.h
rename to include/gmapping/particlefilter/particlefilter.h
index 68322f3..5823525 100644
--- a/particlefilter/particlefilter.h
+++ b/include/gmapping/particlefilter/particlefilter.h
@@ -1,11 +1,12 @@
 #ifndef PARTICLEFILTER_H
 #define PARTICLEFILTER_H
 #include <stdlib.h>
+#include <float.h>
 #include <sys/types.h>
 #include<vector>
 #include<utility>
 #include<cmath>
-#include<utils/gvalues.h>
+#include<gmapping/utils/gvalues.h>
 
 
 /**
@@ -23,7 +24,8 @@ 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;
+	//double lmax=-MAXDOUBLE;
+    double lmax=-DBL_MAX;
 	for (Iterator it=begin; it!=end; it++){
 		lmax=lmax>((double)(*it))? lmax: (double)(*it);
 	}
diff --git a/particlefilter/pf.h b/include/gmapping/particlefilter/pf.h
similarity index 99%
rename from particlefilter/pf.h
rename to include/gmapping/particlefilter/pf.h
index 374b0d8..79d6a38 100644
--- a/particlefilter/pf.h
+++ b/include/gmapping/particlefilter/pf.h
@@ -3,7 +3,7 @@
 #include <stdlib.h>
 #include<vector>
 #include<utility>
-#include<utils/gvalues.h>
+#include<gmapping/utils/gvalues.h>
 
 
 
diff --git a/scanmatcher/eig3.h b/include/gmapping/scanmatcher/eig3.h
similarity index 100%
rename from scanmatcher/eig3.h
rename to include/gmapping/scanmatcher/eig3.h
diff --git a/scanmatcher/gridlinetraversal.h b/include/gmapping/scanmatcher/gridlinetraversal.h
similarity index 98%
rename from scanmatcher/gridlinetraversal.h
rename to include/gmapping/scanmatcher/gridlinetraversal.h
index 15301f0..2eb34a8 100644
--- a/scanmatcher/gridlinetraversal.h
+++ b/include/gmapping/scanmatcher/gridlinetraversal.h
@@ -2,7 +2,7 @@
 #define GRIDLINETRAVERSAL_H
 
 #include <cstdlib>
-#include <utils/point.h>
+#include <gmapping/utils/point.h>
 
 namespace GMapping {
 
diff --git a/scanmatcher/icp.h b/include/gmapping/scanmatcher/icp.h
similarity index 98%
rename from scanmatcher/icp.h
rename to include/gmapping/scanmatcher/icp.h
index ad4df45..64c257c 100644
--- a/scanmatcher/icp.h
+++ b/include/gmapping/scanmatcher/icp.h
@@ -1,7 +1,7 @@
 #ifndef _ICP_H_
 #define _ICP_H_
 
-#include <utils/point.h>
+#include <gmapping/utils/point.h>
 #include <utility>
 #include <list>
 #include <vector>
diff --git a/scanmatcher/lumiles.h b/include/gmapping/scanmatcher/lumiles.h
similarity index 100%
rename from scanmatcher/lumiles.h
rename to include/gmapping/scanmatcher/lumiles.h
diff --git a/scanmatcher/scanmatcher.h b/include/gmapping/scanmatcher/scanmatcher.h
similarity index 95%
rename from scanmatcher/scanmatcher.h
rename to include/gmapping/scanmatcher/scanmatcher.h
index 12fffd6..c970ae1 100644
--- a/scanmatcher/scanmatcher.h
+++ b/include/gmapping/scanmatcher/scanmatcher.h
@@ -1,13 +1,13 @@
 #ifndef SCANMATCHER_H
 #define SCANMATCHER_H
 
-#include "icp.h"
-#include "smmap.h"
-#include <utils/macro_params.h>
-#include <utils/stat.h>
+#include "gmapping/scanmatcher/icp.h"
+#include "gmapping/scanmatcher/smmap.h"
+#include <gmapping/utils/macro_params.h>
+#include <gmapping/utils/stat.h>
 #include <iostream>
-#include <utils/gvalues.h>
-#define LASER_MAXBEAMS 1024
+#include <gmapping/utils/gvalues.h>
+#define LASER_MAXBEAMS 2048
 
 namespace GMapping {
 
@@ -16,6 +16,7 @@ class ScanMatcher{
 		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;
@@ -67,6 +68,9 @@ class ScanMatcher{
 		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{
@@ -82,7 +86,7 @@ inline double ScanMatcher::icpStep(OrientedPoint & pret, const ScanMatcherMap& m
 	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 (*r>m_usableRange||*r==0.0) continue;
 		if (skip) continue;
 		Point phit=lp;
 		phit.x+=*r*cos(lp.theta+*angle);
@@ -148,8 +152,7 @@ inline double ScanMatcher::score(const ScanMatcherMap& map, const OrientedPoint&
 	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;
+		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);
diff --git a/scanmatcher/scanmatcherprocessor.h b/include/gmapping/scanmatcher/scanmatcherprocessor.h
similarity index 87%
rename from scanmatcher/scanmatcherprocessor.h
rename to include/gmapping/scanmatcher/scanmatcherprocessor.h
index e6239e3..b35ccb4 100644
--- a/scanmatcher/scanmatcherprocessor.h
+++ b/include/gmapping/scanmatcher/scanmatcherprocessor.h
@@ -1,11 +1,11 @@
 #ifndef SCANMATCHERPROCESSOR_H
 #define SCANMATCHERPROCESSOR_H
 
-#include <log/sensorlog.h>
-#include <sensor/sensor_range/rangesensor.h>
-#include <sensor/sensor_range/rangereading.h>
+#include <gmapping/log/sensorlog.h>
+#include <gmapping/sensor/sensor_range/rangesensor.h>
+#include <gmapping/sensor/sensor_range/rangereading.h>
 //#include <gsl/gsl_eigen.h>
-#include "scanmatcher.h"
+#include "gmapping/scanmatcher/scanmatcher.h"
 
 namespace GMapping {
 
diff --git a/scanmatcher/smmap.h b/include/gmapping/scanmatcher/smmap.h
similarity index 92%
rename from scanmatcher/smmap.h
rename to include/gmapping/scanmatcher/smmap.h
index 713689a..681165c 100644
--- a/scanmatcher/smmap.h
+++ b/include/gmapping/scanmatcher/smmap.h
@@ -1,8 +1,8 @@
 #ifndef SMMAP_H
 #define SMMAP_H
-#include <grid/map.h>
-#include <grid/harray2d.h>
-#include <utils/point.h>
+#include <gmapping/grid/map.h>
+#include <gmapping/grid/harray2d.h>
+#include <gmapping/utils/point.h>
 #define SIGHT_INC 1
 
 namespace GMapping {
diff --git a/sensor/sensor_base/sensor.h b/include/gmapping/sensor/sensor_base/sensor.h
similarity index 100%
rename from sensor/sensor_base/sensor.h
rename to include/gmapping/sensor/sensor_base/sensor.h
diff --git a/sensor/sensor_base/sensoreading.h b/include/gmapping/sensor/sensor_base/sensoreading.h
similarity index 87%
rename from sensor/sensor_base/sensoreading.h
rename to include/gmapping/sensor/sensor_base/sensoreading.h
index 217ec6f..a51797b 100644
--- a/sensor/sensor_base/sensoreading.h
+++ b/include/gmapping/sensor/sensor_base/sensoreading.h
@@ -1,7 +1,7 @@
 #ifndef SENSORREADING_H
 #define SENSORREADING_H
 
-#include "sensor.h"
+#include "gmapping/sensor/sensor_base/sensor.h"
 namespace GMapping{
 
 class SensorReading{
diff --git a/sensor/sensor_base/sensorreading.h b/include/gmapping/sensor/sensor_base/sensorreading.h
similarity index 89%
rename from sensor/sensor_base/sensorreading.h
rename to include/gmapping/sensor/sensor_base/sensorreading.h
index 5f40812..964ee3d 100644
--- a/sensor/sensor_base/sensorreading.h
+++ b/include/gmapping/sensor/sensor_base/sensorreading.h
@@ -1,7 +1,7 @@
 #ifndef SENSORREADING_H
 #define SENSORREADING_H
 
-#include "sensor.h"
+#include "gmapping/sensor/sensor_base/sensor.h"
 namespace GMapping{
 
 class SensorReading{
diff --git a/sensor/sensor_odometry/odometryreading.h b/include/gmapping/sensor/sensor_odometry/odometryreading.h
similarity index 83%
rename from sensor/sensor_odometry/odometryreading.h
rename to include/gmapping/sensor/sensor_odometry/odometryreading.h
index 6096226..d007d19 100644
--- a/sensor/sensor_odometry/odometryreading.h
+++ b/include/gmapping/sensor/sensor_odometry/odometryreading.h
@@ -2,9 +2,9 @@
 #define ODOMETRYREADING_H
 
 #include <string.h>
-#include <sensor/sensor_base/sensorreading.h>
-#include <utils/point.h>
-#include "odometrysensor.h"
+#include <gmapping/sensor/sensor_base/sensorreading.h>
+#include <gmapping/utils/point.h>
+#include "gmapping/sensor/sensor_odometry/odometrysensor.h"
 
 namespace GMapping{
 
diff --git a/sensor/sensor_odometry/odometrysensor.h b/include/gmapping/sensor/sensor_odometry/odometrysensor.h
similarity index 85%
rename from sensor/sensor_odometry/odometrysensor.h
rename to include/gmapping/sensor/sensor_odometry/odometrysensor.h
index 4e0fbd1..1d18bd3 100644
--- a/sensor/sensor_odometry/odometrysensor.h
+++ b/include/gmapping/sensor/sensor_odometry/odometrysensor.h
@@ -2,7 +2,7 @@
 #define ODOMETRYSENSOR_H
 
 #include <string>
-#include <sensor/sensor_base/sensor.h>
+#include <gmapping/sensor/sensor_base/sensor.h>
 
 namespace GMapping{
 
diff --git a/sensor/sensor_range/rangereading.h b/include/gmapping/sensor/sensor_range/rangereading.h
similarity index 86%
rename from sensor/sensor_range/rangereading.h
rename to include/gmapping/sensor/sensor_range/rangereading.h
index 1c824de..b715a8e 100644
--- a/sensor/sensor_range/rangereading.h
+++ b/include/gmapping/sensor/sensor_range/rangereading.h
@@ -2,8 +2,8 @@
 #define RANGEREADING_H
 
 #include <vector>
-#include <sensor/sensor_base/sensorreading.h>
-#include "rangesensor.h"
+#include <gmapping/sensor/sensor_base/sensorreading.h>
+#include "gmapping/sensor/sensor_range/rangesensor.h"
 
 namespace GMapping{
 
diff --git a/sensor/sensor_range/rangesensor.h b/include/gmapping/sensor/sensor_range/rangesensor.h
similarity index 92%
rename from sensor/sensor_range/rangesensor.h
rename to include/gmapping/sensor/sensor_range/rangesensor.h
index fa2c073..65feffb 100644
--- a/sensor/sensor_range/rangesensor.h
+++ b/include/gmapping/sensor/sensor_range/rangesensor.h
@@ -2,8 +2,8 @@
 #define RANGESENSOR_H
 
 #include <vector>
-#include <sensor/sensor_base/sensor.h>
-#include <utils/point.h>
+#include <gmapping/sensor/sensor_base/sensor.h>
+#include <gmapping/utils/point.h>
 
 namespace GMapping{
 
diff --git a/utils/autoptr.h b/include/gmapping/utils/autoptr.h
similarity index 100%
rename from utils/autoptr.h
rename to include/gmapping/utils/autoptr.h
diff --git a/utils/commandline.h b/include/gmapping/utils/commandline.h
similarity index 100%
rename from utils/commandline.h
rename to include/gmapping/utils/commandline.h
diff --git a/utils/datasmoother.h b/include/gmapping/utils/datasmoother.h
similarity index 99%
rename from utils/datasmoother.h
rename to include/gmapping/utils/datasmoother.h
index ddd8553..5fb311a 100644
--- a/utils/datasmoother.h
+++ b/include/gmapping/utils/datasmoother.h
@@ -5,7 +5,7 @@
 #include <stdio.h>
 #include <math.h>
 #include <values.h>
-#include "stat.h"
+#include "gmapping/utils/stat.h"
 #include <assert.h>
 
 namespace GMapping {
diff --git a/utils/dmatrix.h b/include/gmapping/utils/dmatrix.h
similarity index 100%
rename from utils/dmatrix.h
rename to include/gmapping/utils/dmatrix.h
diff --git a/utils/gvalues.h b/include/gmapping/utils/gvalues.h
similarity index 100%
rename from utils/gvalues.h
rename to include/gmapping/utils/gvalues.h
diff --git a/utils/macro_params.h b/include/gmapping/utils/macro_params.h
similarity index 100%
rename from utils/macro_params.h
rename to include/gmapping/utils/macro_params.h
diff --git a/utils/movement.h b/include/gmapping/utils/movement.h
similarity index 97%
rename from utils/movement.h
rename to include/gmapping/utils/movement.h
index 23a1711..aabe36d 100644
--- a/utils/movement.h
+++ b/include/gmapping/utils/movement.h
@@ -1,7 +1,7 @@
 #ifndef FSRMOVEMENT_H
 #define FSRMOVEMENT_H
 
-#include "point.h"
+#include "gmapping/utils/point.h"
 
 namespace GMapping {
 
diff --git a/utils/optimizer.h b/include/gmapping/utils/optimizer.h
similarity index 99%
rename from utils/optimizer.h
rename to include/gmapping/utils/optimizer.h
index b6b1d57..3d1113b 100644
--- a/utils/optimizer.h
+++ b/include/gmapping/utils/optimizer.h
@@ -1,7 +1,7 @@
 #ifndef _OPTIMIZER_H_
 #define _OPTIMIZER_H_
 
-#include "point.h"
+#include "gmapping/utils/point.h"
 
 namespace GMapping {
 
diff --git a/utils/orientedboundingbox.h b/include/gmapping/utils/orientedboundingbox.h
similarity index 81%
rename from utils/orientedboundingbox.h
rename to include/gmapping/utils/orientedboundingbox.h
index 9e232b4..db74474 100644
--- a/utils/orientedboundingbox.h
+++ b/include/gmapping/utils/orientedboundingbox.h
@@ -5,7 +5,7 @@
 #include <stdio.h>
 #include <math.h>
 
-#include <utils/point.h>
+#include <gmapping/utils/point.h>
 
 namespace GMapping{
 
@@ -23,7 +23,7 @@ class OrientedBoundingBox {
   Point lr;  
 };
 
-#include "orientedboundingbox.hxx"
+#include "gmapping/utils/orientedboundingbox.hxx"
 
 };// end namespace
 
diff --git a/utils/orientedboundingbox.hxx b/include/gmapping/utils/orientedboundingbox.hxx
similarity index 100%
rename from utils/orientedboundingbox.hxx
rename to include/gmapping/utils/orientedboundingbox.hxx
diff --git a/utils/point.h b/include/gmapping/utils/point.h
similarity index 99%
rename from utils/point.h
rename to include/gmapping/utils/point.h
index 92bab48..3735aed 100644
--- a/utils/point.h
+++ b/include/gmapping/utils/point.h
@@ -3,7 +3,7 @@
 #include <assert.h>
 #include <math.h>
 #include <iostream>
-#include "gvalues.h"
+#include "gmapping/utils/gvalues.h"
 
 #define DEBUG_STREAM cerr << __PRETTY_FUNCTION__ << ":" //FIXME
 
diff --git a/utils/printmemusage.h b/include/gmapping/utils/printmemusage.h
similarity index 100%
rename from utils/printmemusage.h
rename to include/gmapping/utils/printmemusage.h
diff --git a/utils/printpgm.h b/include/gmapping/utils/printpgm.h
similarity index 100%
rename from utils/printpgm.h
rename to include/gmapping/utils/printpgm.h
diff --git a/utils/stat.h b/include/gmapping/utils/stat.h
similarity index 98%
rename from utils/stat.h
rename to include/gmapping/utils/stat.h
index 2616eef..62e14fa 100644
--- a/utils/stat.h
+++ b/include/gmapping/utils/stat.h
@@ -1,8 +1,8 @@
 #ifndef STAT_H
 #define STAT_H
-#include "point.h"
+#include "gmapping/utils/point.h"
 #include <vector>
-#include "gvalues.h"
+#include "gmapping/utils/gvalues.h"
 
 namespace GMapping {
 
diff --git a/log/carmenconfiguration.cpp b/log/carmenconfiguration.cpp
index 13784e3..7cac05e 100644
--- a/log/carmenconfiguration.cpp
+++ b/log/carmenconfiguration.cpp
@@ -1,11 +1,11 @@
 #include <cstdlib>
-#include "carmenconfiguration.h"
+#include "gmapping/log/carmenconfiguration.h"
 #include <iostream>
 #include <sstream>
 #include <assert.h>
 #include <sys/types.h>
-#include <sensor_odometry/odometrysensor.h>
-#include <sensor_range/rangesensor.h>
+#include <gmapping/sensor/sensor_odometry/odometrysensor.h>
+#include <gmapping/sensor/sensor_range/rangesensor.h>
 
 
 #define LINEBUFFER_SIZE 10000
diff --git a/log/configuration.cpp b/log/configuration.cpp
index 9c81c5e..a31d7fb 100644
--- a/log/configuration.cpp
+++ b/log/configuration.cpp
@@ -1,4 +1,4 @@
-#include "configuration.h"
+#include "gmapping/log/configuration.h"
 
 namespace GMapping {
 
diff --git a/log/log_plot.cpp b/log/log_plot.cpp
index ad42861..e3a4cc4 100644
--- a/log/log_plot.cpp
+++ b/log/log_plot.cpp
@@ -2,8 +2,8 @@
 #include <fstream>
 #include <iostream>
 #include <sys/types.h>
-#include <log/carmenconfiguration.h>
-#include <log/sensorlog.h>
+#include <gmapping/log/carmenconfiguration.h>
+#include <gmapping/log/sensorlog.h>
 
 
 using namespace std;
diff --git a/log/log_test.cpp b/log/log_test.cpp
index 46aab1e..e377b3a 100644
--- a/log/log_test.cpp
+++ b/log/log_test.cpp
@@ -1,8 +1,8 @@
 #include <cstdlib>
 #include <fstream>
 #include <iostream>
-#include <log/carmenconfiguration.h>
-#include <log/sensorlog.h>
+#include <gmapping/log/carmenconfiguration.h>
+#include <gmapping/log/sensorlog.h>
 
 
 using namespace std;
diff --git a/log/rdk2carmen.cpp b/log/rdk2carmen.cpp
index 199c6f7..f25e8aa 100644
--- a/log/rdk2carmen.cpp
+++ b/log/rdk2carmen.cpp
@@ -1,8 +1,8 @@
 #include <cstdlib>
 #include <fstream>
 #include <iostream>
-#include <log/carmenconfiguration.h>
-#include <log/sensorlog.h>
+#include <gmapping/log/carmenconfiguration.h>
+#include <gmapping/log/sensorlog.h>
 
 
 using namespace std;
diff --git a/log/scanstudio2carmen.cpp b/log/scanstudio2carmen.cpp
index 1fdde8a..0407983 100644
--- a/log/scanstudio2carmen.cpp
+++ b/log/scanstudio2carmen.cpp
@@ -3,7 +3,7 @@
 #include <fstream>
 #include <sstream>
 #include <assert.h>
-#include <utils/point.h>
+#include <gmapping/utils/point.h>
 
 #define MAXLINELENGHT (10240)
 #define MAXREADINGS   (10240)
diff --git a/log/sensorlog.cpp b/log/sensorlog.cpp
index ca5892d..2044d0e 100644
--- a/log/sensorlog.cpp
+++ b/log/sensorlog.cpp
@@ -1,10 +1,10 @@
-#include "sensorlog.h"
+#include "gmapping/log/sensorlog.h"
 
 #include <iostream>
 #include <sstream>
 #include <assert.h>
-#include <sensor_odometry/odometrysensor.h>
-#include <sensor_range/rangesensor.h>
+#include <gmapping/sensor/sensor_odometry/odometrysensor.h>
+#include <gmapping/sensor/sensor_range/rangesensor.h>
 
 #define LINEBUFFER_SIZE 100000
 
diff --git a/log/sensorstream.cpp b/log/sensorstream.cpp
index 3c27b04..f527ced 100644
--- a/log/sensorstream.cpp
+++ b/log/sensorstream.cpp
@@ -1,6 +1,6 @@
 #include <assert.h>
 #include <sstream>
-#include "sensorstream.h"
+#include "gmapping/log/sensorstream.h"
 //#define LINEBUFFER_SIZE 1000000 //for not Cyrill to unbless me, it is better to exagerate :-))
 // Can't declare a buffer that big on the stack.  So we'll risk Cyrill's
 // unblessing, and make it smaller.
diff --git a/package.xml b/package.xml
new file mode 100644
index 0000000..39f2704
--- /dev/null
+++ b/package.xml
@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>openslam_gmapping</name>
+  <version>0.1.1</version>
+  <description>The catkinized verseion of openslam_gmapping package (https://github.com/OpenSLAM-org/openslam_gmapping/tree/79ef0b0e6d9a12d6390ae64c4c00d37d776abefb)</description>
+
+  <maintainer email="ros-orphaned-packages@googlegroups.com">ROS Orphaned Package Maintainers</maintainer>
+  <license>BSD</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>Cyrill Stachniss</author>
+  <author>Udo Frese</author>
+  <author>Giorgio Grisetti</author>
+  <author>Wolfram Burgard</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+</package>
diff --git a/particlefilter/particlefilter_test.cpp b/particlefilter/particlefilter_test.cpp
index 28defed..e920e70 100644
--- a/particlefilter/particlefilter_test.cpp
+++ b/particlefilter/particlefilter_test.cpp
@@ -1,7 +1,7 @@
 #include <stdlib.h>
 #include <iostream>
 #include <fstream>
-#include "particlefilter.h"
+#include "gmapping/particlefilter/particlefilter.h"
 
 using namespace std;
 
diff --git a/particlefilter/range_bearing.cpp b/particlefilter/range_bearing.cpp
index 44f99e0..ac68715 100644
--- a/particlefilter/range_bearing.cpp
+++ b/particlefilter/range_bearing.cpp
@@ -1,8 +1,8 @@
 #include <stdlib.h>
 #include <iostream>
 #include <fstream>
-#include <utils/point.h>
-#include "particlefilter.h"
+#include <gmapping/utils/point.h>
+#include "gmapping/particlefilter/particlefilter.h"
 
 using namespace std;
 using namespace GMapping;
diff --git a/scanmatcher/icptest.cpp b/scanmatcher/icptest.cpp
index 9e1644a..9b5697f 100644
--- a/scanmatcher/icptest.cpp
+++ b/scanmatcher/icptest.cpp
@@ -3,7 +3,7 @@
 #include <fstream>
 
 #include <list>
-#include "icp.h"
+#include "gmapping/scanmatcher/icp.h"
 
 using namespace GMapping;
 using namespace std;
diff --git a/scanmatcher/scanmatch_test.cpp b/scanmatcher/scanmatch_test.cpp
index 371475a..a9a3ccc 100644
--- a/scanmatcher/scanmatch_test.cpp
+++ b/scanmatcher/scanmatch_test.cpp
@@ -1,12 +1,12 @@
 #include <cstdlib>
 #include <fstream>
 #include <iostream>
-#include <log/carmenconfiguration.h>
-#include <log/sensorlog.h>
+#include <gmapping/log/carmenconfiguration.h>
+#include <gmapping/log/sensorlog.h>
 #include <unistd.h>
-#include <utils/commandline.h>
-#include <log/sensorstream.h>
-#include "scanmatcherprocessor.h"
+#include <gmapping/utils/commandline.h>
+#include <gmapping/log/sensorstream.h>
+#include "gmapping/scanmatcher/scanmatcherprocessor.h"
 
 using namespace std;
 using namespace GMapping;
diff --git a/scanmatcher/scanmatcher.cpp b/scanmatcher/scanmatcher.cpp
index d819f16..5d7f291 100644
--- a/scanmatcher/scanmatcher.cpp
+++ b/scanmatcher/scanmatcher.cpp
@@ -3,8 +3,8 @@
 #include <list>
 #include <iostream>
 
-#include "scanmatcher.h"
-#include "gridlinetraversal.h"
+#include "gmapping/scanmatcher/scanmatcher.h"
+#include "gmapping/scanmatcher/gridlinetraversal.h"
 //#define GENERATE_MAPS
 
 namespace GMapping {
@@ -46,6 +46,12 @@ ScanMatcher::ScanMatcher(): m_laserPose(0,0,0){
 	m_lasamplestep=0.01;
 	m_generateMap=false;
 */
+
+   m_linePoints = new IntPoint[20000];
+}
+
+ScanMatcher::~ScanMatcher(){
+	delete [] m_linePoints;
 }
 
 void ScanMatcher::invalidateActiveArea(){
@@ -127,7 +133,7 @@ void ScanMatcher::computeActiveArea(ScanMatcherMap& map, const OrientedPoint& p,
 	/*determine the size of the area*/
 	const double * angle=m_laserAngles+m_initialBeamsSkip;
 	for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++){
-		if (*r>m_laserMaxRange) continue;
+		if (*r>m_laserMaxRange||*r==0.0||isnan(*r)) continue;
 		double d=*r>m_usableRange?m_usableRange:*r;
 		Point phit=lp;
 		phit.x+=d*cos(lp.theta+*angle);
@@ -159,7 +165,7 @@ void ScanMatcher::computeActiveArea(ScanMatcherMap& map, const OrientedPoint& p,
 	for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++)
 		if (m_generateMap){
 			double d=*r;
-			if (d>m_laserMaxRange)
+			if (d>m_laserMaxRange||d==0.0||isnan(d))
 				continue;
 			if (d>m_usableRange)
 				d=m_usableRange;
@@ -167,14 +173,14 @@ void ScanMatcher::computeActiveArea(ScanMatcherMap& map, const OrientedPoint& p,
 			IntPoint p0=map.world2map(lp);
 			IntPoint p1=map.world2map(phit);
 			
-			IntPoint linePoints[20000] ;
+			//IntPoint linePoints[20000] ;
 			GridLineTraversalLine line;
-			line.points=linePoints;
+			line.points=m_linePoints;
 			GridLineTraversal::gridLine(p0, p1, &line);
 			for (int i=0; i<line.num_points-1; i++){
-				assert(map.isInside(linePoints[i]));
-				activeArea.insert(map.storage().patchIndexes(linePoints[i]));
-				assert(linePoints[i].x>=0 && linePoints[i].y>=0);
+				assert(map.isInside(m_linePoints[i]));
+				activeArea.insert(map.storage().patchIndexes(m_linePoints[i]));
+				assert(m_linePoints[i].x>=0 && m_linePoints[i].y>=0);
 			}
 			if (d<m_usableRange){
 				IntPoint cp=map.storage().patchIndexes(p1);
@@ -182,7 +188,7 @@ void ScanMatcher::computeActiveArea(ScanMatcherMap& map, const OrientedPoint& p,
 				activeArea.insert(cp);
 			}
 		} else {
-			if (*r>m_laserMaxRange||*r>m_usableRange) continue;
+			if (*r>m_laserMaxRange||*r>m_usableRange||*r==0.0||isnan(*r)) continue;
 			Point phit=lp;
 			phit.x+=*r*cos(lp.theta+*angle);
 			phit.y+=*r*sin(lp.theta+*angle);
@@ -225,15 +231,15 @@ double ScanMatcher::registerScan(ScanMatcherMap& map, const OrientedPoint& p, co
 	for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++)
 		if (m_generateMap){
 			double d=*r;
-			if (d>m_laserMaxRange)
+			if (d>m_laserMaxRange||d==0.0||isnan(d))
 				continue;
 			if (d>m_usableRange)
 				d=m_usableRange;
 			Point phit=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle));
 			IntPoint p1=map.world2map(phit);
-			IntPoint linePoints[20000] ;
+			//IntPoint linePoints[20000] ;
 			GridLineTraversalLine line;
-			line.points=linePoints;
+			line.points=m_linePoints;
 			GridLineTraversal::gridLine(p0, p1, &line);
 			for (int i=0; i<line.num_points-1; i++){
 				PointAccumulator& cell=map.cell(line.points[i]);
@@ -249,7 +255,7 @@ double ScanMatcher::registerScan(ScanMatcherMap& map, const OrientedPoint& p, co
 				esum+=e;
 			}
 		} else {
-			if (*r>m_laserMaxRange||*r>m_usableRange) continue;
+			if (*r>m_laserMaxRange||*r>m_usableRange||*r==0.0||isnan(*r)) continue;
 			Point phit=lp;
 			phit.x+=*r*cos(lp.theta+*angle);
 			phit.y+=*r*sin(lp.theta+*angle);
diff --git a/scanmatcher/scanmatcher.new.cpp b/scanmatcher/scanmatcher.new.cpp
index 0fdf303..5afdd0e 100644
--- a/scanmatcher/scanmatcher.new.cpp
+++ b/scanmatcher/scanmatcher.new.cpp
@@ -1,8 +1,8 @@
 #include <list>
 #include <iostream>
 
-#include "scanmatcher.h"
-#include "gridlinetraversal.h"
+#include "gmapping/scanmatcher/scanmatcher.h"
+#include "gmapping/scanmatcher/gridlinetraversal.h"
 //#define GENERATE_MAPS
 
 using namespace std;
diff --git a/scanmatcher/scanmatcherprocessor.cpp b/scanmatcher/scanmatcherprocessor.cpp
index 05f3dee..509db0f 100644
--- a/scanmatcher/scanmatcherprocessor.cpp
+++ b/scanmatcher/scanmatcherprocessor.cpp
@@ -1,6 +1,6 @@
 #include <iostream>
-#include "scanmatcherprocessor.h"
-#include "eig3.h"
+#include "gmapping/scanmatcher/scanmatcherprocessor.h"
+#include "gmapping/scanmatcher/eig3.h"
 
 //#define SCANMATHCERPROCESSOR_DEBUG
 namespace GMapping {
diff --git a/scanmatcher/smmap.cpp b/scanmatcher/smmap.cpp
index 365c76d..0a8d59a 100644
--- a/scanmatcher/smmap.cpp
+++ b/scanmatcher/smmap.cpp
@@ -1,4 +1,4 @@
-#include "smmap.h"
+#include "gmapping/scanmatcher/smmap.h"
 
 namespace GMapping {
 
diff --git a/sensor/sensor_base/sensor.cpp b/sensor/sensor_base/sensor.cpp
index a66eb1d..9f8cbfe 100644
--- a/sensor/sensor_base/sensor.cpp
+++ b/sensor/sensor_base/sensor.cpp
@@ -1,4 +1,4 @@
-#include "sensor.h"
+#include "gmapping/sensor/sensor_base/sensor.h"
 
 namespace GMapping{
 
diff --git a/sensor/sensor_base/sensorreading.cpp b/sensor/sensor_base/sensorreading.cpp
index f61bca8..d98ea04 100644
--- a/sensor/sensor_base/sensorreading.cpp
+++ b/sensor/sensor_base/sensorreading.cpp
@@ -1,4 +1,4 @@
-#include "sensorreading.h"
+#include "gmapping/sensor/sensor_base/sensorreading.h"
 
 namespace GMapping{
 
diff --git a/sensor/sensor_odometry/odometryreading.cpp b/sensor/sensor_odometry/odometryreading.cpp
index f1ff756..a19b3de 100644
--- a/sensor/sensor_odometry/odometryreading.cpp
+++ b/sensor/sensor_odometry/odometryreading.cpp
@@ -1,4 +1,4 @@
-#include "odometryreading.h"
+#include "gmapping/sensor/sensor_odometry/odometryreading.h"
 
 namespace GMapping{
 
diff --git a/sensor/sensor_odometry/odometrysensor.cpp b/sensor/sensor_odometry/odometrysensor.cpp
index 62fa91d..77349d7 100644
--- a/sensor/sensor_odometry/odometrysensor.cpp
+++ b/sensor/sensor_odometry/odometrysensor.cpp
@@ -1,4 +1,4 @@
-#include "odometrysensor.h"
+#include "gmapping/sensor/sensor_odometry/odometrysensor.h"
 
 namespace GMapping{
 
diff --git a/sensor/sensor_range/rangereading.cpp b/sensor/sensor_range/rangereading.cpp
index 2edbcdf..df74d30 100644
--- a/sensor/sensor_range/rangereading.cpp
+++ b/sensor/sensor_range/rangereading.cpp
@@ -2,8 +2,8 @@
 #include <iostream>
 #include <assert.h>
 #include <sys/types.h>
-#include <utils/gvalues.h>
-#include "rangereading.h"
+#include <gmapping/utils/gvalues.h>
+#include "gmapping/sensor/sensor_range/rangereading.h"
 
 namespace GMapping{
 
diff --git a/sensor/sensor_range/rangesensor.cpp b/sensor/sensor_range/rangesensor.cpp
index 28bea27..5b259f5 100644
--- a/sensor/sensor_range/rangesensor.cpp
+++ b/sensor/sensor_range/rangesensor.cpp
@@ -1,4 +1,4 @@
-#include "rangesensor.h"
+#include "gmapping/sensor/sensor_range/rangesensor.h"
 
 namespace GMapping{
 
diff --git a/utils/autoptr_test.cpp b/utils/autoptr_test.cpp
index c270c81..4103397 100644
--- a/utils/autoptr_test.cpp
+++ b/utils/autoptr_test.cpp
@@ -1,5 +1,5 @@
 #include <iostream>
-#include "autoptr.h"
+#include "gmapping/utils/autoptr.h"
 
 using namespace std;
 using namespace GMapping;
diff --git a/utils/movement.cpp b/utils/movement.cpp
index 0870c9f..b5372df 100644
--- a/utils/movement.cpp
+++ b/utils/movement.cpp
@@ -1,5 +1,5 @@
-#include "movement.h"
-#include "gvalues.h"
+#include "gmapping/utils/movement.h"
+#include "gmapping/utils/gvalues.h"
 
 namespace GMapping {
 
diff --git a/utils/printmemusage.cpp b/utils/printmemusage.cpp
index 44f4664..483bd59 100644
--- a/utils/printmemusage.cpp
+++ b/utils/printmemusage.cpp
@@ -1,4 +1,4 @@
-#include "printmemusage.h"
+#include "gmapping/utils/printmemusage.h"
 
 namespace GMapping{
 
diff --git a/utils/stat.cpp b/utils/stat.cpp
index 27d3143..30536c4 100644
--- a/utils/stat.cpp
+++ b/utils/stat.cpp
@@ -5,7 +5,7 @@
 //#include <gsl/gsl_eigen.h>
 //#include <gsl/gsl_blas.h>
 #include <math.h>
-#include "utils/stat.h"
+#include "gmapping/utils/stat.h"
 
 namespace GMapping {
 
diff --git a/utils/stat_test.cpp b/utils/stat_test.cpp
index a49dccb..5adcf87 100644
--- a/utils/stat_test.cpp
+++ b/utils/stat_test.cpp
@@ -1,7 +1,7 @@
 #include <iostream>
 #include <fstream>
 #include <math.h>
-#include "stat.h"
+#include "gmapping/utils/stat.h"
 
 using namespace std;
 using namespace GMapping;