search

introlab / rtabmap_ros

RTAB-Map's ROS package.
http://wiki.ros.org/rtabmap_ros
BSD 3-Clause "New" or "Revised" License
975 stars 557 forks source link

Does rtabmap works on different image type and size? #904

Closed julianraheema closed 1 year ago

julianraheema commented 1 year ago

System setup: Ubuntu 20.04 Noetic Camera similar to Realsense D415/D435

I have a blackbox camera that is producing these topics below: /spot/camera/back/image topic: header: seq: 73 stamp: secs: 1677524361 nsecs: 548337425 frame_id: "back_fisheye" height: 480 width: 640 encoding: "mono8" is_bigendian: 1

/spot/camera/back/camera_info topic: header: seq: 113 stamp: secs: 1677524408 nsecs: 767619322 frame_id: "back_fisheye" height: 480 width: 640 distortion_model: "plumb_bob" D: [0.0, 0.0, 0.0, 0.0, 0.0] K: [330.94464111328125, 0.0, 316.32916259765625, 0.0, 329.81549072265625, 249.16798400878906, 0.0, 0.0, 1.0] R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] P: [330.94464111328125, 0.0, 316.32916259765625, 0.0, 0.0, 329.81549072265625, 249.16798400878906, 0.0, 0.0, 0.0, 1.0, 0.0] binning_x: 0 binning_y: 0 roi: x_offset: 0 y_offset: 0 height: 0 width: 0 do_rectify: False

/spot/depth/back/image topic: header: seq: 85 stamp: secs: 1677524365 nsecs: 215576725 frame_id: "back" height: 240 width: 424 encoding: "16UC1" is_bigendian: 0 step: 848

/spot/depth/back/camera_info topic header: seq: 38 stamp: secs: 1677524317 nsecs: 934621403 frame_id: "back" height: 240 width: 424 distortion_model: "plumb_bob" D: [0.0, 0.0, 0.0, 0.0, 0.0] K: [211.1915283203125, 0.0, 215.72769165039062, 0.0, 211.1915283203125, 118.88978576660156, 0.0, 0.0, 1.0] R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] P: [211.1915283203125, 0.0, 215.72769165039062, 0.0, 0.0, 211.1915283203125, 118.88978576660156, 0.0, 0.0, 0.0, 1.0, 0.0] binning_x: 0 binning_y: 0 roi: x_offset: 0 y_offset: 0 height: 0 width: 0 do_rectify: False

/spot/back/laser_scan topic header: seq: 982 stamp: secs: 1677524447 nsecs: 326256348 frame_id: "laser_back_frame" angle_min: -0.7760326862335205 angle_max: 0.7960231304168701 angle_increment: 0.0037164438981562853 time_increment: 0.0 scan_time: 0.032999999821186066 range_min: 0.3499999940395355 range_max: 3.0

              topic                                           rate   min_delta   max_delta   std_dev   window
=====================================================================================
/spot/camera/back/image                       1.366   0.6358      0.8287      0.08678   5     
/spot/depth/back/image                          1.353   0.6461      0.8186      0.07324   5     
/spot/camera/back/camera_info             1.366   0.6358      0.8287      0.08678   6     
/spot/depth/back/camera_info                1.353   0.6461      0.8186      0.07324   6 
/spot/back/laser_scan                             1.131   0.8285      0.9896      0.0657    5

I am trying to get rtabmap to work as this demo http://wiki.ros.org/rtabmap_ros section 3.1 Robot Mapping but no luck. On RVIZ I can see the laser topic, tf, and other topics except the /rtabmap/grid_map topic is not publishing. I have the node /rtabmap/rtabmap running and subscribing to these topics above but keep throwing me this warning:

[ WARN] [1677985356.988388648, 1677524342.208090254]: /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):
   /spot/camera/back/image_rgb_crop \
   /spot/depth/back/image \
   /spot/camera/back/camera_info_crop

