ROS2 Image Pipeline Tutorial

How to create Point Clouds using two USB cameras

Background

Getting Started: Viewing One Camera

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
colcon build
ros2 pkg executables
ls /dev/video*
/dev/video0 /dev/video1 /dev/video2 /dev/video3 /dev/video4 /dev/video5
. install/local_setup.bash
ros2 run opencv_cam opencv_cam_main --ros-args --param index:=4
$ . 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>'
---
ros2 run image_view image_view --ros-args --remap /image:=/image_raw
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
ros2 run camera_calibration cameracalibrator \
--size=8x6 \
--square=0.063 \
--approximate=0.3 \
--no-service-check \
--ros-args --remap /image:=/image_raw
monocular calibration
[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
ros2 run opencv_cam opencv_cam_main --ros-args --param index:=4 --param camera_info_path:=camera-info-left-5.ini
$ 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

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
$ ros2 topic list
/left/camera_info
/left/image_rect
/parameter_events
/right/camera_info
/right/image_rect
/rosout
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
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
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

Create a Disparity Map (DisparityImage)

ros2 run stereo_image_proc disparity_node --ros-args --params-file disparity-params.yaml
$ 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
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
speckle_range = 100
speckle_range = 1000

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
$ 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
[INFO] [1610071478.345338507] [rviz]: Message Filter dropping message: frame 'camera_frame' at time 1610071477.160 for reason 'Unknown'
$ ros2 run tf2_ros static_transform_publisher \
0 0 4 \
0 1.5708 1.5708 \
test_frame_id \
test_child_frame_id
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

Summary

final graph using rqt_graph

Next Steps

Get the Medium app

A button that says 'Download on the App Store', and if clicked it will lead you to the iOS App store
A button that says 'Get it on, Google Play', and if clicked it will lead you to the Google Play store