diff --git a/launch/demo/demo_robot_mapping.launch b/launch/demo/demo_robot_mapping.launch
index 1e41ca5a..55dae5d1 100644
--- a/launch/demo/demo_robot_mapping.launch
+++ b/launch/demo/demo_robot_mapping.launch
@@ -46,6 +46,8 @@
 	  <param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/> <!-- Optimize graph from initial node so /map -> /odom transform will be generated -->
 	  <param name="Reg/Force3DoF"             type="string" value="true"/>
 	  <param name="Grid/FromDepth"            type="string" value="false"/>
+          <param name="Mem/STMSize"               type="string" value="30"/>    <!--increased to 30 to avoid adding too many loop closures on just seen locations -->
+          <param name="RGBD/LocalRadius"          type="string" value="5"/>     <!--limit length of proximity detections -->
 	  
 	  <!-- localization mode -->
 	  <param     if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
diff --git a/launch/rtabmap.launch b/launch/rtabmap.launch
index 40443463..f751e275 100644
--- a/launch/rtabmap.launch
+++ b/launch/rtabmap.launch
@@ -291,7 +291,7 @@
     <remap from="cloud"             to="voxel_cloud" />
 
     <param name="decimation"  type="double" value="2"/>
-    <param name="voxel_size"  type="double" value="0.02"/>
+    <param name="voxel_size"  type="double" value="0.0"/>
     <param name="approx_sync" type="bool"   value="$(arg approx_sync)"/>
   </node>
 
diff --git a/src/MapsManager.cpp b/src/MapsManager.cpp
index 89134df8..491ee705 100644
--- a/src/MapsManager.cpp
+++ b/src/MapsManager.cpp
@@ -110,7 +110,7 @@ void MapsManager::init(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::s
 #ifdef RTABMAP_OCTOMAP
 	pnh.param("octomap_occupancy_thr", octomapOccupancyThr_, octomapOccupancyThr_);
 	UASSERT(octomapOccupancyThr_>=0.0 && octomapOccupancyThr_<=1.0);
-	octomap_ = new OctoMap(occupancyGrid_->getCellSize(), octomapOccupancyThr_);
+	octomap_ = new OctoMap(occupancyGrid_->getCellSize(), octomapOccupancyThr_, occupancyGrid_->isFullUpdate());
 	pnh.param("octomap_tree_depth", octomapTreeDepth_, octomapTreeDepth_);
 	if(octomapTreeDepth_ > 16)
 	{
@@ -287,7 +287,7 @@ void MapsManager::setParameters(const rtabmap::ParametersMap & parameters)
 		delete octomap_;
 		octomap_ = 0;
 	}
-	octomap_ = new OctoMap(occupancyGrid_->getCellSize(), octomapOccupancyThr_);
+	octomap_ = new OctoMap(occupancyGrid_->getCellSize(), octomapOccupancyThr_, occupancyGrid_->isFullUpdate());
 #endif
 #endif
 }