diff --git a/stereo_image_proc/doc/changes.rst b/stereo_image_proc/doc/changes.rst new file mode 100644 index 000000000..264172a21 --- /dev/null +++ b/stereo_image_proc/doc/changes.rst @@ -0,0 +1,12 @@ +Changelog Notes +=============== + +Jazzy Jalisco +------------- +There are several major change between ``Iron`` and ``Jazzy``: + + * All components now properly support ``image_transport`` parameter. + * All components now properly support remapping the ``camera_info`` topic + for an associated ``image`` topic. For instance, if you remap ``image`` + to ``my/image`` then ``my/camera_info`` will be used. Previously you + would have to manually remap the ``camera_info`` topic. diff --git a/stereo_image_proc/doc/components.rst b/stereo_image_proc/doc/components.rst new file mode 100644 index 000000000..5ac76213e --- /dev/null +++ b/stereo_image_proc/doc/components.rst @@ -0,0 +1,122 @@ +Nodes and Components +==================== + +This package includes a number of ROS 2 components that can be assembled +into stereo image processing pipelines. +See the tutorial :ref:`Launch stereo_image_proc Components`. + +Alternatively, each component can be run as a standalone node. + +stereo_image_proc::DisparityNode +-------------------------------- +Performs block matching on a pair of rectified stereo images, producing a +disparity image. Also available as a standalone node with the name +``disparity_node``. + +Subscribed Topics +^^^^^^^^^^^^^^^^^ + * **left/camera_info** (sensor_msgs/CameraInfo): Left camera metadata. + * **left/image_rect** (sensor_msgs/Image): Left monochrome rectified + image stream. + * **right/camera_info** (sensor_msgs/CameraInfo): Right camera metadata. + * **right/image_rect** (sensor_msgs/Image): Right monochrome rectified + image stream. + +Published Topics +^^^^^^^^^^^^^^^^ + * **disparity** (sensor_msgs/DisparityImage): Floating point disparity + image with metadata. + +Parameters +^^^^^^^^^^ + +*Disparity pre-filtering* + + * **prefilter_size** (int, default: 9): Normalization window size, pixels. + * **prefilter_cap** (int, default: 31): Bound on normalized pixel values. + +*Disparity correlation* + + * **correlation_window_size** (int, default: 15): Edge size (pixels) of the + correlation window for matching. Values must be odd, in the range 5 to 255 + (but with an extra performance hit for values larger than 21). Larger values + have smoother disparity results, but smear out small features and depth + discontinuities. + * **min_disparity** (int, default: 0): Minimum disparity, or the offset to the + disparity search window. By setting to a positive value, the cameras become + more "cross-eyed" and will find objects closer to the cameras. If the cameras + are "verged" (inclined toward each other), it may be appropriate to set + min_disparity to a negative value. When min_disparity is greater than 0, + objects at large distances will not be found. + * **disparity_range** (int, default: 64): The size of the disparity search + window (pixels). Together with min_disparity, this defines the "horopter," + the 3D volume that is visible to the stereo algorithm. + +*Disparity post-filtering* + + * **uniqueness_ratio** (double, default: 15.0): Filters disparity readings + based on a comparison to the next-best correlation along the epipolar + line. Matches where uniqueness_ratio > (best_match - next_match) / next_match + are filtered out. + * **texture_threshold** (int, default: 10): Filters disparity readings based on + the amount of texture in the correlation window. + * **speckle_size** (int, default: 100): Filters disparity regions that are less + than this number of pixels. For example, a value of 100 means that all regions + with fewer than 100 pixels will be rejected. + * **speckle_range** (int, default: 4): Groups disparity regions based on their + connectedness. Disparities are grouped together in the same region if they are + within this distance in pixels. + +*Synchronization* + + * **approximate_sync** (bool, default: false): Whether to use approximate + synchronization. Set to true if the left and right cameras do not produce + exactly synced timestamps. + * **approximate_sync_tolerance_seconds** (double, default: 0.0): Tolerance + when using approximate sync. + * **image_transport** (string, default: raw): Image transport to use for left + image subscriber. + * **queue size** (int, default: 5): Size of message queue for each synchronized + topic. You may need to raise this if images take significantly longer to travel + over the network than camera info and/or the delay from disparity processing + is too long. + +stereo_image_proc::PointCloudNode +--------------------------------- +Combines a rectified color image and disparity image to produce a +``sensor_msgs::PointCloud2`` point cloud. Also available as a standalone +node ``point_cloud_node``. + +Note: ROS 1 had both a ``point_cloud`` and ``point_cloud2`` version to +handle the now deprecated and removed ``sensor_msgs::PointCloud`` and +``stereo_msgs::PointCloud2``. This node corresponds to the ``point_cloud2`` +version although we have dropped the ``2``. + +Subscribed Topics +^^^^^^^^^^^^^^^^^ + * **disparity** (stereo_msgs/DisparityImage): Floating point disparity + image with metadata. + * **left/camera_info** (sensor_msgs/CameraInfo): Left camera metadata. + * **left/image_rect_color** (sensor_msgs/Image): Rectified image. + * **rigtht/camera_info** (sensor_msgs/CameraInfo): Right camera metada. + +Published Topics +^^^^^^^^^^^^^^^^ + * **points2** (sensor_msgs/PointCloud2): Stereo point cloud with RGB color. + +Parameters +^^^^^^^^^^ + * **approximate_sync** (bool, default: false): Whether to use approximate + synchronization. Set to true if the left and right cameras do not produce + exactly synced timestamps. + * **avoid_point_cloud_padding** (bool, default: false): Avoid using alignment + padding in the generated point cloud. This reduces bandwidth requirements, + as the point cloud size is halved. Using point clouds without alignment + padding might degrade performance for some algorithms. + * **image_transport** (string, default: raw): Image transport to use for left + image subscriber. + * **queue size** (int, default: 5): Size of message queue for each synchronized + topic. You may need to raise this if images take significantly longer to travel + over the network than camera info and/or the delay from disparity processing + is too long. + * **use_color** (oool, default: true): If false, point cloud will be XYZ only. diff --git a/stereo_image_proc/doc/conf.py b/stereo_image_proc/doc/conf.py new file mode 100644 index 000000000..b6fda617d --- /dev/null +++ b/stereo_image_proc/doc/conf.py @@ -0,0 +1,194 @@ +# Copyright 2024 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# -*- coding: utf-8 -*- +# +# Configuration file for the Sphinx documentation builder. +# +# This file does only contain a selection of the most common options. For a +# full list see the documentation: +# http://www.sphinx-doc.org/en/master/config + +# -- Path setup -------------------------------------------------------------- + +# If extensions (or modules to document with autodoc) are in another directory, +# add these directories to sys.path here. If the directory is relative to the +# documentation root, use os.path.abspath to make it absolute, like shown here. +# +import os +import sys +sys.path.insert(0, os.path.abspath('.')) + + +# -- Project information ----------------------------------------------------- + +project = 'stereo_image_proc' +copyright = '2008-2024, Open Source Robotics Foundation, Inc.' # noqa +author = 'Open Source Robotics Foundation, Inc.' + +# The short X.Y version +version = '' +# The full version, including alpha/beta/rc tags +release = '3.2.1' + + +# -- General configuration --------------------------------------------------- + +# If your documentation needs a minimal Sphinx version, state it here. +# +# needs_sphinx = '1.0' + +# Add any Sphinx extension module names here, as strings. They can be +# extensions coming with Sphinx (named 'sphinx.ext.*') or your custom +# ones. +extensions = [ + 'sphinx.ext.autodoc', + 'sphinx.ext.autosummary', + 'sphinx.ext.doctest', + 'sphinx.ext.coverage', + 'sphinx_rtd_theme', +] + +# Add any paths that contain templates here, relative to this directory. +templates_path = ['_templates'] + +# The suffix(es) of source filenames. +# You can specify multiple suffix as a list of string: +# +# source_suffix = ['.rst', '.md'] +source_suffix = '.rst' + +# The master toctree document. +master_doc = 'index' + +# The language for content autogenerated by Sphinx. Refer to documentation +# for a list of supported languages. +# +# This is also used if you do content translation via gettext catalogs. +# Usually you set "language" from the command line for these cases. +language = None + +# List of patterns, relative to source directory, that match files and +# directories to ignore when looking for source files. +# This pattern also affects html_static_path and html_extra_path. +exclude_patterns = [] + +# The name of the Pygments (syntax highlighting) style to use. +pygments_style = None + + +# -- Options for HTML output ------------------------------------------------- + +# The theme to use for HTML and HTML Help pages. See the documentation for +# a list of builtin themes. +# +html_theme = 'sphinx_rtd_theme' + +# Theme options are theme-specific and customize the look and feel of a theme +# further. For a list of options available for each theme, see the +# documentation. +# +# html_theme_options = {} + +# Add any paths that contain custom static files (such as style sheets) here, +# relative to this directory. They are copied after the builtin static files, +# so a file named "default.css" will overwrite the builtin "default.css". +html_static_path = [] + +# Custom sidebar templates, must be a dictionary that maps document names +# to template names. +# +# The default sidebars (for documents that don't match any pattern) are +# defined by theme itself. Builtin themes are using these templates by +# default: ``['localtoc.html', 'relations.html', 'sourcelink.html', +# 'searchbox.html']``. +# +# html_sidebars = {} + + +# -- Options for HTMLHelp output --------------------------------------------- + +# Output file base name for HTML help builder. +htmlhelp_basename = 'image_proc_doc' + + +# -- Options for LaTeX output ------------------------------------------------ + +latex_elements = { + # The paper size ('letterpaper' or 'a4paper'). + # + # 'papersize': 'letterpaper', + + # The font size ('10pt', '11pt' or '12pt'). + # + # 'pointsize': '10pt', + + # Additional stuff for the LaTeX preamble. + # + # 'preamble': '', + + # Latex figure (float) alignment + # + # 'figure_align': 'htbp', +} + + +# -- Options for manual page output ------------------------------------------ + +# One entry per manual page. List of tuples +# (source start file, name, description, authors, manual section). +man_pages = [ + (master_doc, 'stereo_image_proc', 'stereo_image_proc Documentation', + [author], 1) +] + + +# -- Options for Texinfo output ---------------------------------------------- + +# Grouping the document tree into Texinfo files. List of tuples +# (source start file, target name, title, author, +# dir menu entry, description, category) +texinfo_documents = [ + (master_doc, 'stereo_image_proc', 'stereo_image_proc Documentation', + author, 'stereo_image_proc', 'ROS 2 components for stereo image processing.', + 'Miscellaneous'), +] + + +# -- Options for Epub output ------------------------------------------------- + +# Bibliographic Dublin Core info. +epub_title = project + +# The unique identifier of the text. This can be a ISBN number +# or the project homepage. +# +# epub_identifier = '' + +# A unique identification for the text. +# +# epub_uid = '' + +# A list of files that should not be packed into the epub file. +epub_exclude_files = ['search.html'] + + +# -- Extension configuration ------------------------------------------------- + +autoclass_content = 'both' + +autodoc_default_options = { + 'members': True, # document members + 'undoc-members': True, # also document members without documentation +} diff --git a/stereo_image_proc/doc/configuration.rst b/stereo_image_proc/doc/configuration.rst new file mode 100644 index 000000000..c5d5710e7 --- /dev/null +++ b/stereo_image_proc/doc/configuration.rst @@ -0,0 +1,91 @@ +.. _Configuration: + +Configuration +============= + +Overview +-------- + +The ``stereo_image_proc.launch.py`` launch file provides a full example and +performs rectification and de-mosaicing of raw stereo camera image pairs. +It also perform stereo processing to generate disparity images and point clouds. + +In topic names below, left and right are remapped using the ``left_namespace`` +and ``right_namespace`` launch file parameters. + +|stereo| + +.. |stereo| image:: images/stereo_image_proc.png + :width: 75% + +All processing is on demand. Color processing is performed only if there is a +subscriber to a color topic. Rectification is performed only if there is a +subscriber to a rectified topic. Disparities and point clouds are generated +only if there is a subscriber to a relevant topic. While there are no subscribers +to output topics, the nodes unsubscribe from the ``image_raw`` and +``camera_info`` topics. + +Point clouds are generated in the optical frame of the left imager +(X Right, Y Down, Z out): + +|stereo_frames| + +.. |stereo_frames| image:: images/stereo_frames.png + :width: 50% + + +Quick Start +----------- + +Make sure your stereo camera driver(s) are running. You can check +``ros2 topic list | grep image_raw`` to see the available raw image +topics from compatible drivers. + +To get rectified and/or colorized image streams, you need to launch the +``stereo_image_proc.launch.py`` launch file. If you are running on a +robot, it's probably best to run the launch file there. For example, +if your stereo camera driver(s) are publishing: + +.. code-block:: + + /stereo/left/image_raw + /stereo/left/camera_info + /stereo/right/image_raw + /stereo/right/camera_info + +you could do: + +.. code-block:: bash + + ros2 launch stereo_image_proc stereo_image_proc.launch.py namespace:=stereo + +Notice that we add the ``stereo`` namespace. Internally the launch file +also has a ``left_namespace`` and ``right_namespace`` which default to +``left`` and ``right``. Using this launch file is equivalent to Launching +two instances of the ``image_proc/image_proc.launch.py`` launch file in the +``stereo/left`` and ``stereo/right`` namespaces, with the addition of +the stereo outputs: disparity images (``stereo/disparity`` topic) and point +clouds (``stereo/points2`` topic) + +In a separate terminal (on your home machine, if you are running on a robot): + +.. code-block:: bash + + $ ros2 run image_view image_view image:=/stereo/left/image_rect_color + +This will display a rectified color image from the left stereo camera. +Alternatively, stereo_view will show you both the left and right images +as well as a color-mapped disparity image: + +.. code-block:: bash + + $ ros2 run image_view stereo_view stereo:=/stereo image:=image_rect_color + +To view point clouds, you can use ``rviz2``. + +Choosing Good Stereo Parameters +------------------------------- + +There is an extensive tutorial on the +`ROS 1 wiki `, +however not all components exist in ROS 2 yet. diff --git a/stereo_image_proc/doc/images/disparity.jpg b/stereo_image_proc/doc/images/disparity.jpg new file mode 100644 index 000000000..7b053e012 Binary files /dev/null and b/stereo_image_proc/doc/images/disparity.jpg differ diff --git a/stereo_image_proc/doc/images/raw.png b/stereo_image_proc/doc/images/raw.png new file mode 100644 index 000000000..89954f9b3 Binary files /dev/null and b/stereo_image_proc/doc/images/raw.png differ diff --git a/stereo_image_proc/doc/images/rectified.png b/stereo_image_proc/doc/images/rectified.png new file mode 100644 index 000000000..a00517fac Binary files /dev/null and b/stereo_image_proc/doc/images/rectified.png differ diff --git a/stereo_image_proc/doc/images/stereo_frames.png b/stereo_image_proc/doc/images/stereo_frames.png new file mode 100644 index 000000000..733149ea7 Binary files /dev/null and b/stereo_image_proc/doc/images/stereo_frames.png differ diff --git a/stereo_image_proc/doc/images/stereo_image_proc.png b/stereo_image_proc/doc/images/stereo_image_proc.png new file mode 100644 index 000000000..e54db0dc3 Binary files /dev/null and b/stereo_image_proc/doc/images/stereo_image_proc.png differ diff --git a/stereo_image_proc/doc/index.rst b/stereo_image_proc/doc/index.rst new file mode 100644 index 000000000..c450183fa --- /dev/null +++ b/stereo_image_proc/doc/index.rst @@ -0,0 +1,58 @@ +Overview +======== + +This package contains a number of ROS 2 components, nodes, and launch files +for stereo image processing. + +If porting from ROS 1, please note that the ``stereo_image_proc`` node no +longer exists and instead you should use ``stereo_image_proc.launch.py``. +See the :ref:`Configuration` page for more detail. + +``DisparityNode`` can compute disparity images from incoming stereo +pairs using +`OpenCV's block matching `_ +algorithm. These are best inspected +using ``stereo_view`` which is available in the ``image_view`` package. + +``PointCloudNode`` can produce point clouds, which you can view in ``rviz``, +and process with PCL. + +The image below shows the **left/image_raw** and **right_image_raw**. +These are the raw images from each camera. + +|raw| + +.. |raw| image:: images/raw.png + +Below are the **left/image_rect_color** and **right/image_rect_color**. +These are the rectified images from each camera. +The red lines show that the same point in the real world +lies on the same horizontal line in each rectified image. + +|rectified| + +.. |rectified| image:: images/rectified.png + +The resulting disparity image is shown below, +viewed with ``stereo_view`` from the ``image_view`` package. + +|disparity| + +.. |disparity| image:: images/disparity.jpg + + +.. toctree:: + :maxdepth: 2 + + self + components + configuration + tutorials + changes + stereo_image_proc + +Indices and tables +================== + +* :ref:`genindex` +* :ref:`search` diff --git a/stereo_image_proc/doc/mainpage.dox b/stereo_image_proc/doc/mainpage.dox deleted file mode 100644 index 39b89a1e2..000000000 --- a/stereo_image_proc/doc/mainpage.dox +++ /dev/null @@ -1,14 +0,0 @@ -/** - -@mainpage - -@htmlinclude manifest.html - -@b stereo_image_proc contains a node for performing rectification and -color processing on the raw images produced by a pair of stereo cameras. -It also produces 3d stereo outputs - the disparity image and point cloud. -See http://www.ros.org/wiki/stereo_image_proc for documentation. - -Currently this package has no public code API. - -*/ diff --git a/stereo_image_proc/doc/stereo_frames.svg b/stereo_image_proc/doc/stereo_frames.svg deleted file mode 100644 index bac9f92cd..000000000 --- a/stereo_image_proc/doc/stereo_frames.svg +++ /dev/null @@ -1,1052 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - image/svg+xml - - - - - - - - - - - - - - - - - - - - - - - - - - X - Y - Z - - - - - - - - - - - - - - - - - - - - - - - - LeftImager - RightImager - Point Cloud - - - diff --git a/stereo_image_proc/doc/tutorials.rst b/stereo_image_proc/doc/tutorials.rst new file mode 100644 index 000000000..3293b7a72 --- /dev/null +++ b/stereo_image_proc/doc/tutorials.rst @@ -0,0 +1,47 @@ +Tutorials +========= + +.. _Launch stereo_image_proc Components: + +Launching stereo_image_proc Components +-------------------------------------- +While each of the components is available as a ROS 2 node, the +recommended way to build pipelines is using the components as +this will save overhead by not having to serialize messages +between components. + +For a detailed example, see ``stereo_image_proc.launch.py``. + +Using Compressed Image Transport +-------------------------------- +All of the components and nodes in ``stereo_image_proc`` support +``image_transport``. This allows a subscriber to specify the transport to +be used. By default, this is ``raw``, which means an uncompressed +``sensor_msgs/Image``. When transmitting images over limited bandwidth +networks, such as WiFi, it can be helpful to use ``compressed`` format. + +.. code-block:: bash + + $ ros2 run stereo_image_proc disparity_node --ros-args -p image_transport:=compressed + +Remapping camera_info Topics +---------------------------- +When a ``camera_info`` topic is needed, an image_transport camera subscriber +is typically used. ROS 2 convention for naming ``camera_info`` topics is: + + * camera/image - an image in namespace ``camera``. + * camera/camera_info - the associated camera info + +So if a node subscribes to a topic called ``image``, and the user remaps this +to ``my_camera/image``, then the associated camera info will be automatically +remapped to ``mycamera/camera_info``. + +Most ROS 2 camera drivers will follow the convention, but occasionally they do +not. In this case, you will have to manually remap the camera_info - but due +to the way that ROS 2 remapping works you have to use the fully resolved +camera info topic. An example: + + * ``image`` is remapped to ``my_camera/image``. + * The fully resolved name for the camera info is now ``my_camera/camera_info``. + * If your camera driver actually publishes ``another_ns/camera_info``, then + you would have to remap ``my_camera/camera_info`` to ``another_ns/camera_info``. diff --git a/stereo_image_proc/launch/stereo_image_proc.launch.py b/stereo_image_proc/launch/stereo_image_proc.launch.py index afe9c0cb2..8c2c81cba 100644 --- a/stereo_image_proc/launch/stereo_image_proc.launch.py +++ b/stereo_image_proc/launch/stereo_image_proc.launch.py @@ -53,6 +53,7 @@ def generate_launch_description(): ComposableNode( package='stereo_image_proc', plugin='stereo_image_proc::DisparityNode', + namespace=LaunchConfiguration('namespace'), parameters=[{ 'approximate_sync': LaunchConfiguration('approximate_sync'), 'stereo_algorithm': LaunchConfiguration('stereo_algorithm'), @@ -80,6 +81,7 @@ def generate_launch_description(): ComposableNode( package='stereo_image_proc', plugin='stereo_image_proc::PointCloudNode', + namespace=LaunchConfiguration('namespace'), parameters=[{ 'approximate_sync': LaunchConfiguration('approximate_sync'), 'avoid_point_cloud_padding': LaunchConfiguration('avoid_point_cloud_padding'), @@ -121,6 +123,10 @@ def generate_launch_description(): name='launch_image_proc', default_value='True', description='Whether to launch debayer and rectify nodes from image_proc.' ), + DeclareLaunchArgument( + name='namespace', default_value='', + description='Namespace for all components loaded' + ), DeclareLaunchArgument( name='left_namespace', default_value='left', description='Namespace for the left camera' @@ -223,24 +229,26 @@ def generate_launch_description(): ), GroupAction( [ - PushRosNamespace(LaunchConfiguration('left_namespace')), + PushRosNamespace(LaunchConfiguration('namespace')), IncludeLaunchDescription( PythonLaunchDescriptionSource([ FindPackageShare('image_proc'), '/launch/image_proc.launch.py' ]), - launch_arguments={'container': LaunchConfiguration('container')}.items() + launch_arguments={'container': LaunchConfiguration('container'), + 'namespace': LaunchConfiguration('left_namespace')}.items() ), ], condition=IfCondition(LaunchConfiguration('launch_image_proc')), ), GroupAction( [ - PushRosNamespace(LaunchConfiguration('right_namespace')), + PushRosNamespace(LaunchConfiguration('namespace')), IncludeLaunchDescription( PythonLaunchDescriptionSource([ FindPackageShare('image_proc'), '/launch/image_proc.launch.py' ]), - launch_arguments={'container': LaunchConfiguration('container')}.items() + launch_arguments={'container': LaunchConfiguration('container'), + 'namespace': LaunchConfiguration('right_namespace')}.items() ), ], condition=IfCondition(LaunchConfiguration('launch_image_proc')),