updated demo_hector_mapping.launch with odom_guess and camera options (also enabled proximity detection and updated icp parameters)

launch/demo/demo_hector_mapping.launch

@@ -14,6 +14,17 @@
   <!-- Choose hector_slam or icp_odometry for odometry -->
   <arg name="hector" default="true" />
 
+  <!-- If "hector" above is false, this option feeds wheel odometry to
+       icp_odometry as guess ( to be more robust to corridor-like environments).
+       If so, use original demo_mapping.bag containing wheel odometry! -->
+  <arg name="odom_guess" default="false" />
+
+  <!-- Example with camera or not -->
+  <arg name="camera" default="true" />
+
+  <!-- Example with camera or not -->
+
+
   <param name="use_sim_time" type="bool" value="True"/>
   
   <node if="$(arg hector)" pkg="tf" type="static_transform_publisher" name="scanmatcher_to_base_footprint" 
@@ -53,6 +64,9 @@
 	  
      <param name="frame_id"        type="string" value="base_footprint"/>
 
+     <param if="$(arg odom_guess)" name="odom_frame_id"   type="string" value="icp_odom"/>
+     <param if="$(arg odom_guess)" name="guess_frame_id"  type="string" value="odom"/>
+     
      <param name="Icp/PointToPlane"  type="string" value="true"/>
      <param name="Icp/VoxelSize"     type="string" value="0.05"/>
      <param name="Icp/Epsilon"       type="string" value="0.001"/>
@@ -60,7 +74,7 @@
      <param name="Icp/PointToPlaneRadius"  type="string" value="0.3"/>
      <param name="Icp/MaxCorrespondenceDistance" type="string" value="0.1"/>
      <param name="Icp/PM"             type="string" value="true"/> <!-- use libpointmatcher to handle PointToPlane with 2d scans-->
-     <param name="Icp/PMOutlierRatio" type="string" value="0.95"/>
+     <param name="Icp/PMOutlierRatio" type="string" value="0.85"/>
      <param name="Odom/Strategy"        type="string" value="0"/>
      <param name="Odom/GuessMotion"     type="string" value="true"/>
      <param name="Odom/ResetCountdown"  type="string" value="0"/>
@@ -68,7 +82,7 @@
   </node>
 
   <group ns="rtabmap">
-    <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync" output="screen">
+    <node if="$(arg camera)" pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync" output="screen">
       <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"/>
@@ -81,8 +95,9 @@
     <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="subscribe_rgb"   type="bool" value="false"/>
       <param name="subscribe_depth" type="bool" value="false"/>
-      <param name="subscribe_rgbd"  type="bool" value="true"/>
+      <param name="subscribe_rgbd"  type="bool" value="$(arg camera)"/>
       <param name="subscribe_scan"  type="bool" value="true"/>
 	
       <remap from="scan" to="/jn0/base_scan"/>
@@ -98,12 +113,14 @@
       <!-- RTAB-Map's parameters -->
       <param name="Reg/Strategy"       type="string" value="1"/>    <!-- 0=Visual, 1=ICP, 2=Visual+ICP -->
       <param name="Reg/Force3DoF"      type="string" value="true"/>
-      <param name="RGBD/ProximityBySpace"      type="string" value="false"/>
+      <param name="RGBD/ProximityBySpace"    type="string" value="true"/>
+      <param name="Icp/CorrespondenceRatio"  type="string" value="0.2"/>
+      <param name="Icp/VoxelSize"            type="string" value="0.05"/>
     </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_rgbd"      type="bool" value="true"/>
+      <param name="subscribe_rgbd"      type="bool" value="$(arg camera)"/>
       <param name="subscribe_laserScan" type="bool" value="true"/>
       <param name="frame_id"            type="string" value="base_footprint"/>
     
@@ -120,7 +137,7 @@
   
   <!-- 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">
+   <node if="$(arg camera)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
     <remap from="rgbd_image"      to="/rtabmap/rgbd_image"/>
     <remap from="cloud"           to="voxel_cloud" />
 

