RTAB-Map doesn't create map

asked 2021-09-15 04:53:29 -0500

Hi, I have setup like this : robot with 3D Lidar (Livox Mid 70) and Realsense D435 camera. I want to use RTAB-Map to build map of indoor environment. First I wanted to do combine data from lidar and camera but I can't get it to generate the map, without this static_transform_publisher, there no map frame in tf tree and when I try to run rostpic echo /mapData for example, nothing is being published. What am I doing wrong? Here is my launch file and rviz visualisation of that.

<!-- -->
<launch>
  <param name="robot_description" textfile="$(find bbot_description)/urdf/bbot.urdf" />

  <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
        <param name="publish_frequency" type="double" value="10.0" />
          <param name="rate" value="10.0"/>
  </node>

  <node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher">
        <param name="use_gui" value="false"/>
        <param name="rate" value="10.0"/>
  </node>
  <node pkg="tf" type="static_transform_publisher" name="odom2map" args="0 0 0 0 0 0 map odom 10" />
  <arg name="depth"  default="true"/>
  <arg name="rviz"                    default="true" />
  <arg name="localization"            default="false"/>

  <param name="use_sim_time" type="bool" value="true"/>
  <arg name="cfg"                     default="" />
  <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"/>
  <arg name="odom_frame_id"           default="odom"/>
  <arg name="map_frame_id"            default="map"/>
  <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="output"                  default="screen"/>
  <arg name="publish_tf_map"          default="true"/>
  <arg name="approx_sync"             default="false"/>  

  <!-- RGB-D related topics -->
  <arg name="rgb_topic"               default="/realsense_left/color/image_raw" />
  <arg name="depth_topic"             default="/realsense_left/aligned_depth_to_color/image_raw" />
  <arg name="camera_info_topic"       default="/realsense_left/color/camera_info" />
  <arg name="depth_camera_info_topic" default="/realsense_left/aligned_depth_to_color/camera_info" />

  <arg name="subscribe_rgb"  default="false"/>
  <arg name="subscribe_depth" default="false"/>

  <!-- Already synchronized RGB-D related topic, with rtabmap_ros/rgbd_sync nodelet -->
  <arg name="rgbd_sync"               default="true"/>
  <arg name="approx_rgbd_sync"        default="false"/>
  <arg name="subscribe_rgbd"          default="true"/>
  <arg name="rgbd_topic"              default="rgbd_image" />
  <arg name="depth_scale"             default="1.0" />
  <arg name="rgbd_depth_scale"        default="$(arg depth_scale)" />
  <arg name="rgbd_decimation"         default="1" />

  <arg name="compressed"              default="false"/>
  <arg name="rgb_image_transport"     default="compressed"/>
  <arg name="depth_image_transport"   default="compressedDepth"/>

  <arg name="subscribe_scan"          default="false"/>
  <arg name="scan_topic"              default="/scan"/>
  <arg name="subscribe_scan_cloud"    default="true"/>
  <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"/> 

  <arg name="visual_odometry"          default="true"/>
  <arg name="icp_odometry"             default="false"/>
  <arg name="odom_topic"               default="odom"/>
  <arg name="vo_frame_id"              default="$(arg odom_topic)"/>
  <arg name="publish_tf_odom"          default="true"/>
  <arg name="odom_tf_angular_variance" default="0.001"/>
  <arg name="odom_tf_linear_variance"  default="0.001"/>
  <arg name="odom_args ...
(more)
edit retag flag offensive close merge delete

Comments

Are there warnings in terminal? If /map frame is not published, it means rtabmap is not able to synchronize input data. approx_sync should be true in your case.

matlabbe gravatar image matlabbe  ( 2021-09-15 12:45:08 -0500 )edit

If visual_odomerty is true and icp_odometry is false I get these warnings.

[ WARN] [1631789854.769064870, 1631259816.992558547]: /rtabmap/rgbd_odometry: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. 
/rtabmap/rgbd_odometry subscribed to:
   /rtabmap/rgbd_image
[ WARN] [1631789855.109552608, 1631259817.333685043]: /rtabmap/rtabmap: Did not receive data since 5 seconds! Make sure the input topics are published ("$ rostopic hz my_topic") and the timestamps in their header are set. If topics are coming from different computers, make sure the clocks of the computers are synchronized ("ntpdate"). If topics are not published at the same rate, you could increase "queue_size" parameter (current=100).
/rtabmap/rtabmap subscribed to (approx sync):
   /rtabmap/odom \
   /rtabmap/rgbd_image \
   /rtabmap/scan_cloud
slavenp gravatar image slavenp  ( 2021-09-16 06:00:23 -0500 )edit

If visual_odometry is set to false, and icp_odometry to true, I get this :

[ WARN] [1631790069.474215242, 1631259811.731871153]: IcpOdometry: Transferring value 0.05 of "Icp/VoxelSize" to ros parameter "scan_voxel_size" for convenience. "Icp/VoxelSize" is set to 0.
[ WARN] [1631790069.474696981, 1631259811.731871153]: IcpOdometry: Transferring value 5 of "Icp/PointToPlaneK" to ros parameter "scan_normal_k" for convenience.
[ WARN] [1631790069.475532369, 1631259811.741987441]: IcpOdometry: Transferring value 1.0 of "Icp/PointToPlaneRadius" to ros parameter "scan_normal_radius" for convenience.
[ WARN] (2021-09-16 13:01:09.476) OdometryF2M.cpp:163::OdometryF2M() OdomF2M/BundleAdjustment=1 cannot be used with registration not done only with images (Reg/Strategy=1), disabling bundle adjustment.

Which one should I use and which params to set if i want to for example use camera and lidar for depth. I also have wheel odometry topic in that .bag file if I can use it.

slavenp gravatar image slavenp  ( 2021-09-16 06:03:21 -0500 )edit

To simplify, start with wheel odometry. Set visual_odometry and icp_odometry to false, and set odom_topic argument to your wheel odometry topic.

For info:

  • use visual_odometry if you want to compute visual odometry from D435 (as rgbd_sync is enabled, the warning tells that rgbd_image is not published, verify inputs of rgbd_sync node: rostopic hz /realsense_left/color/image_raw /realsense_left/aligned_depth_to_color/image_raw /realsense_left/color/camera_info).
  • use icp_odometry to use LiDAR for odometry, however the ICP approach used doesn't work with Livox kind of point cloud
matlabbe gravatar image matlabbe  ( 2021-09-16 12:18:49 -0500 )edit