diff --git a/launch/demo/demo_appearance_localization.launch b/launch/demo/demo_appearance_localization.launch
deleted file mode 100644
index 97b6949f..00000000
--- a/launch/demo/demo_appearance_localization.launch
+++ /dev/null
@@ -1,47 +0,0 @@
-<launch>
-	
-	<!-- APPEARANCE-BASED LOCALIZATION VERSION -->
-
-  <group ns="rtabmap">
-	<!-- args: "delete_db_on_start" and "udebug" -->
-	<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="">
-	
-	    <param name="subscribe_depth" type="bool" value="false"/>        <!-- must be false for appearance-based mode -->
-        <param name="subscribe_laserScan" type="bool" value="false"/>    <!-- must be false for appearance-based mode -->
-        <param name="queue_size" type="int" value="10"/>
-    
-        <remap from="rgb/image" to="/image"/>                             <!-- connect to "image" topic of the camera below -->
-      
-		<!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
-	    <param name="RGBD/Enabled" type="string" value="false"/>          <!-- False: appearance-based -->
-	    <param name="Rtabmap/DatabasePath" type="string" value="~/.ros/rtabmap.db"/>  <!-- Database used for localization -->
-		<param name="Rtabmap/ImageBufferSize" type="string" value="0"/>   <!-- process all images -->
-		<param name="Rtabmap/DetectionRate" type="string" value="0"/>     <!-- Go as fast as the camera rate (here 2 Hz, see below) -->
-		<param name="Mem/STMSize" type="string" value="1"/>               <!-- 1 location in short-term memory -->
-		<param name="Mem/IncrementalMemory" type="string" value="false"/>  <!-- false = Localization mode-->
-		<param name="Mem/BadSignaturesIgnored" type="string" value="true"/>
-		<param name="Kp/DetectorStrategy" type="string" value="0"/>   <!-- use SURF -->
-		<param name="Kp/NNStrategy" type="string" value="1"/>         <!-- kdTree -->
-	</node>
-	
-	<!-- visualization of the "infoEx" topic sent by rtabmap node -->
-	<node name="rtabmapviz" pkg="rtabmap_ros" type="rtabmapviz" output="screen" args="-d $(find rtabmap_ros)/launch/config/appearance_gui.ini">
-	    
-	    <!-- This enables the GUI to pause a rtabmap_ros/camera when action "pause" is checked. -->
-	    <!-- NOTE: It is specific to rtabmap_ros/camera. Action "pause" in the GUI will still pause the rtabmap node. -->
-		<param name="camera_node_name" type="string" value="/camera"/>      
-		 
-	</node>
-  </group>
-  
-	<!-- When parameter video_or_images_path is set, the camera uses the directory of images or the video file -->
-	<node name="camera" pkg="rtabmap_ros" type="camera" output="screen">
-		<remap from="image" to="image"/>
-		<param name="device_id" value="0" type="int"/>
-		<param name="video_or_images_path" value="$(find rtabmap_ros)/launch/data/demo_appearance" type="string"/>
-		<param name="frame_rate" value="2.0" type="double"/>
-		<param name="width" value="0" type="int"/>
-		<param name="height" value="0" type="int"/>
-		<param name="auto_restart" value="false" type="bool"/> <!-- Process only one time the data set -->
-	</node>
-</launch>
diff --git a/launch/demo/demo_appearance_mapping.launch b/launch/demo/demo_appearance_mapping.launch
index 78f5f102..5a6d555c 100644
--- a/launch/demo/demo_appearance_mapping.launch
+++ b/launch/demo/demo_appearance_mapping.launch
@@ -4,27 +4,35 @@
 	<!-- WARNING : Database is automatically deleted on each startup -->
 	<!--           See "delete_db_on_start" option below... -->
 
+  <!-- Localization-only mode -->
+  <arg name="localization"      default="false"/>
+  <arg     if="$(arg localization)" name="rtabmap_args"  default=""/>
+  <arg unless="$(arg localization)" name="rtabmap_args"  default="--delete_db_on_start"/>
+
   <group ns="rtabmap">
 	<!-- args: "delete_db_on_start" and "udebug" -->
-	<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
+	<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
 	
-	    <param name="subscribe_depth" type="bool" value="false"/>        <!-- must be false for appearance-based mode -->
+	    <param name="subscribe_depth"     type="bool" value="false"/>    <!-- must be false for appearance-based mode -->
         <param name="subscribe_laserScan" type="bool" value="false"/>    <!-- must be false for appearance-based mode -->
-        <param name="queue_size" type="int" value="10"/>
+        <param name="queue_size"          type="int" value="10"/>
     
         <remap from="rgb/image" to="/image"/>                             <!-- connect to "image" topic of the camera below -->
       
 		<!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
-	    <param name="RGBD/Enabled" type="string" value="false"/>          <!-- False: appearance-based -->
-		<param name="Rtabmap/ImageBufferSize" type="string" value="0"/>   <!-- process all images -->
-		<param name="Rtabmap/DetectionRate" type="string" value="0"/>     <!-- Go as fast as the camera rate (here 2 Hz, see below) -->
-		<param name="Mem/RehearsalSimilarity" type="string" value="0.4"/> <!-- 40% -->
-		<param name="Mem/STMSize" type="string" value="15"/>              <!-- 15 locations in short-term memory -->
-		<param name="Mem/IncrementalMemory" type="string" value="true"/>  <!-- true = SLAM mode -->
-		<param name="Mem/RehearsalIdUpdatedToNewOne" type="string" value="true"/>  <!-- On merging, update to new ID-->
-		<param name="Mem/BadSignaturesIgnored" type="string" value="true"/>
-		<param name="Kp/DetectorStrategy" type="string" value="0"/>   <!-- use SURF -->
-		<param name="Kp/NNStrategy" type="string" value="1"/>         <!-- kdTree -->
+	    <param name="RGBD/Enabled"                   type="string" value="false"/>  <!-- False: appearance-based -->
+		<param name="Rtabmap/ImageBufferSize"        type="string" value="0"/>      <!-- process all images -->
+		<param name="Rtabmap/DetectionRate"          type="string" value="0"/>      <!-- Go as fast as the camera rate (here 2 Hz, see below) -->
+		<param name="Mem/RehearsalSimilarity"        type="string" value="0.4"/>    <!-- 40% -->
+		<param name="Mem/STMSize"                    type="string" value="15"/>     <!-- 15 locations in short-term memory -->
+		<param name="Mem/RehearsalIdUpdatedToNewOne" type="string" value="true"/>   <!-- On merging, update to new ID-->
+		<param name="Mem/BadSignaturesIgnored"       type="string" value="true"/>
+		<param name="Kp/DetectorStrategy"            type="string" value="0"/>      <!-- use SURF -->
+		
+		<!-- localization mode -->
+	    <param     if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
+	    <param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
+	    <param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
 	</node>
 	
 	<!-- visualization of the "infoEx" topic sent by rtabmap node -->
@@ -40,11 +48,12 @@
 	<!-- When parameter video_or_images_path is set, the camera uses the directory of images or the video file -->
 	<node name="camera" pkg="rtabmap_ros" type="camera" output="screen">
 		<remap from="image" to="image"/>
-		<param name="device_id" value="0" type="int"/>
+		
+		<param name="device_id"            value="0" type="int"/>
 		<param name="video_or_images_path" value="$(find rtabmap_ros)/launch/data/demo_appearance" type="string"/>
-		<param name="frame_rate" value="2.0" type="double"/>
-		<param name="width" value="0" type="int"/>
-		<param name="height" value="0" type="int"/>
-		<param name="auto_restart" value="false" type="bool"/> <!-- Process only one time the data set -->
+		<param name="frame_rate"           value="2.0" type="double"/>
+		<param name="width"                value="0" type="int"/>
+		<param name="height"               value="0" type="int"/>
+		<param name="auto_restart"         value="false" type="bool"/> <!-- Process only one time the data set -->
 	</node>
 </launch>
diff --git a/launch/demo/demo_find_object.launch b/launch/demo/demo_find_object.launch
index f48c7a5f..abe1d058 100644
--- a/launch/demo/demo_find_object.launch
+++ b/launch/demo/demo_find_object.launch
@@ -19,46 +19,48 @@
 	  <remap from="odom" to="/base_controller/odom"/>
 	  <remap from="scan" to="/base_scan"/>
 