I am generating laser from the depth image, and I am seeing the laser topic in RVIZ.

Questions: Is the difference between the image size in the topics image and depth causing the problem? Is the image type that rtabmap expected to receive such as 3 channels where the image is one channel? Is it the publication rate? Is it something else?

I am sorry of this big long issue, and I would appreciate your help and support.

matlabbe commented 1 year ago

What is the output of:

rostopic hz /spot/camera/back/image_rgb_crop \
  /spot/depth/back/image \
  /spot/camera/back/camera_info_crop

? This error is related to publishing rate, topics are not published at a similar frame rate or one of this topic is not published.

Based on framerate of the base topics, 1.3 Hz is quite slow (too slow if the depth and color images are not exactly sync). Rtabmap could eventually synchronize them, ideally if they have exact time sync, you may launch with approx_sync:=false to make sure correct color+depth images are sync.

For the resolution, rtabmap can support smaller depth image than color image, but they should have same aspect ratio and the depth image should be registered with color image. If they don't have same aspect ratio, this is a good indication that the depth image is not registered to color image.

rtabmap supports 1 channel gray-scale and 3-channels color images. For depth, it supports 16UC1 (mm) and 32FC1 (m) format.

julianraheema commented 1 year ago

Thank you @matlabbe for your response. Here are the output for the topics that you asked for:

         topic                   rate    min_delta   max_delta   std_dev   window

====================================================================================== /spot/camera/back/image_rgb_crop 0.9823 0.5086 2.349 0.5644 17
/spot/depth/back/image 0.9817 0.4985 2.351 0.5648 17
/spot/camera/back/camera_info_crop 0.9823 0.5086 2.349 0.5644 17

FYI: I am generating /spot/camera/back/rgb_image from /spot/camera/back/image to create 3 channels image then I am using another node to reduce the image size from 640x480 to match the depth image 424x240.

Here is my launch file:

<launch>
  <!-- ROBOT MAPPING VERSION: use this with ROS bag demo_mapping.bag -->
  <!-- WARNING : Database is automatically deleted on each startup -->
  <!--           See "delete_db_on_start" option below... -->

  <!-- Choose visualization -->
  <arg name="rviz" default="true" />
  <arg name="rtabmapviz" default="false" />
  <arg name="compressed" default="false"/>
  <arg name="align_depth" default="false"/>

  <param name="use_sim_time" type="bool" value="true"/>

  <!-- Localization-only mode -->
  <arg name="localization"      default="false"/>
  <arg if="$(arg localization)" name="rtabmap_args"  default=""/>
  <arg unless="$(arg localization)" name="rtabmap_args"  default="--delete_db_on_start"/>

  <include file="$(find spot_description)/launch/description_only.launch" />
  <include file="$(find depthimage_to_laserscan)/launch/image_to_laser.launch" />
  <include file="$(find ros_imresize)/launch/imresize.launch" />

  <node name="odom_corrector" pkg="nav_utils" type="odom_twist_to_child_frame">
    <remap from="/odom" to="/spot/odometry"/>
    <remap from="/odom_out" to="/spot/odometry_corrected"/>
  </node>

  <node pkg="spot_driver" type="gray2rgb.py" name="gray2rgb">
    <remap from="gray_in" to="/spot/camera/back/image"/>
    <remap from="rgb_out" to="/spot/camera/back/image_rgb"/>
  </node>

