0
$\begingroup$

Rosanswers logo

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 a bumblebee stereo camera.

I followed your suggestions and found out that the stamps were always 0 for /stereo_camera/left/camera_info, and /stereo_camera/right/camera_info

I fixed the timestamps. That got rid of the warning message "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..."

Now I'm getting another warning message:

[ WARN] [1547673742.252258621]: odometry: Could not get transform from camera_link to stereo_camera/left (stamp=1547672074.693022) after 0.200000 seconds ("wait_for_transform_duration"=0.200000)! Error="Lookup would require extrapolation into the past.  Requested time 1547672074.693022012 but the earliest data is at time 1547673732.372926027, when looking up transform from frame [stereo_camera/left] to frame [camera_link]. canTransform returned after 0.201424 timeout was 0.2."

To check the flow of the process I'm playing a bag file in infinite loop, so the timestamps increase and then set back. (I'm not looking into the quality of the results yet, I just want to see that I can build some map)

Contrary to the timestamps of /stereo_camera/left/image_color, /stereo_camera/right/image_color, /stereo_camera/left/camera_info, /stereo_camera/right/camera_info, the timestamps from /tf keep growing (I'm seeing that with "rostopic echo /tf/transforms")

The static_transform_publisher is defined in the launcher file (see below).

My questions:

  • Is there a way to set the timestamp of the static_transform_publisher in the launch file?

  • Disregarding the quality of the results, is it valid (i.e. expecting to get some rtabmap output results) to play a sequence of images in infinite loop? (where the timestamps, revert back at the end of the sequence)

  • Is it possible to run stereo_sequence_publisher in infinite loop? (similar to playing infinitely from a bag file)

Thanks,

Avner


<?xml version="1.0"?>
<launch>
    <!-- Choose visualization -->
    <arg name="rviz"       default="true" />
    <arg name="rtabmapviz" default="false" />

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

    <!-- Corresponding config files -->
    <arg name="rviz_cfg"                default="/home/avnerm/.ros/slam_rtabmap.rviz" />

    <!-- Run rosbag inifintely to publish synchronized images -->
    <node pkg="rosbag" type="play" name="rosbag" required="true" args="-l --clock /mnt/hdd2/home/avnerm/avner/Slam/rtabmap/data/dataset3/dataset3ShortSeq.bag "/>

    <!-- 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" /> -->
    <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_camera/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="rviz_cfg"       value="$(arg rviz_cfg)" />
        <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>

EDIT3

I had to set the parameter use_sim_time to true.

rosparam set use_sim_time true
rosparam get use_sim_time
true

Once I have done that, I started seeing the point cloud being generated in rviz.

Thanks for your help.

Avner


Originally posted by Avner on ROS Answers with karma: 96 on 2019-01-09

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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 the same stamps for synchronization. /rtabmap/odom is published by stereo_odometry. If it is not published, it means that stereo_odometry cannot synchronize the topics. To know if they have all the same stamps, you may do in 4 terminals:

$ rostopic echo /stereo_camera/left/image_color/header/stamp
$ rostopic echo /stereo_camera/right/image_color/header/stamp
$ rostopic echo /stereo_camera/left/camera_info/header/stamp
$ rostopic echo /stereo_camera/right/camera_info/header/stamp

Kill the launch publishing the topics, and see manually if the stamps are the same. For reference, take a look at how stereo_sequence_publisher.py is publishing the 4 topics with exactly the same stamp. This assumes that left and right images were a priori synchronized. What kind of stereo camera do you use?


Originally posted by matlabbe with karma: 6409 on 2019-01-10

This answer was ACCEPTED on the original site

Post score: 0


Original comments

Comment by Avner on 2019-01-15:
Thank you for your answer. Please see my EDIT above

Comment by Avner on 2019-01-16:
Please see my EDIT2 above

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.