-	  <remap from="rgb/image" to="/camera/data_throttled_image"/>
-  	  <remap from="depth/image" to="/camera/data_throttled_image_depth"/>
+	  <remap from="rgb/image"       to="/camera/data_throttled_image"/>
+  	  <remap from="depth/image"     to="/camera/data_throttled_image_depth"/>
   	  <remap from="rgb/camera_info" to="/camera/data_throttled_camera_info"/>
   	  
-  	  <param name="rgb/image_transport" type="string" value="compressed"/>
+  	  <param name="rgb/image_transport"   type="string" value="compressed"/>
       <param name="depth/image_transport" type="string" value="compressedDepth"/>
 	
 	  <param name="queue_size" type="int" value="10"/>
 	
 	  <!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
 	  <param name="RGBD/NeighborLinkRefining" type="string" value="true"/>  <!-- Do odometry correction with consecutive laser scans -->
-	  <param name="RGBD/ProximityBySpace" type="string" value="true"/>      <!-- Proximity detection (using estimated position) with locations in WM -->
-	  <param name="Mem/BadSignaturesIgnored" type="string" value="false"/>  <!-- Don't ignore bad images for 3D node creation (e.g. white walls) -->
-	  <param name="Reg/Strategy" type="string" value="1"/>                  <!-- Registration strategy: 0=visual, 1=ICP, 2=visual+ICP -->
-	  <param name="Icp/CorrespondenceRatio" type="string" value="0.3"/>
-	  <param name="Icp/Iterations" type="string" value="30"/>
-	  <param name="Icp/VoxelSize" type="string" value="0.025"/>
-	  <param name="Vis/MinInliers" type="string" value="10"/>                <!-- 3D visual words minimum inliers to accept loop closure -->
-	  <param name="Vis/MaxDepth" type="string" value="4.0"/>                 <!-- 3D visual words maximum depth 0=infinity -->
-	  <param name="Vis/InlierDistance" type="string" value="0.1"/>           <!-- 3D visual words correspondence distance -->
-	  <param name="RGBD/AngularUpdate" type="string" value="0.01"/>             <!-- Update map only if the robot is moving -->
-	  <param name="RGBD/LinearUpdate" type="string" value="0.01"/>              <!-- Update map only if the robot is moving -->
-	  <param name="Rtabmap/TimeThr" type="string" value="700"/>
-	  <param name="Mem/RehearsalSimilarity" type="string" value="0.45"/>
-	  <param name="Mem/NotLinkedNodesKept" type="string" value="false"/>   
+	  <param name="RGBD/ProximityBySpace"     type="string" value="true"/>  <!-- Proximity detection (using estimated position) with locations in WM -->
+	  <param name="Mem/BadSignaturesIgnored"  type="string" value="false"/> <!-- Don't ignore bad images for 3D node creation (e.g. white walls) -->
+	  <param name="Reg/Strategy"              type="string" value="1"/>     <!-- Registration strategy: 0=visual, 1=ICP, 2=visual+ICP -->
+	  <param name="Icp/CorrespondenceRatio"   type="string" value="0.3"/>
+	  <param name="Icp/Iterations"            type="string" value="30"/>
+	  <param name="Icp/VoxelSize"             type="string" value="0.025"/>
+	  <param name="Vis/MinInliers"            type="string" value="10"/>    <!-- 3D visual words minimum inliers to accept loop closure -->
+	  <param name="Vis/MaxDepth"              type="string" value="4.0"/>   <!-- 3D visual words maximum depth 0=infinity -->
+	  <param name="Vis/InlierDistance"        type="string" value="0.1"/>   <!-- 3D visual words correspondence distance -->
+	  <param name="RGBD/AngularUpdate"        type="string" value="0.01"/>  <!-- Update map only if the robot is moving -->
+	  <param name="RGBD/LinearUpdate"         type="string" value="0.01"/>  <!-- Update map only if the robot is moving -->
+	  <param name="Rtabmap/TimeThr"           type="string" value="700"/>
+	  <param name="Mem/RehearsalSimilarity"   type="string" value="0.45"/>
+	  <param name="Mem/NotLinkedNodesKept"    type="string" value="false"/>
+	  <param name="Optimizer/Slam2D"          type="string" value="true"/>
+	  <param name="Reg/Force3DoF"             type="string" value="true"/>   
     </node>
     
     <!-- Visualisation RTAB-Map -->
     <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
   	  <param name="subscribe_depth" type="bool" value="true"/>
-      <param name="subscribe_scan" type="bool" value="true"/>
-      <param name="frame_id" type="string" value="base_footprint"/>
+      <param name="subscribe_scan"  type="bool" value="true"/>
+      <param name="frame_id"        type="string" value="base_footprint"/>
     
-      <remap from="rgb/image" to="/camera/data_throttled_image"/>
-      <remap from="depth/image" to="/camera/data_throttled_image_depth"/>
+      <remap from="rgb/image"       to="/camera/data_throttled_image"/>
+      <remap from="depth/image"     to="/camera/data_throttled_image_depth"/>
       <remap from="rgb/camera_info" to="/camera/data_throttled_camera_info"/>
-      <remap from="scan" to="/base_scan"/>
-      <remap from="odom" to="/base_controller/odom"/>
+      <remap from="scan"            to="/base_scan"/>
+      <remap from="odom"            to="/base_controller/odom"/>
       
-      <param name="rgb/image_transport" type="string" value="compressed"/>
+      <param name="rgb/image_transport"   type="string" value="compressed"/>
       <param name="depth/image_transport" type="string" value="compressedDepth"/>
     </node>
     
@@ -78,7 +80,7 @@
     <remap from="rgb/camera_info" to="/camera/data_throttled_camera_info"/>
     <remap from="cloud"           to="voxel_cloud" />
     
-    <param name="rgb/image_transport" type="string" value="compressed"/>
+    <param name="rgb/image_transport"   type="string" value="compressed"/>
     <param name="depth/image_transport" type="string" value="compressedDepth"/>
     
     <param name="queue_size" type="int" value="10"/>
@@ -87,16 +89,16 @@
 
   <!-- Find-Object -->
   <node name="find_object_3d" pkg="find_object_2d" type="find_object_2d" output="screen">
-    <param name="gui" value="true" type="bool"/>
-    <param name="settings_path" value="$(find rtabmap_ros)/launch/config/find_object.ini" type="str"/>
+    <param name="gui"             value="true" type="bool"/>
+    <param name="settings_path"   value="$(find rtabmap_ros)/launch/config/find_object.ini" type="str"/>
     <param name="subscribe_depth" value="true" type="bool"/>
-    <param name="objects_path" value="$(find rtabmap_ros)/launch/data/books" type="str"/>
+    <param name="objects_path"    value="$(find rtabmap_ros)/launch/data/books" type="str"/>
 		
-    <remap from="rgb/image_rect_color" to="/camera/data_throttled_image"/>
-    <remap from="depth_registered/image_raw" to="/camera/data_throttled_image_depth"/>
+    <remap from="rgb/image_rect_color"         to="/camera/data_throttled_image"/>
+    <remap from="depth_registered/image_raw"   to="/camera/data_throttled_image_depth"/>
     <remap from="depth_registered/camera_info" to="/camera/data_throttled_camera_info"/>
 
-    <param name="rgb/image_transport" type="string" value="compressed"/>
+    <param name="rgb/image_transport"              type="string" value="compressed"/>
     <param name="depth_registered/image_transport" type="string" value="compressedDepth"/>
   </node>
 
diff --git a/launch/demo/demo_hector_mapping.launch b/launch/demo/demo_hector_mapping.launch
index 56c8fc4d..40c8ba56 100644
--- a/launch/demo/demo_hector_mapping.launch
+++ b/launch/demo/demo_hector_mapping.launch
@@ -49,37 +49,39 @@
 	  <param name="frame_id" type="string" value="base_footprint"/>
 	
 	  <param name="subscribe_depth" type="bool" value="true"/>
-	  <param name="subscribe_scan" type="bool" value="true"/>
+	  <param name="subscribe_scan"  type="bool" value="true"/>
 	
 	  <remap from="odom" to="/scanmatch_odom"/>
 	  <remap from="scan" to="/jn0/base_scan"/>
 
-	  <remap from="rgb/image" to="/data_throttled_image"/>
-  	  <remap from="depth/image" to="/data_throttled_image_depth"/>
+	  <remap from="rgb/image"       to="/data_throttled_image"/>
+  	  <remap from="depth/image"     to="/data_throttled_image_depth"/>
   	  <remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
   	
       <param name="rgb/image_transport" type="string" value="compressed"/>
       <param name="depth/image_transport" type="string" value="compressedDepth"/>
 	
 	  <!-- RTAB-Map's parameters -->
