Added lidar_deskewing node and nodelet. Added "deskewing" option for icp_odometry. Updated velodyne and ouster examples with deskewing option. Added velodyne+T265 deskewing example.

launch/tests/test_ouster.launch

@@ -12,14 +12,15 @@
            coming from the first cloud sent by os1_cloud_node, which may be poorly synchronized with IMU data.
     -->
 
-    <!-- Required: -->
-    <arg name="os1_hostname"/>
-    <arg name="os1_udp_dest"/>
+    <arg name="use_sim_time"  default="false"/>
 
-    <arg name="frame_id" default="os1_sensor"/>
+    <!-- Required: -->
+    <arg unless="$(arg use_sim_time)" name="os1_hostname"/>
+    <arg unless="$(arg use_sim_time)" name="os1_udp_dest"/>
+
+    <arg name="frame_id"      default="os1_sensor"/>
     <arg name="rtabmapviz"    default="true"/>
     <arg name="scan_20_hz"    default="true"/>
-    <arg name="use_sim_time"  default="false"/>
     <param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
     
     <!-- Ouster -->

launch/tests/test_ouster_gen2.launch

@@ -40,11 +40,14 @@
 
     <arg name="frame_id"      default="os_sensor"/>
     <arg name="rtabmapviz"    default="true"/>
+    <arg name="deskewing"     default="true"/>
+    <arg name="slerp"         default="false"/>
     <arg name="scan_20_hz"    default="true"/>
     <arg name="voxel_size"    default="0.15"/>           <!-- indoor: 0.1 to 0.3, outdoor: 0.3 to 0.5 --> 
     <arg name="assemble"      default="false"/>
     <arg name="ptp"           default="false"/>          <!-- See comments in header to start before launching the launch -->
-    <arg name="distortion_correction"  default="false"/> <!-- Requires this pull request: https://github.com/ouster-lidar/ouster_example/pull/245 -->
+    <arg name="imu_topic"     default="/os_cloud_node/imu"/>
+    <arg name="scan_topic"    default="/os_cloud_node/points"/>
     
     <param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
     
@@ -56,13 +59,12 @@
       <arg     if="$(arg scan_20_hz)" name="lidar_mode" value="1024x20"/>
       <arg unless="$(arg scan_20_hz)" name="lidar_mode" value="1024x10"/>
       <arg if="$(arg ptp)" name="timestamp_mode" value="TIME_FROM_PTP_1588"/>
-      <arg if="$(arg distortion_correction)" name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
     </include>
 
     <!-- IMU orientation estimation and publish tf accordingly to os_sensor frame -->
     <node pkg="nodelet" type="nodelet" name="imu_nodelet_manager" args="manager">
-      <remap from="imu/data_raw" to="/os_cloud_node/imu"/>
-      <remap from="imu/data" to="/os_cloud_node/imu/data"/>
+      <remap from="imu/data_raw" to="$(arg imu_topic)"/>
+      <remap from="imu/data" to="$(arg imu_topic)/filtered"/>
     </node>
     <node pkg="nodelet" type="nodelet" name="imu_filter" args="load imu_filter_madgwick/ImuFilterNodelet imu_nodelet_manager">
       <param name="use_mag" value="false"/>
@@ -70,15 +72,26 @@
       <param name="publish_tf" value="false"/>
     </node>
     <node pkg="nodelet" type="nodelet" name="imu_to_tf" args="load rtabmap_ros/imu_to_tf imu_nodelet_manager">
-      <remap from="imu/data" to="/os_cloud_node/imu/data"/>
+      <remap from="imu/data" to="$(arg imu_topic)/filtered"/>
       <param name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
       <param name="base_frame_id" value="$(arg frame_id)"/>
     </node> 
