Comparison between realsense D435 vs T265 vs T265+D435 dual setup

[gian]

Hello Mathieu

I realized that the problem is in the interaction between ground normal segmentation  and the values that I set in the costmaps and in rtabmap , indeed the plugin "cut" as ground the planar part of the table ( I can see in rviz ) so the obstacle cloud is considered only the edge of the table.
The sensor frame in the costmaps is the d435 camera , so the min obstacle height with respect to the camera should be -0.7 ( in this way the edge is detected)  . But the segmentation works at rtabmap Grid parameters level ( that are positive respect to the ground ) . How to resolve this problem ?
Thanks
Regards
gian

[matlabbe]

We can set Grid/MaxGroundHeight to 0.1 for example to make sure that all planes over 10 cm are  considered as obstacles. In your case, if the ground appears -40 cm from the frame_id set to rtabmap, the code would work with negative value too.

[gian]

Hello Mathieu

I tried Grid/MaxGroundHeight to 0.1 , or even negative values but the result is always the same : the Ground segmentation  grabs points on all plane surface that it finds  : shelves, tables, piano.
I also commented the Grid/Footprint* params , but it has no interaction on the ground segmentation.
This is the Ground pointcloud where in the FOV there isn't appreciable plane surfaces



And this is the ground in front of a shelf
https://github.com/introlab/rtabmap/blob/c5ec4f337bb077a550f16d00a699b99f5900bb6d/corelib/include/rtabmap/core/impl/util3d_mapping.hpp#L131



As you see is full of planar points
Maybe could be related to this concatenation of ground points and flat surfaces  ?
https://github.com/introlab/rtabmap/blob/c5ec4f337bb077a550f16d00a699b99f5900bb6d/corelib/include/rtabmap/core/impl/util3d_mapping.hpp#L131

Please help
Regards
gian

[matlabbe]

Hi,

I think I understand the bug in https://github.com/introlab/rtabmap/blob/c5ec4f337bb077a550f16d00a699b99f5900bb6d/corelib/include/rtabmap/core/impl/util3d_mapping.hpp#L101-L119

If maxGroundHeight!=0, we should set biggestFlatSurfaceIndex to -1, so that https://github.com/introlab/rtabmap/blob/c5ec4f337bb077a550f16d00a699b99f5900bb6d/corelib/include/rtabmap/core/impl/util3d_mapping.hpp#L119 should not be done and thus ground will be empty. With ground empty, everything else will be obstacles.

I'll try to reproduce the error to test this approach.

Mathieu

[gian]

Thank you Mathieu !!!
gian

[matlabbe]

I was not able to reproduce your problem with the old code, but here the new one with min/max correctly set: https://github.com/introlab/rtabmap/commit/fbc30042c4d0b1a25b29ea3876e2bf625ec9bb42

[gian]

Ok ,one reason more to compile on Jetson nano ! Thank you very much Mathieu,I will let you know
Regards
Gian

[gian]

Hello Mathieu,

I installed 0.20.5 on remote pc and on Jetson companion but no luck , it seems the same behaviour

These are respectively the pointcloud of the shelf , the obstacle and the ground cloud

Could you reproduce it ?
Thank you
gian















 

[matlabbe]

If you can record a database and share it, it could be easier to reproduce your exact setup. Or record a rosbag to depth image, depth camera_info, tf and tf_static (inputs used for obstales_detection)

[gian]

Hello Mathieu,

Thanks for your support and for spending your time .I recorded the following  :

rtabmap db only in front of the shelf ( 18M )

https://we.tl/t-H052Ivzr1G

and related rosbag  with also /obstacles_cloud ( 340M)

https://we.tl/t-FAjqmcRnSs

and a longer rtabmap db ( 30M ) and rosbag ( 1G )

https://we.tl/t-YKaqncGCUW

https://we.tl/t-p9vVWxwWnf

Thank you !
gian

[matlabbe]

Can you share the launch file using obstacles_detection nodelet? I cannot reproduce the problem with the database.

[gian]

Hello Mathieu

 here a the launch files :

EDIT :  you will find that I have commented out t265_odom_frame and /t265/odom/sample on the files ,leaving "odom" frame and /odom topic,  the reason is that until yesterday I have always used that setup until and the obstacle problem was present as well . Yesterday and today (so also  for the recording ) I have tried a node that converts the odometry of the t265 in the odom frame of base_link, but the result is the same.
There isn't wheel odometry , only t265.
this is the node  https://github.com/whoobee/sensor_odom_manager




xacro of the t265 + d435 mount
cat /catkin_ws/src/realsense-ros/realsense2_camera/urdf$ cat mount_t265_d435.urdf.xacro

<?xml version="1.0"?>
<!--<robot name="origins">-->
<robot name="origins">
  <link name="t265_pose_frame"/>
  <link name="d400_link"/>

  <joint name="t265_to_d400" type="fixed">
    <parent link="t265_pose_frame"/>
    <child link="d400_link"/>
    <origin xyz="0.009 0.021 0.027" rpy="0.000 -0.018 0.005"/>
  </joint>