-	  <param name="Reg/Strategy" type="string" value="1"/>          <!-- 0=Visual, 1=ICP, 2=Visual+ICP -->
-	  <param name="Vis/MaxDepth" type="string" value="10.0"/>       <!-- 3D visual words maximum depth 0=infinity -->
+	  <param name="Reg/Strategy"       type="string" value="1"/>    <!-- 0=Visual, 1=ICP, 2=Visual+ICP -->
+	  <param name="Vis/MaxDepth"       type="string" value="10.0"/> <!-- 3D visual words maximum depth 0=infinity -->
 	  <param name="Vis/InlierDistance" type="string" value="0.1"/>  <!-- 3D visual words correspondence distance -->
+	  <param name="Optimizer/Slam2D"   type="string" value="true"/>
+	  <param name="Reg/Force3DoF"      type="string" value="true"/>
     </node>
     
     <!-- Visualisation RTAB-Map -->
     <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
-  	  <param name="subscribe_depth" type="bool" value="true"/>
+  	  <param name="subscribe_depth"     type="bool" value="true"/>
       <param name="subscribe_laserScan" type="bool" value="true"/>
-      <param name="frame_id" type="string" value="base_footprint"/>
+      <param name="frame_id"            type="string" value="base_footprint"/>
     
-      <remap from="rgb/image" to="/data_throttled_image"/>
-      <remap from="depth/image" to="/data_throttled_image_depth"/>
+      <remap from="rgb/image"       to="/data_throttled_image"/>
+      <remap from="depth/image"     to="/data_throttled_image_depth"/>
       <remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
-      <remap from="scan" to="/jn0/base_scan"/>
-      <remap from="odom" to="/scanmatch_odom"/>
+      <remap from="scan"            to="/jn0/base_scan"/>
+      <remap from="odom"            to="/scanmatch_odom"/>
       
-      <param name="rgb/image_transport" type="string" value="compressed"/>
+      <param name="rgb/image_transport"   type="string" value="compressed"/>
       <param name="depth/image_transport" type="string" value="compressedDepth"/>
     </node>
   
@@ -93,7 +95,7 @@
     <remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
     <remap from="cloud"           to="voxel_cloud" />
     
-    <param name="rgb/image_transport" type="string" value="compressed"/>
+    <param name="rgb/image_transport"   type="string" value="compressed"/>
     <param name="depth/image_transport" type="string" value="compressedDepth"/>
 
     <param name="voxel_size" type="double" value="0.01"/>
diff --git a/launch/demo/demo_multi-session_mapping.launch b/launch/demo/demo_multi-session_mapping.launch
index 42b78d54..cbf3fa24 100644
--- a/launch/demo/demo_multi-session_mapping.launch
+++ b/launch/demo/demo_multi-session_mapping.launch
@@ -17,57 +17,56 @@
 	<param name="frame_id" type="string" value="base_footprint"/>
 	
 	<param name="subscribe_depth" type="bool" value="true"/>
-	<param name="subscribe_scan" type="bool" value="true"/>
+	<param name="subscribe_scan"  type="bool" value="true"/>
 	
 	<remap from="odom" to="/base_controller/odom"/>
 	<remap from="scan" to="/base_scan"/>
 
-	<remap from="rgb/image" to="/data_throttled_image"/>
-  	<remap from="depth/image" to="/data_throttled_image_depth"/>
+	<remap from="rgb/image"       to="/data_throttled_image"/>
+  	<remap from="depth/image"     to="/data_throttled_image_depth"/>
   	<remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
   	
-  	<param name="rgb/image_transport" type="string" value="compressed"/>
+  	<param name="rgb/image_transport"   type="string" value="compressed"/>
     <param name="depth/image_transport" type="string" value="compressedDepth"/>
 	
 	<param name="queue_size" type="int" value="10"/>
 	
 	<!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
-	<param name="RGBD/NeighborLinkRefining" type="string" value="false"/> 
-	<param name="RGBD/ProximityBySpace" type="string" value="false"/> 
-	<param name="RGBD/ProximityByTime" type="string" value="false"/> 
-	<param name="RGBD/OptimizeFromGraphEnd" type="string" value="true"/> 
-	<param name="Mem/BadSignaturesIgnored" type="string" value="false"/>
-	<param name="Reg/Strategy" type="string" value="1"/>
-	<param name="Icp/Iterations" type="string" value="30"/>
-	<param name="Icp/VoxelSize" type="string" value="0"/>
-	<param name="Vis/MinInliers" type="string" value="5"/>
-	<param name="Vis/MaxDepth" type="string" value="4.0"/>
-	<param name="Vis/InlierDistance" type="string" value="0.1"/>
-	<param name="RGBD/AngularUpdate" type="string" value="0.01"/>
-	<param name="RGBD/LinearUpdate" type="string" value="0.01"/>
-	<param name="Rtabmap/TimeThr" type="string" value="700"/>
-	<param name="Mem/RehearsalSimilarity" type="string" value="0.45"/>
-	<param name="Kp/TfIdfLikelihoodUsed" type="string" value="false"/>
+	<param name="RGBD/NeighborLinkRefining"  type="string" value="false"/> 
+	<param name="RGBD/ProximityBySpace"      type="string" value="false"/> 
+	<param name="RGBD/ProximityByTime"       type="string" value="false"/> 
+	<param name="RGBD/OptimizeFromGraphEnd"  type="string" value="true"/> 
+	<param name="Reg/Strategy"               type="string" value="1"/>
+	<param name="Icp/Iterations"             type="string" value="30"/>
+	<param name="Icp/VoxelSize"              type="string" value="0"/>
+	<param name="Vis/MinInliers"             type="string" value="5"/>
+	<param name="Vis/MaxDepth"               type="string" value="4.0"/>
+	<param name="RGBD/AngularUpdate"         type="string" value="0.01"/>
+	<param name="RGBD/LinearUpdate"          type="string" value="0.01"/>
+	<param name="Rtabmap/TimeThr"            type="string" value="700"/>
+	<param name="Mem/RehearsalSimilarity"    type="string" value="0.45"/>
+	<param name="Kp/TfIdfLikelihoodUsed"     type="string" value="false"/>
 	<param name="Bayes/FullPredictionUpdate" type="string" value="true"/>
-	<param name="Kp/DetectorStrategy" type="string" value="0"/>   <!-- use SURF -->
-	<param name="Kp/NNStrategy" type="string" value="1"/>         <!-- kdTree -->
-	<param name="Kp/MaxFeatures" type="string" value="400"/> 
+	<param name="Kp/DetectorStrategy"        type="string" value="0"/>   <!-- use SURF -->
+	<param name="Kp/MaxFeatures"             type="string" value="400"/>
+	<param name="Optimizer/Slam2D"           type="string" value="true"/>
+	<param name="Reg/Force3DoF"              type="string" value="true"/> 
   </node>
   
   <!-- Visualisation RTAB-Map -->
   <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
   	  <param name="subscribe_depth" type="bool" value="true"/>
-      <param name="subscribe_scan" type="bool" value="true"/>
-      <param name="queue_size" type="int" value="10"/>
-      <param name="frame_id" type="string" value="base_footprint"/>
+      <param name="subscribe_scan"  type="bool" value="true"/>
+      <param name="queue_size"      type="int" value="10"/>
+      <param name="frame_id"        type="string" value="base_footprint"/>
     
-      <remap from="rgb/image" to="/data_throttled_image"/>
-      <remap from="depth/image" to="/data_throttled_image_depth"/>
+      <remap from="rgb/image"       to="/data_throttled_image"/>
+      <remap from="depth/image"     to="/data_throttled_image_depth"/>
       <remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
-      <remap from="scan" to="/base_scan"/>
-      <remap from="odom" to="/base_controller/odom"/>
+      <remap from="scan"            to="/base_scan"/>
+      <remap from="odom"            to="/base_controller/odom"/>
       
-      <param name="rgb/image_transport" type="string" value="compressed"/>
+      <param name="rgb/image_transport"   type="string" value="compressed"/>
       <param name="depth/image_transport" type="string" value="compressedDepth"/>
   </node>
   
@@ -81,7 +80,7 @@
     <remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
     <remap from="cloud"           to="voxel_cloud" />
     
