search

introlab / rtabmap

RTAB-Map library and standalone application
https://introlab.github.io/rtabmap
Other
2.61k stars 763 forks source link

Use RTAB-Map + OpenVINS with ZED2 Problem #1166

Open ericlai0323 opened 7 months ago

ericlai0323 commented 7 months ago

I compiled OpenVINS and RTAB-Map from source, but I get warnings when running them saying my TF tree connection is not normal. Can someone please help me check what the issue might be?

I think the problem is that the visual odometry is not published correctly, but I can't find where the error is.

Thank you very much!!!

Below is the warning message:

[ INFO] [1700626779.170679455]: Odom: quality=0, std dev=0.000000m|0.000000rad, update time=0.003912s
[ INFO] [1700626779.323585268]: Odom: quality=0, std dev=0.000000m|0.000000rad, update time=0.000348s
[ INFO] [1700626779.366120078]: Odom: quality=0, std dev=0.000000m|0.000000rad, update time=0.000252s
[ INFO] [1700626779.430471599]: Odom: quality=0, std dev=0.000000m|0.000000rad, update time=0.000240s
[ INFO] [1700626779.494406700]: Odom: quality=0, std dev=0.000000m|0.000000rad, update time=0.000248s
[ INFO] [1700626779.561270333]: Odom: quality=0, std dev=0.000000m|0.000000rad, update time=0.000228s
[ INFO] [1700626779.629693937]: Odom: quality=0, std dev=0.000000m|0.000000rad, update time=0.000182s
[ WARN] [1700626779.678284194]: Could not get transform from odom to base_link after 0.500000 seconds (for stamp=1700626779.151464)! Error="Could not find a connection between 'odom' and 'base_link' because they are not part of the same tree.Tf has two or more unconnected trees.. canTransform returned after 0.503433 timeout was 0.5.".
[ INFO] [1700626779.700378208]: Odom: quality=0, std dev=0.000000m|0.000000rad, update time=0.000215s
[ INFO] [1700626779.765972340]: Odom: quality=0, std dev=0.000000m|0.000000rad, update time=0.000284s
[ INFO] [1700626779.831533127]: Odom: quality=0, std dev=0.000000m|0.000000rad, update time=0.000216s
[ INFO] [1700626779.897267018]: Odom: quality=0, std dev=0.000000m|0.000000rad, update time=0.000175s
[ INFO] [1700626779.962457252]: Odom: quality=0, std dev=0.000000m|0.000000rad, update time=0.000217s
[ INFO] [1700626780.031689608]: Odom: quality=0, std dev=0.000000m|0.000000rad, update time=0.000242s
[ INFO] [1700626780.096818767]: Odom: quality=0, std dev=0.000000m|0.000000rad, update time=0.000404s
[ INFO] [1700626780.162721815]: Odom: quality=0, std dev=0.000000m|0.000000rad, update time=0.000266s
[ WARN] [1700626780.184400238]: Could not get transform from odom to base_link after 0.500000 seconds (for stamp=1700626779.312418)! Error="Could not find a connection between 'odom' and 'base_link' because they are not part of the same tree.Tf has two or more unconnected trees.. canTransform returned after 0.502625 timeout was 0.5.".
[ INFO] [1700626780.229442245]: Odom: quality=0, std dev=0.000000m|0.000000rad, update time=0.000233s
[ INFO] [1700626780.296072302]: Odom: quality=0, std dev=0.000000m|0.000000rad, update time=0.000198s
[ INFO] [1700626780.366343862]: Odom: quality=0, std dev=0.000000m|0.000000rad, update time=0.000196s
[ INFO] [1700626780.434357990]: Odom: quality=0, std dev=0.000000m|0.000000rad, update time=0.000229s
[ INFO] [1700626780.498312811]: Odom: quality=0, std dev=0.000000m|0.000000rad, update time=0.000218s
[ INFO] [1700626780.564963036]: Odom: quality=0, std dev=0.000000m|0.000000rad, update time=0.000239s
[ INFO] [1700626780.630942210]: Odom: quality=0, std dev=0.000000m|0.000000rad, update time=0.000199s
[ WARN] [1700626780.689319134]: Could not get transform from odom to base_link after 0.500000 seconds (for stamp=1700626779.358052)! Error="Could not find a connection between 'odom' and 'base_link' because they are not part of the same tree.Tf has two or more unconnected trees.. canTransform returned after 0.503172 timeout was 0.5.".

