ROS2 Image Pipeline Tutorial

How to create Point Clouds using two USB cameras

Background

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

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

bottom of my tea pot

Next Step: Calibration

[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
monocular calibration

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
1280
height
720
[narrow_stereo]
camera matrix
829.298086 0.000000 664.242945
0.000000 826.302904 319.057782
0.000000 0.000000 1.000000
distortion
-0.320171 0.099254 0.000692 0.002468 0.000000
rectification
1.000000 0.000000 0.000000
0.000000 1.000000 0.000000
0.000000 0.000000 1.000000
projection
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

# 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

$ 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
view of stereo camera calibration

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)

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
a not-so-functional disparity image

Tweaking the Disparity Parameters

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

speckle_range = 100
speckle_range = 1000

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:

Create a Point Cloud (PointCloud2)

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

$ 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

final graph using rqt_graph

Next Steps

  • 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.