-    <param name="rgb/image_transport" type="string" value="compressed"/>
+    <param name="rgb/image_transport"   type="string" value="compressed"/>
     <param name="depth/image_transport" type="string" value="compressedDepth"/>
     
     <param name="queue_size" type="int" value="10"/>
diff --git a/launch/demo/demo_robot_localization.launch b/launch/demo/demo_robot_localization.launch
deleted file mode 100644
index 23b8dbe4..00000000
--- a/launch/demo/demo_robot_localization.launch
+++ /dev/null
@@ -1,83 +0,0 @@
-
-<launch>
-
-  <!-- ROBOT LOCALIZATION VERSION: use this with ROS bag demo_mapping.bag -->
-  <!-- A database "~/.ros/rtabmap.db" must be already created from -->
-  <!-- the "demo_robot_mapping.launch" demo -->
-  <!-- Once RTAB-Map GUI started, you can do "Edit->Download Map" to get all the map in the GUI -->
-   
-  <!-- Choose visualization -->
-  <arg name="rviz" default="false" />
-  <arg name="rtabmapviz" default="true" /> 
-  
-  <param name="use_sim_time" type="bool" value="True"/>
-
-  <group ns="rtabmap">
-    <!-- SLAM (robot side) -->
-    <!-- args: "delete_db_on_start" and "udebug" -->
-    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="">
-	  <param name="frame_id" type="string" value="base_footprint"/>
-	  <param name="wait_for_transform" type="bool" value="true"/>
-	
-	  <param name="subscribe_depth" type="bool" value="true"/>
-	  <param name="subscribe_laserScan" type="bool" value="true"/>
-	
-	  <remap from="odom" to="/az3/base_controller/odom"/>
-	  <remap from="scan" to="/jn0/base_scan"/>
-
-	  <remap from="rgb/image" to="/data_throttled_image"/>
-  	  <remap from="depth/image" to="/data_throttled_image_depth"/>
-  	  <remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
-  	
-  	  <param name="rgb/image_transport" type="string" value="compressed"/>
-      <param name="depth/image_transport" type="string" value="compressedDepth"/>
-	
-	  <!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
-      <param name="Rtabmap/DatabasePath" type="string" value="~/.ros/rtabmap.db"/>  <!-- Database used for localization -->
-      <param name="Rtabmap/DetectionRate" type="string" value="1"/>     <!-- Don't need to do relocation very often! Though better results if the same rate as when mapping. -->
-      <param name="Mem/STMSize" type="string" value="1"/>               <!-- 1 location in short-term memory -->
-      <param name="Mem/IncrementalMemory" type="string" value="false"/>  <!-- false = Localization mode-->
-      <param name="Mem/InitWMWithAllNodes" type="string" value="true"/> <!-- Load the full global map in RAM -->
-      <param name="RGBD/PoseScanMatching" type="string" value="true"/>  <!-- Do odometry correction with consecutive laser scans -->
-	  <param name="RGBD/LocalLoopDetectionSpace" type="string" value="true"/>
-	  <param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/> 
-	  <param name="LccIcp/Type" type="string" value="2"/>                      <!-- 0=No ICP, 1=ICP 3D, 2=ICP 2D -->
-	  <param name="LccIcp2/MaxFitness" type="string" value="10"/>
-	  <param name="LccBow/MaxDepth" type="string" value="0.0"/>                <!-- 3D visual words maximum depth 0=infinity -->
-	  <param name="LccBow/InlierDistance" type="string" value="0.1"/>          <!-- 3D visual words correspondence distance -->
-    </node>
-  
-    <!-- Visualisation RTAB-Map -->
-    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
-  	  <param name="subscribe_depth" type="bool" value="true"/>
-      <param name="subscribe_laserScan" type="bool" value="true"/>
-      <param name="frame_id" type="string" value="base_footprint"/>
-      <param name="wait_for_transform" type="bool" value="true"/>
-    
-      <remap from="rgb/image" to="/data_throttled_image"/>
-      <remap from="depth/image" to="/data_throttled_image_depth"/>
-      <remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
-      <remap from="scan" to="/jn0/base_scan"/>
-      <remap from="odom" to="/az3/base_controller/odom"/>
-      
-      <param name="rgb/image_transport" type="string" value="compressed"/>
-      <param name="depth/image_transport" type="string" value="compressedDepth"/>
-    </node>
-  </group>
-
-  <!-- Visualisation RVIZ -->
-  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/demo_robot_mapping.rviz" output="screen"/>
-   <node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
-    <remap from="rgb/image"       to="/data_throttled_image"/>
-    <remap from="depth/image"     to="/data_throttled_image_depth"/>
-    <remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
-    <remap from="cloud"           to="voxel_cloud" />
-    
-    <param name="rgb/image_transport" type="string" value="compressed"/>
-    <param name="depth/image_transport" type="string" value="compressedDepth"/>
-    
-    <param name="queue_size" type="int" value="10"/>
-    <param name="voxel_size" type="double" value="0.01"/>
-  </node>
-
-</launch>
diff --git a/launch/demo/demo_robot_mapping.launch b/launch/demo/demo_robot_mapping.launch
index e09b96dd..0cbd6e56 100644
--- a/launch/demo/demo_robot_mapping.launch
+++ b/launch/demo/demo_robot_mapping.launch
@@ -10,53 +10,63 @@
   <arg name="rtabmapviz" default="true" /> 
   
   <param name="use_sim_time" type="bool" value="True"/>
+  
+  <!-- Localization-only mode -->
+  <arg name="localization"      default="false"/>
+  <arg     if="$(arg localization)" name="rtabmap_args"  default=""/>
+  <arg unless="$(arg localization)" name="rtabmap_args"  default="--delete_db_on_start"/>
 
   <group ns="rtabmap">
     <!-- SLAM (robot side) -->
     <!-- args: "delete_db_on_start" and "udebug" -->
-    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
-	  <param name="frame_id" type="string" value="base_footprint"/>
+    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
+	  <param name="frame_id"           type="string" value="base_footprint"/>
 	  <param name="wait_for_transform" type="bool" value="true"/>
 	
 	  <param name="subscribe_depth" type="bool" value="true"/>
-	  <param name="subscribe_scan" type="bool" value="true"/>
+	  <param name="subscribe_scan"  type="bool" value="true"/>
 	
 	  <remap from="odom" to="/az3/base_controller/odom"/>
 	  <remap from="scan" to="/jn0/base_scan"/>
 
-	  <remap from="rgb/image" to="/data_throttled_image"/>
-  	  <remap from="depth/image" to="/data_throttled_image_depth"/>
+	  <remap from="rgb/image"       to="/data_throttled_image"/>
+  	  <remap from="depth/image"     to="/data_throttled_image_depth"/>
   	  <remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
   	
-      <param name="rgb/image_transport" type="string" value="compressed"/>
+      <param name="rgb/image_transport"   type="string" value="compressed"/>
       <param name="depth/image_transport" type="string" value="compressedDepth"/>
 	
 	  <!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
 	  <param name="RGBD/NeighborLinkRefining" type="string" value="true"/>  <!-- Do odometry correction with consecutive laser scans -->
-	  <param name="RGBD/ProximityBySpace" type="string" value="true"/>  <!-- Local loop closure detection (using estimated position) with locations in WM -->
-	  <param name="RGBD/ProximityByTime" type="string" value="false"/>   <!-- Local loop closure detection with locations in STM -->
-	  <param name="RGBD/OptimizeFromGraphEnd" type="string" value="true"/> 
-          <param name="Reg/Force3DoF" type="string" value="true"/>
-	  <param name="Reg/Strategy" type="string" value="1"/>                      <!-- 0=Visual, 1=ICP, 2=Visual+ICP -->
-	  <param name="Vis/InlierDistance" type="string" value="0.1"/>          <!-- 3D visual words correspondence distance -->
-	  <param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/>    <!-- Optimize graph from initial node so /map -> /odom transform will be generated -->
-	  <param name="Optimizer/Slam2D" type="string" value="true"/>  
+	  <param name="RGBD/ProximityBySpace"     type="string" value="true"/>  <!-- Local loop closure detection (using estimated position) with locations in WM -->
+	  <param name="RGBD/ProximityByTime"      type="string" value="false"/> <!-- Local loop closure detection with locations in STM -->
+	  <param name="RGBD/OptimizeFromGraphEnd" type="string" value="true"/>
+	  <param name="Reg/Strategy"              type="string" value="1"/>     <!-- 0=Visual, 1=ICP, 2=Visual+ICP -->
+	  <param name="Vis/InlierDistance"        type="string" value="0.1"/>   <!-- 3D visual words correspondence distance -->
+	  <param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/> <!-- Optimize graph from initial node so /map -> /odom transform will be generated -->
+	  <param name="Optimizer/Slam2D"          type="string" value="true"/>
+	  <param name="Reg/Force3DoF"             type="string" value="true"/>
+	  
+	  <!-- localization mode -->
+	  <param     if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
+	  <param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
+	  <param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
    </node>
     
     <!-- Visualisation RTAB-Map -->
     <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