Below is launch file:

<?xml version="1.0"?>

<launch>

  <!-- <node pkg="hector_trajectory_server" type="hector_trajectory_server" name="hector_trajectory_server" output="screen">
    <param name="target_frame_name" type="string" value="map" />
    <param name="source_frame_name" type="string" value="base_link" />
    <param name="trajectory_update_rate" type="double" value="4" />
    <param name="trajectory_publish_rate" type="double" value="0.25" />
  </node> -->

  <!-- Choose between depth and stereo, set both to false to do only scan -->
  <arg name="stereo"                    default="true"/>
  <arg     if="$(arg stereo)" name="depth"  default="false"/>
  <arg unless="$(arg stereo)" name="depth"  default="true"/>
  <arg name="subscribe_rgb"  default="$(arg depth)"/>

  <!-- Choose visualization -->
  <arg name="rtabmap_viz"             default="false" />
  <arg name="rviz"                    default="true" />

  <!-- Localization-only mode -->
  <arg name="localization"            default="false"/>
  <arg name="initial_pose"            default=""/>             <!-- Format: "x y z roll pitch yaw" or "x y z qx qy qz qw". Default: see "RGBD/StartAtOrigin" doc -->

  <!-- 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="/home/ericlai/Launch/config/zed2_stereo.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="odom_frame_id_init"      default=""/>                <!-- If set, TF map->odom is published even if no odometry topic has been received yet. The frame id should match the one in 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="~/visual_mapfile/TEST_20231119.db"/>
  <arg name="queue_size"              default="100"/>
  <arg name="wait_for_transform"      default="0.5"/>
  <arg name="args"                    default=""/>
  <arg name="rtabmap_args"            default="$(arg args)"/>   <!-- deprecated, use "args" argument -->
  <arg name="gdb"                     default="false"/>         <!-- Launch nodes in gdb for debugging (apt install xterm gdb) -->
  <arg     if="$(arg gdb)" name="launch_prefix" default="xterm -e gdb -q -ex run --args"/>
  <arg unless="$(arg gdb)" name="launch_prefix" default=""/>
  <arg name="clear_params"            default="true"/>
  <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)"/>
  <arg name="approx_sync_max_interval"  default="0"/> <!-- (sec) 0 means infinite interval duration (used with approx_sync=true) -->

  <!-- RGB-D related topics -->
  <arg name="rgb_topic"               default="/zed/zed_node/rgb/image_rect_color" />
  <arg name="depth_topic"             default="/zed/zed_node/depth/depth_registered" />
  <arg name="camera_info_topic"       default="/zed/zed_node/rgb/camera_info" />
  <arg name="depth_camera_info_topic" default="/zed/zed_node/depth/camera_info" />

  <!-- stereo related topics -->
  <arg name="stereo_namespace"        default="/zed/zed_node/"/>
  <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_gray" />      <!-- 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_sync/rgbd_sync nodelet -->
  <arg name="rgbd_sync"               default="false"/>         <!-- pre-sync rgb_topic, depth_topic, camera_info_topic -->
  <arg name="approx_rgbd_sync"        default="false"/>          <!-- false=exact synchronization -->
  <arg name="subscribe_rgbd"          default="$(arg rgbd_sync)"/>
  <arg name="rgbd_cameras"            default="1"/>
  <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="gen_cloud"               default="false"/> <!-- only works with depth image and if not subscribing to scan_cloud topic-->
  <arg name="gen_cloud_decimation"    default="4"/>
  <arg name="gen_cloud_voxel"         default="0.05"/>

  <arg name="subscribe_scan"          default="false"/>
  <arg name="scan_topic"              default="/scan"/>
  <arg name="subscribe_scan_cloud"    default="$(arg gen_cloud)"/>
  <arg name="scan_cloud_topic"        default="/depth/image_rect_raw"/>
  <arg name="subscribe_scan_descriptor" default="false"/>
  <arg name="scan_descriptor_topic"   default="/scan_descriptor"/>
  <arg name="scan_deskewing"          default="false"/>
  <arg name="scan_deskewing_slerp"    default="false"/>
  <arg name="scan_cloud_max_points"   default="0"/>
  <arg name="scan_cloud_filtered"     default="$(arg scan_deskewing)"/> <!-- use filtered cloud from icp_odometry for mapping -->
  <arg name="gen_scan"                default="true"/> <!-- only works with depth image and if not subscribing to scan topic-->

  <arg name="gen_depth"                  default="false" /> <!-- Generate depth image from scan_cloud -->
  <arg name="gen_depth_decimation"       default="1" />
  <arg name="gen_depth_fill_holes_size"  default="0" />
  <arg name="gen_depth_fill_iterations"  default="1" />
  <arg name="gen_depth_fill_holes_error" default="0.1" />

  <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="0.001"/>         <!-- If TF is used to get odometry, this is the default angular variance -->
  <arg name="odom_tf_linear_variance"  default="0.001"/>         <!-- 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="/zed/zed_node/imu/data"/>          
  <!-- <arg name="imu_topic"                default=""/>           -->
  <arg name="wait_imu_to_init"         default="false"/>
  <arg name="use_odom_features"        default="false"/>

  <arg name="scan_cloud_assembling"              default="false"/>
  <arg name="scan_cloud_assembling_time"         default="1"/>      <!-- max_clouds and time should not be set at the same time -->
  <arg name="scan_cloud_assembling_max_clouds"   default="0"/>      <!-- max_clouds and time should not be set at the same time -->
  <arg name="scan_cloud_assembling_fixed_frame"  default=""/>
  <arg name="scan_cloud_assembling_voxel_size"   default="0.05"/>
  <arg name="scan_cloud_assembling_range_min"    default="0.0"/>    <!-- 0=disabled -->
  <arg name="scan_cloud_assembling_range_max"    default="0.0"/>    <!-- 0=disabled -->
  <arg name="scan_cloud_assembling_noise_radius"   default="0.0"/>    <!-- 0=disabled -->
  <arg name="scan_cloud_assembling_noise_min_neighbors"   default="5"/>

  <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 -->
  <arg name="fiducial_topic"           default="/fiducial_transforms" />  <!-- aruco_detect async subscription, use tag_linear_variance and tag_angular_variance to set covriance -->

  <!-- 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)" />

        <group ns="camera">
          <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_sync/rgbd_sync" clear_params="$(arg clear_params)" output="$(arg output)">
            <remap from="rgb/image"       to="/zed/$(arg rgb_topic_relay)"/>
            <remap from="depth/image"     to="/zed/$(arg depth_topic_relay)"/>
            <remap from="rgb/camera_info" to="/zed/$(arg camera_info_topic)"/>
            <remap from="rgbd_image"      to="/zed/$(arg rgbd_topic_relay)"/>
            <param name="approx_sync"     type="bool"   value="$(arg approx_rgbd_sync)"/>
            <param name="approx_sync_max_interval" type="double" value="$(arg approx_sync_max_interval)"/>
            <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>
    <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="stereo_sync" args="standalone rtabmap_sync/stereo_sync" clear_params="$(arg clear_params)" 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="approx_sync_max_interval" type="double" value="$(arg approx_sync_max_interval)"/>
          <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_util" clear_params="$(arg clear_params)">
           <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>

    <node if="$(arg gen_cloud)" pkg="nodelet" type="nodelet" name="gen_cloud_from_depth" args="standalone rtabmap_util/point_cloud_xyz" clear_params="$(arg clear_params)" output="$(arg output)">
      <remap from="depth/image"       to="$(arg depth_topic_relay)"/>
      <remap from="depth/camera_info" to="$(arg camera_info_topic)"/>
      <remap from="cloud"             to="$(arg scan_cloud_topic)" />

      <param name="decimation"  type="double" value="$(arg gen_cloud_decimation)"/>
      <param name="voxel_size"  type="double" value="$(arg gen_cloud_voxel)"/>
      <param name="approx_sync" type="bool"   value="false"/>
      <param name="approx_sync_max_interval" type="double" value="$(arg approx_sync_max_interval)"/>
    </node>

    <!-- Visual odometry -->
    <group unless="$(arg icp_odometry)">
      <group if="$(arg visual_odometry)">

        <!-- RGB-D Odometry -->
        <node unless="$(arg stereo)" pkg="rtabmap_odom" type="rgbd_odometry" name="rgbd_odometry" clear_params="$(arg clear_params)" 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="approx_sync_max_interval"    type="double" value="$(arg approx_sync_max_interval)"/>
          <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)"/>
          <param name="keep_color"                  type="bool"   value="$(arg use_odom_features)"/>
          <!-- Set to use ORB_SLAM -->
          <param name="Odom/Strategy" value="5"/>
          <param name="OdomORBSLAM/VocPath" value="/home/ericlai/visual_ws/src/ORB_SLAM3/Vocabulary/ORBvoc.txt"/>
        </node>

        <!-- Stereo Odometry -->
        <node if="$(arg stereo)" pkg="rtabmap_odom" type="stereo_odometry" name="stereo_odometry" clear_params="$(arg clear_params)" 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="approx_sync_max_interval"    type="double" value="$(arg approx_sync_max_interval)"/>
          <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)"/>
          <param name="keep_color"                  type="bool"   value="$(arg use_odom_features)"/>

          <param name="Odom/Strategy" value="10"/>
        </node>
      </group>
    </group>

    <!-- ICP Odometry -->
    <node if="$(arg icp_odometry)" pkg="rtabmap_odom" type="icp_odometry" name="icp_odometry" clear_params="$(arg clear_params)" 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="$(falsearg 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)"/>
      <param name="deskewing"                   type="bool"   value="$(arg scan_deskewing)"/>
      <param name="deskewing_slerp"             type="bool"   value="$(arg scan_deskewing_slerp)"/>
    </node>

    <node if="$(eval not icp_odometry and scan_deskewing and subscribe_scan_cloud)" pkg="rtabmap_util" type="lidar_deskewing" name="lidar_deskewing" clear_params="$(arg clear_params)" output="$(arg output)">
      <param name="wait_for_transform" value="$(arg wait_for_transform)"/>
      <param     if="$(arg visual_odometry)" name="fixed_frame_id" value="$(arg vo_frame_id)"/>
      <param unless="$(arg visual_odometry)" name="fixed_frame_id" value="$(arg odom_frame_id)"/>
      <param name="slerp" value="$(arg scan_deskewing_slerp)"/>
      <remap from="input_cloud" to="$(arg scan_cloud_topic)"/>
      <remap from="$(arg scan_cloud_topic)/deskewed" to="odom_filtered_input_scan"/>
    </node>

    <node if="$(arg scan_cloud_assembling)" pkg="rtabmap_util" type="point_cloud_assembler" name="point_cloud_assembler" clear_params="$(arg clear_params)" output="$(arg output)">
      <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="max_clouds"      type="int"    value="$(arg scan_cloud_assembling_max_clouds)"/>
      <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)"/>
      <param name="range_min"       type="double" value="$(arg scan_cloud_assembling_range_min)"/>
      <param name="range_max"       type="double" value="$(arg scan_cloud_assembling_range_max)"/>
      <param name="noise_radius"        type="double" value="$(arg scan_cloud_assembling_noise_radius)"/>
      <param name="noise_min_neighbors" type="int"    value="$(arg scan_cloud_assembling_noise_min_neighbors)"/>
      <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
    </node>

    <!-- Visual SLAM (robot side) -->
    <!-- args: "delete_db_on_start" and "udebug" -->
    <node name="rtabmap" pkg="rtabmap_slam" type="rtabmap" clear_params="$(arg clear_params)" 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 subscribe_rgb)"/>
      <param name="subscribe_rgbd"       type="bool"   value="$(eval subscribe_rgbd or use_odom_features)"/>
      <param name="rgbd_cameras"         type="int"    value="$(arg rgbd_cameras)"/>
      <remap from="rgbd_image0"       to="/left_camera/rgbd_image"/>
      <remap from="rgbd_image1"       to="/right_camera/rgbd_image"/>
      <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="odom_frame_id_init"   type="string" value="$(arg odom_frame_id_init)"/>
      <param name="publish_tf"           type="bool"   value="$(arg publish_tf_map)"/>
      <param name="initial_pose"         type="string" value="$(arg initial_pose)"/>
      <param name="gen_scan"             type="bool"   value="$(arg gen_scan)"/>
      <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="$(eval approx_sync and not use_odom_features)"/>
      <param name="config_path"          type="string" value="$(arg cfg)"/>
      <param name="queue_size"           type="int" value="$(arg queue_size)"/>
      <param if="$(eval not scan_cloud_filtered and not scan_cloud_assembling)" 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)"/>
      <param name="gen_depth"                  type="bool"   value="$(arg gen_depth)" />
      <param name="gen_depth_decimation"       type="int"    value="$(arg gen_depth_decimation)" />
      <param name="gen_depth_fill_holes_size"  type="int"    value="$(arg gen_depth_fill_holes_size)" />
      <param name="gen_depth_fill_iterations"  type="int"    value="$(arg gen_depth_fill_iterations)" />
      <param name="gen_depth_fill_holes_error" type="double" value="$(arg gen_depth_fill_holes_error)" />

      <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     if="$(arg use_odom_features)" from="rgbd_image" to="odom_rgbd_image"/>
      <remap unless="$(arg use_odom_features)" 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="fiducial_transforms"    to="$(arg fiducial_topic)"/>
      <remap from="odom"                   to="$(arg odom_topic)"/>
      <remap from="imu"                    to="$(arg imu_topic)"/>

      <!-- 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)"/>

      <!-- RTAB-Map Output -->
      <!--remap from="grid_map" to="/projected_map"/-->
      <remap from="grid_map" to="/map"/>

      <param name="Grid/3D"                            type="bool" value="true"/> 
      <param name="Grid/Sensor"                        type="string" value="0"/>
      <param name="Grid/MaxObstacleHeight"             type="double" value="3.5"/>

      <param name="Grid/MaxGroundHeight"               type="double" value="3.0"/>
      <param name="Grid/MinGroundHeight"               type="double" value="2.0"/>

      <param name="Grid/RangeMax"                      type="double" value="5.0"/>
      <param name="Grid/RangeMin"                      type="double" value="0.3"/>
      <param name="Grid/FootprintHeight"               type="string" value="3.0"/>  <!--"Footprint height used to filter points over the footprint of the robot. Footprint length and width should be set."-->
      <param name="Grid/FootprintLength"               type="string" value="0.5"/>  <!--"Footprint length used to filter points over the footprint of the robot."-->
      <param name="Grid/FootprintWidth"                type="string" value="0.5"/>  <!--"Footprint width used to filter points over the footprint of the robot. Footprint length should be set."-->

      <param name="GridGlobal/FootprintRadius"         type="string" value="0.01"/>
      <param name="RGBD/ProximityPathMaxNeighbors"     type="string" value="0"/>
      <param name="RGBD/NeighborLinkRefining"          type="string" value="true"/>
      <param name="RGBD/ProximityBySpace"              type="string" value="true"/>
      <param name="RGBD/AngularUpdate"                 type="string" value="0.01"/>
      <param name="RGBD/LinearUpdate"                  type="string" value="0.01"/> <!--"Minimum linear displacement (m) to update the map. Rehearsal is done prior to this, so weights are still updated."-->

      <param name="RGBD/CreateOccupancyGrid"           type="string" value="true"/>

      <!-- Vis/EstimationType NEED TO BE 0 -->
      <param name="Vis/EstimationType"                 type="int" value="0"/> 

      <!-- Rate (Hz) at which new nodes are added to map -->
      <param name="Rtabmap/DetectionRate" type="string" value="1"/> 

      <!-- 2D SLAM -->
      <param name="Reg/Force3DoF" type="bool" value="true"/>
      <!-- <param name="Optimizer/Slam2D" value="true" />  -->

      <!--param name="cloud_noise_filtering_radius" value="0.05"/>
      <param name="cloud_noise_filtering_min_neighbors" value="2"/--> 

      <!-- Loop Closure Constraint -->
      <!-- 0=Visual, 1=ICP (1 requires scan)-->
      <param name="Reg/Strategy" type="string" value="0"/>

      <!-- Loop Closure Detection -->
      <!-- 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE-->
      <param name="Kp/DetectorStrategy"      type="int" value="4"/><!--0-->
      <param name="Vis/FeatureType"          type="int" value="4"/>

      <!-- Maximum visual words per image (bag-of-words) -->
      <param name="Kp/MaxFeatures" type="double" value="1500"/>  

      <!-- Used to extract more or less SURF features -->
      <!--param name="SURF/HessianThreshold" type="string" value="100"/-->
      <!-- Minimum visual inliers to accept loop closure -->
      <param name="Vis/MinInliers" type="double" value="10"/> 
      <!-- Set to false to avoid saving data when robot is not moving -->
      <param name="Mem/NotLinkedNodesKept" type="bool" value="false"/>
      <param     if="$(arg localization)" name="Mem/IncrementalMemory" type="bool" value="false"/>
      <param unless="$(arg localization)" name="Mem/IncrementalMemory" type="bool" value="true"/>
      <param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>

    </node>

    <!-- Visualisation RTAB-Map -->
    <node if="$(arg rtabmap_viz)" pkg="rtabmap_viz" type="rtabmap_viz" name="rtabmap_viz" args="-d $(arg gui_cfg)" clear_params="$(arg clear_params)" 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_rgb"        type="bool"   value="$(arg subscribe_rgb)"/>
      <param name="subscribe_rgbd"       type="bool"   value="$(eval subscribe_rgbd or use_odom_features)"/>
      <param name="subscribe_stereo"     type="bool"   value="$(arg stereo)"/>
      <param unless="$(arg icp_odometry)" name="subscribe_scan"       type="bool"   value="$(arg subscribe_scan)"/>
      <param     if="$(arg icp_odometry)" name="subscribe_scan_cloud" type="bool"   value="$(eval subscribe_scan or subscribe_scan_cloud)"/>
      <param unless="$(arg icp_odometry)" 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="$(eval approx_sync and not use_odom_features)"/>

      <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="/camera/rgbd_image"/>

      <remap     if="$(arg use_odom_features)" from="rgbd_image" to="odom_rgbd_image"/>
      <remap unless="$(arg use_odom_features)" from="rgbd_image" to="$(arg rgbd_topic_relay)"/>

      <remap unless="$(arg icp_odometry)" from="scan" to="$(arg scan_topic)"/>
      <remap     if="$(eval icp_odometry or scan_cloud_filtered)" from="scan_cloud" to="odom_filtered_input_scan"/>
      <remap unless="$(eval icp_odometry or 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_util/point_cloud_xyzrgb" clear_params="$(arg clear_params)" 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="4"/>
    <param name="voxel_size"  type="double" value="0.0"/>
    <param name="approx_sync" type="bool"   value="$(arg approx_sync)"/>
    <param name="approx_sync_max_interval" type="double" value="$(arg approx_sync_max_interval)"/>
  </node>