</robot>

tf between base link and t265_pose_frame

rosrun tf static_transform_publisher 0 0 0.51 0 0 0  base_link  t265_pose_frame 100


resulting tree































launch command

 roslaunch rtabmap_ros rtabmap.launch    args:="-d  --Mem/UseOdomGravity true --Optimizer/GravitySigma 0.3 "    odom_topic:=/odom    frame_id:=base_link    rgbd_sync:=true    depth_topic:=/d400/aligned_depth_to_color/image_raw    rgb_topic:=/d400/color/image_raw    camera_info_topic:=/d400/color/camera_info    approx_rgbd_sync:=false    visual_odometry:=false localization:=false  rtabmapviz:=false rviz:=false




rtabmap.launch

<launch>
  <!-- Convenience launch file to launch odometry, rtabmap and rtabmapviz nodes at once -->

  <!-- For stereo:=false
        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 -->
           
  <!-- For stereo:=true
        Your camera should be calibrated and publishing rectified left and right 
        images + corresponding camera_info msgs. You can use stereo_image_proc for image rectification.
        Example:
           $ roslaunch rtabmap_ros bumblebee.launch -->
     
  <!-- Choose between depth and stereo, set both to false to do only scan -->      
  <arg name="stereo"                    default="false"/>
  <arg     if="$(arg stereo)" name="depth"  default="false"/>
  <arg unless="$(arg stereo)" name="depth"  default="true"/>
 
  <!-- Choose visualization -->
  <arg name="rtabmapviz"              default="false" /> 
  <arg name="rviz"                    default="true" />
  
  <!-- Localization-only mode -->
  <arg name="localization"            default="false"/>
  
  <!-- sim time for convenience, if playing a rosbag -->
  <arg name="use_sim_time"            default="false"/>
  <param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
  
  <!-- Corresponding config files -->
  <arg name="cfg"                     default="" /> <!-- To change RTAB-Map's parameters, set the path of config file (*.ini) generated by the standalone app -->
  <arg name="gui_cfg"                 default="~/.ros/rtabmap_gui.ini" />
  <arg name="rviz_cfg"                default="$(find rtabmap_ros)/launch/config/rgbd.rviz" />
  
  <arg name="frame_id"                default="base_link"/>     <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
  <arg name="odom_frame_id"           default="odom"/>                <!-- If set, TF is used to get odometry instead of the topic -->
  <arg name="map_frame_id"            default="map"/>
  <arg name="ground_truth_frame_id"   default=""/>     <!-- e.g., "world" -->
  <arg name="ground_truth_base_frame_id" default=""/>  <!-- e.g., "tracker", a fake frame matching the frame "frame_id" (but on different TF tree) -->
  <arg name="namespace"               default="rtabmap"/>
  <arg name="database_path"           default="~/.ros/rtabmap.db"/>
  <arg name="queue_size"              default="100"/>
  <arg name="wait_for_transform"      default="0.2"/>
  <arg name="args"                    default=""/>              <!-- delete_db_on_start, udebug -->
  <arg name="rtabmap_args"            default="$(arg args)"/>   <!-- deprecated, use "args" argument -->
  <arg name="launch_prefix"           default=""/>              <!-- for debugging purpose, it fills launch-prefix tag of the nodes -->
  <arg name="output"                  default="screen"/>        <!-- Control node output (screen or log) -->
  <arg name="publish_tf_map"          default="true"/>

  <!-- if timestamps of the input topics are synchronized using approximate or exact time policy-->
  <arg     if="$(arg stereo)" name="approx_sync"  default="false"/>
  <arg unless="$(arg stereo)" name="approx_sync"  default="$(arg depth)"/>         
   
  <!-- RGB-D related topics -->
  <arg name="rgb_topic"               default="/camera/rgb/image_rect_color" />
  <arg name="depth_topic"             default="/camera/depth_registered/image_raw" />
  <arg name="camera_info_topic"       default="/camera/rgb/camera_info" />
  <arg name="depth_camera_info_topic" default="$(arg camera_info_topic)" />
  
  <!-- stereo related topics -->
  <arg name="stereo_namespace"        default="/stereo_camera"/>
  <arg name="left_image_topic"        default="$(arg stereo_namespace)/left/image_rect_color" />
  <arg name="right_image_topic"       default="$(arg stereo_namespace)/right/image_rect" />      <!-- using grayscale image for efficiency -->
  <arg name="left_camera_info_topic"  default="$(arg stereo_namespace)/left/camera_info" />
  <arg name="right_camera_info_topic" default="$(arg stereo_namespace)/right/camera_info" />
  
  <!-- Already synchronized RGB-D related topic, with rtabmap_ros/rgbd_sync nodelet -->
  <arg name="rgbd_sync"               default="false"/>         <!-- pre-sync rgb_topic, depth_topic, camera_info_topic -->
  <arg name="approx_rgbd_sync"        default="true"/>          <!-- false=exact synchronization -->
  <arg name="subscribe_rgbd"          default="$(arg rgbd_sync)"/>
  <arg name="rgbd_topic"              default="rgbd_image" />
  <arg name="depth_scale"             default="1.0" />         <!-- Deprecated, use rgbd_depth_scale instead -->
  <arg name="rgbd_depth_scale"        default="$(arg depth_scale)" />
  <arg name="rgbd_decimation"         default="1" />
  
  <arg name="compressed"              default="false"/>         <!-- If you want to subscribe to compressed image topics -->
  <arg name="rgb_image_transport"     default="compressed"/>    <!-- Common types: compressed, theora (see "rosrun image_transport list_transports") -->
  <arg name="depth_image_transport"   default="compressedDepth"/>  <!-- Depth compatible types: compressedDepth (see "rosrun image_transport list_transports") -->
   
  <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="subscribe_scan_descriptor" default="false"/>
  <arg name="scan_descriptor_topic"   default="/scan_descriptor"/>
  <arg name="scan_cloud_max_points"   default="0"/>
  <arg name="scan_cloud_filtered"     default="false"/> <!-- use filtered cloud from icp_odometry for mapping -->
   
  <arg name="visual_odometry"          default="true"/>          <!-- Launch rtabmap visual odometry node -->
  <arg name="icp_odometry"             default="false"/>         <!-- Launch rtabmap icp odometry node -->
  <arg name="odom_topic"               default="odom"/>          <!-- Odometry topic name -->
  <arg name="vo_frame_id"              default="$(arg odom_topic)"/> <!-- Visual/Icp odometry frame ID for TF -->
  <arg name="publish_tf_odom"          default="true"/>
  <arg name="odom_tf_angular_variance" default="1"/>             <!-- If TF is used to get odometry, this is the default angular variance -->
  <arg name="odom_tf_linear_variance"  default="1"/>             <!-- If TF is used to get odometry, this is the default linear variance -->
  <arg name="odom_args"                default=""/>              <!-- More arguments for odometry (overwrite same parameters in rtabmap_args) -->
  <arg name="odom_sensor_sync"         default="false"/>
  <arg name="odom_guess_frame_id"        default=""/>
  <arg name="odom_guess_min_translation" default="0"/>
  <arg name="odom_guess_min_rotation"    default="0"/>
  <arg name="odom_max_rate"            default="0"/>
  <arg name="odom_expected_rate"       default="0"/>
  <arg name="imu_topic"                default="/imu/data"/>          <!-- only used with VIO approaches -->
  <arg name="wait_imu_to_init"         default="false"/>
  
  <arg name="scan_cloud_assembling"              default="false"/>
  <arg name="scan_cloud_assembling_time"         default="1"/>
  <arg name="scan_cloud_assembling_fixed_frame"  default=""/>
  <arg name="scan_cloud_assembling_voxel_size"   default="0.05"/>
  
  <arg name="subscribe_user_data"      default="false"/>             <!-- user data synchronized subscription -->
  <arg name="user_data_topic"          default="/user_data"/>
  <arg name="user_data_async_topic"    default="/user_data_async" /> <!-- user data async subscription (rate should be lower than map update rate) -->
  
  <arg name="gps_topic"                default="/gps/fix" />         <!-- gps async subscription -->
  
  <arg name="tag_topic"                default="/tag_detections" />  <!-- apriltags async subscription -->
  <arg name="tag_linear_variance"      default="0.0001" />
  <arg name="tag_angular_variance"     default="9999" />             <!-- >=9999 means ignore rotation in optimization, when rotation estimation of the tag is not reliable -->
  
  <!-- These arguments should not be modified directly, see referred topics without "_relay" suffix above -->
  <arg if="$(arg compressed)"     name="rgb_topic_relay"           default="$(arg rgb_topic)_relay"/>
  <arg unless="$(arg compressed)" name="rgb_topic_relay"           default="$(arg rgb_topic)"/>
  <arg if="$(arg compressed)"     name="depth_topic_relay"         default="$(arg depth_topic)_relay"/>
  <arg unless="$(arg compressed)" name="depth_topic_relay"         default="$(arg depth_topic)"/>
  <arg if="$(arg compressed)"     name="left_image_topic_relay"    default="$(arg left_image_topic)_relay"/>
  <arg unless="$(arg compressed)" name="left_image_topic_relay"    default="$(arg left_image_topic)"/>
  <arg if="$(arg compressed)"     name="right_image_topic_relay"   default="$(arg right_image_topic)_relay"/>
  <arg unless="$(arg compressed)" name="right_image_topic_relay"   default="$(arg right_image_topic)"/>
  <arg if="$(arg rgbd_sync)"      name="rgbd_topic_relay"          default="$(arg rgbd_topic)"/>
  <arg unless="$(arg rgbd_sync)"  name="rgbd_topic_relay"          default="$(arg rgbd_topic)_relay"/>

  <!-- Nodes -->
  <group ns="$(arg namespace)">
     
    <!-- relays -->
    <group if="$(arg depth)">
      <group unless="$(arg subscribe_rgbd)">
        <node if="$(arg compressed)" name="republish_rgb" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg rgb_topic) raw out:=$(arg rgb_topic_relay)" />
        <node if="$(arg compressed)" name="republish_depth" type="republish" pkg="image_transport" args="$(arg depth_image_transport) in:=$(arg depth_topic) raw out:=$(arg depth_topic_relay)" />
      </group>
      <group if="$(arg rgbd_sync)">
        <node if="$(arg compressed)" name="republish_rgb" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg rgb_topic) raw out:=$(arg rgb_topic_relay)" />
        <node if="$(arg compressed)" name="republish_depth" type="republish" pkg="image_transport" args="$(arg depth_image_transport) in:=$(arg depth_topic) raw out:=$(arg depth_topic_relay)" />
        <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync" output="$(arg output)">
          <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
          <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
          <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
          <remap from="rgbd_image"      to="$(arg rgbd_topic_relay)"/>
          <param name="approx_sync"     type="bool"   value="$(arg approx_rgbd_sync)"/>
          <param name="queue_size"      type="int"    value="$(arg queue_size)"/>
          <param name="depth_scale"     type="double" value="$(arg rgbd_depth_scale)"/>
          <param name="decimation"     type="double" value="$(arg rgbd_decimation)"/>
        </node>
      </group>
    </group>
    <group if="$(arg stereo)">
      <group unless="$(arg subscribe_rgbd)">
        <node if="$(arg compressed)" name="republish_left"  type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg left_image_topic) raw out:=$(arg left_image_topic_relay)" />
        <node if="$(arg compressed)" name="republish_right" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg right_image_topic) raw out:=$(arg right_image_topic_relay)" />
      </group>
      <group if="$(arg rgbd_sync)">
        <node if="$(arg compressed)" name="republish_left"  type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg left_image_topic) raw out:=$(arg left_image_topic_relay)" />
        <node if="$(arg compressed)" name="republish_right" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg right_image_topic) raw out:=$(arg right_image_topic_relay)" />
        <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/stereo_sync" output="$(arg output)">
          <remap from="left/image_rect"   to="$(arg left_image_topic_relay)"/>
          <remap from="right/image_rect"  to="$(arg right_image_topic_relay)"/>
          <remap from="left/camera_info"  to="$(arg left_camera_info_topic)"/>
          <remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
          <remap from="rgbd_image"      to="$(arg rgbd_topic_relay)"/>
          <param name="approx_sync"     type="bool"   value="$(arg approx_rgbd_sync)"/>
          <param name="queue_size"      type="int"    value="$(arg queue_size)"/>
        </node>
      </group>
    </group>
    
    <group unless="$(arg rgbd_sync)">
       <group if="$(arg subscribe_rgbd)">
         <node name="republish_rgbd_image"  type="rgbd_relay" pkg="rtabmap_ros">
           <remap     if="$(arg compressed)" from="rgbd_image" to="$(arg rgbd_topic)/compressed"/>
           <remap     if="$(arg compressed)" from="$(arg rgbd_topic)/compressed_relay" to="$(arg rgbd_topic_relay)"/>
           <remap unless="$(arg compressed)" from="rgbd_image" to="$(arg rgbd_topic)"/>
           <param if="$(arg compressed)" name="uncompress" value="true"/>
         </node>
      </group>
    </group>

    <!-- Visual odometry -->
    <group unless="$(arg icp_odometry)">
      <group if="$(arg visual_odometry)">
      
        <!-- RGB-D Odometry -->
        <node unless="$(arg stereo)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
          <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
          <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
          <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
          <remap from="rgbd_image"      to="$(arg rgbd_topic_relay)"/>
          <remap from="odom"            to="$(arg odom_topic)"/>
          <remap from="imu"             to="$(arg imu_topic)"/>
      
          <param name="frame_id"                    type="string" value="$(arg frame_id)"/>
          <param name="odom_frame_id"               type="string" value="$(arg vo_frame_id)"/>
          <param name="publish_tf"                  type="bool"   value="$(arg publish_tf_odom)"/>
          <param name="ground_truth_frame_id"       type="string" value="$(arg ground_truth_frame_id)"/>
          <param name="ground_truth_base_frame_id"  type="string" value="$(arg ground_truth_base_frame_id)"/>
          <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
          <param name="wait_imu_to_init"            type="bool"   value="$(arg wait_imu_to_init)"/>
          <param name="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="subscribe_rgbd"              type="bool"   value="$(arg subscribe_rgbd)"/>
          <param name="guess_frame_id"              type="string" value="$(arg odom_guess_frame_id)"/>
          <param name="guess_min_translation"       type="double" value="$(arg odom_guess_min_translation)"/>
          <param name="guess_min_rotation"          type="double" value="$(arg odom_guess_min_rotation)"/>
          <param name="expected_update_rate"        type="double" value="$(arg odom_expected_rate)"/>
          <param name="max_update_rate"             type="double" value="$(arg odom_max_rate)"/>
        </node>

        <!-- Stereo Odometry -->
        <node if="$(arg stereo)" pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
          <remap from="left/image_rect"        to="$(arg left_image_topic_relay)"/>
          <remap from="right/image_rect"       to="$(arg right_image_topic_relay)"/>
          <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
          <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
          <remap from="rgbd_image"             to="$(arg rgbd_topic_relay)"/>
          <remap from="odom"                   to="$(arg odom_topic)"/>
          <remap from="imu"                    to="$(arg imu_topic)"/>
      
          <param name="frame_id"                    type="string" value="$(arg frame_id)"/>
          <param name="odom_frame_id"               type="string" value="$(arg vo_frame_id)"/>
          <param name="publish_tf"                  type="bool"   value="$(arg publish_tf_odom)"/>
          <param name="ground_truth_frame_id"       type="string" value="$(arg ground_truth_frame_id)"/>
          <param name="ground_truth_base_frame_id"  type="string" value="$(arg ground_truth_base_frame_id)"/>
          <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
          <param name="wait_imu_to_init"            type="bool"   value="$(arg wait_imu_to_init)"/>
          <param name="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="subscribe_rgbd"              type="bool"   value="$(arg subscribe_rgbd)"/>
          <param name="guess_frame_id"              type="string" value="$(arg odom_guess_frame_id)"/>
          <param name="guess_min_translation"       type="double" value="$(arg odom_guess_min_translation)"/>
          <param name="guess_min_rotation"          type="double" value="$(arg odom_guess_min_rotation)"/>
          <param name="expected_update_rate"        type="double" value="$(arg odom_expected_rate)"/>
          <param name="max_update_rate"             type="double" value="$(arg odom_max_rate)"/>
        </node>
      </group>
    </group>
    
    <!-- ICP Odometry -->
    <node if="$(arg icp_odometry)" pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="$(arg output)" args="$(arg rtabmap_args) $(arg odom_args)" launch-prefix="$(arg launch_prefix)">
      <remap from="scan"                   to="$(arg scan_topic)"/>
      <remap from="scan_cloud"             to="$(arg scan_cloud_topic)"/>
      <remap from="odom"                   to="$(arg odom_topic)"/>
      <remap from="imu"                    to="$(arg imu_topic)"/>
      
      <param name="frame_id"                    type="string" value="$(arg frame_id)"/>
      <param name="odom_frame_id"               type="string" value="$(arg vo_frame_id)"/>
      <param name="publish_tf"                  type="bool"   value="$(arg publish_tf_odom)"/>
      <param name="ground_truth_frame_id"       type="string" value="$(arg ground_truth_frame_id)"/>
      <param name="ground_truth_base_frame_id"  type="string" value="$(arg ground_truth_base_frame_id)"/>
      <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
      <param name="wait_imu_to_init"            type="bool"   value="$(arg wait_imu_to_init)"/>
      <param name="config_path"                 type="string" value="$(arg cfg)"/>
      <param name="queue_size"                  type="int"    value="$(arg queue_size)"/>
      <param name="guess_frame_id"              type="string" value="$(arg odom_guess_frame_id)"/>
      <param name="guess_min_translation"       type="double" value="$(arg odom_guess_min_translation)"/>
      <param name="guess_min_rotation"          type="double" value="$(arg odom_guess_min_rotation)"/>
      <param name="scan_cloud_max_points"       type="int"    value="$(arg scan_cloud_max_points)"/>
      <param name="expected_update_rate"        type="double" value="$(arg odom_expected_rate)"/>
      <param name="max_update_rate"             type="double" value="$(arg odom_max_rate)"/>
    </node>
    
    <node if="$(arg scan_cloud_assembling)" pkg="rtabmap_ros" type="point_cloud_assembler" name="point_cloud_assembler" output="screen">
      <remap     if="$(arg scan_cloud_filtered)" from="cloud" to="odom_filtered_input_scan"/>
      <remap unless="$(arg scan_cloud_filtered)" from="cloud" to="$(arg scan_cloud_topic)"/>

      <remap from="odom"            to="$(arg odom_topic)"/>
      <param name="assembling_time" type="double" value="$(arg scan_cloud_assembling_time)"/>
      <param name="fixed_frame_id"  type="string" value="$(arg scan_cloud_assembling_fixed_frame)"/>
      <param name="voxel_size"      type="double" value="$(arg scan_cloud_assembling_voxel_size)"/>
    </node>
  
    <!-- Visual SLAM (robot side) -->
    <!-- args: "delete_db_on_start" and "udebug" -->
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="$(arg output)" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
      <param     if="$(arg stereo)" name="subscribe_depth"  type="bool" value="false"/>
      <param unless="$(arg stereo)" name="subscribe_depth"  type="bool" value="$(arg depth)"/>
      <param name="subscribe_rgb"        type="bool"   value="$(arg depth)"/>
      <param name="subscribe_rgbd"       type="bool"   value="$(arg subscribe_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="subscribe_scan_descriptor" type="bool" value="$(arg subscribe_scan_descriptor)"/>
      <param name="subscribe_user_data"  type="bool"   value="$(arg subscribe_user_data)"/>
      <param if="$(arg visual_odometry)" name="subscribe_odom_info" type="bool" value="true"/>
      <param if="$(arg icp_odometry)"    name="subscribe_odom_info" type="bool" value="true"/>
      <param name="frame_id"             type="string" value="$(arg frame_id)"/>
      <param name="map_frame_id"         type="string" value="$(arg map_frame_id)"/>
      <param name="odom_frame_id"        type="string" value="$(arg odom_frame_id)"/>
      <param name="publish_tf"           type="bool"   value="$(arg publish_tf_map)"/>
      <param name="ground_truth_frame_id"       type="string" value="$(arg ground_truth_frame_id)"/>
      <param name="ground_truth_base_frame_id"  type="string" value="$(arg ground_truth_base_frame_id)"/>
      <param name="odom_tf_angular_variance" type="double" value="$(arg odom_tf_angular_variance)"/>
      <param name="odom_tf_linear_variance"  type="double" value="$(arg odom_tf_linear_variance)"/>
      <param name="odom_sensor_sync"         type="bool"   value="$(arg odom_sensor_sync)"/>
      <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="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="scan_cloud_max_points"     type="int" value="$(arg scan_cloud_max_points)"/>
      <param name="landmark_linear_variance"   type="double" value="$(arg tag_linear_variance)"/>
      <param name="landmark_angular_variance"  type="double" value="$(arg tag_angular_variance)"/>      
      
      <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
      <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
      
      <remap from="rgbd_image"      to="$(arg rgbd_topic_relay)"/>
	
      <remap from="left/image_rect"        to="$(arg left_image_topic_relay)"/>
      <remap from="right/image_rect"       to="$(arg right_image_topic_relay)"/>
      <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 if="$(eval scan_cloud_assembling)" from="scan_cloud" to="assembled_cloud"/>
      <remap if="$(eval scan_cloud_filtered and not scan_cloud_assembling)" from="scan_cloud" to="odom_filtered_input_scan"/>
      <remap if="$(eval not scan_cloud_filtered and not scan_cloud_assembling)" from="scan_cloud" to="$(arg scan_cloud_topic)"/>
      <remap from="scan_descriptor"        to="$(arg scan_descriptor_topic)"/>
      <remap from="user_data"              to="$(arg user_data_topic)"/>
      <remap from="user_data_async"        to="$(arg user_data_async_topic)"/>
      <remap from="gps/fix"                to="$(arg gps_topic)"/>
      <remap from="tag_detections"         to="$(arg tag_topic)"/>
      <remap from="odom"                   to="$(arg odom_topic)"/>
      <remap from="imu"                    to="$(arg imu_topic)"/>

  <param name="Grid/MaxObstacleHeight"    type="string" value="2" />
  <param name="Grid/NoiseFilteringRadius" type="string" value="0.0"/>
  <param name="Grid/NoiseFilteringMinNeighbors" type="string" value="5.0"/>

  <param name="Grid/FromDepth"            type="bool" value="true" />
  <!--param name="Grid/DepthDecimation"      type="string" value="4"/-->
  <param name="Grid/3D"                   type="bool" value="true" />
  <param name="Grid/RayTracing"           type="bool" value="true" />
  <param name="Grid/NormalsSegmentation"  type="string" value="true" />
  <param name="Grid/MaxGroundHeight"      type="string" value="0.05" />
  <param name="Grid/MaxGroundAngle"      type="string" value="45" />
  <param name="Grid/RangeMax"      type="string" value="1" />
  <param name="Grid/FootprintHeight" type="string" value="0.1"/>     
  <param name="Grid/FootprintLength" type="string" value="0.1"/>
  <param name="Grid/FootprintWidh" type="string" value="0.1"/>     