-  	  <param name="subscribe_depth" type="bool" value="true"/>
-      <param name="subscribe_scan" type="bool" value="true"/>
-      <param name="frame_id" type="string" value="base_footprint"/>
+  	  <param name="subscribe_depth"    type="bool" value="true"/>
+      <param name="subscribe_scan"     type="bool" value="true"/>
+      <param name="frame_id"           type="string" value="base_footprint"/>
       <param name="wait_for_transform" type="bool" value="true"/>
     
-      <remap from="rgb/image" to="/data_throttled_image"/>
-      <remap from="depth/image" to="/data_throttled_image_depth"/>
+      <remap from="rgb/image"       to="/data_throttled_image"/>
+      <remap from="depth/image"     to="/data_throttled_image_depth"/>
       <remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
-      <remap from="scan" to="/jn0/base_scan"/>
-      <remap from="odom" to="/az3/base_controller/odom"/>
+      <remap from="scan"            to="/jn0/base_scan"/>
+      <remap from="odom"            to="/az3/base_controller/odom"/>
       
-      <param name="rgb/image_transport" type="string" value="compressed"/>
+      <param name="rgb/image_transport"   type="string" value="compressed"/>
       <param name="depth/image_transport" type="string" value="compressedDepth"/>
     </node>
   </group>
@@ -69,7 +79,7 @@
     <remap from="rgb/camera_info" to="/data_throttled_camera_info"/>
     <remap from="cloud"           to="voxel_cloud" />
     
-    <param name="rgb/image_transport" type="string" value="compressed"/>
+    <param name="rgb/image_transport"   type="string" value="compressed"/>
     <param name="depth/image_transport" type="string" value="compressedDepth"/>
     
     <param name="queue_size" type="int" value="10"/>
diff --git a/launch/demo/demo_stereo_outdoor.launch b/launch/demo/demo_stereo_outdoor.launch
index 014d6b1c..00743af6 100644
--- a/launch/demo/demo_stereo_outdoor.launch
+++ b/launch/demo/demo_stereo_outdoor.launch
@@ -21,7 +21,7 @@
    <param name="use_sim_time" type="bool" value="True"/>
    
    <!-- Just to uncompress images for stereo_image_rect -->
-   <node name="republish_left" type="republish" pkg="image_transport" args="compressed in:=/stereo_camera/left/image_raw_throttle raw out:=/stereo_camera/left/image_raw_throttle_relay" />
+   <node name="republish_left"  type="republish" pkg="image_transport" args="compressed in:=/stereo_camera/left/image_raw_throttle raw out:=/stereo_camera/left/image_raw_throttle_relay" />
    <node name="republish_right" type="republish" pkg="image_transport" args="compressed in:=/stereo_camera/right/image_raw_throttle raw out:=/stereo_camera/right/image_raw_throttle_relay" />
 
    <!-- Run the ROS package stereo_image_proc for image rectification -->
@@ -47,26 +47,26 @@
          <remap from="right/camera_info"     to="/stereo_camera/right/camera_info_throttle"/>
          <remap from="odom"                  to="/stereo_odometry"/>
 
-         <param name="frame_id" type="string" value="base_footprint"/>
+         <param name="frame_id"      type="string" value="base_footprint"/>
          <param name="odom_frame_id" type="string" value="odom"/>
 
-         <param name="Odom/Strategy" type="string" value="0"/> <!-- 0=Frame-to-Map, 1=Frame=to=Frame -->
+         <param name="Odom/Strategy"      type="string" value="0"/> <!-- 0=Frame-to-Map, 1=Frame=to=Frame -->
          <param name="Vis/EstimationType" type="string" value="0"/> <!-- 0=3D->3D 1=3D->2D (PnP) -->
-         <param name="Vis/MaxDepth" type="string" value="10"/>
-         <param name="Odom/FillInfoData" type="string" value="$(arg rtabmapviz)"/> 
-         <param name="GFTT/MinDistance" type="string" value="10"/>
-         <param name="GFTT/QualityLevel" type="string" value="0.00001"/> 
+         <param name="Vis/MaxDepth"       type="string" value="10"/>
+         <param name="Odom/FillInfoData"  type="string" value="$(arg rtabmapviz)"/> 
+         <param name="GFTT/MinDistance"   type="string" value="10"/>
+         <param name="GFTT/QualityLevel"  type="string" value="0.00001"/> 
       </node>
    
       <!-- Visual SLAM: args: "delete_db_on_start" and "udebug" -->
       <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
-         <param name="frame_id" type="string" value="base_footprint"/>
+         <param name="frame_id"         type="string" value="base_footprint"/>
          <param name="subscribe_stereo" type="bool" value="true"/>
-         <param name="subscribe_depth" type="bool" value="false"/>
+         <param name="subscribe_depth"  type="bool" value="false"/>
 
-         <remap from="left/image_rect" to="/stereo_camera/left/image_rect_color"/>
-         <remap from="right/image_rect" to="/stereo_camera/right/image_rect"/>
-         <remap from="left/camera_info" to="/stereo_camera/left/camera_info_throttle"/>
+         <remap from="left/image_rect"   to="/stereo_camera/left/image_rect_color"/>
+         <remap from="right/image_rect"  to="/stereo_camera/right/image_rect"/>
+         <remap from="left/camera_info"  to="/stereo_camera/left/camera_info_throttle"/>
          <remap from="right/camera_info" to="/stereo_camera/right/camera_info_throttle"/>
 
          <remap from="odom" to="/stereo_odometry"/>
@@ -74,29 +74,30 @@
          <param name="queue_size" type="int" value="30"/>
 
          <!-- RTAB-Map's parameters -->
-         <param name="Rtabmap/TimeThr" type="string" value="700"/>
-         <param name="Kp/MaxFeatures" type="string" value="200"/>
-         <param name="Kp/MaxDepth"    type="string" value="10"/>
-         <param name="Kp/DetectorStrategy" type="string" value="0"/>   <!-- use SURF -->
-         <param name="SURF/HessianThreshold" type="string" value="1000"/>
-         <param name="Vis/EstimationType" type="string" value="0"/> <!-- 0=3D->3D, 1=3D->2D (PnP) -->
+         <param name="Rtabmap/TimeThr"                   type="string" value="700"/>
+         <param name="Kp/MaxFeatures"                    type="string" value="200"/>
+         <param name="Kp/MaxDepth"                       type="string" value="10"/>
+         <param name="Kp/DetectorStrategy"               type="string" value="0"/>   <!-- use SURF -->
+         <param name="SURF/HessianThreshold"             type="string" value="1000"/>
+         <param name="Vis/EstimationType"                type="string" value="0"/>   <!-- 0=3D->3D, 1=3D->2D (PnP) -->
          <param name="RGBD/LoopClosureReextractFeatures" type="string" value="true"/>
-         <param name="Vis/MaxDepth" type="string" value="10"/>
+         <param name="Vis/MaxDepth"                      type="string" value="10"/>
       </node>
       
       <!-- Visualisation RTAB-Map -->
       <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
-         <param name="subscribe_stereo" type="bool" value="true"/>
+         <param name="subscribe_stereo"    type="bool" value="true"/>
          <param name="subscribe_odom_info" type="bool" value="true"/>
-         <param name="queue_size" type="int" value="10"/>
-         <param name="frame_id" type="string" value="base_footprint"/>
-         <remap from="left/image_rect" to="/stereo_camera/left/image_rect_color"/>
-         <remap from="right/image_rect" to="/stereo_camera/right/image_rect"/>
-         <remap from="left/camera_info" to="/stereo_camera/left/camera_info_throttle"/>
+         <param name="queue_size"          type="int" value="10"/>
+         <param name="frame_id"            type="string" value="base_footprint"/>
+         
+         <remap from="left/image_rect"  to="/stereo_camera/left/image_rect_color"/>
+         <remap from="right/image_rect"  to="/stereo_camera/right/image_rect"/>
+         <remap from="left/camera_info"  to="/stereo_camera/left/camera_info_throttle"/>
          <remap from="right/camera_info" to="/stereo_camera/right/camera_info_throttle"/>
