Unable to run rgdb with OpenLoris Scene Dataset

[VK]

Dear Mathieu,

I'm trying to use rtabmap's rgbd_odometry on a bag file from OpenLoris Scene Dataset https://lifelong-robotic-vision.github.io/dataset/scene.html and I keep receive the following warning
[ WARN] [1617208938.818289296, 1560004895.400903523]: /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"). Parameter "approx_sync" is false, which means that input topics should have all the exact timestamp for the callback to be called.

I double checked the name topics from the bag file, I "played" with various values for the queue size but nothing changed,

When I simply play the bag file, rostopic list seems to be ok:
$ rostopic list
/clock
/d400/accel/imu_info
/d400/accel/sample
/d400/aligned_depth_to_color/camera_info
/d400/aligned_depth_to_color/image_raw
/d400/color/camera_info
/d400/color/image_raw
/d400/depth/camera_info
/d400/depth/image_raw
/d400/gyro/imu_info
/d400/gyro/sample
/d400/imu0
/gt
/odom
/rosout
/rosout_agg
/scan
/t265/accel/imu_info
/t265/accel/sample
/t265/fisheye1/camera_info
/t265/fisheye1/image_raw
/t265/fisheye2/camera_info
/t265/fisheye2/image_raw
/t265/gyro/imu_info
/t265/gyro/sample
/tf_static

I also checked if data  are actually published while the bag is playing
$ rostopic hz /d400/color/image_raw
subscribed to [/d400/color/image_raw]
WARNING: may be using simulated time
no new messages
no new messages
no new messages
average rate: 29.992
        min: 0.030s max: 0.041s std dev: 0.00475s window: 24
average rate: 30.005
        min: 0.030s max: 0.042s std dev: 0.00474s window: 54
average rate: 29.993
        min: 0.030s max: 0.042s std dev: 0.00474s window: 84
average rate: 29.995
        min: 0.030s max: 0.042s std dev: 0.00474s window: 114
average rate: 29.936
        min: 0.030s max: 0.043s std dev: 0.00453s window: 144

I am trying to launch the rgbd_odomerty as following:
roslaunch rtabmap_ros rtabmap.launch    \
   rtabmapviz:=true \
   approx_sync:=true \
   use_sim_time:=true \
   visual_odometry:=true \
   rgbd_odometry:=true \
   depth:=true \
   subscribe_depth:=true \
   frame_id:=base_link \
   camera_info_topic:=/d400/color/camera_info \
   depth_camera_info_topic:=/d400/depth/camera_info \
   rgb_topic:=/d400/color/image_raw \
   depth_topic:=/d400/depth/image_raw \
   queue_size:=30 \
   args:="-d  \
      --Odom/Strategy 0 \
      --Reg/Strategy 0 \
      --Vis/FeatureType 8 \
      --Kp/DetectorStrategy 8"

One thing I noticed is that when using the  $ rosrun tf view_frames, I only take the tf_static transform (which is a tf2 message, not timestamped)
cafe1-1.pdf

Any, advice?

[matlabbe]

Hi,

The camera_info is published only one time at the beginning. rtabmap is expecting camera_info published everytime an image is published. A workaround would be to do a node republishing the camera info:

republish_camera_info.py

#!/usr/bin/env python
import rospy
from sensor_msgs.msg import CameraInfo
from sensor_msgs.msg import Image

def callbackInfo(info):
    global camera_info_msg
    global sub_once
    global camera_info_set
    camera_info_msg = info
    camera_info_set = True
    sub_once.unregister()
    

def callback(image):
    global publisher
    global camera_info_msg
    if camera_info_msg:
        camera_info_msg.header = image.header
        publisher.publish(camera_info_msg)

if __name__ == "__main__":
    camera_info_set = False
    rospy.init_node("republish_camera_info", anonymous=True)
    publisher = rospy.Publisher("camera_info", CameraInfo, queue_size=1)
    rospy.Subscriber("image", Image, callback, queue_size=1)
    sub_once = rospy.Subscriber("camera_info", CameraInfo, callbackInfo, queue_size=1)
    rospy.spin()

RGB-D example

$ roslaunch rtabmap_ros rtabmap.launch \
   rtabmapviz:=true \
   approx_sync:=true \
   use_sim_time:=true \
   frame_id:=base_link \
   camera_info_topic:=/d400/color/camera_info \
   rgb_topic:=/d400/color/image_raw \
   depth_topic:=/d400/aligned_depth_to_color/image_raw \
   queue_size:=10  \
   args:="-d  \
      --Odom/Strategy 0 \
      --Reg/Strategy 0 \
      --Vis/FeatureType 8 \
      --Kp/DetectorStrategy 8"
      
$ python republish_camera_info.py camera_info:=/d400/color/camera_info image:=/d400/color/image_raw

$ rosbag play --clock --pause cafe1-1.bag

// press space bar to start then wait till finished

$ rosservice call /rtabmap/reset_odom

$ rosbag play --clock --pause cafe1-2.bag

Stereo Fisheye example

Note: you should update to this commit to work:

$ roslaunch rtabmap_ros rtabmap.launch \
   rtabmapviz:=true \
   approx_sync:=false \
   use_sim_time:=true \
   stereo:=true \
   frame_id:=base_link \
   left_camera_info_topic:=/t265/fisheye1/camera_info \
   left_image_topic:=/t265/fisheye1/image_raw \
   right_camera_info_topic:=/t265/fisheye2/camera_info \
   right_image_topic:=/t265/fisheye2/image_raw \
   queue_size:=10 \
   args:="-d  \
      --Odom/Strategy 0 \
      --Reg/Strategy 0 \
      --Vis/FeatureType 8 \
      --Kp/DetectorStrategy 8 \
      --Rtabmap/ImagesAlreadyRectified false"
      
$ python republish_camera_info.py camera_info:=/t265/fisheye1/camera_info image:=/t265/fisheye1/image_raw
$ python republish_camera_info.py camera_info:=/t265/fisheye2/camera_info image:=/t265/fisheye2/image_raw

$ rosbag play --clock --pause cafe1-1.bag

// press space bar to start then wait till finished

$ rosservice call /rtabmap/reset_odom

$ rosbag play --clock --pause cafe1-2.bag

Other configurations could be to include the scan and odom topics, I may do some examples another day. To use wheel odometry as input, you would have to make a node taking the odometry topic and publish the corresponding TF, then set visual_odometry to false in the commands above and set odom_topic agument to /odom.  To subscribe to scan, set rgbd_sync to true and subscribe_scan to true, while setting scan_topic to /scan. With scan, you may also want to set Reg/Force3DoF=true, Reg/Strategy=1 and possibly RGBD/NeighborLinkRefining=true (see this example for more details).

cheers,
Mathieu

[VK]

Dear Mathieu,

thank you for your response. I used the script and it worked!

Now, considering the IMU usage, I have some more questions...

1. Is it possible to use the VIO approaches with the rtabmap's rgbd odometry? I have build from source rtabmap_ros, do I have to do something more ?
2. My bag file, contains separate accel and gyro IMU msgs (and separate imu_info topics respectively). Is it possible to use them as separate topics? However, I used a python script for merging them in a single imu topic, though with out computing orientation quaternions, thus I receive an ERROR message that IMU received topic doesn't have orientation set. How can I solve this issue?
3. The separate IMU info topics are published only one time at the beginning. Do I have to merge them as well as the messages and also to republish them (as you did with the camera info topic)?

Thank you in advance,

Valia

[matlabbe]

Hi Valia,

1. Yes, theorically. The default rtabmap's odometry (F2M) uses the IMU in a loosely-coupled manner. For tightly-coupled VIO approaches, like OpenVINS or VINS_Fusion, you could use their package directly and use their output odometry for rtabmap node.
2. Use imu_filter_madgwick (or complementary_filter) on your merged topic, then use output of madgwick as input to rtabmap's nodes.
3. rtabmap's node don't use imu info topic.

cheers,
Mathieu

[matlabbe]

Here is an example using the 2D scan and wheel odometry:

2D scan and wheel odometry

roslaunch rtabmap_ros rtabmap.launch \
   rtabmapviz:=true \
   approx_sync:=true \
   use_sim_time:=true \
   frame_id:=base_link \
   camera_info_topic:=/d400/color/camera_info \
   rgb_topic:=/d400/color/image_raw \
   depth_topic:=/d400/aligned_depth_to_color/image_raw \
   rgbd_sync:=true \
   approx_rgbd_sync:=true \
   subscribe_scan:=true \
   visual_odometry:=false \
   odom_topic:=/odom \
   scan_topic:=/scan \
   queue_size:=10  \
   args:="-d  \
      --Reg/Strategy 1 \
      --Reg/Force3DoF true \
      --RGBD/NeighborLinkRefining true"
      
python republish_camera_info.py camera_info:=/d400/color/camera_info image:=/d400/color/image_raw

rosrun rtabmap_ros odom_msg_to_tf 

# launch with --pause to make sure tf_static topic is received by all nodes (to avoid some tf not defined errors)
rosbag play --clock --pause cafe1-1.bag

# wait till first trajectory is done, then trigger a new session
rosservice call /rtabmap/trigger_new_map

# start next trajectory
rosbag play --clock --pause cafe1-2.bag

2D scan ICP Odometry using wheel odometry as guess

# For best results, use rtabmap built from source with libpointmatcher support
roslaunch rtabmap_ros rtabmap.launch \
   rtabmapviz:=true \
   approx_sync:=true \
   use_sim_time:=true \
   frame_id:=base_link \
   camera_info_topic:=/d400/color/camera_info \
   rgb_topic:=/d400/color/image_raw \
   depth_topic:=/d400/aligned_depth_to_color/image_raw \
   rgbd_sync:=true \
   approx_rgbd_sync:=true \
   subscribe_scan:=true \
   visual_odometry:=false \
   icp_odometry:=true \
   odom_guess_frame_id:="base_odom" \
   scan_topic:=/scan \
   queue_size:=10  \
   args:="-d  \
      --Reg/Strategy 1 \
      --Reg/Force3DoF true"
      
python republish_camera_info.py camera_info:=/d400/color/camera_info image:=/d400/color/image_raw

rosrun rtabmap_ros odom_msg_to_tf 

# launch with --pause to make sure tf_static topic is received by all nodes (to avoid some tf not defined errors)
rosbag play --clock --pause cafe1-1.bag

# wait till first trajectory is done, then trigger a new session
rosservice call /rtabmap/trigger_new_map

# start next trajectory
rosbag play --clock --pause cafe1-2.bag