ROS2 Image Pipeline Tutorial

How to create Point Clouds using two USB cameras

10 min readJan 8, 2021

Background

The ROS image_pipeline packages provides several tools to help with computer vision. In particular, they provide ROS2 nodes to help get stereo vision working. This blog post will detail how to setup this image pipeline and cover some of the issues I faced along the way.

While you may be able to read and apply this blog post without much prior knowledge of ROS, I would highly recommend the “Beginner: CLI” and “Beginner: Client Libraries” tutorials to learn more about how packages work in ROS2. I understood where to look for answers to my questions and it was worth the effort (maybe ~4 hours total).

Getting Started: Viewing One Camera

Using a camera seems like pretty common task. I figured the first step here is to find the canonical usb camera library and see if I can publish the picture to a topic. This proved to be a bit more complicated than I originally imagined. Here are a few of the libraries I found:

camera from demos (cam2image, OpenCV)
usb_cam (libav) https://github.com/ros-drivers/usb_cam.git
libuvc_camera (libuvc) http://wiki.ros.org/libuvc_camera
ros2_usb_camera (OpenCV) https://github.com/klintan/ros2_usb_camera
opencv_cam (OpenCV) https://github.com/clydemcqueen/opencv_cam

My main concern was compatibility with ROS2 and I didn’t want to fuss with the build system too much. I ended up choosing the opencv_cam project. I haven’t faced any issues with it during the project and will likely continue using it. If you don’t want to install a new package, you can experiment with the cam2image node though I am not sure it is compatible with the later stages in this project.

The first step is to create a new workspace and clone the the image_pipeline and image_common repositories:

mkdir test_ws
cd test_ws
mkdir src
cd src
git clone https://github.com/ros-perception/image_pipeline.git
git clone https://github.com/ros-perception/image_common.git
git clone https://github.com/clydemcqueen/opencv_cam.git
git clone https://github.com/ptrmu/ros2_shared.git

You’ll want to make sure you are using the branch specific to your ROS distribution (though I am not sure there will be any incompatibility in message formats if the core APIs are stable).

In the root directory of the project, you can now compile and see what is available to run

colcon build
ros2 pkg executables

Next, find possible video devices:

ls /dev/video*
/dev/video0  /dev/video1  /dev/video2  /dev/video3  /dev/video4  /dev/video5

Note that Linux may create multiple video devices for one camera. In my case, I have a Logitech BRIO that exposes 4 different video devices. The camera I am using in this example will be one I bought on eBay a while back. It looks similar to this one. In the example above, /dev/video{4,5} are from this USB camera. We will use these numbers as device ids for our camera node. You may need to try with various index params

. install/local_setup.bash
ros2 run opencv_cam opencv_cam_main --ros-args --param index:=4

In a separate terminal, double check that the topic is created and creates data:

$ . install/local_setup.bash$ ros2 topic list
/image_raw
/parameter_events
/rosout$ ros2 topic echo --no-arr /image_raw
header:
  stamp:
    sec: 1610063135
    nanosec: 248209953
  frame_id: camera_frame
height: 720
width: 1280
encoding: bgr8
is_bigendian: 0
step: 3840
data: '<sequence type: uint8, length: 2764800>'
---

It looks like the camera is sending Image data to the /image_raw topic. Let’s view this data using the image viewer

ros2 run image_view image_view --ros-args --remap /image:=/image_raw

taa-daa! you should see the contents of the camera in the image viewer

Next Step: Calibration

You should have seen some errors in the above opencv_cam node:

[ERROR] [1610062920.136820591] [camera_calibration_parsers]: Failed to detect content in .ini file
[ERROR] [1610062920.136830394] [opencv_cam]: cannot get camera info, will not publish

Cameras in ROS can have some “metadata” that describe the physical camera that took the Image. This will help later when trying to make sense of the objects inside of the camera as well as comparing the results of two different cameras. To understand more about “camera info,” we need to take a look at the CameraInfo message (melodic link). There is a lot to unpack here and I encourage you to take some time to read a bit more about each of the fields.

To create this file for one camera, you’ll need to use the calibration tools located in the image_pipeline package (we will cover how this process changes for stereo vision later). Here is an example command that will run the calibrator program. You can read through the source code but this ends up using the OpenCV library cv2.CalibrateCamera() . OpenCV has their own documentation on this process and you can read about it more there.

ros2 run camera_calibration cameracalibrator \
  --size=8x6 \
  --square=0.063 \
  --approximate=0.3 \
  --no-service-check \
  --ros-args --remap /image:=/image_raw

Basically, you’ll need some checkerboard pattern to move around the focal area. Here is a link to a 6x8 grid. I ended up using my phone and later a laptop as I don’t have a printer. This seemed to work fine for unblocking the project but I would also like to revisit a more “correct” way to calibrate the camera with a larger checkerboard.