-         <remap from="odom_info" to="odom_info"/>
-         <remap from="odom" to="/stereo_odometry"/>
-         <remap from="mapData" to="mapData"/>
+         <remap from="odom_info"         to="odom_info"/>
+         <remap from="odom"              to="/stereo_odometry"/>
+         <remap from="mapData"           to="mapData"/>
       </node>
          
    </group>
diff --git a/launch/demo/demo_turtlebot_mapping.launch b/launch/demo/demo_turtlebot_mapping.launch
index 03a79bda..5b29e180 100644
--- a/launch/demo/demo_turtlebot_mapping.launch
+++ b/launch/demo/demo_turtlebot_mapping.launch
@@ -68,7 +68,9 @@
 	  <param name="RGBD/AngularUpdate"           type="string" value="0.1"/>    <!-- Update map only if the robot is moving -->
 	  <param name="RGBD/LinearUpdate"            type="string" value="0.1"/>    <!-- Update map only if the robot is moving -->
 	  <param name="Rtabmap/TimeThr"              type="string" value="700"/>
-	  <param name="Mem/RehearsalSimilarity"      type="string" value="0.30"/>   
+	  <param name="Mem/RehearsalSimilarity"      type="string" value="0.30"/>
+	  <param name="Optimizer/Slam2D"             type="string" value="true"/>
+	  <param name="Reg/Force3DoF"                type="string" value="true"/>   
 	  
 	  <!-- localization mode -->
 	  <param     if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
@@ -78,10 +80,11 @@
    
     <!-- Odometry : ONLY for testing without the actual robot! /odom TF should not be already published. -->
     <node if="$(arg rgbd_odometry)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
-      <param name="frame_id"           type="string" value="base_footprint"/>
-      <param name="wait_for_transform_duration" type="double"   value="$(arg wait_for_transform)"/>
-      <param name="Reg/Force3DoF"       type="string"   value="true"/>
-      <param name="Vis/InlierDistance"  type="string"   value="0.05"/>
+      <param name="frame_id"                    type="string" value="base_footprint"/>
+      <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
+      <param name="Reg/Force3DoF"               type="string" value="true"/>
+      <param name="Vis/InlierDistance"          type="string" value="0.05"/>
+      
       <remap from="rgb/image"       to="/camera/rgb/image_rect_color"/>
       <remap from="depth/image"     to="/camera/depth_registered/image_raw"/>
       <remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
@@ -89,9 +92,9 @@
     
     <!-- visualization with rtabmapviz -->
     <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
-  	  <param name="subscribe_depth" type="bool" value="true"/>
-      <param name="subscribe_scan"  type="bool" value="true"/>
-      <param name="frame_id" type="string" value="base_footprint"/>
+  	  <param name="subscribe_depth"             type="bool" value="true"/>
+      <param name="subscribe_scan"              type="bool" value="true"/>
+      <param name="frame_id"                    type="string" value="base_footprint"/>
       <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
     
       <remap from="rgb/image"       to="/camera/rgb/image_rect_color"/>
diff --git a/launch/rgbd_mapping.launch b/launch/rgbd_mapping.launch
index a1cd9142..98022b62 100644
--- a/launch/rgbd_mapping.launch
+++ b/launch/rgbd_mapping.launch
@@ -9,6 +9,9 @@
   <arg name="rviz"                    default="false" />
   <arg name="rtabmapviz"              default="true" /> 
   
+  <!-- Localization-only mode -->
+  <arg name="localization"            default="false"/>
+  
   <!-- Corresponding config files -->
   <arg name="rtabmapviz_cfg"          default="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" />
   <arg name="rviz_cfg"                default="-d $(find rtabmap_ros)/launch/config/rgbd.rviz" />
@@ -29,27 +32,19 @@
   <arg name="subscribe_scan"          default="false"/>         <!-- Assuming 2D scan if set, rtabmap will do 3DoF mapping instead of 6DoF -->
   <arg name="scan_topic"              default="/scan"/>
   
+  <arg name="subscribe_scan_cloud"    default="false"/>         <!-- Assuming 3D scan if set -->
+  <arg name="scan_cloud_topic"        default="/scan_cloud"/>
+  
   <arg name="visual_odometry"         default="true"/>          <!-- Generate visual odometry -->
   <arg name="odom_topic"              default="/odom"/>         <!-- Odometry topic used if visual_odometry is false -->
    
   <arg name="namespace"               default="rtabmap"/>
   <arg name="wait_for_transform"      default="0.1"/>
   
-  <!-- Odometry parameters: -->
-  <arg name="strategy"            default="0" />       <!-- Strategy: 0=Frame-to-Map 1=Frame-to-Frame -->
-  <arg name="feature"             default="6" />       <!-- Feature type: 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK -->
-  <arg name="estimation"          default="0" />       <!-- Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP) -->
-  <arg name="nn"                  default="3" />       <!-- Nearest neighbor strategy : 0=Linear, 1=FLANN_KDTREE, 2=FLANN_LSH, 3=BRUTEFORCE -->
-  <arg name="max_depth"           default="0" />       <!-- Maximum features depth (m) -->
-  <arg name="min_inliers"         default="20" />      <!-- Minimum visual correspondences to accept a transformation (m) -->
-  <arg name="inlier_distance"     default="0.1" />     <!-- RANSAC maximum inliers distance (m) -->
-  <arg name="local_map"           default="1000" />    <!-- Local map size: number of unique features to keep track -->
-  <arg name="variance_inliers"    default="true"/>     <!-- Variance from inverse of inliers count -->  
-        
   <!-- Nodes -->
   <group ns="$(arg namespace)">
   
-    <node if="$(arg compressed)" name="republish_rgb" type="republish" pkg="image_transport" args="compressed in:=$(arg rgb_topic) raw out:=$(arg rgb_topic)" />
+    <node if="$(arg compressed)" name="republish_rgb"   type="republish" pkg="image_transport" args="compressed in:=$(arg rgb_topic) raw out:=$(arg rgb_topic)" />
     <node if="$(arg compressed)" name="republish_depth" type="republish" pkg="image_transport" args="compressedDepth in:=$(arg depth_registered_topic) raw out:=$(arg depth_registered_topic)" />
   
     <!-- Odometry -->
@@ -58,54 +53,52 @@
       <remap from="depth/image"     to="$(arg depth_registered_topic)"/>
       <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
 	  
-	  <param name="frame_id"                 type="string" value="$(arg frame_id)"/>
-	  <param name="wait_for_transform_duration"       type="double"   value="$(arg wait_for_transform)"/>
+	  <param name="frame_id"                    type="string" value="$(arg frame_id)"/>
+	  <param name="wait_for_transform_duration" type="double"   value="$(arg wait_for_transform)"/>
 	  
-	  <param name="Odom/Strategy"            type="string" value="$(arg strategy)"/> 
-	  <param name="Vis/FeatureType"          type="string" value="$(arg feature)"/>  
-	  <param name="Vis/CorNNType"            type="string" value="$(arg nn)"/>
-	  <param name="Vis/EstimationType"       type="string" value="$(arg estimation)"/> 
-	  <param name="Vis/MaxDepth"             type="string" value="$(arg max_depth)"/>  
-	  <param name="Vis/MinInliers"           type="string" value="$(arg min_inliers)"/> 
-	  <param name="Vis/InlierDistance"       type="string" value="$(arg inlier_distance)"/>       
-      <param name="OdomF2M/MaxSize"          type="string" value="$(arg local_map)"/> 
-      <param name="Odom/FillInfoData"        type="string" value="true"/> 
-      <param name="Reg/VarianceFromInliersCount" type="string" value="$(arg variance_inliers)"/>  
+      <param name="Odom/FillInfoData"           type="string" value="true"/> 
     </node>
   
     <!-- Visual SLAM (robot side) -->
     <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