<group ns="rtabmap">
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
      <param name="frame_id"           type="string" value="base_link"/>
      <param name="wait_for_transform" type="bool" value="true"/>
      <param name="subscribe_depth" type="bool" value="true"/>
      <param name="subscribe_scan"  type="bool" value="false"/>
      <param name="odom_frame_id"            type="string" value="odom"/>
      <param name="odom_tf_linear_variance"  type="double" value="0.001"/>
      <param name="odom_tf_angular_variance" type="double" value="0.001"/>
      <param name="approx_sync"     type="bool"   value="false"/>
      <param name="approx_sync_max_interval" type="double" value="2.0"/>
      <remap from="scan" to="/spot/back/laser_scan"/>
      <remap from="rgb/image"       to="/spot/camera/back/image_rgb_crop"/>
      <remap from="depth/image"     to="/spot/depth/back/image"/>
      <remap from="rgb/camera_info" to="/spot/camera/back/camera_info_crop"/>
      <remap from="odom"            to="/spot/odometry_corrected"/>
      <!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
      <param name="RGBD/NeighborLinkRefining" type="string" value="true"/>  
      <param name="RGBD/ProximityBySpace"     type="string" value="true"/>  
      <param name="RGBD/ProximityByTime"      type="string" value="false"/> 
      <param name="RGBD/ProximityPathMaxNeighbors" type="string" value="10"/> 
      <param name="Reg/Strategy"              type="string" value="1"/>    
      <param name="Vis/MinInliers"        type="string" value="12"/>   
      <param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/> 
      <param name="RGBD/OptimizeMaxError"     type="string" value="4"/>     
      <param name="Reg/Force3DoF"             type="string" value="true"/>  
      <param name="Grid/Sensor"            type="string" value="false"/> 
      <param name="Mem/STMSize"               type="string" value="30"/>   
      <param name="RGBD/LocalRadius"          type="string" value="5"/>     
      <param name="Icp/CorrespondenceRatio"   type="string" value="0.2"/>   
      <param name="Icp/Strategy"                    type="string" value="false"/>
      <param name="Icp/PointToPlane"          type="string" value="false"/>
      <param name="Icp/MaxCorrespondenceDistance"  type="string" value="0.15"/>
      <param name="Icp/VoxelSize"             type="string" value="0.05"/>
      <param name="queue_size" type="int" value="100"/>

      <!-- 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 $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
      <param name="subscribe_depth"    type="bool" value="true"/>
      <param name="subscribe_scan"     type="bool" value="true"/>
      <param name="frame_id"           type="string" value="base_link"/>
      <param name="wait_for_transform" type="bool" value="true"/>

      <remap from="rgb/image"       to="/spot/camera/back/image_rgb_crop"/>
      <remap from="depth/image"     to="/spot/depth/back/image"/>
      <remap from="rgb/camera_info" to="/spot/camera/back/camera_info_crop"/>
      <remap from="scan"            to="/spot/back/laser_scan"/>
      <remap from="odom"            to="/spot/odometry_corrected"/>
      <param name="compressed"        value="$(arg compressed)"/>
      <param name="queue_size" type="int" value="100"/>

    </node>
  </group>

  <!-- Visualisation RVIZ -->
  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find spot_viz)/rviz/spot_rtab.rviz" output="screen"/>

  <node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
    <remap from="rgb/image"       to="/spot/camera/back/image_rgb_crop"/>
    <remap from="depth/image"     to="/spot/depth/back/image"/>
    <remap from="rgb/camera_info" to="/spot/camera/back/camera_info_crop"/>
    <remap from="cloud"           to="voxel_cloud" />
    <remap from="odom"            to="/spot/odometry_corrected"/>    
    <param name="queue_size" type="int" value="100"/>
    <param name="voxel_size" type="double" value="0.01"/>
  </node>
</launch>
julianraheema commented 1 year ago

When I plug these topics into rtabmap node, no map is produced. Here is my launch file: /spot/camera/back/image 640x480 mono8 /spot/depth/back/image 424x240 16UC1 /spot/camera/back/camera_info /spot/depth/back/camera_info

SUMMARY
========