<param name="Grid/MapFrameProjection" type="bool" value="false"/>


      
      <!-- 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 $(arg gui_cfg)" output="$(arg output)" launch-prefix="$(arg launch_prefix)">
      <param     if="$(arg stereo)" name="subscribe_depth"  type="bool" value="false"/>
      <param unless="$(arg stereo)" name="subscribe_depth"  type="bool" value="$(arg depth)"/>
      <param name="subscribe_rgbd"       type="bool"   value="$(arg subscribe_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="subscribe_scan_descriptor" type="bool" value="$(arg subscribe_scan_descriptor)"/>
      <param if="$(arg visual_odometry)" name="subscribe_odom_info" type="bool" value="true"/>
      <param if="$(arg icp_odometry)"    name="subscribe_odom_info" type="bool" value="true"/>
      <param name="frame_id"             type="string" value="$(arg frame_id)"/>
      <param name="odom_frame_id"        type="string" value="$(arg odom_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="approx_sync"          type="bool"   value="$(arg approx_sync)"/>
    
      <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
      <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
      
      <remap from="rgbd_image"      to="$(arg rgbd_topic_relay)"/>
      
      <remap from="left/image_rect"        to="$(arg left_image_topic_relay)"/>
      <remap from="right/image_rect"       to="$(arg right_image_topic_relay)"/>
      <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     if="$(arg scan_cloud_filtered)" from="scan_cloud" to="odom_filtered_input_scan"/>
      <remap unless="$(arg scan_cloud_filtered)" from="scan_cloud" to="$(arg scan_cloud_topic)"/>
      <remap from="scan_descriptor"        to="$(arg scan_descriptor_topic)"/>
      <remap from="odom"                   to="$(arg odom_topic)"/>
    </node>
  
  </group>
  
  <!-- Visualization RVIZ -->
  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(arg rviz_cfg)"/>
  <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb" output="$(arg output)">
    <remap if="$(arg stereo)" from="left/image"        to="$(arg left_image_topic_relay)"/>
    <remap if="$(arg stereo)" from="right/image"       to="$(arg right_image_topic_relay)"/>
    <remap if="$(arg stereo)" from="left/camera_info"  to="$(arg left_camera_info_topic)"/>
    <remap if="$(arg stereo)" from="right/camera_info" to="$(arg right_camera_info_topic)"/>
    <remap unless="$(arg subscribe_rgbd)" from="rgb/image"         to="$(arg rgb_topic_relay)"/>
    <remap unless="$(arg subscribe_rgbd)" from="depth/image"       to="$(arg depth_topic_relay)"/>
    <remap unless="$(arg subscribe_rgbd)" from="rgb/camera_info"   to="$(arg camera_info_topic)"/>
    <remap from="rgbd_image"        to="$(arg rgbd_topic_relay)"/>
    <remap from="cloud"             to="voxel_cloud" />

    <param name="decimation"  type="double" value="2"/>
    <param name="voxel_size"  type="double" value="0.0"/>
    <param name="approx_sync" type="bool"   value="$(arg approx_sync)"/>
  </node>