-      <param name="subscribe_depth"    type="bool"   value="true"/>
-      <param name="subscribe_laserScan"     type="bool"   value="$(arg subscribe_scan)"/>
-	  <param name="frame_id"           type="string" value="$(arg frame_id)"/>
-	  <param name="wait_for_transform_duration" type="double"   value="$(arg wait_for_transform)"/>
-	  <param name="database_path"      type="string" value="$(arg database_path)"/>
+      <param name="subscribe_depth"             type="bool"   value="true"/>
+      <param name="subscribe_scan"              type="bool"   value="$(arg subscribe_scan)"/>
+      <param name="subscribe_scan_cloud"        type="bool"   value="$(arg subscribe_scan_cloud)"/>
+	  <param name="frame_id"                    type="string" value="$(arg frame_id)"/>
+	  <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
+	  <param name="database_path"               type="string" value="$(arg database_path)"/>
 	
       <remap from="rgb/image"       to="$(arg rgb_topic)"/>
       <remap from="depth/image"     to="$(arg depth_registered_topic)"/>
       <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
       <remap from="scan"            to="$(arg scan_topic)"/>
+      <remap from="scan_cloud"      to="$(arg scan_cloud_topic)"/>
       <remap unless="$(arg visual_odometry)" from="odom"  to="$(arg odom_topic)"/>
       
       <param name="Rtabmap/TimeThr"              type="string" value="$(arg time_threshold)"/>
       <param name="RGBD/OptimizeFromGraphEnd"    type="string" value="$(arg optimize_from_last_node)"/>
-      <param name="Vis/MinInliers"               type="string" value="$(arg min_inliers)"/>
-      <param name="Vis/InlierDistance"           type="string" value="$(arg inlier_distance)"/>
-      <param name="Vis/EstimationType"           type="string" value="$(arg estimation)"/> 
-      <param name="Reg/VarianceFromInliersCount" type="string" value="$(arg variance_inliers)"/>
       <param name="Mem/SaveDepth16Format"        type="string" value="$(arg convert_depth_to_mm)"/>
       
+      <!-- localization mode -->
+	  <param     if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
+	  <param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
+	  <param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
+
       <!-- when 2D scan is set -->
-      <param if="$(arg subscribe_scan)" name="RGBD/OptimizeSlam2D"          type="string" value="true"/>
-      <param if="$(arg subscribe_scan)" name="RGBD/LocalLoopDetectionSpace" type="string" value="true"/>
-      <param if="$(arg subscribe_scan)" name="LccIcp/Type"                  type="string" value="2"/> 
-	  <param if="$(arg subscribe_scan)" name="LccIcp2/CorrespondenceRatio"  type="string" value="0.25"/>
+      <param if="$(arg subscribe_scan)" name="Optimizer/Slam2D"        type="string" value="true"/>
+	  <param if="$(arg subscribe_scan)" name="Icp/CorrespondenceRatio" type="string" value="0.25"/>
+	  <param if="$(arg subscribe_scan)" name="Reg/Strategy"            type="string" value="1"/> 
+	  <param if="$(arg subscribe_scan)" name="Reg/Force3DoF"           type="string" value="true"/>
+
+	  <!-- when 3D scan is set -->
+	  <param if="$(arg subscribe_scan_cloud)" name="Reg/Strategy"      type="string" value="1"/>
     </node>
   
     <!-- Visualisation RTAB-Map -->
     <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="$(arg rtabmapviz_cfg)" output="screen" launch-prefix="$(arg launch_prefix)">
       <param name="subscribe_depth"       type="bool"   value="true"/>
-      <param name="subscribe_laserScan"   type="bool"   value="$(arg subscribe_scan)"/>
+      <param name="subscribe_scan"        type="bool"   value="$(arg subscribe_scan)"/>
+      <param name="subscribe_scan_cloud"  type="bool"   value="$(arg subscribe_scan_cloud)"/>
       <param name="subscribe_odom_info"   type="bool"   value="$(arg visual_odometry)"/>
       <param name="frame_id"              type="string" value="$(arg frame_id)"/>
       <param name="wait_for_transform_duration"  type="double"   value="$(arg wait_for_transform)"/>
@@ -114,6 +107,7 @@
       <remap from="depth/image"     to="$(arg depth_registered_topic)"/>
       <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
       <remap from="scan"            to="$(arg scan_topic)"/>
+      <remap from="scan_cloud"      to="$(arg scan_cloud_topic)"/>
       <remap unless="$(arg visual_odometry)" from="odom"  to="$(arg odom_topic)"/>
     </node>
   
diff --git a/launch/rtabmap.launch b/launch/rtabmap.launch
index 45089f93..2fc365d3 100644
--- a/launch/rtabmap.launch
+++ b/launch/rtabmap.launch
@@ -54,6 +54,9 @@
    
   <arg name="subscribe_scan"          default="false"/>
   <arg name="scan_topic"              default="/scan"/>
+  
+  <arg name="subscribe_scan_cloud"    default="false"/>
+  <arg name="scan_cloud_topic"        default="/scan_cloud"/>
    
   <arg name="visual_odometry"         default="true"/>          <!-- Launch rtabmap visual odometry node -->
   <arg name="odom_topic"              default="/odom"/>         <!-- Odometry topic used if visual_odometry is false -->
@@ -110,15 +113,16 @@
     <!-- Visual SLAM (robot side) -->
     <!-- args: "delete_db_on_start" and "udebug" -->
     <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
-      <param name="subscribe_depth"     type="bool"   value="$(arg rgbd)"/>
-      <param name="subscribe_stereo"    type="bool"   value="$(arg stereo)"/>
-      <param name="subscribe_laserScan" type="bool"   value="$(arg subscribe_scan)"/>
-      <param name="frame_id"            type="string" value="$(arg frame_id)"/>
+      <param name="subscribe_depth"      type="bool"   value="$(arg rgbd)"/>
+      <param name="subscribe_stereo"     type="bool"   value="$(arg stereo)"/>
+      <param name="subscribe_scan"       type="bool"   value="$(arg subscribe_scan)"/>
+      <param name="subscribe_scan_cloud" type="bool"   value="$(arg subscribe_scan_cloud)"/>
+      <param name="frame_id"             type="string" value="$(arg frame_id)"/>
       <param name="wait_for_transform_duration"  type="double"   value="$(arg wait_for_transform)"/>
-      <param name="database_path"       type="string" value="$(arg database_path)"/>
-      <param name="stereo_approx_sync"  type="bool"   value="$(arg approx_sync)"/>
+      <param name="database_path"        type="string" value="$(arg database_path)"/>
+      <param name="stereo_approx_sync"   type="bool"   value="$(arg approx_sync)"/>
       <param name="config_path"          type="string" value="$(arg cfg)"/>
-      <param name="queue_size"                  type="int" value="$(arg queue_size)"/>
+      <param name="queue_size"           type="int" value="$(arg queue_size)"/>
       
       <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
       <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
@@ -130,6 +134,7 @@
       <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
       
       <remap from="scan"                   to="$(arg scan_topic)"/>
+      <remap from="scan_cloud"             to="$(arg scan_cloud_topic)"/>
       <remap unless="$(arg visual_odometry)" from="odom"  to="$(arg odom_topic)"/>
     </node>
   
@@ -137,11 +142,12 @@
     <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(arg cfg)" output="screen" launch-prefix="$(arg launch_prefix)">
       <param name="subscribe_depth"      type="bool"   value="$(arg rgbd)"/>
       <param name="subscribe_stereo"     type="bool"   value="$(arg stereo)"/>
-      <param name="subscribe_laserScan"  type="bool"   value="$(arg subscribe_scan)"/>
+      <param name="subscribe_scan"       type="bool"   value="$(arg subscribe_scan)"/>
+      <param name="subscribe_scan_cloud" type="bool"   value="$(arg subscribe_scan_cloud)"/>
       <param name="subscribe_odom_info"  type="bool"   value="$(arg visual_odometry)"/>
       <param name="frame_id"             type="string" value="$(arg frame_id)"/>
-      <param name="wait_for_transform_duration"   type="double"   value="$(arg wait_for_transform)"/>
-      <param name="queue_size"                  type="int" value="$(arg queue_size)"/>
+      <param name="wait_for_transform_duration" type="double"   value="$(arg wait_for_transform)"/>
+      <param name="queue_size"           type="int" value="$(arg queue_size)"/>
     
       <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
       <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
@@ -153,6 +159,7 @@
       <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
       
       <remap from="scan"                   to="$(arg scan_topic)"/>
+      <remap from="scan_cloud"             to="$(arg scan_cloud_topic)"/>
       <remap unless="$(arg visual_odometry)" from="odom"  to="$(arg odom_topic)"/>
     </node>
   
diff --git a/launch/stereo_mapping.launch b/launch/stereo_mapping.launch
index 708658d6..d55b810e 100644
--- a/launch/stereo_mapping.launch
+++ b/launch/stereo_mapping.launch
@@ -9,6 +9,9 @@
   <arg name="rtabmapviz"              default="true" /> 
   <arg name="rviz"                    default="false" />
   