PARAMETERS
 * /depthimage_to_laserscan/output_frame_id: laser_back_frame
 * /depthimage_to_laserscan/range_max: 3.0
 * /depthimage_to_laserscan/range_min: 0.35
 * /depthimage_to_laserscan/scan_height: 1
 * /depthimage_to_laserscan/scan_time: 0.033
 * /front_depthimage_to_laserscan/output_frame_id: laser_frontright_...
 * /front_depthimage_to_laserscan/range_max: 3.0
 * /front_depthimage_to_laserscan/range_min: 0.35
 * /front_depthimage_to_laserscan/scan_height: 1
 * /front_depthimage_to_laserscan/scan_time: 0.033
 * /points_xyzrgb/queue_size: 100
 * /points_xyzrgb/voxel_size: 0.01
 * /robot_description: <?xml version="1....
 * /ros_imresize/camera_info: /spot/camera/back...
 * /ros_imresize/resize_height: 240
 * /ros_imresize/resize_width: 424
 * /ros_imresize/topic_crop: /spot/camera/back...
 * /ros_imresize/undistord: True
 * /rosdistro: noetic
 * /rosversion: 1.15.15
 * /rtabmap/rtabmap/Grid/Sensor: false
 * /rtabmap/rtabmap/Icp/CorrespondenceRatio: 0.2
 * /rtabmap/rtabmap/Icp/MaxCorrespondenceDistance: 0.15
 * /rtabmap/rtabmap/Icp/PointToPlane: false
 * /rtabmap/rtabmap/Icp/Strategy: false
 * /rtabmap/rtabmap/Icp/VoxelSize: 0.05
 * /rtabmap/rtabmap/Mem/IncrementalMemory: true
 * /rtabmap/rtabmap/Mem/InitWMWithAllNodes: false
 * /rtabmap/rtabmap/Mem/STMSize: 30
 * /rtabmap/rtabmap/RGBD/LocalRadius: 5
 * /rtabmap/rtabmap/RGBD/NeighborLinkRefining: true
 * /rtabmap/rtabmap/RGBD/OptimizeFromGraphEnd: false
 * /rtabmap/rtabmap/RGBD/OptimizeMaxError: 4
 * /rtabmap/rtabmap/RGBD/ProximityBySpace: true
 * /rtabmap/rtabmap/RGBD/ProximityByTime: false
 * /rtabmap/rtabmap/RGBD/ProximityPathMaxNeighbors: 10
 * /rtabmap/rtabmap/Reg/Force3DoF: true
 * /rtabmap/rtabmap/Reg/Strategy: 1
 * /rtabmap/rtabmap/Vis/MinInliers: 12
 * /rtabmap/rtabmap/approx_sync: True
 * /rtabmap/rtabmap/approx_sync_max_interval: 0.9
 * /rtabmap/rtabmap/frame_id: base_link
 * /rtabmap/rtabmap/odom_frame_id: odom
 * /rtabmap/rtabmap/odom_tf_angular_variance: 0.001
 * /rtabmap/rtabmap/odom_tf_linear_variance: 0.001
 * /rtabmap/rtabmap/queue_size: 100
 * /rtabmap/rtabmap/subscribe_depth: True
 * /rtabmap/rtabmap/subscribe_scan: True
 * /rtabmap/rtabmap/wait_for_transform: True
 * /use_sim_time: True

NODES
  /
    depthimage_to_laserscan (depthimage_to_laserscan/depthimage_to_laserscan)
    front_depthimage_to_laserscan (depthimage_to_laserscan/depthimage_to_laserscan)
    gray2rgb (spot_driver/gray2rgb.py)
    odom_corrector (nav_utils/odom_twist_to_child_frame)
    points_xyzrgb (nodelet/nodelet)
    ros_imresize (ros_imresize/ros_imresize)
    rviz (rviz/rviz)
  /rtabmap/
    rtabmap (rtabmap_ros/rtabmap)