</launch>

navigation_obstacle.launch

<launch>
  <arg name="cmd_vel_topic" default="/cmd_vel" />
  <arg name="move_forward_only" default="false"/>
  <arg name="odom_topic"      default="/odom"/>



  <!-- ROS navigation stack move_base -->
  <group ns="planner">
     <remap from="obstacles_cloud" to="/obstacles_cloud"/>
     <remap from="ground_cloud" to="/ground_cloud"/>
     <remap from="map" to="/rtabmap/grid_map"/>
     <remap from="cmd_vel" to="/cmd_vel"/>
 <remap from="move_base_simple/goal" to="/rtabmap/goal_out"/>
 <!--remap from="odom" to="/t265/odom/sample"/-->
     <node pkg="move_base" type="move_base" respawn="true" name="move_base" output="screen">
        <param name="base_global_planner" value="navfn/NavfnROS"/>
    	<rosparam file="/opt/ros/melodic/share/rtabmap_ros/launch/azimut3/config/costmap_common_params_2d.yaml" command="load" ns="global_costmap"/>
     	<rosparam file="/opt/ros/melodic/share/rtabmap_ros/launch/azimut3/config/costmap_common_params_2d.yaml" command="load" ns="local_costmap" />
    	<rosparam file="/opt/ros/melodic/share/rtabmap_ros/launch/azimut3/config/local_costmap_params.yaml" command="load" ns="local_costmap" />
    	<rosparam file="/opt/ros/melodic/share/rtabmap_ros/launch/azimut3/config/global_costmap_params.yaml" command="load" ns="global_costmap"/>
    	<rosparam file="/opt/ros/melodic/share/rtabmap_ros/launch/azimut3/config/base_local_planner_params.yaml" command="load" />

     </node>
   		
  </group>



  <group ns="camera">
  <!-- Throttling message  
    <node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle realsense2_camera_manager">
      <param name="rate" type="double" value="10"/>
      <param name="decimation" type="int" value="2"/>
   
   <remap from="rgb/image_in"       to="/d400/color/image_raw"/>
      <remap from="depth/image_in"     to="/d400/aligned_depth_to_color/image_raw"/>
      <remap from="rgb/camera_info_in" to="/d400/color/camera_info"/>
 
      <remap from="rgb/image_out"       to="data_resized_image"/>
      <remap from="depth/image_out"     to="data_resized_image_depth"/>
      <remap from="rgb/camera_info_out" to="data_resized_camera_info"/>
    </node>