+    
+    <!-- Lidar Deskewing -->
+    <node if="$(arg deskewing)" pkg="nodelet" type="nodelet" name="lidar_deskewing" args="standalone rtabmap_ros/lidar_deskewing" output="screen">
+      <param name="wait_for_transform" value="0.01"/>
+      <param name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
+      <param name="slerp" value="$(arg slerp)"/>
+      <remap from="input_cloud" to="$(arg scan_topic)"/>
+    </node>
+
+    <arg     if="$(arg deskewing)" name="scan_topic_deskewed" default="$(arg scan_topic)/deskewed"/>
+    <arg unless="$(arg deskewing)" name="scan_topic_deskewed" default="$(arg scan_topic)"/>
 
     <group ns="rtabmap">
       <node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen">
-        <remap from="scan_cloud" to="/os_cloud_node/points"/>
-        <remap from="imu"        to="/os_cloud_node/imu/data"/>
+        <remap from="scan_cloud" to="$(arg scan_topic_deskewed)"/>
+        <remap from="imu"        to="$(arg imu_topic)/filtered"/>
         <param name="frame_id"        type="string" value="$(arg frame_id)"/>  
         <param name="odom_frame_id"   type="string" value="odom"/>
         <param     if="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="25"/>
@@ -116,8 +129,8 @@
         <param name="approx_sync"          type="bool" value="false"/>
         
         <remap     if="$(arg assemble)" from="scan_cloud" to="assembled_cloud"/>
-        <remap unless="$(arg assemble)" from="scan_cloud" to="/os_cloud_node/points"/>
-        <remap from="imu"        to="/os_cloud_node/imu/data"/>
+        <remap unless="$(arg assemble)" from="scan_cloud" to="$(arg scan_topic_deskewed)"/>
+        <remap from="imu"        to="$(arg imu_topic)/filtered"/>
      
         <!-- RTAB-Map's parameters -->
         <param     if="$(arg assemble)" name="Rtabmap/DetectionRate" type="string" value="0"/> <!-- already set 1 Hz in point_cloud_assembler -->
@@ -158,7 +171,7 @@
       </node>
       
       <node if="$(arg assemble)" pkg="rtabmap_ros" type="point_cloud_assembler" name="point_cloud_assembler" output="screen">
-        <remap from="cloud"           to="/os_cloud_node/points"/>
+        <remap from="cloud" to="$(arg scan_topic_deskewed)"/>
         <remap from="odom"            to="odom"/>
         <param name="assembling_time" type="double" value="1" />
         <param name="fixed_frame_id"  type="string" value="" />
@@ -170,7 +183,7 @@
         <param name="subscribe_odom_info" type="bool" value="true"/>
         <param name="subscribe_scan_cloud" type="bool" value="true"/>
         <param name="approx_sync" type="bool" value="false"/>
-        <remap from="scan_cloud" to="/os_cloud_node/points"/>
+        <remap from="scan_cloud" to="odom_filtered_input_scan"/>
       </node>
   </group>
 

launch/tests/test_velodyne.launch

@@ -14,7 +14,9 @@
     <arg name="use_imu"       default="false"/> <!-- Assuming IMU fixed to lidar with /velodyne -> /imu_link TF -->
     <arg name="imu_topic"     default="/imu/data"/>
     <arg name="scan_20_hz"    default="false"/> <!-- If we launch the velodyne with "rpm:=1200" argument -->
-    <arg name="organize_cloud" default="false"/>
+    <arg name="deskewing"     default="true"/>
+    <arg name="slerp"         default="false"/> <!-- If true, a slerp between the first and last time will be used to deskew each point, which is faster than using tf for every point but less accurate -->
+    <arg name="organize_cloud" default="$(arg deskewing)"/> <!-- Should be organized if deskewing is enabled -->
     <arg name="scan_topic"    default="/velodyne_points"/>
     <arg name="use_sim_time"  default="false"/>
     <param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
@@ -24,7 +26,7 @@
     <arg name="queue_size_odom" default="1"/>  <!-- Set to 100 for kitti dataset to make sure all scans are processed -->
     <arg name="loop_ratio"      default="0.2"/>
 