auto-starting new master
process[master]: started with pid [43726]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to b89e6a02-bbbe-11ed-8ce0-a3bc6edd2b63
process[rosout-1]: started with pid [43748]
started core service [/rosout]
process[depthimage_to_laserscan-2]: started with pid [43755]
process[front_depthimage_to_laserscan-3]: started with pid [43756]
process[ros_imresize-4]: started with pid [43757]
process[odom_corrector-5]: started with pid [43758]
process[gray2rgb-6]: started with pid [43765]
process[rtabmap/rtabmap-7]: started with pid [43774]
process[rviz-8]: started with pid [43776]
process[points_xyzrgb-9]: started with pid [43777]
[ERROR] [1678066334.454962986]: Skipped loading plugin with error: XML Document '/opt/ros/noetic/share/tf2_server/tf2_server_test_nodelets.xml' has no Root Element. This likely means the XML is malformed or missing..
[ INFO] [1678066334.513433086]: Retrieving parameters ...
[ INFO] [1678066334.515658308]: Parameters retrieved ...
[ INFO] [1678066334.522530836]: WAITING for ROS camera calibration!

[ INFO] [1678066334.629459366]: rviz version 1.14.19
[ INFO] [1678066334.629509509]: compiled against Qt version 5.12.8
[ INFO] [1678066334.629523673]: compiled against OGRE version 1.9.0 (Ghadamon)
[ INFO] [1678066334.643040134]: Forcing OpenGl version 0.
[ INFO] [1678066334.714893063]: Starting node...
[ERROR] [1678066334.774955500]: Skipped loading plugin with error: XML Document '/opt/ros/noetic/share/tf2_server/tf2_server_test_nodelets.xml' has no Root Element. This likely means the XML is malformed or missing..
[ INFO] [1678066334.785388695]: Initializing nodelet with 8 worker threads.
[ INFO] [1678066334.970413930]: /rtabmap/rtabmap(maps): map_filter_radius          = 0.000000
[ INFO] [1678066334.970452748]: /rtabmap/rtabmap(maps): map_filter_angle           = 30.000000
[ INFO] [1678066334.970479307]: /rtabmap/rtabmap(maps): map_cleanup                = true
[ INFO] [1678066334.970495199]: /rtabmap/rtabmap(maps): map_always_update          = false
[ INFO] [1678066334.970510370]: /rtabmap/rtabmap(maps): map_empty_ray_tracing      = true
[ INFO] [1678066334.970529827]: /rtabmap/rtabmap(maps): cloud_output_voxelized     = true
[ INFO] [1678066334.970548081]: /rtabmap/rtabmap(maps): cloud_subtract_filtering   = false
[ INFO] [1678066334.970565837]: /rtabmap/rtabmap(maps): cloud_subtract_filtering_min_neighbors = 2
[ INFO] [1678066334.970923175]: /rtabmap/rtabmap(maps): octomap_tree_depth         = 16
[ INFO] [1678066334.996167614]: rtabmap: frame_id      = base_link
[ INFO] [1678066334.996230121]: rtabmap: odom_frame_id = odom
[ INFO] [1678066334.996255085]: rtabmap: map_frame_id  = map
[ INFO] [1678066334.996277558]: rtabmap: log_to_rosout_level = 4
[ INFO] [1678066334.996298232]: rtabmap: initial_pose  = 
[ INFO] [1678066334.996318177]: rtabmap: use_action_for_goal  = false
[ INFO] [1678066334.996342710]: rtabmap: tf_delay      = 0.050000
[ INFO] [1678066334.996363903]: rtabmap: tf_tolerance  = 0.100000
[ INFO] [1678066334.996383772]: rtabmap: odom_sensor_sync   = false
[ INFO] [1678066334.996911675]: rtabmap: gen_scan  = false
[ INFO] [1678066334.996976682]: rtabmap: gen_depth  = false
[ INFO] [1678066335.002027377]: Stereo is NOT SUPPORTED
[ INFO] [1678066335.002132916]: OpenGL device: Mesa Intel(R) HD Graphics 630 (KBL GT2)
[ INFO] [1678066335.002205747]: OpenGl version: 4.6 (GLSL 4.6) limited to GLSL 1.4 on Mesa system.
[ INFO] [1678066335.093312571]: Setting RTAB-Map parameter "Grid/Sensor"="false"
[ INFO] [1678066335.116417549]: Setting RTAB-Map parameter "Icp/CorrespondenceRatio"="0.2"
[ INFO] [1678066335.122648454]: Setting RTAB-Map parameter "Icp/MaxCorrespondenceDistance"="0.15"
[ INFO] [1678066335.129725214]: Setting RTAB-Map parameter "Icp/PointToPlane"="false"
[ INFO] [1678066335.141905308]: Setting RTAB-Map parameter "Icp/Strategy"="false"
[ INFO] [1678066335.142279393]: Setting RTAB-Map parameter "Icp/VoxelSize"="0.05"
[ INFO] [1678066335.195969069]: Setting RTAB-Map parameter "Mem/IncrementalMemory"="true"
[ INFO] [1678066335.196416811]: Setting RTAB-Map parameter "Mem/InitWMWithAllNodes"="false"
[ INFO] [1678066335.210861595]: Setting RTAB-Map parameter "Mem/STMSize"="30"
[ INFO] [1678066335.251456402]: Setting RTAB-Map parameter "RGBD/LocalRadius"="5"
[ INFO] [1678066335.257771416]: Setting RTAB-Map parameter "RGBD/NeighborLinkRefining"="true"
[ INFO] [1678066335.259052882]: Setting RTAB-Map parameter "RGBD/OptimizeFromGraphEnd"="false"
[ INFO] [1678066335.259461856]: Setting RTAB-Map parameter "RGBD/OptimizeMaxError"="4"
[ INFO] [1678066335.263932440]: Setting RTAB-Map parameter "RGBD/ProximityBySpace"="true"
[ INFO] [1678066335.264339567]: Setting RTAB-Map parameter "RGBD/ProximityByTime"="false"
[ INFO] [1678066335.272004109]: Setting RTAB-Map parameter "RGBD/ProximityPathMaxNeighbors"="10"
[ INFO] [1678066335.275761633]: Setting RTAB-Map parameter "Reg/Force3DoF"="true"
[ INFO] [1678066335.277411482]: Setting RTAB-Map parameter "Reg/Strategy"="1"
[ INFO] [1678066335.374995155]: Setting RTAB-Map parameter "Vis/MinInliers"="12"
[ INFO] [1678066335.523773639]: Setting "Grid/RangeMax" parameter to 0 (default 5.000000) as "subscribe_scan", "subscribe_scan_cloud" or "gen_scan" is true and Grid/Sensor is 0.
[ INFO] [1678066335.524978163]: RTAB-Map detection rate = 1.000000 Hz
[ INFO] [1678066335.525787843]: rtabmap: Deleted database "/home/xxxxx/.ros/rtabmap.db" (--delete_db_on_start or -d are set).
[ INFO] [1678066335.525849458]: rtabmap: Using database from "/home/xxxxx/.ros/rtabmap.db" (0 MB).
[ INFO] [1678066335.715006489]: rtabmap: Database version = "0.20.23".
[ INFO] [1678066335.715098429]: rtabmap: SLAM mode (Mem/IncrementalMemory=true)
[ INFO] [1678066335.752479743]: /rtabmap/rtabmap: subscribe_depth = true
[ INFO] [1678066335.752562561]: /rtabmap/rtabmap: subscribe_rgb = true
[ INFO] [1678066335.752630419]: /rtabmap/rtabmap: subscribe_stereo = false
[ INFO] [1678066335.752688540]: /rtabmap/rtabmap: subscribe_rgbd = false (rgbd_cameras=1)
[ INFO] [1678066335.752721741]: /rtabmap/rtabmap: subscribe_odom_info = false
[ INFO] [1678066335.752750386]: /rtabmap/rtabmap: subscribe_user_data = false
[ INFO] [1678066335.752795203]: /rtabmap/rtabmap: subscribe_scan = true
[ INFO] [1678066335.752839299]: /rtabmap/rtabmap: subscribe_scan_cloud = false
[ INFO] [1678066335.752867916]: /rtabmap/rtabmap: subscribe_scan_descriptor = false
[ INFO] [1678066335.752897601]: /rtabmap/rtabmap: queue_size    = 100
[ INFO] [1678066335.752924691]: /rtabmap/rtabmap: approx_sync   = true
[ INFO] [1678066335.752988967]: Setup depth callback
[ INFO] [1678066335.780257310]: 
/rtabmap/rtabmap subscribed to (approx sync):
   /spot/camera/back/image \
   /spot/depth/back/image \
   /spot/camera/back/camera_info \
   /spot/back/laser_scan