launch/demo/demo_robot_mapping.launch

@@ -57,6 +57,7 @@
 	  <param name="Icp/PM"                    type="string" value="false"/>
 	  <param name="Icp/PointToPlane"          type="string" value="false"/>
 	  <param name="Icp/MaxCorrespondenceDistance"  type="string" value="0.05"/>
+	  <param name="Icp/VoxelSize"             type="string" value="0.05"/>
 	  
 	  <!-- localization mode -->
 	  <param     if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>

launch/jfr2018/launch_lidar

@@ -1,5 +1,5 @@
 // icp_odometry with guess from odom_combined frame
-$ roslaunch rtabmap_ros rtabmap.launch args:="-d --Rtabmap/PublishRAMUsage true --Reg/Force3DoF true --Reg/Strategy 1 --RGBD/ProximityPathMaxNeighbors 10 --RGBD/ProximityPathFilteringRadius 1 --RGBD/ProximityByTime false --Mem/STMSize 30 --Mem/LaserScanVoxelSize 0.05 --Mem/LaserScanNormalK 5 --Mem/LaserScanNormalRadius 1 --Icp/Epsilon 0.001 --Icp/MaxTranslation 0.5 --RGBD/OptimizeMaxError 1 --Icp/PointToPlane true --Icp/CorrespondenceRatio 0.10 --Icp/PMOutlierRatio 0.95 --Mem/BinDataKept true --Grid/RangeMax 0 --Kp/DetectorStrategy 0 --Kp/MaxFeatures 200 --SURF/HessianThreshold 100 --Vis/MaxFeatures 500 --Mem/UseOdomFeatures false" odom_args:="--Icp/VoxelSize 0.05 --Icp/PointToPlaneRadius 1 --Odom/GuessMotion true --Odom/Strategy 0"  rgbd_sync:=true subscribe_scan:=true frame_id:=base_footprint ground_truth_frame_id:=world ground_truth_base_frame_id:=scan_gt use_sim_time:=true visual_odometry:=false odom_guess_frame_id:=odom_combined odom_guess_min_translation:=0.1 odom_guess_min_rotation:=0.1 odom_topic:=odom icp_odometry:=true database_path:=/media/mathieu/5B60E7B25BDFCB79/bags/rtabmap.db
+$ roslaunch rtabmap_ros rtabmap.launch args:="-d --Rtabmap/PublishRAMUsage true --Reg/Force3DoF true --Reg/Strategy 1 --RGBD/ProximityPathMaxNeighbors 10 --RGBD/ProximityPathFilteringRadius 1 --RGBD/ProximityByTime false --Mem/STMSize 30 --Mem/LaserScanVoxelSize 0.05 --Icp/VoxelSize 0.0 --Mem/LaserScanNormalK 5 --Mem/LaserScanNormalRadius 1 --Icp/Epsilon 0.001 --Icp/MaxTranslation 0.5 --RGBD/OptimizeMaxError 1 --Icp/PointToPlane true --Icp/CorrespondenceRatio 0.10 --Icp/PMOutlierRatio 0.95 --Mem/BinDataKept true --Grid/RangeMax 0 --Kp/DetectorStrategy 0 --Kp/MaxFeatures 200 --SURF/HessianThreshold 100 --Vis/MaxFeatures 500 --Mem/UseOdomFeatures false" odom_args:="--Icp/VoxelSize 0.05 --Icp/PointToPlaneRadius 1 --Odom/GuessMotion true --Odom/Strategy 0"  rgbd_sync:=true subscribe_scan:=true frame_id:=base_footprint ground_truth_frame_id:=world ground_truth_base_frame_id:=scan_gt use_sim_time:=true visual_odometry:=false odom_guess_frame_id:=odom_combined odom_guess_min_translation:=0.1 odom_guess_min_rotation:=0.1 odom_topic:=odom icp_odometry:=true database_path:=/media/mathieu/5B60E7B25BDFCB79/bags/rtabmap.db
 // proximity space short range
 scan_topic:=/base_scan_t_filtered
 // proximity space longer range