+  <!-- Localization-only mode -->
+  <arg name="localization"            default="false"/>
+  
   <!-- Corresponding config files -->
   <arg name="rtabmapviz_cfg"          default="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" />
   <arg name="rviz_cfg"                default="-d $(find rtabmap_ros)/launch/config/rgbd.rviz" />
@@ -32,23 +35,15 @@
   <arg name="subscribe_scan"          default="false"/>         <!-- Assuming 2D scan if set, rtabmap will do 3DoF mapping instead of 6DoF -->
   <arg name="scan_topic"              default="/scan"/>
    
+  <arg name="subscribe_scan_cloud"    default="false"/>         <!-- Assuming 3D scan if set -->
+  <arg name="scan_cloud_topic"        default="/scan_cloud"/>
+  
   <arg name="visual_odometry"         default="true"/>          <!-- Generate visual odometry -->
   <arg name="odom_topic"              default="/odom"/>         <!-- Odometry topic used if visual_odometry is false -->
   
   <arg name="namespace"               default="rtabmap"/>
   <arg name="wait_for_transform"      default="0.1"/>
   
-  <!-- Odometry parameters: -->
-  <arg name="strategy"            default="0" />       <!-- Strategy: 0=Frame-to-Map 1=Frame-to-Frame -->
-  <arg name="feature"             default="6" />       <!-- Feature type: 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK -->
-  <arg name="estimation"          default="1" />       <!-- Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP) -->
-  <arg name="nn"                  default="3" />       <!-- Nearest neighbor strategy : 0=Linear, 1=FLANN_KDTREE 2=FLANN_LSH, 3=BRUTEFORCE -->
-  <arg name="max_depth"           default="0" />       <!-- Maximum features depth (m) -->
-  <arg name="min_inliers"         default="20" />      <!-- Minimum visual correspondences to accept a transformation (m) -->
-  <arg name="inlier_distance"     default="0.1" />     <!-- RANSAC maximum inliers distance (m) -->
-  <arg name="local_map"           default="1000" />    <!-- Local map size: number of unique features to keep track -->
-  <arg name="variance_inliers"    default="true"/>    <!-- Variance from inverse of inliers count -->  
-        
   <!-- Nodes -->
   <group ns="$(arg namespace)">
   
@@ -66,56 +61,54 @@
       <param name="wait_for_transform_duration"       type="double"   value="$(arg wait_for_transform)"/>
       <param name="approx_sync"              type="bool"   value="$(arg approximate_sync)"/>
 	  
-	  <param name="Odom/Strategy"            type="string" value="$(arg strategy)"/> 
-	  <param name="Vis/FeatureType"          type="string" value="$(arg feature)"/>  
-	  <param name="Vis/CorNNType"            type="string" value="$(arg nn)"/>
-	  <param name="Vis/EstimationType"       type="string" value="$(arg estimation)"/> 
-	  <param name="Vis/MaxDepth"             type="string" value="$(arg max_depth)"/>  
-	  <param name="Vis/MinInliers"           type="string" value="$(arg min_inliers)"/> 
-	  <param name="Vis/InlierDistance"       type="string" value="$(arg inlier_distance)"/>       
-      <param name="OdomF2M/MaxSize"          type="string" value="$(arg local_map)"/> 
       <param name="Odom/FillInfoData"        type="string" value="true"/> 
-      <param name="Reg/VarianceFromInliersCount" type="string" value="$(arg variance_inliers)"/>  
     </node>
   
     <!-- Visual SLAM (robot side) -->
     <!-- args: "delete_db_on_start" and "udebug" -->
     <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
-      <param name="subscribe_depth"     type="bool"   value="false"/>
-      <param name="subscribe_stereo"    type="bool"   value="true"/>
-      <param name="subscribe_laserScan"      type="bool"   value="$(arg subscribe_scan)"/>
-      <param name="frame_id"            type="string" value="$(arg frame_id)"/>
+      <param name="subscribe_depth"              type="bool"   value="false"/>
+      <param name="subscribe_stereo"             type="bool"   value="true"/>
+      <param name="subscribe_scan"               type="bool"   value="$(arg subscribe_scan)"/>
+      <param name="subscribe_scan_cloud"         type="bool"   value="$(arg subscribe_scan_cloud)"/>
+      <param name="frame_id"                     type="string" value="$(arg frame_id)"/>
       <param name="wait_for_transform_duration"  type="double"   value="$(arg wait_for_transform)"/>
-      <param name="database_path"       type="string" value="$(arg database_path)"/>
-      <param name="stereo_approx_sync"  type="bool"   value="$(arg approximate_sync)"/>
+      <param name="database_path"                type="string" value="$(arg database_path)"/>
+      <param name="stereo_approx_sync"           type="bool"   value="$(arg approximate_sync)"/>
 	
       <remap from="left/image_rect"        to="$(arg left_image_topic)"/>
       <remap from="right/image_rect"       to="$(arg right_image_topic)"/>
       <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
       <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
       <remap from="scan"                   to="$(arg scan_topic)"/>
+      <remap from="scan_cloud"      to="$(arg scan_cloud_topic)"/>
       <remap unless="$(arg visual_odometry)" from="odom"  to="$(arg odom_topic)"/>
       
       <param name="Rtabmap/TimeThr"              type="string" value="$(arg time_threshold)"/>
       <param name="RGBD/OptimizeFromGraphEnd"    type="string" value="$(arg optimize_from_last_node)"/>
-      <param name="Vis/MinInliers"               type="string" value="$(arg min_inliers)"/>
-      <param name="Vis/InlierDistance"           type="string" value="$(arg inlier_distance)"/>
-      <param name="Vis/EstimationType"           type="string" value="$(arg estimation)"/> 
-      <param name="Reg/VarianceFromInliersCount" type="string" value="$(arg variance_inliers)"/>
       <param name="Mem/SaveDepth16Format"        type="string" value="$(arg convert_depth_to_mm)"/>
       
+      <!-- localization mode -->
+	  <param     if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
+	  <param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
+	  <param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
+
       <!-- when 2D scan is set -->
-      <param if="$(arg subscribe_scan)" name="RGBD/OptimizeSlam2D"          type="string" value="true"/>
-      <param if="$(arg subscribe_scan)" name="RGBD/LocalLoopDetectionSpace" type="string" value="true"/>
-      <param if="$(arg subscribe_scan)" name="LccIcp/Type"                  type="string" value="2"/> 
-	  <param if="$(arg subscribe_scan)" name="LccIcp2/CorrespondenceRatio"  type="string" value="0.25"/>
+      <param if="$(arg subscribe_scan)" name="Optimizer/Slam2D"        type="string" value="true"/>
+	  <param if="$(arg subscribe_scan)" name="Icp/CorrespondenceRatio" type="string" value="0.25"/>
+	  <param if="$(arg subscribe_scan)" name="Reg/Strategy"            type="string" value="1"/> 
+	  <param if="$(arg subscribe_scan)" name="Reg/Force3DoF"           type="string" value="true"/>
+
+	  <!-- when 3D scan is set -->
+	  <param if="$(arg subscribe_scan_cloud)" name="Reg/Strategy"      type="string" value="1"/>
     </node>
   
     <!-- Visualisation RTAB-Map -->
     <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="$(arg rtabmapviz_cfg)" output="screen" launch-prefix="$(arg launch_prefix)">
       <param name="subscribe_depth"           type="bool"   value="false"/>
       <param name="subscribe_stereo"          type="bool"   value="true"/>
-      <param name="subscribe_laserScan"       type="bool"   value="$(arg subscribe_scan)"/>
+      <param name="subscribe_scan"            type="bool"   value="$(arg subscribe_scan)"/>
+      <param name="subscribe_scan_cloud"      type="bool"   value="$(arg subscribe_scan_cloud)"/>
       <param name="subscribe_odom_info"       type="bool"   value="$(arg visual_odometry)"/>
       <param name="frame_id"                  type="string" value="$(arg frame_id)"/>
       <param name="wait_for_transform_duration"   type="double"   value="$(arg wait_for_transform)"/>
@@ -125,6 +118,7 @@
       <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
       <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
       <remap from="scan"                   to="$(arg scan_topic)"/>
+      <remap from="scan_cloud"             to="$(arg scan_cloud_topic)"/>
       <remap unless="$(arg visual_odometry)" from="odom"  to="$(arg odom_topic)"/>
     </node>