[ INFO] [1678066335.928911735]: rtabmap 0.20.23 started...
[ WARN] [1678066339.648352503, 1677524287.219076632]: TF_REPEATED_DATA ignoring data with redundant timestamp for frame odom (parent map) at time 1677524287.319077 according to authority unknown_publisher
Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame odom (parent map) at time 1677524287.319077 according to authority unknown_publisher
         at line 278 in /home/xxxxx/src/autonomous_spot/src/geometry2/tf2/src/buffer_core.cpp
[ INFO] [1678066341.064382533, 1677524288.633589349]: Previous camera info :
header: 
  seq: 3
  stamp: 1677524287.130922131
  frame_id: back_fisheye
height: 480
width: 640
distortion_model: plumb_bob
D[]
  D[0]: 0
  D[1]: 0
  D[2]: 0
  D[3]: 0
  D[4]: 0
K[]
  K[0]: 330.945
  K[1]: 0
  K[2]: 316.329
  K[3]: 0
  K[4]: 329.815
  K[5]: 249.168
  K[6]: 0
  K[7]: 0
  K[8]: 1
R[]
  R[0]: 1
  R[1]: 0
  R[2]: 0
  R[3]: 0
  R[4]: 1
  R[5]: 0
  R[6]: 0
  R[7]: 0
  R[8]: 1
