Ask Your Question
0

rtabmap using stereo camera that exports rectified and disparity images

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

Avner gravatar image

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)

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

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

matlabbe gravatar image

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)

edit flag offensive delete link more

Comments

Thank you for your answer. Please see my EDIT above

Avner gravatar image Avner  ( 2019-01-15 19:12:19 -0500 )edit

Please see my EDIT2 above

Avner gravatar image Avner  ( 2019-01-16 16:49:50 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

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

Seen: 232 times

Last updated: Jan 16 '19