-    <arg name="resolution"         default="0.1"/> <!-- set 0.1-0.3 for indoor, set 0.3-0.5 for outdoor (0.4 for kitti) -->
+    <arg name="resolution"         default="0.05"/> <!-- set 0.05-0.3 for indoor, set 0.3-0.5 for outdoor (0.4 for kitti) -->
     <arg name="iterations"         default="10"/>
     
     <!-- Grid parameters -->
@@ -34,7 +36,7 @@
     <!-- For F2M Odometry -->
     <arg name="ground_normals_up" default="false"/> <!-- set to true when velodyne is always horizontal to ground (ground robot, car, kitti) -->
     <arg name="local_map_size"    default="15000"/>
-    <arg name="key_frame_thr"     default="0.8"/>
+    <arg name="key_frame_thr"     default="0.6"/>
     
     <!-- For FLOAM Odometry -->
     <arg name="floam"    default="false"/> <!-- RTAB-Map should be built with FLOAM http://official-rtab-map-forum.206.s1.nabble.com/icp-odometry-with-LOAM-crash-tp8261p8563.html -->
@@ -46,9 +48,18 @@
        <arg name="organize_cloud" value="$(arg organize_cloud)"/>
     </include>
     
-    <!-- IMU orientation estimation and publish tf accordingly to os1_sensor frame -->
-    <node if="$(arg use_imu)" pkg="rtabmap_ros" type="imu_to_tf" name="imu_to_tf">
-      <remap from="imu/data" to="$(arg imu_topic)"/>
+    <!-- IMU orientation estimation and publish tf -->
+    <node if="$(arg use_imu)" pkg="nodelet" type="nodelet" name="imu_nodelet_manager" args="manager">
+      <remap from="imu/data_raw" to="$(arg imu_topic)"/>
+      <remap from="imu/data" to="$(arg imu_topic)/filtered"/>
+    </node>
+    <node if="$(arg use_imu)" pkg="nodelet" type="nodelet" name="imu_filter" args="load imu_filter_madgwick/ImuFilterNodelet imu_nodelet_manager">
+      <param name="use_mag" value="false"/>
+      <param name="world_frame" value="enu"/>
+      <param name="publish_tf" value="false"/>
+    </node>
+    <node if="$(arg use_imu)" pkg="nodelet" type="nodelet" name="imu_to_tf" args="load rtabmap_ros/imu_to_tf imu_nodelet_manager">
+      <remap from="imu/data" to="$(arg imu_topic)/filtered"/>
       <param name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
       <param name="base_frame_id" value="$(arg frame_id)"/>
     </node> 
@@ -58,12 +69,14 @@
         <remap from="scan_cloud" to="$(arg scan_topic)"/>
         <param name="frame_id"        type="string" value="$(arg frame_id)"/>  
         <param name="odom_frame_id"   type="string" value="odom"/>
+        <param name="deskewing"       type="bool"   value="$(arg deskewing)"/>
+        <param name="deskewing_slerp" type="bool"   value="$(arg slerp)"/>
         <param name="queue_size"      type="int"    value="$(arg queue_size_odom)"/>
         <param name="wait_for_transform_duration" type="double" value="0.2"/>
         <param     if="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="25"/>
         <param unless="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="15"/>
 
-        <remap if="$(arg use_imu)" from="imu" to="$(arg imu_topic)"/>
+        <remap if="$(arg use_imu)" from="imu" to="$(arg imu_topic)/filtered"/>
         <param if="$(arg use_imu)" name="guess_frame_id"   type="string" value="$(arg frame_id)_stabilized"/>
         <param if="$(arg use_imu)" name="wait_imu_to_init" type="bool" value="true"/>
      