After moving your checkerboard around (X, Y, depth (Z), skew), the “calibrate” button will light up and the calibration program will output the CameraInfo to the terminal in the .ini format. I have had some weird cases where Ubuntu thought the program needed to be killed (maybe because it hadn’t updated rendering in ~60 seconds) so just give the program a bit more time (~1/2 minutes). Here is an example of one of my calibration attempts:

[image]width
1280height
720[narrow_stereo]
camera matrix
829.298086 0.000000 664.242945
0.000000 826.302904 319.057782
0.000000 0.000000 1.000000distortion
-0.320171 0.099254 0.000692 0.002468 0.000000rectification
1.000000 0.000000 0.000000
0.000000 1.000000 0.000000
0.000000 0.000000 1.000000projection
647.238342 0.000000 683.202218 0.000000
0.000000 768.937317 313.070606 0.000000
0.000000 0.000000 1.000000 0.000000

I saved this file in the root of the ROS workspace. After you’ve saved the file, you can re-run the opencv_cam node so that you’ll also publish the camera info:

ros2 run opencv_cam opencv_cam_main --ros-args --param index:=4 --param camera_info_path:=camera-info-left-5.ini

Now, you’ll see the /camera_info topic:

$ ros2 topic list
/camera_info
/image_raw
/parameter_events
/rosout$ ros2 topic info /camera_info
Type: sensor_msgs/msg/CameraInfo
Publisher count: 1
Subscription count: 0$ ros2 topic echo  /camera_info
header:
  stamp:
    sec: 1610066349
    nanosec: 815620490
  frame_id: camera_frame
height: 720
width: 1280
distortion_model: plumb_bob
d:
- -0.35072
- 0.105506
- -0.002266
- -0.001482
- 0.0
...

Stereo Vision

The guide above is useful for calibrating one camera. The process for two cameras is similar but will yield a slightly different CameraInfo object. If you look closely at the description for the Projection/camera matrix part of the CameraInfo, you’ll see the fourth column which describes the disparity between the two cameras:

# For a stereo pair, the fourth column [Tx Ty 0]’ is related to the
# position of the optical center of the second camera in the first
# camera’s frame. We assume Tz = 0 so both cameras are in the same
# stereo image plane. The first camera always has Tx = Ty = 0. For
# the right (second) camera of a horizontal stereo pair, Ty = 0 and
# Tx = -fx’ * B, where B is the baseline between the cameras.

Unfortunately, I made the mistake of calibrating the cameras individually which led to the fourth column containing all zeros. While the point cloud node will still work, all of the points will be located on 0,0,0. Hopefully, this guide will help you from making the same mistake.

Stereo Vision Calibration

To begin, start two of the opencv_cam nodes:

$ ros2 run opencv_cam opencv_cam_main \
  --ros-args --param index:=4 \
  --remap __ns:=/left \
  --remap /left/image_raw:=/left/image_rect$ ros2 run opencv_cam opencv_cam_main \
  --ros-args --param index:=6 \
  --remap __ns:=/right \
  --remap /right/image_raw:=/right/image_rect

You should have the following topics:

$ ros2 topic list
/left/camera_info
/left/image_rect
/parameter_events
/right/camera_info
/right/image_rect
/rosout

Now, run the calibration program and supply args to make it run in stereo mode:

ros2 run camera_calibration cameracalibrator \
  --size=8x6 \
  --square=0.063 \
  --approximate=0.3 \
  --no-service-check \
  --ros-args --remap /left:=/left/image_rect \
  --remap /right:=/right/image_rect

Follow the same calibration steps above until the “Calibrate” button is lit up. You should be left with two CameraInfo messages (for left and right). Take a look at the projection section and verify the right camera does not contain all zeros:

left camera:

projection
856.167319 0.000000 197.679413 0.000000
0.000000 856.167319 379.488960 0.000000
0.000000 0.000000 1.000000 0.000000

right camera:

projection
856.167319 0.000000 197.679413 -178.286885
0.000000 856.167319 379.488960 0.000000
0.000000 0.000000 1.000000 0.000000

The value of -178.286885 shows that these two CameraInfo files can be used together to later create Point Clouds.

Create a Disparity Map (DisparityImage)

Make sure to update both of your Camera nodes to use the --param --camera_info_path:={left|right}-camera.ini .

After updating the camera info settings, run the disparity node which will create DisparityImage on the /disparity topic. I created a yaml file to store parameters but you can also use command line args

ros2 run stereo_image_proc disparity_node --ros-args --params-file disparity-params.yaml

Then, check the outputs:

$ ros2 topic list
/disparity
/left/camera_info
/left/image_rect
/parameter_events
/right/camera_info
/right/image_rect
/rosout$ ros2 topic info /disparity
Type: stereo_msgs/msg/DisparityImage
Publisher count: 1
Subscription count: 0

Now, you can view the DisparityImage using the disparity_view node:

ros2 run image_view disparity_view --ros-args --remap image:=/disparity

Tweaking the Disparity Parameters

You can use the rqt_reconfigure package to quickly experiment with various parameters used by the disparity_node :

ros2 run rqt_reconfigure rqt_reconfigure

In the example below, I changed the speckle_size parameter from 100 to 1000. You can tweak the parameters to try and get your Disparity Map

I wasn’t able to get a DisparityImage as “nice looking” as the one in this guide, so be patient and try a several variations of parameters. That guide does give some information of how the Block Matching algorithm works though I wonder if today’s cameras have more pixels and make it more difficult for the block matching algorithm to work. As I don’t have a proper housing for my two cameras (just some cardboard), small variations in the positions of the cameras will have noticeable consequences in the DisparityImage and later the Point Cloud.

Here is an example of a disparity image of my work out stand:

https://images-na.ssl-images-amazon.com/images/I/41fdlbMpO2L.jpg

Create a Point Cloud (PointCloud2)

To create Point Cloud, run the point_cloud_node (the default topic for the DisparityImage is /disparity so we don’t explicitly pass it below)

ros2 run stereo_image_proc point_cloud_node \
  --ros-args --remap left/image_rect_color:=left/image_rect \
  --param approximate_sync:=True \
  --param avoid_point_cloud_padding:=True

Now, view the output ( /points2 ):

$ ros2 topic list
/disparity
/left/camera_info
/left/image_rect
/parameter_events
/points2
/right/camera_info
/right/image_rect
/rosout$ ros2 topic info /points2
Type: sensor_msgs/msg/PointCloud2
Publisher count: 1
Subscription count: 0$ ros2 topic echo /points2 --no-arr
header:
  stamp:
    sec: 1610071282
    nanosec: 552372030
  frame_id: foo
height: 720
width: 1280
fields: '<sequence type: sensor_msgs/msg/PointField, length: 4>'
is_bigendian: false
point_step: 16
row_step: 20480
data: '<sequence type: uint8, length: 14745600>'
is_dense: false

Visualize the Point Cloud

Finally, we are ready to view the PointCloud in Rviz2:

$ rviz2

To add the PointCloud, click Add . Select By topic > /points2 > PointCloud2 and then click OK.

You’ll then see the following in the rviz2 terminal’s output:

[INFO] [1610071478.345338507] [rviz]: Message Filter dropping message: frame 'camera_frame' at time 1610071477.160 for reason 'Unknown'

This is because the the point’s x/y/z coordinates are relative to the left camera but the left camer’s “frame” does not exist in Rviz’s “world.” Fortunately, we can fake the position of a frame using the following:

$ ros2 run tf2_ros static_transform_publisher \
  0 0 4 \
  0 1.5708 1.5708 \
  test_frame_id \
  test_child_frame_id

Next, we need our left camera to reference the test_frame_id. This information is located in the Header of all of the messages so you should be able to use ros2 topic echo to double check that it propagates. Here is the updated opencv_cam command:

ros2 run opencv_cam opencv_cam_main \
  --ros-args --param index:=6 \
  --param camera_info_path:=camera-info-left-5.ini \
  --remap __ns:=/left \
  --remap /left/image_raw:=/left/image_rect \
  --param camera_frame_id:=test_frame_id

You’ll have to update the Global Options > Fixed Frame to use either test_frame_id or test_child_frame_id . Below are images of the finish product. I bend the stereo camera frame (made of cardboard) to change the position of the cameras to change the disparity image. You can see the arms of the workout stand move farther away (disparity is blue) and then move closer (disparity is green).

⚠️ Apologies for the weird blinking/flicker at the end of the GIF. I am not a photo editor and used peek to capture these.

Summary

Creating PointCloud2 message is possible in ROS2 using two USB cameras and various nodes in the image_pipeline and image_common packages. While this tutorial is incomplete for a production-ready computer vision system, it should be enough to get you started and ready to apply to more advanced robotic concepts.

Next Steps

After you have PointCloud data, you’ll want other pieces of data that help represent the state of the robot in a world. Notably:

IMU data to measure movement
More sophisticated joint and frame data

The next step is to explore various SLAM packages to construct a “map” from the point cloud data. I will explore collecting IMU and Servo data with rosserial to try and understand more about visualizing and reporting the state of a robot arm I purchased a while back. After, I would like to learn more about the MoveIt package as well as Cartographer and Octomap.