</launch>

Below is my TF tree: RTABMap OpenVINS_TFTree

Below is RTAB-Map dependency version:

$ rtabmap --version
RTAB-Map:               0.21.2
PCL:                    1.10.0
With VTK:                7.1.1
OpenCV:                  4.2.0
With OpenCV xfeatures2d: false
With OpenCV nonfree:     false
With ORB OcTree:          true
With SuperPoint Torch:   false
With Python3:            false
With FastCV:             false
With OpenGV:              true
With Madgwick:            true
With PDAL:               false
With TORO:                true
With g2o:                 true
With GTSAM:               true
With Vertigo:             true
With CVSBA:              false
With Ceres:              false
With OpenNI2:             true
With Freenect:            true
With Freenect2:          false
With K4W2:               false
With K4A:                false
With DC1394:              true
With FlyCapture2:        false
With ZED:                false
With ZED Open Capture:   false
With RealSense:          false
With RealSense SLAM:     false
With RealSense2:          true
With MYNT EYE S:         false
With DepthAI:            false
With libpointmatcher:     true
With CCCoreLib:          false
With octomap:             true
With cpu-tsdf:           false
With open chisel:        false
With Alice Vision:       false
With LOAM:               false
With FLOAM:              false
With FOVIS:              false
With Viso2:              false
With DVO:                false
With ORB_SLAM:           false
With OKVIS:              false
With MSCKF_VIO:          false
With VINS-Fusion:        false
With OpenVINS:            true
borongyuan commented 7 months ago

It seems there are some configuration issues. OpenVINS has not been called. Because there are only warning messages from RTAB-Map, but not from OpenVINS.

matlabbe commented 6 months ago

I'll suggest to launch stereo_odometry node with --udebug argument.

<arg name="odom_args" default="--udebug"/>