P[]
  P[0]: 330.945
  P[1]: 0
  P[2]: 316.329
  P[3]: 0
  P[4]: 0
  P[5]: 329.815
  P[6]: 249.168
  P[7]: 0
  P[8]: 0
  P[9]: 0
  P[10]: 1
  P[11]: 0
binning_x: 0
binning_y: 0
roi: 
  x_offset: 0
  y_offset: 0
  height: 0
  width: 0
  do_rectify: 0

[ INFO] [1678066341.065003002, 1677524288.633589349]: Undistortion active with param :
[0;
 0;
 0;
 0;
 0]

[ INFO] [1678066341.065257746, 1677524288.633589349]: Undistortion active with param :
[219.25084, 0, 209.56808;
 0, 164.90775, 124.58399;
 0, 0, 1]

[ INFO] [1678066341.065596928, 1677524288.633589349]: New camera info :
header: 
  seq: 3
  stamp: 1677524287.130922131
  frame_id: back_fisheye
height: 240
width: 424
distortion_model: 
D[]
K[]
  K[0]: 219.251
  K[1]: 0
  K[2]: 209.568
  K[3]: 0
  K[4]: 164.908
  K[5]: 124.584
  K[6]: 0
  K[7]: 0
  K[8]: 1
R[]
  R[0]: 1
  R[1]: 0
  R[2]: 0
  R[3]: 0
  R[4]: 1
  R[5]: 0
  R[6]: 0
  R[7]: 0
  R[8]: 1
P[]
  P[0]: 330.945
  P[1]: 0
  P[2]: 316.329
  P[3]: 0
  P[4]: 0
  P[5]: 329.815
  P[6]: 249.168
  P[7]: 0
  P[8]: 0
  P[9]: 0
  P[10]: 1
  P[11]: 0
binning_x: 0
binning_y: 0
roi: 
  x_offset: 0
  y_offset: 0
  height: 0
  width: 0
  do_rectify: 0