--> 

<node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager" />
  <node pkg="nodelet" type="nodelet" name="points_xyz_planner" args="load rtabmap_ros/point_cloud_xyz nodelet_manager">
      <remap from="depth/image"            to="/d400/aligned_depth_to_color/image_raw"/>
      <remap from="depth/camera_info"      to="/d400/color/camera_info"/>
      <remap from="cloud"                  to="cloudXYZ" />
      <param name="decimation" type="int" value="4"/>
      <param name="max_depth"  type="double" value="3.0"/>
      <param name="voxel_size" type="double" value="0.05"/>
     <param name="approx_sync" type="bool" value="false"/>
  </node>





 
    <node pkg="nodelet" type="nodelet" name="obstacles_detection" args="load rtabmap_ros/obstacles_detection nodelet_manager">
     <remap from="cloud" to="cloudXYZ"/>
      <remap from="obstacles" to="/obstacles_cloud"/>
      <remap from="ground"    to="/ground_cloud"/>

      <param name="frame_id"             type="string" value="base_link"/>
      <param name="map_frame_id"         type="string" value="map"/>
      <param name="wait_for_transform"   type="bool" value="true"/>
  <!--param name="Grid/ClusterRadius"             type="string" value="0.2"/-->
  <!--param name="Grid/GroundIsObstacle"          type="string" value="false"/-->
    </node>  
  </group>
