Re: Unable to run rgdb with OpenLoris Scene Dataset

Posted by matlabbe on
URL: http://official-rtab-map-forum.206.s1.nabble.com/Unable-to-run-rgdb-with-OpenLoris-Scene-Dataset-tp7752p7765.html

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