[ INFO] [1678066341.164819048, 1677524288.734709635]: RECEIVED ROS camera calibration!

[ WARN] [1678066341.255826782, 1677524288.825684014]: Could not get transform from odom to base_link after 0.200000 seconds (for stamp=1677524287.130922)! Error="Lookup would require extrapolation 0.188154501s into the past.  Requested time 1677524287.130922079 but the earliest data is at time 1677524287.319076538, when looking up transform from frame [base_link] to frame [odom]. canTransform returned after 0.202224 timeout was 0.2.".
[ INFO] [1678066341.310007118, 1677524288.876194287]: Running

[FATAL] (2023-03-05 17:32:21.455) MsgConversion.cpp:1862::convertRGBDMsgs() Condition (imageWidth/depthWidth == imageHeight/depthHeight) not met! [rgb=640x480 depth=424x240]
[FATAL] [1678066341.455382541, 1677524289.018328854]: [FATAL] (2023-03-05 17:32:21.455) MsgConversion.cpp:1862::convertRGBDMsgs() Condition (imageWidth/depthWidth == imageHeight/depthHeight) not met! [rgb=640x480 depth=424x240]
terminate called after throwing an instance of 'UException'
  what():  [FATAL] (2023-03-05 17:32:21.455) MsgConversion.cpp:1862::convertRGBDMsgs() Condition (imageWidth/depthWidth == imageHeight/depthHeight) not met! [rgb=640x480 depth=424x240]
[rtabmap/rtabmap-7] process has died [pid 43774, exit code -6, cmd /home/xxxxx/src/autonomous_spot/devel/lib/rtabmap_ros/rtabmap --delete_db_on_start scan:=/spot/back/laser_scan rgb/image:=/spot/camera/back/image depth/image:=/spot/depth/back/image rgb/camera_info:=/spot/camera/back/camera_info odom:=/spot/odometry_corrected __name:=rtabmap __log:=/home/xxxxx/.ros/log/b89e6a02-bbbe-11ed-8ce0-a3bc6edd2b63/rtabmap-rtabmap-7.log].
log file: /home/xxxxx/.ros/log/b89e6a02-bbbe-11ed-8ce0-a3bc6edd2b63/rtabmap-rtabmap-7*.log
matlabbe commented 1 year ago

This is the result of my comment:

For the resolution, rtabmap can support smaller depth image than color image, but they should have same aspect ratio and the depth image should be registered with color image. If they don't have same aspect ratio, this is a good indication that the depth image is not registered to color image.

If resolutions are not compatible, it will assert like this.

julianraheema commented 1 year ago

Thank you @matlabbe. Any advice on how to register depth with color image?

matlabbe commented 1 year ago

You could use pointcloud_to_depthimage node. I would set decimation parameter to 2 in your case (added point_cloud_xyz node to convert depth to point cloud if you don't have point cloud topic already from the depth camera):

rosrun rtabmap_ros point_cloud_xyz \
   depth/image:=/spot/depth/back/image \
   depth/camera_info:=/spot/depth/back/camera_info \
   cloud:=/spot/depth/back/cloud

rosrun rtabmap_ros pointcloud_to_depthimage \
   camera_info:=/spot/camera/back/camera_info_crop \
   cloud:=/spot/depth/back/cloud \
   _decimation:=2 \
   _fixed_frame_id:=base_link \
   image_raw:=/spot/depth/back/registered_to_color

Note that fixed_frame_id=base_link only makes sense if the images from color and depth cameras were taken at the same time, otherwise, if you have external odometry, you would have to set to fixed_frame_id=odom. Feed /spot/depth/back/registered_to_color instead of /spot/depth/back/image to rtabmap node.

Note that if you are using latest code from source, change rtabmap_ros by rtabmap_util above.

julianraheema commented 1 year ago

Thank you so much @matlabbe that works. You have been great helpful.