</launch>

Thanks
gian

[matlabbe]

Where is the Grid/MaxGroundHeight set in obstacles_detection?

<node pkg="nodelet" type="nodelet" name="obstacles_detection" args="load rtabmap_ros/obstacles_detection nodelet_manager">
     <remap from="cloud" to="cloudXYZ"/>
      <remap from="obstacles" to="/obstacles_cloud"/>
      <remap from="ground"    to="/ground_cloud"/>

      <param name="frame_id"             type="string" value="base_link"/>
      <param name="map_frame_id"         type="string" value="map"/>
      <param name="wait_for_transform"   type="bool" value="true"/>
  <!--param name="Grid/ClusterRadius"             type="string" value="0.2"/-->
  <!--param name="Grid/GroundIsObstacle"          type="string" value="false"/-->
    </node>  

[gian]

It is in the rtabmap.launch , set to 0.05

[matlabbe]

I saw one under rtabmap node, but you should also set it in obstacles_detection nodelet.

[gian]

That was the problem now it works ! Mathieu  I'm very sorry  to make you waste this time for a misconfiguration !
So finally each Grid parameter associated to the obstacles should be written only in the obstacle nodelet or also in the rtabmap.launch ?
Thanks a lot !

[matlabbe]

Hi,

The Grid parameters in rtabmap node will affect how the occupancy grid is created, while Grid parameters in obstacles_detection will affect the ground and obstacle clouds of the latest frame from the camera.