@@ -92,8 +105,9 @@
         <param name="OdomF2M/ScanMaxSize"        type="string" value="$(arg local_map_size)"/>
         <param name="OdomLOAM/Sensor"            type="string" value="$(arg floam_sensor)"/> 
         <param name="OdomLOAM/Resolution"        type="string" value="$(arg resolution)"/>
-        <param     if="$(arg scan_20_hz)" name="OdomLOAM/ScanPeriod" type="string" value="0.05"/>
-        <param unless="$(arg scan_20_hz)" name="OdomLOAM/ScanPeriod" type="string" value="0.1"/>
+        <param if="$(eval not deskewing and scan_20_hz)" name="OdomLOAM/ScanPeriod" type="string" value="0.05"/>
+        <param if="$(eval not deskewing and not scan_20_hz)" name="OdomLOAM/ScanPeriod" type="string" value="0.1"/>
+        <param if="$(arg deskewing)" name="OdomLOAM/ScanPeriod" type="string" value="0"/>
       </node>
 
       <node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">	  
@@ -105,7 +119,7 @@
         <param name="wait_for_transform_duration" type="double" value="0.2"/>
         
         <remap from="scan_cloud" to="assembled_cloud"/>
-        <remap from="imu" to="$(arg imu_topic)"/>
+        <remap from="imu" to="$(arg imu_topic)/filtered"/>
      
         <!-- RTAB-Map's parameters -->
         <param name="Rtabmap/DetectionRate"          type="string" value="1"/>  
@@ -127,7 +141,7 @@
         <param name="Optimizer/GravitySigma"         type="string" value="0.3"/>
 
         <!-- ICP parameters -->
-        <param name="Icp/VoxelSize"                  type="string" value="0"/> <!-- already voxelized by point_cloud_assembler below -->
+        <param name="Icp/VoxelSize"                  type="string" value="$(arg resolution)"/>
         <param name="Icp/PointToPlaneK"              type="string" value="20"/>
         <param name="Icp/PointToPlaneRadius"         type="string" value="0"/>
         <param name="Icp/PointToPlane"               type="string" value="true"/>
@@ -151,12 +165,12 @@
       </node>
 
       <node pkg="nodelet" type="nodelet" name="point_cloud_assembler" args="standalone rtabmap_ros/point_cloud_assembler" output="screen">
-        <remap from="cloud"           to="$(arg scan_topic)"/>
+        <remap     if="$(arg deskewing)" from="cloud" to="odom_filtered_input_scan"/>
+        <remap unless="$(arg deskewing)" from="cloud" to="$(arg scan_topic)"/>
         <remap from="odom"            to="odom"/>
         <param     if="$(arg scan_20_hz)" name="max_clouds"      type="int"    value="20" />
         <param unless="$(arg scan_20_hz)" name="max_clouds"      type="int"    value="10" />
         <param name="fixed_frame_id"  type="string" value="" />
-        <param name="voxel_size"      type="double" value="$(arg resolution)" />
         <param name="queue_size"      type="int"    value="$(arg queue_size)" />
       </node>
   </group>

launch/tests/test_velodyne_t265_deskewing.launch

