rtabmap using stereo camera that exports rectified and disparity images

asked 2019-01-09 15:14:29 -0500

Avner
96 ●21 ●26 ●29

updated 2019-01-16 17:31:59 -0500

Hi,

I am using stereo camera that is calibrated and exports rectified and disparity images.

The tutorial in here gives two examples of a stereo camera that exports raw images.

In the first stereo use-case here, the raw images are fed into the image_pipeline/stereo_image_proc module, which calculates the rectified, and disparity images. The disparity image is then fed to the rtabmap/disparity_to_depth module that calculates the depth image.

In the other stereo use-case here, the raw images are rectified by the image_pipeline/stereo_image_proc module.

In my case I already have the rectified images, the disparity image, and optionally, the xyz coordinates (in local camera coordinate system).

The rtabmap still requires sensor_msgs/CameraInfo. I'm thinking that the values should be:

D - the images are already undistorted - no distorion params [0.0, 0.0, 0.0, 0.0, 0.0]
K - [fx, 0, cx, 0, fy, cy, 0, 0, 1]
R - no rotation as the images are already aligned [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P - ==K (no need for translation, exterior rotation and interior rotation (K')
    for the master camera  [fx, 0,  cx, 0,    0,  fy, cy, 0, 0,  0,  1,  1]
    for the second camera  [fx, 0,  cx, fx*B, 0,  fy, cy, 0, 0,  0,  1,  1]

My questions:

What should the calibration parameters: D,K,R,P be in my case?
What should the launch file look like?

Thanks,

Avner

EDIT

I created a lauch file and CameraInfo yaml files based on the suggestion below (note that the yaml files have arbitrary values for now, so I don't expect to get quality results). The launcher runs but rtabmap complains and gives the following error message:

[ WARN] [1547600703.636501385]: /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.
/rtabmap/rtabmap subscribed to (exact sync):
   /rtabmap/odom,
   /stereo_camera/left/image_color,
   /stereo_camera/right/image_color,
   /stereo_camera/left/camera_info,
   /stereo_camera/right/camera_info,
   /rtabmap/odom_info

I checked that the images and the cameraInfo messages are transmitting images into ros (I can also see in rviz the right and the left images streaming)

rostopic hz /stereo_camera/left/image_color,
rostopic hz /stereo_camera/right/image_color,
rostopic hz /stereo_camera/left/camera_info,
rostopic hz /stereo_camera/right/camera_info,

But I don't get any messages for the following topics: /rtabmap/odom, /rtabmap/odom_info

For example:

rostopic hz /rtabmap/odom
no new messages
no new messages

...

Why the odom, odom_info are not publishing any messages? How can I check that the timestamps are the same in all the topics? (the warning message advice to check that)

Thanks, Avner

EDIT2

I'm using a dataset that was processed (rectified offline) from ... (more)

answered 2019-01-10 17:20:44 -0500

matlabbe

6409 ●11 ●96 ●80

updated 2019-01-16 10:41:50 -0500

You can look at the example on "Process a directory of stereo images in ROS" section of this page. The images are already rectified, the left and right calibration files:

stereo_20Hz_ros_left.yaml:

#%YAML:1.0
camera_name: stereo_20Hz_left
image_width: 640
image_height: 480
camera_matrix:
   rows: 3
   cols: 3
   data: [ 4.8760873413085938e+02, 0., 3.1811621093750000e+02, 0.,
       4.8760873413085938e+02, 2.4944424438476562e+02, 0., 0., 1. ]
distortion_coefficients:
   rows: 1
   cols: 5
   data: [ 0., 0., 0., 0., 0. ]
distortion_model: plumb_bob
rectification_matrix:
   rows: 3
   cols: 3
   data: [ 1., 0., 0., 0., 1., 0., 0., 0., 1. ]
projection_matrix:
   rows: 3
   cols: 4
   data: [ 4.8760873413085938e+02, 0., 3.1811621093750000e+02, 0., 0.,
       4.8760873413085938e+02, 2.4944424438476562e+02, 0., 0., 0., 1.,
       0. ]

stereo_20Hz_ros_right.yaml:

#%YAML:1.0
camera_name: stereo_20Hz_right
image_width: 640
image_height: 480
camera_matrix:
   rows: 3
   cols: 3
   data: [ 4.8760873413085938e+02, 0., 3.1811621093750000e+02, 0.,
       4.8760873413085938e+02, 2.4944424438476562e+02, 0., 0., 1. ]
distortion_coefficients:
   rows: 1
   cols: 5
   data: [ 0., 0., 0., 0., 0. ]
distortion_model: plumb_bob
rectification_matrix:
   rows: 3
   cols: 3
   data: [ 1., 0., 0., 0., 1., 0., 0., 0., 1. ]
projection_matrix:
   rows: 3
   cols: 4
   data: [ 4.8760873413085938e+02, 0., 3.1811621093750000e+02,
       -5.8362700032946350e+01, 0., 4.8760873413085938e+02,
       2.4944424438476562e+02, 0., 0., 0., 1., 0. ]

To publish the images with corresponding camera_info, the tool stereo_sequence_publisher.py is used. This will publish the rectified left image, rectified right image, left camera_info and right camera_info required by rtabmap in the StereoB case. The corresponding launch file:

<launch>

   <!-- stereo_20Hz directory -->
   <arg name="dir"        default="$(env HOME)/Downloads/stereo_20Hz" />

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

   <arg name="rate"       default="20" />

   <arg name="ground_is_obstacle" default="false"/>
   <arg name="align_with_ground"  default="false"/>

   <!-- Run stereo_sequence_publisher to publish synchronized images -->
   <node name="stereo_pub" pkg="bag_tools" type="stereo_sequence_publisher.py" output="screen">
      <param name="image_dir_left"         value="$(arg dir)/left"/>
      <param name="image_dir_right"        value="$(arg dir)/right"/>
      <param name="file_pattern"           value="*.jpg"/>
      <param name="camera_info_file_left"  value="$(arg dir)/stereo_20Hz_ros_left.yaml"/>
      <param name="camera_info_file_right" value="$(arg dir)/stereo_20Hz_ros_right.yaml"/>
      <param name="frequency"              value="$(arg rate)"/>
   </node>

   <!-- rotate camera so z axis is up and x forward. -->
   <arg name="pi/2" value="1.5707963267948966" />
   <node pkg="tf" type="static_transform_publisher" name="camera_base_link" args="0 0 0 -$(arg pi/2) 0 -$(arg pi/2) camera_link stereo_20Hz_left 100" /> 

   <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
      <arg name="rtabmap_args" value="--delete_db_on_start --Vis/EstimationType 1 --Vis/MinInliers 15 --SURF/HessianThreshold 100 --Grid/3DGroundIsObstacle $(arg ground_is_obstacle) --Odom/AlignWithGround $(arg align_with_ground)" />
      <arg name="stereo"           value="true" />
      <arg name="rviz"             value="$(arg rviz)" />
      <arg name="rtabmapviz"       value="$(arg rtabmapviz)" />
      <arg name="left_image_topic"        value="/stereo_camera/left/image_color" />
      <arg name="right_image_topic"       value="/stereo_camera/right/image_color" />
      <arg name="left_camera_info_topic"  value="/stereo_camera/left/camera_info" />
      <arg name="right_camera_info_topic" value="/stereo_camera/right/camera_info" />
   </include>

</launch>

EDIT: I don't think stereo_image_proc is needed in this case as input images are already rectified, just remap images/camera_info directly to rtabmap. I edited the launch above.

EDIT2 When setting stereo mode for rtabmap.launch, it assumes that topics have exactly ... (more)