diff --git a/launch/rgbd_mapping.launch b/launch/rgbd_mapping.launch
index 2742e338..f9a2b25f 100644
--- a/launch/rgbd_mapping.launch
+++ b/launch/rgbd_mapping.launch
@@ -1,45 +1,38 @@
 
 <launch>
-   
-   <!-- RGB-D MAPPING VERSION -->
-   <!-- WARNING : Database is automatically deleted on each startup -->
-   <!--           See "delete_db_on_start" option below... -->
-  
+   <!-- Your RGB-D sensor should be already started with "depth_registration:=true".
+        Examples:
+           $ roslaunch freenect_launch freenect.launch depth_registration:=true 
+           $ roslaunch openni2_launch openni2.launch depth_registration:=true -->
+ 
    <!-- Choose visualization -->
-   <arg name="rviz"                    default="false" />
-   <arg name="rtabmapviz"              default="true" /> 
+  <arg name="rviz"                    default="false" />
+  <arg name="rtabmapviz"              default="true" /> 
   
-   <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
-   <arg name="frame_id"                default="camera_link"/>
+  <arg name="frame_id"                default="camera_link"/>   <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
+  <arg name="time_threshold"          default="0"/>             <!-- (ms) If not 0 ms, memory management is used to keep processing time on this fixed limit. -->
+  <arg name="optimize_from_last_node" default="false"/>         <!-- Optimize the map from the last node. Should be true on multi-session mapping and when time threshold is set -->
    
-   <!-- Image topics input -->
-   <arg name="rgb_topic"               default="/camera/rgb/image_rect_color" />
-   <arg name="depth_registered_topic"  default="/camera/depth_registered/image_raw" />
-   <arg name="camera_info_topic"       default="/camera/rgb/camera_info" />
+  <arg name="rgb_topic"               default="/camera/rgb/image_rect_color" />
+  <arg name="depth_registered_topic"  default="/camera/depth_registered/image_raw" />
+  <arg name="camera_info_topic"       default="/camera/rgb/camera_info" />
    
-   <arg name="namespace"               default="rtabmap"/>
+  <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="namespace"               default="rtabmap"/>
   
-   <!-- ODOMETRY MAIN ARGUMENTS: 
-        -"strategy"        : Strategy: 0=BOW (bag-of-words) 1=Optical Flow
-        -"feature"         : Feature type: 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK
-        -"nn"              : Nearest neighbor strategy : 0=Linear, 1=FLANN_KDTREE, 2=FLANN_LSH, 3=BRUTEFORCE 
-                             Set to 1 for float descriptor like SIFT/SURF                  
-                             Set to 3 for binary descriptor like ORB/FREAK/BRIEF/BRISK  
-        -"max_depth"       : Maximum features depth (m)  
-        -"min_inliers"     : Minimum visual correspondences to accept a transformation (m)  
-        -"inlier_distance" : RANSAC maximum inliers distance (m)  
-        -"local_map"       : Local map size: number of unique features to keep track 
-        -"odom_info_data"  : Fill odometry info messages with inliers/outliers data.
-    -->
-   <arg name="strategy"            default="0" />
-   <arg name="feature"             default="6" />
-   <arg name="nn"                  default="3" />
-   <arg name="max_depth"           default="4.0" />
-   <arg name="min_inliers"         default="20" />
-   <arg name="inlier_distance"     default="0.02" />
-   <arg name="local_map"           default="1000" />
-   <arg name="odom_info_data"      default="true" />
+  <!-- Odometry parameters: -->
+  <arg name="strategy"            default="0" />       <!-- Strategy: 0=BOW (bag-of-words) 1=Optical Flow -->
+  <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="nn"                  default="3" />       <!-- Nearest neighbor strategy : 0=Linear, 1=FLANN_KDTREE (SIFT, SURF), 2=FLANN_LSH, 3=BRUTEFORCE (ORB/FREAK/BRIEF/BRISK) -->
+  <arg name="max_depth"           default="4.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.02" />    <!-- RANSAC maximum inliers distance (m) -->
+  <arg name="local_map"           default="1000" />    <!-- Local map size: number of unique features to keep track -->
+  <arg name="odom_info_data"      default="true" />    <!-- Fill odometry info messages with inliers/outliers data. -->
         
+  <!-- Nodes -->
   <group ns="$(arg namespace)">
   
     <!-- Odometry -->
@@ -63,25 +56,37 @@
     <!-- args: "delete_db_on_start" and "udebug" -->
     <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
       <param name="subscribe_depth" type="bool"   value="true"/>
+      <param name="subscribe_scan"  type="bool"   value="$(arg subscribe_scan)"/>
 	  <param name="frame_id"        type="string" value="$(arg frame_id)"/>
 	
       <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)"/>
-
-      <param name="LccBow/MinInliers" type="string" value="10"/>
-      <param name="LccBow/InlierDistance" type="string" value="$(arg inlier_distance)"/>
+      <remap from="scan"            to="$(arg scan_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="LccBow/MinInliers"         type="string" value="10"/>
+      <param name="LccBow/InlierDistance"     type="string" value="$(arg inlier_distance)"/>
+      
+      <!-- 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"/>
     </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="$(arg subscribe_scan)"/>
       <param name="subscribe_odom_info" type="bool"   value="$(arg odom_info_data)"/>
       <param name="frame_id"            type="string" value="$(arg frame_id)"/>
     
       <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)"/>
     </node>
   
   </group>
@@ -107,7 +112,8 @@
     <remap from="rgb/camera_info" to="data_odom_sync/camera_info"/>
     <remap from="cloud"           to="voxel_cloud" />
 
-    <param name="voxel_size" type="double" value="0.01"/>
+    <param name="decimation" type="double" value="2"/>
+    <param name="voxel_size" type="double" value="0.02"/>
   </node>
 
 </launch>