@@ -0,0 +1,184 @@
+<!-- -->
+<launch>
+  
+    <!-- 
+    Hand-held 3D lidar mapping example using only a Velodyne PUCK and external odometry (using t265 as example).
+    Prerequisities: rtabmap should be built with libpointmatcher
+    We use T265 only for lidar deskewing and icp_odometry guess in this example.
+    Example:
+     $ roslaunch rtabmap_ros test_velodyne_t265_deskewing.launch
+     $ rosrun rviz rviz -f map
+     $ Show TF and /rtabmap/cloud_map topics
+    -->
+
+    <arg name="rtabmapviz"    default="true"/>
+    <arg name="scan_20_hz"    default="false"/> <!-- If we launch the velodyne with "rpm:=1200" argument -->
+    <arg name="deskewing"     default="true"/>
+    <arg name="slerp"         default="false"/> <!-- If true, a slerp between the first and last time will be used to deskew each point, which is faster than using tf for every point but less accurate -->
+    <arg name="scan_topic"    default="/velodyne_points"/>
+    <arg name="use_sim_time"  default="false"/>
+    <param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
+
+    <arg name="odom_frame_id"   default="t265_odom_frame"/> <!-- input odometry: here we use T265 odometry, but it could be wheel odometry -->
+    <arg name="frame_id"        default="velodyne"/>       <!-- base frame of the robot: for this example, we use velodyne as base frame -->
+    <arg name="queue_size"      default="10"/>
+    <arg name="queue_size_odom" default="1"/>
+    <arg name="loop_ratio"      default="0.2"/>
+
+    <arg name="resolution"         default="0.05"/> <!-- set 0.05-0.3 for indoor, set 0.3-0.5 for outdoor -->
+    <arg name="iterations"         default="10"/>
+    
+    <!-- Grid parameters -->
+    <arg name="ground_is_obstacle" default="true"/>
+    <arg name="grid_max_range"     default="20"/>
+    
+    <!-- For F2M Odometry -->
+    <arg name="ground_normals_up" default="false"/> <!-- set to true when velodyne is always horizontal to ground (ground robot, car) -->
+    <arg name="local_map_size"    default="15000"/>
+    <arg name="key_frame_thr"     default="0.6"/>
+    
+    <!-- For FLOAM Odometry -->
+    <arg name="floam"    default="false"/> <!-- RTAB-Map should be built with FLOAM http://official-rtab-map-forum.206.s1.nabble.com/icp-odometry-with-LOAM-crash-tp8261p8563.html -->
+    <arg name="floam_sensor" default="0"/> <!-- 0=16 rings (VLP16), 1=32 rings, 2=64 rings -->
+    
+    <!-- Static transform between velodyne and T265: TODO: Adjust with real position/orientation!!! -->
+    <node unless="$(arg use_sim_time)" pkg="tf" type="static_transform_publisher" name="T265_to_velodyne_tf" args="-0.01 0 0.055 0 0 0 t265_link velodyne 100"/>
+    
+    <!-- Velodyne sensor VLP16 -->
+    <include unless="$(arg use_sim_time)" file="$(find velodyne_pointcloud)/launch/VLP16_points.launch">
+       <arg     if="$(arg scan_20_hz)" name="rpm" value="1200"/>
+       <arg unless="$(arg scan_20_hz)" name="rpm" value="600"/>
+       <arg name="organize_cloud" value="true"/> <!-- should be organized for deskewing -->
+    </include>
+    
+    <!-- T265 -->
+    <group unless="$(arg use_sim_time)" ns="t265">
+      <include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
+        <arg name="device_type"           value="t265"/>
+        <arg name="serial_no"             value=""/>
+        <arg name="tf_prefix"             value="t265"/>
+        <arg name="initial_reset"         value="false"/>
+        <arg name="enable_fisheye1"       value="false"/>
+        <arg name="enable_fisheye2"       value="false"/>
+        <arg name="topic_odom_in"         value=""/>
+        <arg name="calib_odom_file"       value=""/>
+        <arg name="enable_pose"           value="true"/>
+      </include>
+    </group>
+    
+    <!-- Lidar Deskewing -->
+    <node if="$(arg deskewing)" pkg="nodelet" type="nodelet" name="lidar_deskewing" args="standalone rtabmap_ros/lidar_deskewing" output="screen">
+      <param name="wait_for_transform" value="0.01"/>
+      <param name="fixed_frame_id" value="$(arg odom_frame_id)"/>
+      <param name="slerp" value="$(arg slerp)"/>
+      <remap from="input_cloud" to="$(arg scan_topic)"/>
+    </node>
+
+    <arg     if="$(arg deskewing)" name="scan_topic_deskewed" default="$(arg scan_topic)/deskewed"/>
+    <arg unless="$(arg deskewing)" name="scan_topic_deskewed" default="$(arg scan_topic)"/>
+
+    <group ns="rtabmap">
+      <node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen">
+        <remap from="scan_cloud" to="$(arg scan_topic_deskewed)"/>
+        <param name="guess_frame_id"  type="string" value="$(arg odom_frame_id)"/>
+        <param name="frame_id"        type="string" value="$(arg frame_id)"/>
+        <param name="odom_frame_id"   type="string" value="odom"/>
+        <param name="queue_size"      type="int"    value="$(arg queue_size_odom)"/>
+        <param name="wait_for_transform_duration" type="double" value="0.2"/>
+        <param     if="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="25"/>
+        <param unless="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="15"/>
+     
+        <!-- ICP parameters -->
+        <param name="Icp/PointToPlane"        type="string" value="true"/>
+        <param name="Icp/Iterations"          type="string" value="$(arg iterations)"/>
+        <param     if="$(arg floam)" name="Icp/VoxelSize" type="string" value="0"/>
+        <param unless="$(arg floam)" name="Icp/VoxelSize" type="string" value="$(arg resolution)"/>
+        <param name="Icp/DownsamplingStep"    type="string" value="1"/> <!-- cannot be increased with ring-like lidar -->
+        <param name="Icp/Epsilon"             type="string" value="0.001"/>
+        <param     if="$(arg floam)" name="Icp/PointToPlaneK" type="string" value="0"/>
+        <param unless="$(arg floam)" name="Icp/PointToPlaneK" type="string" value="20"/>
+        <param name="Icp/PointToPlaneRadius"  type="string" value="0"/>
+        <param name="Icp/MaxTranslation"      type="string" value="2"/>
+        <param name="Icp/MaxCorrespondenceDistance" type="string" value="$(eval resolution*10)"/>
+        <param name="Icp/PM"                  type="string" value="true"/> 
+        <param name="Icp/PMOutlierRatio"      type="string" value="0.7"/>
+        <param name="Icp/CorrespondenceRatio" type="string" value="0.01"/>  
+        <param if="$(arg ground_normals_up)" name="Icp/PointToPlaneGroundNormalsUp"  type="string" value="0.8"/>
+
+        <!-- Odom parameters -->       
+        <param name="Odom/ScanKeyFrameThr"       type="string" value="$(arg key_frame_thr)"/>
+        <param     if="$(arg floam)" name="Odom/Strategy" type="string" value="11"/>
+        <param unless="$(arg floam)" name="Odom/Strategy" type="string" value="0"/>
+        <param name="OdomF2M/ScanSubtractRadius" type="string" value="$(arg resolution)"/>
+        <param name="OdomF2M/ScanMaxSize"        type="string" value="$(arg local_map_size)"/>
+        <param name="OdomLOAM/Sensor"            type="string" value="$(arg floam_sensor)"/> 
+        <param name="OdomLOAM/Resolution"        type="string" value="$(arg resolution)"/>
+        <param if="$(eval not deskewing and scan_20_hz)" name="OdomLOAM/ScanPeriod" type="string" value="0.05"/>
+        <param if="$(eval not deskewing and not scan_20_hz)" name="OdomLOAM/ScanPeriod" type="string" value="0.1"/>
+        <param if="$(arg deskewing)" name="OdomLOAM/ScanPeriod" type="string" value="0"/>
+      </node>
+
+      <node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">	  
+        <param name="frame_id"             type="string" value="$(arg frame_id)"/>  
+        <param name="subscribe_depth"      type="bool" value="false"/>
+        <param name="subscribe_rgb"        type="bool" value="false"/>
+        <param name="subscribe_scan_cloud" type="bool" value="true"/>
+        <param name="approx_sync"          type="bool" value="false"/>
+        <param name="wait_for_transform_duration" type="double" value="0.2"/>
+        
+        <remap from="scan_cloud" to="assembled_cloud"/>
+     
+        <!-- RTAB-Map's parameters -->
+        <param name="Rtabmap/DetectionRate"          type="string" value="1"/>  
+        <param name="RGBD/NeighborLinkRefining"      type="string" value="false"/>
+        <param name="RGBD/ProximityBySpace"          type="string" value="true"/>
+        <param name="RGBD/ProximityMaxGraphDepth"    type="string" value="0"/>
+        <param name="RGBD/ProximityPathMaxNeighbors" type="string" value="1"/>
+        <param name="RGBD/AngularUpdate"             type="string" value="0.05"/>
+        <param name="RGBD/LinearUpdate"              type="string" value="0.05"/>
+        <param name="Mem/NotLinkedNodesKept"         type="string" value="false"/>
+        <param name="Mem/STMSize"                    type="string" value="30"/>
+        <param name="Mem/LaserScanNormalK"           type="string" value="20"/>
+        
+        <param name="Reg/Strategy"                   type="string" value="1"/> 
+        <param name="Grid/CellSize"                  type="string" value="$(arg resolution)"/>
+        <param name="Grid/RangeMax"                  type="string" value="$(arg grid_max_range)"/>
+        <param name="Grid/ClusterRadius"             type="string" value="1"/>
+        <param name="Grid/GroundIsObstacle"          type="string" value="$(arg ground_is_obstacle)"/>
+        <param name="Optimizer/GravitySigma"         type="string" value="0.3"/>
+
+        <!-- ICP parameters -->
+        <param name="Icp/VoxelSize"                  type="string" value="$(arg resolution)"/>
+        <param name="Icp/PointToPlaneK"              type="string" value="20"/>
+        <param name="Icp/PointToPlaneRadius"         type="string" value="0"/>
+        <param name="Icp/PointToPlane"               type="string" value="true"/>
+        <param name="Icp/Iterations"                 type="string" value="$(arg iterations)"/>
+        <param name="Icp/Epsilon"                    type="string" value="0.001"/>
+        <param name="Icp/MaxTranslation"             type="string" value="3"/>
+        <param name="Icp/MaxCorrespondenceDistance"  type="string" value="$(eval resolution*10)"/>
+        <param name="Icp/PM"                         type="string" value="true"/> 
+        <param name="Icp/PMOutlierRatio"             type="string" value="0.7"/>
+        <param name="Icp/CorrespondenceRatio"        type="string" value="$(arg loop_ratio)"/>
+      </node>
+
+      <node if="$(arg rtabmapviz)" name="rtabmapviz" pkg="rtabmap_ros" type="rtabmapviz" output="screen">
+        <param name="frame_id" type="string" value="$(arg frame_id)"/>
+        <param name="odom_frame_id" type="string" value="odom"/>
+        <param name="subscribe_odom_info" type="bool" value="true"/>
+        <param name="subscribe_scan_cloud" type="bool" value="true"/>
+        <param name="approx_sync" type="bool" value="false"/>
+        <remap from="scan_cloud" to="odom_filtered_input_scan"/>
+        <remap from="odom_info" to="odom_info"/>
+      </node>
+
+      <node pkg="nodelet" type="nodelet" name="point_cloud_assembler" args="standalone rtabmap_ros/point_cloud_assembler" output="screen">
+        <remap from="cloud"           to="$(arg scan_topic_deskewed)"/>
+        <remap from="odom"            to="odom"/>
+        <param     if="$(arg scan_20_hz)" name="max_clouds"      type="int"    value="20" />
+        <param unless="$(arg scan_20_hz)" name="max_clouds"      type="int"    value="10" />
+        <param name="fixed_frame_id"  type="string" value="" />
+        <param name="queue_size"      type="int"    value="$(arg queue_size)" />
+      </node>
+  </group>
+
+</launch>