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

2026-10-04 00:37:46 +08:00 · 2021-03-01 11:27:43 -05:00
parent fe17d86870
commit e27d939544
3 changed files with 27 additions and 9 deletions
@@ -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" 
@@ -22,7 +33,7 @@
  <!-- Odometry from laser scans -->
  <!-- If argument "hector" is true, we use Hector mapping to generate odometry for us -->
  <node if="$(arg hector)" pkg="hector_mapping" type="hector_mapping" name="hector_mapping" output="screen">
-    
+
    <!-- Frame names -->
    <param name="map_frame" value="hector_map" />
    <param name="base_frame" value="base_footprint" />
@@ -51,7 +62,10 @@
     <remap from="odom"      to="/scanmatch_odom"/>
     <remap from="odom_info"      to="/rtabmap/odom_info"/>
 	  
-     <param name="frame_id"        type="string" value="base_footprint"/>   
+     <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"/>
@@ -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" />

@@ -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"/>
@@ -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