The parameter names are the same because the same algorithm is used under the hood for both nodes. All Grid parameters can be used in obstacles_detection node.

cheers,
Mathieu

[gian]

Thank you Mathieu , this has been a great lesson. About the four realsense camera that you mentioned in this thread

in the context of mapping from a drone , indoor or outdoor , which of the four option would you use ?

Are there different rtabmap approach for pointing down cameras ?

What would be the advantage of adding Vins Fusion or ORB to rtabmap ? An odometry more  resilient to drifts ?

EDIT  in particular I am confused between approach 2 and 3

2) T265 (F2M odometry + fisheye 3D reconstruction, images are rectified at 30 Hz for odometry):

3) T265 (VIO odometry + fisheye 3D reconstruction, images are rectified only at 1 hz for 3d map):

is that  odometry on 3) rely on /t265/odom/sample and  on 2) rely on the default VO f2m odometry  of rtabmap ?
Should we throttle the topic of the rectification ?

Thanks for the great support
regards
gian

[gian]

Hello Mathieu

Please for your help ,I am really stuck on this topics I need your guidance to understand how to go ahead.
Thanks
Regards
gian

[matlabbe]

gian wrote

in the context of mapping from a drone , indoor or outdoor , which of the four option would you use ?

From a computation cost point of view:
Indoor T265+D435
Outdoor T265 could be used alone, but D435 still gives "free" disparity

gian wrote

Are there different rtabmap approach for pointing down cameras ?

not really, just make sure that there are enough visual features to track

gian wrote

What would be the advantage of adding Vins Fusion or ORB to rtabmap ? An odometry more  resilient to drifts ?

VIO approaches may be more robust to fast rotations, at least they may drift, but not get lost.

gian wrote

is that  odometry on 3) rely on /t265/odom/sample and  on 2) rely on the default VO f2m odometry  of rtabmap ?

yes

gian wrote

Should we throttle the topic of the rectification ?

not sure to which setup you are referring