Creating a Stereo Camera in Gazebo

Here describes the experiences of creating a simulated stereo camera in Gazebo simulator.

Stereo Camera Description (URDF)

A physical model of the stereo camera is created using the urdf package. For details on PR2 URDF, please see pr2_description.

Here is an example Gazebo urdf extension that ties two cameras together, as well as specifying some camera parameters:

    <!-- stereo plugin that produces a raw_stereo message
    -->
    <map name="sensor" flag="gazebo">
      <verbatim key="controller_${name}_camera">
        <controller:ros_stereo_camera name="${name}_controller" plugin="libros_stereo_camera.so">
          <alwaysOn>true</alwaysOn>
          <updateRate>20.0</updateRate>
          <leftCamera>${name}_l_sensor</leftCamera>
          <rightCamera>${name}_r_sensor</rightCamera>
          <topicName>${name}/raw_stereo</topicName>
          <frameName>${name}_optical_frame</frameName>
          <CxPrime>320</CxPrime>
          <Cx>320</Cx>
          <Cy>240</Cy>
          <focal_length>320</focal_length> <!-- image_width / (2*tan(hfov_radian /2)) -->
          <distortion_k1>0</distortion_k1>
          <distortion_k2>0</distortion_k2>
          <distortion_k3>0</distortion_k3>
          <distortion_t1>0</distortion_t1>
          <distortion_t2>0</distortion_t2>
          <baseline>${stereo_dy}</baseline> <!-- home pos. of robot has +x forward, +y left -->
          <interface:stereocamera name="${name}_iface" />
        </controller:ros_stereo_camera>
      </verbatim>
    </map>

Stereo Camera Interface

ROS Topic

Similar to stereodcam, the stereo camera plugin publishes the RawStereo message /stereo/raw_stereo.

RawStereo Message

robot_msgs::RawStereo

# This message defines all the information necessary to reconstruct a
# full set of stereo information.  It should be generated directly by
# a driver connected to a set of stereo cameras.  It is only intended
# to be fed into a stereo processing node, and should not otherwise be
# used.  This is the preferred message to log when generating log
# files, as it is the minimal representation of the information.

uint8       NONE=0
uint8       IMAGE_RAW=1
uint8       IMAGE=2
uint8       IMAGE_COLOR=3
uint8       IMAGE_RECT=4
uint8       IMAGE_RECT_COLOR=5

Header        header
StereoInfo    stereo_info
CameraInfo       left_info
uint8         left_type
Image         left_image
CameraInfo       right_info
uint8         right_type
Image         right_image
uint8         has_disparity
DisparityInfo disparity_info
Image         disparity_image

do you know what launch scripts he's running for stereo?

left_image and right_image are expected to be monochromatic (uint8).
has_disparity is set to False, this triggers disparity_info and disparity_image to be generated by stereo_image_proc node.

CameraInfo Message

# This message defines meta information for a camera. It should be in a
# camera namespace and accompanied by up to 5 image topics named:
# 
# image_raw, image, image_color, image_rect, and image_rect_color

Header header

uint32 height
uint32 width

float64[5]  D # Distortion: k1, k2, t1, t2, k3
float64[9]  K # original camera matrix
float64[9]  R # rectification matrix
float64[12] P # projection/camera matrix

# Should put exposure, gain, etc. information here as well

simulated output:

left_info: 
  header: 
    seq: 1
    stamp: 419609999996
    frame_id: stereo_optical_frame
  height: 480
  width: 640
  D: (0.0, 0.0, 0.0, 0.0, 0.0)
  K: (320.0, 0.0, 320.0, 0.0, 320.0, 240.0, 0.0, 0.0, 1.0)
  R: (1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0)
  P: (320.0, 0.0, 320.0, 0.0, 0.0, 320.0, 240.0, 0.0, 0.0, 0.0, 1.0, 0.0)

For reference, see Camera_Calibration. Note that in simulation, $FOV_{h} = 90^\circ$ , camera resolution is set to $$640X480$$ , therefore, $$f_x$$ and $$f_y$$ are computed to be 320.

StereoInfo Message

# This message defines meta information for a stereo pair. It should
# be in a stereo namespace and accompanied by 2 camera namespaces, and
# a disparity image, named:
#
# left, right, and disparity, respectively

Header header

uint32 height
uint32 width

float64[3]  T  # Pose of right camera in left camera coords
float64[3]  Om # rotation vector
float64[16] RP # Reprojection Matrix

simulated output:

stereo_info: 
  header: 
    seq: 1which can be visualized using [:rviz: rviz]

    stamp: 419609999996
    frame_id: stereo_optical_frame
  height: 480
  width: 640
  T: (-0.089999999999999997, 0.0, 0.0)
  Om: (0.0, 0.0, 0.0)
  RP: (1.0, 0.0, 0.0, -320.0, 0.0, 1.0, 0.0, -240.0, 0.0, 0.0, 0.0, 320.0, 0.0, 0.0, 11.111111111111111, -0.0)

Where matrix $$T$$ is the translation from the left camera frame to the right camera frame. $$Om$$ is a rotation from left camera frame to the right camera frame. $$RP$$ is the re-projection matrix. For reference, see Simulator/ros_stereo_camera/videre_manual_v4.4d.pdf.

Launching Stereo Image Processing Node

See simulator tutorials for examples on testing stereo in simulation.