[ros-image-pipeline] 01/01: New upstream version 1.12.20
Leopold Palomo-Avellaneda
leo at alaxarxa.net
Fri Jul 14 11:25:55 UTC 2017
This is an automated email from the git hooks/post-receive script.
lepalom-guest pushed a commit to branch master
in repository ros-image-pipeline.
commit c2415af5cc8ffdf7950fbb380c3236fb0c1459ad
Author: Leopold Palomo-Avellaneda <leo at alaxarxa.net>
Date: Tue Jul 11 11:21:08 2017 +0200
New upstream version 1.12.20
---
.gitignore | 1 +
.travis.yml | 66 ++
README.rst | 7 +
camera_calibration/CHANGELOG.rst | 143 +++
camera_calibration/CMakeLists.txt | 31 +
camera_calibration/button.jpg | Bin 0 -> 21110 bytes
camera_calibration/doc/conf.py | 201 ++++
camera_calibration/doc/index.rst | 18 +
camera_calibration/mainpage.dox | 59 +
camera_calibration/nodes/cameracalibrator.py | 467 ++++++++
camera_calibration/nodes/cameracheck.py | 225 ++++
camera_calibration/package.xml | 30 +
camera_calibration/rosdoc.yaml | 4 +
camera_calibration/scripts/tarfile_calibration.py | 235 ++++
camera_calibration/setup.py | 9 +
.../src/camera_calibration/__init__.py | 0
.../src/camera_calibration/calibrator.py | 1139 ++++++++++++++++++++
camera_calibration/test/directed.py | 274 +++++
camera_calibration/test/multiple_boards.py | 87 ++
depth_image_proc/CHANGELOG.rst | 110 ++
depth_image_proc/CMakeLists.txt | 40 +
.../include/depth_image_proc/depth_conversions.h | 101 ++
.../include/depth_image_proc/depth_traits.h | 71 ++
depth_image_proc/mainpage.dox | 26 +
depth_image_proc/nodelet_plugins.xml | 73 ++
depth_image_proc/package.xml | 46 +
depth_image_proc/src/nodelets/convert_metric.cpp | 140 +++
depth_image_proc/src/nodelets/crop_foremost.cpp | 142 +++
depth_image_proc/src/nodelets/disparity.cpp | 189 ++++
depth_image_proc/src/nodelets/point_cloud_xyz.cpp | 138 +++
.../src/nodelets/point_cloud_xyz_radial.cpp | 235 ++++
depth_image_proc/src/nodelets/point_cloud_xyzi.cpp | 316 ++++++
.../src/nodelets/point_cloud_xyzi_radial.cpp | 313 ++++++
.../src/nodelets/point_cloud_xyzrgb.cpp | 345 ++++++
depth_image_proc/src/nodelets/register.cpp | 263 +++++
image_pipeline/CHANGELOG.rst | 59 +
image_pipeline/CMakeLists.txt | 4 +
image_pipeline/package.xml | 29 +
image_proc/CHANGELOG.rst | 95 ++
image_proc/CMakeLists.txt | 60 ++
image_proc/cfg/CropDecimate.cfg | 35 +
image_proc/cfg/Debayer.cfg | 26 +
image_proc/cfg/Rectify.cfg | 22 +
image_proc/cfg/Resize.cfg | 37 +
.../include/image_proc/advertisement_checker.h | 61 ++
image_proc/include/image_proc/processor.h | 77 ++
image_proc/launch/image_proc.launch | 29 +
image_proc/mainpage.dox | 12 +
image_proc/nodelet_plugins.xml | 48 +
image_proc/package.xml | 39 +
.../src/libimage_proc/advertisement_checker.cpp | 92 ++
image_proc/src/libimage_proc/processor.cpp | 130 +++
image_proc/src/nodelets/crop_decimate.cpp | 346 ++++++
image_proc/src/nodelets/crop_non_zero.cpp | 146 +++
image_proc/src/nodelets/debayer.cpp | 276 +++++
image_proc/src/nodelets/edge_aware.cpp | 804 ++++++++++++++
image_proc/src/nodelets/edge_aware.h | 49 +
image_proc/src/nodelets/rectify.cpp | 170 +++
image_proc/src/nodelets/resize.cpp | 174 +++
image_proc/src/nodes/image_proc.cpp | 97 ++
image_proc/test/CMakeLists.txt | 5 +
image_proc/test/rostest.cpp | 99 ++
image_proc/test/test_bayer.xml | 10 +
image_proc/test/test_rectify.cpp | 205 ++++
image_proc/test/test_rectify.xml | 9 +
image_publisher/CHANGELOG.rst | 45 +
image_publisher/CMakeLists.txt | 29 +
image_publisher/cfg/ImagePublisher.cfg | 44 +
image_publisher/nodelet_plugins.xml | 7 +
image_publisher/package.xml | 35 +
image_publisher/src/node/image_publisher.cpp | 57 +
.../src/nodelet/image_publisher_nodelet.cpp | 194 ++++
image_rotate/CHANGELOG.rst | 82 ++
image_rotate/CMakeLists.txt | 32 +
image_rotate/cfg/ImageRotate.cfg | 59 +
image_rotate/mainpage.dox | 26 +
image_rotate/nodelet_plugins.xml | 7 +
image_rotate/package.xml | 55 +
image_rotate/src/node/image_rotate.cpp | 54 +
image_rotate/src/nodelet/image_rotate_nodelet.cpp | 281 +++++
image_view/CHANGELOG.rst | 177 +++
image_view/CMakeLists.txt | 90 ++
image_view/cfg/ImageView.cfg | 29 +
image_view/mainpage.dox | 11 +
image_view/nodelet_plugins.xml | 11 +
image_view/package.xml | 46 +
image_view/rosdoc.yaml | 4 +
image_view/scripts/extract_images_sync | 99 ++
image_view/src/nodelets/disparity_nodelet.cpp | 439 ++++++++
image_view/src/nodelets/image_nodelet.cpp | 222 ++++
image_view/src/nodelets/window_thread.cpp | 52 +
image_view/src/nodelets/window_thread.h | 44 +
image_view/src/nodes/disparity_view.cpp | 54 +
image_view/src/nodes/extract_images.cpp | 154 +++
image_view/src/nodes/image_saver.cpp | 226 ++++
image_view/src/nodes/image_view.cpp | 212 ++++
image_view/src/nodes/stereo_view.cpp | 573 ++++++++++
image_view/src/nodes/video_recorder.cpp | 141 +++
stereo_image_proc/CHANGELOG.rst | 86 ++
stereo_image_proc/CMakeLists.txt | 50 +
stereo_image_proc/cfg/Disparity.cfg | 39 +
stereo_image_proc/doc/mainpage.dox | 14 +
stereo_image_proc/doc/stereo_frames.svg | 1052 ++++++++++++++++++
.../include/stereo_image_proc/processor.h | 303 ++++++
stereo_image_proc/launch/stereo_image_proc.launch | 36 +
stereo_image_proc/nodelet_plugins.xml | 11 +
stereo_image_proc/package.xml | 40 +
stereo_image_proc/rosdoc.yaml | 4 +
.../src/libstereo_image_proc/processor.cpp | 317 ++++++
stereo_image_proc/src/nodelets/disparity.cpp | 250 +++++
stereo_image_proc/src/nodelets/point_cloud2.cpp | 272 +++++
stereo_image_proc/src/nodes/stereo_image_proc.cpp | 149 +++
wiki_files/dcam-driver.svg | 249 +++++
wiki_files/image_proc.svg | 335 ++++++
wiki_files/image_proc_dual.svg | 335 ++++++
wiki_files/rospack_nosubdirs | 0
wiki_files/stereo_image_proc.svg | 477 ++++++++
wiki_files/stereo_image_proc_stereo.svg | 389 +++++++
wiki_files/stereocam-driver.svg | 290 +++++
wiki_files/stoc.svg | 312 ++++++
120 files changed, 17390 insertions(+)
diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..72723e5
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1 @@
+*pyc
diff --git a/.travis.yml b/.travis.yml
new file mode 100644
index 0000000..fa58ee9
--- /dev/null
+++ b/.travis.yml
@@ -0,0 +1,66 @@
+sudo: required
+dist: trusty
+language: generic
+env:
+ - OPENCV_VERSION=2 ROS_DISTRO=indigo
+ - OPENCV_VERSION=3 ROS_DISTRO=indigo
+ - OPENCV_VERSION=2 ROS_DISTRO=jade
+ - OPENCV_VERSION=3 ROS_DISTRO=jade
+# Version of 4 does not make sense but it's to pass the test below.
+# Disabled as Travis does not support Xenial
+# - OPENCV_VERSION=4 ROS_DISTRO=kinetic
+# Install system dependencies, namely ROS.
+before_install:
+ # Define some config vars.
+ - export ROS_CI_DESKTOP=`lsb_release -cs` # e.g. [precise|trusty]
+ - export CI_SOURCE_PATH=$(pwd)
+ - export REPOSITORY_NAME=${PWD##*/}
+ - export CATKIN_OPTIONS=$CI_SOURCE_PATH/catkin.options
+ - export ROS_PARALLEL_JOBS='-j8 -l6'
+ # Install ROS
+ - sudo sh -c "echo \"deb http://packages.ros.org/ros-shadow-fixed/ubuntu $ROS_CI_DESKTOP main\" > /etc/apt/sources.list.d/ros-latest.list"
+ - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
+ - sudo apt-get update -qq
+ # Install ROS
+ - sudo apt-get install -y python-catkin-pkg python-catkin-tools python-rosdep python-wstool ros-$ROS_DISTRO-catkin
+ - source /opt/ros/$ROS_DISTRO/setup.bash
+ # Setup for rosdep
+ - sudo rosdep init
+ - rosdep update
+
+# Create a catkin workspace with the package under test.
+install:
+ - mkdir -p ~/catkin_ws/src
+
+ # Add the package under test to the workspace.
+ - cd ~/catkin_ws/src
+ - ln -s $CI_SOURCE_PATH . # Link the repo we are testing to the new workspace
+
+ - if [ $OPENCV_VERSION == 3 ]; then sed -i 's at libopencv-dev@opencv3@' $REPOSITORY_NAME/*/package.xml ; fi
+
+# Install all dependencies, using wstool and rosdep.
+# wstool looks for a ROSINSTALL_FILE defined in before_install.
+before_script:
+ # source dependencies: install using wstool.
+ - cd ~/catkin_ws/src
+ - wstool init
+ - if [[ -f $ROSINSTALL_FILE ]] ; then wstool merge $ROSINSTALL_FILE ; fi
+ - wstool up
+
+ # package depdencies: install using rosdep.
+ - cd ~/catkin_ws
+ - rosdep install -q -y -r --from-paths src --ignore-src --rosdistro $ROS_DISTRO
+
+# Compile and test.
+script:
+ - source /opt/ros/$ROS_DISTRO/setup.bash
+ - cd ~/catkin_ws
+ - catkin build -p1 -j1 --no-status
+ - catkin run_tests -p1 -j1
+ - catkin_test_results --all build
+ - catkin clean -b --yes
+ - catkin config --install
+ - catkin build -p1 -j1 --no-status
+after_failure:
+ - find ${HOME}/.ros/test_results -type f -exec echo "== {} ==" \; -exec cat {} \;
+ - for file in ${HOME}/.ros/log/rostest-*; do echo "=== $file ==="; cat $file; done
diff --git a/README.rst b/README.rst
new file mode 100644
index 0000000..c09f142
--- /dev/null
+++ b/README.rst
@@ -0,0 +1,7 @@
+image_pipeline
+==============
+
+.. image:: https://travis-ci.org/ros-perception/image_pipeline.svg?branch=indigo
+ :target: https://travis-ci.org/ros-perception/image_pipeline
+
+This package fills the gap between getting raw images from a camera driver and higher-level vision processing.
diff --git a/camera_calibration/CHANGELOG.rst b/camera_calibration/CHANGELOG.rst
new file mode 100644
index 0000000..71711c4
--- /dev/null
+++ b/camera_calibration/CHANGELOG.rst
@@ -0,0 +1,143 @@
+1.12.20 (2017-04-30)
+--------------------
+* properly save bytes buffer as such
+ This is useful for Python 3 and fixes `#256 <https://github.com/ros-perception/image_pipeline/issues/256>`_.
+* Get tests slightly looser.
+ OpenCV 3.2 gives slightly different results apparently.
+* Use floor division where necessary. (`#247 <https://github.com/ros-perception/image_pipeline/issues/247>`_)
+* Fix and Improve Camera Calibration Checker Node (`#254 <https://github.com/ros-perception/image_pipeline/issues/254>`_)
+ * Fix according to calibrator.py API
+ * Add approximate to cameracheck
+* Force first corner off chessboard to be uppler left.
+ Fixes `#140 <https://github.com/ros-perception/image_pipeline/issues/140>`_
+* fix doc jobs
+ This is a proper fix for `#233 <https://github.com/ros-perception/image_pipeline/issues/233>`_
+* During stereo calibration check that the number of corners detected in the left and right images are the same. This fixes `ros-perception/image_pipeline#225 <https://github.com/ros-perception/image_pipeline/issues/225>`_
+* Contributors: Léonard Gérard, Martin Peris, Vincent Rabaud, hgaiser
+
+1.12.19 (2016-07-24)
+--------------------
+* Fix array check in camerachecky.py
+ This closes `#205 <https://github.com/ros-perception/image_pipeline/issues/205>`_
+* Contributors: Vincent Rabaud
+
+1.12.18 (2016-07-12)
+--------------------
+
+1.12.17 (2016-07-11)
+--------------------
+* fix typo np -> numpy
+* fix failing tests
+* Contributors: Shingo Kitagawa, Vincent Rabaud
+
+1.12.16 (2016-03-19)
+--------------------
+* clean OpenCV dependency in package.xml
+* Contributors: Vincent Rabaud
+
+1.12.15 (2016-01-17)
+--------------------
+* better 16 handling in mkgray
+ This re-uses `#150 <https://github.com/ros-perception/image_pipeline/issues/150>`_ and therefore closes `#150 <https://github.com/ros-perception/image_pipeline/issues/150>`_
+* fix OpenCV2 compatibility
+* fix tests with OpenCV3
+* [Calibrator]: add yaml file with calibration data in output
+* Contributors: Vincent Rabaud, sambrose
+
+1.12.14 (2015-07-22)
+--------------------
+* remove camera_hammer and install Python nodes properly
+ camera_hammer was just a test for camera info, nothing to do with
+ calibration. Plus the test was basic.
+* Correct three errors that prevented the node to work properly.
+* Contributors: Filippo Basso, Vincent Rabaud
+
+1.12.13 (2015-04-06)
+--------------------
+* replace Queue by deque of fixed size for simplicity
+ That is a potential fix for `#112 <https://github.com/ros-perception/image_pipeline/issues/112>`_
+* Contributors: Vincent Rabaud
+
+1.12.12 (2014-12-31)
+--------------------
+* try to improve `#112 <https://github.com/ros-perception/image_pipeline/issues/112>`_
+* Contributors: Vincent Rabaud
+
+1.12.11 (2014-10-26)
+--------------------
+
+1.12.10 (2014-09-28)
+--------------------
+* Update calibrator.py
+ bugfix: stereo calibrator crashed after the signature of the method for the computation of the epipolar error changed but the function call was not updated
+* Contributors: Volker Grabe
+
+1.12.9 (2014-09-21)
+-------------------
+* fix bad Python
+* only analyze the latest image
+ fixes `#97 <https://github.com/ros-perception/image_pipeline/issues/97>`_
+* flips width and height during resize to give correct aspect ratio
+* Contributors: Russell Toris, Vincent Rabaud
+
+1.12.8 (2014-08-19)
+-------------------
+* install scripts in the local bin (they are now rosrun-able again)
+ fixes `#93 <https://github.com/ros-perception/image_pipeline/issues/93>`_
+* fix default Constructor for OpenCV flags
+ this does not change anything in practice as the flag is set by the node.
+ It just fixes the test.
+* Contributors: Vincent Rabaud
+
+1.12.6 (2014-07-27)
+-------------------
+* make sure the GUI is started in its processing thread and fix a typo
+ This fully fixes `#85 <https://github.com/ros-perception/image_pipeline/issues/85>`_
+* fix bad call to save an image
+* have display be in its own thread
+ that could be a fix for `#85 <https://github.com/ros-perception/image_pipeline/issues/85>`_
+* fix bad usage of Numpy
+ fixes `#89 <https://github.com/ros-perception/image_pipeline/issues/89>`_
+* fix asymmetric circle calibration
+ fixes `#35 <https://github.com/ros-perception/image_pipeline/issues/35>`_
+* add more tests
+* improve unittests to include all patterns
+* install Python scripts properly
+ and fixes `#86 <https://github.com/ros-perception/image_pipeline/issues/86>`_
+* fix typo that leads to segfault
+ fixes `#84 <https://github.com/ros-perception/image_pipeline/issues/84>`_
+* also print self.report() on calibrate ... allows to use the params without having to commit them (e.g. for extrensic calibration between to cameras not used as stereo pair)
+* fixes `#76 <https://github.com/ros-perception/image_pipeline/issues/76>`_
+ Move Python approximate time synchronizer to ros_comm
+* remove all trace of cv in Python (use cv2)
+* remove deprecated file (as mentioned in its help)
+* fixes `#25 <https://github.com/ros-perception/image_pipeline/issues/25>`_
+ This is just removing deprecated options that were around since diamondback
+* fixes `#74 <https://github.com/ros-perception/image_pipeline/issues/74>`_
+ calibrator.py is now using the cv2 only API when using cv_bridge.
+ The API got changed too but it seems to only be used internally.
+* Contributors: Vincent Rabaud, ahb
+
+1.12.5 (2014-05-11)
+-------------------
+* Fix `#68 <https://github.com/ros-perception/image_pipeline/issues/68>`_: StringIO issues in calibrator.py
+* fix architecture independent
+* Contributors: Miquel Massot, Vincent Rabaud
+
+1.12.4 (2014-04-28)
+-------------------
+
+1.12.3 (2014-04-12)
+-------------------
+* camera_calibration: Fix Python import order
+* Contributors: Scott K Logan
+
+1.12.2 (2014-04-08)
+-------------------
+* Fixes a typo on stereo camera info service calls
+ Script works after correcting the call names.
+* Contributors: JoonasMelin
+
+1.11.4 (2013-11-23 13:10:55 +0100)
+----------------------------------
+- add visualization during calibration and several calibration flags (#48)
diff --git a/camera_calibration/CMakeLists.txt b/camera_calibration/CMakeLists.txt
new file mode 100644
index 0000000..469d3b8
--- /dev/null
+++ b/camera_calibration/CMakeLists.txt
@@ -0,0 +1,31 @@
+cmake_minimum_required(VERSION 2.8)
+project(camera_calibration)
+
+find_package(catkin REQUIRED)
+catkin_package()
+
+catkin_python_setup()
+
+if(CATKIN_ENABLE_TESTING)
+ # Unit test of calibrator.py
+ catkin_add_nosetests(test/directed.py)
+
+ # Tests simple calibration dataset
+ catkin_download_test_data(camera_calibration.tar.gz http://download.ros.org/data/camera_calibration/camera_calibration.tar.gz
+ DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/tests
+ MD5 6da43ea314640a4c15dd7a90cbc3aee0
+ )
+
+ # Tests multiple checkerboards
+ catkin_download_test_data(multi_board_calibration.tar.gz http://download.ros.org/data/camera_calibration/multi_board_calibration.tar.gz
+ DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/tests
+ MD5 ddc0f69582d140e33f9d3bfb681956bb
+ )
+ catkin_add_nosetests(test/multiple_boards.py)
+endif()
+
+catkin_install_python(PROGRAMS nodes/cameracalibrator.py
+ nodes/cameracheck.py
+ scripts/tarfile_calibration.py
+ DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
diff --git a/camera_calibration/button.jpg b/camera_calibration/button.jpg
new file mode 100644
index 0000000..494a6f6
Binary files /dev/null and b/camera_calibration/button.jpg differ
diff --git a/camera_calibration/doc/conf.py b/camera_calibration/doc/conf.py
new file mode 100644
index 0000000..82ad7fe
--- /dev/null
+++ b/camera_calibration/doc/conf.py
@@ -0,0 +1,201 @@
+# -*- coding: utf-8 -*-
+#
+# camera_calibration documentation build configuration file, created by
+# sphinx-quickstart on Mon Jun 1 14:21:53 2009.
+#
+# This file is execfile()d with the current directory set to its containing dir.
+#
+# Note that not all possible configuration values are present in this
+# autogenerated file.
+#
+# All configuration values have a default; values that are commented out
+# serve to show the default.
+
+import sys, os
+
+# 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.
+#sys.path.append(os.path.abspath('.'))
+
+# -- General configuration -----------------------------------------------------
+
+# 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.doctest', 'sphinx.ext.intersphinx', 'sphinx.ext.pngmath']
+
+# Add any paths that contain templates here, relative to this directory.
+templates_path = ['_templates']
+
+# The suffix of source filenames.
+source_suffix = '.rst'
+
+# The encoding of source files.
+#source_encoding = 'utf-8'
+
+# The master toctree document.
+master_doc = 'index'
+
+# General information about the project.
+project = 'camera_calibration'
+copyright = '2009, Willow Garage, Inc.'
+
+# The version info for the project you're documenting, acts as replacement for
+# |version| and |release|, also used in various other places throughout the
+# built documents.
+#
+# The short X.Y version.
+version = '0.1'
+# The full version, including alpha/beta/rc tags.
+release = '0.1.0'
+
+# The language for content autogenerated by Sphinx. Refer to documentation
+# for a list of supported languages.
+#language = None
+
+# There are two options for replacing |today|: either, you set today to some
+# non-false value, then it is used:
+#today = ''
+# Else, today_fmt is used as the format for a strftime call.
+#today_fmt = '%B %d, %Y'
+
+# List of documents that shouldn't be included in the build.
+#unused_docs = []
+
+# List of directories, relative to source directory, that shouldn't be searched
+# for source files.
+exclude_trees = ['_build']
+
+# The reST default role (used for this markup: `text`) to use for all documents.
+#default_role = None
+
+# If true, '()' will be appended to :func: etc. cross-reference text.
+#add_function_parentheses = True
+
+# If true, the current module name will be prepended to all description
+# unit titles (such as .. function::).
+#add_module_names = True
+
+# If true, sectionauthor and moduleauthor directives will be shown in the
+# output. They are ignored by default.
+#show_authors = False
+
+# The name of the Pygments (syntax highlighting) style to use.
+pygments_style = 'sphinx'
+
+# A list of ignored prefixes for module index sorting.
+#modindex_common_prefix = []
+
+
+# -- Options for HTML output ---------------------------------------------------
+
+# The theme to use for HTML and HTML Help pages. Major themes that come with
+# Sphinx are currently 'default' and 'sphinxdoc'.
+html_theme = 'default'
+
+# 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 themes here, relative to this directory.
+#html_theme_path = []
+
+# The name for this set of Sphinx documents. If None, it defaults to
+# "<project> v<release> documentation".
+#html_title = None
+
+# A shorter title for the navigation bar. Default is the same as html_title.
+#html_short_title = None
+
+# The name of an image file (relative to this directory) to place at the top
+# of the sidebar.
+#html_logo = None
+
+# The name of an image file (within the static path) to use as favicon of the
+# docs. This file should be a Windows icon file (.ico) being 16x16 or 32x32
+# pixels large.
+#html_favicon = None
+
+# 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 = ['_static']
+
+# If not '', a 'Last updated on:' timestamp is inserted at every page bottom,
+# using the given strftime format.
+#html_last_updated_fmt = '%b %d, %Y'
+
+# If true, SmartyPants will be used to convert quotes and dashes to
+# typographically correct entities.
+#html_use_smartypants = True
+
+# Custom sidebar templates, maps document names to template names.
+#html_sidebars = {}
+
+# Additional templates that should be rendered to pages, maps page names to
+# template names.
+#html_additional_pages = {}
+
+# If false, no module index is generated.
+#html_use_modindex = True
+
+# If false, no index is generated.
+#html_use_index = True
+
+# If true, the index is split into individual pages for each letter.
+#html_split_index = False
+
+# If true, links to the reST sources are added to the pages.
+#html_show_sourcelink = True
+
+# If true, an OpenSearch description file will be output, and all pages will
+# contain a <link> tag referring to it. The value of this option must be the
+# base URL from which the finished HTML is served.
+#html_use_opensearch = ''
+
+# If nonempty, this is the file name suffix for HTML files (e.g. ".xhtml").
+#html_file_suffix = ''
+
+# Output file base name for HTML help builder.
+htmlhelp_basename = 'camera_calibrationdoc'
+
+
+# -- Options for LaTeX output --------------------------------------------------
+
+# The paper size ('letter' or 'a4').
+#latex_paper_size = 'letter'
+
+# The font size ('10pt', '11pt' or '12pt').
+#latex_font_size = '10pt'
+
+# Grouping the document tree into LaTeX files. List of tuples
+# (source start file, target name, title, author, documentclass [howto/manual]).
+latex_documents = [
+ ('index', 'camera_calibration.tex', 'stereo\\_utils Documentation',
+ 'James Bowman', 'manual'),
+]
+
+# The name of an image file (relative to this directory) to place at the top of
+# the title page.
+#latex_logo = None
+
+# For "manual" documents, if this is true, then toplevel headings are parts,
+# not chapters.
+#latex_use_parts = False
+
+# Additional stuff for the LaTeX preamble.
+#latex_preamble = ''
+
+# Documents to append as an appendix to all manuals.
+#latex_appendices = []
+
+# If false, no module index is generated.
+#latex_use_modindex = True
+
+# Example configuration for intersphinx: refer to the Python standard library.
+intersphinx_mapping = {
+ 'http://docs.python.org/': None,
+ 'http://docs.scipy.org/doc/numpy' : None,
+ 'http://www.ros.org/doc/api/tf/html/python/' : None
+ }
diff --git a/camera_calibration/doc/index.rst b/camera_calibration/doc/index.rst
new file mode 100644
index 0000000..741e4e5
--- /dev/null
+++ b/camera_calibration/doc/index.rst
@@ -0,0 +1,18 @@
+camera_calibration
+==================
+
+The camera_calibration package contains a user-friendly calibration tool,
+cameracalibrator. This tool uses the following Python classes, which
+conveniently hide some of the complexities of using OpenCV's calibration
+process and chessboard detection, and the details of constructing a ROS
+CameraInfo message. These classes are documented here for people who
+need to extend or make a new calibration tool.
+
+For details on the camera model and camera calibration process, see
+http://docs.opencv.org/master/d9/d0c/group__calib3d.html
+
+.. autoclass:: camera_calibration.calibrator.MonoCalibrator
+ :members:
+
+.. autoclass:: camera_calibration.calibrator.StereoCalibrator
+ :members:
diff --git a/camera_calibration/mainpage.dox b/camera_calibration/mainpage.dox
new file mode 100644
index 0000000..dbcfe69
--- /dev/null
+++ b/camera_calibration/mainpage.dox
@@ -0,0 +1,59 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b The camera_calibration package contains tools for calibrating monocular and stereo cameras.
+
+\section codeapi Code API
+
+camera_calibration does not have a code API.
+
+\section rosapi ROS API
+
+List of nodes:
+- \b calibrationnode
+
+<!-- START: copy for each node -->
+
+<hr>
+
+\subsection node_name calibrationnode
+
+calibrationnode subscribes to ROS raw image topics, and presents a
+calibration window. It can run in both monocular and stereo modes.
+The calibration window shows the current images from the cameras,
+highlighting the checkerboard. When the user presses the "CALIBRATE"
+button, the node computes the camera calibration parameters. When the
+user clicks "UPLOAD", the node uploads these new calibration parameters
+to the camera driver using a service call.
+
+\subsubsection Usage
+\verbatim
+$ node_type1 [standard ROS args]
+\endverbatim
+
+\par Example
+
+\verbatim
+$ rosrun camera_calibration cal.py right:=/my_stereo/right/image_raw left:=/my_stereo/left/image_raw left_camera:=/my_stereo/left right_camera:=/my_stereo/right
+\endverbatim
+
+
+\subsubsection topics ROS topics
+
+Subscribes to:
+- \b "left": [sensor_msgs/Image] left raw image topic, for stereo cameras
+- \b "right": [sensor_msgs/Image] left raw image topic, for stereo cameras
+- \b "image": [sensor_msgs/Image] raw image topic, for monocular cameras
+
+Makes service calls to:
+
+\subsubsection services ROS services
+- \b "foo_service": [std_srvs/FooType] description of foo_service
+- \b "camera/set_camera_info": [sensor_msgs/SetCameraInfo] node of the camera for monocular
+- \b "left_camera/set_camera_info": [sensor_msgs/SetCameraInfo] node of the left stereo camera
+- \b "right_camera/set_camera_info": [sensor_msgs/SetCameraInfo] node of the left stereo camera
+
+<!-- END: copy for each node -->
+
+*/
diff --git a/camera_calibration/nodes/cameracalibrator.py b/camera_calibration/nodes/cameracalibrator.py
new file mode 100755
index 0000000..4a1485f
--- /dev/null
+++ b/camera_calibration/nodes/cameracalibrator.py
@@ -0,0 +1,467 @@
+#!/usr/bin/python
+#
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2009, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# * Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above
+# copyright notice, this list of conditions and the following
+# disclaimer in the documentation and/or other materials provided
+# with the distribution.
+# * Neither the name of the Willow Garage nor the names of its
+# contributors may be used to endorse or promote products derived
+# from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+import rospy
+import sensor_msgs.msg
+import sensor_msgs.srv
+import message_filters
+from message_filters import ApproximateTimeSynchronizer
+
+import os
+from collections import deque
+import threading
+import functools
+import time
+
+import cv2
+import numpy
+
+from camera_calibration.calibrator import MonoCalibrator, StereoCalibrator, ChessboardInfo, Patterns
+from std_msgs.msg import String
+from std_srvs.srv import Empty
+
+class DisplayThread(threading.Thread):
+ """
+ Thread that displays the current images
+ It is its own thread so that all display can be done
+ in one thread to overcome imshow limitations and
+ https://github.com/ros-perception/image_pipeline/issues/85
+ """
+ def __init__(self, queue, opencv_calibration_node):
+ threading.Thread.__init__(self)
+ self.queue = queue
+ self.opencv_calibration_node = opencv_calibration_node
+
+ def run(self):
+ cv2.namedWindow("display", cv2.WINDOW_NORMAL)
+ cv2.setMouseCallback("display", self.opencv_calibration_node.on_mouse)
+ cv2.createTrackbar("scale", "display", 0, 100, self.opencv_calibration_node.on_scale)
+ while True:
+ # wait for an image (could happen at the very beginning when the queue is still empty)
+ while len(self.queue) == 0:
+ time.sleep(0.1)
+ im = self.queue[0]
+ cv2.imshow("display", im)
+ k = cv2.waitKey(6) & 0xFF
+ if k in [27, ord('q')]:
+ rospy.signal_shutdown('Quit')
+ elif k == ord('s'):
+ self.opencv_calibration_node.screendump(im)
+
+class ConsumerThread(threading.Thread):
+ def __init__(self, queue, function):
+ threading.Thread.__init__(self)
+ self.queue = queue
+ self.function = function
+
+ def run(self):
+ while True:
+ # wait for an image (could happen at the very beginning when the queue is still empty)
+ while len(self.queue) == 0:
+ time.sleep(0.1)
+ self.function(self.queue[0])
+
+
+class CalibrationNode:
+ def __init__(self, boards, service_check = True, synchronizer = message_filters.TimeSynchronizer, flags = 0,
+ pattern=Patterns.Chessboard, camera_name='', checkerboard_flags = 0):
+ if service_check:
+ # assume any non-default service names have been set. Wait for the service to become ready
+ for svcname in ["camera", "left_camera", "right_camera"]:
+ remapped = rospy.remap_name(svcname)
+ if remapped != svcname:
+ fullservicename = "%s/set_camera_info" % remapped
+ print("Waiting for service", fullservicename, "...")
+ try:
+ rospy.wait_for_service(fullservicename, 5)
+ print("OK")
+ except rospy.ROSException:
+ print("Service not found")
+ rospy.signal_shutdown('Quit')
+
+ self._boards = boards
+ self._calib_flags = flags
+ self._checkerboard_flags = checkerboard_flags
+ self._pattern = pattern
+ self._camera_name = camera_name
+ lsub = message_filters.Subscriber('left', sensor_msgs.msg.Image)
+ rsub = message_filters.Subscriber('right', sensor_msgs.msg.Image)
+ ts = synchronizer([lsub, rsub], 4)
+ ts.registerCallback(self.queue_stereo)
+
+ msub = message_filters.Subscriber('image', sensor_msgs.msg.Image)
+ msub.registerCallback(self.queue_monocular)
+
+ self.set_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("camera"),
+ sensor_msgs.srv.SetCameraInfo)
+ self.set_left_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("left_camera"),
+ sensor_msgs.srv.SetCameraInfo)
+ self.set_right_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("right_camera"),
+ sensor_msgs.srv.SetCameraInfo)
+
+ self.q_mono = deque([], 1)
+ self.q_stereo = deque([], 1)
+
+ self.c = None
+
+ mth = ConsumerThread(self.q_mono, self.handle_monocular)
+ mth.setDaemon(True)
+ mth.start()
+
+ sth = ConsumerThread(self.q_stereo, self.handle_stereo)
+ sth.setDaemon(True)
+ sth.start()
+
+ def redraw_stereo(self, *args):
+ pass
+ def redraw_monocular(self, *args):
+ pass
+
+ def queue_monocular(self, msg):
+ self.q_mono.append(msg)
+
+ def queue_stereo(self, lmsg, rmsg):
+ self.q_stereo.append((lmsg, rmsg))
+
+ def handle_monocular(self, msg):
+ if self.c == None:
+ if self._camera_name:
+ self.c = MonoCalibrator(self._boards, self._calib_flags, self._pattern, name=self._camera_name,
+ checkerboard_flags=self._checkerboard_flags)
+ else:
+ self.c = MonoCalibrator(self._boards, self._calib_flags, self._pattern,
+ checkerboard_flags=self.checkerboard_flags)
+
+ # This should just call the MonoCalibrator
+ drawable = self.c.handle_msg(msg)
+ self.displaywidth = drawable.scrib.shape[1]
+ self.redraw_monocular(drawable)
+
+ def handle_stereo(self, msg):
+ if self.c == None:
+ if self._camera_name:
+ self.c = StereoCalibrator(self._boards, self._calib_flags, self._pattern, name=self._camera_name,
+ checkerboard_flags=self._checkerboard_flags)
+ else:
+ self.c = StereoCalibrator(self._boards, self._calib_flags, self._pattern,
+ checkerboard_flags=self._checkerboard_flags)
+
+ drawable = self.c.handle_msg(msg)
+ self.displaywidth = drawable.lscrib.shape[1] + drawable.rscrib.shape[1]
+ self.redraw_stereo(drawable)
+
+
+ def check_set_camera_info(self, response):
+ if response.success:
+ return True
+
+ for i in range(10):
+ print("!" * 80)
+ print()
+ print("Attempt to set camera info failed: " + response.status_message)
+ print()
+ for i in range(10):
+ print("!" * 80)
+ print()
+ rospy.logerr('Unable to set camera info for calibration. Failure message: %s' % response.status_message)
+ return False
+
+ def do_upload(self):
+ self.c.report()
+ print(self.c.ost())
+ info = self.c.as_message()
+
+ rv = True
+ if self.c.is_mono:
+ response = self.set_camera_info_service(info)
+ rv = self.check_set_camera_info(response)
+ else:
+ response = self.set_left_camera_info_service(info[0])
+ rv = rv and self.check_set_camera_info(response)
+ response = self.set_right_camera_info_service(info[1])
+ rv = rv and self.check_set_camera_info(response)
+ return rv
+
+
+class OpenCVCalibrationNode(CalibrationNode):
+ """ Calibration node with an OpenCV Gui """
+ FONT_FACE = cv2.FONT_HERSHEY_SIMPLEX
+ FONT_SCALE = 0.6
+ FONT_THICKNESS = 2
+
+ def __init__(self, *args, **kwargs):
+
+ CalibrationNode.__init__(self, *args, **kwargs)
+
+ self.queue_display = deque([], 1)
+ self.display_thread = DisplayThread(self.queue_display, self)
+ self.display_thread.setDaemon(True)
+ self.display_thread.start()
+
+ @classmethod
+ def putText(cls, img, text, org, color = (0,0,0)):
+ cv2.putText(img, text, org, cls.FONT_FACE, cls.FONT_SCALE, color, thickness = cls.FONT_THICKNESS)
+
+ @classmethod
+ def getTextSize(cls, text):
+ return cv2.getTextSize(text, cls.FONT_FACE, cls.FONT_SCALE, cls.FONT_THICKNESS)[0]
+
+ def on_mouse(self, event, x, y, flags, param):
+ if event == cv2.EVENT_LBUTTONDOWN and self.displaywidth < x:
+ if self.c.goodenough:
+ if 180 <= y < 280:
+ self.c.do_calibration()
+ if self.c.calibrated:
+ if 280 <= y < 380:
+ self.c.do_save()
+ elif 380 <= y < 480:
+ # Only shut down if we set camera info correctly, #3993
+ if self.do_upload():
+ rospy.signal_shutdown('Quit')
+
+ def on_scale(self, scalevalue):
+ if self.c.calibrated:
+ self.c.set_alpha(scalevalue / 100.0)
+
+ def button(self, dst, label, enable):
+ dst.fill(255)
+ size = (dst.shape[1], dst.shape[0])
+ if enable:
+ color = (155, 155, 80)
+ else:
+ color = (224, 224, 224)
+ cv2.circle(dst, (size[0] // 2, size[1] // 2), min(size) // 2, color, -1)
+ (w, h) = self.getTextSize(label)
+ self.putText(dst, label, ((size[0] - w) // 2, (size[1] + h) // 2), (255,255,255))
+
+ def buttons(self, display):
+ x = self.displaywidth
+ self.button(display[180:280,x:x+100], "CALIBRATE", self.c.goodenough)
+ self.button(display[280:380,x:x+100], "SAVE", self.c.calibrated)
+ self.button(display[380:480,x:x+100], "COMMIT", self.c.calibrated)
+
+ def y(self, i):
+ """Set up right-size images"""
+ return 30 + 40 * i
+
+ def screendump(self, im):
+ i = 0
+ while os.access("/tmp/dump%d.png" % i, os.R_OK):
+ i += 1
+ cv2.imwrite("/tmp/dump%d.png" % i, im)
+
+ def redraw_monocular(self, drawable):
+ height = drawable.scrib.shape[0]
+ width = drawable.scrib.shape[1]
+
+ display = numpy.zeros((max(480, height), width + 100, 3), dtype=numpy.uint8)
+ display[0:height, 0:width,:] = drawable.scrib
+ display[0:height, width:width+100,:].fill(255)
+
+
+ self.buttons(display)
+ if not self.c.calibrated:
+ if drawable.params:
+ for i, (label, lo, hi, progress) in enumerate(drawable.params):
+ (w,_) = self.getTextSize(label)
+ self.putText(display, label, (width + (100 - w) // 2, self.y(i)))
+ color = (0,255,0)
+ if progress < 1.0:
+ color = (0, int(progress*255.), 255)
+ cv2.line(display,
+ (int(width + lo * 100), self.y(i) + 20),
+ (int(width + hi * 100), self.y(i) + 20),
+ color, 4)
+
+ else:
+ self.putText(display, "lin.", (width, self.y(0)))
+ linerror = drawable.linear_error
+ if linerror < 0:
+ msg = "?"
+ else:
+ msg = "%.2f" % linerror
+ #print "linear", linerror
+ self.putText(display, msg, (width, self.y(1)))
+
+ self.queue_display.append(display)
+
+ def redraw_stereo(self, drawable):
+ height = drawable.lscrib.shape[0]
+ width = drawable.lscrib.shape[1]
+
+ display = numpy.zeros((max(480, height), 2 * width + 100, 3), dtype=numpy.uint8)
+ display[0:height, 0:width,:] = drawable.lscrib
+ display[0:height, width:2*width,:] = drawable.rscrib
+ display[0:height, 2*width:2*width+100,:].fill(255)
+
+ self.buttons(display)
+
+ if not self.c.calibrated:
+ if drawable.params:
+ for i, (label, lo, hi, progress) in enumerate(drawable.params):
+ (w,_) = self.getTextSize(label)
+ self.putText(display, label, (2 * width + (100 - w) // 2, self.y(i)))
+ color = (0,255,0)
+ if progress < 1.0:
+ color = (0, int(progress*255.), 255)
+ cv2.line(display,
+ (int(2 * width + lo * 100), self.y(i) + 20),
+ (int(2 * width + hi * 100), self.y(i) + 20),
+ color, 4)
+
+ else:
+ self.putText(display, "epi.", (2 * width, self.y(0)))
+ if drawable.epierror == -1:
+ msg = "?"
+ else:
+ msg = "%.2f" % drawable.epierror
+ self.putText(display, msg, (2 * width, self.y(1)))
+ # TODO dim is never set anywhere. Supposed to be observed chessboard size?
+ if drawable.dim != -1:
+ self.putText(display, "dim", (2 * width, self.y(2)))
+ self.putText(display, "%.3f" % drawable.dim, (2 * width, self.y(3)))
+
+ self.queue_display.append(display)
+
+
+def main():
+ from optparse import OptionParser, OptionGroup
+ parser = OptionParser("%prog --size SIZE1 --square SQUARE1 [ --size SIZE2 --square SQUARE2 ]",
+ description=None)
+ parser.add_option("-c", "--camera_name",
+ type="string", default='narrow_stereo',
+ help="name of the camera to appear in the calibration file")
+ group = OptionGroup(parser, "Chessboard Options",
+ "You must specify one or more chessboards as pairs of --size and --square options.")
+ group.add_option("-p", "--pattern",
+ type="string", default="chessboard",
+ help="calibration pattern to detect - 'chessboard', 'circles', 'acircles'")
+ group.add_option("-s", "--size",
+ action="append", default=[],
+ help="chessboard size as NxM, counting interior corners (e.g. a standard chessboard is 7x7)")
+ group.add_option("-q", "--square",
+ action="append", default=[],
+ help="chessboard square size in meters")
+ parser.add_option_group(group)
+ group = OptionGroup(parser, "ROS Communication Options")
+ group.add_option("--approximate",
+ type="float", default=0.0,
+ help="allow specified slop (in seconds) when pairing images from unsynchronized stereo cameras")
+ group.add_option("--no-service-check",
+ action="store_false", dest="service_check", default=True,
+ help="disable check for set_camera_info services at startup")
+ parser.add_option_group(group)
+ group = OptionGroup(parser, "Calibration Optimizer Options")
+ group.add_option("--fix-principal-point",
+ action="store_true", default=False,
+ help="fix the principal point at the image center")
+ group.add_option("--fix-aspect-ratio",
+ action="store_true", default=False,
+ help="enforce focal lengths (fx, fy) are equal")
+ group.add_option("--zero-tangent-dist",
+ action="store_true", default=False,
+ help="set tangential distortion coefficients (p1, p2) to zero")
+ group.add_option("-k", "--k-coefficients",
+ type="int", default=2, metavar="NUM_COEFFS",
+ help="number of radial distortion coefficients to use (up to 6, default %default)")
+ group.add_option("--disable_calib_cb_fast_check", action='store_true', default=False,
+ help="uses the CALIB_CB_FAST_CHECK flag for findChessboardCorners")
+ parser.add_option_group(group)
+ options, args = parser.parse_args()
+
+ if len(options.size) != len(options.square):
+ parser.error("Number of size and square inputs must be the same!")
+
+ if not options.square:
+ options.square.append("0.108")
+ options.size.append("8x6")
+
+ boards = []
+ for (sz, sq) in zip(options.size, options.square):
+ size = tuple([int(c) for c in sz.split('x')])
+ boards.append(ChessboardInfo(size[0], size[1], float(sq)))
+
+ if options.approximate == 0.0:
+ sync = message_filters.TimeSynchronizer
+ else:
+ sync = functools.partial(ApproximateTimeSynchronizer, slop=options.approximate)
+
+ num_ks = options.k_coefficients
+
+ calib_flags = 0
+ if options.fix_principal_point:
+ calib_flags |= cv2.CALIB_FIX_PRINCIPAL_POINT
+ if options.fix_aspect_ratio:
+ calib_flags |= cv2.CALIB_FIX_ASPECT_RATIO
+ if options.zero_tangent_dist:
+ calib_flags |= cv2.CALIB_ZERO_TANGENT_DIST
+ if (num_ks > 3):
+ calib_flags |= cv2.CALIB_RATIONAL_MODEL
+ if (num_ks < 6):
+ calib_flags |= cv2.CALIB_FIX_K6
+ if (num_ks < 5):
+ calib_flags |= cv2.CALIB_FIX_K5
+ if (num_ks < 4):
+ calib_flags |= cv2.CALIB_FIX_K4
+ if (num_ks < 3):
+ calib_flags |= cv2.CALIB_FIX_K3
+ if (num_ks < 2):
+ calib_flags |= cv2.CALIB_FIX_K2
+ if (num_ks < 1):
+ calib_flags |= cv2.CALIB_FIX_K1
+
+ pattern = Patterns.Chessboard
+ if options.pattern == 'circles':
+ pattern = Patterns.Circles
+ elif options.pattern == 'acircles':
+ pattern = Patterns.ACircles
+ elif options.pattern != 'chessboard':
+ print('Unrecognized pattern %s, defaulting to chessboard' % options.pattern)
+
+ if options.disable_calib_cb_fast_check:
+ checkerboard_flags = 0
+ else:
+ checkerboard_flags = cv2.CALIB_CB_FAST_CHECK
+
+ rospy.init_node('cameracalibrator')
+ node = OpenCVCalibrationNode(boards, options.service_check, sync, calib_flags, pattern, options.camera_name,
+ checkerboard_flags=checkerboard_flags)
+ rospy.spin()
+
+if __name__ == "__main__":
+ try:
+ main()
+ except Exception as e:
+ import traceback
+ traceback.print_exc()
diff --git a/camera_calibration/nodes/cameracheck.py b/camera_calibration/nodes/cameracheck.py
new file mode 100755
index 0000000..29eb76d
--- /dev/null
+++ b/camera_calibration/nodes/cameracheck.py
@@ -0,0 +1,225 @@
+#!/usr/bin/python
+#
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2009, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# * Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above
+# copyright notice, this list of conditions and the following
+# disclaimer in the documentation and/or other materials provided
+# with the distribution.
+# * Neither the name of the Willow Garage nor the names of its
+# contributors may be used to endorse or promote products derived
+# from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+import rospy
+import sensor_msgs.msg
+import sensor_msgs.srv
+import cv_bridge
+
+import math
+import os
+import sys
+import operator
+import time
+import functools
+try:
+ from queue import Queue
+except ImportError:
+ from Queue import Queue
+import threading
+import tarfile
+
+import cv2
+
+import message_filters
+from message_filters import ApproximateTimeSynchronizer
+
+import image_geometry
+import numpy
+
+from camera_calibration.calibrator import MonoCalibrator, StereoCalibrator, ChessboardInfo
+
+def mean(seq):
+ return sum(seq) / len(seq)
+
+def lmin(seq1, seq2):
+ """ Pairwise minimum of two sequences """
+ return [min(a, b) for (a, b) in zip(seq1, seq2)]
+
+def lmax(seq1, seq2):
+ """ Pairwise maximum of two sequences """
+ return [max(a, b) for (a, b) in zip(seq1, seq2)]
+
+class ConsumerThread(threading.Thread):
+ def __init__(self, queue, function):
+ threading.Thread.__init__(self)
+ self.queue = queue
+ self.function = function
+
+ def run(self):
+ while not rospy.is_shutdown():
+ while not rospy.is_shutdown():
+ m = self.queue.get()
+ if self.queue.empty():
+ break
+ self.function(m)
+
+class CameraCheckerNode:
+
+ def __init__(self, chess_size, dim, approximate=0):
+ self.board = ChessboardInfo()
+ self.board.n_cols = chess_size[0]
+ self.board.n_rows = chess_size[1]
+ self.board.dim = dim
+
+ image_topic = rospy.resolve_name("monocular") + "/image_rect"
+ camera_topic = rospy.resolve_name("monocular") + "/camera_info"
+
+ tosync_mono = [
+ (image_topic, sensor_msgs.msg.Image),
+ (camera_topic, sensor_msgs.msg.CameraInfo),
+ ]
+
+ if approximate <= 0:
+ sync = message_filters.TimeSynchronizer
+ else:
+ sync = functools.partial(ApproximateTimeSynchronizer, slop=approximate)
+
+ tsm = sync([message_filters.Subscriber(topic, type) for (topic, type) in tosync_mono], 10)
+ tsm.registerCallback(self.queue_monocular)
+
+ left_topic = rospy.resolve_name("stereo") + "/left/image_rect"
+ left_camera_topic = rospy.resolve_name("stereo") + "/left/camera_info"
+ right_topic = rospy.resolve_name("stereo") + "/right/image_rect"
+ right_camera_topic = rospy.resolve_name("stereo") + "/right/camera_info"
+
+ tosync_stereo = [
+ (left_topic, sensor_msgs.msg.Image),
+ (left_camera_topic, sensor_msgs.msg.CameraInfo),
+ (right_topic, sensor_msgs.msg.Image),
+ (right_camera_topic, sensor_msgs.msg.CameraInfo)
+ ]
+
+ tss = sync([message_filters.Subscriber(topic, type) for (topic, type) in tosync_stereo], 10)
+ tss.registerCallback(self.queue_stereo)
+
+ self.br = cv_bridge.CvBridge()
+
+ self.q_mono = Queue()
+ self.q_stereo = Queue()
+
+ mth = ConsumerThread(self.q_mono, self.handle_monocular)
+ mth.setDaemon(True)
+ mth.start()
+
+ sth = ConsumerThread(self.q_stereo, self.handle_stereo)
+ sth.setDaemon(True)
+ sth.start()
+
+ self.mc = MonoCalibrator([self.board])
+ self.sc = StereoCalibrator([self.board])
+
+ def queue_monocular(self, msg, cmsg):
+ self.q_mono.put((msg, cmsg))
+
+ def queue_stereo(self, lmsg, lcmsg, rmsg, rcmsg):
+ self.q_stereo.put((lmsg, lcmsg, rmsg, rcmsg))
+
+ def mkgray(self, msg):
+ return self.mc.mkgray(msg)
+
+ def image_corners(self, im):
+ (ok, corners, b) = self.mc.get_corners(im)
+ if ok:
+ return corners
+ else:
+ return None
+
+ def handle_monocular(self, msg):
+
+ (image, camera) = msg
+ gray = self.mkgray(image)
+ C = self.image_corners(gray)
+ if C is not None:
+ linearity_rms = self.mc.linear_error(C, self.board)
+
+ # Add in reprojection check
+ image_points = C
+ object_points = self.mc.mk_object_points([self.board], use_board_size=True)[0]
+ dist_coeffs = numpy.zeros((4, 1))
+ camera_matrix = numpy.array( [ [ camera.P[0], camera.P[1], camera.P[2] ],
+ [ camera.P[4], camera.P[5], camera.P[6] ],
+ [ camera.P[8], camera.P[9], camera.P[10] ] ] )
+ ok, rot, trans = cv2.solvePnP(object_points, image_points, camera_matrix, dist_coeffs)
+ # Convert rotation into a 3x3 Rotation Matrix
+ rot3x3, _ = cv2.Rodrigues(rot)
+ # Reproject model points into image
+ object_points_world = numpy.asmatrix(rot3x3) * numpy.asmatrix(object_points.squeeze().T) + numpy.asmatrix(trans)
+ reprojected_h = camera_matrix * object_points_world
+ reprojected = (reprojected_h[0:2, :] / reprojected_h[2, :])
+ reprojection_errors = image_points.squeeze().T - reprojected
+
+ reprojection_rms = numpy.sqrt(numpy.sum(numpy.array(reprojection_errors) ** 2) / numpy.product(reprojection_errors.shape))
+
+ # Print the results
+ print("Linearity RMS Error: %.3f Pixels Reprojection RMS Error: %.3f Pixels" % (linearity_rms, reprojection_rms))
+ else:
+ print('no chessboard')
+
+ def handle_stereo(self, msg):
+
+ (lmsg, lcmsg, rmsg, rcmsg) = msg
+ lgray = self.mkgray(lmsg)
+ rgray = self.mkgray(rmsg)
+
+ L = self.image_corners(lgray)
+ R = self.image_corners(rgray)
+ if L is not None and R is not None:
+ epipolar = self.sc.epipolar_error(L, R)
+
+ dimension = self.sc.chessboard_size(L, R, self.board, msg=(lcmsg, rcmsg))
+
+ print("epipolar error: %f pixels dimension: %f m" % (epipolar, dimension))
+ else:
+ print("no chessboard")
+
+def main():
+ from optparse import OptionParser
+ rospy.init_node('cameracheck')
+ parser = OptionParser()
+ parser.add_option("-s", "--size", default="8x6", help="specify chessboard size as nxm [default: %default]")
+ parser.add_option("-q", "--square", default=".108", help="specify chessboard square size in meters [default: %default]")
+ parser.add_option("--approximate",
+ type="float", default=0.0,
+ help="allow specified slop (in seconds) when pairing images from unsynchronized stereo cameras")
+
+ options, args = parser.parse_args()
+ size = tuple([int(c) for c in options.size.split('x')])
+ dim = float(options.square)
+ approximate = float(options.approximate)
+ CameraCheckerNode(size, dim, approximate)
+ rospy.spin()
+
+if __name__ == "__main__":
+ main()
diff --git a/camera_calibration/package.xml b/camera_calibration/package.xml
new file mode 100644
index 0000000..ee1e5a9
--- /dev/null
+++ b/camera_calibration/package.xml
@@ -0,0 +1,30 @@
+<package>
+ <name>camera_calibration</name>
+ <version>1.12.20</version>
+ <description>
+ camera_calibration allows easy calibration of monocular or stereo
+ cameras using a checkerboard calibration target.
+ </description>
+ <author>James Bowman</author>
+ <author>Patrick Mihelich</author>
+ <maintainer email="vincent.rabaud at gmail.com">Vincent Rabaud</maintainer>
+
+ <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
+
+ <test_depend>rostest</test_depend>
+
+ <run_depend>cv_bridge</run_depend>
+ <run_depend>image_geometry</run_depend>
+ <run_depend>message_filters</run_depend>
+ <run_depend>rospy</run_depend>
+ <run_depend>std_srvs</run_depend>
+ <run_depend>sensor_msgs</run_depend>
+
+ <license>BSD</license>
+ <url>http://www.ros.org/wiki/camera_calibration</url>
+
+ <export>
+ <rosdoc config="rosdoc.yaml" />
+ <architecture_independent/>
+ </export>
+</package>
diff --git a/camera_calibration/rosdoc.yaml b/camera_calibration/rosdoc.yaml
new file mode 100644
index 0000000..0136cd7
--- /dev/null
+++ b/camera_calibration/rosdoc.yaml
@@ -0,0 +1,4 @@
+ - builder: sphinx
+ name: Python API
+ output_dir: python
+ sphinx_root_dir: doc
diff --git a/camera_calibration/scripts/tarfile_calibration.py b/camera_calibration/scripts/tarfile_calibration.py
new file mode 100755
index 0000000..d66cb03
--- /dev/null
+++ b/camera_calibration/scripts/tarfile_calibration.py
@@ -0,0 +1,235 @@
+#!/usr/bin/env python
+#
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2009, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# * Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above
+# copyright notice, this list of conditions and the following
+# disclaimer in the documentation and/or other materials provided
+# with the distribution.
+# * Neither the name of the Willow Garage nor the names of its
+# contributors may be used to endorse or promote products derived
+# from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+import os
+import sys
+import numpy
+
+import cv2
+import cv_bridge
+import tarfile
+
+from camera_calibration.calibrator import MonoCalibrator, StereoCalibrator, CalibrationException, ChessboardInfo
+
+import rospy
+import sensor_msgs.srv
+
+def waitkey():
+ k = cv2.waitKey(6)
+ return k
+
+def display(win_name, img):
+ cv2.namedWindow(win_name, cv2.WINDOW_NORMAL)
+ cv2.imshow( win_name, numpy.asarray( img[:,:] ))
+ k=-1
+ while k ==-1:
+ k=waitkey()
+ cv2.destroyWindow(win_name)
+ if k in [27, ord('q')]:
+ rospy.signal_shutdown('Quit')
+
+
+def cal_from_tarfile(boards, tarname, mono = False, upload = False, calib_flags = 0, visualize = False, alpha=1.0):
+ if mono:
+ calibrator = MonoCalibrator(boards, calib_flags)
+ else:
+ calibrator = StereoCalibrator(boards, calib_flags)
+
+ calibrator.do_tarfile_calibration(tarname)
+
+ print(calibrator.ost())
+
+ if upload:
+ info = calibrator.as_message()
+ if mono:
+ set_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("camera"), sensor_msgs.srv.SetCameraInfo)
+
+ response = set_camera_info_service(info)
+ if not response.success:
+ raise RuntimeError("connected to set_camera_info service, but failed setting camera_info")
+ else:
+ set_left_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("left_camera"), sensor_msgs.srv.SetCameraInfo)
+ set_right_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("right_camera"), sensor_msgs.srv.SetCameraInfo)
+
+ response1 = set_left_camera_info_service(info[0])
+ response2 = set_right_camera_info_service(info[1])
+ if not (response1.success and response2.success):
+ raise RuntimeError("connected to set_camera_info service, but failed setting camera_info")
+
+ if visualize:
+
+ #Show rectified images
+ calibrator.set_alpha(alpha)
+
+ archive = tarfile.open(tarname, 'r')
+ if mono:
+ for f in archive.getnames():
+ if f.startswith('left') and (f.endswith('.pgm') or f.endswith('png')):
+ filedata = archive.extractfile(f).read()
+ file_bytes = numpy.asarray(bytearray(filedata), dtype=numpy.uint8)
+ im=cv2.imdecode(file_bytes,cv2.CV_LOAD_IMAGE_COLOR)
+
+ bridge = cv_bridge.CvBridge()
+ try:
+ msg=bridge.cv2_to_imgmsg(im, "bgr8")
+ except cv_bridge.CvBridgeError as e:
+ print(e)
+
+ #handle msg returns the recitifed image with corner detection once camera is calibrated.
+ drawable=calibrator.handle_msg(msg)
+ vis=numpy.asarray( drawable.scrib[:,:])
+ #Display. Name of window:f
+ display(f, vis)
+ else:
+ limages = [ f for f in archive.getnames() if (f.startswith('left') and (f.endswith('pgm') or f.endswith('png'))) ]
+ limages.sort()
+ rimages = [ f for f in archive.getnames() if (f.startswith('right') and (f.endswith('pgm') or f.endswith('png'))) ]
+ rimages.sort()
+
+ if not len(limages) == len(rimages):
+ raise RuntimeError("Left, right images don't match. %d left images, %d right" % (len(limages), len(rimages)))
+
+ for i in range(len(limages)):
+ l=limages[i]
+ r=rimages[i]
+
+ if l.startswith('left') and (l.endswith('.pgm') or l.endswith('png')) and r.startswith('right') and (r.endswith('.pgm') or r.endswith('png')):
+ # LEFT IMAGE
+ filedata = archive.extractfile(l).read()
+ file_bytes = numpy.asarray(bytearray(filedata), dtype=numpy.uint8)
+ im_left=cv2.imdecode(file_bytes,cv2.CV_LOAD_IMAGE_COLOR)
+
+ bridge = cv_bridge.CvBridge()
+ try:
+ msg_left=bridge.cv2_to_imgmsg(im_left, "bgr8")
+ except cv_bridge.CvBridgeError as e:
+ print(e)
+
+ #RIGHT IMAGE
+ filedata = archive.extractfile(r).read()
+ file_bytes = numpy.asarray(bytearray(filedata), dtype=numpy.uint8)
+ im_right=cv2.imdecode(file_bytes,cv2.CV_LOAD_IMAGE_COLOR)
+ try:
+ msg_right=bridge.cv2_to_imgmsg(im_right, "bgr8")
+ except cv_bridge.CvBridgeError as e:
+ print(e)
+
+ drawable=calibrator.handle_msg([ msg_left,msg_right] )
+
+ h, w = numpy.asarray(drawable.lscrib[:,:]).shape[:2]
+ vis = numpy.zeros((h, w*2, 3), numpy.uint8)
+ vis[:h, :w ,:] = numpy.asarray(drawable.lscrib[:,:])
+ vis[:h, w:w*2, :] = numpy.asarray(drawable.rscrib[:,:])
+
+ display(l+" "+r,vis)
+
+
+if __name__ == '__main__':
+ from optparse import OptionParser
+ parser = OptionParser("%prog TARFILE [ opts ]")
+ parser.add_option("--mono", default=False, action="store_true", dest="mono",
+ help="Monocular calibration only. Calibrates left images.")
+ parser.add_option("-s", "--size", default=[], action="append", dest="size",
+ help="specify chessboard size as NxM [default: 8x6]")
+ parser.add_option("-q", "--square", default=[], action="append", dest="square",
+ help="specify chessboard square size in meters [default: 0.108]")
+ parser.add_option("--upload", default=False, action="store_true", dest="upload",
+ help="Upload results to camera(s).")
+ parser.add_option("--fix-principal-point", action="store_true", default=False,
+ help="fix the principal point at the image center")
+ parser.add_option("--fix-aspect-ratio", action="store_true", default=False,
+ help="enforce focal lengths (fx, fy) are equal")
+ parser.add_option("--zero-tangent-dist", action="store_true", default=False,
+ help="set tangential distortion coefficients (p1, p2) to zero")
+ parser.add_option("-k", "--k-coefficients", type="int", default=2, metavar="NUM_COEFFS",
+ help="number of radial distortion coefficients to use (up to 6, default %default)")
+ parser.add_option("--visualize", action="store_true", default=False,
+ help="visualize rectified images after calibration")
+ parser.add_option("-a", "--alpha", type="float", default=1.0, metavar="ALPHA",
+ help="zoom for visualization of rectifies images. Ranges from 0 (zoomed in, all pixels in calibrated image are valid) to 1 (zoomed out, all pixels in original image are in calibrated image). default %default)")
+
+ options, args = parser.parse_args()
+
+ if len(options.size) != len(options.square):
+ parser.error("Number of size and square inputs must be the same!")
+
+ if not options.square:
+ options.square.append("0.108")
+ options.size.append("8x6")
+
+ boards = []
+ for (sz, sq) in zip(options.size, options.square):
+ info = ChessboardInfo()
+ info.dim = float(sq)
+ size = tuple([int(c) for c in sz.split('x')])
+ info.n_cols = size[0]
+ info.n_rows = size[1]
+
+ boards.append(info)
+
+ if not boards:
+ parser.error("Must supply at least one chessboard")
+
+ if not args:
+ parser.error("Must give tarfile name")
+ if not os.path.exists(args[0]):
+ parser.error("Tarfile %s does not exist. Please select valid tarfile" % args[0])
+
+ tarname = args[0]
+
+ num_ks = options.k_coefficients
+
+ calib_flags = 0
+ if options.fix_principal_point:
+ calib_flags |= cv2.CALIB_FIX_PRINCIPAL_POINT
+ if options.fix_aspect_ratio:
+ calib_flags |= cv2.CALIB_FIX_ASPECT_RATIO
+ if options.zero_tangent_dist:
+ calib_flags |= cv2.CALIB_ZERO_TANGENT_DIST
+ if (num_ks > 3):
+ calib_flags |= cv2.CALIB_RATIONAL_MODEL
+ if (num_ks < 6):
+ calib_flags |= cv2.CALIB_FIX_K6
+ if (num_ks < 5):
+ calib_flags |= cv2.CALIB_FIX_K5
+ if (num_ks < 4):
+ calib_flags |= cv2.CALIB_FIX_K4
+ if (num_ks < 3):
+ calib_flags |= cv2.CALIB_FIX_K3
+ if (num_ks < 2):
+ calib_flags |= cv2.CALIB_FIX_K2
+ if (num_ks < 1):
+ calib_flags |= cv2.CALIB_FIX_K1
+
+ cal_from_tarfile(boards, tarname, options.mono, options.upload, calib_flags, options.visualize, options.alpha)
diff --git a/camera_calibration/setup.py b/camera_calibration/setup.py
new file mode 100644
index 0000000..fd51af2
--- /dev/null
+++ b/camera_calibration/setup.py
@@ -0,0 +1,9 @@
+#!/usr/bin/env python
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup()
+d['packages'] = ['camera_calibration']
+d['package_dir'] = {'':'src'}
+
+setup(**d)
diff --git a/camera_calibration/src/camera_calibration/__init__.py b/camera_calibration/src/camera_calibration/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/camera_calibration/src/camera_calibration/calibrator.py b/camera_calibration/src/camera_calibration/calibrator.py
new file mode 100644
index 0000000..37ca2df
--- /dev/null
+++ b/camera_calibration/src/camera_calibration/calibrator.py
@@ -0,0 +1,1139 @@
+#!/usr/bin/env python
+#
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2009, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# * Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above
+# copyright notice, this list of conditions and the following
+# disclaimer in the documentation and/or other materials provided
+# with the distribution.
+# * Neither the name of the Willow Garage nor the names of its
+# contributors may be used to endorse or promote products derived
+# from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+try:
+ from cStringIO import StringIO
+except ImportError:
+ from io import StringIO
+from io import BytesIO
+import cv2
+import cv_bridge
+import image_geometry
+import math
+import numpy.linalg
+import pickle
+import random
+import sensor_msgs.msg
+import tarfile
+import time
+from distutils.version import LooseVersion
+
+
+# Supported calibration patterns
+class Patterns:
+ Chessboard, Circles, ACircles = list(range(3))
+
+class CalibrationException(Exception):
+ pass
+
+# TODO: Make pattern per-board?
+class ChessboardInfo(object):
+ def __init__(self, n_cols = 0, n_rows = 0, dim = 0.0):
+ self.n_cols = n_cols
+ self.n_rows = n_rows
+ self.dim = dim
+
+# Make all private!!!!!
+def lmin(seq1, seq2):
+ """ Pairwise minimum of two sequences """
+ return [min(a, b) for (a, b) in zip(seq1, seq2)]
+
+def lmax(seq1, seq2):
+ """ Pairwise maximum of two sequences """
+ return [max(a, b) for (a, b) in zip(seq1, seq2)]
+
+def _pdist(p1, p2):
+ """
+ Distance bwt two points. p1 = (x, y), p2 = (x, y)
+ """
+ return math.sqrt(math.pow(p1[0] - p2[0], 2) + math.pow(p1[1] - p2[1], 2))
+
+def _get_outside_corners(corners, board):
+ """
+ Return the four corners of the board as a whole, as (up_left, up_right, down_right, down_left).
+ """
+ xdim = board.n_cols
+ ydim = board.n_rows
+
+ if corners.shape[1] * corners.shape[0] != xdim * ydim:
+ raise Exception("Invalid number of corners! %d corners. X: %d, Y: %d" % (corners.shape[1] * corners.shape[0],
+ xdim, ydim))
+
+ up_left = corners[0,0]
+ up_right = corners[xdim - 1,0]
+ down_right = corners[-1,0]
+ down_left = corners[-xdim,0]
+
+ return (up_left, up_right, down_right, down_left)
+
+def _get_skew(corners, board):
+ """
+ Get skew for given checkerboard detection.
+ Scaled to [0,1], which 0 = no skew, 1 = high skew
+ Skew is proportional to the divergence of three outside corners from 90 degrees.
+ """
+ # TODO Using three nearby interior corners might be more robust, outside corners occasionally
+ # get mis-detected
+ up_left, up_right, down_right, _ = _get_outside_corners(corners, board)
+
+ def angle(a, b, c):
+ """
+ Return angle between lines ab, bc
+ """
+ ab = a - b
+ cb = c - b
+ return math.acos(numpy.dot(ab,cb) / (numpy.linalg.norm(ab) * numpy.linalg.norm(cb)))
+
+ skew = min(1.0, 2. * abs((math.pi / 2.) - angle(up_left, up_right, down_right)))
+ return skew
+
+def _get_area(corners, board):
+ """
+ Get 2d image area of the detected checkerboard.
+ The projected checkerboard is assumed to be a convex quadrilateral, and the area computed as
+ |p X q|/2; see http://mathworld.wolfram.com/Quadrilateral.html.
+ """
+ (up_left, up_right, down_right, down_left) = _get_outside_corners(corners, board)
+ a = up_right - up_left
+ b = down_right - up_right
+ c = down_left - down_right
+ p = b + c
+ q = a + b
+ return abs(p[0]*q[1] - p[1]*q[0]) / 2.
+
+def _get_corners(img, board, refine = True, checkerboard_flags=0):
+ """
+ Get corners for a particular chessboard for an image
+ """
+ h = img.shape[0]
+ w = img.shape[1]
+ if len(img.shape) == 3 and img.shape[2] == 3:
+ mono = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
+ else:
+ mono = img
+ (ok, corners) = cv2.findChessboardCorners(mono, (board.n_cols, board.n_rows), flags = cv2.CALIB_CB_ADAPTIVE_THRESH |
+ cv2.CALIB_CB_NORMALIZE_IMAGE | checkerboard_flags)
+ if not ok:
+ return (ok, corners)
+
+ # If any corners are within BORDER pixels of the screen edge, reject the detection by setting ok to false
+ # NOTE: This may cause problems with very low-resolution cameras, where 8 pixels is a non-negligible fraction
+ # of the image size. See http://answers.ros.org/question/3155/how-can-i-calibrate-low-resolution-cameras
+ BORDER = 8
+ if not all([(BORDER < corners[i, 0, 0] < (w - BORDER)) and (BORDER < corners[i, 0, 1] < (h - BORDER)) for i in range(corners.shape[0])]):
+ ok = False
+
+ # Ensure that all corner-arrays are going from top to bottom.
+ if corners[0, 0, 1] > corners[-1, 0, 1]:
+ corners = numpy.copy(numpy.flipud(corners))
+
+ if refine and ok:
+ # Use a radius of half the minimum distance between corners. This should be large enough to snap to the
+ # correct corner, but not so large as to include a wrong corner in the search window.
+ min_distance = float("inf")
+ for row in range(board.n_rows):
+ for col in range(board.n_cols - 1):
+ index = row*board.n_rows + col
+ min_distance = min(min_distance, _pdist(corners[index, 0], corners[index + 1, 0]))
+ for row in range(board.n_rows - 1):
+ for col in range(board.n_cols):
+ index = row*board.n_rows + col
+ min_distance = min(min_distance, _pdist(corners[index, 0], corners[index + board.n_cols, 0]))
+ radius = int(math.ceil(min_distance * 0.5))
+ cv2.cornerSubPix(mono, corners, (radius,radius), (-1,-1),
+ ( cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.1 ))
+
+ return (ok, corners)
+
+def _get_circles(img, board, pattern):
+ """
+ Get circle centers for a symmetric or asymmetric grid
+ """
+ h = img.shape[0]
+ w = img.shape[1]
+ if len(img.shape) == 3 and img.shape[2] == 3:
+ mono = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
+ else:
+ mono = img
+
+ flag = cv2.CALIB_CB_SYMMETRIC_GRID
+ if pattern == Patterns.ACircles:
+ flag = cv2.CALIB_CB_ASYMMETRIC_GRID
+ mono_arr = numpy.array(mono)
+ (ok, corners) = cv2.findCirclesGrid(mono_arr, (board.n_cols, board.n_rows), flags=flag)
+
+ # In symmetric case, findCirclesGrid does not detect the target if it's turned sideways. So we try
+ # again with dimensions swapped - not so efficient.
+ # TODO Better to add as second board? Corner ordering will change.
+ if not ok and pattern == Patterns.Circles:
+ (ok, corners) = cv2.findCirclesGrid(mono_arr, (board.n_rows, board.n_cols), flags=flag)
+
+ return (ok, corners)
+
+
+# TODO self.size needs to come from CameraInfo, full resolution
+class Calibrator(object):
+ """
+ Base class for calibration system
+ """
+ def __init__(self, boards, flags=0, pattern=Patterns.Chessboard, name='', checkerboard_flags=cv2.CALIB_CB_FAST_CHECK):
+ # Ordering the dimensions for the different detectors is actually a minefield...
+ if pattern == Patterns.Chessboard:
+ # Make sure n_cols > n_rows to agree with OpenCV CB detector output
+ self._boards = [ChessboardInfo(max(i.n_cols, i.n_rows), min(i.n_cols, i.n_rows), i.dim) for i in boards]
+ elif pattern == Patterns.ACircles:
+ # 7x4 and 4x7 are actually different patterns. Assume square-ish pattern, so n_rows > n_cols.
+ self._boards = [ChessboardInfo(min(i.n_cols, i.n_rows), max(i.n_cols, i.n_rows), i.dim) for i in boards]
+ elif pattern == Patterns.Circles:
+ # We end up having to check both ways anyway
+ self._boards = boards
+
+ # Set to true after we perform calibration
+ self.calibrated = False
+ self.calib_flags = flags
+ self.checkerboard_flags = checkerboard_flags
+ self.pattern = pattern
+ self.br = cv_bridge.CvBridge()
+
+ # self.db is list of (parameters, image) samples for use in calibration. parameters has form
+ # (X, Y, size, skew) all normalized to [0,1], to keep track of what sort of samples we've taken
+ # and ensure enough variety.
+ self.db = []
+ # For each db sample, we also record the detected corners.
+ self.good_corners = []
+ # Set to true when we have sufficiently varied samples to calibrate
+ self.goodenough = False
+ self.param_ranges = [0.7, 0.7, 0.4, 0.5]
+ self.name = name
+
+ def mkgray(self, msg):
+ """
+ Convert a message into a 8-bit 1 channel monochrome OpenCV image
+ """
+ # as cv_bridge automatically scales, we need to remove that behavior
+ # TODO: get a Python API in cv_bridge to check for the image depth.
+ if self.br.encoding_to_dtype_with_channels(msg.encoding)[0] in ['uint16', 'int16']:
+ mono16 = self.br.imgmsg_to_cv2(msg, '16UC1')
+ mono8 = numpy.array(numpy.clip(mono16, 0, 255), dtype=numpy.uint8)
+ return mono8
+ elif 'FC1' in msg.encoding:
+ # floating point image handling
+ img = self.br.imgmsg_to_cv2(msg, "passthrough")
+ _, max_val, _, _ = cv2.minMaxLoc(img)
+ if max_val > 0:
+ scale = 255.0 / max_val
+ mono_img = (img * scale).astype(numpy.uint8)
+ else:
+ mono_img = img.astype(numpy.uint8)
+ return mono_img
+ else:
+ return self.br.imgmsg_to_cv2(msg, "mono8")
+
+ def get_parameters(self, corners, board, size):
+ """
+ Return list of parameters [X, Y, size, skew] describing the checkerboard view.
+ """
+ (width, height) = size
+ Xs = corners[:,:,0]
+ Ys = corners[:,:,1]
+ area = _get_area(corners, board)
+ border = math.sqrt(area)
+ # For X and Y, we "shrink" the image all around by approx. half the board size.
+ # Otherwise large boards are penalized because you can't get much X/Y variation.
+ p_x = min(1.0, max(0.0, (numpy.mean(Xs) - border / 2) / (width - border)))
+ p_y = min(1.0, max(0.0, (numpy.mean(Ys) - border / 2) / (height - border)))
+ p_size = math.sqrt(area / (width * height))
+ skew = _get_skew(corners, board)
+ params = [p_x, p_y, p_size, skew]
+ return params
+
+ def is_good_sample(self, params):
+ """
+ Returns true if the checkerboard detection described by params should be added to the database.
+ """
+ if not self.db:
+ return True
+
+ def param_distance(p1, p2):
+ return sum([abs(a-b) for (a,b) in zip(p1, p2)])
+
+ db_params = [sample[0] for sample in self.db]
+ d = min([param_distance(params, p) for p in db_params])
+ #print "d = %.3f" % d #DEBUG
+ # TODO What's a good threshold here? Should it be configurable?
+ return d > 0.2
+
+ _param_names = ["X", "Y", "Size", "Skew"]
+
+ def compute_goodenough(self):
+ if not self.db:
+ return None
+
+ # Find range of checkerboard poses covered by samples in database
+ all_params = [sample[0] for sample in self.db]
+ min_params = all_params[0]
+ max_params = all_params[0]
+ for params in all_params[1:]:
+ min_params = lmin(min_params, params)
+ max_params = lmax(max_params, params)
+ # Don't reward small size or skew
+ min_params = [min_params[0], min_params[1], 0., 0.]
+
+ # For each parameter, judge how much progress has been made toward adequate variation
+ progress = [min((hi - lo) / r, 1.0) for (lo, hi, r) in zip(min_params, max_params, self.param_ranges)]
+ # If we have lots of samples, allow calibration even if not all parameters are green
+ # TODO Awkward that we update self.goodenough instead of returning it
+ self.goodenough = (len(self.db) >= 40) or all([p == 1.0 for p in progress])
+
+ return list(zip(self._param_names, min_params, max_params, progress))
+
+ def mk_object_points(self, boards, use_board_size = False):
+ opts = []
+ for i, b in enumerate(boards):
+ num_pts = b.n_cols * b.n_rows
+ opts_loc = numpy.zeros((num_pts, 1, 3), numpy.float32)
+ for j in range(num_pts):
+ opts_loc[j, 0, 0] = (j / b.n_cols)
+ if self.pattern == Patterns.ACircles:
+ opts_loc[j, 0, 1] = 2*(j % b.n_cols) + (opts_loc[j, 0, 0] % 2)
+ else:
+ opts_loc[j, 0, 1] = (j % b.n_cols)
+ opts_loc[j, 0, 2] = 0
+ if use_board_size:
+ opts_loc[j, 0, :] = opts_loc[j, 0, :] * b.dim
+ opts.append(opts_loc)
+ return opts
+
+ def get_corners(self, img, refine = True):
+ """
+ Use cvFindChessboardCorners to find corners of chessboard in image.
+
+ Check all boards. Return corners for first chessboard that it detects
+ if given multiple size chessboards.
+
+ Returns (ok, corners, board)
+ """
+
+ for b in self._boards:
+ if self.pattern == Patterns.Chessboard:
+ (ok, corners) = _get_corners(img, b, refine, self.checkerboard_flags)
+ else:
+ (ok, corners) = _get_circles(img, b, self.pattern)
+ if ok:
+ return (ok, corners, b)
+ return (False, None, None)
+
+ def downsample_and_detect(self, img):
+ """
+ Downsample the input image to approximately VGA resolution and detect the
+ calibration target corners in the full-size image.
+
+ Combines these apparently orthogonal duties as an optimization. Checkerboard
+ detection is too expensive on large images, so it's better to do detection on
+ the smaller display image and scale the corners back up to the correct size.
+
+ Returns (scrib, corners, downsampled_corners, board, (x_scale, y_scale)).
+ """
+ # Scale the input image down to ~VGA size
+ height = img.shape[0]
+ width = img.shape[1]
+ scale = math.sqrt( (width*height) / (640.*480.) )
+ if scale > 1.0:
+ scrib = cv2.resize(img, (int(width / scale), int(height / scale)))
+ else:
+ scrib = img
+ # Due to rounding, actual horizontal/vertical scaling may differ slightly
+ x_scale = float(width) / scrib.shape[1]
+ y_scale = float(height) / scrib.shape[0]
+
+ if self.pattern == Patterns.Chessboard:
+ # Detect checkerboard
+ (ok, downsampled_corners, board) = self.get_corners(scrib, refine = True)
+
+ # Scale corners back to full size image
+ corners = None
+ if ok:
+ if scale > 1.0:
+ # Refine up-scaled corners in the original full-res image
+ # TODO Does this really make a difference in practice?
+ corners_unrefined = downsampled_corners.copy()
+ corners_unrefined[:, :, 0] *= x_scale
+ corners_unrefined[:, :, 1] *= y_scale
+ radius = int(math.ceil(scale))
+ if len(img.shape) == 3 and img.shape[2] == 3:
+ mono = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
+ else:
+ mono = img
+ cv2.cornerSubPix(mono, corners_unrefined, (radius,radius), (-1,-1),
+ ( cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.1 ))
+ corners = corners_unrefined
+ else:
+ corners = downsampled_corners
+ else:
+ # Circle grid detection is fast even on large images
+ (ok, corners, board) = self.get_corners(img)
+ # Scale corners to downsampled image for display
+ downsampled_corners = None
+ if ok:
+ if scale > 1.0:
+ downsampled_corners = corners.copy()
+ downsampled_corners[:,:,0] /= x_scale
+ downsampled_corners[:,:,1] /= y_scale
+ else:
+ downsampled_corners = corners
+
+ return (scrib, corners, downsampled_corners, board, (x_scale, y_scale))
+
+
+ def lrmsg(self, d, k, r, p):
+ """ Used by :meth:`as_message`. Return a CameraInfo message for the given calibration matrices """
+ msg = sensor_msgs.msg.CameraInfo()
+ (msg.width, msg.height) = self.size
+ if d.size > 5:
+ msg.distortion_model = "rational_polynomial"
+ else:
+ msg.distortion_model = "plumb_bob"
+ msg.D = numpy.ravel(d).copy().tolist()
+ msg.K = numpy.ravel(k).copy().tolist()
+ msg.R = numpy.ravel(r).copy().tolist()
+ msg.P = numpy.ravel(p).copy().tolist()
+ return msg
+
+ def lrreport(self, d, k, r, p):
+ print("D = ", numpy.ravel(d).tolist())
+ print("K = ", numpy.ravel(k).tolist())
+ print("R = ", numpy.ravel(r).tolist())
+ print("P = ", numpy.ravel(p).tolist())
+
+ def lrost(self, name, d, k, r, p):
+ calmessage = (
+ "# oST version 5.0 parameters\n"
+ + "\n"
+ + "\n"
+ + "[image]\n"
+ + "\n"
+ + "width\n"
+ + str(self.size[0]) + "\n"
+ + "\n"
+ + "height\n"
+ + str(self.size[1]) + "\n"
+ + "\n"
+ + "[%s]" % name + "\n"
+ + "\n"
+ + "camera matrix\n"
+ + " ".join(["%8f" % k[0,i] for i in range(3)]) + "\n"
+ + " ".join(["%8f" % k[1,i] for i in range(3)]) + "\n"
+ + " ".join(["%8f" % k[2,i] for i in range(3)]) + "\n"
+ + "\n"
+ + "distortion\n"
+ + " ".join(["%8f" % d[i,0] for i in range(d.shape[0])]) + "\n"
+ + "\n"
+ + "rectification\n"
+ + " ".join(["%8f" % r[0,i] for i in range(3)]) + "\n"
+ + " ".join(["%8f" % r[1,i] for i in range(3)]) + "\n"
+ + " ".join(["%8f" % r[2,i] for i in range(3)]) + "\n"
+ + "\n"
+ + "projection\n"
+ + " ".join(["%8f" % p[0,i] for i in range(4)]) + "\n"
+ + " ".join(["%8f" % p[1,i] for i in range(4)]) + "\n"
+ + " ".join(["%8f" % p[2,i] for i in range(4)]) + "\n"
+ + "\n")
+ assert len(calmessage) < 525, "Calibration info must be less than 525 bytes"
+ return calmessage
+
+ def lryaml(self, name, d, k, r, p):
+ calmessage = (""
+ + "image_width: " + str(self.size[0]) + "\n"
+ + "image_height: " + str(self.size[1]) + "\n"
+ + "camera_name: " + name + "\n"
+ + "camera_matrix:\n"
+ + " rows: 3\n"
+ + " cols: 3\n"
+ + " data: [" + ", ".join(["%8f" % i for i in k.reshape(1,9)[0]]) + "]\n"
+ + "distortion_model: " + ("rational_polynomial" if d.size > 5 else "plumb_bob") + "\n"
+ + "distortion_coefficients:\n"
+ + " rows: 1\n"
+ + " cols: 5\n"
+ + " data: [" + ", ".join(["%8f" % d[i,0] for i in range(d.shape[0])]) + "]\n"
+ + "rectification_matrix:\n"
+ + " rows: 3\n"
+ + " cols: 3\n"
+ + " data: [" + ", ".join(["%8f" % i for i in r.reshape(1,9)[0]]) + "]\n"
+ + "projection_matrix:\n"
+ + " rows: 3\n"
+ + " cols: 4\n"
+ + " data: [" + ", ".join(["%8f" % i for i in p.reshape(1,12)[0]]) + "]\n"
+ + "")
+ return calmessage
+
+ def do_save(self):
+ filename = '/tmp/calibrationdata.tar.gz'
+ tf = tarfile.open(filename, 'w:gz')
+ self.do_tarfile_save(tf) # Must be overridden in subclasses
+ tf.close()
+ print(("Wrote calibration data to", filename))
+
+def image_from_archive(archive, name):
+ """
+ Load image PGM file from tar archive.
+
+ Used for tarfile loading and unit test.
+ """
+ member = archive.getmember(name)
+ imagefiledata = numpy.fromstring(archive.extractfile(member).read(), numpy.uint8)
+ imagefiledata.resize((1, imagefiledata.size))
+ return cv2.imdecode(imagefiledata, cv2.IMREAD_COLOR)
+
+class ImageDrawable(object):
+ """
+ Passed to CalibrationNode after image handled. Allows plotting of images
+ with detected corner points
+ """
+ def __init__(self):
+ self.params = None
+
+class MonoDrawable(ImageDrawable):
+ def __init__(self):
+ ImageDrawable.__init__(self)
+ self.scrib = None
+ self.linear_error = -1.0
+
+
+class StereoDrawable(ImageDrawable):
+ def __init__(self):
+ ImageDrawable.__init__(self)
+ self.lscrib = None
+ self.rscrib = None
+ self.epierror = -1
+ self.dim = -1
+
+
+class MonoCalibrator(Calibrator):
+ """
+ Calibration class for monocular cameras::
+
+ images = [cv2.imread("mono%d.png") for i in range(8)]
+ mc = MonoCalibrator()
+ mc.cal(images)
+ print mc.as_message()
+ """
+
+ is_mono = True # TODO Could get rid of is_mono
+
+ def __init__(self, *args, **kwargs):
+ if 'name' not in kwargs:
+ kwargs['name'] = 'narrow_stereo/left'
+ super(MonoCalibrator, self).__init__(*args, **kwargs)
+
+ def cal(self, images):
+ """
+ Calibrate camera from given images
+ """
+ goodcorners = self.collect_corners(images)
+ self.cal_fromcorners(goodcorners)
+ self.calibrated = True
+
+ def collect_corners(self, images):
+ """
+ :param images: source images containing chessboards
+ :type images: list of :class:`cvMat`
+
+ Find chessboards in all images.
+
+ Return [ (corners, ChessboardInfo) ]
+ """
+ self.size = (images[0].shape[1], images[0].shape[0])
+ corners = [self.get_corners(i) for i in images]
+
+ goodcorners = [(co, b) for (ok, co, b) in corners if ok]
+ if not goodcorners:
+ raise CalibrationException("No corners found in images!")
+ return goodcorners
+
+ def cal_fromcorners(self, good):
+ """
+ :param good: Good corner positions and boards
+ :type good: [(corners, ChessboardInfo)]
+
+
+ """
+ boards = [ b for (_, b) in good ]
+
+ ipts = [ points for (points, _) in good ]
+ opts = self.mk_object_points(boards)
+
+ self.intrinsics = numpy.zeros((3, 3), numpy.float64)
+ if self.calib_flags & cv2.CALIB_RATIONAL_MODEL:
+ self.distortion = numpy.zeros((8, 1), numpy.float64) # rational polynomial
+ else:
+ self.distortion = numpy.zeros((5, 1), numpy.float64) # plumb bob
+ # If FIX_ASPECT_RATIO flag set, enforce focal lengths have 1/1 ratio
+ self.intrinsics[0,0] = 1.0
+ self.intrinsics[1,1] = 1.0
+ cv2.calibrateCamera(
+ opts, ipts,
+ self.size, self.intrinsics,
+ self.distortion,
+ flags = self.calib_flags)
+
+ # R is identity matrix for monocular calibration
+ self.R = numpy.eye(3, dtype=numpy.float64)
+ self.P = numpy.zeros((3, 4), dtype=numpy.float64)
+
+ self.set_alpha(0.0)
+
+ def set_alpha(self, a):
+ """
+ Set the alpha value for the calibrated camera solution. The alpha
+ value is a zoom, and ranges from 0 (zoomed in, all pixels in
+ calibrated image are valid) to 1 (zoomed out, all pixels in
+ original image are in calibrated image).
+ """
+
+ # NOTE: Prior to Electric, this code was broken such that we never actually saved the new
+ # camera matrix. In effect, this enforced P = [K|0] for monocular cameras.
+ # TODO: Verify that OpenCV #1199 gets applied (improved GetOptimalNewCameraMatrix)
+ ncm, _ = cv2.getOptimalNewCameraMatrix(self.intrinsics, self.distortion, self.size, a)
+ for j in range(3):
+ for i in range(3):
+ self.P[j,i] = ncm[j, i]
+ self.mapx, self.mapy = cv2.initUndistortRectifyMap(self.intrinsics, self.distortion, self.R, ncm, self.size, cv2.CV_32FC1)
+
+ def remap(self, src):
+ """
+ :param src: source image
+ :type src: :class:`cvMat`
+
+ Apply the post-calibration undistortion to the source image
+ """
+ return cv2.remap(src, self.mapx, self.mapy, cv2.INTER_LINEAR)
+
+ def undistort_points(self, src):
+ """
+ :param src: N source pixel points (u,v) as an Nx2 matrix
+ :type src: :class:`cvMat`
+
+ Apply the post-calibration undistortion to the source points
+ """
+
+ return cv2.undistortPoints(src, self.intrinsics, self.distortion, R = self.R, P = self.P)
+
+ def as_message(self):
+ """ Return the camera calibration as a CameraInfo message """
+ return self.lrmsg(self.distortion, self.intrinsics, self.R, self.P)
+
+ def from_message(self, msg, alpha = 0.0):
+ """ Initialize the camera calibration from a CameraInfo message """
+
+ self.size = (msg.width, msg.height)
+ self.intrinsics = numpy.array(msg.K, dtype=numpy.float64, copy=True).reshape((3, 3))
+ self.distortion = numpy.array(msg.D, dtype=numpy.float64, copy=True).reshape((len(msg.D), 1))
+ self.R = numpy.array(msg.R, dtype=numpy.float64, copy=True).reshape((3, 3))
+ self.P = numpy.array(msg.P, dtype=numpy.float64, copy=True).reshape((3, 4))
+
+ self.set_alpha(0.0)
+
+ def report(self):
+ self.lrreport(self.distortion, self.intrinsics, self.R, self.P)
+
+ def ost(self):
+ return self.lrost(self.name, self.distortion, self.intrinsics, self.R, self.P)
+
+ def yaml(self):
+ return self.lryaml(self.name, self.distortion, self.intrinsics, self.R, self.P)
+
+ def linear_error_from_image(self, image):
+ """
+ Detect the checkerboard and compute the linear error.
+ Mainly for use in tests.
+ """
+ _, corners, _, board, _ = self.downsample_and_detect(image)
+ if corners is None:
+ return None
+
+ undistorted = self.undistort_points(corners)
+ return self.linear_error(undistorted, board)
+
+ @staticmethod
+ def linear_error(corners, b):
+
+ """
+ Returns the linear error for a set of corners detected in the unrectified image.
+ """
+
+ if corners is None:
+ return None
+
+ def pt2line(x0, y0, x1, y1, x2, y2):
+ """ point is (x0, y0), line is (x1, y1, x2, y2) """
+ return abs((x2 - x1) * (y1 - y0) - (x1 - x0) * (y2 - y1)) / math.sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2)
+
+ cc = b.n_cols
+ cr = b.n_rows
+ errors = []
+ for r in range(cr):
+ (x1, y1) = corners[(cc * r) + 0, 0]
+ (x2, y2) = corners[(cc * r) + cc - 1, 0]
+ for i in range(1, cc - 1):
+ (x0, y0) = corners[(cc * r) + i, 0]
+ errors.append(pt2line(x0, y0, x1, y1, x2, y2))
+ if errors:
+ return math.sqrt(sum([e**2 for e in errors]) / len(errors))
+ else:
+ return None
+
+
+ def handle_msg(self, msg):
+ """
+ Detects the calibration target and, if found and provides enough new information,
+ adds it to the sample database.
+
+ Returns a MonoDrawable message with the display image and progress info.
+ """
+ gray = self.mkgray(msg)
+ linear_error = -1
+
+ # Get display-image-to-be (scrib) and detection of the calibration target
+ scrib_mono, corners, downsampled_corners, board, (x_scale, y_scale) = self.downsample_and_detect(gray)
+
+ if self.calibrated:
+ # Show rectified image
+ # TODO Pull out downsampling code into function
+ gray_remap = self.remap(gray)
+ gray_rect = gray_remap
+ if x_scale != 1.0 or y_scale != 1.0:
+ gray_rect = cv2.resize(gray_remap, (scrib_mono.shape[1], scrib_mono.shape[0]))
+
+ scrib = cv2.cvtColor(gray_rect, cv2.COLOR_GRAY2BGR)
+
+ if corners is not None:
+ # Report linear error
+ undistorted = self.undistort_points(corners)
+ linear_error = self.linear_error(undistorted, board)
+
+ # Draw rectified corners
+ scrib_src = undistorted.copy()
+ scrib_src[:,:,0] /= x_scale
+ scrib_src[:,:,1] /= y_scale
+ cv2.drawChessboardCorners(scrib, (board.n_cols, board.n_rows), scrib_src, True)
+
+ else:
+ scrib = cv2.cvtColor(scrib_mono, cv2.COLOR_GRAY2BGR)
+ if corners is not None:
+ # Draw (potentially downsampled) corners onto display image
+ cv2.drawChessboardCorners(scrib, (board.n_cols, board.n_rows), downsampled_corners, True)
+
+ # Add sample to database only if it's sufficiently different from any previous sample.
+ params = self.get_parameters(corners, board, (gray.shape[1], gray.shape[0]))
+ if self.is_good_sample(params):
+ self.db.append((params, gray))
+ self.good_corners.append((corners, board))
+ print(("*** Added sample %d, p_x = %.3f, p_y = %.3f, p_size = %.3f, skew = %.3f" % tuple([len(self.db)] + params)))
+
+ rv = MonoDrawable()
+ rv.scrib = scrib
+ rv.params = self.compute_goodenough()
+ rv.linear_error = linear_error
+ return rv
+
+ def do_calibration(self, dump = False):
+ if not self.good_corners:
+ print("**** Collecting corners for all images! ****") #DEBUG
+ images = [i for (p, i) in self.db]
+ self.good_corners = self.collect_corners(images)
+ # Dump should only occur if user wants it
+ if dump:
+ pickle.dump((self.is_mono, self.size, self.good_corners),
+ open("/tmp/camera_calibration_%08x.pickle" % random.getrandbits(32), "w"))
+ self.size = (self.db[0][1].shape[1], self.db[0][1].shape[0]) # TODO Needs to be set externally
+ self.cal_fromcorners(self.good_corners)
+ self.calibrated = True
+ # DEBUG
+ print((self.report()))
+ print((self.ost()))
+
+ def do_tarfile_save(self, tf):
+ """ Write images and calibration solution to a tarfile object """
+
+ def taradd(name, buf):
+ if isinstance(buf, basestring):
+ s = StringIO(buf)
+ else:
+ s = BytesIO(buf)
+ ti = tarfile.TarInfo(name)
+ ti.size = len(s.getvalue())
+ ti.uname = 'calibrator'
+ ti.mtime = int(time.time())
+ tf.addfile(tarinfo=ti, fileobj=s)
+
+ ims = [("left-%04d.png" % i, im) for i,(_, im) in enumerate(self.db)]
+ for (name, im) in ims:
+ taradd(name, cv2.imencode(".png", im)[1].tostring())
+ taradd('ost.yaml', self.yaml())
+ taradd('ost.txt', self.ost())
+
+ def do_tarfile_calibration(self, filename):
+ archive = tarfile.open(filename, 'r')
+
+ limages = [ image_from_archive(archive, f) for f in archive.getnames() if (f.startswith('left') and (f.endswith('.pgm') or f.endswith('png'))) ]
+
+ self.cal(limages)
+
+# TODO Replicate MonoCalibrator improvements in stereo
+class StereoCalibrator(Calibrator):
+ """
+ Calibration class for stereo cameras::
+
+ limages = [cv2.imread("left%d.png") for i in range(8)]
+ rimages = [cv2.imread("right%d.png") for i in range(8)]
+ sc = StereoCalibrator()
+ sc.cal(limages, rimages)
+ print sc.as_message()
+ """
+
+ is_mono = False
+
+ def __init__(self, *args, **kwargs):
+ if 'name' not in kwargs:
+ kwargs['name'] = 'narrow_stereo'
+ super(StereoCalibrator, self).__init__(*args, **kwargs)
+ self.l = MonoCalibrator(*args, **kwargs)
+ self.r = MonoCalibrator(*args, **kwargs)
+ # Collecting from two cameras in a horizontal stereo rig, can't get
+ # full X range in the left camera.
+ self.param_ranges[0] = 0.4
+
+ def cal(self, limages, rimages):
+ """
+ :param limages: source left images containing chessboards
+ :type limages: list of :class:`cvMat`
+ :param rimages: source right images containing chessboards
+ :type rimages: list of :class:`cvMat`
+
+ Find chessboards in images, and runs the OpenCV calibration solver.
+ """
+ goodcorners = self.collect_corners(limages, rimages)
+ self.size = (limages[0].shape[1], limages[0].shape[0])
+ self.l.size = self.size
+ self.r.size = self.size
+ self.cal_fromcorners(goodcorners)
+ self.calibrated = True
+
+ def collect_corners(self, limages, rimages):
+ """
+ For a sequence of left and right images, find pairs of images where both
+ left and right have a chessboard, and return their corners as a list of pairs.
+ """
+ # Pick out (corners, board) tuples
+ lcorners = [ self.downsample_and_detect(i)[1:4:2] for i in limages]
+ rcorners = [ self.downsample_and_detect(i)[1:4:2] for i in rimages]
+ good = [(lco, rco, b) for ((lco, b), (rco, br)) in zip( lcorners, rcorners)
+ if (lco is not None and rco is not None)]
+
+ if len(good) == 0:
+ raise CalibrationException("No corners found in images!")
+ return good
+
+ def cal_fromcorners(self, good):
+ # Perform monocular calibrations
+ lcorners = [(l, b) for (l, r, b) in good]
+ rcorners = [(r, b) for (l, r, b) in good]
+ self.l.cal_fromcorners(lcorners)
+ self.r.cal_fromcorners(rcorners)
+
+ lipts = [ l for (l, _, _) in good ]
+ ripts = [ r for (_, r, _) in good ]
+ boards = [ b for (_, _, b) in good ]
+
+ opts = self.mk_object_points(boards, True)
+
+ flags = cv2.CALIB_FIX_INTRINSIC
+
+ self.T = numpy.zeros((3, 1), dtype=numpy.float64)
+ self.R = numpy.eye(3, dtype=numpy.float64)
+ if LooseVersion(cv2.__version__).version[0] == 2:
+ cv2.stereoCalibrate(opts, lipts, ripts, self.size,
+ self.l.intrinsics, self.l.distortion,
+ self.r.intrinsics, self.r.distortion,
+ self.R, # R
+ self.T, # T
+ criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 1, 1e-5),
+ flags = flags)
+ else:
+ cv2.stereoCalibrate(opts, lipts, ripts,
+ self.l.intrinsics, self.l.distortion,
+ self.r.intrinsics, self.r.distortion,
+ self.size,
+ self.R, # R
+ self.T, # T
+ criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 1, 1e-5),
+ flags = flags)
+
+ self.set_alpha(0.0)
+
+ def set_alpha(self, a):
+ """
+ Set the alpha value for the calibrated camera solution. The
+ alpha value is a zoom, and ranges from 0 (zoomed in, all pixels
+ in calibrated image are valid) to 1 (zoomed out, all pixels in
+ original image are in calibrated image).
+ """
+
+ cv2.stereoRectify(self.l.intrinsics,
+ self.l.distortion,
+ self.r.intrinsics,
+ self.r.distortion,
+ self.size,
+ self.R,
+ self.T,
+ self.l.R, self.r.R, self.l.P, self.r.P,
+ alpha = a)
+
+ cv2.initUndistortRectifyMap(self.l.intrinsics, self.l.distortion, self.l.R, self.l.P, self.size, cv2.CV_32FC1,
+ self.l.mapx, self.l.mapy)
+ cv2.initUndistortRectifyMap(self.r.intrinsics, self.r.distortion, self.r.R, self.r.P, self.size, cv2.CV_32FC1,
+ self.r.mapx, self.r.mapy)
+
+ def as_message(self):
+ """
+ Return the camera calibration as a pair of CameraInfo messages, for left
+ and right cameras respectively.
+ """
+
+ return (self.lrmsg(self.l.distortion, self.l.intrinsics, self.l.R, self.l.P),
+ self.lrmsg(self.r.distortion, self.r.intrinsics, self.r.R, self.r.P))
+
+ def from_message(self, msgs, alpha = 0.0):
+ """ Initialize the camera calibration from a pair of CameraInfo messages. """
+ self.size = (msgs[0].width, msgs[0].height)
+
+ self.T = numpy.zeros((3, 1), dtype=numpy.float64)
+ self.R = numpy.eye(3, dtype=numpy.float64)
+
+ self.l.from_message(msgs[0])
+ self.r.from_message(msgs[1])
+ # Need to compute self.T and self.R here, using the monocular parameters above
+ if False:
+ self.set_alpha(0.0)
+
+ def report(self):
+ print("\nLeft:")
+ self.lrreport(self.l.distortion, self.l.intrinsics, self.l.R, self.l.P)
+ print("\nRight:")
+ self.lrreport(self.r.distortion, self.r.intrinsics, self.r.R, self.r.P)
+ print("self.T ", numpy.ravel(self.T).tolist())
+ print("self.R ", numpy.ravel(self.R).tolist())
+
+ def ost(self):
+ return (self.lrost(self.name + "/left", self.l.distortion, self.l.intrinsics, self.l.R, self.l.P) +
+ self.lrost(self.name + "/right", self.r.distortion, self.r.intrinsics, self.r.R, self.r.P))
+
+ def yaml(self, suffix, info):
+ return self.lryaml(self.name + suffix, info.distortion, info.intrinsics, info.R, info.P)
+
+ # TODO Get rid of "from_images" versions of these, instead have function to get undistorted corners
+ def epipolar_error_from_images(self, limage, rimage):
+ """
+ Detect the checkerboard in both images and compute the epipolar error.
+ Mainly for use in tests.
+ """
+ lcorners = self.downsample_and_detect(limage)[1]
+ rcorners = self.downsample_and_detect(rimage)[1]
+ if lcorners is None or rcorners is None:
+ return None
+
+ lundistorted = self.l.undistort_points(lcorners)
+ rundistorted = self.r.undistort_points(rcorners)
+
+ return self.epipolar_error(lundistorted, rundistorted)
+
+ def epipolar_error(self, lcorners, rcorners):
+ """
+ Compute the epipolar error from two sets of matching undistorted points
+ """
+ d = lcorners[:,:,1] - rcorners[:,:,1]
+ return numpy.sqrt(numpy.square(d).sum() / d.size)
+
+ def chessboard_size_from_images(self, limage, rimage):
+ _, lcorners, _, board, _ = self.downsample_and_detect(limage)
+ _, rcorners, _, board, _ = self.downsample_and_detect(rimage)
+ if lcorners is None or rcorners is None:
+ return None
+
+ lundistorted = self.l.undistort_points(lcorners)
+ rundistorted = self.r.undistort_points(rcorners)
+
+ return self.chessboard_size(lundistorted, rundistorted, board)
+
+ def chessboard_size(self, lcorners, rcorners, board, msg = None):
+ """
+ Compute the square edge length from two sets of matching undistorted points
+ given the current calibration.
+ :param msg: a tuple of (left_msg, right_msg)
+ """
+ # Project the points to 3d
+ cam = image_geometry.StereoCameraModel()
+ if msg == None:
+ msg = self.as_message()
+ cam.fromCameraInfo(*msg)
+ disparities = lcorners[:,:,0] - rcorners[:,:,0]
+ pt3d = [cam.projectPixelTo3d((lcorners[i,0,0], lcorners[i,0,1]), disparities[i,0]) for i in range(lcorners.shape[0]) ]
+ def l2(p0, p1):
+ return math.sqrt(sum([(c0 - c1) ** 2 for (c0, c1) in zip(p0, p1)]))
+
+ # Compute the length from each horizontal and vertical line, and return the mean
+ cc = board.n_cols
+ cr = board.n_rows
+ lengths = (
+ [l2(pt3d[cc * r + 0], pt3d[cc * r + (cc - 1)]) / (cc - 1) for r in range(cr)] +
+ [l2(pt3d[c + 0], pt3d[c + (cc * (cr - 1))]) / (cr - 1) for c in range(cc)])
+ return sum(lengths) / len(lengths)
+
+ def handle_msg(self, msg):
+ # TODO Various asserts that images have same dimension, same board detected...
+ (lmsg, rmsg) = msg
+ lgray = self.mkgray(lmsg)
+ rgray = self.mkgray(rmsg)
+ epierror = -1
+
+ # Get display-images-to-be and detections of the calibration target
+ lscrib_mono, lcorners, ldownsampled_corners, lboard, (x_scale, y_scale) = self.downsample_and_detect(lgray)
+ rscrib_mono, rcorners, rdownsampled_corners, rboard, _ = self.downsample_and_detect(rgray)
+
+ if self.calibrated:
+ # Show rectified images
+ lremap = self.l.remap(lgray)
+ rremap = self.r.remap(rgray)
+ lrect = lremap
+ rrect = rremap
+ if x_scale != 1.0 or y_scale != 1.0:
+ lrect = cv2.resize(lremap, (lscrib_mono.shape[1], lscrib_mono.shape[0]))
+ rrect = cv2.resize(rremap, (rscrib_mono.shape[1], rscrib_mono.shape[0]))
+
+ lscrib = cv2.cvtColor(lrect, cv2.COLOR_GRAY2BGR)
+ rscrib = cv2.cvtColor(rrect, cv2.COLOR_GRAY2BGR)
+
+ # Draw rectified corners
+ if lcorners is not None:
+ lundistorted = self.l.undistort_points(lcorners)
+ scrib_src = lundistorted.copy()
+ scrib_src[:,:,0] /= x_scale
+ scrib_src[:,:,1] /= y_scale
+ cv2.drawChessboardCorners(lscrib, (lboard.n_cols, lboard.n_rows), scrib_src, True)
+
+ if rcorners is not None:
+ rundistorted = self.r.undistort_points(rcorners)
+ scrib_src = rundistorted.copy()
+ scrib_src[:,:,0] /= x_scale
+ scrib_src[:,:,1] /= y_scale
+ cv2.drawChessboardCorners(rscrib, (rboard.n_cols, rboard.n_rows), scrib_src, True)
+
+ # Report epipolar error
+ if lcorners is not None and rcorners is not None and len(lcorners) == len(rcorners):
+ epierror = self.epipolar_error(lundistorted, rundistorted)
+
+ else:
+ lscrib = cv2.cvtColor(lscrib_mono, cv2.COLOR_GRAY2BGR)
+ rscrib = cv2.cvtColor(rscrib_mono, cv2.COLOR_GRAY2BGR)
+ # Draw any detected chessboards onto display (downsampled) images
+ if lcorners is not None:
+ cv2.drawChessboardCorners(lscrib, (lboard.n_cols, lboard.n_rows),
+ ldownsampled_corners, True)
+ if rcorners is not None:
+ cv2.drawChessboardCorners(rscrib, (rboard.n_cols, rboard.n_rows),
+ rdownsampled_corners, True)
+
+ # Add sample to database only if it's sufficiently different from any previous sample
+ if lcorners is not None and rcorners is not None and len(lcorners) == len(rcorners):
+ params = self.get_parameters(lcorners, lboard, (lgray.shape[1], lgray.shape[0]))
+ if self.is_good_sample(params):
+ self.db.append( (params, lgray, rgray) )
+ self.good_corners.append( (lcorners, rcorners, lboard) )
+ print(("*** Added sample %d, p_x = %.3f, p_y = %.3f, p_size = %.3f, skew = %.3f" % tuple([len(self.db)] + params)))
+
+ rv = StereoDrawable()
+ rv.lscrib = lscrib
+ rv.rscrib = rscrib
+ rv.params = self.compute_goodenough()
+ rv.epierror = epierror
+ return rv
+
+ def do_calibration(self, dump = False):
+ # TODO MonoCalibrator collects corners if needed here
+ # Dump should only occur if user wants it
+ if dump:
+ pickle.dump((self.is_mono, self.size, self.good_corners),
+ open("/tmp/camera_calibration_%08x.pickle" % random.getrandbits(32), "w"))
+ self.size = (self.db[0][1].shape[1], self.db[0][1].shape[0]) # TODO Needs to be set externally
+ self.l.size = self.size
+ self.r.size = self.size
+ self.cal_fromcorners(self.good_corners)
+ self.calibrated = True
+ # DEBUG
+ print((self.report()))
+ print((self.ost()))
+
+ def do_tarfile_save(self, tf):
+ """ Write images and calibration solution to a tarfile object """
+ ims = ([("left-%04d.png" % i, im) for i,(_, im, _) in enumerate(self.db)] +
+ [("right-%04d.png" % i, im) for i,(_, _, im) in enumerate(self.db)])
+
+ def taradd(name, buf):
+ if isinstance(buf, basestring):
+ s = StringIO(buf)
+ else:
+ s = BytesIO(buf)
+ ti = tarfile.TarInfo(name)
+ ti.size = len(s.getvalue())
+ ti.uname = 'calibrator'
+ ti.mtime = int(time.time())
+ tf.addfile(tarinfo=ti, fileobj=s)
+
+ for (name, im) in ims:
+ taradd(name, cv2.imencode(".png", im)[1].tostring())
+ taradd('left.yaml', self.yaml("/left", self.l))
+ taradd('right.yaml', self.yaml("/right", self.r))
+ taradd('ost.txt', self.ost())
+
+ def do_tarfile_calibration(self, filename):
+ archive = tarfile.open(filename, 'r')
+ limages = [ image_from_archive(archive, f) for f in archive.getnames() if (f.startswith('left') and (f.endswith('pgm') or f.endswith('png'))) ]
+ rimages = [ image_from_archive(archive, f) for f in archive.getnames() if (f.startswith('right') and (f.endswith('pgm') or f.endswith('png'))) ]
+
+ if not len(limages) == len(rimages):
+ raise CalibrationException("Left, right images don't match. %d left images, %d right" % (len(limages), len(rimages)))
+
+ ##\todo Check that the filenames match and stuff
+
+ self.cal(limages, rimages)
diff --git a/camera_calibration/test/directed.py b/camera_calibration/test/directed.py
new file mode 100644
index 0000000..43a7aaa
--- /dev/null
+++ b/camera_calibration/test/directed.py
@@ -0,0 +1,274 @@
+#!/usr/bin/env python
+#
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2009, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# * Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above
+# copyright notice, this list of conditions and the following
+# disclaimer in the documentation and/or other materials provided
+# with the distribution.
+# * Neither the name of the Willow Garage nor the names of its
+# contributors may be used to endorse or promote products derived
+# from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+import roslib
+import rosunit
+import rospy
+import cv2
+
+import collections
+import copy
+import numpy
+import os
+import sys
+import tarfile
+import unittest
+
+from camera_calibration.calibrator import MonoCalibrator, StereoCalibrator, \
+ Patterns, CalibrationException, ChessboardInfo, image_from_archive
+
+board = ChessboardInfo()
+board.n_cols = 8
+board.n_rows = 6
+board.dim = 0.108
+
+class TestDirected(unittest.TestCase):
+ def setUp(self):
+ tar_path = roslib.packages.find_resource('camera_calibration', 'camera_calibration.tar.gz')[0]
+ self.tar = tarfile.open(tar_path, 'r')
+ self.limages = [image_from_archive(self.tar, "wide/left%04d.pgm" % i) for i in range(3, 15)]
+ self.rimages = [image_from_archive(self.tar, "wide/right%04d.pgm" % i) for i in range(3, 15)]
+ self.l = {}
+ self.r = {}
+ self.sizes = [(320,240), (640,480), (800,600), (1024,768)]
+ for dim in self.sizes:
+ self.l[dim] = []
+ self.r[dim] = []
+ for li,ri in zip(self.limages, self.rimages):
+ rli = cv2.resize(li, (dim[0], dim[1]))
+ rri = cv2.resize(ri, (dim[0], dim[1]))
+ self.l[dim].append(rli)
+ self.r[dim].append(rri)
+
+ def assert_good_mono(self, c, dim, max_err):
+ #c.report()
+ self.assert_(len(c.ost()) > 0)
+ lin_err = 0
+ n = 0
+ for img in self.l[dim]:
+ lin_err_local = c.linear_error_from_image(img)
+ if lin_err_local:
+ lin_err += lin_err_local
+ n += 1
+ if n > 0:
+ lin_err /= n
+ self.assert_(0.0 < lin_err, 'lin_err is %f' % lin_err)
+ self.assert_(lin_err < max_err, 'lin_err is %f' % lin_err)
+
+ flat = c.remap(img)
+ self.assertEqual(img.shape, flat.shape)
+
+ def test_monocular(self):
+ # Run the calibrator, produce a calibration, check it
+ mc = MonoCalibrator([ board ], cv2.CALIB_FIX_K3)
+ max_errs = [0.1, 0.2, 0.4, 0.7]
+ for i, dim in enumerate(self.sizes):
+ mc.cal(self.l[dim])
+ self.assert_good_mono(mc, dim, max_errs[i])
+
+ # Make another calibration, import previous calibration as a message,
+ # and assert that the new one is good.
+
+ mc2 = MonoCalibrator([board])
+ mc2.from_message(mc.as_message())
+ self.assert_good_mono(mc2, dim, max_errs[i])
+
+ def test_stereo(self):
+ epierrors = [0.1, 0.2, 0.45, 1.0]
+ for i, dim in enumerate(self.sizes):
+ print("Dim =", dim)
+ sc = StereoCalibrator([board], cv2.CALIB_FIX_K3)
+ sc.cal(self.l[dim], self.r[dim])
+
+ sc.report()
+ #print sc.ost()
+
+ # NOTE: epipolar error currently increases with resolution.
+ # At highest res expect error ~0.75
+ epierror = 0
+ n = 0
+ for l_img, r_img in zip(self.l[dim], self.r[dim]):
+ epierror_local = sc.epipolar_error_from_images(l_img, r_img)
+ if epierror_local:
+ epierror += epierror_local
+ n += 1
+ epierror /= n
+ self.assert_(epierror < epierrors[i],
+ 'Epipolar error is %f for resolution i = %d' % (epierror, i))
+
+ self.assertAlmostEqual(sc.chessboard_size_from_images(self.l[dim][0], self.r[dim][0]), .108, 2)
+
+ #print sc.as_message()
+
+ img = self.l[dim][0]
+ flat = sc.l.remap(img)
+ self.assertEqual(img.shape, flat.shape)
+ flat = sc.r.remap(img)
+ self.assertEqual(img.shape, flat.shape)
+
+ sc2 = StereoCalibrator([board])
+ sc2.from_message(sc.as_message())
+ # sc2.set_alpha(1.0)
+ #sc2.report()
+ self.assert_(len(sc2.ost()) > 0)
+
+ def test_nochecker(self):
+ # Run with same images, but looking for an incorrect chessboard size (8, 7).
+ # Should raise an exception because of lack of input points.
+ new_board = copy.deepcopy(board)
+ new_board.n_cols = 8
+ new_board.n_rows = 7
+
+ sc = StereoCalibrator([new_board])
+ self.assertRaises(CalibrationException, lambda: sc.cal(self.limages, self.rimages))
+ mc = MonoCalibrator([new_board])
+ self.assertRaises(CalibrationException, lambda: mc.cal(self.limages))
+
+
+
+class TestArtificial(unittest.TestCase):
+ Setup = collections.namedtuple('Setup', ['pattern', 'cols', 'rows', 'lin_err', 'K_err'])
+
+ def setUp(self):
+ # Define some image transforms that will simulate a camera position
+ M = []
+ cv2.getPerspectiveTransform
+ self.K = numpy.array([[500, 0, 250], [0, 500, 250], [0, 0, 1]], numpy.float32)
+ self.D = numpy.array([])
+ # physical size of the board
+ self.board_width_dim = 1
+
+ # Generate data for different grid types. For each grid type, define the different sizes of
+ # grid that are recognized (n row, n col)
+ # Patterns.Circles, Patterns.ACircles
+ self.setups = [ self.Setup(pattern=Patterns.Chessboard, cols=7, rows=8, lin_err=0.2, K_err=8.2),
+ self.Setup(pattern=Patterns.Circles, cols=7, rows=8, lin_err=0.1, K_err=4),
+ self.Setup(pattern=Patterns.ACircles, cols=3, rows=5, lin_err=0.1, K_err=8) ]
+ self.limages = []
+ self.rimages = []
+ for setup in self.setups:
+ self.limages.append([])
+ self.rimages.append([])
+
+ # Create the pattern
+ if setup.pattern == Patterns.Chessboard:
+ pattern = numpy.zeros((50*(setup.rows+3), 50*(setup.cols+3), 1), numpy.uint8)
+ pattern.fill(255)
+ for j in range(1, setup.rows+2):
+ for i in range(1+(j%2), setup.cols+2, 2):
+ pattern[50*j:50*(j+1), 50*i:50*(i+1)].fill(0)
+ elif setup.pattern == Patterns.Circles:
+ pattern = numpy.zeros((50*(setup.rows+2), 50*(setup.cols+2), 1), numpy.uint8)
+ pattern.fill(255)
+ for j in range(1, setup.rows+1):
+ for i in range(1, setup.cols+1):
+ cv2.circle(pattern, (50*i + 25, 50*j + 25), 15, (0,0,0), -1 )
+ elif setup.pattern == Patterns.ACircles:
+ x = 60
+ pattern = numpy.zeros((x*(setup.rows+2), x*(setup.cols+5), 1), numpy.uint8)
+ pattern.fill(255)
+ for j in range(1, setup.rows+1):
+ for i in range(0, setup.cols):
+ cv2.circle(pattern, (x*(1 + 2*i + (j%2)) + x/2, x*j + x/2), x/3, (0,0,0), -1)
+
+ rows, cols, _ = pattern.shape
+ object_points_2d = numpy.array([[0, 0], [0, cols-1], [rows-1, cols-1], [rows-1, 0]], numpy.float32)
+ object_points_3d = numpy.array([[0, 0, 0], [0, cols-1, 0], [rows-1, cols-1, 0], [rows-1, 0, 0]], numpy.float32)
+ object_points_3d *= self.board_width_dim/float(cols)
+
+ # create the artificial view points
+ rvec = [ [0, 0, 0], [0, 0, 0.4], [0, 0.4, 0], [0.4, 0, 0], [0.4, 0.4, 0], [0.4, 0, 0.4], [0, 0.4, 0.4], [0.4, 0.4, 0.4] ]
+ tvec = [ [-0.5, -0.5, 3], [-0.5, -0.5, 3], [-0.5, -0.1, 3], [-0.1, -0.5, 3], [-0.1, -0.1, 3], [-0.1, -0.5, 3], [-0.5, -0.1, 3], [-0.1, 0.1, 3] ]
+
+ dsize = (480, 640)
+ for i in range(len(rvec)):
+ R = numpy.array(rvec[i], numpy.float32)
+ T = numpy.array(tvec[i], numpy.float32)
+
+ image_points, _ = cv2.projectPoints(object_points_3d, R, T, self.K, self.D)
+
+ # deduce the perspective transform
+ M.append(cv2.getPerspectiveTransform(object_points_2d, image_points))
+
+ # project the pattern according to the different cameras
+ pattern_warped = cv2.warpPerspective(pattern, M[i], dsize)
+ self.limages[-1].append(pattern_warped)
+
+ def assert_good_mono(self, c, images, max_err):
+ #c.report()
+ self.assert_(len(c.ost()) > 0)
+ lin_err = 0
+ n = 0
+ for img in images:
+ lin_err_local = c.linear_error_from_image(img)
+ if lin_err_local:
+ lin_err += lin_err_local
+ n += 1
+ if n > 0:
+ lin_err /= n
+ print("linear error is %f" % lin_err)
+ self.assert_(0.0 < lin_err, 'lin_err is %f' % lin_err)
+ self.assert_(lin_err < max_err, 'lin_err is %f' % lin_err)
+
+ flat = c.remap(img)
+ self.assertEqual(img.shape, flat.shape)
+
+ def test_monocular(self):
+ # Run the calibrator, produce a calibration, check it
+ for i, setup in enumerate(self.setups):
+ board = ChessboardInfo()
+ board.n_cols = setup.cols
+ board.n_rows = setup.rows
+ board.dim = self.board_width_dim
+
+ mc = MonoCalibrator([ board ], flags=cv2.CALIB_FIX_K3, pattern=setup.pattern)
+
+ if 0:
+ # display the patterns viewed by the camera
+ for pattern_warped in self.limages[i]:
+ cv2.imshow("toto", pattern_warped)
+ cv2.waitKey(0)
+
+ mc.cal(self.limages[i])
+ self.assert_good_mono(mc, self.limages[i], setup.lin_err)
+
+ # Make sure the intrinsics are similar
+ err_intrinsics = numpy.linalg.norm(mc.intrinsics - self.K, ord=numpy.inf)
+ self.assert_(err_intrinsics < setup.K_err,
+ 'intrinsics error is %f for resolution i = %d' % (err_intrinsics, i))
+ print('intrinsics error is %f' % numpy.linalg.norm(mc.intrinsics - self.K, ord=numpy.inf))
+
+if __name__ == '__main__':
+ #rosunit.unitrun('camera_calibration', 'directed', TestDirected)
+ rosunit.unitrun('camera_calibration', 'artificial', TestArtificial)
diff --git a/camera_calibration/test/multiple_boards.py b/camera_calibration/test/multiple_boards.py
new file mode 100644
index 0000000..a726a70
--- /dev/null
+++ b/camera_calibration/test/multiple_boards.py
@@ -0,0 +1,87 @@
+#!/usr/bin/env python
+#
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2009, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# * Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above
+# copyright notice, this list of conditions and the following
+# disclaimer in the documentation and/or other materials provided
+# with the distribution.
+# * Neither the name of the Willow Garage nor the names of its
+# contributors may be used to endorse or promote products derived
+# from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+import roslib
+import rostest
+import rospy
+import unittest
+import tarfile
+import copy
+import os, sys
+
+from camera_calibration.calibrator import StereoCalibrator, ChessboardInfo, image_from_archive
+
+# Large board used for PR2 calibration
+board = ChessboardInfo()
+board.n_cols = 7
+board.n_rows = 6
+board.dim = 0.108
+
+class TestMultipleBoards(unittest.TestCase):
+ def test_multiple_boards(self):
+ small_board = ChessboardInfo()
+ small_board.n_cols = 5
+ small_board.n_rows = 4
+ small_board.dim = 0.025
+
+ stereo_cal = StereoCalibrator([board, small_board])
+
+ my_archive_name = roslib.packages.find_resource('camera_calibration', 'multi_board_calibration.tar.gz')[0]
+ stereo_cal.do_tarfile_calibration(my_archive_name)
+
+ stereo_cal.report()
+ stereo_cal.ost()
+
+ # Check error for big image
+ archive = tarfile.open(my_archive_name)
+ l1_big = image_from_archive(archive, "left-0000.png")
+ r1_big = image_from_archive(archive, "right-0000.png")
+ epi_big = stereo_cal.epipolar_error_from_images(l1_big, r1_big)
+ self.assert_(epi_big < 1.0, "Epipolar error for large checkerboard > 1.0. Error: %.2f" % epi_big)
+
+ # Small checkerboard has larger error threshold for now
+ l1_sm = image_from_archive(archive, "left-0012-sm.png")
+ r1_sm = image_from_archive(archive, "right-0012-sm.png")
+ epi_sm = stereo_cal.epipolar_error_from_images(l1_sm, r1_sm)
+ self.assert_(epi_sm < 2.0, "Epipolar error for small checkerboard > 2.0. Error: %.2f" % epi_sm)
+
+
+
+if __name__ == '__main__':
+ if 1:
+ rostest.unitrun('camera_calibration', 'test_multi_board_cal', TestMultipleBoards)
+ else:
+ suite = unittest.TestSuite()
+ suite.addTest(TestMultipleBoards('test_multi_board_cal'))
+ unittest.TextTestRunner(verbosity=2).run(suite)
diff --git a/depth_image_proc/CHANGELOG.rst b/depth_image_proc/CHANGELOG.rst
new file mode 100644
index 0000000..8d33a79
--- /dev/null
+++ b/depth_image_proc/CHANGELOG.rst
@@ -0,0 +1,110 @@
+1.12.20 (2017-04-30)
+--------------------
+* Fix CMake warnings about Eigen.
+* Convert depth image metric from [m] to [mm]
+* address gcc6 build error
+ With gcc6, compiling fails with `stdlib.h: No such file or directory`,
+ as including '-isystem /usr/include' breaks with gcc6, cf.,
+ https://gcc.gnu.org/bugzilla/show_bug.cgi?id=70129.
+ This commit addresses this issue for this package in the same way
+ it was addressed in various other ROS packages. A list of related
+ commits and pull requests is at:
+ https://github.com/ros/rosdistro/issues/12783
+ Signed-off-by: Lukas Bulwahn <lukas.bulwahn at oss.bmw-carit.de>
+* Contributors: Kentaro Wada, Lukas Bulwahn, Vincent Rabaud
+
+1.12.19 (2016-07-24)
+--------------------
+
+1.12.18 (2016-07-12)
+--------------------
+
+1.12.17 (2016-07-11)
+--------------------
+
+1.12.16 (2016-03-19)
+--------------------
+* check number of channels before the process
+* search minimum value with OpenCV
+* Use OpenCV to be faster
+* Add a feature for a depth image to crop foremost image
+* Contributors: Kenta Yonekura
+
+1.12.15 (2016-01-17)
+--------------------
+* Add option for exact time sync for point_cloud_xyzrgb
+* simplify OpenCV3 conversion
+* Contributors: Kentaro Wada, Vincent Rabaud
+
+1.12.14 (2015-07-22)
+--------------------
+
+1.12.13 (2015-04-06)
+--------------------
+* Add radial point cloud processors
+* Contributors: Hunter Laux
+
+1.12.12 (2014-12-31)
+--------------------
+* adds range_max
+* exports depth_conversions
+ with convert for xyz PC only
+* exports DepthTraits
+* Contributors: enriquefernandez
+
+1.12.11 (2014-10-26)
+--------------------
+
+1.12.10 (2014-09-28)
+--------------------
+
+1.12.9 (2014-09-21)
+-------------------
+* get code to compile with OpenCV3
+ fixes `#96 <https://github.com/ros-perception/image_pipeline/issues/96>`_
+* Contributors: Vincent Rabaud
+
+1.12.8 (2014-08-19)
+-------------------
+
+1.12.6 (2014-07-27)
+-------------------
+* Add point_cloud_xyzi nodelet
+ This is for cameras that output depth and intensity images.
+ It's based on the point_cloud_xyzrgb nodelet.
+* Missing runtime dependency - eigen_conversions
+ `libdepth_image_proc` is missing this dependency at runtime
+ ```
+ > ldd libdepth_image_proc.so | grep eigen
+ libeigen_conversions.so => not found
+ ```
+ Which causes the following error on loading depth_image_proc:
+ ```
+ [ INFO] [1402564815.530736554]: /camera/rgb/camera_info -> /camera/rgb/camera_info
+ [ERROR] [1402564815.727176562]: Failed to load nodelet [/camera/depth_metric_rect] of type
+ [depth_image_proc/convert_metric]: Failed to load library /opt/ros/indigo/lib//libdepth_image_proc.so.
+ Make sure that you are calling the PLUGINLIB_EXPORT_CLASS macro in the library code, and that
+ names are consistent between this macro and your XML. Error string: Could not load library (Poco
+ exception = libeigen_conversions.so: cannot open shared object file: No such file or directory)
+ [FATAL] [1402564815.727410623]: Service call failed!
+ ```
+* Contributors: Daniel Stonier, Hunter Laux
+
+1.12.4 (2014-04-28)
+-------------------
+* depth_image_proc: fix missing symbols in nodelets
+* Contributors: Michael Ferguson
+
+1.12.3 (2014-04-12)
+-------------------
+
+1.12.2 (2014-04-08)
+-------------------
+
+1.12.1 (2014-04-06)
+-------------------
+* replace tf usage by tf2 usage
+
+1.12.0 (2014-04-04)
+-------------------
+* remove PCL dependency
diff --git a/depth_image_proc/CMakeLists.txt b/depth_image_proc/CMakeLists.txt
new file mode 100644
index 0000000..060d295
--- /dev/null
+++ b/depth_image_proc/CMakeLists.txt
@@ -0,0 +1,40 @@
+cmake_minimum_required(VERSION 2.8)
+project(depth_image_proc)
+
+find_package(catkin REQUIRED cmake_modules cv_bridge eigen_conversions image_geometry image_transport message_filters nodelet sensor_msgs stereo_msgs tf2 tf2_ros)
+
+catkin_package(
+ INCLUDE_DIRS include
+ LIBRARIES ${PROJECT_NAME})
+
+find_package(Boost REQUIRED)
+find_package(Eigen3 QUIET)
+if(NOT EIGEN3_FOUND)
+ find_package(Eigen REQUIRED)
+ set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS})
+endif()
+find_package(OpenCV REQUIRED)
+include_directories(include ${BOOST_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS})
+
+add_library(${PROJECT_NAME} src/nodelets/convert_metric.cpp
+ src/nodelets/crop_foremost.cpp
+ src/nodelets/disparity.cpp
+ src/nodelets/point_cloud_xyz.cpp
+ src/nodelets/point_cloud_xyzrgb.cpp
+ src/nodelets/point_cloud_xyzi.cpp
+ src/nodelets/point_cloud_xyz_radial.cpp
+ src/nodelets/point_cloud_xyzi_radial.cpp
+ src/nodelets/register.cpp
+)
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
+
+install(DIRECTORY include/${PROJECT_NAME}/
+ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+ FILES_MATCHING PATTERN "*.h")
+
+install(TARGETS ${PROJECT_NAME}
+ DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+)
+install(FILES nodelet_plugins.xml
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
diff --git a/depth_image_proc/include/depth_image_proc/depth_conversions.h b/depth_image_proc/include/depth_image_proc/depth_conversions.h
new file mode 100644
index 0000000..9d4ad0c
--- /dev/null
+++ b/depth_image_proc/include/depth_image_proc/depth_conversions.h
@@ -0,0 +1,101 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+#ifndef DEPTH_IMAGE_PROC_DEPTH_CONVERSIONS
+#define DEPTH_IMAGE_PROC_DEPTH_CONVERSIONS
+
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/point_cloud2_iterator.h>
+#include <image_geometry/pinhole_camera_model.h>
+#include <depth_image_proc/depth_traits.h>
+
+#include <limits>
+
+namespace depth_image_proc {
+
+typedef sensor_msgs::PointCloud2 PointCloud;
+
+// Handles float or uint16 depths
+template<typename T>
+void convert(
+ const sensor_msgs::ImageConstPtr& depth_msg,
+ PointCloud::Ptr& cloud_msg,
+ const image_geometry::PinholeCameraModel& model,
+ double range_max = 0.0)
+{
+ // Use correct principal point from calibration
+ float center_x = model.cx();
+ float center_y = model.cy();
+
+ // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y)
+ double unit_scaling = DepthTraits<T>::toMeters( T(1) );
+ float constant_x = unit_scaling / model.fx();
+ float constant_y = unit_scaling / model.fy();
+ float bad_point = std::numeric_limits<float>::quiet_NaN();
+
+ sensor_msgs::PointCloud2Iterator<float> iter_x(*cloud_msg, "x");
+ sensor_msgs::PointCloud2Iterator<float> iter_y(*cloud_msg, "y");
+ sensor_msgs::PointCloud2Iterator<float> iter_z(*cloud_msg, "z");
+ const T* depth_row = reinterpret_cast<const T*>(&depth_msg->data[0]);
+ int row_step = depth_msg->step / sizeof(T);
+ for (int v = 0; v < (int)cloud_msg->height; ++v, depth_row += row_step)
+ {
+ for (int u = 0; u < (int)cloud_msg->width; ++u, ++iter_x, ++iter_y, ++iter_z)
+ {
+ T depth = depth_row[u];
+
+ // Missing points denoted by NaNs
+ if (!DepthTraits<T>::valid(depth))
+ {
+ if (range_max != 0.0)
+ {
+ depth = DepthTraits<T>::fromMeters(range_max);
+ }
+ else
+ {
+ *iter_x = *iter_y = *iter_z = bad_point;
+ continue;
+ }
+ }
+
+ // Fill in XYZ
+ *iter_x = (u - center_x) * depth * constant_x;
+ *iter_y = (v - center_y) * depth * constant_y;
+ *iter_z = DepthTraits<T>::toMeters(depth);
+ }
+ }
+}
+
+} // namespace depth_image_proc
+
+#endif
diff --git a/depth_image_proc/include/depth_image_proc/depth_traits.h b/depth_image_proc/include/depth_image_proc/depth_traits.h
new file mode 100644
index 0000000..9c76de9
--- /dev/null
+++ b/depth_image_proc/include/depth_image_proc/depth_traits.h
@@ -0,0 +1,71 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+#ifndef DEPTH_IMAGE_PROC_DEPTH_TRAITS
+#define DEPTH_IMAGE_PROC_DEPTH_TRAITS
+
+#include <algorithm>
+#include <limits>
+
+namespace depth_image_proc {
+
+// Encapsulate differences between processing float and uint16_t depths
+template<typename T> struct DepthTraits {};
+
+template<>
+struct DepthTraits<uint16_t>
+{
+ static inline bool valid(uint16_t depth) { return depth != 0; }
+ static inline float toMeters(uint16_t depth) { return depth * 0.001f; } // originally mm
+ static inline uint16_t fromMeters(float depth) { return (depth * 1000.0f) + 0.5f; }
+ static inline void initializeBuffer(std::vector<uint8_t>& buffer) {} // Do nothing - already zero-filled
+};
+
+template<>
+struct DepthTraits<float>
+{
+ static inline bool valid(float depth) { return std::isfinite(depth); }
+ static inline float toMeters(float depth) { return depth; }
+ static inline float fromMeters(float depth) { return depth; }
+
+ static inline void initializeBuffer(std::vector<uint8_t>& buffer)
+ {
+ float* start = reinterpret_cast<float*>(&buffer[0]);
+ float* end = reinterpret_cast<float*>(&buffer[0] + buffer.size());
+ std::fill(start, end, std::numeric_limits<float>::quiet_NaN());
+ }
+};
+
+} // namespace depth_image_proc
+
+#endif
diff --git a/depth_image_proc/mainpage.dox b/depth_image_proc/mainpage.dox
new file mode 100644
index 0000000..6b0414b
--- /dev/null
+++ b/depth_image_proc/mainpage.dox
@@ -0,0 +1,26 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b depth_image_proc is ...
+
+<!--
+Provide an overview of your package.
+-->
+
+
+\section codeapi Code API
+
+<!--
+Provide links to specific auto-generated API documentation within your
+package that is of particular interest to a reader. Doxygen will
+document pretty much every part of your code, so do your best here to
+point the reader to the actual API.
+
+If your codebase is fairly large or has different sets of APIs, you
+should use the doxygen 'group' tag to keep these APIs together. For
+example, the roscpp documentation has 'libros' group.
+-->
+
+
+*/
diff --git a/depth_image_proc/nodelet_plugins.xml b/depth_image_proc/nodelet_plugins.xml
new file mode 100644
index 0000000..de29b96
--- /dev/null
+++ b/depth_image_proc/nodelet_plugins.xml
@@ -0,0 +1,73 @@
+<library path="lib/libdepth_image_proc">
+
+ <class name="depth_image_proc/convert_metric"
+ type="depth_image_proc::ConvertMetricNodelet"
+ base_class_type="nodelet::Nodelet">
+ <description>
+ Nodelet to convert raw uint16 depth image in mm to float depth image in m.
+ </description>
+ </class>
+
+ <class name="depth_image_proc/disparity"
+ type="depth_image_proc::DisparityNodelet"
+ base_class_type="nodelet::Nodelet">
+ <description>
+ Nodelet to convert depth image to disparity image.
+ </description>
+ </class>
+
+ <class name="depth_image_proc/point_cloud_xyz"
+ type="depth_image_proc::PointCloudXyzNodelet"
+ base_class_type="nodelet::Nodelet">
+ <description>
+ Nodelet to convert depth image to XYZ point cloud.
+ </description>
+ </class>
+
+ <class name="depth_image_proc/point_cloud_xyzrgb"
+ type="depth_image_proc::PointCloudXyzrgbNodelet"
+ base_class_type="nodelet::Nodelet">
+ <description>
+ Nodelet to combine registered depth image and RGB image into XYZRGB point cloud.
+ </description>
+ </class>
+
+ <class name="depth_image_proc/point_cloud_xyzi"
+ type="depth_image_proc::PointCloudXyziNodelet"
+ base_class_type="nodelet::Nodelet">
+ <description>
+ Nodelet to combine registered depth image and intensity image into XYZI point cloud.
+ </description>
+ </class>
+ <class name="depth_image_proc/point_cloud_xyz_radial"
+ type="depth_image_proc::PointCloudXyzRadialNodelet"
+ base_class_type="nodelet::Nodelet">
+ <description>
+ Nodelet to convert an Radial depth map to a point.
+ </description>
+ </class>
+
+ <class name="depth_image_proc/point_cloud_xyzi_radial"
+ type="depth_image_proc::PointCloudXyziRadialNodelet"
+ base_class_type="nodelet::Nodelet">
+ <description>
+ Nodelet to convert an Radial depth and intensity map to a point.
+ </description>
+ </class>
+ <class name="depth_image_proc/register"
+ type="depth_image_proc::RegisterNodelet"
+ base_class_type="nodelet::Nodelet">
+ <description>
+ Nodelet to create a depth image registered to another camera frame.
+ </description>
+ </class>
+
+ <class name="depth_image_proc/crop_foremost"
+ type="depth_image_proc::CropForemostNodelet"
+ base_class_type="nodelet::Nodelet">
+ <description>
+ Nodelet to crop a depth image from foremost depth to a specific range.
+ </description>
+ </class>
+
+</library>
diff --git a/depth_image_proc/package.xml b/depth_image_proc/package.xml
new file mode 100644
index 0000000..4ac94ba
--- /dev/null
+++ b/depth_image_proc/package.xml
@@ -0,0 +1,46 @@
+<package>
+ <name>depth_image_proc</name>
+ <version>1.12.20</version>
+ <description>
+
+ Contains nodelets for processing depth images such as those
+ produced by OpenNI camera. Functions include creating disparity
+ images and point clouds, as well as registering (reprojecting)
+ a depth image into another camera frame.
+
+ </description>
+ <author>Patrick Mihelich</author>
+ <maintainer email="vincent.rabaud at gmail.com">Vincent Rabaud</maintainer>
+ <license>BSD</license>
+ <url>http://ros.org/wiki/depth_image_proc</url>
+
+ <export>
+ <nodelet plugin="${prefix}/nodelet_plugins.xml" />
+ </export>
+
+ <buildtool_depend>catkin</buildtool_depend>
+
+ <test_depend>rostest</test_depend>
+
+ <build_depend>boost</build_depend>
+ <build_depend>cmake_modules</build_depend>
+ <build_depend>cv_bridge</build_depend>
+ <build_depend>eigen_conversions</build_depend>
+ <build_depend>image_geometry</build_depend>
+ <build_depend>image_transport</build_depend>
+ <build_depend>message_filters</build_depend>
+ <build_depend>nodelet</build_depend>
+ <build_depend>sensor_msgs</build_depend>
+ <build_depend>stereo_msgs</build_depend>
+ <build_depend>tf2</build_depend>
+ <build_depend>tf2_ros</build_depend>
+
+ <run_depend>boost</run_depend>
+ <run_depend>cv_bridge</run_depend>
+ <run_depend>eigen_conversions</run_depend>
+ <run_depend>image_geometry</run_depend>
+ <run_depend>image_transport</run_depend>
+ <run_depend>nodelet</run_depend>
+ <run_depend>tf2</run_depend>
+ <run_depend>tf2_ros</run_depend>
+</package>
diff --git a/depth_image_proc/src/nodelets/convert_metric.cpp b/depth_image_proc/src/nodelets/convert_metric.cpp
new file mode 100644
index 0000000..d5d6676
--- /dev/null
+++ b/depth_image_proc/src/nodelets/convert_metric.cpp
@@ -0,0 +1,140 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+#include <ros/ros.h>
+#include <nodelet/nodelet.h>
+#include <image_transport/image_transport.h>
+#include <sensor_msgs/image_encodings.h>
+#include <boost/thread.hpp>
+
+namespace depth_image_proc {
+
+namespace enc = sensor_msgs::image_encodings;
+
+class ConvertMetricNodelet : public nodelet::Nodelet
+{
+ // Subscriptions
+ boost::shared_ptr<image_transport::ImageTransport> it_;
+ image_transport::Subscriber sub_raw_;
+
+ // Publications
+ boost::mutex connect_mutex_;
+ image_transport::Publisher pub_depth_;
+
+ virtual void onInit();
+
+ void connectCb();
+
+ void depthCb(const sensor_msgs::ImageConstPtr& raw_msg);
+};
+
+void ConvertMetricNodelet::onInit()
+{
+ ros::NodeHandle& nh = getNodeHandle();
+ it_.reset(new image_transport::ImageTransport(nh));
+
+ // Monitor whether anyone is subscribed to the output
+ image_transport::SubscriberStatusCallback connect_cb = boost::bind(&ConvertMetricNodelet::connectCb, this);
+ // Make sure we don't enter connectCb() between advertising and assigning to pub_depth_
+ boost::lock_guard<boost::mutex> lock(connect_mutex_);
+ pub_depth_ = it_->advertise("image", 1, connect_cb, connect_cb);
+}
+
+// Handles (un)subscribing when clients (un)subscribe
+void ConvertMetricNodelet::connectCb()
+{
+ boost::lock_guard<boost::mutex> lock(connect_mutex_);
+ if (pub_depth_.getNumSubscribers() == 0)
+ {
+ sub_raw_.shutdown();
+ }
+ else if (!sub_raw_)
+ {
+ image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle());
+ sub_raw_ = it_->subscribe("image_raw", 1, &ConvertMetricNodelet::depthCb, this, hints);
+ }
+}
+
+void ConvertMetricNodelet::depthCb(const sensor_msgs::ImageConstPtr& raw_msg)
+{
+ // Allocate new Image message
+ sensor_msgs::ImagePtr depth_msg( new sensor_msgs::Image );
+ depth_msg->header = raw_msg->header;
+ depth_msg->height = raw_msg->height;
+ depth_msg->width = raw_msg->width;
+
+ // Set data, encoding and step after converting the metric.
+ if (raw_msg->encoding == enc::TYPE_16UC1)
+ {
+ depth_msg->encoding = enc::TYPE_32FC1;
+ depth_msg->step = raw_msg->width * (enc::bitDepth(depth_msg->encoding) / 8);
+ depth_msg->data.resize(depth_msg->height * depth_msg->step);
+ // Fill in the depth image data, converting mm to m
+ float bad_point = std::numeric_limits<float>::quiet_NaN ();
+ const uint16_t* raw_data = reinterpret_cast<const uint16_t*>(&raw_msg->data[0]);
+ float* depth_data = reinterpret_cast<float*>(&depth_msg->data[0]);
+ for (unsigned index = 0; index < depth_msg->height * depth_msg->width; ++index)
+ {
+ uint16_t raw = raw_data[index];
+ depth_data[index] = (raw == 0) ? bad_point : (float)raw * 0.001f;
+ }
+ }
+ else if (raw_msg->encoding == enc::TYPE_32FC1)
+ {
+ depth_msg->encoding = enc::TYPE_16UC1;
+ depth_msg->step = raw_msg->width * (enc::bitDepth(depth_msg->encoding) / 8);
+ depth_msg->data.resize(depth_msg->height * depth_msg->step);
+ // Fill in the depth image data, converting m to mm
+ uint16_t bad_point = 0;
+ const float* raw_data = reinterpret_cast<const float*>(&raw_msg->data[0]);
+ uint16_t* depth_data = reinterpret_cast<uint16_t*>(&depth_msg->data[0]);
+ for (unsigned index = 0; index < depth_msg->height * depth_msg->width; ++index)
+ {
+ float raw = raw_data[index];
+ depth_data[index] = std::isnan(raw) ? bad_point : (uint16_t)(raw * 1000);
+ }
+ }
+ else
+ {
+ ROS_ERROR("Unsupported image conversion from %s.", raw_msg->encoding.c_str());
+ return;
+ }
+
+ pub_depth_.publish(depth_msg);
+}
+
+} // namespace depth_image_proc
+
+// Register as nodelet
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(depth_image_proc::ConvertMetricNodelet,nodelet::Nodelet);
diff --git a/depth_image_proc/src/nodelets/crop_foremost.cpp b/depth_image_proc/src/nodelets/crop_foremost.cpp
new file mode 100755
index 0000000..68ed8e2
--- /dev/null
+++ b/depth_image_proc/src/nodelets/crop_foremost.cpp
@@ -0,0 +1,142 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+//#include <ros/ros.h>
+#include <nodelet/nodelet.h>
+#include <image_transport/image_transport.h>
+#include <boost/thread.hpp>
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/imgproc/imgproc.hpp>
+
+namespace depth_image_proc {
+
+namespace enc = sensor_msgs::image_encodings;
+
+class CropForemostNodelet : public nodelet::Nodelet
+{
+ // Subscriptions
+ boost::shared_ptr<image_transport::ImageTransport> it_;
+ image_transport::Subscriber sub_raw_;
+
+ // Publications
+ boost::mutex connect_mutex_;
+ image_transport::Publisher pub_depth_;
+
+ virtual void onInit();
+
+ void connectCb();
+
+ void depthCb(const sensor_msgs::ImageConstPtr& raw_msg);
+
+ double distance_;
+};
+
+void CropForemostNodelet::onInit()
+{
+ ros::NodeHandle& nh = getNodeHandle();
+ ros::NodeHandle& private_nh = getPrivateNodeHandle();
+ private_nh.getParam("distance", distance_);
+ it_.reset(new image_transport::ImageTransport(nh));
+
+ // Monitor whether anyone is subscribed to the output
+ image_transport::SubscriberStatusCallback connect_cb = boost::bind(&CropForemostNodelet::connectCb, this);
+ // Make sure we don't enter connectCb() between advertising and assigning to pub_depth_
+ boost::lock_guard<boost::mutex> lock(connect_mutex_);
+ pub_depth_ = it_->advertise("image", 1, connect_cb, connect_cb);
+}
+
+// Handles (un)subscribing when clients (un)subscribe
+void CropForemostNodelet::connectCb()
+{
+ boost::lock_guard<boost::mutex> lock(connect_mutex_);
+ if (pub_depth_.getNumSubscribers() == 0)
+ {
+ sub_raw_.shutdown();
+ }
+ else if (!sub_raw_)
+ {
+ image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle());
+ sub_raw_ = it_->subscribe("image_raw", 1, &CropForemostNodelet::depthCb, this, hints);
+ }
+}
+
+void CropForemostNodelet::depthCb(const sensor_msgs::ImageConstPtr& raw_msg)
+{
+ cv_bridge::CvImagePtr cv_ptr;
+ try
+ {
+ cv_ptr = cv_bridge::toCvCopy(raw_msg);
+ }
+ catch (cv_bridge::Exception& e)
+ {
+ ROS_ERROR("cv_bridge exception: %s", e.what());
+ return;
+ }
+
+ // Check the number of channels
+ if(sensor_msgs::image_encodings::numChannels(raw_msg->encoding) != 1){
+ NODELET_ERROR_THROTTLE(2, "Only grayscale image is acceptable, got [%s]", raw_msg->encoding.c_str());
+ return;
+ }
+
+ // search the min value without invalid value "0"
+ double minVal;
+ cv::minMaxIdx(cv_ptr->image, &minVal, 0, 0, 0, cv_ptr->image != 0);
+
+ int imtype = cv_bridge::getCvType(raw_msg->encoding);
+ switch (imtype){
+ case CV_8UC1:
+ case CV_8SC1:
+ case CV_32F:
+ cv::threshold(cv_ptr->image, cv_ptr->image, minVal + distance_, 0, CV_THRESH_TOZERO_INV);
+ break;
+ case CV_16UC1:
+ case CV_16SC1:
+ case CV_32SC1:
+ case CV_64F:
+ // 8 bit or 32 bit floating array is required to use cv::threshold
+ cv_ptr->image.convertTo(cv_ptr->image, CV_32F);
+ cv::threshold(cv_ptr->image, cv_ptr->image, minVal + distance_, 1, CV_THRESH_TOZERO_INV);
+
+ cv_ptr->image.convertTo(cv_ptr->image, imtype);
+ break;
+ }
+
+ pub_depth_.publish(cv_ptr->toImageMsg());
+}
+
+} // namespace depth_image_proc
+
+// Register as nodelet
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(depth_image_proc::CropForemostNodelet,nodelet::Nodelet);
diff --git a/depth_image_proc/src/nodelets/disparity.cpp b/depth_image_proc/src/nodelets/disparity.cpp
new file mode 100644
index 0000000..dd82537
--- /dev/null
+++ b/depth_image_proc/src/nodelets/disparity.cpp
@@ -0,0 +1,189 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+#include <boost/version.hpp>
+#if ((BOOST_VERSION / 100) % 1000) >= 53
+#include <boost/thread/lock_guard.hpp>
+#endif
+
+#include <ros/ros.h>
+#include <nodelet/nodelet.h>
+#include <image_transport/image_transport.h>
+#include <image_transport/subscriber_filter.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/time_synchronizer.h>
+#include <sensor_msgs/image_encodings.h>
+#include <stereo_msgs/DisparityImage.h>
+#include <depth_image_proc/depth_traits.h>
+
+namespace depth_image_proc {
+
+namespace enc = sensor_msgs::image_encodings;
+
+class DisparityNodelet : public nodelet::Nodelet
+{
+ boost::shared_ptr<image_transport::ImageTransport> left_it_;
+ ros::NodeHandlePtr right_nh_;
+ image_transport::SubscriberFilter sub_depth_image_;
+ message_filters::Subscriber<sensor_msgs::CameraInfo> sub_info_;
+ typedef message_filters::TimeSynchronizer<sensor_msgs::Image, sensor_msgs::CameraInfo> Sync;
+ boost::shared_ptr<Sync> sync_;
+
+ boost::mutex connect_mutex_;
+ ros::Publisher pub_disparity_;
+ double min_range_;
+ double max_range_;
+ double delta_d_;
+
+ virtual void onInit();
+
+ void connectCb();
+
+ void depthCb(const sensor_msgs::ImageConstPtr& depth_msg,
+ const sensor_msgs::CameraInfoConstPtr& info_msg);
+
+ template<typename T>
+ void convert(const sensor_msgs::ImageConstPtr& depth_msg,
+ stereo_msgs::DisparityImagePtr& disp_msg);
+};
+
+void DisparityNodelet::onInit()
+{
+ ros::NodeHandle &nh = getNodeHandle();
+ ros::NodeHandle &private_nh = getPrivateNodeHandle();
+ ros::NodeHandle left_nh(nh, "left");
+ left_it_.reset(new image_transport::ImageTransport(left_nh));
+ right_nh_.reset( new ros::NodeHandle(nh, "right") );
+
+ // Read parameters
+ int queue_size;
+ private_nh.param("queue_size", queue_size, 5);
+ private_nh.param("min_range", min_range_, 0.0);
+ private_nh.param("max_range", max_range_, std::numeric_limits<double>::infinity());
+ private_nh.param("delta_d", delta_d_, 0.125);
+
+ // Synchronize inputs. Topic subscriptions happen on demand in the connection callback.
+ sync_.reset( new Sync(sub_depth_image_, sub_info_, queue_size) );
+ sync_->registerCallback(boost::bind(&DisparityNodelet::depthCb, this, _1, _2));
+
+ // Monitor whether anyone is subscribed to the output
+ ros::SubscriberStatusCallback connect_cb = boost::bind(&DisparityNodelet::connectCb, this);
+ // Make sure we don't enter connectCb() between advertising and assigning to pub_disparity_
+ boost::lock_guard<boost::mutex> lock(connect_mutex_);
+ pub_disparity_ = left_nh.advertise<stereo_msgs::DisparityImage>("disparity", 1, connect_cb, connect_cb);
+}
+
+// Handles (un)subscribing when clients (un)subscribe
+void DisparityNodelet::connectCb()
+{
+ boost::lock_guard<boost::mutex> lock(connect_mutex_);
+ if (pub_disparity_.getNumSubscribers() == 0)
+ {
+ sub_depth_image_.unsubscribe();
+ sub_info_ .unsubscribe();
+ }
+ else if (!sub_depth_image_.getSubscriber())
+ {
+ image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle());
+ sub_depth_image_.subscribe(*left_it_, "image_rect", 1, hints);
+ sub_info_.subscribe(*right_nh_, "camera_info", 1);
+ }
+}
+
+void DisparityNodelet::depthCb(const sensor_msgs::ImageConstPtr& depth_msg,
+ const sensor_msgs::CameraInfoConstPtr& info_msg)
+{
+ // Allocate new DisparityImage message
+ stereo_msgs::DisparityImagePtr disp_msg( new stereo_msgs::DisparityImage );
+ disp_msg->header = depth_msg->header;
+ disp_msg->image.header = disp_msg->header;
+ disp_msg->image.encoding = enc::TYPE_32FC1;
+ disp_msg->image.height = depth_msg->height;
+ disp_msg->image.width = depth_msg->width;
+ disp_msg->image.step = disp_msg->image.width * sizeof (float);
+ disp_msg->image.data.resize( disp_msg->image.height * disp_msg->image.step, 0.0f );
+ double fx = info_msg->P[0];
+ disp_msg->T = -info_msg->P[3] / fx;
+ disp_msg->f = fx;
+ // Remaining fields depend on device characteristics, so rely on user input
+ disp_msg->min_disparity = disp_msg->f * disp_msg->T / max_range_;
+ disp_msg->max_disparity = disp_msg->f * disp_msg->T / min_range_;
+ disp_msg->delta_d = delta_d_;
+
+ if (depth_msg->encoding == enc::TYPE_16UC1)
+ {
+ convert<uint16_t>(depth_msg, disp_msg);
+ }
+ else if (depth_msg->encoding == enc::TYPE_32FC1)
+ {
+ convert<float>(depth_msg, disp_msg);
+ }
+ else
+ {
+ NODELET_ERROR_THROTTLE(5, "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str());
+ return;
+ }
+
+ pub_disparity_.publish(disp_msg);
+}
+
+template<typename T>
+void DisparityNodelet::convert(const sensor_msgs::ImageConstPtr& depth_msg,
+ stereo_msgs::DisparityImagePtr& disp_msg)
+{
+ // For each depth Z, disparity d = fT / Z
+ float unit_scaling = DepthTraits<T>::toMeters( T(1) );
+ float constant = disp_msg->f * disp_msg->T / unit_scaling;
+
+ const T* depth_row = reinterpret_cast<const T*>(&depth_msg->data[0]);
+ int row_step = depth_msg->step / sizeof(T);
+ float* disp_data = reinterpret_cast<float*>(&disp_msg->image.data[0]);
+ for (int v = 0; v < (int)depth_msg->height; ++v)
+ {
+ for (int u = 0; u < (int)depth_msg->width; ++u)
+ {
+ T depth = depth_row[u];
+ if (DepthTraits<T>::valid(depth))
+ *disp_data = constant / depth;
+ ++disp_data;
+ }
+
+ depth_row += row_step;
+ }
+}
+
+} // namespace depth_image_proc
+
+// Register as nodelet
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(depth_image_proc::DisparityNodelet,nodelet::Nodelet);
diff --git a/depth_image_proc/src/nodelets/point_cloud_xyz.cpp b/depth_image_proc/src/nodelets/point_cloud_xyz.cpp
new file mode 100644
index 0000000..084ae73
--- /dev/null
+++ b/depth_image_proc/src/nodelets/point_cloud_xyz.cpp
@@ -0,0 +1,138 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+#include <ros/ros.h>
+#include <nodelet/nodelet.h>
+#include <image_transport/image_transport.h>
+#include <sensor_msgs/image_encodings.h>
+#include <image_geometry/pinhole_camera_model.h>
+#include <boost/thread.hpp>
+#include <depth_image_proc/depth_conversions.h>
+
+#include <sensor_msgs/point_cloud2_iterator.h>
+
+namespace depth_image_proc {
+
+namespace enc = sensor_msgs::image_encodings;
+
+class PointCloudXyzNodelet : public nodelet::Nodelet
+{
+ // Subscriptions
+ boost::shared_ptr<image_transport::ImageTransport> it_;
+ image_transport::CameraSubscriber sub_depth_;
+ int queue_size_;
+
+ // Publications
+ boost::mutex connect_mutex_;
+ typedef sensor_msgs::PointCloud2 PointCloud;
+ ros::Publisher pub_point_cloud_;
+
+ image_geometry::PinholeCameraModel model_;
+
+ virtual void onInit();
+
+ void connectCb();
+
+ void depthCb(const sensor_msgs::ImageConstPtr& depth_msg,
+ const sensor_msgs::CameraInfoConstPtr& info_msg);
+};
+
+void PointCloudXyzNodelet::onInit()
+{
+ ros::NodeHandle& nh = getNodeHandle();
+ ros::NodeHandle& private_nh = getPrivateNodeHandle();
+ it_.reset(new image_transport::ImageTransport(nh));
+
+ // Read parameters
+ private_nh.param("queue_size", queue_size_, 5);
+
+ // Monitor whether anyone is subscribed to the output
+ ros::SubscriberStatusCallback connect_cb = boost::bind(&PointCloudXyzNodelet::connectCb, this);
+ // Make sure we don't enter connectCb() between advertising and assigning to pub_point_cloud_
+ boost::lock_guard<boost::mutex> lock(connect_mutex_);
+ pub_point_cloud_ = nh.advertise<PointCloud>("points", 1, connect_cb, connect_cb);
+}
+
+// Handles (un)subscribing when clients (un)subscribe
+void PointCloudXyzNodelet::connectCb()
+{
+ boost::lock_guard<boost::mutex> lock(connect_mutex_);
+ if (pub_point_cloud_.getNumSubscribers() == 0)
+ {
+ sub_depth_.shutdown();
+ }
+ else if (!sub_depth_)
+ {
+ image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle());
+ sub_depth_ = it_->subscribeCamera("image_rect", queue_size_, &PointCloudXyzNodelet::depthCb, this, hints);
+ }
+}
+
+void PointCloudXyzNodelet::depthCb(const sensor_msgs::ImageConstPtr& depth_msg,
+ const sensor_msgs::CameraInfoConstPtr& info_msg)
+{
+ PointCloud::Ptr cloud_msg(new PointCloud);
+ cloud_msg->header = depth_msg->header;
+ cloud_msg->height = depth_msg->height;
+ cloud_msg->width = depth_msg->width;
+ cloud_msg->is_dense = false;
+ cloud_msg->is_bigendian = false;
+
+ sensor_msgs::PointCloud2Modifier pcd_modifier(*cloud_msg);
+ pcd_modifier.setPointCloud2FieldsByString(1, "xyz");
+
+ // Update camera model
+ model_.fromCameraInfo(info_msg);
+
+ if (depth_msg->encoding == enc::TYPE_16UC1)
+ {
+ convert<uint16_t>(depth_msg, cloud_msg, model_);
+ }
+ else if (depth_msg->encoding == enc::TYPE_32FC1)
+ {
+ convert<float>(depth_msg, cloud_msg, model_);
+ }
+ else
+ {
+ NODELET_ERROR_THROTTLE(5, "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str());
+ return;
+ }
+
+ pub_point_cloud_.publish (cloud_msg);
+}
+
+} // namespace depth_image_proc
+
+// Register as nodelet
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(depth_image_proc::PointCloudXyzNodelet,nodelet::Nodelet);
diff --git a/depth_image_proc/src/nodelets/point_cloud_xyz_radial.cpp b/depth_image_proc/src/nodelets/point_cloud_xyz_radial.cpp
new file mode 100644
index 0000000..847cbfd
--- /dev/null
+++ b/depth_image_proc/src/nodelets/point_cloud_xyz_radial.cpp
@@ -0,0 +1,235 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the Willow Garage nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+#include <ros/ros.h>
+#include <nodelet/nodelet.h>
+#include <image_transport/image_transport.h>
+#include <sensor_msgs/image_encodings.h>
+#include <image_geometry/pinhole_camera_model.h>
+#include <boost/thread.hpp>
+#include <depth_image_proc/depth_traits.h>
+
+#include <sensor_msgs/point_cloud2_iterator.h>
+
+namespace depth_image_proc {
+
+ namespace enc = sensor_msgs::image_encodings;
+
+ class PointCloudXyzRadialNodelet : public nodelet::Nodelet
+ {
+ // Subscriptions
+ boost::shared_ptr<image_transport::ImageTransport> it_;
+ image_transport::CameraSubscriber sub_depth_;
+ int queue_size_;
+
+ // Publications
+ boost::mutex connect_mutex_;
+ typedef sensor_msgs::PointCloud2 PointCloud;
+ ros::Publisher pub_point_cloud_;
+
+
+ std::vector<double> D_;
+ boost::array<double, 9> K_;
+
+ int width_;
+ int height_;
+
+ cv::Mat binned;
+
+ virtual void onInit();
+
+ void connectCb();
+
+ void depthCb(const sensor_msgs::ImageConstPtr& depth_msg,
+ const sensor_msgs::CameraInfoConstPtr& info_msg);
+
+ // Handles float or uint16 depths
+ template<typename T>
+ void convert(const sensor_msgs::ImageConstPtr& depth_msg, PointCloud::Ptr& cloud_msg);
+ };
+
+ cv::Mat initMatrix(cv::Mat cameraMatrix, cv::Mat distCoeffs, int width, int height, bool radial)
+ {
+ int i,j;
+ int totalsize = width*height;
+ cv::Mat pixelVectors(1,totalsize,CV_32FC3);
+ cv::Mat dst(1,totalsize,CV_32FC3);
+
+ cv::Mat sensorPoints(cv::Size(height,width), CV_32FC2);
+ cv::Mat undistortedSensorPoints(1,totalsize, CV_32FC2);
+
+ std::vector<cv::Mat> ch;
+ for(j = 0; j < height; j++)
+ {
+ for(i = 0; i < width; i++)
+ {
+ cv::Vec2f &p = sensorPoints.at<cv::Vec2f>(i,j);
+ p[0] = i;
+ p[1] = j;
+ }
+ }
+
+ sensorPoints = sensorPoints.reshape(2,1);
+
+ cv::undistortPoints(sensorPoints, undistortedSensorPoints, cameraMatrix, distCoeffs);
+
+ ch.push_back(undistortedSensorPoints);
+ ch.push_back(cv::Mat::ones(1,totalsize,CV_32FC1));
+ cv::merge(ch,pixelVectors);
+
+ if(radial)
+ {
+ for(i = 0; i < totalsize; i++)
+ {
+ normalize(pixelVectors.at<cv::Vec3f>(i),
+ dst.at<cv::Vec3f>(i));
+ }
+ pixelVectors = dst;
+ }
+ return pixelVectors.reshape(3,width);
+ }
+
+
+ void PointCloudXyzRadialNodelet::onInit()
+ {
+ ros::NodeHandle& nh = getNodeHandle();
+ ros::NodeHandle& private_nh = getPrivateNodeHandle();
+ it_.reset(new image_transport::ImageTransport(nh));
+
+ // Read parameters
+ private_nh.param("queue_size", queue_size_, 5);
+
+ // Monitor whether anyone is subscribed to the output
+ ros::SubscriberStatusCallback connect_cb =
+ boost::bind(&PointCloudXyzRadialNodelet::connectCb, this);
+ // Make sure we don't enter connectCb() between advertising and assigning to pub_point_cloud_
+ boost::lock_guard<boost::mutex> lock(connect_mutex_);
+ pub_point_cloud_ = nh.advertise<PointCloud>("points", 1, connect_cb, connect_cb);
+ }
+
+ // Handles (un)subscribing when clients (un)subscribe
+ void PointCloudXyzRadialNodelet::connectCb()
+ {
+ boost::lock_guard<boost::mutex> lock(connect_mutex_);
+ if (pub_point_cloud_.getNumSubscribers() == 0)
+ {
+ sub_depth_.shutdown();
+ }
+ else if (!sub_depth_)
+ {
+ image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle());
+ sub_depth_ = it_->subscribeCamera("image_raw",
+ queue_size_,
+ &PointCloudXyzRadialNodelet::depthCb,
+ this, hints);
+ }
+ }
+
+ void PointCloudXyzRadialNodelet::depthCb(const sensor_msgs::ImageConstPtr& depth_msg,
+ const sensor_msgs::CameraInfoConstPtr& info_msg)
+ {
+ PointCloud::Ptr cloud_msg(new PointCloud);
+ cloud_msg->header = depth_msg->header;
+ cloud_msg->height = depth_msg->height;
+ cloud_msg->width = depth_msg->width;
+ cloud_msg->is_dense = false;
+ cloud_msg->is_bigendian = false;
+
+ sensor_msgs::PointCloud2Modifier pcd_modifier(*cloud_msg);
+ pcd_modifier.setPointCloud2FieldsByString(1, "xyz");
+
+ if(info_msg->D != D_ || info_msg->K != K_ || width_ != info_msg->width ||
+ height_ != info_msg->height)
+ {
+ D_ = info_msg->D;
+ K_ = info_msg->K;
+ width_ = info_msg->width;
+ height_ = info_msg->height;
+ binned = initMatrix(cv::Mat_<double>(3, 3, &K_[0]),cv::Mat(D_),width_,height_,true);
+ }
+
+ if (depth_msg->encoding == enc::TYPE_16UC1)
+ {
+ convert<uint16_t>(depth_msg, cloud_msg);
+ }
+ else if (depth_msg->encoding == enc::TYPE_32FC1)
+ {
+ convert<float>(depth_msg, cloud_msg);
+ }
+ else
+ {
+ NODELET_ERROR_THROTTLE(5, "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str());
+ return;
+ }
+
+ pub_point_cloud_.publish (cloud_msg);
+ }
+
+ template<typename T>
+ void PointCloudXyzRadialNodelet::convert(const sensor_msgs::ImageConstPtr& depth_msg, PointCloud::Ptr& cloud_msg)
+ {
+ // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y)
+ double unit_scaling = DepthTraits<T>::toMeters( T(1) );
+ float bad_point = std::numeric_limits<float>::quiet_NaN();
+
+ sensor_msgs::PointCloud2Iterator<float> iter_x(*cloud_msg, "x");
+ sensor_msgs::PointCloud2Iterator<float> iter_y(*cloud_msg, "y");
+ sensor_msgs::PointCloud2Iterator<float> iter_z(*cloud_msg, "z");
+ const T* depth_row = reinterpret_cast<const T*>(&depth_msg->data[0]);
+ int row_step = depth_msg->step / sizeof(T);
+ for (int v = 0; v < (int)cloud_msg->height; ++v, depth_row += row_step)
+ {
+ for (int u = 0; u < (int)cloud_msg->width; ++u, ++iter_x, ++iter_y, ++iter_z)
+ {
+ T depth = depth_row[u];
+
+ // Missing points denoted by NaNs
+ if (!DepthTraits<T>::valid(depth))
+ {
+ *iter_x = *iter_y = *iter_z = bad_point;
+ continue;
+ }
+ const cv::Vec3f &cvPoint = binned.at<cv::Vec3f>(u,v) * DepthTraits<T>::toMeters(depth);
+ // Fill in XYZ
+ *iter_x = cvPoint(0);
+ *iter_y = cvPoint(1);
+ *iter_z = cvPoint(2);
+ }
+ }
+ }
+
+} // namespace depth_image_proc
+
+// Register as nodelet
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(depth_image_proc::PointCloudXyzRadialNodelet,nodelet::Nodelet);
diff --git a/depth_image_proc/src/nodelets/point_cloud_xyzi.cpp b/depth_image_proc/src/nodelets/point_cloud_xyzi.cpp
new file mode 100644
index 0000000..9260442
--- /dev/null
+++ b/depth_image_proc/src/nodelets/point_cloud_xyzi.cpp
@@ -0,0 +1,316 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+#include <boost/version.hpp>
+#if ((BOOST_VERSION / 100) % 1000) >= 53
+#include <boost/thread/lock_guard.hpp>
+#endif
+
+#include <ros/ros.h>
+#include <nodelet/nodelet.h>
+#include <image_transport/image_transport.h>
+#include <image_transport/subscriber_filter.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/synchronizer.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <sensor_msgs/image_encodings.h>
+#include <sensor_msgs/point_cloud2_iterator.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <image_geometry/pinhole_camera_model.h>
+#include <depth_image_proc/depth_traits.h>
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/imgproc/imgproc.hpp>
+
+namespace depth_image_proc {
+
+using namespace message_filters::sync_policies;
+namespace enc = sensor_msgs::image_encodings;
+
+class PointCloudXyziNodelet : public nodelet::Nodelet
+{
+ ros::NodeHandlePtr intensity_nh_;
+ boost::shared_ptr<image_transport::ImageTransport> intensity_it_, depth_it_;
+
+ // Subscriptions
+ image_transport::SubscriberFilter sub_depth_, sub_intensity_;
+ message_filters::Subscriber<sensor_msgs::CameraInfo> sub_info_;
+ typedef ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> SyncPolicy;
+ typedef message_filters::Synchronizer<SyncPolicy> Synchronizer;
+ boost::shared_ptr<Synchronizer> sync_;
+
+ // Publications
+ boost::mutex connect_mutex_;
+ typedef sensor_msgs::PointCloud2 PointCloud;
+ ros::Publisher pub_point_cloud_;
+
+ image_geometry::PinholeCameraModel model_;
+
+ virtual void onInit();
+
+ void connectCb();
+
+ void imageCb(const sensor_msgs::ImageConstPtr& depth_msg,
+ const sensor_msgs::ImageConstPtr& intensity_msg,
+ const sensor_msgs::CameraInfoConstPtr& info_msg);
+
+ template<typename T, typename T2>
+ void convert(const sensor_msgs::ImageConstPtr& depth_msg,
+ const sensor_msgs::ImageConstPtr& intensity_msg,
+ const PointCloud::Ptr& cloud_msg);
+};
+
+void PointCloudXyziNodelet::onInit()
+{
+ ros::NodeHandle& nh = getNodeHandle();
+ ros::NodeHandle& private_nh = getPrivateNodeHandle();
+ intensity_nh_.reset( new ros::NodeHandle(nh, "intensity") );
+ ros::NodeHandle depth_nh(nh, "depth");
+ intensity_it_ .reset( new image_transport::ImageTransport(*intensity_nh_) );
+ depth_it_.reset( new image_transport::ImageTransport(depth_nh) );
+
+ // Read parameters
+ int queue_size;
+ private_nh.param("queue_size", queue_size, 5);
+
+ // Synchronize inputs. Topic subscriptions happen on demand in the connection callback.
+ sync_.reset( new Synchronizer(SyncPolicy(queue_size), sub_depth_, sub_intensity_, sub_info_) );
+ sync_->registerCallback(boost::bind(&PointCloudXyziNodelet::imageCb, this, _1, _2, _3));
+
+ // Monitor whether anyone is subscribed to the output
+ ros::SubscriberStatusCallback connect_cb = boost::bind(&PointCloudXyziNodelet::connectCb, this);
+ // Make sure we don't enter connectCb() between advertising and assigning to pub_point_cloud_
+ boost::lock_guard<boost::mutex> lock(connect_mutex_);
+ pub_point_cloud_ = depth_nh.advertise<PointCloud>("points", 1, connect_cb, connect_cb);
+}
+
+// Handles (un)subscribing when clients (un)subscribe
+void PointCloudXyziNodelet::connectCb()
+{
+ boost::lock_guard<boost::mutex> lock(connect_mutex_);
+ if (pub_point_cloud_.getNumSubscribers() == 0)
+ {
+ sub_depth_.unsubscribe();
+ sub_intensity_ .unsubscribe();
+ sub_info_ .unsubscribe();
+ }
+ else if (!sub_depth_.getSubscriber())
+ {
+ ros::NodeHandle& private_nh = getPrivateNodeHandle();
+ // parameter for depth_image_transport hint
+ std::string depth_image_transport_param = "depth_image_transport";
+
+ // depth image can use different transport.(e.g. compressedDepth)
+ image_transport::TransportHints depth_hints("raw",ros::TransportHints(), private_nh, depth_image_transport_param);
+ sub_depth_.subscribe(*depth_it_, "image_rect", 1, depth_hints);
+
+ // intensity uses normal ros transport hints.
+ image_transport::TransportHints hints("raw", ros::TransportHints(), private_nh);
+ sub_intensity_.subscribe(*intensity_it_, "image_rect", 1, hints);
+ sub_info_.subscribe(*intensity_nh_, "camera_info", 1);
+ }
+}
+
+void PointCloudXyziNodelet::imageCb(const sensor_msgs::ImageConstPtr& depth_msg,
+ const sensor_msgs::ImageConstPtr& intensity_msg_in,
+ const sensor_msgs::CameraInfoConstPtr& info_msg)
+{
+ // Check for bad inputs
+ if (depth_msg->header.frame_id != intensity_msg_in->header.frame_id)
+ {
+ NODELET_ERROR_THROTTLE(5, "Depth image frame id [%s] doesn't match image frame id [%s]",
+ depth_msg->header.frame_id.c_str(), intensity_msg_in->header.frame_id.c_str());
+ return;
+ }
+
+ // Update camera model
+ model_.fromCameraInfo(info_msg);
+
+ // Check if the input image has to be resized
+ sensor_msgs::ImageConstPtr intensity_msg = intensity_msg_in;
+ if (depth_msg->width != intensity_msg->width || depth_msg->height != intensity_msg->height)
+ {
+ sensor_msgs::CameraInfo info_msg_tmp = *info_msg;
+ info_msg_tmp.width = depth_msg->width;
+ info_msg_tmp.height = depth_msg->height;
+ float ratio = float(depth_msg->width)/float(intensity_msg->width);
+ info_msg_tmp.K[0] *= ratio;
+ info_msg_tmp.K[2] *= ratio;
+ info_msg_tmp.K[4] *= ratio;
+ info_msg_tmp.K[5] *= ratio;
+ info_msg_tmp.P[0] *= ratio;
+ info_msg_tmp.P[2] *= ratio;
+ info_msg_tmp.P[5] *= ratio;
+ info_msg_tmp.P[6] *= ratio;
+ model_.fromCameraInfo(info_msg_tmp);
+
+ cv_bridge::CvImageConstPtr cv_ptr;
+ try
+ {
+ cv_ptr = cv_bridge::toCvShare(intensity_msg, intensity_msg->encoding);
+ }
+ catch (cv_bridge::Exception& e)
+ {
+ ROS_ERROR("cv_bridge exception: %s", e.what());
+ return;
+ }
+ cv_bridge::CvImage cv_rsz;
+ cv_rsz.header = cv_ptr->header;
+ cv_rsz.encoding = cv_ptr->encoding;
+ cv::resize(cv_ptr->image.rowRange(0,depth_msg->height/ratio), cv_rsz.image, cv::Size(depth_msg->width, depth_msg->height));
+ if ((intensity_msg->encoding == enc::MONO8) || (intensity_msg->encoding == enc::MONO16))
+ intensity_msg = cv_rsz.toImageMsg();
+ else
+ intensity_msg = cv_bridge::toCvCopy(cv_rsz.toImageMsg(), enc::MONO8)->toImageMsg();
+
+ //NODELET_ERROR_THROTTLE(5, "Depth resolution (%ux%u) does not match resolution (%ux%u)",
+ // depth_msg->width, depth_msg->height, rgb_msg->width, rgb_msg->height);
+ //return;
+ } else
+ intensity_msg = intensity_msg_in;
+
+ // Supported color encodings: MONO8, MONO16
+ if (intensity_msg->encoding != enc::MONO8 || intensity_msg->encoding != enc::MONO16)
+ {
+ try
+ {
+ intensity_msg = cv_bridge::toCvCopy(intensity_msg, enc::MONO8)->toImageMsg();
+ }
+ catch (cv_bridge::Exception& e)
+ {
+ NODELET_ERROR_THROTTLE(5, "Unsupported encoding [%s]: %s", intensity_msg->encoding.c_str(), e.what());
+ return;
+ }
+ }
+
+ // Allocate new point cloud message
+ PointCloud::Ptr cloud_msg (new PointCloud);
+ cloud_msg->header = depth_msg->header; // Use depth image time stamp
+ cloud_msg->height = depth_msg->height;
+ cloud_msg->width = depth_msg->width;
+ cloud_msg->is_dense = false;
+ cloud_msg->is_bigendian = false;
+
+ sensor_msgs::PointCloud2Modifier pcd_modifier(*cloud_msg);
+// pcd_modifier.setPointCloud2FieldsByString(2, "xyz", "i");
+ pcd_modifier.setPointCloud2Fields(4,
+ "x", 1, sensor_msgs::PointField::FLOAT32,
+ "y", 1, sensor_msgs::PointField::FLOAT32,
+ "z", 1, sensor_msgs::PointField::FLOAT32,
+ "intensity", 1, sensor_msgs::PointField::FLOAT32);
+
+
+ if (depth_msg->encoding == enc::TYPE_16UC1 &&
+ intensity_msg->encoding == enc::MONO8)
+ {
+ convert<uint16_t, uint8_t>(depth_msg, intensity_msg, cloud_msg);
+ }
+ else if (depth_msg->encoding == enc::TYPE_16UC1 &&
+ intensity_msg->encoding == enc::MONO16)
+ {
+ convert<uint16_t, uint16_t>(depth_msg, intensity_msg, cloud_msg);
+ }
+ else if (depth_msg->encoding == enc::TYPE_32FC1 &&
+ intensity_msg->encoding == enc::MONO8)
+ {
+ convert<float, uint8_t>(depth_msg, intensity_msg, cloud_msg);
+ }
+ else if (depth_msg->encoding == enc::TYPE_32FC1 &&
+ intensity_msg->encoding == enc::MONO16)
+ {
+ convert<float, uint16_t>(depth_msg, intensity_msg, cloud_msg);
+ }
+ else
+ {
+ NODELET_ERROR_THROTTLE(5, "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str());
+ return;
+ }
+
+ pub_point_cloud_.publish (cloud_msg);
+}
+
+template<typename T, typename T2>
+void PointCloudXyziNodelet::convert(const sensor_msgs::ImageConstPtr& depth_msg,
+ const sensor_msgs::ImageConstPtr& intensity_msg,
+ const PointCloud::Ptr& cloud_msg)
+{
+ // Use correct principal point from calibration
+ float center_x = model_.cx();
+ float center_y = model_.cy();
+
+ // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y)
+ double unit_scaling = DepthTraits<T>::toMeters( T(1) );
+ float constant_x = unit_scaling / model_.fx();
+ float constant_y = unit_scaling / model_.fy();
+ float bad_point = std::numeric_limits<float>::quiet_NaN ();
+
+ const T* depth_row = reinterpret_cast<const T*>(&depth_msg->data[0]);
+ int row_step = depth_msg->step / sizeof(T);
+
+ const T2* inten_row = reinterpret_cast<const T2*>(&intensity_msg->data[0]);
+ int inten_row_step = intensity_msg->step / sizeof(T2);
+
+ sensor_msgs::PointCloud2Iterator<float> iter_x(*cloud_msg, "x");
+ sensor_msgs::PointCloud2Iterator<float> iter_y(*cloud_msg, "y");
+ sensor_msgs::PointCloud2Iterator<float> iter_z(*cloud_msg, "z");
+ sensor_msgs::PointCloud2Iterator<float> iter_i(*cloud_msg, "intensity");
+
+ for (int v = 0; v < int(cloud_msg->height); ++v, depth_row += row_step, inten_row += inten_row_step)
+ {
+ for (int u = 0; u < int(cloud_msg->width); ++u, ++iter_x, ++iter_y, ++iter_z, ++iter_i)
+ {
+ T depth = depth_row[u];
+ T2 inten = inten_row[u];
+ // Check for invalid measurements
+ if (!DepthTraits<T>::valid(depth))
+ {
+ *iter_x = *iter_y = *iter_z = bad_point;
+ }
+ else
+ {
+ // Fill in XYZ
+ *iter_x = (u - center_x) * depth * constant_x;
+ *iter_y = (v - center_y) * depth * constant_y;
+ *iter_z = DepthTraits<T>::toMeters(depth);
+ }
+
+ // Fill in intensity
+ *iter_i = inten;
+ }
+ }
+}
+
+} // namespace depth_image_proc
+
+// Register as nodelet
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(depth_image_proc::PointCloudXyziNodelet,nodelet::Nodelet);
diff --git a/depth_image_proc/src/nodelets/point_cloud_xyzi_radial.cpp b/depth_image_proc/src/nodelets/point_cloud_xyzi_radial.cpp
new file mode 100644
index 0000000..1d9294d
--- /dev/null
+++ b/depth_image_proc/src/nodelets/point_cloud_xyzi_radial.cpp
@@ -0,0 +1,313 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the Willow Garage nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+#include <ros/ros.h>
+#include <nodelet/nodelet.h>
+#include <image_transport/image_transport.h>
+#include <sensor_msgs/image_encodings.h>
+#include <image_transport/subscriber_filter.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/synchronizer.h>
+#include <message_filters/sync_policies/exact_time.h>
+#include <image_geometry/pinhole_camera_model.h>
+#include <boost/thread.hpp>
+#include <depth_image_proc/depth_traits.h>
+
+#include <sensor_msgs/point_cloud2_iterator.h>
+
+namespace depth_image_proc {
+
+ using namespace message_filters::sync_policies;
+ namespace enc = sensor_msgs::image_encodings;
+ typedef ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> SyncPolicy;
+
+ class PointCloudXyziRadialNodelet : public nodelet::Nodelet
+ {
+ ros::NodeHandlePtr intensity_nh_;
+
+ // Subscriptions
+ boost::shared_ptr<image_transport::ImageTransport> intensity_it_, depth_it_;
+ image_transport::SubscriberFilter sub_depth_, sub_intensity_;
+ message_filters::Subscriber<sensor_msgs::CameraInfo> sub_info_;
+
+ int queue_size_;
+
+ // Publications
+ boost::mutex connect_mutex_;
+ typedef sensor_msgs::PointCloud2 PointCloud;
+ ros::Publisher pub_point_cloud_;
+
+
+ typedef message_filters::Synchronizer<SyncPolicy> Synchronizer;
+ boost::shared_ptr<Synchronizer> sync_;
+
+ std::vector<double> D_;
+ boost::array<double, 9> K_;
+
+ int width_;
+ int height_;
+
+ cv::Mat transform_;
+
+ virtual void onInit();
+
+ void connectCb();
+
+ void imageCb(const sensor_msgs::ImageConstPtr& depth_msg,
+ const sensor_msgs::ImageConstPtr& intensity_msg_in,
+ const sensor_msgs::CameraInfoConstPtr& info_msg);
+
+ // Handles float or uint16 depths
+ template<typename T>
+ void convert_depth(const sensor_msgs::ImageConstPtr& depth_msg, PointCloud::Ptr& cloud_msg);
+
+ template<typename T>
+ void convert_intensity(const sensor_msgs::ImageConstPtr &inten_msg, PointCloud::Ptr& cloud_msg);
+
+ cv::Mat initMatrix(cv::Mat cameraMatrix, cv::Mat distCoeffs, int width, int height, bool radial);
+
+ };
+
+ cv::Mat PointCloudXyziRadialNodelet::initMatrix(cv::Mat cameraMatrix, cv::Mat distCoeffs, int width, int height, bool radial)
+ {
+ int i,j;
+ int totalsize = width*height;
+ cv::Mat pixelVectors(1,totalsize,CV_32FC3);
+ cv::Mat dst(1,totalsize,CV_32FC3);
+
+ cv::Mat sensorPoints(cv::Size(height,width), CV_32FC2);
+ cv::Mat undistortedSensorPoints(1,totalsize, CV_32FC2);
+
+ std::vector<cv::Mat> ch;
+ for(j = 0; j < height; j++)
+ {
+ for(i = 0; i < width; i++)
+ {
+ cv::Vec2f &p = sensorPoints.at<cv::Vec2f>(i,j);
+ p[0] = i;
+ p[1] = j;
+ }
+ }
+
+ sensorPoints = sensorPoints.reshape(2,1);
+
+ cv::undistortPoints(sensorPoints, undistortedSensorPoints, cameraMatrix, distCoeffs);
+
+ ch.push_back(undistortedSensorPoints);
+ ch.push_back(cv::Mat::ones(1,totalsize,CV_32FC1));
+ cv::merge(ch,pixelVectors);
+
+ if(radial)
+ {
+ for(i = 0; i < totalsize; i++)
+ {
+ normalize(pixelVectors.at<cv::Vec3f>(i),
+ dst.at<cv::Vec3f>(i));
+ }
+ pixelVectors = dst;
+ }
+ return pixelVectors.reshape(3,width);
+ }
+
+
+ void PointCloudXyziRadialNodelet::onInit()
+ {
+ ros::NodeHandle& nh = getNodeHandle();
+ ros::NodeHandle& private_nh = getPrivateNodeHandle();
+
+ intensity_nh_.reset( new ros::NodeHandle(nh, "intensity") );
+ ros::NodeHandle depth_nh(nh, "depth");
+ intensity_it_ .reset( new image_transport::ImageTransport(*intensity_nh_) );
+ depth_it_.reset( new image_transport::ImageTransport(depth_nh) );
+
+ // Read parameters
+ private_nh.param("queue_size", queue_size_, 5);
+
+ // Synchronize inputs. Topic subscriptions happen on demand in the connection callback.
+ sync_.reset( new Synchronizer(SyncPolicy(queue_size_), sub_depth_, sub_intensity_, sub_info_) );
+ sync_->registerCallback(boost::bind(&PointCloudXyziRadialNodelet::imageCb, this, _1, _2, _3));
+
+ // Monitor whether anyone is subscribed to the output
+ ros::SubscriberStatusCallback connect_cb =
+ boost::bind(&PointCloudXyziRadialNodelet::connectCb, this);
+ // Make sure we don't enter connectCb() between advertising and assigning to pub_point_cloud_
+ boost::lock_guard<boost::mutex> lock(connect_mutex_);
+ pub_point_cloud_ = nh.advertise<PointCloud>("points", 20, connect_cb, connect_cb);
+ }
+
+ // Handles (un)subscribing when clients (un)subscribe
+ void PointCloudXyziRadialNodelet::connectCb()
+ {
+ boost::lock_guard<boost::mutex> lock(connect_mutex_);
+
+ if (pub_point_cloud_.getNumSubscribers() == 0)
+ {
+ sub_depth_.unsubscribe();
+ sub_intensity_.unsubscribe();
+ sub_info_.unsubscribe();
+ }
+ else if (!sub_depth_.getSubscriber())
+ {
+ ros::NodeHandle& private_nh = getPrivateNodeHandle();
+ // parameter for depth_image_transport hint
+ std::string depth_image_transport_param = "depth_image_transport";
+
+ // depth image can use different transport.(e.g. compressedDepth)
+ image_transport::TransportHints depth_hints("raw",ros::TransportHints(), private_nh, depth_image_transport_param);
+ sub_depth_.subscribe(*depth_it_, "image_raw", 5, depth_hints);
+
+ // intensity uses normal ros transport hints.
+ image_transport::TransportHints hints("raw", ros::TransportHints(), private_nh);
+ sub_intensity_.subscribe(*intensity_it_, "image_raw", 5, hints);
+ sub_info_.subscribe(*intensity_nh_, "camera_info", 5);
+ }
+ }
+
+ void PointCloudXyziRadialNodelet::imageCb(const sensor_msgs::ImageConstPtr& depth_msg,
+ const sensor_msgs::ImageConstPtr& intensity_msg,
+ const sensor_msgs::CameraInfoConstPtr& info_msg)
+ {
+ PointCloud::Ptr cloud_msg(new PointCloud);
+ cloud_msg->header = depth_msg->header;
+ cloud_msg->height = depth_msg->height;
+ cloud_msg->width = depth_msg->width;
+ cloud_msg->is_dense = false;
+ cloud_msg->is_bigendian = false;
+
+ sensor_msgs::PointCloud2Modifier pcd_modifier(*cloud_msg);
+ pcd_modifier.setPointCloud2Fields(4,
+ "x", 1, sensor_msgs::PointField::FLOAT32,
+ "y", 1, sensor_msgs::PointField::FLOAT32,
+ "z", 1, sensor_msgs::PointField::FLOAT32,
+ "intensity", 1, sensor_msgs::PointField::FLOAT32);
+
+
+ if(info_msg->D != D_ || info_msg->K != K_ || width_ != info_msg->width ||
+ height_ != info_msg->height)
+ {
+ D_ = info_msg->D;
+ K_ = info_msg->K;
+ width_ = info_msg->width;
+ height_ = info_msg->height;
+ transform_ = initMatrix(cv::Mat_<double>(3, 3, &K_[0]),cv::Mat(D_),width_,height_,true);
+ }
+
+ if (depth_msg->encoding == enc::TYPE_16UC1)
+ {
+ convert_depth<uint16_t>(depth_msg, cloud_msg);
+ }
+ else if (depth_msg->encoding == enc::TYPE_32FC1)
+ {
+ convert_depth<float>(depth_msg, cloud_msg);
+ }
+ else
+ {
+ NODELET_ERROR_THROTTLE(5, "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str());
+ return;
+ }
+
+ if(intensity_msg->encoding == enc::TYPE_16UC1)
+ {
+ convert_intensity<uint16_t>(intensity_msg, cloud_msg);
+
+ }
+ else if(intensity_msg->encoding == enc::MONO8)
+ {
+ convert_intensity<uint8_t>(intensity_msg, cloud_msg);
+ }
+ else
+ {
+ NODELET_ERROR_THROTTLE(5, "Intensity image has unsupported encoding [%s]", intensity_msg->encoding.c_str());
+ return;
+ }
+
+ pub_point_cloud_.publish (cloud_msg);
+ }
+
+ template<typename T>
+ void PointCloudXyziRadialNodelet::convert_depth(const sensor_msgs::ImageConstPtr& depth_msg,
+ PointCloud::Ptr& cloud_msg)
+ {
+ // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y)
+ double unit_scaling = DepthTraits<T>::toMeters( T(1) );
+ float bad_point = std::numeric_limits<float>::quiet_NaN();
+
+ sensor_msgs::PointCloud2Iterator<float> iter_x(*cloud_msg, "x");
+ sensor_msgs::PointCloud2Iterator<float> iter_y(*cloud_msg, "y");
+ sensor_msgs::PointCloud2Iterator<float> iter_z(*cloud_msg, "z");
+ const T* depth_row = reinterpret_cast<const T*>(&depth_msg->data[0]);
+
+ int row_step = depth_msg->step / sizeof(T);
+ for (int v = 0; v < (int)cloud_msg->height; ++v, depth_row += row_step)
+ {
+ for (int u = 0; u < (int)cloud_msg->width; ++u, ++iter_x, ++iter_y, ++iter_z)
+ {
+ T depth = depth_row[u];
+
+ // Missing points denoted by NaNs
+ if (!DepthTraits<T>::valid(depth))
+ {
+ *iter_x = *iter_y = *iter_z = bad_point;
+ continue;
+ }
+ const cv::Vec3f &cvPoint = transform_.at<cv::Vec3f>(u,v) * DepthTraits<T>::toMeters(depth);
+ // Fill in XYZ
+ *iter_x = cvPoint(0);
+ *iter_y = cvPoint(1);
+ *iter_z = cvPoint(2);
+ }
+ }
+ }
+
+ template<typename T>
+ void PointCloudXyziRadialNodelet::convert_intensity(const sensor_msgs::ImageConstPtr& intensity_msg,
+ PointCloud::Ptr& cloud_msg)
+ {
+ sensor_msgs::PointCloud2Iterator<float> iter_i(*cloud_msg, "intensity");
+ const T* inten_row = reinterpret_cast<const T*>(&intensity_msg->data[0]);
+
+ const int i_row_step = intensity_msg->step/sizeof(T);
+ for (int v = 0; v < (int)cloud_msg->height; ++v, inten_row += i_row_step)
+ {
+ for (int u = 0; u < (int)cloud_msg->width; ++u, ++iter_i)
+ {
+ *iter_i = inten_row[u];
+ }
+ }
+ }
+
+} // namespace depth_image_proc
+
+// Register as nodelet
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(depth_image_proc::PointCloudXyziRadialNodelet,nodelet::Nodelet);
diff --git a/depth_image_proc/src/nodelets/point_cloud_xyzrgb.cpp b/depth_image_proc/src/nodelets/point_cloud_xyzrgb.cpp
new file mode 100644
index 0000000..bd680aa
--- /dev/null
+++ b/depth_image_proc/src/nodelets/point_cloud_xyzrgb.cpp
@@ -0,0 +1,345 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+#include <boost/version.hpp>
+#if ((BOOST_VERSION / 100) % 1000) >= 53
+#include <boost/thread/lock_guard.hpp>
+#endif
+
+#include <ros/ros.h>
+#include <nodelet/nodelet.h>
+#include <image_transport/image_transport.h>
+#include <image_transport/subscriber_filter.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/synchronizer.h>
+#include <message_filters/sync_policies/exact_time.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <sensor_msgs/image_encodings.h>
+#include <sensor_msgs/point_cloud2_iterator.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <image_geometry/pinhole_camera_model.h>
+#include <depth_image_proc/depth_traits.h>
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/imgproc/imgproc.hpp>
+
+namespace depth_image_proc {
+
+using namespace message_filters::sync_policies;
+namespace enc = sensor_msgs::image_encodings;
+
+class PointCloudXyzrgbNodelet : public nodelet::Nodelet
+{
+ ros::NodeHandlePtr rgb_nh_;
+ boost::shared_ptr<image_transport::ImageTransport> rgb_it_, depth_it_;
+
+ // Subscriptions
+ image_transport::SubscriberFilter sub_depth_, sub_rgb_;
+ message_filters::Subscriber<sensor_msgs::CameraInfo> sub_info_;
+ typedef ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> SyncPolicy;
+ typedef ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> ExactSyncPolicy;
+ typedef message_filters::Synchronizer<SyncPolicy> Synchronizer;
+ typedef message_filters::Synchronizer<ExactSyncPolicy> ExactSynchronizer;
+ boost::shared_ptr<Synchronizer> sync_;
+ boost::shared_ptr<ExactSynchronizer> exact_sync_;
+
+ // Publications
+ boost::mutex connect_mutex_;
+ typedef sensor_msgs::PointCloud2 PointCloud;
+ ros::Publisher pub_point_cloud_;
+
+ image_geometry::PinholeCameraModel model_;
+
+ virtual void onInit();
+
+ void connectCb();
+
+ void imageCb(const sensor_msgs::ImageConstPtr& depth_msg,
+ const sensor_msgs::ImageConstPtr& rgb_msg,
+ const sensor_msgs::CameraInfoConstPtr& info_msg);
+
+ template<typename T>
+ void convert(const sensor_msgs::ImageConstPtr& depth_msg,
+ const sensor_msgs::ImageConstPtr& rgb_msg,
+ const PointCloud::Ptr& cloud_msg,
+ int red_offset, int green_offset, int blue_offset, int color_step);
+};
+
+void PointCloudXyzrgbNodelet::onInit()
+{
+ ros::NodeHandle& nh = getNodeHandle();
+ ros::NodeHandle& private_nh = getPrivateNodeHandle();
+ rgb_nh_.reset( new ros::NodeHandle(nh, "rgb") );
+ ros::NodeHandle depth_nh(nh, "depth_registered");
+ rgb_it_ .reset( new image_transport::ImageTransport(*rgb_nh_) );
+ depth_it_.reset( new image_transport::ImageTransport(depth_nh) );
+
+ // Read parameters
+ int queue_size;
+ private_nh.param("queue_size", queue_size, 5);
+ bool use_exact_sync;
+ private_nh.param("exact_sync", use_exact_sync, false);
+
+ // Synchronize inputs. Topic subscriptions happen on demand in the connection callback.
+ if (use_exact_sync)
+ {
+ exact_sync_.reset( new ExactSynchronizer(ExactSyncPolicy(queue_size), sub_depth_, sub_rgb_, sub_info_) );
+ exact_sync_->registerCallback(boost::bind(&PointCloudXyzrgbNodelet::imageCb, this, _1, _2, _3));
+ }
+ else
+ {
+ sync_.reset( new Synchronizer(SyncPolicy(queue_size), sub_depth_, sub_rgb_, sub_info_) );
+ sync_->registerCallback(boost::bind(&PointCloudXyzrgbNodelet::imageCb, this, _1, _2, _3));
+ }
+
+ // Monitor whether anyone is subscribed to the output
+ ros::SubscriberStatusCallback connect_cb = boost::bind(&PointCloudXyzrgbNodelet::connectCb, this);
+ // Make sure we don't enter connectCb() between advertising and assigning to pub_point_cloud_
+ boost::lock_guard<boost::mutex> lock(connect_mutex_);
+ pub_point_cloud_ = depth_nh.advertise<PointCloud>("points", 1, connect_cb, connect_cb);
+}
+
+// Handles (un)subscribing when clients (un)subscribe
+void PointCloudXyzrgbNodelet::connectCb()
+{
+ boost::lock_guard<boost::mutex> lock(connect_mutex_);
+ if (pub_point_cloud_.getNumSubscribers() == 0)
+ {
+ sub_depth_.unsubscribe();
+ sub_rgb_ .unsubscribe();
+ sub_info_ .unsubscribe();
+ }
+ else if (!sub_depth_.getSubscriber())
+ {
+ ros::NodeHandle& private_nh = getPrivateNodeHandle();
+ // parameter for depth_image_transport hint
+ std::string depth_image_transport_param = "depth_image_transport";
+
+ // depth image can use different transport.(e.g. compressedDepth)
+ image_transport::TransportHints depth_hints("raw",ros::TransportHints(), private_nh, depth_image_transport_param);
+ sub_depth_.subscribe(*depth_it_, "image_rect", 1, depth_hints);
+
+ // rgb uses normal ros transport hints.
+ image_transport::TransportHints hints("raw", ros::TransportHints(), private_nh);
+ sub_rgb_ .subscribe(*rgb_it_, "image_rect_color", 1, hints);
+ sub_info_ .subscribe(*rgb_nh_, "camera_info", 1);
+ }
+}
+
+void PointCloudXyzrgbNodelet::imageCb(const sensor_msgs::ImageConstPtr& depth_msg,
+ const sensor_msgs::ImageConstPtr& rgb_msg_in,
+ const sensor_msgs::CameraInfoConstPtr& info_msg)
+{
+ // Check for bad inputs
+ if (depth_msg->header.frame_id != rgb_msg_in->header.frame_id)
+ {
+ NODELET_ERROR_THROTTLE(5, "Depth image frame id [%s] doesn't match RGB image frame id [%s]",
+ depth_msg->header.frame_id.c_str(), rgb_msg_in->header.frame_id.c_str());
+ return;
+ }
+
+ // Update camera model
+ model_.fromCameraInfo(info_msg);
+
+ // Check if the input image has to be resized
+ sensor_msgs::ImageConstPtr rgb_msg = rgb_msg_in;
+ if (depth_msg->width != rgb_msg->width || depth_msg->height != rgb_msg->height)
+ {
+ sensor_msgs::CameraInfo info_msg_tmp = *info_msg;
+ info_msg_tmp.width = depth_msg->width;
+ info_msg_tmp.height = depth_msg->height;
+ float ratio = float(depth_msg->width)/float(rgb_msg->width);
+ info_msg_tmp.K[0] *= ratio;
+ info_msg_tmp.K[2] *= ratio;
+ info_msg_tmp.K[4] *= ratio;
+ info_msg_tmp.K[5] *= ratio;
+ info_msg_tmp.P[0] *= ratio;
+ info_msg_tmp.P[2] *= ratio;
+ info_msg_tmp.P[5] *= ratio;
+ info_msg_tmp.P[6] *= ratio;
+ model_.fromCameraInfo(info_msg_tmp);
+
+ cv_bridge::CvImageConstPtr cv_ptr;
+ try
+ {
+ cv_ptr = cv_bridge::toCvShare(rgb_msg, rgb_msg->encoding);
+ }
+ catch (cv_bridge::Exception& e)
+ {
+ ROS_ERROR("cv_bridge exception: %s", e.what());
+ return;
+ }
+ cv_bridge::CvImage cv_rsz;
+ cv_rsz.header = cv_ptr->header;
+ cv_rsz.encoding = cv_ptr->encoding;
+ cv::resize(cv_ptr->image.rowRange(0,depth_msg->height/ratio), cv_rsz.image, cv::Size(depth_msg->width, depth_msg->height));
+ if ((rgb_msg->encoding == enc::RGB8) || (rgb_msg->encoding == enc::BGR8) || (rgb_msg->encoding == enc::MONO8))
+ rgb_msg = cv_rsz.toImageMsg();
+ else
+ rgb_msg = cv_bridge::toCvCopy(cv_rsz.toImageMsg(), enc::RGB8)->toImageMsg();
+
+ //NODELET_ERROR_THROTTLE(5, "Depth resolution (%ux%u) does not match RGB resolution (%ux%u)",
+ // depth_msg->width, depth_msg->height, rgb_msg->width, rgb_msg->height);
+ //return;
+ } else
+ rgb_msg = rgb_msg_in;
+
+ // Supported color encodings: RGB8, BGR8, MONO8
+ int red_offset, green_offset, blue_offset, color_step;
+ if (rgb_msg->encoding == enc::RGB8)
+ {
+ red_offset = 0;
+ green_offset = 1;
+ blue_offset = 2;
+ color_step = 3;
+ }
+ else if (rgb_msg->encoding == enc::BGR8)
+ {
+ red_offset = 2;
+ green_offset = 1;
+ blue_offset = 0;
+ color_step = 3;
+ }
+ else if (rgb_msg->encoding == enc::MONO8)
+ {
+ red_offset = 0;
+ green_offset = 0;
+ blue_offset = 0;
+ color_step = 1;
+ }
+ else
+ {
+ try
+ {
+ rgb_msg = cv_bridge::toCvCopy(rgb_msg, enc::RGB8)->toImageMsg();
+ }
+ catch (cv_bridge::Exception& e)
+ {
+ NODELET_ERROR_THROTTLE(5, "Unsupported encoding [%s]: %s", rgb_msg->encoding.c_str(), e.what());
+ return;
+ }
+ red_offset = 0;
+ green_offset = 1;
+ blue_offset = 2;
+ color_step = 3;
+ }
+
+ // Allocate new point cloud message
+ PointCloud::Ptr cloud_msg (new PointCloud);
+ cloud_msg->header = depth_msg->header; // Use depth image time stamp
+ cloud_msg->height = depth_msg->height;
+ cloud_msg->width = depth_msg->width;
+ cloud_msg->is_dense = false;
+ cloud_msg->is_bigendian = false;
+
+ sensor_msgs::PointCloud2Modifier pcd_modifier(*cloud_msg);
+ pcd_modifier.setPointCloud2FieldsByString(2, "xyz", "rgb");
+
+ if (depth_msg->encoding == enc::TYPE_16UC1)
+ {
+ convert<uint16_t>(depth_msg, rgb_msg, cloud_msg, red_offset, green_offset, blue_offset, color_step);
+ }
+ else if (depth_msg->encoding == enc::TYPE_32FC1)
+ {
+ convert<float>(depth_msg, rgb_msg, cloud_msg, red_offset, green_offset, blue_offset, color_step);
+ }
+ else
+ {
+ NODELET_ERROR_THROTTLE(5, "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str());
+ return;
+ }
+
+ pub_point_cloud_.publish (cloud_msg);
+}
+
+template<typename T>
+void PointCloudXyzrgbNodelet::convert(const sensor_msgs::ImageConstPtr& depth_msg,
+ const sensor_msgs::ImageConstPtr& rgb_msg,
+ const PointCloud::Ptr& cloud_msg,
+ int red_offset, int green_offset, int blue_offset, int color_step)
+{
+ // Use correct principal point from calibration
+ float center_x = model_.cx();
+ float center_y = model_.cy();
+
+ // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y)
+ double unit_scaling = DepthTraits<T>::toMeters( T(1) );
+ float constant_x = unit_scaling / model_.fx();
+ float constant_y = unit_scaling / model_.fy();
+ float bad_point = std::numeric_limits<float>::quiet_NaN ();
+
+ const T* depth_row = reinterpret_cast<const T*>(&depth_msg->data[0]);
+ int row_step = depth_msg->step / sizeof(T);
+ const uint8_t* rgb = &rgb_msg->data[0];
+ int rgb_skip = rgb_msg->step - rgb_msg->width * color_step;
+
+ sensor_msgs::PointCloud2Iterator<float> iter_x(*cloud_msg, "x");
+ sensor_msgs::PointCloud2Iterator<float> iter_y(*cloud_msg, "y");
+ sensor_msgs::PointCloud2Iterator<float> iter_z(*cloud_msg, "z");
+ sensor_msgs::PointCloud2Iterator<uint8_t> iter_r(*cloud_msg, "r");
+ sensor_msgs::PointCloud2Iterator<uint8_t> iter_g(*cloud_msg, "g");
+ sensor_msgs::PointCloud2Iterator<uint8_t> iter_b(*cloud_msg, "b");
+ sensor_msgs::PointCloud2Iterator<uint8_t> iter_a(*cloud_msg, "a");
+
+ for (int v = 0; v < int(cloud_msg->height); ++v, depth_row += row_step, rgb += rgb_skip)
+ {
+ for (int u = 0; u < int(cloud_msg->width); ++u, rgb += color_step, ++iter_x, ++iter_y, ++iter_z, ++iter_a, ++iter_r, ++iter_g, ++iter_b)
+ {
+ T depth = depth_row[u];
+
+ // Check for invalid measurements
+ if (!DepthTraits<T>::valid(depth))
+ {
+ *iter_x = *iter_y = *iter_z = bad_point;
+ }
+ else
+ {
+ // Fill in XYZ
+ *iter_x = (u - center_x) * depth * constant_x;
+ *iter_y = (v - center_y) * depth * constant_y;
+ *iter_z = DepthTraits<T>::toMeters(depth);
+ }
+
+ // Fill in color
+ *iter_a = 255;
+ *iter_r = rgb[red_offset];
+ *iter_g = rgb[green_offset];
+ *iter_b = rgb[blue_offset];
+ }
+ }
+}
+
+} // namespace depth_image_proc
+
+// Register as nodelet
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(depth_image_proc::PointCloudXyzrgbNodelet,nodelet::Nodelet);
diff --git a/depth_image_proc/src/nodelets/register.cpp b/depth_image_proc/src/nodelets/register.cpp
new file mode 100644
index 0000000..980b3cb
--- /dev/null
+++ b/depth_image_proc/src/nodelets/register.cpp
@@ -0,0 +1,263 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+#include <ros/ros.h>
+#include <nodelet/nodelet.h>
+#include <image_transport/image_transport.h>
+#include <image_transport/subscriber_filter.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/synchronizer.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+#include <sensor_msgs/image_encodings.h>
+#include <image_geometry/pinhole_camera_model.h>
+#include <Eigen/Geometry>
+#include <eigen_conversions/eigen_msg.h>
+#include <depth_image_proc/depth_traits.h>
+
+namespace depth_image_proc {
+
+using namespace message_filters::sync_policies;
+namespace enc = sensor_msgs::image_encodings;
+
+class RegisterNodelet : public nodelet::Nodelet
+{
+ ros::NodeHandlePtr nh_depth_, nh_rgb_;
+ boost::shared_ptr<image_transport::ImageTransport> it_depth_;
+
+ // Subscriptions
+ image_transport::SubscriberFilter sub_depth_image_;
+ message_filters::Subscriber<sensor_msgs::CameraInfo> sub_depth_info_, sub_rgb_info_;
+ boost::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+ boost::shared_ptr<tf2_ros::TransformListener> tf_;
+ typedef ApproximateTime<sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> SyncPolicy;
+ typedef message_filters::Synchronizer<SyncPolicy> Synchronizer;
+ boost::shared_ptr<Synchronizer> sync_;
+
+ // Publications
+ boost::mutex connect_mutex_;
+ image_transport::CameraPublisher pub_registered_;
+
+ image_geometry::PinholeCameraModel depth_model_, rgb_model_;
+
+ virtual void onInit();
+
+ void connectCb();
+
+ void imageCb(const sensor_msgs::ImageConstPtr& depth_image_msg,
+ const sensor_msgs::CameraInfoConstPtr& depth_info_msg,
+ const sensor_msgs::CameraInfoConstPtr& rgb_info_msg);
+
+ template<typename T>
+ void convert(const sensor_msgs::ImageConstPtr& depth_msg,
+ const sensor_msgs::ImagePtr& registered_msg,
+ const Eigen::Affine3d& depth_to_rgb);
+};
+
+void RegisterNodelet::onInit()
+{
+ ros::NodeHandle& nh = getNodeHandle();
+ ros::NodeHandle& private_nh = getPrivateNodeHandle();
+ nh_depth_.reset( new ros::NodeHandle(nh, "depth") );
+ nh_rgb_.reset( new ros::NodeHandle(nh, "rgb") );
+ it_depth_.reset( new image_transport::ImageTransport(*nh_depth_) );
+ tf_buffer_.reset( new tf2_ros::Buffer );
+ tf_.reset( new tf2_ros::TransformListener(*tf_buffer_) );
+
+ // Read parameters
+ int queue_size;
+ private_nh.param("queue_size", queue_size, 5);
+
+ // Synchronize inputs. Topic subscriptions happen on demand in the connection callback.
+ sync_.reset( new Synchronizer(SyncPolicy(queue_size), sub_depth_image_, sub_depth_info_, sub_rgb_info_) );
+ sync_->registerCallback(boost::bind(&RegisterNodelet::imageCb, this, _1, _2, _3));
+
+ // Monitor whether anyone is subscribed to the output
+ image_transport::ImageTransport it_depth_reg(ros::NodeHandle(nh, "depth_registered"));
+ image_transport::SubscriberStatusCallback image_connect_cb = boost::bind(&RegisterNodelet::connectCb, this);
+ ros::SubscriberStatusCallback info_connect_cb = boost::bind(&RegisterNodelet::connectCb, this);
+ // Make sure we don't enter connectCb() between advertising and assigning to pub_registered_
+ boost::lock_guard<boost::mutex> lock(connect_mutex_);
+ pub_registered_ = it_depth_reg.advertiseCamera("image_rect", 1,
+ image_connect_cb, image_connect_cb,
+ info_connect_cb, info_connect_cb);
+}
+
+// Handles (un)subscribing when clients (un)subscribe
+void RegisterNodelet::connectCb()
+{
+ boost::lock_guard<boost::mutex> lock(connect_mutex_);
+ if (pub_registered_.getNumSubscribers() == 0)
+ {
+ sub_depth_image_.unsubscribe();
+ sub_depth_info_ .unsubscribe();
+ sub_rgb_info_ .unsubscribe();
+ }
+ else if (!sub_depth_image_.getSubscriber())
+ {
+ image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle());
+ sub_depth_image_.subscribe(*it_depth_, "image_rect", 1, hints);
+ sub_depth_info_ .subscribe(*nh_depth_, "camera_info", 1);
+ sub_rgb_info_ .subscribe(*nh_rgb_, "camera_info", 1);
+ }
+}
+
+void RegisterNodelet::imageCb(const sensor_msgs::ImageConstPtr& depth_image_msg,
+ const sensor_msgs::CameraInfoConstPtr& depth_info_msg,
+ const sensor_msgs::CameraInfoConstPtr& rgb_info_msg)
+{
+ // Update camera models - these take binning & ROI into account
+ depth_model_.fromCameraInfo(depth_info_msg);
+ rgb_model_ .fromCameraInfo(rgb_info_msg);
+
+ // Query tf2 for transform from (X,Y,Z) in depth camera frame to RGB camera frame
+ Eigen::Affine3d depth_to_rgb;
+ try
+ {
+ geometry_msgs::TransformStamped transform = tf_buffer_->lookupTransform (
+ rgb_info_msg->header.frame_id, depth_info_msg->header.frame_id,
+ depth_info_msg->header.stamp);
+
+ tf::transformMsgToEigen(transform.transform, depth_to_rgb);
+ }
+ catch (tf2::TransformException& ex)
+ {
+ NODELET_WARN_THROTTLE(2, "TF2 exception:\n%s", ex.what());
+ return;
+ /// @todo Can take on order of a minute to register a disconnect callback when we
+ /// don't call publish() in this cb. What's going on roscpp?
+ }
+
+ // Allocate registered depth image
+ sensor_msgs::ImagePtr registered_msg( new sensor_msgs::Image );
+ registered_msg->header.stamp = depth_image_msg->header.stamp;
+ registered_msg->header.frame_id = rgb_info_msg->header.frame_id;
+ registered_msg->encoding = depth_image_msg->encoding;
+
+ cv::Size resolution = rgb_model_.reducedResolution();
+ registered_msg->height = resolution.height;
+ registered_msg->width = resolution.width;
+ // step and data set in convert(), depend on depth data type
+
+ if (depth_image_msg->encoding == enc::TYPE_16UC1)
+ {
+ convert<uint16_t>(depth_image_msg, registered_msg, depth_to_rgb);
+ }
+ else if (depth_image_msg->encoding == enc::TYPE_32FC1)
+ {
+ convert<float>(depth_image_msg, registered_msg, depth_to_rgb);
+ }
+ else
+ {
+ NODELET_ERROR_THROTTLE(5, "Depth image has unsupported encoding [%s]", depth_image_msg->encoding.c_str());
+ return;
+ }
+
+ // Registered camera info is the same as the RGB info, but uses the depth timestamp
+ sensor_msgs::CameraInfoPtr registered_info_msg( new sensor_msgs::CameraInfo(*rgb_info_msg) );
+ registered_info_msg->header.stamp = registered_msg->header.stamp;
+
+ pub_registered_.publish(registered_msg, registered_info_msg);
+}
+
+template<typename T>
+void RegisterNodelet::convert(const sensor_msgs::ImageConstPtr& depth_msg,
+ const sensor_msgs::ImagePtr& registered_msg,
+ const Eigen::Affine3d& depth_to_rgb)
+{
+ // Allocate memory for registered depth image
+ registered_msg->step = registered_msg->width * sizeof(T);
+ registered_msg->data.resize( registered_msg->height * registered_msg->step );
+ // data is already zero-filled in the uint16 case, but for floats we want to initialize everything to NaN.
+ DepthTraits<T>::initializeBuffer(registered_msg->data);
+
+ // Extract all the parameters we need
+ double inv_depth_fx = 1.0 / depth_model_.fx();
+ double inv_depth_fy = 1.0 / depth_model_.fy();
+ double depth_cx = depth_model_.cx(), depth_cy = depth_model_.cy();
+ double depth_Tx = depth_model_.Tx(), depth_Ty = depth_model_.Ty();
+ double rgb_fx = rgb_model_.fx(), rgb_fy = rgb_model_.fy();
+ double rgb_cx = rgb_model_.cx(), rgb_cy = rgb_model_.cy();
+ double rgb_Tx = rgb_model_.Tx(), rgb_Ty = rgb_model_.Ty();
+
+ // Transform the depth values into the RGB frame
+ /// @todo When RGB is higher res, interpolate by rasterizing depth triangles onto the registered image
+ const T* depth_row = reinterpret_cast<const T*>(&depth_msg->data[0]);
+ int row_step = depth_msg->step / sizeof(T);
+ T* registered_data = reinterpret_cast<T*>(®istered_msg->data[0]);
+ int raw_index = 0;
+ for (unsigned v = 0; v < depth_msg->height; ++v, depth_row += row_step)
+ {
+ for (unsigned u = 0; u < depth_msg->width; ++u, ++raw_index)
+ {
+ T raw_depth = depth_row[u];
+ if (!DepthTraits<T>::valid(raw_depth))
+ continue;
+
+ double depth = DepthTraits<T>::toMeters(raw_depth);
+
+ /// @todo Combine all operations into one matrix multiply on (u,v,d)
+ // Reproject (u,v,Z) to (X,Y,Z,1) in depth camera frame
+ Eigen::Vector4d xyz_depth;
+ xyz_depth << ((u - depth_cx)*depth - depth_Tx) * inv_depth_fx,
+ ((v - depth_cy)*depth - depth_Ty) * inv_depth_fy,
+ depth,
+ 1;
+
+ // Transform to RGB camera frame
+ Eigen::Vector4d xyz_rgb = depth_to_rgb * xyz_depth;
+
+ // Project to (u,v) in RGB image
+ double inv_Z = 1.0 / xyz_rgb.z();
+ int u_rgb = (rgb_fx*xyz_rgb.x() + rgb_Tx)*inv_Z + rgb_cx + 0.5;
+ int v_rgb = (rgb_fy*xyz_rgb.y() + rgb_Ty)*inv_Z + rgb_cy + 0.5;
+
+ if (u_rgb < 0 || u_rgb >= (int)registered_msg->width ||
+ v_rgb < 0 || v_rgb >= (int)registered_msg->height)
+ continue;
+
+ T& reg_depth = registered_data[v_rgb*registered_msg->width + u_rgb];
+ T new_depth = DepthTraits<T>::fromMeters(xyz_rgb.z());
+ // Validity and Z-buffer checks
+ if (!DepthTraits<T>::valid(reg_depth) || reg_depth > new_depth)
+ reg_depth = new_depth;
+ }
+ }
+}
+
+} // namespace depth_image_proc
+
+// Register as nodelet
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(depth_image_proc::RegisterNodelet,nodelet::Nodelet);
diff --git a/image_pipeline/CHANGELOG.rst b/image_pipeline/CHANGELOG.rst
new file mode 100644
index 0000000..37a0a19
--- /dev/null
+++ b/image_pipeline/CHANGELOG.rst
@@ -0,0 +1,59 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package image_pipeline
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.12.20 (2017-04-30)
+--------------------
+* Update package.xml (`#263 <https://github.com/ros-perception/image_pipeline/issues/263>`_)
+* Contributors: Kei Okada
+
+1.12.19 (2016-07-24)
+--------------------
+
+1.12.18 (2016-07-12)
+--------------------
+
+1.12.17 (2016-07-11)
+--------------------
+
+1.12.16 (2016-03-19)
+--------------------
+
+1.12.15 (2016-01-17)
+--------------------
+
+1.12.14 (2015-07-22)
+--------------------
+
+1.12.13 (2015-04-06)
+--------------------
+
+1.12.12 (2014-12-31)
+--------------------
+
+1.12.11 (2014-10-26)
+--------------------
+
+1.12.10 (2014-09-28)
+--------------------
+
+1.12.9 (2014-09-21)
+-------------------
+
+1.12.8 (2014-08-19)
+-------------------
+
+1.12.6 (2014-07-27)
+-------------------
+
+1.12.4 (2014-04-28)
+-------------------
+
+1.12.3 (2014-04-12)
+-------------------
+
+1.12.2 (2014-04-08)
+-------------------
+
+1.11.7 (2014-03-28)
+-------------------
diff --git a/image_pipeline/CMakeLists.txt b/image_pipeline/CMakeLists.txt
new file mode 100644
index 0000000..69f7132
--- /dev/null
+++ b/image_pipeline/CMakeLists.txt
@@ -0,0 +1,4 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(image_pipeline)
+find_package(catkin REQUIRED)
+catkin_metapackage()
diff --git a/image_pipeline/package.xml b/image_pipeline/package.xml
new file mode 100644
index 0000000..5f1e8fe
--- /dev/null
+++ b/image_pipeline/package.xml
@@ -0,0 +1,29 @@
+<package>
+ <name>image_pipeline</name>
+ <version>1.12.20</version>
+ <description>image_pipeline fills the gap between getting raw images from a camera driver and higher-level vision processing.</description>
+ <author>Patrick Mihelich</author>
+ <author>James Bowman</author>
+ <maintainer email="vincent.rabaud at gmail.com">Vincent Rabaud</maintainer>
+ <license>BSD</license>
+
+ <url type="website">http://www.ros.org/wiki/image_pipeline</url>
+ <url type="bugtracker">https://github.com/ros-perception/image_pipeline/issues</url>
+ <url type="repository">https://github.com/ros-perception/image_pipeline</url>
+
+ <buildtool_depend>catkin</buildtool_depend>
+
+ <!-- Old stack contents -->
+ <run_depend>camera_calibration</run_depend>
+ <run_depend>depth_image_proc</run_depend>
+ <run_depend>image_proc</run_depend>
+ <run_depend>image_publisher</run_depend>
+ <run_depend>image_rotate</run_depend>
+ <run_depend>image_view</run_depend>
+ <run_depend>stereo_image_proc</run_depend>
+
+ <export>
+ <metapackage/>
+ </export>
+
+</package>
diff --git a/image_proc/CHANGELOG.rst b/image_proc/CHANGELOG.rst
new file mode 100644
index 0000000..a77c378
--- /dev/null
+++ b/image_proc/CHANGELOG.rst
@@ -0,0 +1,95 @@
+1.12.20 (2017-04-30)
+--------------------
+* Add nodelet to resize image and camera_info (`#273 <https://github.com/ros-perception/image_pipeline/issues/273>`_)
+ * Add nodelet to resize image and camera_info
+ * Depends on nodelet_topic_tools
+ * Use recursive_mutex for mutex guard for dynamic reconfiguring
+* Fix nodelet name: crop_nonZero -> crop_non_zero (`#269 <https://github.com/ros-perception/image_pipeline/issues/269>`_)
+ Fix https://github.com/ros-perception/image_pipeline/issues/217
+* Fix permission of executable files unexpectedly (`#260 <https://github.com/ros-perception/image_pipeline/issues/260>`_)
+* address gcc6 build error
+ With gcc6, compiling fails with `stdlib.h: No such file or directory`,
+ as including '-isystem /usr/include' breaks with gcc6, cf.,
+ https://gcc.gnu.org/bugzilla/show_bug.cgi?id=70129.
+ This commit addresses this issue for this package in the same way
+ it was addressed in various other ROS packages. A list of related
+ commits and pull requests is at:
+ https://github.com/ros/rosdistro/issues/12783
+ Signed-off-by: Lukas Bulwahn <lukas.bulwahn at oss.bmw-carit.de>
+* Contributors: Kentaro Wada, Lukas Bulwahn
+
+1.12.19 (2016-07-24)
+--------------------
+
+1.12.18 (2016-07-12)
+--------------------
+
+1.12.17 (2016-07-11)
+--------------------
+
+1.12.16 (2016-03-19)
+--------------------
+* clean OpenCV dependency in package.xml
+* issue `#180 <https://github.com/ros-perception/image_pipeline/issues/180>`_ Check if all distortion coefficients are zero.
+ Test with:
+ rostest --reuse-master --text image_proc test_rectify.xml
+ Can also test interactively with vimjay image_rect.launch, which brings up an rqt gui and camera info distortion coefficients can be dynamically reconfigured.
+* Add a feature to crop the largest valid (non zero) area
+ Remove unnecessary headers
+ change a filename to fit for the ROS convention
+* Contributors: Kenta Yonekura, Lucas Walter, Vincent Rabaud
+
+1.12.15 (2016-01-17)
+--------------------
+* simplify OpenCV3 conversion
+* Contributors: Vincent Rabaud
+
+1.12.14 (2015-07-22)
+--------------------
+
+1.12.13 (2015-04-06)
+--------------------
+* fix dependencies
+* Contributors: Vincent Rabaud
+
+1.12.12 (2014-12-31)
+--------------------
+
+1.12.11 (2014-10-26)
+--------------------
+
+1.12.10 (2014-09-28)
+--------------------
+
+1.12.9 (2014-09-21)
+-------------------
+* get code to compile with OpenCV3
+ fixes `#96 <https://github.com/ros-perception/image_pipeline/issues/96>`_
+* Contributors: Vincent Rabaud
+
+1.12.8 (2014-08-19)
+-------------------
+
+1.12.6 (2014-07-27)
+-------------------
+
+1.12.4 (2014-04-28)
+-------------------
+
+1.12.3 (2014-04-12)
+-------------------
+
+1.12.2 (2014-04-08)
+-------------------
+
+1.12.1 (2014-04-06)
+-------------------
+* get proper opencv dependency
+* Contributors: Vincent Rabaud
+
+1.11.7 (2014-03-28)
+-------------------
+
+1.11.6 (2014-01-29 00:38:55 +0100)
+----------------------------------
+- fix bad OpenCV linkage (#53)
diff --git a/image_proc/CMakeLists.txt b/image_proc/CMakeLists.txt
new file mode 100644
index 0000000..682f3d8
--- /dev/null
+++ b/image_proc/CMakeLists.txt
@@ -0,0 +1,60 @@
+cmake_minimum_required(VERSION 2.8)
+project(image_proc)
+
+find_package(catkin REQUIRED)
+
+find_package(catkin REQUIRED cv_bridge dynamic_reconfigure image_geometry image_transport nodelet nodelet_topic_tools roscpp sensor_msgs)
+find_package(OpenCV REQUIRED)
+find_package(Boost REQUIRED COMPONENTS thread)
+
+# Dynamic reconfigure support
+generate_dynamic_reconfigure_options(cfg/CropDecimate.cfg cfg/Debayer.cfg cfg/Rectify.cfg cfg/Resize.cfg)
+
+catkin_package(
+ CATKIN_DEPENDS image_geometry roscpp sensor_msgs
+ DEPENDS OpenCV
+ INCLUDE_DIRS include
+ LIBRARIES ${PROJECT_NAME}
+)
+
+include_directories(include ${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS})
+
+# Nodelet library
+add_library(${PROJECT_NAME} src/libimage_proc/processor.cpp
+ src/nodelets/debayer.cpp
+ src/nodelets/rectify.cpp
+ src/nodelets/resize.cpp
+ src/nodelets/crop_decimate.cpp
+ src/libimage_proc/advertisement_checker.cpp
+ src/nodelets/edge_aware.cpp
+ src/nodelets/crop_non_zero.cpp
+)
+add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg)
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${OpenCV_LIBRARIES})
+
+install(TARGETS ${PROJECT_NAME}
+ DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+)
+install(DIRECTORY include/${PROJECT_NAME}/
+ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+)
+install(FILES nodelet_plugins.xml
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+# Standalone node
+add_executable(image_proc_exe src/nodes/image_proc.cpp)
+target_link_libraries(image_proc_exe ${PROJECT_NAME} ${Boost_LIBRARIES} ${OpenCV_LIBRARIES})
+SET_TARGET_PROPERTIES(image_proc_exe PROPERTIES OUTPUT_NAME image_proc)
+install(TARGETS image_proc_exe
+ DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+# install the launch file
+install(DIRECTORY launch
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/
+)
+
+if(CATKIN_ENABLE_TESTING)
+ add_subdirectory(test)
+endif()
diff --git a/image_proc/cfg/CropDecimate.cfg b/image_proc/cfg/CropDecimate.cfg
new file mode 100755
index 0000000..e00ff75
--- /dev/null
+++ b/image_proc/cfg/CropDecimate.cfg
@@ -0,0 +1,35 @@
+#! /usr/bin/env python
+
+PACKAGE='image_proc'
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+# Decimation parameters
+gen.add("decimation_x", int_t, 0, "Number of pixels to decimate to one horizontally", 1, 1, 16)
+gen.add("decimation_y", int_t, 0, "Number of pixels to decimate to one vertically", 1, 1, 16)
+
+# ROI parameters
+# Maximums are arbitrary set to the resolution of a 5Mp Prosilica, since we can't set
+# the dynamically.
+gen.add("x_offset", int_t, 0, "X offset of the region of interest", 0, 0, 2447)
+gen.add("y_offset", int_t, 0, "Y offset of the region of interest", 0, 0, 2049)
+gen.add("width", int_t, 0, "Width of the region of interest", 0, 0, 2448)
+gen.add("height", int_t, 0, "Height of the region of interest", 0, 0, 2050)
+
+interpolate_enum = gen.enum([ gen.const("NN", int_t, 0, "Nearest-neighbor sampling"),
+ gen.const("Linear", int_t, 1, "Bilinear interpolation"),
+ gen.const("Cubic", int_t, 2, "Bicubic interpolation over 4x4 neighborhood"),
+ gen.const("Area", int_t, 3, "Resampling using pixel area relation"),
+ gen.const("Lanczos4", int_t, 4, "Lanczos interpolation over 8x8 neighborhood")],
+ "interpolation type")
+
+gen.add("interpolation", int_t, 0,
+ "Sampling algorithm",
+ 0, 0, 4, edit_method = interpolate_enum)
+
+# First string value is node name, used only for generating documentation
+# Second string value ("CropDecimate") is name of class and generated
+# .h file, with "Config" added, so class CropDecimateConfig
+exit(gen.generate(PACKAGE, "image_proc", "CropDecimate"))
diff --git a/image_proc/cfg/Debayer.cfg b/image_proc/cfg/Debayer.cfg
new file mode 100755
index 0000000..b5bc6d9
--- /dev/null
+++ b/image_proc/cfg/Debayer.cfg
@@ -0,0 +1,26 @@
+#! /usr/bin/env python
+
+PACKAGE='image_proc'
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+debayer_enum = gen.enum([ gen.const("Bilinear", int_t, 0,
+ "Fast algorithm using bilinear interpolation"),
+ gen.const("EdgeAware", int_t, 1,
+ "Edge-aware algorithm"),
+ gen.const("EdgeAwareWeighted", int_t, 2,
+ "Weighted edge-aware algorithm"),
+ gen.const("VNG", int_t, 3,
+ "Slow but high quality Variable Number of Gradients algorithm")],
+ "Debayering algorithm")
+
+gen.add("debayer", int_t, 0,
+ "Debayering algorithm",
+ 0, 0, 3, edit_method = debayer_enum)
+
+# First string value is node name, used only for generating documentation
+# Second string value ("Debayer") is name of class and generated
+# .h file, with "Config" added, so class DebayerConfig
+exit(gen.generate(PACKAGE, "image_proc", "Debayer"))
diff --git a/image_proc/cfg/Rectify.cfg b/image_proc/cfg/Rectify.cfg
new file mode 100755
index 0000000..c264cd2
--- /dev/null
+++ b/image_proc/cfg/Rectify.cfg
@@ -0,0 +1,22 @@
+#! /usr/bin/env python
+
+PACKAGE='image_proc'
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+interpolate_enum = gen.enum([ gen.const("NN", int_t, 0, "Nearest neighbor"),
+ gen.const("Linear", int_t, 1, "Linear"),
+ gen.const("Cubic", int_t, 2, "Cubic"),
+ gen.const("Lanczos4", int_t, 4, "Lanczos4")],
+ "interpolation type")
+
+gen.add("interpolation", int_t, 0,
+ "Interpolation algorithm between source image pixels",
+ 1, 0, 4, edit_method = interpolate_enum)
+
+# First string value is node name, used only for generating documentation
+# Second string value ("Rectify") is name of class and generated
+# .h file, with "Config" added, so class RectifyConfig
+exit(gen.generate(PACKAGE, "image_proc", "Rectify"))
diff --git a/image_proc/cfg/Resize.cfg b/image_proc/cfg/Resize.cfg
new file mode 100755
index 0000000..a299654
--- /dev/null
+++ b/image_proc/cfg/Resize.cfg
@@ -0,0 +1,37 @@
+#! /usr/bin/env python
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+
+PACKAGE = 'image_proc'
+ID = 'Resize'
+
+gen = ParameterGenerator()
+
+interpolate_enum = gen.enum([gen.const('NN', int_t, 0, 'Nearest neighbor'),
+ gen.const('Linear', int_t, 1, 'Linear'),
+ gen.const('Cubic', int_t, 2, 'Cubic'),
+ gen.const('Lanczos4', int_t, 4, 'Lanczos4')],
+ 'interpolation type')
+gen.add('interpolation', int_t, level=0,
+ description='Interpolation algorithm between source image pixels',
+ default=1, min=0, max=4, edit_method=interpolate_enum)
+
+gen.add('use_scale', bool_t, level=0,
+ description='Flag to use scale instead of static size.',
+ default=True)
+gen.add('scale_height', double_t, level=0,
+ description='Scale of height.',
+ default=1, min=0, max=10)
+gen.add('scale_width', double_t, level=0,
+ description='Scale of width',
+ default=1, min=0, max=10)
+
+gen.add('height', int_t, level=0,
+ description='Destination height. Ignored if negative.',
+ default=-1, min=-1)
+gen.add('width', int_t, level=0,
+ description='Destination width. Ignored if negative.',
+ default=-1, min=-1)
+
+exit(gen.generate(PACKAGE, PACKAGE, ID))
diff --git a/image_proc/include/image_proc/advertisement_checker.h b/image_proc/include/image_proc/advertisement_checker.h
new file mode 100644
index 0000000..cb7e64b
--- /dev/null
+++ b/image_proc/include/image_proc/advertisement_checker.h
@@ -0,0 +1,61 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+#ifndef IMAGE_PROC_ADVERTISEMENT_CHECKER_H
+#define IMAGE_PROC_ADVERTISEMENT_CHECKER_H
+
+#include <ros/ros.h>
+
+namespace image_proc {
+
+class AdvertisementChecker
+{
+ ros::NodeHandle nh_;
+ std::string name_;
+ ros::WallTimer timer_;
+ ros::V_string topics_;
+
+ void timerCb();
+
+public:
+ AdvertisementChecker(const ros::NodeHandle& nh = ros::NodeHandle(),
+ const std::string& name = std::string());
+
+ void start(const ros::V_string& topics, double duration);
+
+ void stop();
+};
+
+} // namespace image_proc
+
+#endif
diff --git a/image_proc/include/image_proc/processor.h b/image_proc/include/image_proc/processor.h
new file mode 100644
index 0000000..df2d3e4
--- /dev/null
+++ b/image_proc/include/image_proc/processor.h
@@ -0,0 +1,77 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+#ifndef IMAGE_PROC_PROCESSOR_H
+#define IMAGE_PROC_PROCESSOR_H
+
+#include <opencv2/core/core.hpp>
+#include <image_geometry/pinhole_camera_model.h>
+#include <sensor_msgs/Image.h>
+
+namespace image_proc {
+
+struct ImageSet
+{
+ std::string color_encoding;
+ cv::Mat mono;
+ cv::Mat rect;
+ cv::Mat color;
+ cv::Mat rect_color;
+};
+
+class Processor
+{
+public:
+ Processor()
+ : interpolation_(cv::INTER_LINEAR)
+ {
+ }
+
+ int interpolation_;
+
+ enum {
+ MONO = 1 << 0,
+ RECT = 1 << 1,
+ COLOR = 1 << 2,
+ RECT_COLOR = 1 << 3,
+ ALL = MONO | RECT | COLOR | RECT_COLOR
+ };
+
+ bool process(const sensor_msgs::ImageConstPtr& raw_image,
+ const image_geometry::PinholeCameraModel& model,
+ ImageSet& output, int flags = ALL) const;
+};
+
+} //namespace image_proc
+
+#endif
diff --git a/image_proc/launch/image_proc.launch b/image_proc/launch/image_proc.launch
new file mode 100644
index 0000000..bc321f0
--- /dev/null
+++ b/image_proc/launch/image_proc.launch
@@ -0,0 +1,29 @@
+<!-- Launch in the camera namespace containing "image_raw" and "camera_info" -->
+<launch>
+
+ <arg name="manager" /> <!-- Must be globally qualified -->
+ <arg name="respawn" default="false" />
+ <!-- TODO Arguments for debayer, interpolation methods? -->
+
+ <arg if="$(arg respawn)" name="bond" value="" />
+ <arg unless="$(arg respawn)" name="bond" value="--no-bond" />
+
+ <!-- Debayered images -->
+ <node pkg="nodelet" type="nodelet" name="debayer"
+ args="load image_proc/debayer $(arg manager) $(arg bond)"
+ respawn="$(arg respawn)" />
+
+ <!-- Monochrome rectified image -->
+ <node pkg="nodelet" type="nodelet" name="rectify_mono"
+ args="load image_proc/rectify $(arg manager) $(arg bond)"
+ respawn="$(arg respawn)" />
+
+ <!-- Color rectified image -->
+ <node pkg="nodelet" type="nodelet" name="rectify_color"
+ args="load image_proc/rectify $(arg manager) $(arg bond)"
+ respawn="$(arg respawn)">
+ <remap from="image_mono" to="image_color" />
+ <remap from="image_rect" to="image_rect_color" />
+ </node>
+
+</launch>
diff --git a/image_proc/mainpage.dox b/image_proc/mainpage.dox
new file mode 100644
index 0000000..ee611a5
--- /dev/null
+++ b/image_proc/mainpage.dox
@@ -0,0 +1,12 @@
+/**
+ at mainpage
+ at htmlinclude manifest.html
+
+ at b image_proc provides a node for performing single image rectification and
+color processing on the raw images produced by a camera. The outputs of
+image_proc are suitable for visual processing by other nodes. See
+http://www.ros.org/wiki/image_proc for documentation.
+
+Currently this package has no public code API.
+
+*/
diff --git a/image_proc/nodelet_plugins.xml b/image_proc/nodelet_plugins.xml
new file mode 100644
index 0000000..56835b2
--- /dev/null
+++ b/image_proc/nodelet_plugins.xml
@@ -0,0 +1,48 @@
+<library path="lib/libimage_proc">
+
+ <class name="image_proc/debayer"
+ type="image_proc::DebayerNodelet"
+ base_class_type="nodelet::Nodelet">
+ <description>
+ Nodelet to debayer (if needed) a raw camera image stream.
+ </description>
+ </class>
+
+ <class name="image_proc/rectify"
+ type="image_proc::RectifyNodelet"
+ base_class_type="nodelet::Nodelet">
+ <description>
+ Nodelet to rectify an unrectified camera image stream.
+ </description>
+ </class>
+
+ <class name="image_proc/resize"
+ type="image_proc::ResizeNodelet"
+ base_class_type="nodelet::Nodelet">
+ <description>
+ Nodelet to resize image and camera_info.
+ </description>
+ </class>
+
+ <class name="image_proc/crop_decimate"
+ type="image_proc::CropDecimateNodelet"
+ base_class_type="nodelet::Nodelet">
+ <description>
+ Nodelet to apply decimation (software binning) and ROI to a raw camera
+ image post-capture.
+ </description>
+ </class>
+
+ <!-- Deprecated Nodelet API -->
+ <class name="image_proc/crop_nonZero"
+ type="image_proc::CropNonZeroNodelet"
+ base_class_type="nodelet::Nodelet" />
+ <class name="image_proc/crop_non_zero"
+ type="image_proc::CropNonZeroNodelet"
+ base_class_type="nodelet::Nodelet">
+ <description>
+ Nodelet to crop the largest valid (Non Zero) area of the image.
+ </description>
+ </class>
+
+</library>
diff --git a/image_proc/package.xml b/image_proc/package.xml
new file mode 100644
index 0000000..3e948b7
--- /dev/null
+++ b/image_proc/package.xml
@@ -0,0 +1,39 @@
+<package>
+ <name>image_proc</name>
+ <version>1.12.20</version>
+ <description>Single image rectification and color processing.</description>
+ <author>Patrick Mihelich</author>
+ <author>Kurt Konolige</author>
+ <author>Jeremy Leibs</author>
+ <maintainer email="vincent.rabaud at gmail.com">Vincent Rabaud</maintainer>
+ <license>BSD</license>
+ <url>http://www.ros.org/wiki/image_proc</url>
+
+ <export>
+ <nodelet plugin="${prefix}/nodelet_plugins.xml" />
+ </export>
+
+ <buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
+
+ <test_depend>rostest</test_depend>
+ <test_depend>camera_calibration_parsers</test_depend>
+
+ <build_depend>boost</build_depend>
+ <build_depend>cv_bridge</build_depend>
+ <build_depend>dynamic_reconfigure</build_depend>
+ <build_depend>image_geometry</build_depend>
+ <build_depend>image_transport</build_depend>
+ <build_depend>nodelet</build_depend>
+ <build_depend>nodelet_topic_tools</build_depend>
+ <build_depend>roscpp</build_depend>
+ <build_depend>sensor_msgs</build_depend>
+
+ <run_depend>cv_bridge</run_depend>
+ <run_depend>dynamic_reconfigure</run_depend>
+ <run_depend>image_geometry</run_depend>
+ <run_depend>image_transport</run_depend>
+ <run_depend>nodelet</run_depend>
+ <run_depend>nodelet_topic_tools</run_depend>
+ <run_depend>roscpp</run_depend>
+ <run_depend>sensor_msgs</run_depend>
+</package>
diff --git a/image_proc/src/libimage_proc/advertisement_checker.cpp b/image_proc/src/libimage_proc/advertisement_checker.cpp
new file mode 100644
index 0000000..ee1b17a
--- /dev/null
+++ b/image_proc/src/libimage_proc/advertisement_checker.cpp
@@ -0,0 +1,92 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+#include "image_proc/advertisement_checker.h"
+#include <boost/foreach.hpp>
+
+namespace image_proc {
+
+AdvertisementChecker::AdvertisementChecker(const ros::NodeHandle& nh,
+ const std::string& name)
+ : nh_(nh),
+ name_(name)
+{
+}
+
+void AdvertisementChecker::timerCb()
+{
+ ros::master::V_TopicInfo topic_info;
+ if (!ros::master::getTopics(topic_info)) return;
+
+ ros::V_string::iterator topic_it = topics_.begin();
+ while (topic_it != topics_.end())
+ {
+ // Should use std::find_if
+ bool found = false;
+ ros::master::V_TopicInfo::iterator info_it = topic_info.begin();
+ while (!found && info_it != topic_info.end())
+ {
+ found = (*topic_it == info_it->name);
+ ++info_it;
+ }
+ if (found)
+ topic_it = topics_.erase(topic_it);
+ else
+ {
+ ROS_WARN_NAMED(name_, "The input topic '%s' is not yet advertised", topic_it->c_str());
+ ++topic_it;
+ }
+ }
+
+ if (topics_.empty())
+ stop();
+}
+
+void AdvertisementChecker::start(const ros::V_string& topics, double duration)
+{
+ topics_.clear();
+ BOOST_FOREACH(const std::string& topic, topics)
+ topics_.push_back(nh_.resolveName(topic));
+
+ ros::NodeHandle nh;
+ timer_ = nh.createWallTimer(ros::WallDuration(duration),
+ boost::bind(&AdvertisementChecker::timerCb, this));
+ timerCb();
+}
+
+void AdvertisementChecker::stop()
+{
+ timer_.stop();
+}
+
+} // namespace image_proc
diff --git a/image_proc/src/libimage_proc/processor.cpp b/image_proc/src/libimage_proc/processor.cpp
new file mode 100644
index 0000000..254b90a
--- /dev/null
+++ b/image_proc/src/libimage_proc/processor.cpp
@@ -0,0 +1,130 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+#include "image_proc/processor.h"
+#include <sensor_msgs/image_encodings.h>
+#include <ros/console.h>
+
+namespace image_proc {
+
+namespace enc = sensor_msgs::image_encodings;
+
+bool Processor::process(const sensor_msgs::ImageConstPtr& raw_image,
+ const image_geometry::PinholeCameraModel& model,
+ ImageSet& output, int flags) const
+{
+ static const int MONO_EITHER = MONO | RECT;
+ static const int COLOR_EITHER = COLOR | RECT_COLOR;
+ if (!(flags & ALL)) return true;
+
+ // Check if raw_image is color
+ const std::string& raw_encoding = raw_image->encoding;
+ int raw_type = CV_8UC1;
+ if (raw_encoding == enc::BGR8 || raw_encoding == enc::RGB8) {
+ raw_type = CV_8UC3;
+ output.color_encoding = raw_encoding;
+ }
+ // Construct cv::Mat pointing to raw_image data
+ const cv::Mat raw(raw_image->height, raw_image->width, raw_type,
+ const_cast<uint8_t*>(&raw_image->data[0]), raw_image->step);
+
+ ///////////////////////////////////////////////////////
+ // Construct colorized (unrectified) images from raw //
+ ///////////////////////////////////////////////////////
+
+ // Bayer case
+ if (raw_encoding.find("bayer") != std::string::npos) {
+ // Convert to color BGR
+ /// @todo Faster to convert directly to mono when color is not requested, but OpenCV doesn't support
+ int code = 0;
+ if (raw_encoding == enc::BAYER_RGGB8)
+ code = cv::COLOR_BayerBG2BGR;
+ else if (raw_encoding == enc::BAYER_BGGR8)
+ code = cv::COLOR_BayerRG2BGR;
+ else if (raw_encoding == enc::BAYER_GBRG8)
+ code = cv::COLOR_BayerGR2BGR;
+ else if (raw_encoding == enc::BAYER_GRBG8)
+ code = cv::COLOR_BayerGB2BGR;
+ else {
+ ROS_ERROR("[image_proc] Unsupported encoding '%s'", raw_encoding.c_str());
+ return false;
+ }
+ cv::cvtColor(raw, output.color, code);
+ output.color_encoding = enc::BGR8;
+
+ if (flags & MONO_EITHER)
+ cv::cvtColor(output.color, output.mono, cv::COLOR_BGR2GRAY);
+ }
+ // Color case
+ else if (raw_type == CV_8UC3) {
+ output.color = raw;
+ if (flags & MONO_EITHER) {
+ int code = (raw_encoding == enc::BGR8) ? cv::COLOR_BGR2GRAY : cv::COLOR_RGB2GRAY;
+ cv::cvtColor(output.color, output.mono, code);
+ }
+ }
+ // Mono case
+ else if (raw_encoding == enc::MONO8) {
+ output.mono = raw;
+ if (flags & COLOR_EITHER) {
+ output.color_encoding = enc::MONO8;
+ output.color = raw;
+ }
+ }
+ // 8UC3 does not specify a color encoding. Is it BGR, RGB, HSV, XYZ, LUV...?
+ else if (raw_encoding == enc::TYPE_8UC3) {
+ ROS_ERROR("[image_proc] Ambiguous encoding '8UC3'. The camera driver "
+ "should set the encoding to 'bgr8' or 'rgb8'.");
+ return false;
+ }
+ // Something else we can't handle
+ else {
+ ROS_ERROR("[image_proc] Unsupported encoding '%s'", raw_encoding.c_str());
+ return false;
+ }
+
+ //////////////////////////////////////////////////////
+ // Construct rectified images from colorized images //
+ //////////////////////////////////////////////////////
+
+ /// @todo If no distortion, could just point to the colorized data. But copy is
+ /// already way faster than remap.
+ if (flags & RECT)
+ model.rectifyImage(output.mono, output.rect, interpolation_);
+ if (flags & RECT_COLOR)
+ model.rectifyImage(output.color, output.rect_color, interpolation_);
+
+ return true;
+}
+
+} //namespace image_proc
diff --git a/image_proc/src/nodelets/crop_decimate.cpp b/image_proc/src/nodelets/crop_decimate.cpp
new file mode 100644
index 0000000..62a7880
--- /dev/null
+++ b/image_proc/src/nodelets/crop_decimate.cpp
@@ -0,0 +1,346 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+#include <boost/version.hpp>
+#if ((BOOST_VERSION / 100) % 1000) >= 53
+#include <boost/thread/lock_guard.hpp>
+#endif
+
+#include <ros/ros.h>
+#include <nodelet/nodelet.h>
+#include <image_transport/image_transport.h>
+#include <sensor_msgs/image_encodings.h>
+#include <dynamic_reconfigure/server.h>
+#include <cv_bridge/cv_bridge.h>
+#include <image_proc/CropDecimateConfig.h>
+#include <opencv2/imgproc/imgproc.hpp>
+
+namespace image_proc {
+
+using namespace cv_bridge; // CvImage, toCvShare
+
+class CropDecimateNodelet : public nodelet::Nodelet
+{
+ // ROS communication
+ boost::shared_ptr<image_transport::ImageTransport> it_in_, it_out_;
+ image_transport::CameraSubscriber sub_;
+ int queue_size_;
+
+ boost::mutex connect_mutex_;
+ image_transport::CameraPublisher pub_;
+
+ // Dynamic reconfigure
+ boost::recursive_mutex config_mutex_;
+ typedef image_proc::CropDecimateConfig Config;
+ typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
+ boost::shared_ptr<ReconfigureServer> reconfigure_server_;
+ Config config_;
+
+ virtual void onInit();
+
+ void connectCb();
+
+ void imageCb(const sensor_msgs::ImageConstPtr& image_msg,
+ const sensor_msgs::CameraInfoConstPtr& info_msg);
+
+ void configCb(Config &config, uint32_t level);
+};
+
+void CropDecimateNodelet::onInit()
+{
+ ros::NodeHandle& nh = getNodeHandle();
+ ros::NodeHandle& private_nh = getPrivateNodeHandle();
+ ros::NodeHandle nh_in (nh, "camera");
+ ros::NodeHandle nh_out(nh, "camera_out");
+ it_in_ .reset(new image_transport::ImageTransport(nh_in));
+ it_out_.reset(new image_transport::ImageTransport(nh_out));
+
+ // Read parameters
+ private_nh.param("queue_size", queue_size_, 5);
+
+ // Set up dynamic reconfigure
+ reconfigure_server_.reset(new ReconfigureServer(config_mutex_, private_nh));
+ ReconfigureServer::CallbackType f = boost::bind(&CropDecimateNodelet::configCb, this, _1, _2);
+ reconfigure_server_->setCallback(f);
+
+ // Monitor whether anyone is subscribed to the output
+ image_transport::SubscriberStatusCallback connect_cb = boost::bind(&CropDecimateNodelet::connectCb, this);
+ ros::SubscriberStatusCallback connect_cb_info = boost::bind(&CropDecimateNodelet::connectCb, this);
+ // Make sure we don't enter connectCb() between advertising and assigning to pub_
+ boost::lock_guard<boost::mutex> lock(connect_mutex_);
+ pub_ = it_out_->advertiseCamera("image_raw", 1, connect_cb, connect_cb, connect_cb_info, connect_cb_info);
+}
+
+// Handles (un)subscribing when clients (un)subscribe
+void CropDecimateNodelet::connectCb()
+{
+ boost::lock_guard<boost::mutex> lock(connect_mutex_);
+ if (pub_.getNumSubscribers() == 0)
+ sub_.shutdown();
+ else if (!sub_)
+ {
+ image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle());
+ sub_ = it_in_->subscribeCamera("image_raw", queue_size_, &CropDecimateNodelet::imageCb, this, hints);
+ }
+}
+
+template <typename T>
+void debayer2x2toBGR(const cv::Mat& src, cv::Mat& dst, int R, int G1, int G2, int B)
+{
+ typedef cv::Vec<T, 3> DstPixel; // 8- or 16-bit BGR
+ dst.create(src.rows / 2, src.cols / 2, cv::DataType<DstPixel>::type);
+
+ int src_row_step = src.step1();
+ int dst_row_step = dst.step1();
+ const T* src_row = src.ptr<T>();
+ T* dst_row = dst.ptr<T>();
+
+ // 2x2 downsample and debayer at once
+ for (int y = 0; y < dst.rows; ++y)
+ {
+ for (int x = 0; x < dst.cols; ++x)
+ {
+ dst_row[x*3 + 0] = src_row[x*2 + B];
+ dst_row[x*3 + 1] = (src_row[x*2 + G1] + src_row[x*2 + G2]) / 2;
+ dst_row[x*3 + 2] = src_row[x*2 + R];
+ }
+ src_row += src_row_step * 2;
+ dst_row += dst_row_step;
+ }
+}
+
+// Templated on pixel size, in bytes (MONO8 = 1, BGR8 = 3, RGBA16 = 8, ...)
+template <int N>
+void decimate(const cv::Mat& src, cv::Mat& dst, int decimation_x, int decimation_y)
+{
+ dst.create(src.rows / decimation_y, src.cols / decimation_x, src.type());
+
+ int src_row_step = src.step[0] * decimation_y;
+ int src_pixel_step = N * decimation_x;
+ int dst_row_step = dst.step[0];
+
+ const uint8_t* src_row = src.ptr();
+ uint8_t* dst_row = dst.ptr();
+
+ for (int y = 0; y < dst.rows; ++y)
+ {
+ const uint8_t* src_pixel = src_row;
+ uint8_t* dst_pixel = dst_row;
+ for (int x = 0; x < dst.cols; ++x)
+ {
+ memcpy(dst_pixel, src_pixel, N); // Should inline with small, fixed N
+ src_pixel += src_pixel_step;
+ dst_pixel += N;
+ }
+ src_row += src_row_step;
+ dst_row += dst_row_step;
+ }
+}
+
+void CropDecimateNodelet::imageCb(const sensor_msgs::ImageConstPtr& image_msg,
+ const sensor_msgs::CameraInfoConstPtr& info_msg)
+{
+ /// @todo Check image dimensions match info_msg
+ /// @todo Publish tweaks to config_ so they appear in reconfigure_gui
+
+ Config config;
+ {
+ boost::lock_guard<boost::recursive_mutex> lock(config_mutex_);
+ config = config_;
+ }
+ int decimation_x = config.decimation_x;
+ int decimation_y = config.decimation_y;
+
+ // Compute the ROI we'll actually use
+ bool is_bayer = sensor_msgs::image_encodings::isBayer(image_msg->encoding);
+ if (is_bayer)
+ {
+ // Odd offsets for Bayer images basically change the Bayer pattern, but that's
+ // unnecessarily complicated to support
+ config.x_offset &= ~0x1;
+ config.y_offset &= ~0x1;
+ config.width &= ~0x1;
+ config.height &= ~0x1;
+ }
+
+ int max_width = image_msg->width - config.x_offset;
+ int max_height = image_msg->height - config.y_offset;
+ int width = config.width;
+ int height = config.height;
+ if (width == 0 || width > max_width)
+ width = max_width;
+ if (height == 0 || height > max_height)
+ height = max_height;
+
+ // On no-op, just pass the messages along
+ if (decimation_x == 1 &&
+ decimation_y == 1 &&
+ config.x_offset == 0 &&
+ config.y_offset == 0 &&
+ width == (int)image_msg->width &&
+ height == (int)image_msg->height)
+ {
+ pub_.publish(image_msg, info_msg);
+ return;
+ }
+
+ // Get a cv::Mat view of the source data
+ CvImageConstPtr source = toCvShare(image_msg);
+
+ // Except in Bayer downsampling case, output has same encoding as the input
+ CvImage output(source->header, source->encoding);
+ // Apply ROI (no copy, still a view of the image_msg data)
+ output.image = source->image(cv::Rect(config.x_offset, config.y_offset, width, height));
+
+ // Special case: when decimating Bayer images, we first do a 2x2 decimation to BGR
+ if (is_bayer && (decimation_x > 1 || decimation_y > 1))
+ {
+ if (decimation_x % 2 != 0 || decimation_y % 2 != 0)
+ {
+ NODELET_ERROR_THROTTLE(2, "Odd decimation not supported for Bayer images");
+ return;
+ }
+
+ cv::Mat bgr;
+ int step = output.image.step1();
+ if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_RGGB8)
+ debayer2x2toBGR<uint8_t>(output.image, bgr, 0, 1, step, step + 1);
+ else if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_BGGR8)
+ debayer2x2toBGR<uint8_t>(output.image, bgr, step + 1, 1, step, 0);
+ else if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_GBRG8)
+ debayer2x2toBGR<uint8_t>(output.image, bgr, step, 0, step + 1, 1);
+ else if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_GRBG8)
+ debayer2x2toBGR<uint8_t>(output.image, bgr, 1, 0, step + 1, step);
+ else if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_RGGB16)
+ debayer2x2toBGR<uint16_t>(output.image, bgr, 0, 1, step, step + 1);
+ else if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_BGGR16)
+ debayer2x2toBGR<uint16_t>(output.image, bgr, step + 1, 1, step, 0);
+ else if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_GBRG16)
+ debayer2x2toBGR<uint16_t>(output.image, bgr, step, 0, step + 1, 1);
+ else if (image_msg->encoding == sensor_msgs::image_encodings::BAYER_GRBG16)
+ debayer2x2toBGR<uint16_t>(output.image, bgr, 1, 0, step + 1, step);
+ else
+ {
+ NODELET_ERROR_THROTTLE(2, "Unrecognized Bayer encoding '%s'", image_msg->encoding.c_str());
+ return;
+ }
+
+ output.image = bgr;
+ output.encoding = (bgr.depth() == CV_8U) ? sensor_msgs::image_encodings::BGR8
+ : sensor_msgs::image_encodings::BGR16;
+ decimation_x /= 2;
+ decimation_y /= 2;
+ }
+
+ // Apply further downsampling, if necessary
+ if (decimation_x > 1 || decimation_y > 1)
+ {
+ cv::Mat decimated;
+
+ if (config.interpolation == image_proc::CropDecimate_NN)
+ {
+ // Use optimized method instead of OpenCV's more general NN resize
+ int pixel_size = output.image.elemSize();
+ switch (pixel_size)
+ {
+ // Currently support up through 4-channel float
+ case 1:
+ decimate<1>(output.image, decimated, decimation_x, decimation_y);
+ break;
+ case 2:
+ decimate<2>(output.image, decimated, decimation_x, decimation_y);
+ break;
+ case 3:
+ decimate<3>(output.image, decimated, decimation_x, decimation_y);
+ break;
+ case 4:
+ decimate<4>(output.image, decimated, decimation_x, decimation_y);
+ break;
+ case 6:
+ decimate<6>(output.image, decimated, decimation_x, decimation_y);
+ break;
+ case 8:
+ decimate<8>(output.image, decimated, decimation_x, decimation_y);
+ break;
+ case 12:
+ decimate<12>(output.image, decimated, decimation_x, decimation_y);
+ break;
+ case 16:
+ decimate<16>(output.image, decimated, decimation_x, decimation_y);
+ break;
+ default:
+ NODELET_ERROR_THROTTLE(2, "Unsupported pixel size, %d bytes", pixel_size);
+ return;
+ }
+ }
+ else
+ {
+ // Linear, cubic, area, ...
+ cv::Size size(output.image.cols / decimation_x, output.image.rows / decimation_y);
+ cv::resize(output.image, decimated, size, 0.0, 0.0, config.interpolation);
+ }
+
+ output.image = decimated;
+ }
+
+ // Create output Image message
+ /// @todo Could save copies by allocating this above and having output.image alias it
+ sensor_msgs::ImagePtr out_image = output.toImageMsg();
+
+ // Create updated CameraInfo message
+ sensor_msgs::CameraInfoPtr out_info = boost::make_shared<sensor_msgs::CameraInfo>(*info_msg);
+ int binning_x = std::max((int)info_msg->binning_x, 1);
+ int binning_y = std::max((int)info_msg->binning_y, 1);
+ out_info->binning_x = binning_x * config.decimation_x;
+ out_info->binning_y = binning_y * config.decimation_y;
+ out_info->roi.x_offset += config.x_offset * binning_x;
+ out_info->roi.y_offset += config.y_offset * binning_y;
+ out_info->roi.height = height * binning_y;
+ out_info->roi.width = width * binning_x;
+ // If no ROI specified, leave do_rectify as-is. If ROI specified, set do_rectify = true.
+ if (width != (int)image_msg->width || height != (int)image_msg->height)
+ out_info->roi.do_rectify = true;
+
+ pub_.publish(out_image, out_info);
+}
+
+void CropDecimateNodelet::configCb(Config &config, uint32_t level)
+{
+ config_ = config;
+}
+
+} // namespace image_proc
+
+// Register nodelet
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS( image_proc::CropDecimateNodelet, nodelet::Nodelet)
diff --git a/image_proc/src/nodelets/crop_non_zero.cpp b/image_proc/src/nodelets/crop_non_zero.cpp
new file mode 100644
index 0000000..4c8cd05
--- /dev/null
+++ b/image_proc/src/nodelets/crop_non_zero.cpp
@@ -0,0 +1,146 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+#include <nodelet/nodelet.h>
+#include <image_transport/image_transport.h>
+#include <boost/thread.hpp>
+#include <cv_bridge/cv_bridge.h>
+//#include <algorithm> // for std::max_element
+
+namespace image_proc {
+
+namespace enc = sensor_msgs::image_encodings;
+
+class CropNonZeroNodelet : public nodelet::Nodelet
+{
+ // Subscriptions
+ boost::shared_ptr<image_transport::ImageTransport> it_;
+ image_transport::Subscriber sub_raw_;
+
+ // Publications
+ boost::mutex connect_mutex_;
+ image_transport::Publisher pub_;
+
+ virtual void onInit();
+
+ void connectCb();
+
+ void imageCb(const sensor_msgs::ImageConstPtr& raw_msg);
+};
+
+void CropNonZeroNodelet::onInit()
+{
+ ros::NodeHandle& nh = getNodeHandle();
+ it_.reset(new image_transport::ImageTransport(nh));
+
+ // Monitor whether anyone is subscribed to the output
+ image_transport::SubscriberStatusCallback connect_cb = boost::bind(&CropNonZeroNodelet::connectCb, this);
+ // Make sure we don't enter connectCb() between advertising and assigning to pub_depth_
+ boost::lock_guard<boost::mutex> lock(connect_mutex_);
+ pub_ = it_->advertise("image", 1, connect_cb, connect_cb);
+}
+
+// Handles (un)subscribing when clients (un)subscribe
+void CropNonZeroNodelet::connectCb()
+{
+ boost::lock_guard<boost::mutex> lock(connect_mutex_);
+ if (pub_.getNumSubscribers() == 0)
+ {
+ sub_raw_.shutdown();
+ }
+ else if (!sub_raw_)
+ {
+ image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle());
+ sub_raw_ = it_->subscribe("image_raw", 1, &CropNonZeroNodelet::imageCb, this, hints);
+ }
+}
+
+void CropNonZeroNodelet::imageCb(const sensor_msgs::ImageConstPtr& raw_msg)
+{
+ cv_bridge::CvImagePtr cv_ptr;
+ try
+ {
+ cv_ptr = cv_bridge::toCvCopy(raw_msg);
+ }
+ catch (cv_bridge::Exception& e)
+ {
+ ROS_ERROR("cv_bridge exception: %s", e.what());
+ return;
+ }
+
+ // Check the number of channels
+ if(sensor_msgs::image_encodings::numChannels(raw_msg->encoding) != 1){
+ NODELET_ERROR_THROTTLE(2, "Only grayscale image is acceptable, got [%s]", raw_msg->encoding.c_str());
+ return;
+ }
+
+ std::vector<std::vector<cv::Point> >cnt;
+ cv::Mat1b m(raw_msg->width, raw_msg->height);
+
+ if (raw_msg->encoding == enc::TYPE_8UC1){
+ m = cv_ptr->image;
+ }else{
+ double minVal, maxVal;
+ cv::minMaxIdx(cv_ptr->image, &minVal, &maxVal, 0, 0, cv_ptr->image != 0.);
+ double ra = maxVal - minVal;
+
+ cv_ptr->image.convertTo(m, CV_8U, 255./ra, -minVal*255./ra);
+ }
+
+ cv::findContours(m, cnt, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
+
+ // search the largest area
+ /* // -std=c++11
+ std::vector<std::vector<cv::Point> >::iterator it = std::max_element(cnt.begin(), cnt.end(), [](std::vector<cv::Point> a, std::vector<cv::Point> b) {
+ return a.size() < b.size();
+ });
+ */
+ std::vector<std::vector<cv::Point> >::iterator it = cnt.begin();
+ for(std::vector<std::vector<cv::Point> >::iterator i=cnt.begin();i!=cnt.end();++i){
+ it = (*it).size() < (*i).size() ? i : it;
+ }
+ cv::Rect r = cv::boundingRect(cnt[std::distance(cnt.begin(), it)]);
+
+ cv_bridge::CvImage out_msg;
+ out_msg.header = raw_msg->header;
+ out_msg.encoding = raw_msg->encoding;
+ out_msg.image = cv_ptr->image(r);
+
+ pub_.publish(out_msg.toImageMsg());
+}
+
+} // namespace image_proc
+
+// Register as nodelet
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(image_proc::CropNonZeroNodelet,nodelet::Nodelet);
diff --git a/image_proc/src/nodelets/debayer.cpp b/image_proc/src/nodelets/debayer.cpp
new file mode 100644
index 0000000..608cfe4
--- /dev/null
+++ b/image_proc/src/nodelets/debayer.cpp
@@ -0,0 +1,276 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+#include <boost/make_shared.hpp>
+#include <boost/version.hpp>
+#if ((BOOST_VERSION / 100) % 1000) >= 53
+#include <boost/thread/lock_guard.hpp>
+#endif
+
+#include <ros/ros.h>
+#include <nodelet/nodelet.h>
+#include <image_transport/image_transport.h>
+#include <sensor_msgs/image_encodings.h>
+#include <dynamic_reconfigure/server.h>
+#include <image_proc/DebayerConfig.h>
+
+#include <opencv2/imgproc/imgproc.hpp>
+// Until merged into OpenCV
+#include "edge_aware.h"
+
+#include <cv_bridge/cv_bridge.h>
+
+namespace image_proc {
+
+namespace enc = sensor_msgs::image_encodings;
+
+class DebayerNodelet : public nodelet::Nodelet
+{
+ // ROS communication
+ boost::shared_ptr<image_transport::ImageTransport> it_;
+ image_transport::Subscriber sub_raw_;
+
+ boost::mutex connect_mutex_;
+ image_transport::Publisher pub_mono_;
+ image_transport::Publisher pub_color_;
+
+ // Dynamic reconfigure
+ boost::recursive_mutex config_mutex_;
+ typedef image_proc::DebayerConfig Config;
+ typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
+ boost::shared_ptr<ReconfigureServer> reconfigure_server_;
+ Config config_;
+
+ virtual void onInit();
+
+ void connectCb();
+
+ void imageCb(const sensor_msgs::ImageConstPtr& raw_msg);
+
+ void configCb(Config &config, uint32_t level);
+};
+
+void DebayerNodelet::onInit()
+{
+ ros::NodeHandle &nh = getNodeHandle();
+ ros::NodeHandle &private_nh = getPrivateNodeHandle();
+ it_.reset(new image_transport::ImageTransport(nh));
+
+ // Set up dynamic reconfigure
+ reconfigure_server_.reset(new ReconfigureServer(config_mutex_, private_nh));
+ ReconfigureServer::CallbackType f = boost::bind(&DebayerNodelet::configCb, this, _1, _2);
+ reconfigure_server_->setCallback(f);
+
+ // Monitor whether anyone is subscribed to the output
+ typedef image_transport::SubscriberStatusCallback ConnectCB;
+ ConnectCB connect_cb = boost::bind(&DebayerNodelet::connectCb, this);
+ // Make sure we don't enter connectCb() between advertising and assigning to pub_XXX
+ boost::lock_guard<boost::mutex> lock(connect_mutex_);
+ pub_mono_ = it_->advertise("image_mono", 1, connect_cb, connect_cb);
+ pub_color_ = it_->advertise("image_color", 1, connect_cb, connect_cb);
+}
+
+// Handles (un)subscribing when clients (un)subscribe
+void DebayerNodelet::connectCb()
+{
+ boost::lock_guard<boost::mutex> lock(connect_mutex_);
+ if (pub_mono_.getNumSubscribers() == 0 && pub_color_.getNumSubscribers() == 0)
+ sub_raw_.shutdown();
+ else if (!sub_raw_)
+ {
+ image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle());
+ sub_raw_ = it_->subscribe("image_raw", 1, &DebayerNodelet::imageCb, this, hints);
+ }
+}
+
+void DebayerNodelet::imageCb(const sensor_msgs::ImageConstPtr& raw_msg)
+{
+ int bit_depth = enc::bitDepth(raw_msg->encoding);
+ //@todo Fix as soon as bitDepth fixes it
+ if (raw_msg->encoding == enc::YUV422)
+ bit_depth = 8;
+
+ // First publish to mono if needed
+ if (pub_mono_.getNumSubscribers())
+ {
+ if (enc::isMono(raw_msg->encoding))
+ pub_mono_.publish(raw_msg);
+ else
+ {
+ if ((bit_depth != 8) && (bit_depth != 16))
+ {
+ NODELET_WARN_THROTTLE(30,
+ "Raw image data from topic '%s' has unsupported depth: %d",
+ sub_raw_.getTopic().c_str(), bit_depth);
+ } else {
+ // Use cv_bridge to convert to Mono. If a type is not supported,
+ // it will error out there
+ sensor_msgs::ImagePtr gray_msg;
+ try
+ {
+ if (bit_depth == 8)
+ gray_msg = cv_bridge::toCvCopy(raw_msg, enc::MONO8)->toImageMsg();
+ else
+ gray_msg = cv_bridge::toCvCopy(raw_msg, enc::MONO16)->toImageMsg();
+ pub_mono_.publish(gray_msg);
+ }
+ catch (cv_bridge::Exception &e)
+ {
+ NODELET_WARN_THROTTLE(30, "cv_bridge conversion error: '%s'", e.what());
+ }
+ }
+ }
+ }
+
+ // Next, publish to color
+ if (!pub_color_.getNumSubscribers())
+ return;
+
+ if (enc::isMono(raw_msg->encoding))
+ {
+ // For monochrome, no processing needed!
+ pub_color_.publish(raw_msg);
+
+ // Warn if the user asked for color
+ NODELET_WARN_THROTTLE(30,
+ "Color topic '%s' requested, but raw image data from topic '%s' is grayscale",
+ pub_color_.getTopic().c_str(), sub_raw_.getTopic().c_str());
+ }
+ else if (enc::isColor(raw_msg->encoding))
+ {
+ pub_color_.publish(raw_msg);
+ }
+ else if (enc::isBayer(raw_msg->encoding)) {
+ int type = bit_depth == 8 ? CV_8U : CV_16U;
+ const cv::Mat bayer(raw_msg->height, raw_msg->width, CV_MAKETYPE(type, 1),
+ const_cast<uint8_t*>(&raw_msg->data[0]), raw_msg->step);
+
+ sensor_msgs::ImagePtr color_msg = boost::make_shared<sensor_msgs::Image>();
+ color_msg->header = raw_msg->header;
+ color_msg->height = raw_msg->height;
+ color_msg->width = raw_msg->width;
+ color_msg->encoding = bit_depth == 8? enc::BGR8 : enc::BGR16;
+ color_msg->step = color_msg->width * 3 * (bit_depth / 8);
+ color_msg->data.resize(color_msg->height * color_msg->step);
+
+ cv::Mat color(color_msg->height, color_msg->width, CV_MAKETYPE(type, 3),
+ &color_msg->data[0], color_msg->step);
+
+ int algorithm;
+ {
+ boost::lock_guard<boost::recursive_mutex> lock(config_mutex_);
+ algorithm = config_.debayer;
+ }
+
+ if (algorithm == Debayer_EdgeAware ||
+ algorithm == Debayer_EdgeAwareWeighted)
+ {
+ // These algorithms are not in OpenCV yet
+ if (raw_msg->encoding != enc::BAYER_GRBG8)
+ {
+ NODELET_WARN_THROTTLE(30, "Edge aware algorithms currently only support GRBG8 Bayer. "
+ "Falling back to bilinear interpolation.");
+ algorithm = Debayer_Bilinear;
+ }
+ else
+ {
+ if (algorithm == Debayer_EdgeAware)
+ debayerEdgeAware(bayer, color);
+ else
+ debayerEdgeAwareWeighted(bayer, color);
+ }
+ }
+ if (algorithm == Debayer_Bilinear ||
+ algorithm == Debayer_VNG)
+ {
+ int code = -1;
+ if (raw_msg->encoding == enc::BAYER_RGGB8 ||
+ raw_msg->encoding == enc::BAYER_RGGB16)
+ code = cv::COLOR_BayerBG2BGR;
+ else if (raw_msg->encoding == enc::BAYER_BGGR8 ||
+ raw_msg->encoding == enc::BAYER_BGGR16)
+ code = cv::COLOR_BayerRG2BGR;
+ else if (raw_msg->encoding == enc::BAYER_GBRG8 ||
+ raw_msg->encoding == enc::BAYER_GBRG16)
+ code = cv::COLOR_BayerGR2BGR;
+ else if (raw_msg->encoding == enc::BAYER_GRBG8 ||
+ raw_msg->encoding == enc::BAYER_GRBG16)
+ code = cv::COLOR_BayerGB2BGR;
+
+ if (algorithm == Debayer_VNG)
+ code += cv::COLOR_BayerBG2BGR_VNG - cv::COLOR_BayerBG2BGR;
+
+ cv::cvtColor(bayer, color, code);
+ }
+
+ pub_color_.publish(color_msg);
+ }
+ else if (raw_msg->encoding == enc::YUV422)
+ {
+ // Use cv_bridge to convert to BGR8
+ sensor_msgs::ImagePtr color_msg;
+ try
+ {
+ color_msg = cv_bridge::toCvCopy(raw_msg, enc::BGR8)->toImageMsg();
+ pub_color_.publish(color_msg);
+ }
+ catch (cv_bridge::Exception &e)
+ {
+ NODELET_WARN_THROTTLE(30, "cv_bridge conversion error: '%s'", e.what());
+ }
+ }
+ else if (raw_msg->encoding == enc::TYPE_8UC3)
+ {
+ // 8UC3 does not specify a color encoding. Is it BGR, RGB, HSV, XYZ, LUV...?
+ NODELET_ERROR_THROTTLE(10,
+ "Raw image topic '%s' has ambiguous encoding '8UC3'. The "
+ "source should set the encoding to 'bgr8' or 'rgb8'.",
+ sub_raw_.getTopic().c_str());
+ }
+ else
+ {
+ NODELET_ERROR_THROTTLE(10, "Raw image topic '%s' has unsupported encoding '%s'",
+ sub_raw_.getTopic().c_str(), raw_msg->encoding.c_str());
+ }
+}
+
+void DebayerNodelet::configCb(Config &config, uint32_t level)
+{
+ config_ = config;
+}
+
+} // namespace image_proc
+
+// Register nodelet
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS( image_proc::DebayerNodelet, nodelet::Nodelet)
diff --git a/image_proc/src/nodelets/edge_aware.cpp b/image_proc/src/nodelets/edge_aware.cpp
new file mode 100644
index 0000000..c882c8d
--- /dev/null
+++ b/image_proc/src/nodelets/edge_aware.cpp
@@ -0,0 +1,804 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+#include "edge_aware.h"
+
+#define AVG(a,b) (((int)(a) + (int)(b)) >> 1)
+#define AVG3(a,b,c) (((int)(a) + (int)(b) + (int)(c)) / 3)
+#define AVG4(a,b,c,d) (((int)(a) + (int)(b) + (int)(c) + (int)(d)) >> 2)
+#define WAVG4(a,b,c,d,x,y) ( ( ((int)(a) + (int)(b)) * (int)(x) + ((int)(c) + (int)(d)) * (int)(y) ) / ( 2 * ((int)(x) + (int(y))) ) )
+using namespace std;
+
+namespace image_proc {
+
+void debayerEdgeAware(const cv::Mat& bayer, cv::Mat& color)
+{
+ unsigned width = bayer.cols;
+ unsigned height = bayer.rows;
+ unsigned rgb_line_step = color.step[0];
+ unsigned rgb_line_skip = rgb_line_step - width * 3;
+ int bayer_line_step = bayer.step[0];
+ int bayer_line_step2 = bayer_line_step * 2;
+
+ unsigned char* rgb_buffer = color.data;
+ unsigned char* bayer_pixel = bayer.data;
+ unsigned yIdx, xIdx;
+
+ int dh, dv;
+
+ // first two pixel values for first two lines
+ // Bayer 0 1 2
+ // 0 G r g
+ // line_step b g b
+ // line_step2 g r g
+
+ rgb_buffer[3] = rgb_buffer[0] = bayer_pixel[1]; // red pixel
+ rgb_buffer[1] = bayer_pixel[0]; // green pixel
+ rgb_buffer[rgb_line_step + 2] = rgb_buffer[2] = bayer_pixel[bayer_line_step]; // blue;
+
+ // Bayer 0 1 2
+ // 0 g R g
+ // line_step b g b
+ // line_step2 g r g
+ //rgb_pixel[3] = bayer_pixel[1];
+ rgb_buffer[4] = AVG3 (bayer_pixel[0], bayer_pixel[2], bayer_pixel[bayer_line_step + 1]);
+ rgb_buffer[rgb_line_step + 5] = rgb_buffer[5] = AVG (bayer_pixel[bayer_line_step], bayer_pixel[bayer_line_step + 2]);
+
+ // BGBG line
+ // Bayer 0 1 2
+ // 0 g r g
+ // line_step B g b
+ // line_step2 g r g
+ rgb_buffer[rgb_line_step + 3] = rgb_buffer[rgb_line_step ] = AVG (bayer_pixel[1], bayer_pixel[bayer_line_step2 + 1]);
+ rgb_buffer[rgb_line_step + 1] = AVG3 (bayer_pixel[0], bayer_pixel[bayer_line_step + 1], bayer_pixel[bayer_line_step2]);
+ //rgb_pixel[rgb_line_step + 2] = bayer_pixel[line_step];
+
+ // pixel (1, 1) 0 1 2
+ // 0 g r g
+ // line_step b G b
+ // line_step2 g r g
+ //rgb_pixel[rgb_line_step + 3] = AVG( bayer_pixel[1] , bayer_pixel[line_step2+1] );
+ rgb_buffer[rgb_line_step + 4] = bayer_pixel[bayer_line_step + 1];
+ //rgb_pixel[rgb_line_step + 5] = AVG( bayer_pixel[line_step] , bayer_pixel[line_step+2] );
+
+ rgb_buffer += 6;
+ bayer_pixel += 2;
+ // rest of the first two lines
+ for (xIdx = 2; xIdx < width - 2; xIdx += 2, rgb_buffer += 6, bayer_pixel += 2)
+ {
+ // GRGR line
+ // Bayer -1 0 1 2
+ // 0 r G r g
+ // line_step g b g b
+ // line_step2 r g r g
+ rgb_buffer[0] = AVG (bayer_pixel[1], bayer_pixel[-1]);
+ rgb_buffer[1] = bayer_pixel[0];
+ rgb_buffer[2] = bayer_pixel[bayer_line_step + 1];
+
+ // Bayer -1 0 1 2
+ // 0 r g R g
+ // line_step g b g b
+ // line_step2 r g r g
+ rgb_buffer[3] = bayer_pixel[1];
+ rgb_buffer[4] = AVG3 (bayer_pixel[0], bayer_pixel[2], bayer_pixel[bayer_line_step + 1]);
+ rgb_buffer[rgb_line_step + 5] = rgb_buffer[5] = AVG (bayer_pixel[bayer_line_step], bayer_pixel[bayer_line_step + 2]);
+
+ // BGBG line
+ // Bayer -1 0 1 2
+ // 0 r g r g
+ // line_step g B g b
+ // line_step2 r g r g
+ rgb_buffer[rgb_line_step ] = AVG4 (bayer_pixel[1], bayer_pixel[bayer_line_step2 + 1], bayer_pixel[-1], bayer_pixel[bayer_line_step2 - 1]);
+ rgb_buffer[rgb_line_step + 1] = AVG4 (bayer_pixel[0], bayer_pixel[bayer_line_step2], bayer_pixel[bayer_line_step - 1], bayer_pixel[bayer_line_step + 1]);
+ rgb_buffer[rgb_line_step + 2] = bayer_pixel[bayer_line_step];
+
+ // Bayer -1 0 1 2
+ // 0 r g r g
+ // line_step g b G b
+ // line_step2 r g r g
+ rgb_buffer[rgb_line_step + 3] = AVG (bayer_pixel[1], bayer_pixel[bayer_line_step2 + 1]);
+ rgb_buffer[rgb_line_step + 4] = bayer_pixel[bayer_line_step + 1];
+ //rgb_pixel[rgb_line_step + 5] = AVG( bayer_pixel[line_step] , bayer_pixel[line_step+2] );
+ }
+
+ // last two pixel values for first two lines
+ // GRGR line
+ // Bayer -1 0 1
+ // 0 r G r
+ // line_step g b g
+ // line_step2 r g r
+ rgb_buffer[0] = AVG (bayer_pixel[1], bayer_pixel[-1]);
+ rgb_buffer[1] = bayer_pixel[0];
+ rgb_buffer[rgb_line_step + 5] = rgb_buffer[rgb_line_step + 2] = rgb_buffer[5] = rgb_buffer[2] = bayer_pixel[bayer_line_step];
+
+ // Bayer -1 0 1
+ // 0 r g R
+ // line_step g b g
+ // line_step2 r g r
+ rgb_buffer[3] = bayer_pixel[1];
+ rgb_buffer[4] = AVG (bayer_pixel[0], bayer_pixel[bayer_line_step + 1]);
+ //rgb_pixel[5] = bayer_pixel[line_step];
+
+ // BGBG line
+ // Bayer -1 0 1
+ // 0 r g r
+ // line_step g B g
+ // line_step2 r g r
+ rgb_buffer[rgb_line_step ] = AVG4 (bayer_pixel[1], bayer_pixel[bayer_line_step2 + 1], bayer_pixel[-1], bayer_pixel[bayer_line_step2 - 1]);
+ rgb_buffer[rgb_line_step + 1] = AVG4 (bayer_pixel[0], bayer_pixel[bayer_line_step2], bayer_pixel[bayer_line_step - 1], bayer_pixel[bayer_line_step + 1]);
+ //rgb_pixel[rgb_line_step + 2] = bayer_pixel[line_step];
+
+ // Bayer -1 0 1
+ // 0 r g r
+ // line_step g b G
+ // line_step2 r g r
+ rgb_buffer[rgb_line_step + 3] = AVG (bayer_pixel[1], bayer_pixel[bayer_line_step2 + 1]);
+ rgb_buffer[rgb_line_step + 4] = bayer_pixel[bayer_line_step + 1];
+ //rgb_pixel[rgb_line_step + 5] = bayer_pixel[line_step];
+
+ bayer_pixel += bayer_line_step + 2;
+ rgb_buffer += rgb_line_step + 6 + rgb_line_skip;
+ // main processing
+ for (yIdx = 2; yIdx < height - 2; yIdx += 2)
+ {
+ // first two pixel values
+ // Bayer 0 1 2
+ // -1 b g b
+ // 0 G r g
+ // line_step b g b
+ // line_step2 g r g
+
+ rgb_buffer[3] = rgb_buffer[0] = bayer_pixel[1]; // red pixel
+ rgb_buffer[1] = bayer_pixel[0]; // green pixel
+ rgb_buffer[2] = AVG (bayer_pixel[bayer_line_step], bayer_pixel[-bayer_line_step]); // blue;
+
+ // Bayer 0 1 2
+ // -1 b g b
+ // 0 g R g
+ // line_step b g b
+ // line_step2 g r g
+ //rgb_pixel[3] = bayer_pixel[1];
+ rgb_buffer[4] = AVG4 (bayer_pixel[0], bayer_pixel[2], bayer_pixel[bayer_line_step + 1], bayer_pixel[1 - bayer_line_step]);
+ rgb_buffer[5] = AVG4 (bayer_pixel[bayer_line_step], bayer_pixel[bayer_line_step + 2], bayer_pixel[-bayer_line_step], bayer_pixel[2 - bayer_line_step]);
+
+ // BGBG line
+ // Bayer 0 1 2
+ // 0 g r g
+ // line_step B g b
+ // line_step2 g r g
+ rgb_buffer[rgb_line_step + 3] = rgb_buffer[rgb_line_step ] = AVG (bayer_pixel[1], bayer_pixel[bayer_line_step2 + 1]);
+ rgb_buffer[rgb_line_step + 1] = AVG3 (bayer_pixel[0], bayer_pixel[bayer_line_step + 1], bayer_pixel[bayer_line_step2]);
+ rgb_buffer[rgb_line_step + 2] = bayer_pixel[bayer_line_step];
+
+ // pixel (1, 1) 0 1 2
+ // 0 g r g
+ // line_step b G b
+ // line_step2 g r g
+ //rgb_pixel[rgb_line_step + 3] = AVG( bayer_pixel[1] , bayer_pixel[line_step2+1] );
+ rgb_buffer[rgb_line_step + 4] = bayer_pixel[bayer_line_step + 1];
+ rgb_buffer[rgb_line_step + 5] = AVG (bayer_pixel[bayer_line_step], bayer_pixel[bayer_line_step + 2]);
+
+ rgb_buffer += 6;
+ bayer_pixel += 2;
+ // continue with rest of the line
+ for (xIdx = 2; xIdx < width - 2; xIdx += 2, rgb_buffer += 6, bayer_pixel += 2)
+ {
+ // GRGR line
+ // Bayer -1 0 1 2
+ // -1 g b g b
+ // 0 r G r g
+ // line_step g b g b
+ // line_step2 r g r g
+ rgb_buffer[0] = AVG (bayer_pixel[1], bayer_pixel[-1]);
+ rgb_buffer[1] = bayer_pixel[0];
+ rgb_buffer[2] = AVG (bayer_pixel[bayer_line_step], bayer_pixel[-bayer_line_step]);
+
+ // Bayer -1 0 1 2
+ // -1 g b g b
+ // 0 r g R g
+ // line_step g b g b
+ // line_step2 r g r g
+
+ dh = abs (bayer_pixel[0] - bayer_pixel[2]);
+ dv = abs (bayer_pixel[-bayer_line_step + 1] - bayer_pixel[bayer_line_step + 1]);
+
+ if (dh > dv)
+ rgb_buffer[4] = AVG (bayer_pixel[-bayer_line_step + 1], bayer_pixel[bayer_line_step + 1]);
+ else if (dv > dh)
+ rgb_buffer[4] = AVG (bayer_pixel[0], bayer_pixel[2]);
+ else
+ rgb_buffer[4] = AVG4 (bayer_pixel[-bayer_line_step + 1], bayer_pixel[bayer_line_step + 1], bayer_pixel[0], bayer_pixel[2]);
+
+ rgb_buffer[3] = bayer_pixel[1];
+ rgb_buffer[5] = AVG4 (bayer_pixel[-bayer_line_step], bayer_pixel[2 - bayer_line_step], bayer_pixel[bayer_line_step], bayer_pixel[bayer_line_step + 2]);
+
+ // BGBG line
+ // Bayer -1 0 1 2
+ // -1 g b g b
+ // 0 r g r g
+ // line_step g B g b
+ // line_step2 r g r g
+ rgb_buffer[rgb_line_step ] = AVG4 (bayer_pixel[1], bayer_pixel[bayer_line_step2 + 1], bayer_pixel[-1], bayer_pixel[bayer_line_step2 - 1]);
+ rgb_buffer[rgb_line_step + 2] = bayer_pixel[bayer_line_step];
+
+ dv = abs (bayer_pixel[0] - bayer_pixel[bayer_line_step2]);
+ dh = abs (bayer_pixel[bayer_line_step - 1] - bayer_pixel[bayer_line_step + 1]);
+
+ if (dv > dh)
+ rgb_buffer[rgb_line_step + 1] = AVG (bayer_pixel[bayer_line_step - 1], bayer_pixel[bayer_line_step + 1]);
+ else if (dh > dv)
+ rgb_buffer[rgb_line_step + 1] = AVG (bayer_pixel[0], bayer_pixel[bayer_line_step2]);
+ else
+ rgb_buffer[rgb_line_step + 1] = AVG4 (bayer_pixel[0], bayer_pixel[bayer_line_step2], bayer_pixel[bayer_line_step - 1], bayer_pixel[bayer_line_step + 1]);
+
+ // Bayer -1 0 1 2
+ // -1 g b g b
+ // 0 r g r g
+ // line_step g b G b
+ // line_step2 r g r g
+ rgb_buffer[rgb_line_step + 3] = AVG (bayer_pixel[1], bayer_pixel[bayer_line_step2 + 1]);
+ rgb_buffer[rgb_line_step + 4] = bayer_pixel[bayer_line_step + 1];
+ rgb_buffer[rgb_line_step + 5] = AVG (bayer_pixel[bayer_line_step], bayer_pixel[bayer_line_step + 2]);
+ }
+
+ // last two pixels of the line
+ // last two pixel values for first two lines
+ // GRGR line
+ // Bayer -1 0 1
+ // 0 r G r
+ // line_step g b g
+ // line_step2 r g r
+ rgb_buffer[0] = AVG (bayer_pixel[1], bayer_pixel[-1]);
+ rgb_buffer[1] = bayer_pixel[0];
+ rgb_buffer[rgb_line_step + 5] = rgb_buffer[rgb_line_step + 2] = rgb_buffer[5] = rgb_buffer[2] = bayer_pixel[bayer_line_step];
+
+ // Bayer -1 0 1
+ // 0 r g R
+ // line_step g b g
+ // line_step2 r g r
+ rgb_buffer[3] = bayer_pixel[1];
+ rgb_buffer[4] = AVG (bayer_pixel[0], bayer_pixel[bayer_line_step + 1]);
+ //rgb_pixel[5] = bayer_pixel[line_step];
+
+ // BGBG line
+ // Bayer -1 0 1
+ // 0 r g r
+ // line_step g B g
+ // line_step2 r g r
+ rgb_buffer[rgb_line_step ] = AVG4 (bayer_pixel[1], bayer_pixel[bayer_line_step2 + 1], bayer_pixel[-1], bayer_pixel[bayer_line_step2 - 1]);
+ rgb_buffer[rgb_line_step + 1] = AVG4 (bayer_pixel[0], bayer_pixel[bayer_line_step2], bayer_pixel[bayer_line_step - 1], bayer_pixel[bayer_line_step + 1]);
+ //rgb_pixel[rgb_line_step + 2] = bayer_pixel[line_step];
+
+ // Bayer -1 0 1
+ // 0 r g r
+ // line_step g b G
+ // line_step2 r g r
+ rgb_buffer[rgb_line_step + 3] = AVG (bayer_pixel[1], bayer_pixel[bayer_line_step2 + 1]);
+ rgb_buffer[rgb_line_step + 4] = bayer_pixel[bayer_line_step + 1];
+ //rgb_pixel[rgb_line_step + 5] = bayer_pixel[line_step];
+
+ bayer_pixel += bayer_line_step + 2;
+ rgb_buffer += rgb_line_step + 6 + rgb_line_skip;
+ }
+
+ //last two lines
+ // Bayer 0 1 2
+ // -1 b g b
+ // 0 G r g
+ // line_step b g b
+
+ rgb_buffer[rgb_line_step + 3] = rgb_buffer[rgb_line_step ] = rgb_buffer[3] = rgb_buffer[0] = bayer_pixel[1]; // red pixel
+ rgb_buffer[1] = bayer_pixel[0]; // green pixel
+ rgb_buffer[rgb_line_step + 2] = rgb_buffer[2] = bayer_pixel[bayer_line_step]; // blue;
+
+ // Bayer 0 1 2
+ // -1 b g b
+ // 0 g R g
+ // line_step b g b
+ //rgb_pixel[3] = bayer_pixel[1];
+ rgb_buffer[4] = AVG4 (bayer_pixel[0], bayer_pixel[2], bayer_pixel[bayer_line_step + 1], bayer_pixel[1 - bayer_line_step]);
+ rgb_buffer[5] = AVG4 (bayer_pixel[bayer_line_step], bayer_pixel[bayer_line_step + 2], bayer_pixel[-bayer_line_step], bayer_pixel[2 - bayer_line_step]);
+
+ // BGBG line
+ // Bayer 0 1 2
+ // -1 b g b
+ // 0 g r g
+ // line_step B g b
+ //rgb_pixel[rgb_line_step ] = bayer_pixel[1];
+ rgb_buffer[rgb_line_step + 1] = AVG (bayer_pixel[0], bayer_pixel[bayer_line_step + 1]);
+ rgb_buffer[rgb_line_step + 2] = bayer_pixel[bayer_line_step];
+
+ // Bayer 0 1 2
+ // -1 b g b
+ // 0 g r g
+ // line_step b G b
+ //rgb_pixel[rgb_line_step + 3] = AVG( bayer_pixel[1] , bayer_pixel[line_step2+1] );
+ rgb_buffer[rgb_line_step + 4] = bayer_pixel[bayer_line_step + 1];
+ rgb_buffer[rgb_line_step + 5] = AVG (bayer_pixel[bayer_line_step], bayer_pixel[bayer_line_step + 2]);
+
+ rgb_buffer += 6;
+ bayer_pixel += 2;
+ // rest of the last two lines
+ for (xIdx = 2; xIdx < width - 2; xIdx += 2, rgb_buffer += 6, bayer_pixel += 2)
+ {
+ // GRGR line
+ // Bayer -1 0 1 2
+ // -1 g b g b
+ // 0 r G r g
+ // line_step g b g b
+ rgb_buffer[0] = AVG (bayer_pixel[1], bayer_pixel[-1]);
+ rgb_buffer[1] = bayer_pixel[0];
+ rgb_buffer[2] = AVG (bayer_pixel[bayer_line_step], bayer_pixel[-bayer_line_step]);
+
+ // Bayer -1 0 1 2
+ // -1 g b g b
+ // 0 r g R g
+ // line_step g b g b
+ rgb_buffer[rgb_line_step + 3] = rgb_buffer[3] = bayer_pixel[1];
+ rgb_buffer[4] = AVG4 (bayer_pixel[0], bayer_pixel[2], bayer_pixel[bayer_line_step + 1], bayer_pixel[1 - bayer_line_step]);
+ rgb_buffer[5] = AVG4 (bayer_pixel[bayer_line_step], bayer_pixel[bayer_line_step + 2], bayer_pixel[-bayer_line_step], bayer_pixel[-bayer_line_step + 2]);
+
+ // BGBG line
+ // Bayer -1 0 1 2
+ // -1 g b g b
+ // 0 r g r g
+ // line_step g B g b
+ rgb_buffer[rgb_line_step ] = AVG (bayer_pixel[-1], bayer_pixel[1]);
+ rgb_buffer[rgb_line_step + 1] = AVG3 (bayer_pixel[0], bayer_pixel[bayer_line_step - 1], bayer_pixel[bayer_line_step + 1]);
+ rgb_buffer[rgb_line_step + 2] = bayer_pixel[bayer_line_step];
+
+
+ // Bayer -1 0 1 2
+ // -1 g b g b
+ // 0 r g r g
+ // line_step g b G b
+ //rgb_pixel[rgb_line_step + 3] = bayer_pixel[1];
+ rgb_buffer[rgb_line_step + 4] = bayer_pixel[bayer_line_step + 1];
+ rgb_buffer[rgb_line_step + 5] = AVG (bayer_pixel[bayer_line_step], bayer_pixel[bayer_line_step + 2]);
+ }
+
+ // last two pixel values for first two lines
+ // GRGR line
+ // Bayer -1 0 1
+ // -1 g b g
+ // 0 r G r
+ // line_step g b g
+ rgb_buffer[rgb_line_step ] = rgb_buffer[0] = AVG (bayer_pixel[1], bayer_pixel[-1]);
+ rgb_buffer[1] = bayer_pixel[0];
+ rgb_buffer[5] = rgb_buffer[2] = AVG (bayer_pixel[bayer_line_step], bayer_pixel[-bayer_line_step]);
+
+ // Bayer -1 0 1
+ // -1 g b g
+ // 0 r g R
+ // line_step g b g
+ rgb_buffer[rgb_line_step + 3] = rgb_buffer[3] = bayer_pixel[1];
+ rgb_buffer[4] = AVG3 (bayer_pixel[0], bayer_pixel[bayer_line_step + 1], bayer_pixel[-bayer_line_step + 1]);
+ //rgb_pixel[5] = AVG( bayer_pixel[line_step], bayer_pixel[-line_step] );
+
+ // BGBG line
+ // Bayer -1 0 1
+ // -1 g b g
+ // 0 r g r
+ // line_step g B g
+ //rgb_pixel[rgb_line_step ] = AVG2( bayer_pixel[-1], bayer_pixel[1] );
+ rgb_buffer[rgb_line_step + 1] = AVG3 (bayer_pixel[0], bayer_pixel[bayer_line_step - 1], bayer_pixel[bayer_line_step + 1]);
+ rgb_buffer[rgb_line_step + 5] = rgb_buffer[rgb_line_step + 2] = bayer_pixel[bayer_line_step];
+
+ // Bayer -1 0 1
+ // -1 g b g
+ // 0 r g r
+ // line_step g b G
+ //rgb_pixel[rgb_line_step + 3] = bayer_pixel[1];
+ rgb_buffer[rgb_line_step + 4] = bayer_pixel[bayer_line_step + 1];
+ //rgb_pixel[rgb_line_step + 5] = bayer_pixel[line_step];
+}
+
+void debayerEdgeAwareWeighted(const cv::Mat& bayer, cv::Mat& color)
+{
+ unsigned width = bayer.cols;
+ unsigned height = bayer.rows;
+ unsigned rgb_line_step = color.step[0];
+ unsigned rgb_line_skip = rgb_line_step - width * 3;
+ int bayer_line_step = bayer.step[0];
+ int bayer_line_step2 = bayer_line_step * 2;
+
+ unsigned char* rgb_buffer = color.data;
+ unsigned char* bayer_pixel = bayer.data;
+ unsigned yIdx, xIdx;
+
+ int dh, dv;
+
+ // first two pixel values for first two lines
+ // Bayer 0 1 2
+ // 0 G r g
+ // line_step b g b
+ // line_step2 g r g
+
+ rgb_buffer[3] = rgb_buffer[0] = bayer_pixel[1]; // red pixel
+ rgb_buffer[1] = bayer_pixel[0]; // green pixel
+ rgb_buffer[rgb_line_step + 2] = rgb_buffer[2] = bayer_pixel[bayer_line_step]; // blue;
+
+ // Bayer 0 1 2
+ // 0 g R g
+ // line_step b g b
+ // line_step2 g r g
+ //rgb_pixel[3] = bayer_pixel[1];
+ rgb_buffer[4] = AVG3 (bayer_pixel[0], bayer_pixel[2], bayer_pixel[bayer_line_step + 1]);
+ rgb_buffer[rgb_line_step + 5] = rgb_buffer[5] = AVG (bayer_pixel[bayer_line_step], bayer_pixel[bayer_line_step + 2]);
+
+ // BGBG line
+ // Bayer 0 1 2
+ // 0 g r g
+ // line_step B g b
+ // line_step2 g r g
+ rgb_buffer[rgb_line_step + 3] = rgb_buffer[rgb_line_step ] = AVG (bayer_pixel[1], bayer_pixel[bayer_line_step2 + 1]);
+ rgb_buffer[rgb_line_step + 1] = AVG3 (bayer_pixel[0], bayer_pixel[bayer_line_step + 1], bayer_pixel[bayer_line_step2]);
+ //rgb_pixel[rgb_line_step + 2] = bayer_pixel[line_step];
+
+ // pixel (1, 1) 0 1 2
+ // 0 g r g
+ // line_step b G b
+ // line_step2 g r g
+ //rgb_pixel[rgb_line_step + 3] = AVG( bayer_pixel[1] , bayer_pixel[line_step2+1] );
+ rgb_buffer[rgb_line_step + 4] = bayer_pixel[bayer_line_step + 1];
+ //rgb_pixel[rgb_line_step + 5] = AVG( bayer_pixel[line_step] , bayer_pixel[line_step+2] );
+
+ rgb_buffer += 6;
+ bayer_pixel += 2;
+ // rest of the first two lines
+ for (xIdx = 2; xIdx < width - 2; xIdx += 2, rgb_buffer += 6, bayer_pixel += 2)
+ {
+ // GRGR line
+ // Bayer -1 0 1 2
+ // 0 r G r g
+ // line_step g b g b
+ // line_step2 r g r g
+ rgb_buffer[0] = AVG (bayer_pixel[1], bayer_pixel[-1]);
+ rgb_buffer[1] = bayer_pixel[0];
+ rgb_buffer[2] = bayer_pixel[bayer_line_step + 1];
+
+ // Bayer -1 0 1 2
+ // 0 r g R g
+ // line_step g b g b
+ // line_step2 r g r g
+ rgb_buffer[3] = bayer_pixel[1];
+ rgb_buffer[4] = AVG3 (bayer_pixel[0], bayer_pixel[2], bayer_pixel[bayer_line_step + 1]);
+ rgb_buffer[rgb_line_step + 5] = rgb_buffer[5] = AVG (bayer_pixel[bayer_line_step], bayer_pixel[bayer_line_step + 2]);
+
+ // BGBG line
+ // Bayer -1 0 1 2
+ // 0 r g r g
+ // line_step g B g b
+ // line_step2 r g r g
+ rgb_buffer[rgb_line_step ] = AVG4 (bayer_pixel[1], bayer_pixel[bayer_line_step2 + 1], bayer_pixel[-1], bayer_pixel[bayer_line_step2 - 1]);
+ rgb_buffer[rgb_line_step + 1] = AVG4 (bayer_pixel[0], bayer_pixel[bayer_line_step2], bayer_pixel[bayer_line_step - 1], bayer_pixel[bayer_line_step + 1]);
+ rgb_buffer[rgb_line_step + 2] = bayer_pixel[bayer_line_step];
+
+ // Bayer -1 0 1 2
+ // 0 r g r g
+ // line_step g b G b
+ // line_step2 r g r g
+ rgb_buffer[rgb_line_step + 3] = AVG (bayer_pixel[1], bayer_pixel[bayer_line_step2 + 1]);
+ rgb_buffer[rgb_line_step + 4] = bayer_pixel[bayer_line_step + 1];
+ //rgb_pixel[rgb_line_step + 5] = AVG( bayer_pixel[line_step] , bayer_pixel[line_step+2] );
+ }
+
+ // last two pixel values for first two lines
+ // GRGR line
+ // Bayer -1 0 1
+ // 0 r G r
+ // line_step g b g
+ // line_step2 r g r
+ rgb_buffer[0] = AVG (bayer_pixel[1], bayer_pixel[-1]);
+ rgb_buffer[1] = bayer_pixel[0];
+ rgb_buffer[rgb_line_step + 5] = rgb_buffer[rgb_line_step + 2] = rgb_buffer[5] = rgb_buffer[2] = bayer_pixel[bayer_line_step];
+
+ // Bayer -1 0 1
+ // 0 r g R
+ // line_step g b g
+ // line_step2 r g r
+ rgb_buffer[3] = bayer_pixel[1];
+ rgb_buffer[4] = AVG (bayer_pixel[0], bayer_pixel[bayer_line_step + 1]);
+ //rgb_pixel[5] = bayer_pixel[line_step];
+
+ // BGBG line
+ // Bayer -1 0 1
+ // 0 r g r
+ // line_step g B g
+ // line_step2 r g r
+ rgb_buffer[rgb_line_step ] = AVG4 (bayer_pixel[1], bayer_pixel[bayer_line_step2 + 1], bayer_pixel[-1], bayer_pixel[bayer_line_step2 - 1]);
+ rgb_buffer[rgb_line_step + 1] = AVG4 (bayer_pixel[0], bayer_pixel[bayer_line_step2], bayer_pixel[bayer_line_step - 1], bayer_pixel[bayer_line_step + 1]);
+ //rgb_pixel[rgb_line_step + 2] = bayer_pixel[line_step];
+
+ // Bayer -1 0 1
+ // 0 r g r
+ // line_step g b G
+ // line_step2 r g r
+ rgb_buffer[rgb_line_step + 3] = AVG (bayer_pixel[1], bayer_pixel[bayer_line_step2 + 1]);
+ rgb_buffer[rgb_line_step + 4] = bayer_pixel[bayer_line_step + 1];
+ //rgb_pixel[rgb_line_step + 5] = bayer_pixel[line_step];
+
+ bayer_pixel += bayer_line_step + 2;
+ rgb_buffer += rgb_line_step + 6 + rgb_line_skip;
+ // main processing
+ for (yIdx = 2; yIdx < height - 2; yIdx += 2)
+ {
+ // first two pixel values
+ // Bayer 0 1 2
+ // -1 b g b
+ // 0 G r g
+ // line_step b g b
+ // line_step2 g r g
+
+ rgb_buffer[3] = rgb_buffer[0] = bayer_pixel[1]; // red pixel
+ rgb_buffer[1] = bayer_pixel[0]; // green pixel
+ rgb_buffer[2] = AVG (bayer_pixel[bayer_line_step], bayer_pixel[-bayer_line_step]); // blue;
+
+ // Bayer 0 1 2
+ // -1 b g b
+ // 0 g R g
+ // line_step b g b
+ // line_step2 g r g
+ //rgb_pixel[3] = bayer_pixel[1];
+ rgb_buffer[4] = AVG4 (bayer_pixel[0], bayer_pixel[2], bayer_pixel[bayer_line_step + 1], bayer_pixel[1 - bayer_line_step]);
+ rgb_buffer[5] = AVG4 (bayer_pixel[bayer_line_step], bayer_pixel[bayer_line_step + 2], bayer_pixel[-bayer_line_step], bayer_pixel[2 - bayer_line_step]);
+
+ // BGBG line
+ // Bayer 0 1 2
+ // 0 g r g
+ // line_step B g b
+ // line_step2 g r g
+ rgb_buffer[rgb_line_step + 3] = rgb_buffer[rgb_line_step ] = AVG (bayer_pixel[1], bayer_pixel[bayer_line_step2 + 1]);
+ rgb_buffer[rgb_line_step + 1] = AVG3 (bayer_pixel[0], bayer_pixel[bayer_line_step + 1], bayer_pixel[bayer_line_step2]);
+ rgb_buffer[rgb_line_step + 2] = bayer_pixel[bayer_line_step];
+
+ // pixel (1, 1) 0 1 2
+ // 0 g r g
+ // line_step b G b
+ // line_step2 g r g
+ //rgb_pixel[rgb_line_step + 3] = AVG( bayer_pixel[1] , bayer_pixel[line_step2+1] );
+ rgb_buffer[rgb_line_step + 4] = bayer_pixel[bayer_line_step + 1];
+ rgb_buffer[rgb_line_step + 5] = AVG (bayer_pixel[bayer_line_step], bayer_pixel[bayer_line_step + 2]);
+
+ rgb_buffer += 6;
+ bayer_pixel += 2;
+ // continue with rest of the line
+ for (xIdx = 2; xIdx < width - 2; xIdx += 2, rgb_buffer += 6, bayer_pixel += 2)
+ {
+ // GRGR line
+ // Bayer -1 0 1 2
+ // -1 g b g b
+ // 0 r G r g
+ // line_step g b g b
+ // line_step2 r g r g
+ rgb_buffer[0] = AVG (bayer_pixel[1], bayer_pixel[-1]);
+ rgb_buffer[1] = bayer_pixel[0];
+ rgb_buffer[2] = AVG (bayer_pixel[bayer_line_step], bayer_pixel[-bayer_line_step]);
+
+ // Bayer -1 0 1 2
+ // -1 g b g b
+ // 0 r g R g
+ // line_step g b g b
+ // line_step2 r g r g
+
+ dh = abs (bayer_pixel[0] - bayer_pixel[2]);
+ dv = abs (bayer_pixel[-bayer_line_step + 1] - bayer_pixel[bayer_line_step + 1]);
+
+ if (dv == 0 && dh == 0)
+ rgb_buffer[4] = AVG4 (bayer_pixel[1 - bayer_line_step], bayer_pixel[1 + bayer_line_step], bayer_pixel[0], bayer_pixel[2]);
+ else
+ rgb_buffer[4] = WAVG4 (bayer_pixel[1 - bayer_line_step], bayer_pixel[1 + bayer_line_step], bayer_pixel[0], bayer_pixel[2], dh, dv);
+ rgb_buffer[3] = bayer_pixel[1];
+ rgb_buffer[5] = AVG4 (bayer_pixel[-bayer_line_step], bayer_pixel[2 - bayer_line_step], bayer_pixel[bayer_line_step], bayer_pixel[bayer_line_step + 2]);
+
+ // BGBG line
+ // Bayer -1 0 1 2
+ // -1 g b g b
+ // 0 r g r g
+ // line_step g B g b
+ // line_step2 r g r g
+ rgb_buffer[rgb_line_step ] = AVG4 (bayer_pixel[1], bayer_pixel[bayer_line_step2 + 1], bayer_pixel[-1], bayer_pixel[bayer_line_step2 - 1]);
+ rgb_buffer[rgb_line_step + 2] = bayer_pixel[bayer_line_step];
+
+ dv = abs (bayer_pixel[0] - bayer_pixel[bayer_line_step2]);
+ dh = abs (bayer_pixel[bayer_line_step - 1] - bayer_pixel[bayer_line_step + 1]);
+
+ if (dv == 0 && dh == 0)
+ rgb_buffer[rgb_line_step + 1] = AVG4 (bayer_pixel[0], bayer_pixel[bayer_line_step2], bayer_pixel[bayer_line_step - 1], bayer_pixel[bayer_line_step + 1]);
+ else
+ rgb_buffer[rgb_line_step + 1] = WAVG4 (bayer_pixel[0], bayer_pixel[bayer_line_step2], bayer_pixel[bayer_line_step - 1], bayer_pixel[bayer_line_step + 1], dh, dv);
+
+ // Bayer -1 0 1 2
+ // -1 g b g b
+ // 0 r g r g
+ // line_step g b G b
+ // line_step2 r g r g
+ rgb_buffer[rgb_line_step + 3] = AVG (bayer_pixel[1], bayer_pixel[bayer_line_step2 + 1]);
+ rgb_buffer[rgb_line_step + 4] = bayer_pixel[bayer_line_step + 1];
+ rgb_buffer[rgb_line_step + 5] = AVG (bayer_pixel[bayer_line_step], bayer_pixel[bayer_line_step + 2]);
+ }
+
+ // last two pixels of the line
+ // last two pixel values for first two lines
+ // GRGR line
+ // Bayer -1 0 1
+ // 0 r G r
+ // line_step g b g
+ // line_step2 r g r
+ rgb_buffer[0] = AVG (bayer_pixel[1], bayer_pixel[-1]);
+ rgb_buffer[1] = bayer_pixel[0];
+ rgb_buffer[rgb_line_step + 5] = rgb_buffer[rgb_line_step + 2] = rgb_buffer[5] = rgb_buffer[2] = bayer_pixel[bayer_line_step];
+
+ // Bayer -1 0 1
+ // 0 r g R
+ // line_step g b g
+ // line_step2 r g r
+ rgb_buffer[3] = bayer_pixel[1];
+ rgb_buffer[4] = AVG (bayer_pixel[0], bayer_pixel[bayer_line_step + 1]);
+ //rgb_pixel[5] = bayer_pixel[line_step];
+
+ // BGBG line
+ // Bayer -1 0 1
+ // 0 r g r
+ // line_step g B g
+ // line_step2 r g r
+ rgb_buffer[rgb_line_step ] = AVG4 (bayer_pixel[1], bayer_pixel[bayer_line_step2 + 1], bayer_pixel[-1], bayer_pixel[bayer_line_step2 - 1]);
+ rgb_buffer[rgb_line_step + 1] = AVG4 (bayer_pixel[0], bayer_pixel[bayer_line_step2], bayer_pixel[bayer_line_step - 1], bayer_pixel[bayer_line_step + 1]);
+ //rgb_pixel[rgb_line_step + 2] = bayer_pixel[line_step];
+
+ // Bayer -1 0 1
+ // 0 r g r
+ // line_step g b G
+ // line_step2 r g r
+ rgb_buffer[rgb_line_step + 3] = AVG (bayer_pixel[1], bayer_pixel[bayer_line_step2 + 1]);
+ rgb_buffer[rgb_line_step + 4] = bayer_pixel[bayer_line_step + 1];
+ //rgb_pixel[rgb_line_step + 5] = bayer_pixel[line_step];
+
+ bayer_pixel += bayer_line_step + 2;
+ rgb_buffer += rgb_line_step + 6 + rgb_line_skip;
+ }
+
+ //last two lines
+ // Bayer 0 1 2
+ // -1 b g b
+ // 0 G r g
+ // line_step b g b
+
+ rgb_buffer[rgb_line_step + 3] = rgb_buffer[rgb_line_step ] = rgb_buffer[3] = rgb_buffer[0] = bayer_pixel[1]; // red pixel
+ rgb_buffer[1] = bayer_pixel[0]; // green pixel
+ rgb_buffer[rgb_line_step + 2] = rgb_buffer[2] = bayer_pixel[bayer_line_step]; // blue;
+
+ // Bayer 0 1 2
+ // -1 b g b
+ // 0 g R g
+ // line_step b g b
+ //rgb_pixel[3] = bayer_pixel[1];
+ rgb_buffer[4] = AVG4 (bayer_pixel[0], bayer_pixel[2], bayer_pixel[bayer_line_step + 1], bayer_pixel[1 - bayer_line_step]);
+ rgb_buffer[5] = AVG4 (bayer_pixel[bayer_line_step], bayer_pixel[bayer_line_step + 2], bayer_pixel[-bayer_line_step], bayer_pixel[2 - bayer_line_step]);
+
+ // BGBG line
+ // Bayer 0 1 2
+ // -1 b g b
+ // 0 g r g
+ // line_step B g b
+ //rgb_pixel[rgb_line_step ] = bayer_pixel[1];
+ rgb_buffer[rgb_line_step + 1] = AVG (bayer_pixel[0], bayer_pixel[bayer_line_step + 1]);
+ rgb_buffer[rgb_line_step + 2] = bayer_pixel[bayer_line_step];
+
+ // Bayer 0 1 2
+ // -1 b g b
+ // 0 g r g
+ // line_step b G b
+ //rgb_pixel[rgb_line_step + 3] = AVG( bayer_pixel[1] , bayer_pixel[line_step2+1] );
+ rgb_buffer[rgb_line_step + 4] = bayer_pixel[bayer_line_step + 1];
+ rgb_buffer[rgb_line_step + 5] = AVG (bayer_pixel[bayer_line_step], bayer_pixel[bayer_line_step + 2]);
+
+ rgb_buffer += 6;
+ bayer_pixel += 2;
+ // rest of the last two lines
+ for (xIdx = 2; xIdx < width - 2; xIdx += 2, rgb_buffer += 6, bayer_pixel += 2)
+ {
+ // GRGR line
+ // Bayer -1 0 1 2
+ // -1 g b g b
+ // 0 r G r g
+ // line_step g b g b
+ rgb_buffer[0] = AVG (bayer_pixel[1], bayer_pixel[-1]);
+ rgb_buffer[1] = bayer_pixel[0];
+ rgb_buffer[2] = AVG (bayer_pixel[bayer_line_step], bayer_pixel[-bayer_line_step]);
+
+ // Bayer -1 0 1 2
+ // -1 g b g b
+ // 0 r g R g
+ // line_step g b g b
+ rgb_buffer[rgb_line_step + 3] = rgb_buffer[3] = bayer_pixel[1];
+ rgb_buffer[4] = AVG4 (bayer_pixel[0], bayer_pixel[2], bayer_pixel[bayer_line_step + 1], bayer_pixel[1 - bayer_line_step]);
+ rgb_buffer[5] = AVG4 (bayer_pixel[bayer_line_step], bayer_pixel[bayer_line_step + 2], bayer_pixel[-bayer_line_step], bayer_pixel[-bayer_line_step + 2]);
+
+ // BGBG line
+ // Bayer -1 0 1 2
+ // -1 g b g b
+ // 0 r g r g
+ // line_step g B g b
+ rgb_buffer[rgb_line_step ] = AVG (bayer_pixel[-1], bayer_pixel[1]);
+ rgb_buffer[rgb_line_step + 1] = AVG3 (bayer_pixel[0], bayer_pixel[bayer_line_step - 1], bayer_pixel[bayer_line_step + 1]);
+ rgb_buffer[rgb_line_step + 2] = bayer_pixel[bayer_line_step];
+
+
+ // Bayer -1 0 1 2
+ // -1 g b g b
+ // 0 r g r g
+ // line_step g b G b
+ //rgb_pixel[rgb_line_step + 3] = bayer_pixel[1];
+ rgb_buffer[rgb_line_step + 4] = bayer_pixel[bayer_line_step + 1];
+ rgb_buffer[rgb_line_step + 5] = AVG (bayer_pixel[bayer_line_step], bayer_pixel[bayer_line_step + 2]);
+ }
+
+ // last two pixel values for first two lines
+ // GRGR line
+ // Bayer -1 0 1
+ // -1 g b g
+ // 0 r G r
+ // line_step g b g
+ rgb_buffer[rgb_line_step ] = rgb_buffer[0] = AVG (bayer_pixel[1], bayer_pixel[-1]);
+ rgb_buffer[1] = bayer_pixel[0];
+ rgb_buffer[5] = rgb_buffer[2] = AVG (bayer_pixel[bayer_line_step], bayer_pixel[-bayer_line_step]);
+
+ // Bayer -1 0 1
+ // -1 g b g
+ // 0 r g R
+ // line_step g b g
+ rgb_buffer[rgb_line_step + 3] = rgb_buffer[3] = bayer_pixel[1];
+ rgb_buffer[4] = AVG3 (bayer_pixel[0], bayer_pixel[bayer_line_step + 1], bayer_pixel[-bayer_line_step + 1]);
+ //rgb_pixel[5] = AVG( bayer_pixel[line_step], bayer_pixel[-line_step] );
+
+ // BGBG line
+ // Bayer -1 0 1
+ // -1 g b g
+ // 0 r g r
+ // line_step g B g
+ //rgb_pixel[rgb_line_step ] = AVG2( bayer_pixel[-1], bayer_pixel[1] );
+ rgb_buffer[rgb_line_step + 1] = AVG3 (bayer_pixel[0], bayer_pixel[bayer_line_step - 1], bayer_pixel[bayer_line_step + 1]);
+ rgb_buffer[rgb_line_step + 5] = rgb_buffer[rgb_line_step + 2] = bayer_pixel[bayer_line_step];
+
+ // Bayer -1 0 1
+ // -1 g b g
+ // 0 r g r
+ // line_step g b G
+ //rgb_pixel[rgb_line_step + 3] = bayer_pixel[1];
+ rgb_buffer[rgb_line_step + 4] = bayer_pixel[bayer_line_step + 1];
+ //rgb_pixel[rgb_line_step + 5] = bayer_pixel[line_step];
+
+}
+
+} // namespace image_proc
diff --git a/image_proc/src/nodelets/edge_aware.h b/image_proc/src/nodelets/edge_aware.h
new file mode 100644
index 0000000..dbc4ec1
--- /dev/null
+++ b/image_proc/src/nodelets/edge_aware.h
@@ -0,0 +1,49 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+#ifndef IMAGE_PROC_EDGE_AWARE
+#define IMAGE_PROC_EDGE_AWARE
+
+#include <opencv2/core/core.hpp>
+
+// Edge-aware debayering algorithms, intended for eventual inclusion in OpenCV.
+
+namespace image_proc {
+
+void debayerEdgeAware(const cv::Mat& bayer, cv::Mat& color);
+
+void debayerEdgeAwareWeighted(const cv::Mat& bayer, cv::Mat& color);
+
+} // namespace image_proc
+
+#endif
diff --git a/image_proc/src/nodelets/rectify.cpp b/image_proc/src/nodelets/rectify.cpp
new file mode 100644
index 0000000..dd2490c
--- /dev/null
+++ b/image_proc/src/nodelets/rectify.cpp
@@ -0,0 +1,170 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+#include <boost/version.hpp>
+#if ((BOOST_VERSION / 100) % 1000) >= 53
+#include <boost/thread/lock_guard.hpp>
+#endif
+
+#include <ros/ros.h>
+#include <nodelet/nodelet.h>
+#include <image_transport/image_transport.h>
+#include <image_geometry/pinhole_camera_model.h>
+#include <cv_bridge/cv_bridge.h>
+#include <dynamic_reconfigure/server.h>
+#include <image_proc/RectifyConfig.h>
+
+namespace image_proc {
+
+class RectifyNodelet : public nodelet::Nodelet
+{
+ // ROS communication
+ boost::shared_ptr<image_transport::ImageTransport> it_;
+ image_transport::CameraSubscriber sub_camera_;
+ int queue_size_;
+
+ boost::mutex connect_mutex_;
+ image_transport::Publisher pub_rect_;
+
+ // Dynamic reconfigure
+ boost::recursive_mutex config_mutex_;
+ typedef image_proc::RectifyConfig Config;
+ typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
+ boost::shared_ptr<ReconfigureServer> reconfigure_server_;
+ Config config_;
+
+ // Processing state (note: only safe because we're using single-threaded NodeHandle!)
+ image_geometry::PinholeCameraModel model_;
+
+ virtual void onInit();
+
+ void connectCb();
+
+ void imageCb(const sensor_msgs::ImageConstPtr& image_msg,
+ const sensor_msgs::CameraInfoConstPtr& info_msg);
+
+ void configCb(Config &config, uint32_t level);
+};
+
+void RectifyNodelet::onInit()
+{
+ ros::NodeHandle &nh = getNodeHandle();
+ ros::NodeHandle &private_nh = getPrivateNodeHandle();
+ it_.reset(new image_transport::ImageTransport(nh));
+
+ // Read parameters
+ private_nh.param("queue_size", queue_size_, 5);
+
+ // Set up dynamic reconfigure
+ reconfigure_server_.reset(new ReconfigureServer(config_mutex_, private_nh));
+ ReconfigureServer::CallbackType f = boost::bind(&RectifyNodelet::configCb, this, _1, _2);
+ reconfigure_server_->setCallback(f);
+
+ // Monitor whether anyone is subscribed to the output
+ image_transport::SubscriberStatusCallback connect_cb = boost::bind(&RectifyNodelet::connectCb, this);
+ // Make sure we don't enter connectCb() between advertising and assigning to pub_rect_
+ boost::lock_guard<boost::mutex> lock(connect_mutex_);
+ pub_rect_ = it_->advertise("image_rect", 1, connect_cb, connect_cb);
+}
+
+// Handles (un)subscribing when clients (un)subscribe
+void RectifyNodelet::connectCb()
+{
+ boost::lock_guard<boost::mutex> lock(connect_mutex_);
+ if (pub_rect_.getNumSubscribers() == 0)
+ sub_camera_.shutdown();
+ else if (!sub_camera_)
+ {
+ image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle());
+ sub_camera_ = it_->subscribeCamera("image_mono", queue_size_, &RectifyNodelet::imageCb, this, hints);
+ }
+}
+
+void RectifyNodelet::imageCb(const sensor_msgs::ImageConstPtr& image_msg,
+ const sensor_msgs::CameraInfoConstPtr& info_msg)
+{
+ // Verify camera is actually calibrated
+ if (info_msg->K[0] == 0.0) {
+ NODELET_ERROR_THROTTLE(30, "Rectified topic '%s' requested but camera publishing '%s' "
+ "is uncalibrated", pub_rect_.getTopic().c_str(),
+ sub_camera_.getInfoTopic().c_str());
+ return;
+ }
+
+ // If zero distortion, just pass the message along
+ bool zero_distortion = true;
+ for (size_t i = 0; i < info_msg->D.size(); ++i)
+ {
+ if (info_msg->D[i] != 0.0)
+ {
+ zero_distortion = false;
+ break;
+ }
+ }
+ // This will be true if D is empty/zero sized
+ if (zero_distortion)
+ {
+ pub_rect_.publish(image_msg);
+ return;
+ }
+
+ // Update the camera model
+ model_.fromCameraInfo(info_msg);
+
+ // Create cv::Mat views onto both buffers
+ const cv::Mat image = cv_bridge::toCvShare(image_msg)->image;
+ cv::Mat rect;
+
+ // Rectify and publish
+ int interpolation;
+ {
+ boost::lock_guard<boost::recursive_mutex> lock(config_mutex_);
+ interpolation = config_.interpolation;
+ }
+ model_.rectifyImage(image, rect, interpolation);
+
+ // Allocate new rectified image message
+ sensor_msgs::ImagePtr rect_msg = cv_bridge::CvImage(image_msg->header, image_msg->encoding, rect).toImageMsg();
+ pub_rect_.publish(rect_msg);
+}
+
+void RectifyNodelet::configCb(Config &config, uint32_t level)
+{
+ config_ = config;
+}
+
+} // namespace image_proc
+
+// Register nodelet
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS( image_proc::RectifyNodelet, nodelet::Nodelet)
diff --git a/image_proc/src/nodelets/resize.cpp b/image_proc/src/nodelets/resize.cpp
new file mode 100644
index 0000000..44fc9a3
--- /dev/null
+++ b/image_proc/src/nodelets/resize.cpp
@@ -0,0 +1,174 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2017, Kentaro Wada.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Kentaro Wada nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+#include <cv_bridge/cv_bridge.h>
+#include <dynamic_reconfigure/server.h>
+#include <nodelet/nodelet.h>
+#include <nodelet_topic_tools/nodelet_lazy.h>
+#include <ros/ros.h>
+#include <sensor_msgs/CameraInfo.h>
+#include <sensor_msgs/Image.h>
+
+#include "image_proc/ResizeConfig.h"
+
+namespace image_proc
+{
+
+class ResizeNodelet : public nodelet_topic_tools::NodeletLazy
+{
+protected:
+ // ROS communication
+ ros::Publisher pub_image_;
+ ros::Publisher pub_info_;
+ ros::Subscriber sub_info_;
+ ros::Subscriber sub_image_;
+
+ // Dynamic reconfigure
+ boost::recursive_mutex config_mutex_;
+ typedef image_proc::ResizeConfig Config;
+ typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
+ boost::shared_ptr<ReconfigureServer> reconfigure_server_;
+ Config config_;
+
+ virtual void onInit();
+ virtual void subscribe();
+ virtual void unsubscribe();
+
+ void imageCb(const sensor_msgs::ImageConstPtr& image_msg);
+ void infoCb(const sensor_msgs::CameraInfoConstPtr& info_msg);
+
+ void configCb(Config &config, uint32_t level);
+};
+
+void ResizeNodelet::onInit()
+{
+ nodelet_topic_tools::NodeletLazy::onInit();
+
+ // Set up dynamic reconfigure
+ reconfigure_server_.reset(new ReconfigureServer(config_mutex_, *pnh_));
+ ReconfigureServer::CallbackType f = boost::bind(&ResizeNodelet::configCb, this, _1, _2);
+ reconfigure_server_->setCallback(f);
+
+ pub_info_ = advertise<sensor_msgs::CameraInfo>(*pnh_, "camera_info", 1);
+ pub_image_ = advertise<sensor_msgs::Image>(*pnh_, "image", 1);
+
+ onInitPostProcess();
+}
+
+void ResizeNodelet::configCb(Config &config, uint32_t level)
+{
+ config_ = config;
+}
+
+void ResizeNodelet::subscribe()
+{
+ sub_info_ = nh_->subscribe("camera_info", 1, &ResizeNodelet::infoCb, this);
+ sub_image_ = nh_->subscribe("image", 1, &ResizeNodelet::imageCb, this);
+}
+
+void ResizeNodelet::unsubscribe()
+{
+ sub_info_.shutdown();
+ sub_image_.shutdown();
+}
+
+void ResizeNodelet::infoCb(const sensor_msgs::CameraInfoConstPtr& info_msg)
+{
+ Config config;
+ {
+ boost::lock_guard<boost::recursive_mutex> lock(config_mutex_);
+ config = config_;
+ }
+
+ sensor_msgs::CameraInfo dst_info_msg = *info_msg;
+
+ double scale_y;
+ double scale_x;
+ if (config.use_scale)
+ {
+ scale_y = config.scale_height;
+ scale_x = config.scale_width;
+ dst_info_msg.height = static_cast<int>(info_msg->height * config.scale_height);
+ dst_info_msg.width = static_cast<int>(info_msg->width * config.scale_width);
+ }
+ else
+ {
+ scale_y = static_cast<double>(config.height) / info_msg->height;
+ scale_x = static_cast<double>(config.width) / info_msg->width;
+ dst_info_msg.height = config.height;
+ dst_info_msg.width = config.width;
+ }
+
+ dst_info_msg.K[0] = dst_info_msg.K[0] * scale_x; // fx
+ dst_info_msg.K[2] = dst_info_msg.K[2] * scale_x; // cx
+ dst_info_msg.K[4] = dst_info_msg.K[4] * scale_y; // fy
+ dst_info_msg.K[5] = dst_info_msg.K[5] * scale_y; // cy
+
+ dst_info_msg.P[0] = dst_info_msg.P[0] * scale_x; // fx
+ dst_info_msg.P[2] = dst_info_msg.P[2] * scale_x; // cx
+ dst_info_msg.P[3] = dst_info_msg.P[3] * scale_x; // T
+ dst_info_msg.P[5] = dst_info_msg.P[5] * scale_y; // fy
+ dst_info_msg.P[6] = dst_info_msg.P[6] * scale_y; // cy
+
+ pub_info_.publish(dst_info_msg);
+}
+
+void ResizeNodelet::imageCb(const sensor_msgs::ImageConstPtr& image_msg)
+{
+ Config config;
+ {
+ boost::lock_guard<boost::recursive_mutex> lock(config_mutex_);
+ config = config_;
+ }
+
+ cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(image_msg);
+
+ if (config.use_scale)
+ {
+ cv::resize(cv_ptr->image, cv_ptr->image, cv::Size(0, 0),
+ config.scale_width, config.scale_height, config.interpolation);
+ }
+ else
+ {
+ int height = config.height == -1 ? image_msg->height : config.height;
+ int width = config.width == -1 ? image_msg->width : config.width;
+ cv::resize(cv_ptr->image, cv_ptr->image, cv::Size(width, height), 0, 0, config.interpolation);
+ }
+
+ pub_image_.publish(cv_ptr->toImageMsg());
+}
+
+} // namespace image_proc
+
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(image_proc::ResizeNodelet, nodelet::Nodelet)
diff --git a/image_proc/src/nodes/image_proc.cpp b/image_proc/src/nodes/image_proc.cpp
new file mode 100644
index 0000000..52612ab
--- /dev/null
+++ b/image_proc/src/nodes/image_proc.cpp
@@ -0,0 +1,97 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#include <ros/ros.h>
+#include <nodelet/loader.h>
+#include <image_proc/advertisement_checker.h>
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv, "image_proc");
+
+ // Check for common user errors
+ if (ros::names::remap("camera") != "camera")
+ {
+ ROS_WARN("Remapping 'camera' has no effect! Start image_proc in the "
+ "camera namespace instead.\nExample command-line usage:\n"
+ "\t$ ROS_NAMESPACE=%s rosrun image_proc image_proc",
+ ros::names::remap("camera").c_str());
+ }
+ if (ros::this_node::getNamespace() == "/")
+ {
+ ROS_WARN("Started in the global namespace! This is probably wrong. Start image_proc "
+ "in the camera namespace.\nExample command-line usage:\n"
+ "\t$ ROS_NAMESPACE=my_camera rosrun image_proc image_proc");
+ }
+
+ // Shared parameters to be propagated to nodelet private namespaces
+ ros::NodeHandle private_nh("~");
+ XmlRpc::XmlRpcValue shared_params;
+ int queue_size;
+ if (private_nh.getParam("queue_size", queue_size))
+ shared_params["queue_size"] = queue_size;
+
+ nodelet::Loader manager(false); // Don't bring up the manager ROS API
+ nodelet::M_string remappings;
+ nodelet::V_string my_argv;
+
+ // Debayer nodelet, image_raw -> image_mono, image_color
+ std::string debayer_name = ros::this_node::getName() + "_debayer";
+ manager.load(debayer_name, "image_proc/debayer", remappings, my_argv);
+
+ // Rectify nodelet, image_mono -> image_rect
+ std::string rectify_mono_name = ros::this_node::getName() + "_rectify_mono";
+ if (shared_params.valid())
+ ros::param::set(rectify_mono_name, shared_params);
+ manager.load(rectify_mono_name, "image_proc/rectify", remappings, my_argv);
+
+ // Rectify nodelet, image_color -> image_rect_color
+ // NOTE: Explicitly resolve any global remappings here, so they don't get hidden.
+ remappings["image_mono"] = ros::names::resolve("image_color");
+ remappings["image_rect"] = ros::names::resolve("image_rect_color");
+ std::string rectify_color_name = ros::this_node::getName() + "_rectify_color";
+ if (shared_params.valid())
+ ros::param::set(rectify_color_name, shared_params);
+ manager.load(rectify_color_name, "image_proc/rectify", remappings, my_argv);
+
+ // Check for only the original camera topics
+ ros::V_string topics;
+ topics.push_back(ros::names::resolve("image_raw"));
+ topics.push_back(ros::names::resolve("camera_info"));
+ image_proc::AdvertisementChecker check_inputs(ros::NodeHandle(), ros::this_node::getName());
+ check_inputs.start(topics, 60.0);
+
+ ros::spin();
+ return 0;
+}
diff --git a/image_proc/test/CMakeLists.txt b/image_proc/test/CMakeLists.txt
new file mode 100644
index 0000000..52fa285
--- /dev/null
+++ b/image_proc/test/CMakeLists.txt
@@ -0,0 +1,5 @@
+find_package(rostest REQUIRED)
+#catkin_add_gtest(image_proc_rostest rostest.cpp)
+#target_link_libraries(image_proc_rostest ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+add_rostest_gtest(image_proc_test_rectify test_rectify.xml test_rectify.cpp)
+target_link_libraries(image_proc_test_rectify ${catkin_LIBRARIES})
diff --git a/image_proc/test/rostest.cpp b/image_proc/test/rostest.cpp
new file mode 100644
index 0000000..b389fdb
--- /dev/null
+++ b/image_proc/test/rostest.cpp
@@ -0,0 +1,99 @@
+#include <ros/ros.h>
+#include <gtest/gtest.h>
+#include <camera_calibration_parsers/parse.h>
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/highgui/highgui.hpp>
+#include <image_transport/image_transport.h>
+
+#include <boost/foreach.hpp>
+
+class ImageProcTest : public testing::Test
+{
+protected:
+ virtual void SetUp()
+ {
+ ros::NodeHandle local_nh("~");
+
+ // Determine topic names
+ std::string camera_ns = nh.resolveName("camera") + "/";
+ if (camera_ns == "/camera")
+ throw "Must remap 'camera' to the camera namespace.";
+ topic_raw = camera_ns + "image_raw";
+ topic_mono = camera_ns + "image_mono";
+ topic_rect = camera_ns + "image_rect";
+ topic_color = camera_ns + "image_color";
+ topic_rect_color = camera_ns + "image_rect_color";
+
+ // Load raw image and cam info
+ /// @todo Make these cmd-line args instead?
+ std::string raw_image_file, cam_info_file;
+ if (!local_nh.getParam("raw_image_file", raw_image_file))
+ throw "Must set parameter ~raw_image_file.";
+ if (!local_nh.getParam("camera_info_file", cam_info_file))
+ throw "Must set parameter ~camera_info_file.";
+
+ /// @todo Test variety of encodings for raw image (bayer, mono, color)
+ cv::Mat img = cv::imread(raw_image_file, 0);
+ raw_image = cv_bridge::CvImage(std_msgs::Header(), "mono8", img).toImageMsg();
+ std::string cam_name;
+ if (!camera_calibration_parsers::readCalibration(cam_info_file, cam_name, cam_info))
+ throw "Failed to read camera info file.";
+
+ // Create raw camera publisher
+ image_transport::ImageTransport it(nh);
+ cam_pub = it.advertiseCamera(topic_raw, 1);
+
+ // Wait for image_proc to be operational
+ ros::master::V_TopicInfo topics;
+ while (true) {
+ if (ros::master::getTopics(topics)) {
+ BOOST_FOREACH(ros::master::TopicInfo& topic, topics) {
+ if (topic.name == topic_rect_color)
+ return;
+ }
+ }
+ ros::Duration(0.5).sleep();
+ }
+ }
+
+ ros::NodeHandle nh;
+ std::string topic_raw;
+ std::string topic_mono;
+ std::string topic_rect;
+ std::string topic_color;
+ std::string topic_rect_color;
+
+ sensor_msgs::ImagePtr raw_image;
+ sensor_msgs::CameraInfo cam_info;
+ image_transport::CameraPublisher cam_pub;
+
+ void publishRaw()
+ {
+ cam_pub.publish(*raw_image, cam_info);
+ }
+};
+
+void callback(const sensor_msgs::ImageConstPtr& msg)
+{
+ ROS_FATAL("Got an image");
+ ros::shutdown();
+}
+
+TEST_F(ImageProcTest, monoSubscription)
+{
+ ROS_INFO("In test. Subscribing.");
+ ros::Subscriber mono_sub = nh.subscribe(topic_mono, 1, callback);
+ ROS_INFO("Publishing.");
+ publishRaw();
+
+ ROS_INFO("Spinning.");
+ ros::spin();
+ ROS_INFO("Done.");
+}
+
+int main(int argc, char** argv)
+{
+ ros::init(argc, argv, "imageproc_rostest");
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/image_proc/test/test_bayer.xml b/image_proc/test/test_bayer.xml
new file mode 100644
index 0000000..a6d2a1a
--- /dev/null
+++ b/image_proc/test/test_bayer.xml
@@ -0,0 +1,10 @@
+<launch>
+ <node name="forearm_image_proc" pkg="image_proc" type="image_proc">
+ <remap from="camera" to="/forearm_camera_r"/>
+ </node>
+ <test test-name="test_bayer" pkg="image_proc" type="image_proc_rostest">
+ <remap from="camera" to="/forearm_camera_r"/>
+ <param name="raw_image_file" value="/u/mihelich/ros/vision_opencv/image_geometry/test/forearm_screenshot_3.png" />
+ <param name="camera_info_file" value="/u/mihelich/ros/vision_opencv/image_geometry/test/forearm.ini" />
+ </test>
+</launch>
diff --git a/image_proc/test/test_rectify.cpp b/image_proc/test/test_rectify.cpp
new file mode 100644
index 0000000..f03832d
--- /dev/null
+++ b/image_proc/test/test_rectify.cpp
@@ -0,0 +1,205 @@
+#include <ros/ros.h>
+#include <gtest/gtest.h>
+#include <camera_calibration_parsers/parse.h>
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/highgui/highgui.hpp>
+#include <image_transport/image_transport.h>
+#include <sensor_msgs/CameraInfo.h>
+#include <sensor_msgs/distortion_models.h>
+
+class ImageProcRectifyTest : public testing::Test
+{
+protected:
+ virtual void SetUp()
+ {
+ // Determine topic names
+ std::string camera_ns = nh_.resolveName("camera") + "/";
+ if (camera_ns == "/camera")
+ throw "Must remap 'camera' to the camera namespace.";
+ topic_raw_ = camera_ns + "image_raw";
+ topic_mono_ = camera_ns + "image_mono";
+ topic_rect_ = camera_ns + "image_rect";
+ topic_color_ = camera_ns + "image_color";
+ topic_rect_color_ = camera_ns + "image_rect_color";
+
+ // Taken from vision_opencv/image_geometry/test/utest.cpp
+ double D[] = {-0.363528858080088, 0.16117037733986861, -8.1109585007538829e-05, -0.00044776712298447841, 0.0};
+ double K[] = {430.15433020105519, 0.0, 311.71339830549732,
+ 0.0, 430.60920415473657, 221.06824942698509,
+ 0.0, 0.0, 1.0};
+ double R[] = {0.99806560714807102, 0.0068562422224214027, 0.061790256276695904,
+ -0.0067522959054715113, 0.99997541519165112, -0.0018909025066874664,
+ -0.061801701660692349, 0.0014700186639396652, 0.99808736527268516};
+ double P[] = {295.53402059708782, 0.0, 285.55760765075684, 0.0,
+ 0.0, 295.53402059708782, 223.29617881774902, 0.0,
+ 0.0, 0.0, 1.0, 0.0};
+
+ cam_info_.header.frame_id = "tf_frame";
+ cam_info_.height = 480;
+ cam_info_.width = 640;
+ // No ROI
+ cam_info_.D.resize(5);
+ std::copy(D, D+5, cam_info_.D.begin());
+ std::copy(K, K+9, cam_info_.K.begin());
+ std::copy(R, R+9, cam_info_.R.begin());
+ std::copy(P, P+12, cam_info_.P.begin());
+ cam_info_.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB;
+
+ distorted_image_ = cv::Mat(cv::Size(cam_info_.width, cam_info_.height), CV_8UC3);
+ // draw a grid
+ const cv::Scalar color = cv::Scalar(255, 255, 255);
+ // draw the lines thick so the proportion of error due to
+ // interpolation is reduced
+ const int thickness = 7;
+ const int type = 8;
+ for (size_t y = 0; y <= cam_info_.height; y += cam_info_.height/10)
+ {
+ cv::line(distorted_image_,
+ cv::Point(0, y), cv::Point(cam_info_.width, y),
+ color, type, thickness);
+ }
+ for (size_t x = 0; x <= cam_info_.width; x += cam_info_.width/10)
+ {
+ // draw the lines thick so the prorportion of interpolation error is reduced
+ cv::line(distorted_image_,
+ cv::Point(x, 0), cv::Point(x, cam_info_.height),
+ color, type, thickness);
+ }
+
+ raw_image_ = cv_bridge::CvImage(std_msgs::Header(), "bgr8",
+ distorted_image_).toImageMsg();
+
+ // Create raw camera subscriber and publisher
+ image_transport::ImageTransport it(nh_);
+ cam_pub_ = it.advertiseCamera(topic_raw_, 1);
+ }
+
+ ros::NodeHandle nh_;
+ std::string topic_raw_;
+ std::string topic_mono_;
+ std::string topic_rect_;
+ std::string topic_color_;
+ std::string topic_rect_color_;
+
+ cv::Mat distorted_image_;
+ sensor_msgs::ImagePtr raw_image_;
+ bool has_new_image_;
+ cv::Mat received_image_;
+ sensor_msgs::CameraInfo cam_info_;
+ image_transport::CameraPublisher cam_pub_;
+ image_transport::Subscriber cam_sub_;
+
+public:
+ void imageCallback(const sensor_msgs::ImageConstPtr& msg)
+ {
+ cv_bridge::CvImageConstPtr cv_ptr;
+ try
+ {
+ cv_ptr = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8);
+ }
+ catch (cv_bridge::Exception& e)
+ {
+ ROS_FATAL("cv_bridge exception: %s", e.what());
+ return;
+ }
+ received_image_ = cv_ptr->image.clone();
+ has_new_image_ = true;
+ }
+
+ void publishRaw()
+ {
+ has_new_image_ = false;
+ cam_pub_.publish(*raw_image_, cam_info_);
+ }
+};
+
+TEST_F(ImageProcRectifyTest, rectifyTest)
+{
+ ROS_INFO("In test. Subscribing.");
+ image_transport::ImageTransport it(nh_);
+ cam_sub_ = it.subscribe(topic_rect_, 1, &ImageProcRectifyTest::imageCallback,
+ dynamic_cast<ImageProcRectifyTest*>(this));
+ // Wait for image_proc to be operational
+ bool wait_for_topic = true;
+ while (wait_for_topic)
+ {
+ // @todo this fails without the additional 0.5 second sleep after the
+ // publisher comes online, which means on a slower or more heavily
+ // loaded system it may take longer than 0.5 seconds, and the test
+ // would hang until the timeout is reached and fail.
+ if (cam_sub_.getNumPublishers() > 0)
+ wait_for_topic = false;
+ ros::Duration(0.5).sleep();
+ }
+
+ // All the tests are the same as from
+ // vision_opencv/image_geometry/test/utest.cpp
+ // default cam info
+
+ // Just making this number up, maybe ought to be larger
+ // since a completely different image would be on the order of
+ // width * height * 255 = 78e6
+ const double diff_threshold = 10000.0;
+ double error;
+
+ // use original cam_info
+ publishRaw();
+ while (!has_new_image_)
+ {
+ ros::spinOnce();
+ ros::Duration(0.5).sleep();
+ }
+ // Test that rectified image is sufficiently different
+ // using default distortion
+ error = cv::norm(distorted_image_, received_image_, cv::NORM_L1);
+ // Just making this number up, maybe ought to be larger
+ EXPECT_GT(error, diff_threshold);
+
+ // Test that rectified image is sufficiently different
+ // using default distortion but with first element zeroed
+ // out.
+ sensor_msgs::CameraInfo cam_info_orig = cam_info_;
+ cam_info_.D[0] = 0.0;
+ publishRaw();
+ while (!has_new_image_)
+ {
+ ros::spinOnce();
+ ros::Duration(0.5).sleep();
+ }
+ error = cv::norm(distorted_image_, received_image_, cv::NORM_L1);
+ EXPECT_GT(error, diff_threshold);
+
+ // Test that rectified image is the same using zero distortion
+ cam_info_.D.assign(cam_info_.D.size(), 0);
+ publishRaw();
+ while (!has_new_image_)
+ {
+ ros::spinOnce();
+ ros::Duration(0.5).sleep();
+ }
+ error = cv::norm(distorted_image_, received_image_, cv::NORM_L1);
+ EXPECT_EQ(error, 0);
+
+
+ // Test that rectified image is the same using empty distortion
+ cam_info_.D.clear();
+ publishRaw();
+ while (!has_new_image_)
+ {
+ ros::spinOnce();
+ ros::Duration(0.5).sleep();
+ }
+ error = cv::norm(distorted_image_, received_image_, cv::NORM_L1);
+
+ EXPECT_EQ(error, 0);
+
+ // restore the original cam_info for other tests added in the future
+ cam_info_ = cam_info_orig;
+}
+
+int main(int argc, char** argv)
+{
+ ros::init(argc, argv, "image_proc_test_rectify");
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/image_proc/test/test_rectify.xml b/image_proc/test/test_rectify.xml
new file mode 100644
index 0000000..f1a451c
--- /dev/null
+++ b/image_proc/test/test_rectify.xml
@@ -0,0 +1,9 @@
+<launch>
+ <group ns="camera">
+ <node name="rectify_image_proc" pkg="image_proc" type="image_proc"
+ output="screen">
+ </node>
+ </group>
+ <test test-name="image_proc_test_rectify" pkg="image_proc" type="image_proc_test_rectify">
+ </test>
+</launch>
diff --git a/image_publisher/CHANGELOG.rst b/image_publisher/CHANGELOG.rst
new file mode 100644
index 0000000..ed64773
--- /dev/null
+++ b/image_publisher/CHANGELOG.rst
@@ -0,0 +1,45 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package image_publisher
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.12.20 (2017-04-30)
+--------------------
+* explicitly cast to std::vector<double> to make gcc6 happy
+ With gcc6, compiling image_publisher fails with this error:
+ ```
+ /[...]/image_publisher/src/nodelet/image_publisher_nodelet.cpp: In member function 'virtual void image_publisher::ImagePublisherNodelet::onInit()':
+ /[...]/image_publisher/src/nodelet/image_publisher_nodelet.cpp:180:43: error: ambiguous overload for 'operator=' (operand types are 'sensor_msgs::CameraInfo\_<std::allocator<void> >::_D_type {aka std::vector<double>}' and 'boost::assign_detail::generic_list<int>')
+ camera_info\_.D = list_of(0)(0)(0)(0)(0);
+ ```
+ After adding an initial explicit type cast for the assignment,
+ compiling fails further with:
+ ```
+ | /[...]/image_publisher/src/nodelet/image_publisher_nodelet.cpp: In member function 'virtual void image_publisher::ImagePublisherNodelet::onInit()':
+ | /[...]/image_publisher/src/nodelet/image_publisher_nodelet.cpp:180:65: error: call of overloaded 'vector(boost::assign_detail::generic_list<int>&)' is ambiguous
+ | camera_info\_.D = std::vector<double> (list_of(0)(0)(0)(0)(0));
+ ```
+ Various sources on the internet [1, 2, 3] point to use the
+ `convert_to_container` method; hence, this commit follows those
+ suggestions and with that image_publisher compiles with gcc6.
+ [1] http://stackoverflow.com/questions/16211410/ambiguity-when-using-boostassignlist-of-to-construct-a-stdvector
+ [2] http://stackoverflow.com/questions/12352692/`ambiguous-call-with-list-of-in-vs2010/12362548#12362548 <https://github.com/ambiguous-call-with-list-of-in-vs2010/12362548/issues/12362548>`_
+ [3] http://stackoverflow.com/questions/13285272/using-boostassignlist-of?rq=1
+ Signed-off-by: Lukas Bulwahn <lukas.bulwahn at oss.bmw-carit.de>
+* address gcc6 build error
+ With gcc6, compiling fails with `stdlib.h: No such file or directory`,
+ as including '-isystem /usr/include' breaks with gcc6, cf.,
+ https://gcc.gnu.org/bugzilla/show_bug.cgi?id=70129.
+ This commit addresses this issue for this package in the same way
+ it was addressed in various other ROS packages. A list of related
+ commits and pull requests is at:
+ https://github.com/ros/rosdistro/issues/12783
+ Signed-off-by: Lukas Bulwahn <lukas.bulwahn at oss.bmw-carit.de>
+* Contributors: Lukas Bulwahn
+
+1.12.19 (2016-07-24)
+--------------------
+* add image_publisher
+* Contributors: Kei Okada
+
+* add image_publisher
+* Contributors: Kei Okada
diff --git a/image_publisher/CMakeLists.txt b/image_publisher/CMakeLists.txt
new file mode 100644
index 0000000..431109c
--- /dev/null
+++ b/image_publisher/CMakeLists.txt
@@ -0,0 +1,29 @@
+cmake_minimum_required(VERSION 2.8)
+project(image_publisher)
+
+find_package(catkin REQUIRED COMPONENTS cv_bridge dynamic_reconfigure camera_info_manager sensor_msgs image_transport nodelet roscpp)
+
+# generate the dynamic_reconfigure config file
+generate_dynamic_reconfigure_options(cfg/ImagePublisher.cfg)
+
+catkin_package()
+
+include_directories(${catkin_INCLUDE_DIRS})
+
+add_library(${PROJECT_NAME} SHARED src/nodelet/image_publisher_nodelet.cpp)
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg)
+install(TARGETS image_publisher
+ DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+)
+
+add_executable(image_publisher_exe src/node/image_publisher.cpp)
+SET_TARGET_PROPERTIES(image_publisher_exe PROPERTIES OUTPUT_NAME image_publisher)
+target_link_libraries(image_publisher_exe ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
+
+install(TARGETS image_publisher_exe
+ DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+install(FILES nodelet_plugins.xml
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
diff --git a/image_publisher/cfg/ImagePublisher.cfg b/image_publisher/cfg/ImagePublisher.cfg
new file mode 100755
index 0000000..6b17660
--- /dev/null
+++ b/image_publisher/cfg/ImagePublisher.cfg
@@ -0,0 +1,44 @@
+#! /usr/bin/env python
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2010, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# * Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above
+# copyright notice, this list of conditions and the following
+# disclaimer in the documentation and/or other materials provided
+# with the distribution.
+# * Neither the name of Willow Garage, Inc. nor the names of its
+# contributors may be used to endorse or promote products derived
+# from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+
+PACKAGE='image_publisher'
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+gen.add("frame_id", str_t, 0, "Frame to use for camera image", "camera")
+gen.add("publish_rate", double_t, 0, "Rate to publish image", 10, 0.1, 30)
+gen.add("camera_info_url", str_t, 0, "Path to camera_info", "")
+
+exit(gen.generate(PACKAGE, "image_publisher", "ImagePublisher"))
diff --git a/image_publisher/nodelet_plugins.xml b/image_publisher/nodelet_plugins.xml
new file mode 100644
index 0000000..8d3a82c
--- /dev/null
+++ b/image_publisher/nodelet_plugins.xml
@@ -0,0 +1,7 @@
+<library path="lib/libimage_publisher">
+
+ <class name="image_publisher/image_publisher" type="image_publisher::ImagePublisherNodelet" base_class_type="nodelet::Nodelet">
+ <description>Nodelet to publish sensor_msgs/Image</description>
+ </class>
+
+</library>
diff --git a/image_publisher/package.xml b/image_publisher/package.xml
new file mode 100644
index 0000000..0c199bc
--- /dev/null
+++ b/image_publisher/package.xml
@@ -0,0 +1,35 @@
+<package>
+ <name>image_publisher</name>
+ <version>1.12.20</version>
+ <description>
+ <p>
+ Contains a node publish an image stream from single image file
+ or avi motion file.
+ </p>
+ </description>
+ <author>Kei Okada</author>
+ <maintainer email="vincent.rabaud at gmail.com">Vincent Rabaud</maintainer>
+ <license>BSD</license>
+ <url>http://ros.org/wiki/image_publisher</url>
+
+ <buildtool_depend>catkin</buildtool_depend>
+
+ <build_depend>cv_bridge</build_depend>
+ <build_depend>dynamic_reconfigure</build_depend>
+ <build_depend>camera_info_manager</build_depend>
+ <build_depend>image_transport</build_depend>
+ <build_depend>nodelet</build_depend>
+ <build_depend>roscpp</build_depend>
+ <build_depend>sensor_msgs</build_depend>
+
+ <run_depend>cv_bridge</run_depend>
+ <run_depend>dynamic_reconfigure</run_depend>
+ <run_depend>camera_info_manager</run_depend>
+ <run_depend>image_transport</run_depend>
+ <run_depend>nodelet</run_depend>
+ <run_depend>roscpp</run_depend>
+ <run_depend>sensor_msgs</run_depend>
+ <export>
+ <nodelet plugin="${prefix}/nodelet_plugins.xml"/>
+ </export>
+</package>
diff --git a/image_publisher/src/node/image_publisher.cpp b/image_publisher/src/node/image_publisher.cpp
new file mode 100644
index 0000000..890445b
--- /dev/null
+++ b/image_publisher/src/node/image_publisher.cpp
@@ -0,0 +1,57 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+#include <ros/ros.h>
+#include <nodelet/loader.h>
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv, "image_publisher", ros::init_options::AnonymousName);
+
+ if (argc <= 1) {
+ ROS_ERROR("image_publisher requires filename. Typical command-line usage:\n"
+ "\t$ rosrun image_publisher image_publisher <filename>");
+ return 1;
+ }
+
+ ros::param::set("~filename", argv[1]);
+ nodelet::Loader manager(false);
+ nodelet::M_string remappings;
+ nodelet::V_string my_argv(argv + 1, argv + argc);
+ my_argv.push_back("--shutdown-on-close"); // Internal
+
+ manager.load(ros::this_node::getName(), "image_publisher/image_publisher", remappings, my_argv);
+
+ ros::spin();
+ return 0;
+}
diff --git a/image_publisher/src/nodelet/image_publisher_nodelet.cpp b/image_publisher/src/nodelet/image_publisher_nodelet.cpp
new file mode 100644
index 0000000..26e1352
--- /dev/null
+++ b/image_publisher/src/nodelet/image_publisher_nodelet.cpp
@@ -0,0 +1,194 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2014, JSK Lab.
+* 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#include <ros/ros.h>
+#include <nodelet/nodelet.h>
+#include <cv_bridge/cv_bridge.h>
+#include <image_publisher/ImagePublisherConfig.h>
+#include <image_transport/image_transport.h>
+#include <sensor_msgs/CameraInfo.h>
+#include <camera_info_manager/camera_info_manager.h>
+#include <opencv2/highgui/highgui.hpp>
+#include <dynamic_reconfigure/server.h>
+#include <boost/assign.hpp>
+using namespace boost::assign;
+
+namespace image_publisher {
+class ImagePublisherNodelet : public nodelet::Nodelet
+{
+ dynamic_reconfigure::Server<image_publisher::ImagePublisherConfig> srv;
+
+ image_transport::CameraPublisher pub_;
+
+ boost::shared_ptr<image_transport::ImageTransport> it_;
+ ros::NodeHandle nh_;
+
+ cv::VideoCapture cap_;
+ cv::Mat image_;
+ int subscriber_count_;
+ ros::Timer timer_;
+
+ std::string frame_id_;
+ std::string filename_;
+ bool flip_image_;
+ int flip_value_;
+ sensor_msgs::CameraInfo camera_info_;
+
+
+ void reconfigureCallback(image_publisher::ImagePublisherConfig &new_config, uint32_t level)
+ {
+ frame_id_ = new_config.frame_id;
+
+ timer_ = nh_.createTimer(ros::Duration(1.0/new_config.publish_rate), &ImagePublisherNodelet::do_work, this);
+
+ camera_info_manager::CameraInfoManager c(nh_);
+ if ( !new_config.camera_info_url.empty() ) {
+ try {
+ c.validateURL(new_config.camera_info_url);
+ c.loadCameraInfo(new_config.camera_info_url);
+ camera_info_ = c.getCameraInfo();
+ } catch(cv::Exception &e) {
+ NODELET_ERROR("camera calibration failed to load: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
+ }
+ }
+
+
+ }
+
+ void do_work(const ros::TimerEvent& event)
+ {
+ // Transform the image.
+ try
+ {
+ if ( cap_.isOpened() ) {
+ if ( ! cap_.read(image_) ) {
+ cap_.set(CV_CAP_PROP_POS_FRAMES, 0);
+ }
+ }
+ if (flip_image_)
+ cv::flip(image_, image_, flip_value_);
+
+ sensor_msgs::ImagePtr out_img = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_).toImageMsg();
+ out_img->header.frame_id = frame_id_;
+ out_img->header.stamp = ros::Time::now();
+ camera_info_.header.frame_id = out_img->header.frame_id;
+ camera_info_.header.stamp = out_img->header.stamp;
+
+ pub_.publish(*out_img, camera_info_);
+ }
+ catch (cv::Exception &e)
+ {
+ NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
+ }
+ }
+
+ void connectCb(const image_transport::SingleSubscriberPublisher& ssp)
+ {
+ subscriber_count_++;
+ }
+
+ void disconnectCb(const image_transport::SingleSubscriberPublisher&)
+ {
+ subscriber_count_--;
+ }
+
+public:
+ virtual void onInit()
+ {
+ subscriber_count_ = 0;
+ nh_ = getPrivateNodeHandle();
+ it_ = boost::shared_ptr<image_transport::ImageTransport>(new image_transport::ImageTransport(nh_));
+ pub_ = image_transport::ImageTransport(nh_).advertiseCamera("image_raw", 1);
+
+ nh_.param("filename", filename_, std::string(""));
+ NODELET_INFO("File name for publishing image is : %s", filename_.c_str());
+ try {
+ image_ = cv::imread(filename_, CV_LOAD_IMAGE_COLOR);
+ if ( image_.empty() ) { // if filename is motion file or device file
+ try { // if filename is number
+ int num = boost::lexical_cast<int>(filename_);//num is 1234798797
+ cap_.open(num);
+ } catch(boost::bad_lexical_cast &) { // if file name is string
+ cap_.open(filename_);
+ }
+ CV_Assert(cap_.isOpened());
+ cap_.read(image_);
+ cap_.set(CV_CAP_PROP_POS_FRAMES, 0);
+ }
+ CV_Assert(!image_.empty());
+ }
+ catch (cv::Exception &e)
+ {
+ NODELET_ERROR("Failed to load image (%s): %s %s %s %i", filename_.c_str(), e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
+ }
+
+ bool flip_horizontal;
+ nh_.param("flip_horizontal", flip_horizontal, false);
+ NODELET_INFO("Flip horizontal image is : %s", ((flip_horizontal)?"true":"false"));
+
+ bool flip_vertical;
+ nh_.param("flip_vertical", flip_vertical, false);
+ NODELET_INFO("Flip flip_vertical image is : %s", ((flip_vertical)?"true":"false"));
+
+ // From http://docs.opencv.org/modules/core/doc/operations_on_arrays.html#void flip(InputArray src, OutputArray dst, int flipCode)
+ // FLIP_HORIZONTAL == 1, FLIP_VERTICAL == 0 or FLIP_BOTH == -1
+ flip_image_ = true;
+ if (flip_horizontal && flip_vertical)
+ flip_value_ = 0; // flip both, horizontal and vertical
+ else if (flip_horizontal)
+ flip_value_ = 1;
+ else if (flip_vertical)
+ flip_value_ = -1;
+ else
+ flip_image_ = false;
+
+ camera_info_.width = image_.cols;
+ camera_info_.height = image_.rows;
+ camera_info_.distortion_model = "plumb_bob";
+ camera_info_.D = list_of(0)(0)(0)(0)(0).convert_to_container<std::vector<double> >();
+ camera_info_.K = list_of(1)(0)(camera_info_.width/2)(0)(1)(camera_info_.height/2)(0)(0)(1);
+ camera_info_.R = list_of(1)(0)(0)(0)(1)(0)(0)(0)(1);
+ camera_info_.P = list_of(1)(0)(camera_info_.width/2)(0)(0)(1)(camera_info_.height/2)(0)(0)(0)(1)(0);
+
+ timer_ = nh_.createTimer(ros::Duration(1), &ImagePublisherNodelet::do_work, this);
+
+ dynamic_reconfigure::Server<image_publisher::ImagePublisherConfig>::CallbackType f =
+ boost::bind(&ImagePublisherNodelet::reconfigureCallback, this, _1, _2);
+ srv.setCallback(f);
+ }
+};
+}
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(image_publisher::ImagePublisherNodelet, nodelet::Nodelet);
diff --git a/image_rotate/CHANGELOG.rst b/image_rotate/CHANGELOG.rst
new file mode 100644
index 0000000..469439c
--- /dev/null
+++ b/image_rotate/CHANGELOG.rst
@@ -0,0 +1,82 @@
+1.12.20 (2017-04-30)
+--------------------
+* Fix CMake warnings about Eigen.
+* address gcc6 build error
+ With gcc6, compiling fails with `stdlib.h: No such file or directory`,
+ as including '-isystem /usr/include' breaks with gcc6, cf.,
+ https://gcc.gnu.org/bugzilla/show_bug.cgi?id=70129.
+ This commit addresses this issue for this package in the same way
+ it was addressed in various other ROS packages. A list of related
+ commits and pull requests is at:
+ https://github.com/ros/rosdistro/issues/12783
+ Signed-off-by: Lukas Bulwahn <lukas.bulwahn at oss.bmw-carit.de>
+* Contributors: Lukas Bulwahn, Vincent Rabaud
+
+1.12.19 (2016-07-24)
+--------------------
+* Fix frames if it is empty to rotate image
+* Contributors: Kentaro Wada
+
+1.12.18 (2016-07-12)
+--------------------
+
+1.12.17 (2016-07-11)
+--------------------
+
+1.12.16 (2016-03-19)
+--------------------
+* clean OpenCV dependency in package.xml
+* Contributors: Vincent Rabaud
+
+1.12.15 (2016-01-17)
+--------------------
+
+1.12.14 (2015-07-22)
+--------------------
+
+1.12.13 (2015-04-06)
+--------------------
+
+1.12.12 (2014-12-31)
+--------------------
+
+1.12.11 (2014-10-26)
+--------------------
+
+1.12.10 (2014-09-28)
+--------------------
+
+1.12.9 (2014-09-21)
+-------------------
+
+1.12.8 (2014-08-19)
+-------------------
+
+1.12.6 (2014-07-27)
+-------------------
+
+1.12.4 (2014-04-28)
+-------------------
+
+1.12.3 (2014-04-12)
+-------------------
+
+1.12.2 (2014-04-08)
+-------------------
+* use NODELET_** macros instead of ROS_** macros
+* use getNodeHandle rather than getPrivateNodeHandle
+* add executable to load image_rotate/image_rotate nodelet.
+ add xml file to export nodelet definition.
+ Conflicts:
+ image_rotate/package.xml
+* make image_rotate nodelet class
+ Conflicts:
+ image_rotate/CMakeLists.txt
+ image_rotate/package.xml
+ image_rotate/src/nodelet/image_rotate_nodelet.cpp
+* move image_rotate.cpp to nodelet directory according to the directory convenstion of image_pipeline
+* Contributors: Ryohei Ueda
+
+1.12.1 (2014-04-06)
+-------------------
+* replace tf usage by tf2 usage
diff --git a/image_rotate/CMakeLists.txt b/image_rotate/CMakeLists.txt
new file mode 100644
index 0000000..147b1db
--- /dev/null
+++ b/image_rotate/CMakeLists.txt
@@ -0,0 +1,32 @@
+cmake_minimum_required(VERSION 2.8)
+project(image_rotate)
+
+find_package(catkin REQUIRED COMPONENTS cmake_modules cv_bridge dynamic_reconfigure image_transport nodelet roscpp tf2 tf2_geometry_msgs)
+
+# generate the dynamic_reconfigure config file
+generate_dynamic_reconfigure_options(cfg/ImageRotate.cfg)
+
+catkin_package()
+
+find_package(OpenCV REQUIRED core imgproc)
+
+# add the executable
+include_directories(${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS})
+
+add_library(${PROJECT_NAME} SHARED src/nodelet/image_rotate_nodelet.cpp)
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
+add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg)
+install(TARGETS image_rotate
+ DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+)
+
+add_executable(image_rotate_exe src/node/image_rotate.cpp)
+SET_TARGET_PROPERTIES(image_rotate_exe PROPERTIES OUTPUT_NAME image_rotate)
+target_link_libraries(image_rotate_exe ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
+
+install(TARGETS image_rotate_exe
+ DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+install(FILES nodelet_plugins.xml
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
diff --git a/image_rotate/cfg/ImageRotate.cfg b/image_rotate/cfg/ImageRotate.cfg
new file mode 100755
index 0000000..892b231
--- /dev/null
+++ b/image_rotate/cfg/ImageRotate.cfg
@@ -0,0 +1,59 @@
+#! /usr/bin/env python
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2010, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# * Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above
+# copyright notice, this list of conditions and the following
+# disclaimer in the documentation and/or other materials provided
+# with the distribution.
+# * Neither the name of Willow Garage, Inc. nor the names of its
+# contributors may be used to endorse or promote products derived
+# from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+
+PACKAGE='image_rotate'
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+gen.add("target_frame_id", str_t, 0, "Frame in which the target vector is specified. Empty means the input frame.", "base_link")
+gen.add("target_x", double_t, 0, "X coordinate of the target vector", 0, -10, 10)
+gen.add("target_y", double_t, 0, "Y coordinate of the target vector", 0, -10, 10)
+gen.add("target_z", double_t, 0, "Z coordinate of the target vector", 1, -10, 10)
+
+gen.add("source_frame_id", str_t, 0, "Frame in which the source vector is specified. Empty means the input frame.", "")
+gen.add("source_x", double_t, 0, "X coordinate of the direction the target should be aligned with.", 0, -10, 10)
+gen.add("source_y", double_t, 0, "Y coordinate of the direction the target should be aligned with.", -1, -10, 10)
+gen.add("source_z", double_t, 0, "Z coordinate of the direction the target should be aligned with.", 0, -10, 10)
+
+gen.add("output_frame_id", str_t, 0, "Frame to publish for the image's new orientation. Empty means add '_rotated' suffix to the image frame.", "")
+gen.add("input_frame_id", str_t, 0, "Frame to use for the original camera image. Empty means that the frame in the image or camera_info should be used depending on use_camera_info.", "")
+
+gen.add("use_camera_info", bool_t, 0, "Indicates that the camera_info topic should be subscribed to to get the default input_frame_id. Otherwise the frame from the image message will be used.", True)
+
+gen.add("max_angular_rate", double_t, 0, "Limits the rate at which the image can rotate (rad/s). Zero means no limit.", 10, 0, 100)
+
+gen.add("output_image_size", double_t, 0, "Size of the output image as a function of the input image size. Can be varied continuously between the following special settings: 0 ensures no black ever appears, 1 is small image dimension, 2 is large image dimension, 3 is image diagonal.", 2, 0, 3)
+
+exit(gen.generate(PACKAGE, "image_rotate", "ImageRotate"))
diff --git a/image_rotate/mainpage.dox b/image_rotate/mainpage.dox
new file mode 100644
index 0000000..43b15c9
--- /dev/null
+++ b/image_rotate/mainpage.dox
@@ -0,0 +1,26 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b image_rotate is ...
+
+<!--
+Provide an overview of your package.
+-->
+
+
+\section codeapi Code API
+
+<!--
+Provide links to specific auto-generated API documentation within your
+package that is of particular interest to a reader. Doxygen will
+document pretty much every part of your code, so do your best here to
+point the reader to the actual API.
+
+If your codebase is fairly large or has different sets of APIs, you
+should use the doxygen 'group' tag to keep these APIs together. For
+example, the roscpp documentation has 'libros' group.
+-->
+
+
+*/
diff --git a/image_rotate/nodelet_plugins.xml b/image_rotate/nodelet_plugins.xml
new file mode 100644
index 0000000..862f136
--- /dev/null
+++ b/image_rotate/nodelet_plugins.xml
@@ -0,0 +1,7 @@
+<library path="lib/libimage_rotate">
+
+ <class name="image_rotate/image_rotate" type="image_rotate::ImageRotateNodelet" base_class_type="nodelet::Nodelet">
+ <description>Nodelet to rotate sensor_msgs/Image</description>
+ </class>
+
+</library>
diff --git a/image_rotate/package.xml b/image_rotate/package.xml
new file mode 100644
index 0000000..ebaaed8
--- /dev/null
+++ b/image_rotate/package.xml
@@ -0,0 +1,55 @@
+<package>
+ <name>image_rotate</name>
+ <version>1.12.20</version>
+ <description>
+ <p>
+ Contains a node that rotates an image stream in a way that minimizes
+ the angle between a vector in some arbitrary frame and a vector in the
+ camera frame. The frame of the outgoing image is published by the node.
+ </p>
+ <p>
+ This node is intended to allow camera images to be visualized in an
+ orientation that is more intuitive than the hardware-constrained
+ orientation of the physical camera. This is particularly helpful, for
+ example, to show images from the PR2's forearm cameras with a
+ consistent up direction, despite the fact that the forearms need to
+ rotate in arbitrary ways during manipulation.
+ </p>
+ <p>
+ It is not recommended to use the output from this node for further
+ computation, as it interpolates the source image, introduces black
+ borders, and does not output a camera_info.
+ </p>
+ </description>
+ <author>Blaise Gassend</author>
+ <maintainer email="vincent.rabaud at gmail.com">Vincent Rabaud</maintainer>
+ <license>BSD</license>
+ <url>http://ros.org/wiki/image_rotate</url>
+
+ <buildtool_depend>catkin</buildtool_depend>
+
+ <test_depend>rostest</test_depend>
+
+ <build_depend>cmake_modules</build_depend>
+ <build_depend>cv_bridge</build_depend>
+ <build_depend>dynamic_reconfigure</build_depend>
+ <build_depend>geometry_msgs</build_depend>
+ <build_depend>image_transport</build_depend>
+ <build_depend>nodelet</build_depend>
+ <build_depend>roscpp</build_depend>
+ <build_depend>tf2</build_depend>
+ <build_depend>tf2_geometry_msgs</build_depend>
+ <build_depend>tf2_ros</build_depend>
+
+ <run_depend>cv_bridge</run_depend>
+ <run_depend>dynamic_reconfigure</run_depend>
+ <run_depend>image_transport</run_depend>
+ <run_depend>nodelet</run_depend>
+ <run_depend>roscpp</run_depend>
+ <run_depend>tf2</run_depend>
+ <run_depend>tf2_geometry_msgs</run_depend>
+ <run_depend>tf2_ros</run_depend>
+ <export>
+ <nodelet plugin="${prefix}/nodelet_plugins.xml"/>
+ </export>
+</package>
diff --git a/image_rotate/src/node/image_rotate.cpp b/image_rotate/src/node/image_rotate.cpp
new file mode 100644
index 0000000..d7e573b
--- /dev/null
+++ b/image_rotate/src/node/image_rotate.cpp
@@ -0,0 +1,54 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+#include <ros/ros.h>
+#include <nodelet/loader.h>
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv, "image_rotate", ros::init_options::AnonymousName);
+ if (ros::names::remap("image") == "image") {
+ ROS_WARN("Topic 'image' has not been remapped! Typical command-line usage:\n"
+ "\t$ rosrun image_rotate image_rotate image:=<image topic> [transport]");
+ }
+
+ nodelet::Loader manager(false);
+ nodelet::M_string remappings;
+ nodelet::V_string my_argv(argv + 1, argv + argc);
+ my_argv.push_back("--shutdown-on-close"); // Internal
+
+ manager.load(ros::this_node::getName(), "image_rotate/image_rotate", remappings, my_argv);
+
+ ros::spin();
+ return 0;
+}
diff --git a/image_rotate/src/nodelet/image_rotate_nodelet.cpp b/image_rotate/src/nodelet/image_rotate_nodelet.cpp
new file mode 100644
index 0000000..6cfb89a
--- /dev/null
+++ b/image_rotate/src/nodelet/image_rotate_nodelet.cpp
@@ -0,0 +1,281 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2014, JSK Lab.
+* 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/********************************************************************
+* image_rotate_nodelet.cpp
+* this is a forked version of image_rotate.
+* this image_rotate_nodelet supports:
+* 1) nodelet
+* 2) tf and tf2
+*********************************************************************/
+
+#include <ros/ros.h>
+#include <nodelet/nodelet.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <image_rotate/ImageRotateConfig.h>
+#include <geometry_msgs/Vector3Stamped.h>
+#include <image_transport/image_transport.h>
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <dynamic_reconfigure/server.h>
+#include <math.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+
+namespace image_rotate {
+class ImageRotateNodelet : public nodelet::Nodelet
+{
+ tf2_ros::Buffer tf_buffer_;
+ boost::shared_ptr<tf2_ros::TransformListener> tf_sub_;
+ tf2_ros::TransformBroadcaster tf_pub_;
+
+ image_rotate::ImageRotateConfig config_;
+ dynamic_reconfigure::Server<image_rotate::ImageRotateConfig> srv;
+
+ image_transport::Publisher img_pub_;
+ image_transport::Subscriber img_sub_;
+ image_transport::CameraSubscriber cam_sub_;
+
+ geometry_msgs::Vector3Stamped target_vector_;
+ geometry_msgs::Vector3Stamped source_vector_;
+
+ boost::shared_ptr<image_transport::ImageTransport> it_;
+ ros::NodeHandle nh_;
+
+ int subscriber_count_;
+ double angle_;
+ ros::Time prev_stamp_;
+
+ void reconfigureCallback(image_rotate::ImageRotateConfig &new_config, uint32_t level)
+ {
+ config_ = new_config;
+ target_vector_.vector.x = config_.target_x;
+ target_vector_.vector.y = config_.target_y;
+ target_vector_.vector.z = config_.target_z;
+
+ source_vector_.vector.x = config_.source_x;
+ source_vector_.vector.y = config_.source_y;
+ source_vector_.vector.z = config_.source_z;
+ if (subscriber_count_)
+ { // @todo Could do this without an interruption at some point.
+ unsubscribe();
+ subscribe();
+ }
+ }
+
+ const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame)
+ {
+ if (frame.empty())
+ return image_frame;
+ return frame;
+ }
+
+ void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info)
+ {
+ do_work(msg, cam_info->header.frame_id);
+ }
+
+ void imageCallback(const sensor_msgs::ImageConstPtr& msg)
+ {
+ do_work(msg, msg->header.frame_id);
+ }
+
+ void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg)
+ {
+ try
+ {
+ std::string input_frame_id = frameWithDefault(config_.input_frame_id, input_frame_from_msg);
+ std::string target_frame_id = frameWithDefault(config_.target_frame_id, input_frame_from_msg);
+ std::string source_frame_id = frameWithDefault(config_.source_frame_id, input_frame_from_msg);
+
+ // Transform the target vector into the image frame.
+ target_vector_.header.stamp = msg->header.stamp;
+ target_vector_.header.frame_id = target_frame_id;
+ geometry_msgs::Vector3Stamped target_vector_transformed;
+ geometry_msgs::TransformStamped transform = tf_buffer_.lookupTransform(target_frame_id, input_frame_id, msg->header.stamp);
+ tf2::doTransform(target_vector_, target_vector_transformed, transform);
+
+ // Transform the source vector into the image frame.
+ source_vector_.header.stamp = msg->header.stamp;
+ source_vector_.header.frame_id = source_frame_id;
+ geometry_msgs::Vector3Stamped source_vector_transformed;
+ transform = tf_buffer_.lookupTransform(source_frame_id, input_frame_id, msg->header.stamp);
+ tf2::doTransform(source_vector_, source_vector_transformed, transform);
+
+ //NODELET_INFO("target: %f %f %f", target_vector_.x(), target_vector_.y(), target_vector_.z());
+ //NODELET_INFO("target_transformed: %f %f %f", target_vector_transformed.x(), target_vector_transformed.y(), target_vector_transformed.z());
+ //NODELET_INFO("source: %f %f %f", source_vector_.x(), source_vector_.y(), source_vector_.z());
+ //NODELET_INFO("source_transformed: %f %f %f", source_vector_transformed.x(), source_vector_transformed.y(), source_vector_transformed.z());
+
+ // Calculate the angle of the rotation.
+ double angle = angle_;
+ if ((target_vector_transformed.vector.x != 0 || target_vector_transformed.vector.y != 0) &&
+ (source_vector_transformed.vector.x != 0 || source_vector_transformed.vector.y != 0))
+ {
+ angle = atan2(target_vector_transformed.vector.y, target_vector_transformed.vector.x);
+ angle -= atan2(source_vector_transformed.vector.y, source_vector_transformed.vector.x);
+ }
+
+ // Rate limit the rotation.
+ if (config_.max_angular_rate == 0)
+ angle_ = angle;
+ else
+ {
+ double delta = fmod(angle - angle_, 2.0 * M_PI);
+ if (delta > M_PI)
+ delta -= 2.0 * M_PI;
+ else if (delta < - M_PI)
+ delta += 2.0 * M_PI;
+
+ double max_delta = config_.max_angular_rate * (msg->header.stamp - prev_stamp_).toSec();
+ if (delta > max_delta)
+ delta = max_delta;
+ else if (delta < -max_delta)
+ delta = - max_delta;
+
+ angle_ += delta;
+ }
+ angle_ = fmod(angle_, 2.0 * M_PI);
+ }
+ catch (tf2::TransformException &e)
+ {
+ NODELET_ERROR("Transform error: %s", e.what());
+ }
+
+ //NODELET_INFO("angle: %f", 180 * angle_ / M_PI);
+
+ // Publish the transform.
+ geometry_msgs::TransformStamped transform;
+ transform.transform.translation.x = 0;
+ transform.transform.translation.y = 0;
+ transform.transform.translation.z = 0;
+ tf2::convert(tf2::Quaternion(tf2::Vector3(0.0, 0.0, 1.0), angle_), transform.transform.rotation);
+ transform.header.frame_id = msg->header.frame_id;
+ transform.child_frame_id = frameWithDefault(config_.output_frame_id, msg->header.frame_id + "_rotated");
+ transform.header.stamp = msg->header.stamp;
+ tf_pub_.sendTransform(transform);
+
+ // Transform the image.
+ try
+ {
+ // Convert the image into something opencv can handle.
+ cv::Mat in_image = cv_bridge::toCvShare(msg, msg->encoding)->image;
+
+ // Compute the output image size.
+ int max_dim = in_image.cols > in_image.rows ? in_image.cols : in_image.rows;
+ int min_dim = in_image.cols < in_image.rows ? in_image.cols : in_image.rows;
+ int noblack_dim = min_dim / sqrt(2);
+ int diag_dim = sqrt(in_image.cols*in_image.cols + in_image.rows*in_image.rows);
+ int out_size;
+ int candidates[] = { noblack_dim, min_dim, max_dim, diag_dim, diag_dim }; // diag_dim repeated to simplify limit case.
+ int step = config_.output_image_size;
+ out_size = candidates[step] + (candidates[step + 1] - candidates[step]) * (config_.output_image_size - step);
+ //NODELET_INFO("out_size: %d", out_size);
+
+ // Compute the rotation matrix.
+ cv::Mat rot_matrix = cv::getRotationMatrix2D(cv::Point2f(in_image.cols / 2.0, in_image.rows / 2.0), 180 * angle_ / M_PI, 1);
+ cv::Mat translation = rot_matrix.col(2);
+ rot_matrix.at<double>(0, 2) += (out_size - in_image.cols) / 2.0;
+ rot_matrix.at<double>(1, 2) += (out_size - in_image.rows) / 2.0;
+
+ // Do the rotation
+ cv::Mat out_image;
+ cv::warpAffine(in_image, out_image, rot_matrix, cv::Size(out_size, out_size));
+
+ // Publish the image.
+ sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, msg->encoding, out_image).toImageMsg();
+ out_img->header.frame_id = transform.child_frame_id;
+ img_pub_.publish(out_img);
+ }
+ catch (cv::Exception &e)
+ {
+ NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
+ }
+
+ prev_stamp_ = msg->header.stamp;
+ }
+
+ void subscribe()
+ {
+ NODELET_DEBUG("Subscribing to image topic.");
+ if (config_.use_camera_info && config_.input_frame_id.empty())
+ cam_sub_ = it_->subscribeCamera("image", 3, &ImageRotateNodelet::imageCallbackWithInfo, this);
+ else
+ img_sub_ = it_->subscribe("image", 3, &ImageRotateNodelet::imageCallback, this);
+ }
+
+ void unsubscribe()
+ {
+ NODELET_DEBUG("Unsubscribing from image topic.");
+ img_sub_.shutdown();
+ cam_sub_.shutdown();
+ }
+
+ void connectCb(const image_transport::SingleSubscriberPublisher& ssp)
+ {
+ if (subscriber_count_++ == 0) {
+ subscribe();
+ }
+ }
+
+ void disconnectCb(const image_transport::SingleSubscriberPublisher&)
+ {
+ subscriber_count_--;
+ if (subscriber_count_ == 0) {
+ unsubscribe();
+ }
+ }
+
+public:
+ virtual void onInit()
+ {
+ nh_ = getNodeHandle();
+ it_ = boost::shared_ptr<image_transport::ImageTransport>(new image_transport::ImageTransport(nh_));
+ subscriber_count_ = 0;
+ angle_ = 0;
+ prev_stamp_ = ros::Time(0, 0);
+ tf_sub_.reset(new tf2_ros::TransformListener(tf_buffer_));
+ image_transport::SubscriberStatusCallback connect_cb = boost::bind(&ImageRotateNodelet::connectCb, this, _1);
+ image_transport::SubscriberStatusCallback disconnect_cb = boost::bind(&ImageRotateNodelet::disconnectCb, this, _1);
+ img_pub_ = image_transport::ImageTransport(ros::NodeHandle(nh_, "rotated")).advertise("image", 1, connect_cb, disconnect_cb);
+
+ dynamic_reconfigure::Server<image_rotate::ImageRotateConfig>::CallbackType f =
+ boost::bind(&ImageRotateNodelet::reconfigureCallback, this, _1, _2);
+ srv.setCallback(f);
+ }
+};
+}
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(image_rotate::ImageRotateNodelet, nodelet::Nodelet);
diff --git a/image_view/CHANGELOG.rst b/image_view/CHANGELOG.rst
new file mode 100644
index 0000000..92751bf
--- /dev/null
+++ b/image_view/CHANGELOG.rst
@@ -0,0 +1,177 @@
+1.12.20 (2017-04-30)
+--------------------
+* DisparityViewNodelet: fixed freeze (`#244 <https://github.com/ros-perception/image_pipeline/issues/244>`_)
+* launch image view with a predefined window size (`#257 <https://github.com/ros-perception/image_pipeline/issues/257>`_)
+* Remove python-opencv run_depend for image_view (`#270 <https://github.com/ros-perception/image_pipeline/issues/270>`_)
+ The `python-opencv` dependency pulls in the system OpenCV v2.4 which is
+ not required since the `image_view` package depends on `cv_bridge` which
+ pulls in `opencv3` and `opencv3` provides the python library that
+ `image_view` can use.
+* Fix encoding error message (`#253 <https://github.com/ros-perception/image_pipeline/issues/253>`_)
+ * Fix encoding error message
+ * Update image_saver.cpp
+ Allow compilation on older compilers
+* Including stereo_msgs dep fixes `#248 <https://github.com/ros-perception/image_pipeline/issues/248>`_ (`#249 <https://github.com/ros-perception/image_pipeline/issues/249>`_)
+* Add no gui mode to just visualize & publish with image_view (`#241 <https://github.com/ros-perception/image_pipeline/issues/241>`_)
+* stere_view: fixed empty left, right, disparity windows with opencv3
+* Apply value scaling to depth/float image with min/max image value
+ If min/max image value is specified we just use it, and if not,
+ - 32FC1: we assume depth image with meter metric, and 10[m] as the max range.
+ - 16UC1: we assume depth image with milimeter metric, and 10 * 1000[mm] as the max range.
+* Depends on cv_bridge 1.11.13 for CvtColorForDisplayOptions
+ Close `#238 <https://github.com/ros-perception/image_pipeline/issues/238>`_
+* fix doc jobs
+ This is a proper fix for `#233 <https://github.com/ros-perception/image_pipeline/issues/233>`_
+* address gcc6 build error
+ With gcc6, compiling fails with `stdlib.h: No such file or directory`,
+ as including '-isystem /usr/include' breaks with gcc6, cf.,
+ https://gcc.gnu.org/bugzilla/show_bug.cgi?id=70129.
+ This commit addresses this issue for this package in the same way
+ it was addressed in various other ROS packages. A list of related
+ commits and pull requests is at:
+ https://github.com/ros/rosdistro/issues/12783
+ Signed-off-by: Lukas Bulwahn <lukas.bulwahn at oss.bmw-carit.de>
+* Contributors: Christopher Wecht, Kartik Mohta, Kei Okada, Kentaro Wada, Lukas Bulwahn, Léonard Gérard, Vincent Rabaud, cwecht, mryellow
+
+1.12.19 (2016-07-24)
+--------------------
+* Add colormap option in video_recorder
+* Merge pull request `#203 <https://github.com/ros-perception/image_pipeline/issues/203>`_ from wkentaro/video-recorder-timestamp
+ [image_view] Stamped video output filename for video recorder
+* bump version requirement for cv_bridge dep
+ Closes `#215 <https://github.com/ros-perception/image_pipeline/issues/215>`_
+* Request for saving image with start/end two triggers
+* Stamped video output filename
+ - _filename:=output.avi _stamped_filename:=false -> output.avi
+ - _filename:=_out.avi _stamped_filename:=true -> 1466299931.584632829_out.avi
+ - _filename:=$HOME/.ros/.avi _stamped_filename:=true -> /home/ubuntu/.ros/1466299931.584632829.avi
+* Revert max_depth_range to default value for cvtColorForDisplay
+* Contributors: Kentaro Wada, Vincent Rabaud
+
+1.12.18 (2016-07-12)
+--------------------
+* Use image_transport::Subscriber aside from ros::Subscriber
+* Refactor: Remove subscription of camera_info in video_recorder
+* Add colormap options for displaying image topic
+* Use CvtColorForDisplayOptions for cvtColorForDisplay
+* Contributors: Kentaro Wada, Vincent Rabaud
+
+1.12.17 (2016-07-11)
+--------------------
+* Fix timestamp to get correct fps in video_recorder
+* Get correct fps in video_recorder.cpp
+* Do dynamic scaling for float images
+* Contributors: Kentaro Wada
+
+1.12.16 (2016-03-19)
+--------------------
+* Remove code for roslib on .cfg files
+ Closes `#185 <https://github.com/ros-perception/image_pipeline/issues/185>`_
+* add cv::waitKey for opencv3 installed from source to fix freezing issue
+* when no image is saved, do not save camera info
+ When the images are not recorded because "save_all_image" is false and "save_image_service" is false, the frame count should not be incremented and the camera info should not be written to disk.
+* Add std_srvs to catkin find_package()
+* Contributors: Jeremy Kerfs, Jochen Sprickerhof, Kentaro Wada, Krishneel
+
+1.12.15 (2016-01-17)
+--------------------
+* simplify the OpenCV dependency
+* [image_view] Configure do_dynamic_scaling param with dynamic_reconfigure
+* [image_view] Scale 16UC1 depth image
+* fix compilation
+* Extract images which are synchronized with message_filters
+* [image_view] Show full path when failed to save image
+* [image_view] Enable to specify transport with arg
+* [image_view] feedback: no need threading for callback
+* [image_view/image_view] Make as a node
+* Added sensor_msgs::Image conversion to cv::Mat from rqt_image_view in
+ order to be able to create videos from kinect depth images (cv_bridge
+ currently doesn't support 16UC1 image encoding).
+ Code adapted from:
+ https://github.com/ros-visualization/rqt_common_plugins/blob/groovy-devel/rqt_image_view/src/rqt_image_view/image_view.cpp
+* simplify OpenCV3 conversion
+* use the color conversion for display from cv_bridge
+* Contributors: Carlos Costa, Kentaro Wada, Vincent Rabaud
+
+1.12.14 (2015-07-22)
+--------------------
+* reduce the differences between OpenCV2 and 3
+* do not build GUIs on Android
+ This fixes `#137 <https://github.com/ros-perception/image_pipeline/issues/137>`_
+* Contributors: Vincent Rabaud
+
+1.12.13 (2015-04-06)
+--------------------
+
+1.12.12 (2014-12-31)
+--------------------
+* Convert function to inline to avoid duplicates with image_transport
+* Revert "remove GTK dependency"
+ This reverts commit a6e15e796a40385fbbf8da05966aa47d179dcb46.
+ Conflicts:
+ image_view/CMakeLists.txt
+ image_view/src/nodelets/disparity_nodelet.cpp
+ image_view/src/nodes/stereo_view.cpp
+* Revert "make sure waitKey is called after imshow"
+ This reverts commit d13e3ed6af819459bca221ece779964a74beefac.
+* Revert "brings back window_thread"
+ This reverts commit 41a655e8e99910c13a3e7f1ebfdd083207cef76f.
+* Contributors: Gary Servin, Vincent Rabaud
+
+1.12.11 (2014-10-26)
+--------------------
+* brings back window_thread
+ This fixes `#102 <https://github.com/ros-perception/image_pipeline/issues/102>`_ fully
+* small optimizations
+* add the image_transport parameter
+* Contributors: Vincent Rabaud
+
+1.12.10 (2014-09-28)
+--------------------
+
+1.12.9 (2014-09-21)
+-------------------
+* get code to compile with OpenCV3
+ fixes `#96 <https://github.com/ros-perception/image_pipeline/issues/96>`_
+* Contributors: Vincent Rabaud
+
+1.12.8 (2014-08-19)
+-------------------
+
+1.12.6 (2014-07-27)
+-------------------
+* make sure waitKey is called after imshow
+* remove GTK dependency
+* small speedups
+* Contributors: Vincent Rabaud
+
+1.12.5 (2014-05-11)
+-------------------
+* image_view: Add depend on gtk2
+* Contributors: Scott K Logan
+
+1.12.4 (2014-04-28)
+-------------------
+* fixes `#65 <https://github.com/ros-perception/image_pipeline/issues/65>`_
+* Contributors: Vincent Rabaud
+
+1.12.3 (2014-04-12)
+-------------------
+
+1.12.2 (2014-04-08)
+-------------------
+
+1.12.1 (2014-04-06)
+-------------------
+* get proper opencv dependency
+* Contributors: Vincent Rabaud
+
+1.11.7 (2014-03-28)
+-------------------
+* Added requirement for core.
+* Contributors: Jonathan J Hunt
+
+1.11.3 (2013-10-06 20:21:55 +0100)
+----------------------------------
+- #41: allow image_saver to save image topics
+- #40: use proper download URL
diff --git a/image_view/CMakeLists.txt b/image_view/CMakeLists.txt
new file mode 100644
index 0000000..0b98148
--- /dev/null
+++ b/image_view/CMakeLists.txt
@@ -0,0 +1,90 @@
+cmake_minimum_required(VERSION 2.8)
+project(image_view)
+
+find_package(catkin REQUIRED COMPONENTS camera_calibration_parsers cv_bridge dynamic_reconfigure image_transport message_filters message_generation nodelet rosconsole roscpp std_srvs stereo_msgs)
+generate_dynamic_reconfigure_options(cfg/ImageView.cfg)
+
+catkin_package(CATKIN_DEPENDS dynamic_reconfigure)
+find_package(Boost REQUIRED COMPONENTS signals thread)
+find_package(OpenCV REQUIRED)
+
+include_directories(${Boost_INCLUDE_DIRS}
+ ${catkin_INCLUDE_DIRS}
+ ${OpenCV_INCLUDE_DIRS}
+)
+
+# Extra tools
+add_executable(extract_images src/nodes/extract_images.cpp)
+target_link_libraries(extract_images ${catkin_LIBRARIES}
+ ${OpenCV_LIBRARIES}
+)
+
+add_executable(image_saver src/nodes/image_saver.cpp)
+target_link_libraries(image_saver ${catkin_LIBRARIES}
+ ${OpenCV_LIBRARIES}
+)
+
+add_executable(video_recorder src/nodes/video_recorder.cpp)
+target_link_libraries(video_recorder ${catkin_LIBRARIES}
+ ${OpenCV_LIBRARIES}
+)
+
+install(TARGETS extract_images image_saver video_recorder
+ DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+# Deal with the GUI's
+if(ANDROID)
+ return()
+endif()
+
+find_package(GTK2)
+add_definitions(-DHAVE_GTK)
+include_directories(${GTK2_INCLUDE_DIRS})
+
+# Nodelet library
+add_library(image_view src/nodelets/image_nodelet.cpp src/nodelets/disparity_nodelet.cpp src/nodelets/window_thread.cpp)
+target_link_libraries(image_view ${catkin_LIBRARIES}
+ ${GTK_LIBRARIES}
+ ${GTK2_LIBRARIES}
+ ${OpenCV_LIBRARIES}
+ ${Boost_LIBRARIES}
+)
+install(TARGETS image_view
+ DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+)
+
+# Image viewers
+add_executable(image_view_exe src/nodes/image_view.cpp)
+add_dependencies(image_view_exe ${PROJECT_NAME}_gencfg)
+SET_TARGET_PROPERTIES(image_view_exe PROPERTIES OUTPUT_NAME image_view)
+target_link_libraries(image_view_exe ${catkin_LIBRARIES}
+ ${OpenCV_LIBRARIES}
+ ${Boost_LIBRARIES}
+)
+
+add_executable(disparity_view src/nodes/disparity_view.cpp)
+target_link_libraries(disparity_view ${catkin_LIBRARIES}
+ ${OpenCV_LIBRARIES}
+)
+
+add_executable(stereo_view src/nodes/stereo_view.cpp)
+target_link_libraries(stereo_view ${Boost_LIBRARIES}
+ ${catkin_LIBRARIES}
+ ${GTK_LIBRARIES}
+ ${GTK2_LIBRARIES}
+ ${OpenCV_LIBRARIES}
+)
+
+install(TARGETS disparity_view image_view_exe stereo_view
+ DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+install(FILES nodelet_plugins.xml
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+# Python programs
+catkin_install_python(
+ PROGRAMS scripts/extract_images_sync
+ DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
diff --git a/image_view/cfg/ImageView.cfg b/image_view/cfg/ImageView.cfg
new file mode 100755
index 0000000..8cc0888
--- /dev/null
+++ b/image_view/cfg/ImageView.cfg
@@ -0,0 +1,29 @@
+#! /usr/bin/env python
+
+PACKAGE='image_view'
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+edit_method_colormap = gen.enum([
+ gen.const("NO_COLORMAP", int_t, -1, "NO_COLORMAP"),
+ gen.const("AUTUMN", int_t, 0, "COLORMAP_AUTUMN"),
+ gen.const("BONE", int_t, 1, "COLORMAP_BONE"),
+ gen.const("JET", int_t, 2, "COLORMAP_JET"),
+ gen.const("WINTER", int_t, 3, "COLORMAP_WINTER"),
+ gen.const("RAINBOW", int_t, 4, "COLORMAP_RAINBOW"),
+ gen.const("OCEAN", int_t, 5, "COLORMAP_OCEAN"),
+ gen.const("SUMMER", int_t, 6, "COLORMAP_SUMMER"),
+ gen.const("SPRING", int_t, 7, "COLORMAP_SPRING"),
+ gen.const("COOL", int_t, 8, "COLORMAP_COOL"),
+ gen.const("HSV", int_t, 9, "COLORMAP_HSV"),
+ gen.const("PINK", int_t, 10, "COLORMAP_PINK"),
+ gen.const("HOT", int_t, 11, "COLORMAP_HOT"),
+], "colormap")
+
+gen.add('do_dynamic_scaling', bool_t, 0, 'Do dynamic scaling about pixel values or not', False)
+gen.add('colormap', int_t, 0, "colormap", -1, -1, 11, edit_method=edit_method_colormap);
+gen.add('min_image_value', double_t, 0, "Minimum image value for scaling depth/float image.", default=0, min=0);
+gen.add('max_image_value', double_t, 0, "Maximum image value for scaling depth/float image.", default=0, min=0);
+
+exit(gen.generate(PACKAGE, 'image_view', 'ImageView'))
diff --git a/image_view/mainpage.dox b/image_view/mainpage.dox
new file mode 100644
index 0000000..87cfb37
--- /dev/null
+++ b/image_view/mainpage.dox
@@ -0,0 +1,11 @@
+/**
+ at mainpage image_view
+
+ at htmlinclude manifest.html
+
+ at b image_view is a simple utility for viewing an image topic. For usage see
+http://www.ros.org/wiki/image_view.
+
+Currently this package has no public code API.
+
+*/
diff --git a/image_view/nodelet_plugins.xml b/image_view/nodelet_plugins.xml
new file mode 100644
index 0000000..b6bd0fc
--- /dev/null
+++ b/image_view/nodelet_plugins.xml
@@ -0,0 +1,11 @@
+<library path="lib/libimage_view">
+
+ <class name="image_view/image" type="image_view::ImageNodelet" base_class_type="nodelet::Nodelet">
+ <description>Nodelet to view a sensor_msgs/Image topic</description>
+ </class>
+
+ <class name="image_view/disparity" type="image_view::DisparityNodelet" base_class_type="nodelet::Nodelet">
+ <description>Nodelet to view a stereo_msgs/DisparityImage topic</description>
+ </class>
+
+</library>
diff --git a/image_view/package.xml b/image_view/package.xml
new file mode 100644
index 0000000..0dda359
--- /dev/null
+++ b/image_view/package.xml
@@ -0,0 +1,46 @@
+<package>
+ <name>image_view</name>
+ <version>1.12.20</version>
+ <description>
+ A simple viewer for ROS image topics. Includes a specialized viewer
+ for stereo + disparity images.
+ </description>
+ <author>Patrick Mihelich</author>
+ <maintainer email="vincent.rabaud at gmail.com">Vincent Rabaud</maintainer>
+ <license>BSD</license>
+ <url>http://www.ros.org/wiki/image_view</url>
+
+ <export>
+ <rosdoc config="rosdoc.yaml" />
+ <nodelet plugin="${prefix}/nodelet_plugins.xml" />
+ </export>
+
+ <buildtool_depend>catkin</buildtool_depend>
+
+ <test_depend>rostest</test_depend>
+
+ <build_depend>camera_calibration_parsers</build_depend>
+ <build_depend version_gte="1.11.13">cv_bridge</build_depend>
+ <build_depend>dynamic_reconfigure</build_depend>
+ <build_depend>gtk2</build_depend>
+ <build_depend>image_transport</build_depend>
+ <build_depend>message_filters</build_depend>
+ <build_depend>message_generation</build_depend>
+ <build_depend>nodelet</build_depend>
+ <build_depend>rosconsole</build_depend>
+ <build_depend>roscpp</build_depend>
+ <build_depend>sensor_msgs</build_depend>
+ <build_depend>std_srvs</build_depend>
+ <build_depend>stereo_msgs</build_depend>
+
+ <run_depend>camera_calibration_parsers</run_depend>
+ <run_depend version_gte="1.11.13">cv_bridge</run_depend>
+ <run_depend>dynamic_reconfigure</run_depend>
+ <run_depend>gtk2</run_depend>
+ <run_depend>image_transport</run_depend>
+ <run_depend>message_filters</run_depend>
+ <run_depend>nodelet</run_depend>
+ <run_depend>rosconsole</run_depend>
+ <run_depend>roscpp</run_depend>
+ <run_depend>std_srvs</run_depend>
+</package>
diff --git a/image_view/rosdoc.yaml b/image_view/rosdoc.yaml
new file mode 100644
index 0000000..976fdf0
--- /dev/null
+++ b/image_view/rosdoc.yaml
@@ -0,0 +1,4 @@
+ - builder: doxygen
+ name: C++ API
+ output_dir: c++
+ file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox'
diff --git a/image_view/scripts/extract_images_sync b/image_view/scripts/extract_images_sync
new file mode 100755
index 0000000..cd86ddd
--- /dev/null
+++ b/image_view/scripts/extract_images_sync
@@ -0,0 +1,99 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2015, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# * Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above
+# copyright notice, this list of conditions and the following
+# disclaimer in the documentation and/or other materials provided
+# with the distribution.
+# * Neither the name of the Willow Garage nor the names of its
+# contributors may be used to endorse or promote products derived
+# from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+"""Save images of multiple topics with timestamp synchronization.
+
+Usage: rosrun image_view extract_images_sync _inputs:='[<topic_0>, <topic_1>]'
+"""
+
+import sys
+
+import cv2
+
+import cv_bridge
+import message_filters
+import rospy
+from sensor_msgs.msg import Image
+
+
+class ExtractImagesSync(object):
+
+ def __init__(self):
+ self.seq = 0
+ self.fname_fmt = rospy.get_param(
+ '~filename_format', 'frame%04i_%i.jpg')
+ self.do_dynamic_scaling = rospy.get_param(
+ '~do_dynamic_scaling', False)
+ img_topics = rospy.get_param('~inputs', None)
+ if img_topics is None:
+ rospy.logwarn("""\
+extract_images_sync: rosparam '~inputs' has not been specified! \
+Typical command-line usage:
+\t$ rosrun image_view extract_images_sync _inputs:=<image_topic>
+\t$ rosrun image_view extract_images_sync \
+_inputs:='[<image_topic>, <image_topic>]'""")
+ sys.exit(1)
+ if not isinstance(img_topics, list):
+ img_topics = [img_topics]
+ subs = []
+ for t in img_topics:
+ subs.append(message_filters.Subscriber(t, Image))
+ if rospy.get_param('~approximate_sync', False):
+ sync = message_filters.ApproximateTimeSynchronizer(
+ subs, queue_size=100, slop=.1)
+ else:
+ sync = message_filters.TimeSynchronizer(
+ subs, queue_size=100)
+ sync.registerCallback(self.save)
+
+ def save(self, *imgmsgs):
+ seq = self.seq
+ bridge = cv_bridge.CvBridge()
+ for i, imgmsg in enumerate(imgmsgs):
+ img = bridge.imgmsg_to_cv2(imgmsg)
+ channels = img.shape[2] if img.ndim == 3 else 1
+ encoding_in = bridge.dtype_with_channels_to_cvtype2(
+ img.dtype, channels)
+ img = cv_bridge.cvtColorForDisplay(
+ img, encoding_in=encoding_in, encoding_out='',
+ do_dynamic_scaling=self.do_dynamic_scaling)
+ fname = self.fname_fmt % (seq, i)
+ print('Save image as {0}'.format(fname))
+ cv2.imwrite(fname, img)
+ self.seq = seq + 1
+
+
+if __name__ == '__main__':
+ rospy.init_node('extract_images_sync')
+ extractor = ExtractImagesSync()
+ rospy.spin()
diff --git a/image_view/src/nodelets/disparity_nodelet.cpp b/image_view/src/nodelets/disparity_nodelet.cpp
new file mode 100644
index 0000000..e177a87
--- /dev/null
+++ b/image_view/src/nodelets/disparity_nodelet.cpp
@@ -0,0 +1,439 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+#include <ros/ros.h>
+#include <nodelet/nodelet.h>
+#include <sensor_msgs/image_encodings.h>
+#include <stereo_msgs/DisparityImage.h>
+#include <opencv2/highgui/highgui.hpp>
+#include "window_thread.h"
+
+#ifdef HAVE_GTK
+#include <gtk/gtk.h>
+
+// Platform-specific workaround for #3026: image_view doesn't close when
+// closing image window. On platforms using GTK+ we connect this to the
+// window's "destroy" event so that image_view exits.
+static void destroyNode(GtkWidget *widget, gpointer data)
+{
+ exit(0);
+}
+
+static void destroyNodelet(GtkWidget *widget, gpointer data)
+{
+ // We can't actually unload the nodelet from here, but we can at least
+ // unsubscribe from the image topic.
+ reinterpret_cast<ros::Subscriber*>(data)->shutdown();
+}
+#endif
+
+
+namespace image_view {
+
+class DisparityNodelet : public nodelet::Nodelet
+{
+ // colormap for disparities, RGB order
+ static unsigned char colormap[];
+
+ std::string window_name_;
+ ros::Subscriber sub_;
+ cv::Mat_<cv::Vec3b> disparity_color_;
+ bool initialized;
+
+ virtual void onInit();
+
+ void imageCb(const stereo_msgs::DisparityImageConstPtr& msg);
+
+public:
+ ~DisparityNodelet();
+};
+
+DisparityNodelet::~DisparityNodelet()
+{
+ cv::destroyWindow(window_name_);
+}
+
+void DisparityNodelet::onInit()
+{
+ initialized = false;
+ ros::NodeHandle nh = getNodeHandle();
+ ros::NodeHandle local_nh = getPrivateNodeHandle();
+ const std::vector<std::string>& argv = getMyArgv();
+
+ // Internal option, should be used only by image_view nodes
+ bool shutdown_on_close = std::find(argv.begin(), argv.end(),
+ "--shutdown-on-close") != argv.end();
+
+ // Default window name is the resolved topic name
+ std::string topic = nh.resolveName("image");
+ local_nh.param("window_name", window_name_, topic);
+
+ bool autosize;
+ local_nh.param("autosize", autosize, false);
+
+ //cv::namedWindow(window_name_, autosize ? cv::WND_PROP_AUTOSIZE : 0);
+#if CV_MAJOR_VERSION ==2
+#ifdef HAVE_GTK
+ // Register appropriate handler for when user closes the display window
+ GtkWidget *widget = GTK_WIDGET( cvGetWindowHandle(window_name_.c_str()) );
+ if (shutdown_on_close)
+ g_signal_connect(widget, "destroy", G_CALLBACK(destroyNode), NULL);
+ else
+ g_signal_connect(widget, "destroy", G_CALLBACK(destroyNodelet), &sub_);
+#endif
+ // Start the OpenCV window thread so we don't have to waitKey() somewhere
+ startWindowThread();
+#endif
+
+ sub_ = nh.subscribe<stereo_msgs::DisparityImage>(topic, 1, &DisparityNodelet::imageCb, this);
+}
+
+void DisparityNodelet::imageCb(const stereo_msgs::DisparityImageConstPtr& msg)
+{
+ // Check for common errors in input
+ if (msg->min_disparity == 0.0 && msg->max_disparity == 0.0)
+ {
+ NODELET_ERROR_THROTTLE(30, "Disparity image fields min_disparity and "
+ "max_disparity are not set");
+ return;
+ }
+ if (msg->image.encoding != sensor_msgs::image_encodings::TYPE_32FC1)
+ {
+ NODELET_ERROR_THROTTLE(30, "Disparity image must be 32-bit floating point "
+ "(encoding '32FC1'), but has encoding '%s'",
+ msg->image.encoding.c_str());
+ return;
+ }
+
+ if(!initialized) {
+ cv::namedWindow(window_name_, false ? cv::WND_PROP_AUTOSIZE : 0);
+ initialized = true;
+ }
+ // Colormap and display the disparity image
+ float min_disparity = msg->min_disparity;
+ float max_disparity = msg->max_disparity;
+ float multiplier = 255.0f / (max_disparity - min_disparity);
+
+ const cv::Mat_<float> dmat(msg->image.height, msg->image.width,
+ (float*)&msg->image.data[0], msg->image.step);
+ disparity_color_.create(msg->image.height, msg->image.width);
+
+ for (int row = 0; row < disparity_color_.rows; ++row) {
+ const float* d = dmat[row];
+ cv::Vec3b *disparity_color = disparity_color_[row],
+ *disparity_color_end = disparity_color + disparity_color_.cols;
+ for (; disparity_color < disparity_color_end; ++disparity_color, ++d) {
+ int index = (*d - min_disparity) * multiplier + 0.5;
+ index = std::min(255, std::max(0, index));
+ // Fill as BGR
+ (*disparity_color)[2] = colormap[3*index + 0];
+ (*disparity_color)[1] = colormap[3*index + 1];
+ (*disparity_color)[0] = colormap[3*index + 2];
+ }
+ }
+
+ /// @todo For Electric, consider option to draw outline of valid window
+#if 0
+ sensor_msgs::RegionOfInterest valid = msg->valid_window;
+ cv::Point tl(valid.x_offset, valid.y_offset), br(valid.x_offset + valid.width, valid.y_offset + valid.height);
+ cv::rectangle(disparity_color_, tl, br, CV_RGB(255,0,0), 1);
+#endif
+
+ cv::imshow(window_name_, disparity_color_);
+ cv::waitKey(10);
+}
+
+unsigned char DisparityNodelet::colormap[768] =
+ { 150, 150, 150,
+ 107, 0, 12,
+ 106, 0, 18,
+ 105, 0, 24,
+ 103, 0, 30,
+ 102, 0, 36,
+ 101, 0, 42,
+ 99, 0, 48,
+ 98, 0, 54,
+ 97, 0, 60,
+ 96, 0, 66,
+ 94, 0, 72,
+ 93, 0, 78,
+ 92, 0, 84,
+ 91, 0, 90,
+ 89, 0, 96,
+ 88, 0, 102,
+ 87, 0, 108,
+ 85, 0, 114,
+ 84, 0, 120,
+ 83, 0, 126,
+ 82, 0, 131,
+ 80, 0, 137,
+ 79, 0, 143,
+ 78, 0, 149,
+ 77, 0, 155,
+ 75, 0, 161,
+ 74, 0, 167,
+ 73, 0, 173,
+ 71, 0, 179,
+ 70, 0, 185,
+ 69, 0, 191,
+ 68, 0, 197,
+ 66, 0, 203,
+ 65, 0, 209,
+ 64, 0, 215,
+ 62, 0, 221,
+ 61, 0, 227,
+ 60, 0, 233,
+ 59, 0, 239,
+ 57, 0, 245,
+ 56, 0, 251,
+ 55, 0, 255,
+ 54, 0, 255,
+ 52, 0, 255,
+ 51, 0, 255,
+ 50, 0, 255,
+ 48, 0, 255,
+ 47, 0, 255,
+ 46, 0, 255,
+ 45, 0, 255,
+ 43, 0, 255,
+ 42, 0, 255,
+ 41, 0, 255,
+ 40, 0, 255,
+ 38, 0, 255,
+ 37, 0, 255,
+ 36, 0, 255,
+ 34, 0, 255,
+ 33, 0, 255,
+ 32, 0, 255,
+ 31, 0, 255,
+ 29, 0, 255,
+ 28, 0, 255,
+ 27, 0, 255,
+ 26, 0, 255,
+ 24, 0, 255,
+ 23, 0, 255,
+ 22, 0, 255,
+ 20, 0, 255,
+ 19, 0, 255,
+ 18, 0, 255,
+ 17, 0, 255,
+ 15, 0, 255,
+ 14, 0, 255,
+ 13, 0, 255,
+ 11, 0, 255,
+ 10, 0, 255,
+ 9, 0, 255,
+ 8, 0, 255,
+ 6, 0, 255,
+ 5, 0, 255,
+ 4, 0, 255,
+ 3, 0, 255,
+ 1, 0, 255,
+ 0, 4, 255,
+ 0, 10, 255,
+ 0, 16, 255,
+ 0, 22, 255,
+ 0, 28, 255,
+ 0, 34, 255,
+ 0, 40, 255,
+ 0, 46, 255,
+ 0, 52, 255,
+ 0, 58, 255,
+ 0, 64, 255,
+ 0, 70, 255,
+ 0, 76, 255,
+ 0, 82, 255,
+ 0, 88, 255,
+ 0, 94, 255,
+ 0, 100, 255,
+ 0, 106, 255,
+ 0, 112, 255,
+ 0, 118, 255,
+ 0, 124, 255,
+ 0, 129, 255,
+ 0, 135, 255,
+ 0, 141, 255,
+ 0, 147, 255,
+ 0, 153, 255,
+ 0, 159, 255,
+ 0, 165, 255,
+ 0, 171, 255,
+ 0, 177, 255,
+ 0, 183, 255,
+ 0, 189, 255,
+ 0, 195, 255,
+ 0, 201, 255,
+ 0, 207, 255,
+ 0, 213, 255,
+ 0, 219, 255,
+ 0, 225, 255,
+ 0, 231, 255,
+ 0, 237, 255,
+ 0, 243, 255,
+ 0, 249, 255,
+ 0, 255, 255,
+ 0, 255, 249,
+ 0, 255, 243,
+ 0, 255, 237,
+ 0, 255, 231,
+ 0, 255, 225,
+ 0, 255, 219,
+ 0, 255, 213,
+ 0, 255, 207,
+ 0, 255, 201,
+ 0, 255, 195,
+ 0, 255, 189,
+ 0, 255, 183,
+ 0, 255, 177,
+ 0, 255, 171,
+ 0, 255, 165,
+ 0, 255, 159,
+ 0, 255, 153,
+ 0, 255, 147,
+ 0, 255, 141,
+ 0, 255, 135,
+ 0, 255, 129,
+ 0, 255, 124,
+ 0, 255, 118,
+ 0, 255, 112,
+ 0, 255, 106,
+ 0, 255, 100,
+ 0, 255, 94,
+ 0, 255, 88,
+ 0, 255, 82,
+ 0, 255, 76,
+ 0, 255, 70,
+ 0, 255, 64,
+ 0, 255, 58,
+ 0, 255, 52,
+ 0, 255, 46,
+ 0, 255, 40,
+ 0, 255, 34,
+ 0, 255, 28,
+ 0, 255, 22,
+ 0, 255, 16,
+ 0, 255, 10,
+ 0, 255, 4,
+ 2, 255, 0,
+ 8, 255, 0,
+ 14, 255, 0,
+ 20, 255, 0,
+ 26, 255, 0,
+ 32, 255, 0,
+ 38, 255, 0,
+ 44, 255, 0,
+ 50, 255, 0,
+ 56, 255, 0,
+ 62, 255, 0,
+ 68, 255, 0,
+ 74, 255, 0,
+ 80, 255, 0,
+ 86, 255, 0,
+ 92, 255, 0,
+ 98, 255, 0,
+ 104, 255, 0,
+ 110, 255, 0,
+ 116, 255, 0,
+ 122, 255, 0,
+ 128, 255, 0,
+ 133, 255, 0,
+ 139, 255, 0,
+ 145, 255, 0,
+ 151, 255, 0,
+ 157, 255, 0,
+ 163, 255, 0,
+ 169, 255, 0,
+ 175, 255, 0,
+ 181, 255, 0,
+ 187, 255, 0,
+ 193, 255, 0,
+ 199, 255, 0,
+ 205, 255, 0,
+ 211, 255, 0,
+ 217, 255, 0,
+ 223, 255, 0,
+ 229, 255, 0,
+ 235, 255, 0,
+ 241, 255, 0,
+ 247, 255, 0,
+ 253, 255, 0,
+ 255, 251, 0,
+ 255, 245, 0,
+ 255, 239, 0,
+ 255, 233, 0,
+ 255, 227, 0,
+ 255, 221, 0,
+ 255, 215, 0,
+ 255, 209, 0,
+ 255, 203, 0,
+ 255, 197, 0,
+ 255, 191, 0,
+ 255, 185, 0,
+ 255, 179, 0,
+ 255, 173, 0,
+ 255, 167, 0,
+ 255, 161, 0,
+ 255, 155, 0,
+ 255, 149, 0,
+ 255, 143, 0,
+ 255, 137, 0,
+ 255, 131, 0,
+ 255, 126, 0,
+ 255, 120, 0,
+ 255, 114, 0,
+ 255, 108, 0,
+ 255, 102, 0,
+ 255, 96, 0,
+ 255, 90, 0,
+ 255, 84, 0,
+ 255, 78, 0,
+ 255, 72, 0,
+ 255, 66, 0,
+ 255, 60, 0,
+ 255, 54, 0,
+ 255, 48, 0,
+ 255, 42, 0,
+ 255, 36, 0,
+ 255, 30, 0,
+ 255, 24, 0,
+ 255, 18, 0,
+ 255, 12, 0,
+ 255, 6, 0,
+ 255, 0, 0,
+ };
+
+} // namespace image_view
+
+// Register the nodelet
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS( image_view::DisparityNodelet, nodelet::Nodelet)
diff --git a/image_view/src/nodelets/image_nodelet.cpp b/image_view/src/nodelets/image_nodelet.cpp
new file mode 100644
index 0000000..9c3ffc7
--- /dev/null
+++ b/image_view/src/nodelets/image_nodelet.cpp
@@ -0,0 +1,222 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+#include <ros/ros.h>
+#include <nodelet/nodelet.h>
+#include <image_transport/image_transport.h>
+
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/highgui/highgui.hpp>
+#include "window_thread.h"
+
+#include <boost/thread.hpp>
+#include <boost/format.hpp>
+
+#ifdef HAVE_GTK
+#include <gtk/gtk.h>
+
+// Platform-specific workaround for #3026: image_view doesn't close when
+// closing image window. On platforms using GTK+ we connect this to the
+// window's "destroy" event so that image_view exits.
+static void destroyNode(GtkWidget *widget, gpointer data)
+{
+ /// @todo On ros::shutdown(), the node hangs. Why?
+ //ros::shutdown();
+ exit(0); // brute force solution
+}
+
+static void destroyNodelet(GtkWidget *widget, gpointer data)
+{
+ // We can't actually unload the nodelet from here, but we can at least
+ // unsubscribe from the image topic.
+ reinterpret_cast<image_transport::Subscriber*>(data)->shutdown();
+}
+#endif
+
+
+namespace image_view {
+
+class ImageNodelet : public nodelet::Nodelet
+{
+ image_transport::Subscriber sub_;
+
+ boost::mutex image_mutex_;
+ cv::Mat last_image_;
+
+ std::string window_name_;
+ boost::format filename_format_;
+ int count_;
+
+ virtual void onInit();
+
+ void imageCb(const sensor_msgs::ImageConstPtr& msg);
+
+ static void mouseCb(int event, int x, int y, int flags, void* param);
+
+public:
+ ImageNodelet();
+
+ ~ImageNodelet();
+};
+
+ImageNodelet::ImageNodelet()
+ : filename_format_(""), count_(0)
+{
+}
+
+ImageNodelet::~ImageNodelet()
+{
+ cv::destroyWindow(window_name_);
+}
+
+void ImageNodelet::onInit()
+{
+ ros::NodeHandle nh = getNodeHandle();
+ ros::NodeHandle local_nh = getPrivateNodeHandle();
+
+ // Command line argument parsing
+ const std::vector<std::string>& argv = getMyArgv();
+ // First positional argument is the transport type
+ std::string transport;
+ local_nh.param("image_transport", transport, std::string("raw"));
+ for (int i = 0; i < (int)argv.size(); ++i)
+ {
+ if (argv[i][0] != '-')
+ {
+ transport = argv[i];
+ break;
+ }
+ }
+ NODELET_INFO_STREAM("Using transport \"" << transport << "\"");
+ // Internal option, should be used only by the image_view node
+ bool shutdown_on_close = std::find(argv.begin(), argv.end(),
+ "--shutdown-on-close") != argv.end();
+
+ // Default window name is the resolved topic name
+ std::string topic = nh.resolveName("image");
+ local_nh.param("window_name", window_name_, topic);
+
+ bool autosize;
+ local_nh.param("autosize", autosize, false);
+
+ std::string format_string;
+ local_nh.param("filename_format", format_string, std::string("frame%04i.jpg"));
+ filename_format_.parse(format_string);
+
+ cv::namedWindow(window_name_, autosize ? cv::WND_PROP_AUTOSIZE : 0);
+ cv::setMouseCallback(window_name_, &ImageNodelet::mouseCb, this);
+
+#ifdef HAVE_GTK
+ // Register appropriate handler for when user closes the display window
+ GtkWidget *widget = GTK_WIDGET( cvGetWindowHandle(window_name_.c_str()) );
+ if (shutdown_on_close)
+ g_signal_connect(widget, "destroy", G_CALLBACK(destroyNode), NULL);
+ else
+ g_signal_connect(widget, "destroy", G_CALLBACK(destroyNodelet), &sub_);
+#endif
+
+ // Start the OpenCV window thread so we don't have to waitKey() somewhere
+ startWindowThread();
+
+ image_transport::ImageTransport it(nh);
+ image_transport::TransportHints hints(transport, ros::TransportHints(), getPrivateNodeHandle());
+ sub_ = it.subscribe(topic, 1, &ImageNodelet::imageCb, this, hints);
+}
+
+void ImageNodelet::imageCb(const sensor_msgs::ImageConstPtr& msg)
+{
+ image_mutex_.lock();
+
+ // We want to scale floating point images so that they display nicely
+ bool do_dynamic_scaling = (msg->encoding.find("F") != std::string::npos);
+
+ // Convert to OpenCV native BGR color
+ try {
+ cv_bridge::CvtColorForDisplayOptions options;
+ options.do_dynamic_scaling = do_dynamic_scaling;
+ last_image_ = cvtColorForDisplay(cv_bridge::toCvShare(msg), "", options)->image;
+ }
+ catch (cv_bridge::Exception& e) {
+ NODELET_ERROR_THROTTLE(30, "Unable to convert '%s' image for display: '%s'",
+ msg->encoding.c_str(), e.what());
+ }
+
+ // Must release the mutex before calling cv::imshow, or can deadlock against
+ // OpenCV's window mutex.
+ image_mutex_.unlock();
+ if (!last_image_.empty())
+ cv::imshow(window_name_, last_image_);
+}
+
+void ImageNodelet::mouseCb(int event, int x, int y, int flags, void* param)
+{
+ ImageNodelet *this_ = reinterpret_cast<ImageNodelet*>(param);
+ // Trick to use NODELET_* logging macros in static function
+ boost::function<const std::string&()> getName =
+ boost::bind(&ImageNodelet::getName, this_);
+
+ if (event == cv::EVENT_LBUTTONDOWN)
+ {
+ NODELET_WARN_ONCE("Left-clicking no longer saves images. Right-click instead.");
+ return;
+ }
+ if (event != cv::EVENT_RBUTTONDOWN)
+ return;
+
+ boost::lock_guard<boost::mutex> guard(this_->image_mutex_);
+
+ const cv::Mat &image = this_->last_image_;
+ if (image.empty())
+ {
+ NODELET_WARN("Couldn't save image, no data!");
+ return;
+ }
+
+ std::string filename = (this_->filename_format_ % this_->count_).str();
+ if (cv::imwrite(filename, image))
+ {
+ NODELET_INFO("Saved image %s", filename.c_str());
+ this_->count_++;
+ }
+ else
+ {
+ /// @todo Show full path, ask if user has permission to write there
+ NODELET_ERROR("Failed to save image.");
+ }
+}
+
+} // namespace image_view
+
+// Register the nodelet
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS( image_view::ImageNodelet, nodelet::Nodelet)
diff --git a/image_view/src/nodelets/window_thread.cpp b/image_view/src/nodelets/window_thread.cpp
new file mode 100644
index 0000000..8e1c8eb
--- /dev/null
+++ b/image_view/src/nodelets/window_thread.cpp
@@ -0,0 +1,52 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+#include "window_thread.h"
+#include <opencv2/highgui/highgui.hpp>
+#include <boost/thread.hpp>
+
+namespace {
+void startWindowThreadLocal() {
+ cv::startWindowThread();
+}
+}
+
+namespace image_view {
+
+void startWindowThread()
+{
+ static boost::once_flag cv_thread_flag = BOOST_ONCE_INIT;
+ boost::call_once(&startWindowThreadLocal, cv_thread_flag);
+}
+
+} // namespace image_view
diff --git a/image_view/src/nodelets/window_thread.h b/image_view/src/nodelets/window_thread.h
new file mode 100644
index 0000000..9418e79
--- /dev/null
+++ b/image_view/src/nodelets/window_thread.h
@@ -0,0 +1,44 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+#ifndef IMAGE_VIEW_WINDOW_THREAD_H
+#define IMAGE_VIEW_WINDOW_THREAD_H
+
+namespace image_view {
+
+// Makes absolutely sure we only start the OpenCV window thread once
+void startWindowThread();
+
+} // namespace image_view
+
+#endif
diff --git a/image_view/src/nodes/disparity_view.cpp b/image_view/src/nodes/disparity_view.cpp
new file mode 100644
index 0000000..d013b22
--- /dev/null
+++ b/image_view/src/nodes/disparity_view.cpp
@@ -0,0 +1,54 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+#include <ros/ros.h>
+#include <nodelet/loader.h>
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv, "disparity_view", ros::init_options::AnonymousName);
+ if (ros::names::remap("image") == "image") {
+ ROS_WARN("Topic 'image' has not been remapped! Typical command-line usage:\n"
+ "\t$ rosrun image_view disparity_view image:=<disparity image topic>");
+ }
+
+ nodelet::Loader manager(false);
+ nodelet::M_string remappings;
+ nodelet::V_string my_argv(argv + 1, argv + argc);
+ my_argv.push_back("--shutdown-on-close"); // Internal
+
+ manager.load(ros::this_node::getName(), "image_view/disparity", remappings, my_argv);
+
+ ros::spin();
+ return 0;
+}
diff --git a/image_view/src/nodes/extract_images.cpp b/image_view/src/nodes/extract_images.cpp
new file mode 100644
index 0000000..9ebdd48
--- /dev/null
+++ b/image_view/src/nodes/extract_images.cpp
@@ -0,0 +1,154 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#include <opencv2/highgui/highgui.hpp>
+
+#include <ros/ros.h>
+#include <sensor_msgs/Image.h>
+#include <cv_bridge/cv_bridge.h>
+#include <image_transport/image_transport.h>
+
+#include <boost/thread.hpp>
+#include <boost/format.hpp>
+
+class ExtractImages
+{
+private:
+ image_transport::Subscriber sub_;
+
+ sensor_msgs::ImageConstPtr last_msg_;
+ boost::mutex image_mutex_;
+
+ std::string window_name_;
+ boost::format filename_format_;
+ int count_;
+ double _time;
+ double sec_per_frame_;
+
+#if defined(_VIDEO)
+ CvVideoWriter* video_writer;
+#endif //_VIDEO
+
+public:
+ ExtractImages(const ros::NodeHandle& nh, const std::string& transport)
+ : filename_format_(""), count_(0), _time(ros::Time::now().toSec())
+ {
+ std::string topic = nh.resolveName("image");
+ ros::NodeHandle local_nh("~");
+
+ std::string format_string;
+ local_nh.param("filename_format", format_string, std::string("frame%04i.jpg"));
+ filename_format_.parse(format_string);
+
+ local_nh.param("sec_per_frame", sec_per_frame_, 0.1);
+
+ image_transport::ImageTransport it(nh);
+ sub_ = it.subscribe(topic, 1, &ExtractImages::image_cb, this, transport);
+
+#if defined(_VIDEO)
+ video_writer = 0;
+#endif
+
+ ROS_INFO("Initialized sec per frame to %f", sec_per_frame_);
+ }
+
+ ~ExtractImages()
+ {
+ }
+
+ void image_cb(const sensor_msgs::ImageConstPtr& msg)
+ {
+ boost::lock_guard<boost::mutex> guard(image_mutex_);
+
+ // Hang on to message pointer for sake of mouse_cb
+ last_msg_ = msg;
+
+ // May want to view raw bayer data
+ // NB: This is hacky, but should be OK since we have only one image CB.
+ if (msg->encoding.find("bayer") != std::string::npos)
+ boost::const_pointer_cast<sensor_msgs::Image>(msg)->encoding = "mono8";
+
+ cv::Mat image;
+ try
+ {
+ image = cv_bridge::toCvShare(msg, "bgr8")->image;
+ } catch(cv_bridge::Exception)
+ {
+ ROS_ERROR("Unable to convert %s image to bgr8", msg->encoding.c_str());
+ }
+
+ double delay = ros::Time::now().toSec()-_time;
+ if(delay >= sec_per_frame_)
+ {
+ _time = ros::Time::now().toSec();
+
+ if (!image.empty()) {
+ std::string filename = (filename_format_ % count_).str();
+
+#if !defined(_VIDEO)
+ cv::imwrite(filename, image);
+#else
+ if(!video_writer)
+ {
+ video_writer = cvCreateVideoWriter("video.avi", CV_FOURCC('M','J','P','G'),
+ int(1.0/sec_per_frame_), cvSize(image->width, image->height));
+ }
+
+ cvWriteFrame(video_writer, image);
+#endif // _VIDEO
+
+ ROS_INFO("Saved image %s", filename.c_str());
+ count_++;
+ } else {
+ ROS_WARN("Couldn't save image, no data!");
+ }
+ }
+ }
+};
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv, "extract_images", ros::init_options::AnonymousName);
+ ros::NodeHandle n;
+ if (n.resolveName("image") == "/image") {
+ ROS_WARN("extract_images: image has not been remapped! Typical command-line usage:\n"
+ "\t$ ./extract_images image:=<image topic> [transport]");
+ }
+
+ ExtractImages view(n, (argc > 1) ? argv[1] : "raw");
+
+ ros::spin();
+
+ return 0;
+}
diff --git a/image_view/src/nodes/image_saver.cpp b/image_view/src/nodes/image_saver.cpp
new file mode 100644
index 0000000..d3509c3
--- /dev/null
+++ b/image_view/src/nodes/image_saver.cpp
@@ -0,0 +1,226 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+#include <opencv2/highgui/highgui.hpp>
+
+#include <ros/ros.h>
+#include <cv_bridge/cv_bridge.h>
+#include <image_transport/image_transport.h>
+#include <camera_calibration_parsers/parse.h>
+#include <boost/format.hpp>
+
+#include <std_srvs/Empty.h>
+#include <std_srvs/Trigger.h>
+
+boost::format g_format;
+bool save_all_image, save_image_service;
+std::string encoding;
+bool request_start_end;
+
+
+bool service(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) {
+ save_image_service = true;
+ return true;
+}
+
+/** Class to deal with which callback to call whether we have CameraInfo or not
+ */
+class Callbacks {
+public:
+ Callbacks() : is_first_image_(true), has_camera_info_(false), count_(0) {
+ }
+
+ bool callbackStartSave(std_srvs::Trigger::Request &req,
+ std_srvs::Trigger::Response &res)
+ {
+ ROS_INFO("Received start saving request");
+ start_time_ = ros::Time::now();
+ end_time_ = ros::Time(0);
+
+ res.success = true;
+ return true;
+ }
+
+ bool callbackEndSave(std_srvs::Trigger::Request &req,
+ std_srvs::Trigger::Response &res)
+ {
+ ROS_INFO("Received end saving request");
+ end_time_ = ros::Time::now();
+
+ res.success = true;
+ return true;
+ }
+
+ void callbackWithoutCameraInfo(const sensor_msgs::ImageConstPtr& image_msg)
+ {
+ if (is_first_image_) {
+ is_first_image_ = false;
+
+ // Wait a tiny bit to see whether callbackWithCameraInfo is called
+ ros::Duration(0.001).sleep();
+ }
+
+ if (has_camera_info_)
+ return;
+
+ // saving flag priority:
+ // 1. request by service.
+ // 2. request by topic about start and end.
+ // 3. flag 'save_all_image'.
+ if (!save_image_service && request_start_end) {
+ if (start_time_ == ros::Time(0))
+ return;
+ else if (start_time_ > image_msg->header.stamp)
+ return; // wait for message which comes after start_time
+ else if ((end_time_ != ros::Time(0)) && (end_time_ < image_msg->header.stamp))
+ return; // skip message which comes after end_time
+ }
+
+ // save the image
+ std::string filename;
+ if (!saveImage(image_msg, filename))
+ return;
+
+ count_++;
+ }
+
+ void callbackWithCameraInfo(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& info)
+ {
+ has_camera_info_ = true;
+
+ if (!save_image_service && request_start_end) {
+ if (start_time_ == ros::Time(0))
+ return;
+ else if (start_time_ > image_msg->header.stamp)
+ return; // wait for message which comes after start_time
+ else if ((end_time_ != ros::Time(0)) && (end_time_ < image_msg->header.stamp))
+ return; // skip message which comes after end_time
+ }
+
+ // save the image
+ std::string filename;
+ if (!saveImage(image_msg, filename))
+ return;
+
+ // save the CameraInfo
+ if (info) {
+ filename = filename.replace(filename.rfind("."), filename.length(), ".ini");
+ camera_calibration_parsers::writeCalibration(filename, "camera", *info);
+ }
+
+ count_++;
+ }
+private:
+ bool saveImage(const sensor_msgs::ImageConstPtr& image_msg, std::string &filename) {
+ cv::Mat image;
+ try
+ {
+ image = cv_bridge::toCvShare(image_msg, encoding)->image;
+ } catch(cv_bridge::Exception)
+ {
+ ROS_ERROR("Unable to convert %s image to %s", image_msg->encoding.c_str(), encoding.c_str());
+ return false;
+ }
+
+ if (!image.empty()) {
+ try {
+ filename = (g_format).str();
+ } catch (...) { g_format.clear(); }
+ try {
+ filename = (g_format % count_).str();
+ } catch (...) { g_format.clear(); }
+ try {
+ filename = (g_format % count_ % "jpg").str();
+ } catch (...) { g_format.clear(); }
+
+ if ( save_all_image || save_image_service ) {
+ cv::imwrite(filename, image);
+ ROS_INFO("Saved image %s", filename.c_str());
+
+ save_image_service = false;
+ } else {
+ return false;
+ }
+ } else {
+ ROS_WARN("Couldn't save image, no data!");
+ return false;
+ }
+ return true;
+ }
+
+private:
+ bool is_first_image_;
+ bool has_camera_info_;
+ size_t count_;
+ ros::Time start_time_;
+ ros::Time end_time_;
+};
+
+int main(int argc, char** argv)
+{
+ ros::init(argc, argv, "image_saver", ros::init_options::AnonymousName);
+ ros::NodeHandle nh;
+ image_transport::ImageTransport it(nh);
+ std::string topic = nh.resolveName("image");
+
+ Callbacks callbacks;
+ // Useful when CameraInfo is being published
+ image_transport::CameraSubscriber sub_image_and_camera = it.subscribeCamera(topic, 1,
+ &Callbacks::callbackWithCameraInfo,
+ &callbacks);
+ // Useful when CameraInfo is not being published
+ image_transport::Subscriber sub_image = it.subscribe(
+ topic, 1, boost::bind(&Callbacks::callbackWithoutCameraInfo, &callbacks, _1));
+
+ ros::NodeHandle local_nh("~");
+ std::string format_string;
+ local_nh.param("filename_format", format_string, std::string("left%04i.%s"));
+ local_nh.param("encoding", encoding, std::string("bgr8"));
+ local_nh.param("save_all_image", save_all_image, true);
+ local_nh.param("request_start_end", request_start_end, false);
+ g_format.parse(format_string);
+ ros::ServiceServer save = local_nh.advertiseService ("save", service);
+
+ if (request_start_end && !save_all_image)
+ ROS_WARN("'request_start_end' is true, so overwriting 'save_all_image' as true");
+
+ // FIXME(unkown): This does not make services appear
+ // if (request_start_end) {
+ ros::ServiceServer srv_start = local_nh.advertiseService(
+ "start", &Callbacks::callbackStartSave, &callbacks);
+ ros::ServiceServer srv_end = local_nh.advertiseService(
+ "end", &Callbacks::callbackEndSave, &callbacks);
+ // }
+
+ ros::spin();
+}
diff --git a/image_view/src/nodes/image_view.cpp b/image_view/src/nodes/image_view.cpp
new file mode 100644
index 0000000..fdb8eb3
--- /dev/null
+++ b/image_view/src/nodes/image_view.cpp
@@ -0,0 +1,212 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+#include <image_view/ImageViewConfig.h>
+
+#include <ros/ros.h>
+#include <image_transport/image_transport.h>
+#include <dynamic_reconfigure/server.h>
+
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/highgui/highgui.hpp>
+
+#include <boost/format.hpp>
+#include <boost/thread.hpp>
+#include <boost/filesystem.hpp>
+
+int g_count;
+cv::Mat g_last_image;
+boost::format g_filename_format;
+boost::mutex g_image_mutex;
+std::string g_window_name;
+bool g_gui;
+ros::Publisher g_pub;
+bool g_do_dynamic_scaling;
+int g_colormap;
+double g_min_image_value;
+double g_max_image_value;
+
+void reconfigureCb(image_view::ImageViewConfig &config, uint32_t level)
+{
+ boost::mutex::scoped_lock lock(g_image_mutex);
+ g_do_dynamic_scaling = config.do_dynamic_scaling;
+ g_colormap = config.colormap;
+ g_min_image_value = config.min_image_value;
+ g_max_image_value = config.max_image_value;
+}
+
+void imageCb(const sensor_msgs::ImageConstPtr& msg)
+{
+ boost::mutex::scoped_lock lock(g_image_mutex);
+
+ // Convert to OpenCV native BGR color
+ cv_bridge::CvImageConstPtr cv_ptr;
+ try {
+ cv_bridge::CvtColorForDisplayOptions options;
+ options.do_dynamic_scaling = g_do_dynamic_scaling;
+ options.colormap = g_colormap;
+ // Set min/max value for scaling to visualize depth/float image.
+ if (g_min_image_value == g_max_image_value) {
+ // Not specified by rosparam, then set default value.
+ // Because of current sensor limitation, we use 10m as default of max range of depth
+ // with consistency to the configuration in rqt_image_view.
+ options.min_image_value = 0;
+ if (msg->encoding == "32FC1") {
+ options.max_image_value = 10; // 10 [m]
+ } else if (msg->encoding == "16UC1") {
+ options.max_image_value = 10 * 1000; // 10 * 1000 [mm]
+ }
+ } else {
+ options.min_image_value = g_min_image_value;
+ options.max_image_value = g_max_image_value;
+ }
+ cv_ptr = cv_bridge::cvtColorForDisplay(cv_bridge::toCvShare(msg), "", options);
+ g_last_image = cv_ptr->image;
+ } catch (cv_bridge::Exception& e) {
+ ROS_ERROR_THROTTLE(30, "Unable to convert '%s' image for display: '%s'",
+ msg->encoding.c_str(), e.what());
+ }
+ if (g_gui && !g_last_image.empty()) {
+ const cv::Mat &image = g_last_image;
+ cv::imshow(g_window_name, image);
+ cv::waitKey(3);
+ }
+ if (g_pub.getNumSubscribers() > 0) {
+ g_pub.publish(cv_ptr);
+ }
+}
+
+static void mouseCb(int event, int x, int y, int flags, void* param)
+{
+ if (event == cv::EVENT_LBUTTONDOWN) {
+ ROS_WARN_ONCE("Left-clicking no longer saves images. Right-click instead.");
+ return;
+ } else if (event != cv::EVENT_RBUTTONDOWN) {
+ return;
+ }
+
+ boost::mutex::scoped_lock lock(g_image_mutex);
+
+ const cv::Mat &image = g_last_image;
+
+ if (image.empty()) {
+ ROS_WARN("Couldn't save image, no data!");
+ return;
+ }
+
+ std::string filename = (g_filename_format % g_count).str();
+ if (cv::imwrite(filename, image)) {
+ ROS_INFO("Saved image %s", filename.c_str());
+ g_count++;
+ } else {
+ boost::filesystem::path full_path = boost::filesystem::complete(filename);
+ ROS_ERROR_STREAM("Failed to save image. Have permission to write there?: " << full_path);
+ }
+}
+
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv, "image_view", ros::init_options::AnonymousName);
+ if (ros::names::remap("image") == "image") {
+ ROS_WARN("Topic 'image' has not been remapped! Typical command-line usage:\n"
+ "\t$ rosrun image_view image_view image:=<image topic> [transport]");
+ }
+
+ ros::NodeHandle nh;
+ ros::NodeHandle local_nh("~");
+
+ // Default window name is the resolved topic name
+ std::string topic = nh.resolveName("image");
+ local_nh.param("window_name", g_window_name, topic);
+ local_nh.param("gui", g_gui, true); // gui/no_gui mode
+
+ if (g_gui) {
+ std::string format_string;
+ local_nh.param("filename_format", format_string, std::string("frame%04i.jpg"));
+ g_filename_format.parse(format_string);
+
+ // Handle window size
+ bool autosize;
+ local_nh.param("autosize", autosize, false);
+ cv::namedWindow(g_window_name, autosize ? (CV_WINDOW_AUTOSIZE | CV_WINDOW_KEEPRATIO | CV_GUI_EXPANDED) : 0);
+ cv::setMouseCallback(g_window_name, &mouseCb);
+
+ if(autosize == false)
+ {
+ if(local_nh.hasParam("width") && local_nh.hasParam("height"))
+ {
+ int width;
+ local_nh.getParam("width", width);
+ int height;
+ local_nh.getParam("height", height);
+ cv::resizeWindow(g_window_name, width, height);
+ }
+ }
+
+ // Start the OpenCV window thread so we don't have to waitKey() somewhere
+ cv::startWindowThread();
+ }
+
+ // Handle transport
+ // priority:
+ // 1. command line argument
+ // 2. rosparam '~image_transport'
+ std::string transport;
+ local_nh.param("image_transport", transport, std::string("raw"));
+ ros::V_string myargv;
+ ros::removeROSArgs(argc, argv, myargv);
+ for (size_t i = 1; i < myargv.size(); ++i) {
+ if (myargv[i][0] != '-') {
+ transport = myargv[i];
+ break;
+ }
+ }
+ ROS_INFO_STREAM("Using transport \"" << transport << "\"");
+ image_transport::ImageTransport it(nh);
+ image_transport::TransportHints hints(transport, ros::TransportHints(), local_nh);
+ image_transport::Subscriber sub = it.subscribe(topic, 1, imageCb, hints);
+ g_pub = local_nh.advertise<sensor_msgs::Image>("output", 1);
+
+ dynamic_reconfigure::Server<image_view::ImageViewConfig> srv;
+ dynamic_reconfigure::Server<image_view::ImageViewConfig>::CallbackType f =
+ boost::bind(&reconfigureCb, _1, _2);
+ srv.setCallback(f);
+
+ ros::spin();
+
+ if (g_gui) {
+ cv::destroyWindow(g_window_name);
+ }
+ return 0;
+}
diff --git a/image_view/src/nodes/stereo_view.cpp b/image_view/src/nodes/stereo_view.cpp
new file mode 100644
index 0000000..64a01cc
--- /dev/null
+++ b/image_view/src/nodes/stereo_view.cpp
@@ -0,0 +1,573 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#include <opencv2/highgui/highgui.hpp>
+
+#include <ros/ros.h>
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/image_encodings.h>
+#include <stereo_msgs/DisparityImage.h>
+#include <cv_bridge/cv_bridge.h>
+
+#include <message_filters/subscriber.h>
+#include <message_filters/synchronizer.h>
+#include <message_filters/sync_policies/exact_time.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <image_transport/subscriber_filter.h>
+
+#include <boost/thread.hpp>
+#include <boost/format.hpp>
+
+#ifdef HAVE_GTK
+#include <gtk/gtk.h>
+
+// Platform-specific workaround for #3026: image_view doesn't close when
+// closing image window. On platforms using GTK+ we connect this to the
+// window's "destroy" event so that image_view exits.
+static void destroy(GtkWidget *widget, gpointer data)
+{
+ ros::shutdown();
+}
+#endif
+
+namespace enc = sensor_msgs::image_encodings;
+
+// colormap for disparities, RGB
+static unsigned char colormap[768] =
+ { 150, 150, 150,
+ 107, 0, 12,
+ 106, 0, 18,
+ 105, 0, 24,
+ 103, 0, 30,
+ 102, 0, 36,
+ 101, 0, 42,
+ 99, 0, 48,
+ 98, 0, 54,
+ 97, 0, 60,
+ 96, 0, 66,
+ 94, 0, 72,
+ 93, 0, 78,
+ 92, 0, 84,
+ 91, 0, 90,
+ 89, 0, 96,
+ 88, 0, 102,
+ 87, 0, 108,
+ 85, 0, 114,
+ 84, 0, 120,
+ 83, 0, 126,
+ 82, 0, 131,
+ 80, 0, 137,
+ 79, 0, 143,
+ 78, 0, 149,
+ 77, 0, 155,
+ 75, 0, 161,
+ 74, 0, 167,
+ 73, 0, 173,
+ 71, 0, 179,
+ 70, 0, 185,
+ 69, 0, 191,
+ 68, 0, 197,
+ 66, 0, 203,
+ 65, 0, 209,
+ 64, 0, 215,
+ 62, 0, 221,
+ 61, 0, 227,
+ 60, 0, 233,
+ 59, 0, 239,
+ 57, 0, 245,
+ 56, 0, 251,
+ 55, 0, 255,
+ 54, 0, 255,
+ 52, 0, 255,
+ 51, 0, 255,
+ 50, 0, 255,
+ 48, 0, 255,
+ 47, 0, 255,
+ 46, 0, 255,
+ 45, 0, 255,
+ 43, 0, 255,
+ 42, 0, 255,
+ 41, 0, 255,
+ 40, 0, 255,
+ 38, 0, 255,
+ 37, 0, 255,
+ 36, 0, 255,
+ 34, 0, 255,
+ 33, 0, 255,
+ 32, 0, 255,
+ 31, 0, 255,
+ 29, 0, 255,
+ 28, 0, 255,
+ 27, 0, 255,
+ 26, 0, 255,
+ 24, 0, 255,
+ 23, 0, 255,
+ 22, 0, 255,
+ 20, 0, 255,
+ 19, 0, 255,
+ 18, 0, 255,
+ 17, 0, 255,
+ 15, 0, 255,
+ 14, 0, 255,
+ 13, 0, 255,
+ 11, 0, 255,
+ 10, 0, 255,
+ 9, 0, 255,
+ 8, 0, 255,
+ 6, 0, 255,
+ 5, 0, 255,
+ 4, 0, 255,
+ 3, 0, 255,
+ 1, 0, 255,
+ 0, 4, 255,
+ 0, 10, 255,
+ 0, 16, 255,
+ 0, 22, 255,
+ 0, 28, 255,
+ 0, 34, 255,
+ 0, 40, 255,
+ 0, 46, 255,
+ 0, 52, 255,
+ 0, 58, 255,
+ 0, 64, 255,
+ 0, 70, 255,
+ 0, 76, 255,
+ 0, 82, 255,
+ 0, 88, 255,
+ 0, 94, 255,
+ 0, 100, 255,
+ 0, 106, 255,
+ 0, 112, 255,
+ 0, 118, 255,
+ 0, 124, 255,
+ 0, 129, 255,
+ 0, 135, 255,
+ 0, 141, 255,
+ 0, 147, 255,
+ 0, 153, 255,
+ 0, 159, 255,
+ 0, 165, 255,
+ 0, 171, 255,
+ 0, 177, 255,
+ 0, 183, 255,
+ 0, 189, 255,
+ 0, 195, 255,
+ 0, 201, 255,
+ 0, 207, 255,
+ 0, 213, 255,
+ 0, 219, 255,
+ 0, 225, 255,
+ 0, 231, 255,
+ 0, 237, 255,
+ 0, 243, 255,
+ 0, 249, 255,
+ 0, 255, 255,
+ 0, 255, 249,
+ 0, 255, 243,
+ 0, 255, 237,
+ 0, 255, 231,
+ 0, 255, 225,
+ 0, 255, 219,
+ 0, 255, 213,
+ 0, 255, 207,
+ 0, 255, 201,
+ 0, 255, 195,
+ 0, 255, 189,
+ 0, 255, 183,
+ 0, 255, 177,
+ 0, 255, 171,
+ 0, 255, 165,
+ 0, 255, 159,
+ 0, 255, 153,
+ 0, 255, 147,
+ 0, 255, 141,
+ 0, 255, 135,
+ 0, 255, 129,
+ 0, 255, 124,
+ 0, 255, 118,
+ 0, 255, 112,
+ 0, 255, 106,
+ 0, 255, 100,
+ 0, 255, 94,
+ 0, 255, 88,
+ 0, 255, 82,
+ 0, 255, 76,
+ 0, 255, 70,
+ 0, 255, 64,
+ 0, 255, 58,
+ 0, 255, 52,
+ 0, 255, 46,
+ 0, 255, 40,
+ 0, 255, 34,
+ 0, 255, 28,
+ 0, 255, 22,
+ 0, 255, 16,
+ 0, 255, 10,
+ 0, 255, 4,
+ 2, 255, 0,
+ 8, 255, 0,
+ 14, 255, 0,
+ 20, 255, 0,
+ 26, 255, 0,
+ 32, 255, 0,
+ 38, 255, 0,
+ 44, 255, 0,
+ 50, 255, 0,
+ 56, 255, 0,
+ 62, 255, 0,
+ 68, 255, 0,
+ 74, 255, 0,
+ 80, 255, 0,
+ 86, 255, 0,
+ 92, 255, 0,
+ 98, 255, 0,
+ 104, 255, 0,
+ 110, 255, 0,
+ 116, 255, 0,
+ 122, 255, 0,
+ 128, 255, 0,
+ 133, 255, 0,
+ 139, 255, 0,
+ 145, 255, 0,
+ 151, 255, 0,
+ 157, 255, 0,
+ 163, 255, 0,
+ 169, 255, 0,
+ 175, 255, 0,
+ 181, 255, 0,
+ 187, 255, 0,
+ 193, 255, 0,
+ 199, 255, 0,
+ 205, 255, 0,
+ 211, 255, 0,
+ 217, 255, 0,
+ 223, 255, 0,
+ 229, 255, 0,
+ 235, 255, 0,
+ 241, 255, 0,
+ 247, 255, 0,
+ 253, 255, 0,
+ 255, 251, 0,
+ 255, 245, 0,
+ 255, 239, 0,
+ 255, 233, 0,
+ 255, 227, 0,
+ 255, 221, 0,
+ 255, 215, 0,
+ 255, 209, 0,
+ 255, 203, 0,
+ 255, 197, 0,
+ 255, 191, 0,
+ 255, 185, 0,
+ 255, 179, 0,
+ 255, 173, 0,
+ 255, 167, 0,
+ 255, 161, 0,
+ 255, 155, 0,
+ 255, 149, 0,
+ 255, 143, 0,
+ 255, 137, 0,
+ 255, 131, 0,
+ 255, 126, 0,
+ 255, 120, 0,
+ 255, 114, 0,
+ 255, 108, 0,
+ 255, 102, 0,
+ 255, 96, 0,
+ 255, 90, 0,
+ 255, 84, 0,
+ 255, 78, 0,
+ 255, 72, 0,
+ 255, 66, 0,
+ 255, 60, 0,
+ 255, 54, 0,
+ 255, 48, 0,
+ 255, 42, 0,
+ 255, 36, 0,
+ 255, 30, 0,
+ 255, 24, 0,
+ 255, 18, 0,
+ 255, 12, 0,
+ 255, 6, 0,
+ 255, 0, 0,
+ };
+
+inline void increment(int* value)
+{
+ ++(*value);
+}
+
+using namespace sensor_msgs;
+using namespace stereo_msgs;
+using namespace message_filters::sync_policies;
+
+// Note: StereoView is NOT nodelet-based, as it synchronizes the three streams.
+class StereoView
+{
+private:
+ image_transport::SubscriberFilter left_sub_, right_sub_;
+ message_filters::Subscriber<DisparityImage> disparity_sub_;
+ typedef ExactTime<Image, Image, DisparityImage> ExactPolicy;
+ typedef ApproximateTime<Image, Image, DisparityImage> ApproximatePolicy;
+ typedef message_filters::Synchronizer<ExactPolicy> ExactSync;
+ typedef message_filters::Synchronizer<ApproximatePolicy> ApproximateSync;
+ boost::shared_ptr<ExactSync> exact_sync_;
+ boost::shared_ptr<ApproximateSync> approximate_sync_;
+ int queue_size_;
+
+ ImageConstPtr last_left_msg_, last_right_msg_;
+ cv::Mat last_left_image_, last_right_image_;
+ cv::Mat_<cv::Vec3b> disparity_color_;
+ boost::mutex image_mutex_;
+
+ boost::format filename_format_;
+ int save_count_;
+
+ ros::WallTimer check_synced_timer_;
+ int left_received_, right_received_, disp_received_, all_received_;
+
+public:
+ StereoView(const std::string& transport)
+ : filename_format_(""), save_count_(0),
+ left_received_(0), right_received_(0), disp_received_(0), all_received_(0)
+ {
+ // Read local parameters
+ ros::NodeHandle local_nh("~");
+ bool autosize;
+ local_nh.param("autosize", autosize, true);
+
+ std::string format_string;
+ local_nh.param("filename_format", format_string, std::string("%s%04i.jpg"));
+ filename_format_.parse(format_string);
+
+ // Do GUI window setup
+ int flags = autosize ? cv::WND_PROP_AUTOSIZE : 0;
+ cv::namedWindow("left", flags);
+ cv::namedWindow("right", flags);
+ cv::namedWindow("disparity", flags);
+ cv::setMouseCallback("left", &StereoView::mouseCb, this);
+ cv::setMouseCallback("right", &StereoView::mouseCb, this);
+ cv::setMouseCallback("disparity", &StereoView::mouseCb, this);
+#if CV_MAJOR_VERSION == 2
+#ifdef HAVE_GTK
+ g_signal_connect(GTK_WIDGET( cvGetWindowHandle("left") ),
+ "destroy", G_CALLBACK(destroy), NULL);
+ g_signal_connect(GTK_WIDGET( cvGetWindowHandle("right") ),
+ "destroy", G_CALLBACK(destroy), NULL);
+ g_signal_connect(GTK_WIDGET( cvGetWindowHandle("disparity") ),
+ "destroy", G_CALLBACK(destroy), NULL);
+#endif
+ cvStartWindowThread();
+#endif
+
+ // Resolve topic names
+ ros::NodeHandle nh;
+ std::string stereo_ns = nh.resolveName("stereo");
+ std::string left_topic = ros::names::clean(stereo_ns + "/left/" + nh.resolveName("image"));
+ std::string right_topic = ros::names::clean(stereo_ns + "/right/" + nh.resolveName("image"));
+ std::string disparity_topic = ros::names::clean(stereo_ns + "/disparity");
+ ROS_INFO("Subscribing to:\n\t* %s\n\t* %s\n\t* %s", left_topic.c_str(), right_topic.c_str(),
+ disparity_topic.c_str());
+
+ // Subscribe to three input topics.
+ image_transport::ImageTransport it(nh);
+ left_sub_.subscribe(it, left_topic, 1, transport);
+ right_sub_.subscribe(it, right_topic, 1, transport);
+ disparity_sub_.subscribe(nh, disparity_topic, 1);
+
+ // Complain every 30s if the topics appear unsynchronized
+ left_sub_.registerCallback(boost::bind(increment, &left_received_));
+ right_sub_.registerCallback(boost::bind(increment, &right_received_));
+ disparity_sub_.registerCallback(boost::bind(increment, &disp_received_));
+ check_synced_timer_ = nh.createWallTimer(ros::WallDuration(15.0),
+ boost::bind(&StereoView::checkInputsSynchronized, this));
+
+ // Synchronize input topics. Optionally do approximate synchronization.
+ local_nh.param("queue_size", queue_size_, 5);
+ bool approx;
+ local_nh.param("approximate_sync", approx, false);
+ if (approx)
+ {
+ approximate_sync_.reset( new ApproximateSync(ApproximatePolicy(queue_size_),
+ left_sub_, right_sub_, disparity_sub_) );
+ approximate_sync_->registerCallback(boost::bind(&StereoView::imageCb, this, _1, _2, _3));
+ }
+ else
+ {
+ exact_sync_.reset( new ExactSync(ExactPolicy(queue_size_),
+ left_sub_, right_sub_, disparity_sub_) );
+ exact_sync_->registerCallback(boost::bind(&StereoView::imageCb, this, _1, _2, _3));
+ }
+ }
+
+ ~StereoView()
+ {
+ cv::destroyAllWindows();
+ }
+
+ void imageCb(const ImageConstPtr& left, const ImageConstPtr& right,
+ const DisparityImageConstPtr& disparity_msg)
+ {
+ ++all_received_; // For error checking
+
+ image_mutex_.lock();
+
+ // May want to view raw bayer data
+ if (left->encoding.find("bayer") != std::string::npos)
+ boost::const_pointer_cast<Image>(left)->encoding = "mono8";
+ if (right->encoding.find("bayer") != std::string::npos)
+ boost::const_pointer_cast<Image>(right)->encoding = "mono8";
+
+ // Hang on to image data for sake of mouseCb
+ last_left_msg_ = left;
+ last_right_msg_ = right;
+ try {
+ last_left_image_ = cv_bridge::toCvShare(left, "bgr8")->image;
+ last_right_image_ = cv_bridge::toCvShare(right, "bgr8")->image;
+ }
+ catch (cv_bridge::Exception& e) {
+ ROS_ERROR("Unable to convert one of '%s' or '%s' to 'bgr8'",
+ left->encoding.c_str(), right->encoding.c_str());
+ }
+
+ // Colormap and display the disparity image
+ float min_disparity = disparity_msg->min_disparity;
+ float max_disparity = disparity_msg->max_disparity;
+ float multiplier = 255.0f / (max_disparity - min_disparity);
+
+ assert(disparity_msg->image.encoding == enc::TYPE_32FC1);
+ const cv::Mat_<float> dmat(disparity_msg->image.height, disparity_msg->image.width,
+ (float*)&disparity_msg->image.data[0], disparity_msg->image.step);
+ disparity_color_.create(disparity_msg->image.height, disparity_msg->image.width);
+
+ for (int row = 0; row < disparity_color_.rows; ++row) {
+ const float* d = dmat[row];
+ for (int col = 0; col < disparity_color_.cols; ++col) {
+ int index = (d[col] - min_disparity) * multiplier + 0.5;
+ index = std::min(255, std::max(0, index));
+ // Fill as BGR
+ disparity_color_(row, col)[2] = colormap[3*index + 0];
+ disparity_color_(row, col)[1] = colormap[3*index + 1];
+ disparity_color_(row, col)[0] = colormap[3*index + 2];
+ }
+ }
+
+ // Must release the mutex before calling cv::imshow, or can deadlock against
+ // OpenCV's window mutex.
+ image_mutex_.unlock();
+ if (!last_left_image_.empty()) {
+ cv::imshow("left", last_left_image_);
+ cv::waitKey(1);
+ }
+ if (!last_right_image_.empty()) {
+ cv::imshow("right", last_right_image_);
+ cv::waitKey(1);
+ }
+ cv::imshow("disparity", disparity_color_);
+ cv::waitKey(1);
+ }
+
+ void saveImage(const char* prefix, const cv::Mat& image)
+ {
+ if (!image.empty()) {
+ std::string filename = (filename_format_ % prefix % save_count_).str();
+ cv::imwrite(filename, image);
+ ROS_INFO("Saved image %s", filename.c_str());
+ } else {
+ ROS_WARN("Couldn't save %s image, no data!", prefix);
+ }
+ }
+
+ static void mouseCb(int event, int x, int y, int flags, void* param)
+ {
+ if (event == cv::EVENT_LBUTTONDOWN)
+ {
+ ROS_WARN_ONCE("Left-clicking no longer saves images. Right-click instead.");
+ return;
+ }
+ if (event != cv::EVENT_RBUTTONDOWN)
+ return;
+
+ StereoView *sv = (StereoView*)param;
+ boost::lock_guard<boost::mutex> guard(sv->image_mutex_);
+
+ sv->saveImage("left", sv->last_left_image_);
+ sv->saveImage("right", sv->last_right_image_);
+ sv->saveImage("disp", sv->disparity_color_);
+ sv->save_count_++;
+ }
+
+ void checkInputsSynchronized()
+ {
+ int threshold = 3 * all_received_;
+ if (left_received_ >= threshold || right_received_ >= threshold || disp_received_ >= threshold) {
+ ROS_WARN("[stereo_view] Low number of synchronized left/right/disparity triplets received.\n"
+ "Left images received: %d (topic '%s')\n"
+ "Right images received: %d (topic '%s')\n"
+ "Disparity images received: %d (topic '%s')\n"
+ "Synchronized triplets: %d\n"
+ "Possible issues:\n"
+ "\t* stereo_image_proc is not running.\n"
+ "\t Does `rosnode info %s` show any connections?\n"
+ "\t* The cameras are not synchronized.\n"
+ "\t Try restarting stereo_view with parameter _approximate_sync:=True\n"
+ "\t* The network is too slow. One or more images are dropped from each triplet.\n"
+ "\t Try restarting stereo_view, increasing parameter 'queue_size' (currently %d)",
+ left_received_, left_sub_.getTopic().c_str(),
+ right_received_, right_sub_.getTopic().c_str(),
+ disp_received_, disparity_sub_.getTopic().c_str(),
+ all_received_, ros::this_node::getName().c_str(), queue_size_);
+ }
+ }
+};
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv, "stereo_view", ros::init_options::AnonymousName);
+ if (ros::names::remap("stereo") == "stereo") {
+ ROS_WARN("'stereo' has not been remapped! Example command-line usage:\n"
+ "\t$ rosrun image_view stereo_view stereo:=narrow_stereo image:=image_color");
+ }
+ if (ros::names::remap("image") == "/image_raw") {
+ ROS_WARN("There is a delay between when the camera drivers publish the raw images and "
+ "when stereo_image_proc publishes the computed point cloud. stereo_view "
+ "may fail to synchronize these topics without a large queue_size.");
+ }
+
+ std::string transport = argc > 1 ? argv[1] : "raw";
+ StereoView view(transport);
+
+ ros::spin();
+ return 0;
+}
diff --git a/image_view/src/nodes/video_recorder.cpp b/image_view/src/nodes/video_recorder.cpp
new file mode 100644
index 0000000..00041dc
--- /dev/null
+++ b/image_view/src/nodes/video_recorder.cpp
@@ -0,0 +1,141 @@
+/****************************************************************************
+* Software License Agreement (Apache License)
+*
+* Copyright (C) 2012-2013 Open Source Robotics Foundation
+*
+* 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.
+*
+*****************************************************************************/
+
+#include <opencv2/highgui/highgui.hpp>
+#include <ros/ros.h>
+#include <sensor_msgs/image_encodings.h>
+#include <cv_bridge/cv_bridge.h>
+#include <image_transport/image_transport.h>
+#include <camera_calibration_parsers/parse.h>
+#if CV_MAJOR_VERSION == 3
+#include <opencv2/videoio.hpp>
+#endif
+
+cv::VideoWriter outputVideo;
+
+int g_count = 0;
+ros::Time g_last_wrote_time = ros::Time(0);
+std::string encoding;
+std::string codec;
+int fps;
+std::string filename;
+double min_depth_range;
+double max_depth_range;
+bool use_dynamic_range;
+int colormap;
+
+
+void callback(const sensor_msgs::ImageConstPtr& image_msg)
+{
+ if (!outputVideo.isOpened()) {
+
+ cv::Size size(image_msg->width, image_msg->height);
+
+ outputVideo.open(filename,
+#if CV_MAJOR_VERSION == 3
+ cv::VideoWriter::fourcc(codec.c_str()[0],
+#else
+ CV_FOURCC(codec.c_str()[0],
+#endif
+ codec.c_str()[1],
+ codec.c_str()[2],
+ codec.c_str()[3]),
+ fps,
+ size,
+ true);
+
+ if (!outputVideo.isOpened())
+ {
+ ROS_ERROR("Could not create the output video! Check filename and/or support for codec.");
+ exit(-1);
+ }
+
+ ROS_INFO_STREAM("Starting to record " << codec << " video at " << size << "@" << fps << "fps. Press Ctrl+C to stop recording." );
+
+ }
+
+ if ((image_msg->header.stamp - g_last_wrote_time) < ros::Duration(1 / fps))
+ {
+ // Skip to get video with correct fps
+ return;
+ }
+
+ try
+ {
+ cv_bridge::CvtColorForDisplayOptions options;
+ options.do_dynamic_scaling = use_dynamic_range;
+ options.min_image_value = min_depth_range;
+ options.max_image_value = max_depth_range;
+ options.colormap = colormap;
+ const cv::Mat image = cv_bridge::cvtColorForDisplay(cv_bridge::toCvShare(image_msg), encoding, options)->image;
+ if (!image.empty()) {
+ outputVideo << image;
+ ROS_INFO_STREAM("Recording frame " << g_count << "\x1b[1F");
+ g_count++;
+ g_last_wrote_time = image_msg->header.stamp;
+ } else {
+ ROS_WARN("Frame skipped, no data!");
+ }
+ } catch(cv_bridge::Exception)
+ {
+ ROS_ERROR("Unable to convert %s image to %s", image_msg->encoding.c_str(), encoding.c_str());
+ return;
+ }
+}
+
+int main(int argc, char** argv)
+{
+ ros::init(argc, argv, "video_recorder", ros::init_options::AnonymousName);
+ ros::NodeHandle nh;
+ ros::NodeHandle local_nh("~");
+ local_nh.param("filename", filename, std::string("output.avi"));
+ bool stamped_filename;
+ local_nh.param("stamped_filename", stamped_filename, false);
+ local_nh.param("fps", fps, 15);
+ local_nh.param("codec", codec, std::string("MJPG"));
+ local_nh.param("encoding", encoding, std::string("bgr8"));
+ // cv_bridge::CvtColorForDisplayOptions
+ local_nh.param("min_depth_range", min_depth_range, 0.0);
+ local_nh.param("max_depth_range", max_depth_range, 0.0);
+ local_nh.param("use_dynamic_depth_range", use_dynamic_range, false);
+ local_nh.param("colormap", colormap, -1);
+
+ if (stamped_filename) {
+ std::size_t found = filename.find_last_of("/\\");
+ std::string path = filename.substr(0, found + 1);
+ std::string basename = filename.substr(found + 1);
+ std::stringstream ss;
+ ss << ros::Time::now().toNSec() << basename;
+ filename = path + ss.str();
+ ROS_INFO("Video recording to %s", filename.c_str());
+ }
+
+ if (codec.size() != 4) {
+ ROS_ERROR("The video codec must be a FOURCC identifier (4 chars)");
+ exit(-1);
+ }
+
+ image_transport::ImageTransport it(nh);
+ std::string topic = nh.resolveName("image");
+ image_transport::Subscriber sub_image = it.subscribe(topic, 1, callback);
+
+ ROS_INFO_STREAM("Waiting for topic " << topic << "...");
+ ros::spin();
+ std::cout << "\nVideo saved as " << filename << std::endl;
+}
diff --git a/stereo_image_proc/CHANGELOG.rst b/stereo_image_proc/CHANGELOG.rst
new file mode 100644
index 0000000..1d7d017
--- /dev/null
+++ b/stereo_image_proc/CHANGELOG.rst
@@ -0,0 +1,86 @@
+1.12.20 (2017-04-30)
+--------------------
+* fix doc jobs
+ This is a proper fix for `#233 <https://github.com/ros-perception/image_pipeline/issues/233>`_
+* address gcc6 build error
+ With gcc6, compiling fails with `stdlib.h: No such file or directory`,
+ as including '-isystem /usr/include' breaks with gcc6, cf.,
+ https://gcc.gnu.org/bugzilla/show_bug.cgi?id=70129.
+ This commit addresses this issue for this package in the same way
+ it was addressed in various other ROS packages. A list of related
+ commits and pull requests is at:
+ https://github.com/ros/rosdistro/issues/12783
+ Signed-off-by: Lukas Bulwahn <lukas.bulwahn at oss.bmw-carit.de>
+* Contributors: Lukas Bulwahn, Vincent Rabaud
+
+1.12.19 (2016-07-24)
+--------------------
+
+1.12.18 (2016-07-12)
+--------------------
+
+1.12.17 (2016-07-11)
+--------------------
+
+1.12.16 (2016-03-19)
+--------------------
+* clean OpenCV dependency in package.xml
+* Contributors: Vincent Rabaud
+
+1.12.15 (2016-01-17)
+--------------------
+* simplify OpenCV3 conversion
+* Contributors: Vincent Rabaud
+
+1.12.14 (2015-07-22)
+--------------------
+* add StereoSGBM and it can be chosen from dynamic_reconfigure
+* Contributors: Ryohei Ueda
+
+1.12.13 (2015-04-06)
+--------------------
+* get code to compile with OpenCV3
+* modify pointcloud data format of stereo_image_proc using point_cloud2_iterator
+* Contributors: Hiroaki Yaguchi, Vincent Rabaud
+
+1.12.12 (2014-12-31)
+--------------------
+
+1.12.11 (2014-10-26)
+--------------------
+
+1.12.10 (2014-09-28)
+--------------------
+
+1.12.9 (2014-09-21)
+-------------------
+* get code to compile with OpenCV3
+ fixes `#96 <https://github.com/ros-perception/image_pipeline/issues/96>`_
+* Contributors: Vincent Rabaud
+
+1.12.8 (2014-08-19)
+-------------------
+
+1.12.6 (2014-07-27)
+-------------------
+
+1.12.4 (2014-04-28)
+-------------------
+
+1.12.3 (2014-04-12)
+-------------------
+
+1.12.2 (2014-04-08)
+-------------------
+
+1.12.0 (2014-04-04)
+-------------------
+* remove PointCloud1 nodelets
+
+1.11.5 (2013-12-07 13:42:55 +0100)
+----------------------------------
+- fix compilation on OSX (#50)
+
+1.11.4 (2013-11-23 13:10:55 +0100)
+----------------------------------
+- convert images to MONO8 when computing disparity if needed (#49)
diff --git a/stereo_image_proc/CMakeLists.txt b/stereo_image_proc/CMakeLists.txt
new file mode 100644
index 0000000..e201436
--- /dev/null
+++ b/stereo_image_proc/CMakeLists.txt
@@ -0,0 +1,50 @@
+cmake_minimum_required(VERSION 2.8)
+project(stereo_image_proc)
+
+find_package(catkin REQUIRED cv_bridge dynamic_reconfigure image_geometry image_proc image_transport message_filters nodelet sensor_msgs stereo_msgs)
+find_package(Boost REQUIRED COMPONENTS thread)
+
+# Dynamic reconfigure support
+generate_dynamic_reconfigure_options(cfg/Disparity.cfg)
+
+catkin_package(
+ CATKIN_DEPENDS image_geometry image_proc sensor_msgs stereo_msgs
+ INCLUDE_DIRS include
+ LIBRARIES ${PROJECT_NAME}
+)
+
+include_directories(include)
+
+find_package(OpenCV REQUIRED)
+include_directories(${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS})
+
+# Nodelet library
+add_library(${PROJECT_NAME} src/libstereo_image_proc/processor.cpp src/nodelets/disparity.cpp src/nodelets/point_cloud2.cpp)
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}
+ ${OpenCV_LIBRARIES}
+)
+add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg)
+install(TARGETS ${PROJECT_NAME}
+ DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+)
+install(FILES nodelet_plugins.xml
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+# Standalone node
+add_executable(stereoimageproc_exe src/nodes/stereo_image_proc.cpp)
+target_link_libraries(stereoimageproc_exe stereo_image_proc)
+SET_TARGET_PROPERTIES(stereoimageproc_exe PROPERTIES OUTPUT_NAME stereo_image_proc)
+install(TARGETS stereoimageproc_exe
+ DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+# install the launch file
+install(DIRECTORY launch
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/
+)
+
+# install the include directory
+install(DIRECTORY include/${PROJECT_NAME}/
+ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+)
diff --git a/stereo_image_proc/cfg/Disparity.cfg b/stereo_image_proc/cfg/Disparity.cfg
new file mode 100755
index 0000000..4c502e7
--- /dev/null
+++ b/stereo_image_proc/cfg/Disparity.cfg
@@ -0,0 +1,39 @@
+#! /usr/bin/env python
+
+# Declare parameters that control stereo processing
+
+PACKAGE='stereo_image_proc'
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+stereo_algo_enum = gen.enum([gen.const("StereoBM", int_t, 0, "Block Matching"),
+ gen.const("StereoSGBM", int_t, 1, "SemiGlobal Block Matching")],
+ "stereo algorithm")
+gen.add("stereo_algorithm", int_t, 0, "sterel algorithm", 0, 0, 1,
+ edit_method = stereo_algo_enum)
+# disparity block matching pre-filtering parameters
+gen.add("prefilter_size", int_t, 0, "Normalization window size, pixels", 9, 5, 255)
+gen.add("prefilter_cap", int_t, 0, "Bound on normalized pixel values", 31, 1, 63)
+
+# disparity block matching correlation parameters
+gen.add("correlation_window_size", int_t, 0, "SAD correlation window width, pixels", 15, 5, 255)
+gen.add("min_disparity", int_t, 0, "Disparity to begin search at, pixels (may be negative)", 0, -128, 128)
+gen.add("disparity_range", int_t, 0, "Number of disparities to search, pixels", 64, 32, 256)
+# TODO What about trySmallerWindows?
+
+# disparity block matching post-filtering parameters
+# NOTE: Making uniqueness_ratio int_t instead of double_t to work around dynamic_reconfigure gui issue
+gen.add("uniqueness_ratio", double_t, 0, "Filter out if best match does not sufficiently exceed the next-best match", 15, 0, 100)
+gen.add("texture_threshold", int_t, 0, "Filter out if SAD window response does not exceed texture threshold", 10, 0, 10000)
+gen.add("speckle_size", int_t, 0, "Reject regions smaller than this size, pixels", 100, 0, 1000)
+gen.add("speckle_range", int_t, 0, "Max allowed difference between detected disparities", 4, 0, 31)
+gen.add("fullDP", bool_t, 0, "Run the full variant of the algorithm, only available in SGBM", False)
+gen.add("P1", double_t, 0, "The first parameter controlling the disparity smoothness, only available in SGBM", 200, 0, 4000)
+gen.add("P2", double_t, 0, "The second parameter controlling the disparity smoothness., only available in SGBM", 400, 0, 4000)
+gen.add("disp12MaxDiff", int_t, 0, "Maximum allowed difference (in integer pixel units) in the left-right disparity check, only available in SGBM", 0, 0, 128)
+# First string value is node name, used only for generating documentation
+# Second string value ("Disparity") is name of class and generated
+# .h file, with "Config" added, so class DisparityConfig
+exit(gen.generate(PACKAGE, "stereo_image_proc", "Disparity"))
diff --git a/stereo_image_proc/doc/mainpage.dox b/stereo_image_proc/doc/mainpage.dox
new file mode 100644
index 0000000..39b89a1
--- /dev/null
+++ b/stereo_image_proc/doc/mainpage.dox
@@ -0,0 +1,14 @@
+/**
+
+ at mainpage
+
+ at htmlinclude manifest.html
+
+ at 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
new file mode 100644
index 0000000..bac9f92
--- /dev/null
+++ b/stereo_image_proc/doc/stereo_frames.svg
@@ -0,0 +1,1052 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<!-- Created with Inkscape (http://www.inkscape.org/) -->
+
+<svg
+ xmlns:dc="http://purl.org/dc/elements/1.1/"
+ xmlns:cc="http://creativecommons.org/ns#"
+ xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
+ xmlns:svg="http://www.w3.org/2000/svg"
+ xmlns="http://www.w3.org/2000/svg"
+ xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
+ xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
+ sodipodi:docname="stereo_frames.svg"
+ inkscape:version="0.47pre4 r22446"
+ version="1.1"
+ id="svg2"
+ height="1052.3622047"
+ width="744.09448819">
+ <sodipodi:namedview
+ id="base"
+ pagecolor="#ffffff"
+ bordercolor="#666666"
+ borderopacity="1.0"
+ inkscape:pageopacity="1"
+ inkscape:pageshadow="2"
+ inkscape:zoom="0.90509668"
+ inkscape:cx="180.41471"
+ inkscape:cy="618.77172"
+ inkscape:document-units="px"
+ inkscape:current-layer="layer1"
+ showgrid="false"
+ inkscape:snap-grids="true"
+ inkscape:snap-to-guides="true"
+ inkscape:window-width="1448"
+ inkscape:window-height="1152"
+ inkscape:window-x="976"
+ inkscape:window-y="107"
+ inkscape:window-maximized="0">
+ <sodipodi:guide
+ position="0,0"
+ orientation="0,744.09448"
+ id="guide3683" />
+ <sodipodi:guide
+ position="744.09448,0"
+ orientation="-1052.3622,0"
+ id="guide3685" />
+ <sodipodi:guide
+ position="744.09448,1052.3622"
+ orientation="0,-744.09448"
+ id="guide3687" />
+ <sodipodi:guide
+ position="0,1052.3622"
+ orientation="1052.3622,0"
+ id="guide3689" />
+ <sodipodi:guide
+ position="0,0"
+ orientation="0,744.09448"
+ id="guide3691" />
+ <sodipodi:guide
+ position="744.09448,0"
+ orientation="-1052.3622,0"
+ id="guide3693" />
+ <sodipodi:guide
+ position="744.09448,1052.3622"
+ orientation="0,-744.09448"
+ id="guide3695" />
+ <sodipodi:guide
+ position="0,1052.3622"
+ orientation="1052.3622,0"
+ id="guide3697" />
+ </sodipodi:namedview>
+ <defs
+ id="defs4">
+ <marker
+ inkscape:stockid="Arrow2Mend"
+ orient="auto"
+ refY="0.0"
+ refX="0.0"
+ id="Arrow2Mend"
+ style="overflow:visible;">
+ <path
+ id="path3733"
+ style="font-size:12.0;fill-rule:evenodd;stroke-width:0.62500000;stroke-linejoin:round;"
+ d="M 8.7185878,4.0337352 L -2.2072895,0.016013256 L 8.7185884,-4.0017078 C 6.9730900,-1.6296469 6.9831476,1.6157441 8.7185878,4.0337352 z "
+ transform="scale(0.6) rotate(180) translate(0,0)" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow1Lend"
+ orient="auto"
+ refY="0.0"
+ refX="0.0"
+ id="Arrow1Lend"
+ style="overflow:visible;">
+ <path
+ id="path3709"
+ d="M 0.0,0.0 L 5.0,-5.0 L -12.5,0.0 L 5.0,5.0 L 0.0,0.0 z "
+ style="fill-rule:evenodd;stroke:#000000;stroke-width:1.0pt;marker-start:none;"
+ transform="scale(0.8) rotate(180) translate(12.5,0)" />
+ </marker>
+ <inkscape:perspective
+ sodipodi:type="inkscape:persp3d"
+ inkscape:vp_x="48.535565 : 28.02202 : 0"
+ inkscape:vp_y="-19.704059 : 11.376144 : 0"
+ inkscape:vp_z="5.7301869e-16 : 9.3584146 : 0"
+ inkscape:persp3d-origin="156.5601 : 640.75245 : 1"
+ id="perspective10" />
+ <inkscape:perspective
+ id="perspective3614"
+ inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
+ inkscape:vp_z="1 : 0.5 : 1"
+ inkscape:vp_y="0 : 1000 : 0"
+ inkscape:vp_x="0 : 0.5 : 1"
+ sodipodi:type="inkscape:persp3d" />
+ <inkscape:perspective
+ id="perspective3638"
+ inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
+ inkscape:vp_z="1 : 0.5 : 1"
+ inkscape:vp_y="0 : 1000 : 0"
+ inkscape:vp_x="0 : 0.5 : 1"
+ sodipodi:type="inkscape:persp3d" />
+ <inkscape:perspective
+ id="perspective3660"
+ inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
+ inkscape:vp_z="1 : 0.5 : 1"
+ inkscape:vp_y="0 : 1000 : 0"
+ inkscape:vp_x="0 : 0.5 : 1"
+ sodipodi:type="inkscape:persp3d" />
+ <marker
+ inkscape:stockid="Arrow1Lend2"
+ orient="auto"
+ refY="0.0"
+ refX="0.0"
+ id="Arrow1Lend2"
+ style="overflow:visible;">
+ <path
+ id="path4160"
+ d="M 0.0,0.0 L 5.0,-5.0 L -12.5,0.0 L 5.0,5.0 L 0.0,0.0 z "
+ style="marker-start:none;stroke:#ff0000;stroke-width:1.0pt;fill:#ff0000;fill-rule:evenodd"
+ transform="scale(0.8) rotate(180) translate(12.5,0)" />
+ </marker>
+ <inkscape:perspective
+ sodipodi:type="inkscape:persp3d"
+ inkscape:vp_x="0 : 0.5 : 1"
+ inkscape:vp_y="0 : 1000 : 0"
+ inkscape:vp_z="1 : 0.5 : 1"
+ inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
+ id="perspective4232" />
+ <marker
+ inkscape:stockid="Arrow1Lend2"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="Arrow1Lend2-3"
+ style="overflow:visible">
+ <path
+ id="path4160-4"
+ d="M 0,0 5,-5 -12.5,0 5,5 0,0 z"
+ style="fill:#ff0000;fill-rule:evenodd;stroke:#ff0000;stroke-width:1pt;marker-start:none"
+ transform="matrix(-0.8,0,0,-0.8,-10,0)" />
+ </marker>
+ <inkscape:perspective
+ sodipodi:type="inkscape:persp3d"
+ inkscape:vp_x="0 : 0.5 : 1"
+ inkscape:vp_y="0 : 1000 : 0"
+ inkscape:vp_z="1 : 0.5 : 1"
+ inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
+ id="perspective4260" />
+ <marker
+ inkscape:stockid="Arrow1Lend2"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="Arrow1Lend2-5"
+ style="overflow:visible">
+ <path
+ id="path4160-1"
+ d="M 0,0 5,-5 -12.5,0 5,5 0,0 z"
+ style="fill:#ff0000;fill-rule:evenodd;stroke:#ff0000;stroke-width:1pt;marker-start:none"
+ transform="matrix(-0.8,0,0,-0.8,-10,0)" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow1Lend2U"
+ orient="auto"
+ refY="0.0"
+ refX="0.0"
+ id="Arrow1Lend2U"
+ style="overflow:visible;">
+ <path
+ id="path4306"
+ d="M 0.0,0.0 L 5.0,-5.0 L -12.5,0.0 L 5.0,5.0 L 0.0,0.0 z "
+ style="fill-rule:evenodd;marker-start:none;stroke:#00ff00;stroke-width:1.0pt;fill:#00ff00"
+ transform="scale(0.8) rotate(180) translate(12.5,0)" />
+ </marker>
+ <inkscape:perspective
+ id="perspective4379"
+ inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
+ inkscape:vp_z="1 : 0.5 : 1"
+ inkscape:vp_y="0 : 1000 : 0"
+ inkscape:vp_x="0 : 0.5 : 1"
+ sodipodi:type="inkscape:persp3d" />
+ <marker
+ inkscape:stockid="Arrow1Lend2"
+ orient="auto"
+ refY="0"
+ refX="0"
+ id="Arrow1Lend2-8"
+ style="overflow:visible">
+ <path
+ id="path4160-7"
+ d="M 0,0 5,-5 -12.5,0 5,5 0,0 z"
+ style="fill:#ff0000;fill-rule:evenodd;stroke:#ff0000;stroke-width:1pt;marker-start:none"
+ transform="matrix(-0.8,0,0,-0.8,-10,0)" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow1Lend2W"
+ orient="auto"
+ refY="0.0"
+ refX="0.0"
+ id="Arrow1Lend2W"
+ style="overflow:visible;">
+ <path
+ id="path4430"
+ d="M 0.0,0.0 L 5.0,-5.0 L -12.5,0.0 L 5.0,5.0 L 0.0,0.0 z "
+ style="fill-rule:evenodd;marker-start:none;stroke:#00ff00;stroke-width:1.0pt;fill:#00ff00"
+ transform="scale(0.8) rotate(180) translate(12.5,0)" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow1Lend2Uq"
+ orient="auto"
+ refY="0.0"
+ refX="0.0"
+ id="Arrow1Lend2Uq"
+ style="overflow:visible;">
+ <path
+ id="path4433"
+ d="M 0.0,0.0 L 5.0,-5.0 L -12.5,0.0 L 5.0,5.0 L 0.0,0.0 z "
+ style="marker-start:none;stroke:#ff0000;stroke-width:1.0pt;fill:#ff0000;fill-rule:evenodd"
+ transform="scale(0.8) rotate(180) translate(12.5,0)" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow1Lend2T"
+ orient="auto"
+ refY="0.0"
+ refX="0.0"
+ id="Arrow1Lend2T"
+ style="overflow:visible;">
+ <path
+ id="path4436"
+ d="M 0.0,0.0 L 5.0,-5.0 L -12.5,0.0 L 5.0,5.0 L 0.0,0.0 z "
+ style="fill-rule:evenodd;marker-start:none;stroke:#0000ff;stroke-width:1.0pt;fill:#0000ff"
+ transform="scale(0.8) rotate(180) translate(12.5,0)" />
+ </marker>
+ <inkscape:perspective
+ sodipodi:type="inkscape:persp3d"
+ inkscape:vp_x="0 : 0.5 : 1"
+ inkscape:vp_y="0 : 1000 : 0"
+ inkscape:vp_z="1 : 0.5 : 1"
+ inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
+ id="perspective6100" />
+ <inkscape:perspective
+ sodipodi:type="inkscape:persp3d"
+ inkscape:vp_x="0 : 0.5 : 1"
+ inkscape:vp_y="0 : 1000 : 0"
+ inkscape:vp_z="1 : 0.5 : 1"
+ inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
+ id="perspective6125" />
+ <inkscape:perspective
+ sodipodi:type="inkscape:persp3d"
+ inkscape:vp_x="0 : 0.5 : 1"
+ inkscape:vp_y="0 : 1000 : 0"
+ inkscape:vp_z="1 : 0.5 : 1"
+ inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
+ id="perspective6154" />
+ <inkscape:perspective
+ sodipodi:type="inkscape:persp3d"
+ inkscape:vp_x="0 : 0.5 : 1"
+ inkscape:vp_y="0 : 1000 : 0"
+ inkscape:vp_z="1 : 0.5 : 1"
+ inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
+ id="perspective6176" />
+ <inkscape:perspective
+ sodipodi:type="inkscape:persp3d"
+ inkscape:vp_x="0 : 0.5 : 1"
+ inkscape:vp_y="0 : 1000 : 0"
+ inkscape:vp_z="1 : 0.5 : 1"
+ inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
+ id="perspective6176-1" />
+ <inkscape:perspective
+ sodipodi:type="inkscape:persp3d"
+ inkscape:vp_x="0 : 0.5 : 1"
+ inkscape:vp_y="0 : 1000 : 0"
+ inkscape:vp_z="1 : 0.5 : 1"
+ inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
+ id="perspective6176-2" />
+ <inkscape:perspective
+ sodipodi:type="inkscape:persp3d"
+ inkscape:vp_x="0 : 0.5 : 1"
+ inkscape:vp_y="0 : 1000 : 0"
+ inkscape:vp_z="1 : 0.5 : 1"
+ inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
+ id="perspective6176-9" />
+ <inkscape:perspective
+ sodipodi:type="inkscape:persp3d"
+ inkscape:vp_x="0 : 0.5 : 1"
+ inkscape:vp_y="0 : 1000 : 0"
+ inkscape:vp_z="1 : 0.5 : 1"
+ inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
+ id="perspective6176-4" />
+ <inkscape:perspective
+ sodipodi:type="inkscape:persp3d"
+ inkscape:vp_x="0 : 0.5 : 1"
+ inkscape:vp_y="0 : 1000 : 0"
+ inkscape:vp_z="1 : 0.5 : 1"
+ inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
+ id="perspective6176-6" />
+ <inkscape:perspective
+ sodipodi:type="inkscape:persp3d"
+ inkscape:vp_x="0 : 0.5 : 1"
+ inkscape:vp_y="0 : 1000 : 0"
+ inkscape:vp_z="1 : 0.5 : 1"
+ inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
+ id="perspective6176-0" />
+ <inkscape:perspective
+ sodipodi:type="inkscape:persp3d"
+ inkscape:vp_x="0 : 0.5 : 1"
+ inkscape:vp_y="0 : 1000 : 0"
+ inkscape:vp_z="1 : 0.5 : 1"
+ inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
+ id="perspective6176-5" />
+ <inkscape:perspective
+ sodipodi:type="inkscape:persp3d"
+ inkscape:vp_x="0 : 0.5 : 1"
+ inkscape:vp_y="0 : 1000 : 0"
+ inkscape:vp_z="1 : 0.5 : 1"
+ inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
+ id="perspective6176-8" />
+ <inkscape:perspective
+ sodipodi:type="inkscape:persp3d"
+ inkscape:vp_x="0 : 0.5 : 1"
+ inkscape:vp_y="0 : 1000 : 0"
+ inkscape:vp_z="1 : 0.5 : 1"
+ inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
+ id="perspective6176-7" />
+ <inkscape:perspective
+ sodipodi:type="inkscape:persp3d"
+ inkscape:vp_x="0 : 0.5 : 1"
+ inkscape:vp_y="0 : 1000 : 0"
+ inkscape:vp_z="1 : 0.5 : 1"
+ inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
+ id="perspective6176-3" />
+ <inkscape:perspective
+ sodipodi:type="inkscape:persp3d"
+ inkscape:vp_x="0 : 0.5 : 1"
+ inkscape:vp_y="0 : 1000 : 0"
+ inkscape:vp_z="1 : 0.5 : 1"
+ inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
+ id="perspective6176-04" />
+ <inkscape:perspective
+ sodipodi:type="inkscape:persp3d"
+ inkscape:vp_x="0 : 0.5 : 1"
+ inkscape:vp_y="0 : 1000 : 0"
+ inkscape:vp_z="1 : 0.5 : 1"
+ inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
+ id="perspective6176-31" />
+ <inkscape:perspective
+ sodipodi:type="inkscape:persp3d"
+ inkscape:vp_x="0 : 0.5 : 1"
+ inkscape:vp_y="0 : 1000 : 0"
+ inkscape:vp_z="1 : 0.5 : 1"
+ inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
+ id="perspective6176-00" />
+ <inkscape:perspective
+ sodipodi:type="inkscape:persp3d"
+ inkscape:vp_x="0 : 0.5 : 1"
+ inkscape:vp_y="0 : 1000 : 0"
+ inkscape:vp_z="1 : 0.5 : 1"
+ inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
+ id="perspective6176-75" />
+ <inkscape:perspective
+ sodipodi:type="inkscape:persp3d"
+ inkscape:vp_x="0 : 0.5 : 1"
+ inkscape:vp_y="0 : 1000 : 0"
+ inkscape:vp_z="1 : 0.5 : 1"
+ inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
+ id="perspective6176-84" />
+ <inkscape:perspective
+ sodipodi:type="inkscape:persp3d"
+ inkscape:vp_x="0 : 0.5 : 1"
+ inkscape:vp_y="0 : 1000 : 0"
+ inkscape:vp_z="1 : 0.5 : 1"
+ inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
+ id="perspective6176-78" />
+ <inkscape:perspective
+ sodipodi:type="inkscape:persp3d"
+ inkscape:vp_x="0 : 0.5 : 1"
+ inkscape:vp_y="0 : 1000 : 0"
+ inkscape:vp_z="1 : 0.5 : 1"
+ inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
+ id="perspective6176-12" />
+ <inkscape:perspective
+ sodipodi:type="inkscape:persp3d"
+ inkscape:vp_x="0 : 0.5 : 1"
+ inkscape:vp_y="0 : 1000 : 0"
+ inkscape:vp_z="1 : 0.5 : 1"
+ inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
+ id="perspective6176-09" />
+ <inkscape:perspective
+ sodipodi:type="inkscape:persp3d"
+ inkscape:vp_x="0 : 0.5 : 1"
+ inkscape:vp_y="0 : 1000 : 0"
+ inkscape:vp_z="1 : 0.5 : 1"
+ inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
+ id="perspective6176-85" />
+ <inkscape:perspective
+ sodipodi:type="inkscape:persp3d"
+ inkscape:vp_x="0 : 0.5 : 1"
+ inkscape:vp_y="0 : 1000 : 0"
+ inkscape:vp_z="1 : 0.5 : 1"
+ inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
+ id="perspective6176-096" />
+ <inkscape:perspective
+ sodipodi:type="inkscape:persp3d"
+ inkscape:vp_x="0 : 0.5 : 1"
+ inkscape:vp_y="0 : 1000 : 0"
+ inkscape:vp_z="1 : 0.5 : 1"
+ inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
+ id="perspective6380" />
+ <inkscape:perspective
+ sodipodi:type="inkscape:persp3d"
+ inkscape:vp_x="0 : 0.5 : 1"
+ inkscape:vp_y="0 : 1000 : 0"
+ inkscape:vp_z="1 : 0.5 : 1"
+ inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
+ id="perspective6405" />
+ <inkscape:perspective
+ sodipodi:type="inkscape:persp3d"
+ inkscape:vp_x="0 : 0.5 : 1"
+ inkscape:vp_y="0 : 1000 : 0"
+ inkscape:vp_z="1 : 0.5 : 1"
+ inkscape:persp3d-origin="0.5 : 0.33333333 : 1"
+ id="perspective6430" />
+ <marker
+ inkscape:stockid="Arrow1Lend2Wh"
+ orient="auto"
+ refY="0.0"
+ refX="0.0"
+ id="Arrow1Lend2Wh"
+ style="overflow:visible;">
+ <path
+ id="path6517"
+ d="M 0.0,0.0 L 5.0,-5.0 L -12.5,0.0 L 5.0,5.0 L 0.0,0.0 z "
+ style="marker-start:none;stroke:#008c00;stroke-width:1.0pt;fill:#008c00;fill-rule:evenodd"
+ transform="scale(0.8) rotate(180) translate(12.5,0)" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow1Lend2Whr"
+ orient="auto"
+ refY="0.0"
+ refX="0.0"
+ id="Arrow1Lend2Whr"
+ style="overflow:visible;">
+ <path
+ id="path8029"
+ d="M 0.0,0.0 L 5.0,-5.0 L -12.5,0.0 L 5.0,5.0 L 0.0,0.0 z "
+ style="fill-rule:evenodd;marker-start:none;stroke:#145914;stroke-width:1.0pt;fill:#145914"
+ transform="scale(0.8) rotate(180) translate(12.5,0)" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow1Lend2WhrR"
+ orient="auto"
+ refY="0.0"
+ refX="0.0"
+ id="Arrow1Lend2WhrR"
+ style="overflow:visible;">
+ <path
+ id="path8280"
+ d="M 0.0,0.0 L 5.0,-5.0 L -12.5,0.0 L 5.0,5.0 L 0.0,0.0 z "
+ style="marker-start:none;stroke:#146f14;stroke-width:1.0pt;fill:#146f14;fill-rule:evenodd"
+ transform="scale(0.8) rotate(180) translate(12.5,0)" />
+ </marker>
+ </defs>
+ <metadata
+ id="metadata7">
+ <rdf:RDF>
+ <cc:Work
+ rdf:about="">
+ <dc:format>image/svg+xml</dc:format>
+ <dc:type
+ rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
+ <dc:title></dc:title>
+ </cc:Work>
+ </rdf:RDF>
+ </metadata>
+ <g
+ inkscape:label="Layer 1"
+ inkscape:groupmode="layer"
+ id="layer1">
+ <g
+ sodipodi:type="inkscape:box3d"
+ style="fill:none;stroke:#000000;stroke-width:1;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;stroke-dashoffset:0"
+ id="g2816"
+ inkscape:perspectiveID="#perspective10"
+ inkscape:corner0="2.8178928 : 0.8637518 : 0 : 1"
+ inkscape:corner7="0.55750066 : -0.059673562 : 4.6741689 : 1"
+ inkscape:export-filename="/wg/arc/vpradeep/ros-64/pkgs-trunk/image_pipeline/stereo_image_proc/docs/stereo_frames.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90">
+ <path
+ sodipodi:type="inkscape:box3dside"
+ id="path2822"
+ style="fill:#8686bf;fill-rule:evenodd;stroke:#000000;stroke-width:1;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none"
+ inkscape:box3dsidetype="3"
+ d="m 276.3087,322.82052 -109.70941,63.34075 18.19523,10.50502 109.70941,-63.34075 z" />
+ <path
+ sodipodi:type="inkscape:box3dside"
+ id="path2818"
+ style="fill:#353564;fill-rule:evenodd;stroke:#000000;stroke-width:1;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none"
+ inkscape:box3dsidetype="6"
+ d="m 276.3087,322.82052 18.19523,10.50502 0,-43.74281 -18.19523,-10.50502 z" />
+ <path
+ sodipodi:type="inkscape:box3dside"
+ id="path2820"
+ style="fill:#4d4d9f;fill-rule:evenodd;stroke:#000000;stroke-width:1;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none"
+ inkscape:box3dsidetype="5"
+ d="m 276.3087,322.82052 -109.70941,63.34075 0,-43.74281 109.70941,-63.34075 z" />
+ <path
+ sodipodi:type="inkscape:box3dside"
+ id="path2826"
+ style="fill:#afafde;fill-rule:evenodd;stroke:#000000;stroke-width:1;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none"
+ inkscape:box3dsidetype="13"
+ d="m 294.50393,333.32554 -109.70941,63.34075 0,-43.74281 109.70941,-63.34075 z" />
+ <path
+ sodipodi:type="inkscape:box3dside"
+ id="path2824"
+ style="fill:#d7d7ff;fill-rule:evenodd;stroke:#000000;stroke-width:1;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none"
+ inkscape:box3dsidetype="14"
+ d="m 166.59929,386.16127 18.19523,10.50502 0,-43.74281 -18.19523,-10.50502 z" />
+ <path
+ sodipodi:type="inkscape:box3dside"
+ id="path2828"
+ style="fill:#e9e9ff;fill-rule:evenodd;stroke:#000000;stroke-width:1;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none"
+ inkscape:box3dsidetype="11"
+ d="m 276.3087,279.07771 -109.70941,63.34075 18.19523,10.50502 109.70941,-63.34075 z" />
+ </g>
+ <path
+ sodipodi:type="arc"
+ style="fill:none;stroke:#000000;stroke-width:4.91616058;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;stroke-dashoffset:0"
+ id="path2832"
+ sodipodi:cx="330.35144"
+ sodipodi:cy="506.56415"
+ sodipodi:rx="36.460194"
+ sodipodi:ry="36.460194"
+ d="m 301.02132,528.22262 a 36.460194,36.460194 0 0 1 56.93665,-45.47496"
+ transform="matrix(0.20341077,-0.11743871,0,0.20341077,203.32922,260.48884)"
+ sodipodi:start="2.5055324"
+ sodipodi:end="5.5713574"
+ sodipodi:open="true"
+ inkscape:export-filename="/wg/arc/vpradeep/ros-64/pkgs-trunk/image_pipeline/stereo_image_proc/docs/stereo_frames.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90" />
+ <path
+ sodipodi:type="arc"
+ style="fill:#969696;fill-opacity:1;stroke:#000000;stroke-width:4.91616058;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;stroke-dashoffset:0"
+ id="path2832-0"
+ sodipodi:cx="330.35144"
+ sodipodi:cy="506.56415"
+ sodipodi:rx="36.460194"
+ sodipodi:ry="36.460194"
+ d="m 366.81163,506.56415 a 36.460194,36.460194 0 1 1 -72.92038,0 36.460194,36.460194 0 1 1 72.92038,0 z"
+ transform="matrix(0.20341077,-0.11743871,0,0.20341077,216.01427,267.97508)"
+ inkscape:export-filename="/wg/arc/vpradeep/ros-64/pkgs-trunk/image_pipeline/stereo_image_proc/docs/stereo_frames.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90" />
+ <path
+ style="fill:none;stroke:#000000;stroke-width:1;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none"
+ d="m 275.72505,316.5193 13.34955,7.70736"
+ id="path3628"
+ inkscape:export-filename="/wg/arc/vpradeep/ros-64/pkgs-trunk/image_pipeline/stereo_image_proc/docs/stereo_frames.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90" />
+ <path
+ style="fill:none;stroke:#000000;stroke-width:1;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none"
+ d="m 264.68329,332.654 13.34956,7.70737"
+ id="path3628-5"
+ inkscape:export-filename="/wg/arc/vpradeep/ros-64/pkgs-trunk/image_pipeline/stereo_image_proc/docs/stereo_frames.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90" />
+ <path
+ sodipodi:type="arc"
+ style="fill:none;stroke:#000000;stroke-width:4.91616058;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;stroke-dashoffset:0"
+ id="path2832-7"
+ sodipodi:cx="330.35144"
+ sodipodi:cy="506.56415"
+ sodipodi:rx="36.460194"
+ sodipodi:ry="36.460194"
+ d="m 301.02132,528.22262 a 36.460194,36.460194 0 0 1 56.93665,-45.47496"
+ transform="matrix(0.20341077,-0.11743871,0,0.20341077,143.47395,295.98382)"
+ sodipodi:start="2.5055324"
+ sodipodi:end="5.5713574"
+ sodipodi:open="true"
+ inkscape:export-filename="/wg/arc/vpradeep/ros-64/pkgs-trunk/image_pipeline/stereo_image_proc/docs/stereo_frames.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90" />
+ <path
+ sodipodi:type="arc"
+ style="fill:#969696;fill-opacity:1;stroke:#000000;stroke-width:4.91616058;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;stroke-dashoffset:0"
+ id="path2832-0-0"
+ sodipodi:cx="330.35144"
+ sodipodi:cy="506.56415"
+ sodipodi:rx="36.460194"
+ sodipodi:ry="36.460194"
+ d="m 366.81163,506.56415 a 36.460194,36.460194 0 1 1 -72.92038,0 36.460194,36.460194 0 1 1 72.92038,0 z"
+ transform="matrix(0.20341077,-0.11743871,0,0.20341077,156.15901,303.47006)"
+ inkscape:export-filename="/wg/arc/vpradeep/ros-64/pkgs-trunk/image_pipeline/stereo_image_proc/docs/stereo_frames.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90" />
+ <path
+ style="fill:none;stroke:#000000;stroke-width:1;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none"
+ d="m 215.86979,352.01428 13.34955,7.70737"
+ id="path3628-1"
+ inkscape:export-filename="/wg/arc/vpradeep/ros-64/pkgs-trunk/image_pipeline/stereo_image_proc/docs/stereo_frames.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90" />
+ <path
+ style="fill:none;stroke:#000000;stroke-width:1;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none"
+ d="m 204.82803,368.14898 13.34955,7.70737"
+ id="path3628-5-1"
+ inkscape:export-filename="/wg/arc/vpradeep/ros-64/pkgs-trunk/image_pipeline/stereo_image_proc/docs/stereo_frames.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90" />
+ <path
+ style="fill:none;stroke:#0000ff;stroke-width:1;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;marker-end:url(#Arrow1Lend2T)"
+ d="m 284.17077,333.01784 29.0667,16.78166"
+ id="path3701"
+ inkscape:export-filename="/wg/arc/vpradeep/ros-64/pkgs-trunk/image_pipeline/stereo_image_proc/docs/stereo_frames.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90" />
+ <path
+ style="fill:none;stroke:#ff0000;stroke-width:1;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;marker-end:url(#Arrow1Lend2Uq)"
+ d="m 285.22266,333.20729 -29.0667,16.78166"
+ id="path3701-2"
+ inkscape:export-filename="/wg/arc/vpradeep/ros-64/pkgs-trunk/image_pipeline/stereo_image_proc/docs/stereo_frames.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90" />
+ <path
+ style="stroke-linejoin:miter;marker-end:url(#Arrow1Lend2WhrR);stroke-opacity:1;stroke:#146f14;stroke-linecap:butt;stroke-miterlimit:4;stroke-dasharray:none;stroke-width:1;fill:none"
+ d="m 284.93511,333.61469 10e-6,33.999"
+ id="path3701-3"
+ inkscape:export-filename="/wg/arc/vpradeep/ros-64/pkgs-trunk/image_pipeline/stereo_image_proc/docs/stereo_frames.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90" />
+ <text
+ id="text6088"
+ y="358.95593"
+ x="244.14062"
+ style="font-size:40px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;font-family:arial;-inkscape-font-specification:arial"
+ xml:space="preserve"
+ inkscape:export-filename="/wg/arc/vpradeep/ros-64/pkgs-trunk/image_pipeline/stereo_image_proc/docs/stereo_frames.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90"><tspan
+ style="font-size:16px"
+ y="358.95593"
+ x="244.14062"
+ id="tspan6090"
+ sodipodi:role="line">X</tspan></text>
+ <text
+ id="text6088-7"
+ y="383.92856"
+ x="279.78824"
+ style="font-size:40px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;font-family:arial;-inkscape-font-specification:arial"
+ xml:space="preserve"
+ inkscape:export-filename="/wg/arc/vpradeep/ros-64/pkgs-trunk/image_pipeline/stereo_image_proc/docs/stereo_frames.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90"><tspan
+ style="font-size:16px"
+ y="383.92856"
+ x="279.78824"
+ id="tspan6090-7"
+ sodipodi:role="line">Y</tspan></text>
+ <text
+ id="text6088-7-5"
+ y="348.25049"
+ x="318.85449"
+ style="font-size:40px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;font-family:arial;-inkscape-font-specification:arial"
+ xml:space="preserve"
+ inkscape:export-filename="/wg/arc/vpradeep/ros-64/pkgs-trunk/image_pipeline/stereo_image_proc/docs/stereo_frames.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90"><tspan
+ style="font-size:16px"
+ y="348.25049"
+ x="318.85449"
+ id="tspan6090-7-5"
+ sodipodi:role="line">Z</tspan></text>
+ <path
+ transform="matrix(0.10083494,0,0,0.10083494,332.84897,346.72344)"
+ d="m 357.03125,454.31531 a 23.828125,23.828125 0 1 1 -47.65625,0 23.828125,23.828125 0 1 1 47.65625,0 z"
+ sodipodi:ry="23.828125"
+ sodipodi:rx="23.828125"
+ sodipodi:cy="454.31531"
+ sodipodi:cx="333.20312"
+ id="path6144"
+ style="fill:#0000ff;fill-opacity:1;stroke:#000000;stroke-width:9.91719723;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;stroke-dashoffset:0"
+ sodipodi:type="arc"
+ inkscape:export-filename="/wg/arc/vpradeep/ros-64/pkgs-trunk/image_pipeline/stereo_image_proc/docs/stereo_frames.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90" />
+ <path
+ transform="matrix(0.10083494,0,0,0.10083494,332.34729,371.2285)"
+ d="m 357.03125,454.31531 a 23.828125,23.828125 0 1 1 -47.65625,0 23.828125,23.828125 0 1 1 47.65625,0 z"
+ sodipodi:ry="23.828125"
+ sodipodi:rx="23.828125"
+ sodipodi:cy="454.31531"
+ sodipodi:cx="333.20312"
+ id="path6144-3"
+ style="fill:#0000ff;fill-opacity:1;stroke:#000000;stroke-width:9.91719723;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;stroke-dashoffset:0"
+ sodipodi:type="arc"
+ inkscape:export-filename="/wg/arc/vpradeep/ros-64/pkgs-trunk/image_pipeline/stereo_image_proc/docs/stereo_frames.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90" />
+ <path
+ transform="matrix(0.10083494,0,0,0.10083494,325.31604,420.44725)"
+ d="m 357.03125,454.31531 a 23.828125,23.828125 0 1 1 -47.65625,0 23.828125,23.828125 0 1 1 47.65625,0 z"
+ sodipodi:ry="23.828125"
+ sodipodi:rx="23.828125"
+ sodipodi:cy="454.31531"
+ sodipodi:cx="333.20312"
+ id="path6144-3-6"
+ style="fill:#0000ff;fill-opacity:1;stroke:#000000;stroke-width:9.91719723;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;stroke-dashoffset:0"
+ sodipodi:type="arc"
+ inkscape:export-filename="/wg/arc/vpradeep/ros-64/pkgs-trunk/image_pipeline/stereo_image_proc/docs/stereo_frames.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90" />
+ <path
+ transform="matrix(0.10083494,0,0,0.10083494,319.06604,458.7285)"
+ d="m 357.03125,454.31531 a 23.828125,23.828125 0 1 1 -47.65625,0 23.828125,23.828125 0 1 1 47.65625,0 z"
+ sodipodi:ry="23.828125"
+ sodipodi:rx="23.828125"
+ sodipodi:cy="454.31531"
+ sodipodi:cx="333.20312"
+ id="path6144-3-2"
+ style="fill:#0000ff;fill-opacity:1;stroke:#000000;stroke-width:9.91719723;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;stroke-dashoffset:0"
+ sodipodi:type="arc"
+ inkscape:export-filename="/wg/arc/vpradeep/ros-64/pkgs-trunk/image_pipeline/stereo_image_proc/docs/stereo_frames.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90" />
+ <path
+ transform="matrix(0.10083494,0,0,0.10083494,296.40979,416.541)"
+ d="m 357.03125,454.31531 a 23.828125,23.828125 0 1 1 -47.65625,0 23.828125,23.828125 0 1 1 47.65625,0 z"
+ sodipodi:ry="23.828125"
+ sodipodi:rx="23.828125"
+ sodipodi:cy="454.31531"
+ sodipodi:cx="333.20312"
+ id="path6144-3-0"
+ style="fill:#0000ff;fill-opacity:1;stroke:#000000;stroke-width:9.91719723;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;stroke-dashoffset:0"
+ sodipodi:type="arc"
+ inkscape:export-filename="/wg/arc/vpradeep/ros-64/pkgs-trunk/image_pipeline/stereo_image_proc/docs/stereo_frames.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90" />
+ <path
+ transform="matrix(0.10083494,0,0,0.10083494,303.23236,385.93822)"
+ d="m 357.03125,454.31531 a 23.828125,23.828125 0 1 1 -47.65625,0 23.828125,23.828125 0 1 1 47.65625,0 z"
+ sodipodi:ry="23.828125"
+ sodipodi:rx="23.828125"
+ sodipodi:cy="454.31531"
+ sodipodi:cx="333.20312"
+ id="path6144-3-4"
+ style="fill:#0000ff;fill-opacity:1;stroke:#000000;stroke-width:9.91719723;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;stroke-dashoffset:0"
+ sodipodi:type="arc"
+ inkscape:export-filename="/wg/arc/vpradeep/ros-64/pkgs-trunk/image_pipeline/stereo_image_proc/docs/stereo_frames.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90" />
+ <path
+ transform="matrix(0.10083494,0,0,0.10083494,294.06604,404.82225)"
+ d="m 357.03125,454.31531 a 23.828125,23.828125 0 1 1 -47.65625,0 23.828125,23.828125 0 1 1 47.65625,0 z"
+ sodipodi:ry="23.828125"
+ sodipodi:rx="23.828125"
+ sodipodi:cy="454.31531"
+ sodipodi:cx="333.20312"
+ id="path6144-3-3"
+ style="fill:#0000ff;fill-opacity:1;stroke:#000000;stroke-width:9.91719723;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;stroke-dashoffset:0"
+ sodipodi:type="arc"
+ inkscape:export-filename="/wg/arc/vpradeep/ros-64/pkgs-trunk/image_pipeline/stereo_image_proc/docs/stereo_frames.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90" />
+ <path
+ transform="matrix(0.10083494,0,0,0.10083494,337.03479,396.2285)"
+ d="m 357.03125,454.31531 a 23.828125,23.828125 0 1 1 -47.65625,0 23.828125,23.828125 0 1 1 47.65625,0 z"
+ sodipodi:ry="23.828125"
+ sodipodi:rx="23.828125"
+ sodipodi:cy="454.31531"
+ sodipodi:cx="333.20312"
+ id="path6144-3-8"
+ style="fill:#0000ff;fill-opacity:1;stroke:#000000;stroke-width:9.91719723;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;stroke-dashoffset:0"
+ sodipodi:type="arc"
+ inkscape:export-filename="/wg/arc/vpradeep/ros-64/pkgs-trunk/image_pipeline/stereo_image_proc/docs/stereo_frames.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90" />
+ <path
+ transform="matrix(0.10083494,0,0,0.10083494,315.15979,367.07225)"
+ d="m 357.03125,454.31531 a 23.828125,23.828125 0 1 1 -47.65625,0 23.828125,23.828125 0 1 1 47.65625,0 z"
+ sodipodi:ry="23.828125"
+ sodipodi:rx="23.828125"
+ sodipodi:cy="454.31531"
+ sodipodi:cx="333.20312"
+ id="path6144-3-7"
+ style="fill:#0000ff;fill-opacity:1;stroke:#000000;stroke-width:9.91719723;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;stroke-dashoffset:0"
+ sodipodi:type="arc"
+ inkscape:export-filename="/wg/arc/vpradeep/ros-64/pkgs-trunk/image_pipeline/stereo_image_proc/docs/stereo_frames.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90" />
+ <path
+ transform="matrix(0.10083494,0,0,0.10083494,275.31604,362.63475)"
+ d="m 357.03125,454.31531 a 23.828125,23.828125 0 1 1 -47.65625,0 23.828125,23.828125 0 1 1 47.65625,0 z"
+ sodipodi:ry="23.828125"
+ sodipodi:rx="23.828125"
+ sodipodi:cy="454.31531"
+ sodipodi:cx="333.20312"
+ id="path6144-3-61"
+ style="fill:#0000ff;fill-opacity:1;stroke:#000000;stroke-width:9.91719723;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;stroke-dashoffset:0"
+ sodipodi:type="arc"
+ inkscape:export-filename="/wg/arc/vpradeep/ros-64/pkgs-trunk/image_pipeline/stereo_image_proc/docs/stereo_frames.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90" />
+ <path
+ transform="matrix(0.10083494,0,0,0.10083494,269.84729,399.3535)"
+ d="m 357.03125,454.31531 a 23.828125,23.828125 0 1 1 -47.65625,0 23.828125,23.828125 0 1 1 47.65625,0 z"
+ sodipodi:ry="23.828125"
+ sodipodi:rx="23.828125"
+ sodipodi:cy="454.31531"
+ sodipodi:cx="333.20312"
+ id="path6144-3-08"
+ style="fill:#0000ff;fill-opacity:1;stroke:#000000;stroke-width:9.91719723;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;stroke-dashoffset:0"
+ sodipodi:type="arc"
+ inkscape:export-filename="/wg/arc/vpradeep/ros-64/pkgs-trunk/image_pipeline/stereo_image_proc/docs/stereo_frames.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90" />
+ <path
+ transform="matrix(0.10083494,0,0,0.10083494,283.12854,438.416)"
+ d="m 357.03125,454.31531 a 23.828125,23.828125 0 1 1 -47.65625,0 23.828125,23.828125 0 1 1 47.65625,0 z"
+ sodipodi:ry="23.828125"
+ sodipodi:rx="23.828125"
+ sodipodi:cy="454.31531"
+ sodipodi:cx="333.20312"
+ id="path6144-3-1"
+ style="fill:#0000ff;fill-opacity:1;stroke:#000000;stroke-width:9.91719723;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;stroke-dashoffset:0"
+ sodipodi:type="arc"
+ inkscape:export-filename="/wg/arc/vpradeep/ros-64/pkgs-trunk/image_pipeline/stereo_image_proc/docs/stereo_frames.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90" />
+ <path
+ transform="matrix(0.10083494,0,0,0.10083494,283.12854,394.666)"
+ d="m 357.03125,454.31531 a 23.828125,23.828125 0 1 1 -47.65625,0 23.828125,23.828125 0 1 1 47.65625,0 z"
+ sodipodi:ry="23.828125"
+ sodipodi:rx="23.828125"
+ sodipodi:cy="454.31531"
+ sodipodi:cx="333.20312"
+ id="path6144-3-28"
+ style="fill:#0000ff;fill-opacity:1;stroke:#000000;stroke-width:9.91719723;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;stroke-dashoffset:0"
+ sodipodi:type="arc"
+ inkscape:export-filename="/wg/arc/vpradeep/ros-64/pkgs-trunk/image_pipeline/stereo_image_proc/docs/stereo_frames.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90" />
+ <path
+ transform="matrix(0.10083494,0,0,0.10083494,296.40979,355.6035)"
+ d="m 357.03125,454.31531 a 23.828125,23.828125 0 1 1 -47.65625,0 23.828125,23.828125 0 1 1 47.65625,0 z"
+ sodipodi:ry="23.828125"
+ sodipodi:rx="23.828125"
+ sodipodi:cy="454.31531"
+ sodipodi:cx="333.20312"
+ id="path6144-3-9"
+ style="fill:#0000ff;fill-opacity:1;stroke:#000000;stroke-width:9.91719723;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;stroke-dashoffset:0"
+ sodipodi:type="arc"
+ inkscape:export-filename="/wg/arc/vpradeep/ros-64/pkgs-trunk/image_pipeline/stereo_image_proc/docs/stereo_frames.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90" />
+ <path
+ transform="matrix(0.10083494,0,0,0.10083494,348.54098,373.23985)"
+ d="m 357.03125,454.31531 a 23.828125,23.828125 0 1 1 -47.65625,0 23.828125,23.828125 0 1 1 47.65625,0 z"
+ sodipodi:ry="23.828125"
+ sodipodi:rx="23.828125"
+ sodipodi:cy="454.31531"
+ sodipodi:cx="333.20312"
+ id="path6144-3-5"
+ style="fill:#0000ff;fill-opacity:1;stroke:#000000;stroke-width:9.91719723;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;stroke-dashoffset:0"
+ sodipodi:type="arc"
+ inkscape:export-filename="/wg/arc/vpradeep/ros-64/pkgs-trunk/image_pipeline/stereo_image_proc/docs/stereo_frames.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90" />
+ <path
+ transform="matrix(0.10083494,0,0,0.10083494,361.25354,399.3535)"
+ d="m 357.03125,454.31531 a 23.828125,23.828125 0 1 1 -47.65625,0 23.828125,23.828125 0 1 1 47.65625,0 z"
+ sodipodi:ry="23.828125"
+ sodipodi:rx="23.828125"
+ sodipodi:cy="454.31531"
+ sodipodi:cx="333.20312"
+ id="path6144-3-11"
+ style="fill:#0000ff;fill-opacity:1;stroke:#000000;stroke-width:9.91719723;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;stroke-dashoffset:0"
+ sodipodi:type="arc"
+ inkscape:export-filename="/wg/arc/vpradeep/ros-64/pkgs-trunk/image_pipeline/stereo_image_proc/docs/stereo_frames.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90" />
+ <path
+ transform="matrix(0.10083494,0,0,0.10083494,347.19104,421.2285)"
+ d="m 357.03125,454.31531 a 23.828125,23.828125 0 1 1 -47.65625,0 23.828125,23.828125 0 1 1 47.65625,0 z"
+ sodipodi:ry="23.828125"
+ sodipodi:rx="23.828125"
+ sodipodi:cy="454.31531"
+ sodipodi:cx="333.20312"
+ id="path6144-3-83"
+ style="fill:#0000ff;fill-opacity:1;stroke:#000000;stroke-width:9.91719723;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;stroke-dashoffset:0"
+ sodipodi:type="arc"
+ inkscape:export-filename="/wg/arc/vpradeep/ros-64/pkgs-trunk/image_pipeline/stereo_image_proc/docs/stereo_frames.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90" />
+ <path
+ transform="matrix(0.10083494,0,0,0.10083494,358.90979,453.25975)"
+ d="m 357.03125,454.31531 a 23.828125,23.828125 0 1 1 -47.65625,0 23.828125,23.828125 0 1 1 47.65625,0 z"
+ sodipodi:ry="23.828125"
+ sodipodi:rx="23.828125"
+ sodipodi:cy="454.31531"
+ sodipodi:cx="333.20312"
+ id="path6144-3-18"
+ style="fill:#0000ff;fill-opacity:1;stroke:#000000;stroke-width:9.91719723;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;stroke-dashoffset:0"
+ sodipodi:type="arc"
+ inkscape:export-filename="/wg/arc/vpradeep/ros-64/pkgs-trunk/image_pipeline/stereo_image_proc/docs/stereo_frames.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90" />
+ <path
+ transform="matrix(0.10083494,0,0,0.10083494,380.00354,429.82225)"
+ d="m 357.03125,454.31531 a 23.828125,23.828125 0 1 1 -47.65625,0 23.828125,23.828125 0 1 1 47.65625,0 z"
+ sodipodi:ry="23.828125"
+ sodipodi:rx="23.828125"
+ sodipodi:cy="454.31531"
+ sodipodi:cx="333.20312"
+ id="path6144-3-45"
+ style="fill:#0000ff;fill-opacity:1;stroke:#000000;stroke-width:9.91719723;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;stroke-dashoffset:0"
+ sodipodi:type="arc"
+ inkscape:export-filename="/wg/arc/vpradeep/ros-64/pkgs-trunk/image_pipeline/stereo_image_proc/docs/stereo_frames.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90" />
+ <path
+ transform="matrix(0.10083494,0,0,0.10083494,390.94104,374.3535)"
+ d="m 357.03125,454.31531 a 23.828125,23.828125 0 1 1 -47.65625,0 23.828125,23.828125 0 1 1 47.65625,0 z"
+ sodipodi:ry="23.828125"
+ sodipodi:rx="23.828125"
+ sodipodi:cy="454.31531"
+ sodipodi:cx="333.20312"
+ id="path6144-3-60"
+ style="fill:#0000ff;fill-opacity:1;stroke:#000000;stroke-width:9.91719723;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;stroke-dashoffset:0"
+ sodipodi:type="arc"
+ inkscape:export-filename="/wg/arc/vpradeep/ros-64/pkgs-trunk/image_pipeline/stereo_image_proc/docs/stereo_frames.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90" />
+ <path
+ transform="matrix(0.10083494,0,0,0.10083494,310.01185,334.81709)"
+ d="m 357.03125,454.31531 a 23.828125,23.828125 0 1 1 -47.65625,0 23.828125,23.828125 0 1 1 47.65625,0 z"
+ sodipodi:ry="23.828125"
+ sodipodi:rx="23.828125"
+ sodipodi:cy="454.31531"
+ sodipodi:cx="333.20312"
+ id="path6144-3-32"
+ style="fill:#0000ff;fill-opacity:1;stroke:#000000;stroke-width:9.91719723;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;stroke-dashoffset:0"
+ sodipodi:type="arc"
+ inkscape:export-filename="/wg/arc/vpradeep/ros-64/pkgs-trunk/image_pipeline/stereo_image_proc/docs/stereo_frames.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90" />
+ <path
+ transform="matrix(0.10083494,0,0,0.10083494,357.34729,365.75975)"
+ d="m 357.03125,454.31531 a 23.828125,23.828125 0 1 1 -47.65625,0 23.828125,23.828125 0 1 1 47.65625,0 z"
+ sodipodi:ry="23.828125"
+ sodipodi:rx="23.828125"
+ sodipodi:cy="454.31531"
+ sodipodi:cx="333.20312"
+ id="path6144-3-89"
+ style="fill:#0000ff;fill-opacity:1;stroke:#000000;stroke-width:9.91719723;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;stroke-dashoffset:0"
+ sodipodi:type="arc"
+ inkscape:export-filename="/wg/arc/vpradeep/ros-64/pkgs-trunk/image_pipeline/stereo_image_proc/docs/stereo_frames.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90" />
+ <path
+ transform="matrix(0.10083494,0,0,0.10083494,356.57277,350.98105)"
+ d="m 357.03125,454.31531 a 23.828125,23.828125 0 1 1 -47.65625,0 23.828125,23.828125 0 1 1 47.65625,0 z"
+ sodipodi:ry="23.828125"
+ sodipodi:rx="23.828125"
+ sodipodi:cy="454.31531"
+ sodipodi:cx="333.20312"
+ id="path6144-3-12"
+ style="fill:#0000ff;fill-opacity:1;stroke:#000000;stroke-width:9.91719723;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;stroke-dashoffset:0"
+ sodipodi:type="arc"
+ inkscape:export-filename="/wg/arc/vpradeep/ros-64/pkgs-trunk/image_pipeline/stereo_image_proc/docs/stereo_frames.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90" />
+ <text
+ id="text6088-7-6"
+ y="250.38168"
+ x="268.0455"
+ style="font-size:40px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;font-family:arial;-inkscape-font-specification:arial"
+ xml:space="preserve"
+ inkscape:export-filename="/wg/arc/vpradeep/ros-64/pkgs-trunk/image_pipeline/stereo_image_proc/docs/stereo_frames.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90"><tspan
+ style="font-size:16px;text-align:center;text-anchor:middle"
+ y="250.38168"
+ x="268.0455"
+ id="tspan6090-7-3"
+ sodipodi:role="line">Left</tspan><tspan
+ style="font-size:16px;text-align:center;text-anchor:middle"
+ y="270.38168"
+ x="268.0455"
+ sodipodi:role="line"
+ id="tspan8465">Imager</tspan></text>
+ <text
+ id="text6088-7-6-1"
+ y="297.09918"
+ x="176.94826"
+ style="font-size:40px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;font-family:arial;-inkscape-font-specification:arial"
+ xml:space="preserve"
+ inkscape:export-filename="/wg/arc/vpradeep/ros-64/pkgs-trunk/image_pipeline/stereo_image_proc/docs/stereo_frames.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90"><tspan
+ style="font-size:16px;text-align:center;text-anchor:middle"
+ y="297.09918"
+ x="176.94826"
+ id="tspan6090-7-3-8"
+ sodipodi:role="line">Right</tspan><tspan
+ style="font-size:16px;text-align:center;text-anchor:middle"
+ y="317.09918"
+ x="176.94826"
+ sodipodi:role="line"
+ id="tspan8463">Imager</tspan></text>
+ <text
+ id="text6088-7-6-8"
+ y="376.31558"
+ x="355.49841"
+ style="font-size:40px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;font-family:arial;-inkscape-font-specification:arial"
+ xml:space="preserve"
+ inkscape:export-filename="/wg/arc/vpradeep/ros-64/pkgs-trunk/image_pipeline/stereo_image_proc/docs/stereo_frames.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90"><tspan
+ style="font-size:16px"
+ y="376.31558"
+ x="355.49841"
+ id="tspan6090-7-3-2"
+ sodipodi:role="line">Point Cloud</tspan></text>
+ <path
+ style="fill:none;stroke:#323232;stroke-width:1;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:1,3;stroke-dashoffset:0;marker-mid:none;marker-end:url(#Arrow2Mend)"
+ d="m 283.87636,333.58322 50.50373,94.02457"
+ id="path7100"
+ inkscape:export-filename="/wg/arc/vpradeep/ros-64/pkgs-trunk/image_pipeline/stereo_image_proc/docs/stereo_frames.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90" />
+ </g>
+</svg>
diff --git a/stereo_image_proc/include/stereo_image_proc/processor.h b/stereo_image_proc/include/stereo_image_proc/processor.h
new file mode 100644
index 0000000..df763e2
--- /dev/null
+++ b/stereo_image_proc/include/stereo_image_proc/processor.h
@@ -0,0 +1,303 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+#ifndef STEREO_IMAGE_PROC_PROCESSOR_H
+#define STEREO_IMAGE_PROC_PROCESSOR_H
+
+#include <image_proc/processor.h>
+#include <image_geometry/stereo_camera_model.h>
+#include <stereo_msgs/DisparityImage.h>
+#include <sensor_msgs/PointCloud.h>
+#include <sensor_msgs/PointCloud2.h>
+
+namespace stereo_image_proc {
+
+struct StereoImageSet
+{
+ image_proc::ImageSet left;
+ image_proc::ImageSet right;
+ stereo_msgs::DisparityImage disparity;
+ sensor_msgs::PointCloud points;
+ sensor_msgs::PointCloud2 points2;
+};
+
+class StereoProcessor
+{
+public:
+
+ StereoProcessor()
+#if CV_MAJOR_VERSION == 3
+ {
+ block_matcher_ = cv::StereoBM::create();
+ sg_block_matcher_ = cv::StereoSGBM::create(1, 1, 10);
+#else
+ : block_matcher_(cv::StereoBM::BASIC_PRESET),
+ sg_block_matcher_()
+ {
+#endif
+ }
+
+ enum StereoType
+ {
+ BM, SGBM
+ };
+
+ enum {
+ LEFT_MONO = 1 << 0,
+ LEFT_RECT = 1 << 1,
+ LEFT_COLOR = 1 << 2,
+ LEFT_RECT_COLOR = 1 << 3,
+ RIGHT_MONO = 1 << 4,
+ RIGHT_RECT = 1 << 5,
+ RIGHT_COLOR = 1 << 6,
+ RIGHT_RECT_COLOR = 1 << 7,
+ DISPARITY = 1 << 8,
+ POINT_CLOUD = 1 << 9,
+ POINT_CLOUD2 = 1 << 10,
+
+ LEFT_ALL = LEFT_MONO | LEFT_RECT | LEFT_COLOR | LEFT_RECT_COLOR,
+ RIGHT_ALL = RIGHT_MONO | RIGHT_RECT | RIGHT_COLOR | RIGHT_RECT_COLOR,
+ STEREO_ALL = DISPARITY | POINT_CLOUD | POINT_CLOUD2,
+ ALL = LEFT_ALL | RIGHT_ALL | STEREO_ALL
+ };
+
+ inline
+ StereoType getStereoType() const {return current_stereo_algorithm_;}
+ inline
+ void setStereoType(StereoType type) {current_stereo_algorithm_ = type;}
+
+ int getInterpolation() const;
+ void setInterpolation(int interp);
+
+ // Disparity pre-filtering parameters
+
+ int getPreFilterSize() const;
+ void setPreFilterSize(int size);
+
+ int getPreFilterCap() const;
+ void setPreFilterCap(int cap);
+
+ // Disparity correlation parameters
+
+ int getCorrelationWindowSize() const;
+ void setCorrelationWindowSize(int size);
+
+ int getMinDisparity() const;
+ void setMinDisparity(int min_d);
+
+ int getDisparityRange() const;
+ void setDisparityRange(int range); // Number of pixels to search
+
+ // Disparity post-filtering parameters
+
+ int getTextureThreshold() const;
+ void setTextureThreshold(int threshold);
+
+ float getUniquenessRatio() const;
+ void setUniquenessRatio(float ratio);
+
+ int getSpeckleSize() const;
+ void setSpeckleSize(int size);
+
+ int getSpeckleRange() const;
+ void setSpeckleRange(int range);
+
+ // SGBM only
+ int getSgbmMode() const;
+ void setSgbmMode(int fullDP);
+
+ int getP1() const;
+ void setP1(int P1);
+
+ int getP2() const;
+ void setP2(int P2);
+
+ int getDisp12MaxDiff() const;
+ void setDisp12MaxDiff(int disp12MaxDiff);
+
+ // Do all the work!
+ bool process(const sensor_msgs::ImageConstPtr& left_raw,
+ const sensor_msgs::ImageConstPtr& right_raw,
+ const image_geometry::StereoCameraModel& model,
+ StereoImageSet& output, int flags) const;
+
+ void processDisparity(const cv::Mat& left_rect, const cv::Mat& right_rect,
+ const image_geometry::StereoCameraModel& model,
+ stereo_msgs::DisparityImage& disparity) const;
+
+ void processPoints(const stereo_msgs::DisparityImage& disparity,
+ const cv::Mat& color, const std::string& encoding,
+ const image_geometry::StereoCameraModel& model,
+ sensor_msgs::PointCloud& points) const;
+ void processPoints2(const stereo_msgs::DisparityImage& disparity,
+ const cv::Mat& color, const std::string& encoding,
+ const image_geometry::StereoCameraModel& model,
+ sensor_msgs::PointCloud2& points) const;
+
+private:
+ image_proc::Processor mono_processor_;
+
+ mutable cv::Mat_<int16_t> disparity16_; // scratch buffer for 16-bit signed disparity image
+#if CV_MAJOR_VERSION == 3
+ mutable cv::Ptr<cv::StereoBM> block_matcher_; // contains scratch buffers for block matching
+ mutable cv::Ptr<cv::StereoSGBM> sg_block_matcher_;
+#else
+ mutable cv::StereoBM block_matcher_; // contains scratch buffers for block matching
+ mutable cv::StereoSGBM sg_block_matcher_;
+#endif
+ StereoType current_stereo_algorithm_;
+ // scratch buffers for speckle filtering
+ mutable cv::Mat_<uint32_t> labels_;
+ mutable cv::Mat_<uint32_t> wavefront_;
+ mutable cv::Mat_<uint8_t> region_types_;
+ // scratch buffer for dense point cloud
+ mutable cv::Mat_<cv::Vec3f> dense_points_;
+};
+
+
+inline int StereoProcessor::getInterpolation() const
+{
+ return mono_processor_.interpolation_;
+}
+
+inline void StereoProcessor::setInterpolation(int interp)
+{
+ mono_processor_.interpolation_ = interp;
+}
+
+// For once, a macro is used just to avoid errors
+#define STEREO_IMAGE_PROC_OPENCV2(GET, SET, TYPE, PARAM) \
+inline TYPE StereoProcessor::GET() const \
+{ \
+ if (current_stereo_algorithm_ == BM) \
+ return block_matcher_.state->PARAM; \
+ return sg_block_matcher_.PARAM; \
+} \
+ \
+inline void StereoProcessor::SET(TYPE param) \
+{ \
+ block_matcher_.state->PARAM = param; \
+ sg_block_matcher_.PARAM = param; \
+}
+
+#define STEREO_IMAGE_PROC_OPENCV3(GET, SET, TYPE, GET_OPENCV, SET_OPENCV) \
+inline TYPE StereoProcessor::GET() const \
+{ \
+ if (current_stereo_algorithm_ == BM) \
+ return block_matcher_->GET_OPENCV(); \
+ return sg_block_matcher_->GET_OPENCV(); \
+} \
+\
+inline void StereoProcessor::SET(TYPE param) \
+{ \
+ block_matcher_->SET_OPENCV(param); \
+ sg_block_matcher_->SET_OPENCV(param); \
+}
+
+#if CV_MAJOR_VERSION == 3
+STEREO_IMAGE_PROC_OPENCV3(getPreFilterCap, setPreFilterCap, int, getPreFilterCap, setPreFilterCap)
+STEREO_IMAGE_PROC_OPENCV3(getCorrelationWindowSize, setCorrelationWindowSize, int, getBlockSize, setBlockSize)
+STEREO_IMAGE_PROC_OPENCV3(getMinDisparity, setMinDisparity, int, getMinDisparity, setMinDisparity)
+STEREO_IMAGE_PROC_OPENCV3(getDisparityRange, setDisparityRange, int, getNumDisparities, setNumDisparities)
+STEREO_IMAGE_PROC_OPENCV3(getUniquenessRatio, setUniquenessRatio, float, getUniquenessRatio, setUniquenessRatio)
+STEREO_IMAGE_PROC_OPENCV3(getSpeckleSize, setSpeckleSize, int, getSpeckleWindowSize, setSpeckleWindowSize)
+STEREO_IMAGE_PROC_OPENCV3(getSpeckleRange, setSpeckleRange, int, getSpeckleRange, setSpeckleRange)
+#else
+STEREO_IMAGE_PROC_OPENCV2(getPreFilterCap, setPreFilterCap, int, preFilterCap)
+STEREO_IMAGE_PROC_OPENCV2(getCorrelationWindowSize, setCorrelationWindowSize, int, SADWindowSize)
+STEREO_IMAGE_PROC_OPENCV2(getMinDisparity, setMinDisparity, int, minDisparity)
+STEREO_IMAGE_PROC_OPENCV2(getDisparityRange, setDisparityRange, int, numberOfDisparities)
+STEREO_IMAGE_PROC_OPENCV2(getUniquenessRatio, setUniquenessRatio, float, uniquenessRatio)
+STEREO_IMAGE_PROC_OPENCV2(getSpeckleSize, setSpeckleSize, int, speckleWindowSize)
+STEREO_IMAGE_PROC_OPENCV2(getSpeckleRange, setSpeckleRange, int, speckleRange)
+#endif
+
+#define STEREO_IMAGE_PROC_BM_ONLY_OPENCV2(GET, SET, TYPE, PARAM) \
+inline TYPE StereoProcessor::GET() const \
+{ \
+ return block_matcher_.state->PARAM; \
+} \
+\
+inline void StereoProcessor::SET(TYPE param) \
+{ \
+ block_matcher_.state->PARAM = param; \
+}
+
+#define STEREO_IMAGE_PROC_SGBM_ONLY_OPENCV2(GET, SET, TYPE, PARAM) \
+inline TYPE StereoProcessor::GET() const \
+{ \
+ return sg_block_matcher_.PARAM; \
+} \
+\
+inline void StereoProcessor::SET(TYPE param) \
+{ \
+ sg_block_matcher_.PARAM = param; \
+}
+
+#define STEREO_IMAGE_PROC_ONLY_OPENCV3(MEMBER, GET, SET, TYPE, GET_OPENCV, SET_OPENCV) \
+inline TYPE StereoProcessor::GET() const \
+{ \
+ return MEMBER->GET_OPENCV(); \
+} \
+\
+inline void StereoProcessor::SET(TYPE param) \
+{ \
+ MEMBER->SET_OPENCV(param); \
+}
+
+// BM only
+#if CV_MAJOR_VERSION == 3
+STEREO_IMAGE_PROC_ONLY_OPENCV3(block_matcher_, getPreFilterSize, setPreFilterSize, int, getPreFilterSize, setPreFilterSize)
+STEREO_IMAGE_PROC_ONLY_OPENCV3(block_matcher_, getTextureThreshold, setTextureThreshold, int, getTextureThreshold, setTextureThreshold)
+#else
+STEREO_IMAGE_PROC_BM_ONLY_OPENCV2(getPreFilterSize, setPreFilterSize, int, preFilterSize)
+STEREO_IMAGE_PROC_BM_ONLY_OPENCV2(getTextureThreshold, setTextureThreshold, int, textureThreshold)
+#endif
+
+// SGBM specific
+#if CV_MAJOR_VERSION == 3
+// getSgbmMode can return MODE_SGBM = 0, MODE_HH = 1. FullDP == 1 was MODE_HH so we're good
+STEREO_IMAGE_PROC_ONLY_OPENCV3(sg_block_matcher_, getSgbmMode, setSgbmMode, int, getMode, setMode)
+STEREO_IMAGE_PROC_ONLY_OPENCV3(sg_block_matcher_, getP1, setP1, int, getP1, setP1)
+STEREO_IMAGE_PROC_ONLY_OPENCV3(sg_block_matcher_, getP2, setP2, int, getP2, setP2)
+STEREO_IMAGE_PROC_ONLY_OPENCV3(sg_block_matcher_, getDisp12MaxDiff, setDisp12MaxDiff, int, getDisp12MaxDiff, setDisp12MaxDiff)
+#else
+STEREO_IMAGE_PROC_SGBM_ONLY_OPENCV2(getSgbmMode, setSgbmMode, int, fullDP)
+STEREO_IMAGE_PROC_SGBM_ONLY_OPENCV2(getP1, setP1, int, P1)
+STEREO_IMAGE_PROC_SGBM_ONLY_OPENCV2(getP2, setP2, int, P2)
+STEREO_IMAGE_PROC_SGBM_ONLY_OPENCV2(getDisp12MaxDiff, setDisp12MaxDiff, int, disp12MaxDiff)
+#endif
+
+} //namespace stereo_image_proc
+
+#endif
diff --git a/stereo_image_proc/launch/stereo_image_proc.launch b/stereo_image_proc/launch/stereo_image_proc.launch
new file mode 100644
index 0000000..605efd7
--- /dev/null
+++ b/stereo_image_proc/launch/stereo_image_proc.launch
@@ -0,0 +1,36 @@
+<!-- Launch in the camera namespace containing "image_raw" and "camera_info" -->
+<launch>
+
+ <arg name="manager" /> <!-- Must be globally qualified -->
+ <arg name="respawn" default="false" />
+ <arg name="left" default="left" />
+ <arg name="right" default="right" />
+ <!-- TODO Arguments for sync policy, etc? -->
+
+ <arg if="$(arg respawn)" name="bond" value="" />
+ <arg unless="$(arg respawn)" name="bond" value="--no-bond" />
+
+ <!-- Basic processing for left camera -->
+ <include file="$(find image_proc)/launch/image_proc.launch"
+ ns="$(arg left)">
+ <arg name="manager" value="$(arg manager)" />
+ <arg name="respawn" value="$(arg respawn)" />
+ </include>
+
+ <!-- Basic processing for right camera -->
+ <include file="$(find image_proc)/launch/image_proc.launch"
+ ns="$(arg right)">
+ <arg name="manager" value="$(arg manager)" />
+ <arg name="respawn" value="$(arg respawn)" />
+ </include>
+
+ <!-- Disparity image -->
+ <node pkg="nodelet" type="nodelet" name="disparity"
+ args="load stereo_image_proc/disparity $(arg manager) $(arg bond)"
+ respawn="$(arg respawn)" />
+
+ <!-- PointCloud2 -->
+ <node pkg="nodelet" type="nodelet" name="point_cloud2"
+ args="load stereo_image_proc/point_cloud2 $(arg manager) $(arg bond)"
+ respawn="$(arg respawn)" />
+</launch>
diff --git a/stereo_image_proc/nodelet_plugins.xml b/stereo_image_proc/nodelet_plugins.xml
new file mode 100644
index 0000000..3512efc
--- /dev/null
+++ b/stereo_image_proc/nodelet_plugins.xml
@@ -0,0 +1,11 @@
+<library path="lib/libstereo_image_proc">
+
+ <class name="stereo_image_proc/disparity" type="stereo_image_proc::DisparityNodelet" base_class_type="nodelet::Nodelet">
+ <description>Nodelet to perform stereo processing on a pair of rectified image streams, producing disparity images</description>
+ </class>
+
+ <class name="stereo_image_proc/point_cloud2" type="stereo_image_proc::PointCloud2Nodelet" base_class_type="nodelet::Nodelet">
+ <description>Nodelet to produce XYZRGB PointCloud2 messages</description>
+ </class>
+
+</library>
diff --git a/stereo_image_proc/package.xml b/stereo_image_proc/package.xml
new file mode 100644
index 0000000..c95f23b
--- /dev/null
+++ b/stereo_image_proc/package.xml
@@ -0,0 +1,40 @@
+<package>
+ <name>stereo_image_proc</name>
+ <version>1.12.20</version>
+ <description>Stereo and single image rectification and disparity processing.</description>
+ <author>Patrick Mihelich</author>
+ <author>Kurt Konolige</author>
+ <author>Jeremy Leibs</author>
+ <maintainer email="vincent.rabaud at gmail.com">Vincent Rabaud</maintainer>
+ <license>BSD</license>
+ <url>http://www.ros.org/wiki/stereo_image_proc</url>
+
+ <export>
+ <rosdoc config="rosdoc.yaml" />
+ <nodelet plugin="${prefix}/nodelet_plugins.xml" />
+ </export>
+
+ <buildtool_depend>catkin</buildtool_depend>
+
+ <test_depend>rostest</test_depend>
+
+ <build_depend>cv_bridge</build_depend>
+ <build_depend>dynamic_reconfigure</build_depend>
+ <build_depend>image_geometry</build_depend>
+ <build_depend>image_proc</build_depend>
+ <build_depend>image_transport</build_depend>
+ <build_depend>message_filters</build_depend>
+ <build_depend>nodelet</build_depend>
+ <build_depend>sensor_msgs</build_depend>
+ <build_depend>stereo_msgs</build_depend>
+
+ <run_depend>cv_bridge</run_depend>
+ <run_depend>dynamic_reconfigure</run_depend>
+ <run_depend>image_geometry</run_depend>
+ <run_depend>image_proc</run_depend>
+ <run_depend>image_transport</run_depend>
+ <run_depend>message_filters</run_depend>
+ <run_depend>nodelet</run_depend>
+ <run_depend>sensor_msgs</run_depend>
+ <run_depend>stereo_msgs</run_depend>
+</package>
diff --git a/stereo_image_proc/rosdoc.yaml b/stereo_image_proc/rosdoc.yaml
new file mode 100644
index 0000000..976fdf0
--- /dev/null
+++ b/stereo_image_proc/rosdoc.yaml
@@ -0,0 +1,4 @@
+ - builder: doxygen
+ name: C++ API
+ output_dir: c++
+ file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox'
diff --git a/stereo_image_proc/src/libstereo_image_proc/processor.cpp b/stereo_image_proc/src/libstereo_image_proc/processor.cpp
new file mode 100644
index 0000000..cecbf97
--- /dev/null
+++ b/stereo_image_proc/src/libstereo_image_proc/processor.cpp
@@ -0,0 +1,317 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+#include <ros/assert.h>
+#include "stereo_image_proc/processor.h"
+#include <sensor_msgs/image_encodings.h>
+#include <cmath>
+#include <limits>
+
+namespace stereo_image_proc {
+
+bool StereoProcessor::process(const sensor_msgs::ImageConstPtr& left_raw,
+ const sensor_msgs::ImageConstPtr& right_raw,
+ const image_geometry::StereoCameraModel& model,
+ StereoImageSet& output, int flags) const
+{
+ // Do monocular processing on left and right images
+ int left_flags = flags & LEFT_ALL;
+ int right_flags = flags & RIGHT_ALL;
+ if (flags & STEREO_ALL) {
+ // Need the rectified images for stereo processing
+ left_flags |= LEFT_RECT;
+ right_flags |= RIGHT_RECT;
+ }
+ if (flags & (POINT_CLOUD | POINT_CLOUD2)) {
+ flags |= DISPARITY;
+ // Need the color channels for the point cloud
+ left_flags |= LEFT_RECT_COLOR;
+ }
+ if (!mono_processor_.process(left_raw, model.left(), output.left, left_flags))
+ return false;
+ if (!mono_processor_.process(right_raw, model.right(), output.right, right_flags >> 4))
+ return false;
+
+ // Do block matching to produce the disparity image
+ if (flags & DISPARITY) {
+ processDisparity(output.left.rect, output.right.rect, model, output.disparity);
+ }
+
+ // Project disparity image to 3d point cloud
+ if (flags & POINT_CLOUD) {
+ processPoints(output.disparity, output.left.rect_color, output.left.color_encoding, model, output.points);
+ }
+
+ // Project disparity image to 3d point cloud
+ if (flags & POINT_CLOUD2) {
+ processPoints2(output.disparity, output.left.rect_color, output.left.color_encoding, model, output.points2);
+ }
+
+ return true;
+}
+
+void StereoProcessor::processDisparity(const cv::Mat& left_rect, const cv::Mat& right_rect,
+ const image_geometry::StereoCameraModel& model,
+ stereo_msgs::DisparityImage& disparity) const
+{
+ // Fixed-point disparity is 16 times the true value: d = d_fp / 16.0 = x_l - x_r.
+ static const int DPP = 16; // disparities per pixel
+ static const double inv_dpp = 1.0 / DPP;
+
+ // Block matcher produces 16-bit signed (fixed point) disparity image
+ if (current_stereo_algorithm_ == BM)
+#if CV_MAJOR_VERSION == 3
+ block_matcher_->compute(left_rect, right_rect, disparity16_);
+ else
+ sg_block_matcher_->compute(left_rect, right_rect, disparity16_);
+#else
+ block_matcher_(left_rect, right_rect, disparity16_);
+ else
+ sg_block_matcher_(left_rect, right_rect, disparity16_);
+#endif
+
+ // Fill in DisparityImage image data, converting to 32-bit float
+ sensor_msgs::Image& dimage = disparity.image;
+ dimage.height = disparity16_.rows;
+ dimage.width = disparity16_.cols;
+ dimage.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
+ dimage.step = dimage.width * sizeof(float);
+ dimage.data.resize(dimage.step * dimage.height);
+ cv::Mat_<float> dmat(dimage.height, dimage.width, (float*)&dimage.data[0], dimage.step);
+ // We convert from fixed-point to float disparity and also adjust for any x-offset between
+ // the principal points: d = d_fp*inv_dpp - (cx_l - cx_r)
+ disparity16_.convertTo(dmat, dmat.type(), inv_dpp, -(model.left().cx() - model.right().cx()));
+ ROS_ASSERT(dmat.data == &dimage.data[0]);
+ /// @todo is_bigendian? :)
+
+ // Stereo parameters
+ disparity.f = model.right().fx();
+ disparity.T = model.baseline();
+
+ /// @todo Window of (potentially) valid disparities
+
+ // Disparity search range
+ disparity.min_disparity = getMinDisparity();
+ disparity.max_disparity = getMinDisparity() + getDisparityRange() - 1;
+ disparity.delta_d = inv_dpp;
+}
+
+inline bool isValidPoint(const cv::Vec3f& pt)
+{
+ // Check both for disparities explicitly marked as invalid (where OpenCV maps pt.z to MISSING_Z)
+ // and zero disparities (point mapped to infinity).
+ return pt[2] != image_geometry::StereoCameraModel::MISSING_Z && !std::isinf(pt[2]);
+}
+
+void StereoProcessor::processPoints(const stereo_msgs::DisparityImage& disparity,
+ const cv::Mat& color, const std::string& encoding,
+ const image_geometry::StereoCameraModel& model,
+ sensor_msgs::PointCloud& points) const
+{
+ // Calculate dense point cloud
+ const sensor_msgs::Image& dimage = disparity.image;
+ const cv::Mat_<float> dmat(dimage.height, dimage.width, (float*)&dimage.data[0], dimage.step);
+ model.projectDisparityImageTo3d(dmat, dense_points_, true);
+
+ // Fill in sparse point cloud message
+ points.points.resize(0);
+ points.channels.resize(3);
+ points.channels[0].name = "rgb";
+ points.channels[0].values.resize(0);
+ points.channels[1].name = "u";
+ points.channels[1].values.resize(0);
+ points.channels[2].name = "v";
+ points.channels[2].values.resize(0);
+
+ for (int32_t u = 0; u < dense_points_.rows; ++u) {
+ for (int32_t v = 0; v < dense_points_.cols; ++v) {
+ if (isValidPoint(dense_points_(u,v))) {
+ // x,y,z
+ geometry_msgs::Point32 pt;
+ pt.x = dense_points_(u,v)[0];
+ pt.y = dense_points_(u,v)[1];
+ pt.z = dense_points_(u,v)[2];
+ points.points.push_back(pt);
+ // u,v
+ points.channels[1].values.push_back(u);
+ points.channels[2].values.push_back(v);
+ }
+ }
+ }
+
+ // Fill in color
+ namespace enc = sensor_msgs::image_encodings;
+ points.channels[0].values.reserve(points.points.size());
+ if (encoding == enc::MONO8) {
+ for (int32_t u = 0; u < dense_points_.rows; ++u) {
+ for (int32_t v = 0; v < dense_points_.cols; ++v) {
+ if (isValidPoint(dense_points_(u,v))) {
+ uint8_t g = color.at<uint8_t>(u,v);
+ int32_t rgb = (g << 16) | (g << 8) | g;
+ points.channels[0].values.push_back(*(float*)(&rgb));
+ }
+ }
+ }
+ }
+ else if (encoding == enc::RGB8) {
+ for (int32_t u = 0; u < dense_points_.rows; ++u) {
+ for (int32_t v = 0; v < dense_points_.cols; ++v) {
+ if (isValidPoint(dense_points_(u,v))) {
+ const cv::Vec3b& rgb = color.at<cv::Vec3b>(u,v);
+ int32_t rgb_packed = (rgb[0] << 16) | (rgb[1] << 8) | rgb[2];
+ points.channels[0].values.push_back(*(float*)(&rgb_packed));
+ }
+ }
+ }
+ }
+ else if (encoding == enc::BGR8) {
+ for (int32_t u = 0; u < dense_points_.rows; ++u) {
+ for (int32_t v = 0; v < dense_points_.cols; ++v) {
+ if (isValidPoint(dense_points_(u,v))) {
+ const cv::Vec3b& bgr = color.at<cv::Vec3b>(u,v);
+ int32_t rgb_packed = (bgr[2] << 16) | (bgr[1] << 8) | bgr[0];
+ points.channels[0].values.push_back(*(float*)(&rgb_packed));
+ }
+ }
+ }
+ }
+ else {
+ ROS_WARN("Could not fill color channel of the point cloud, unrecognized encoding '%s'", encoding.c_str());
+ }
+}
+
+void StereoProcessor::processPoints2(const stereo_msgs::DisparityImage& disparity,
+ const cv::Mat& color, const std::string& encoding,
+ const image_geometry::StereoCameraModel& model,
+ sensor_msgs::PointCloud2& points) const
+{
+ // Calculate dense point cloud
+ const sensor_msgs::Image& dimage = disparity.image;
+ const cv::Mat_<float> dmat(dimage.height, dimage.width, (float*)&dimage.data[0], dimage.step);
+ model.projectDisparityImageTo3d(dmat, dense_points_, true);
+
+ // Fill in sparse point cloud message
+ points.height = dense_points_.rows;
+ points.width = dense_points_.cols;
+ points.fields.resize (4);
+ points.fields[0].name = "x";
+ points.fields[0].offset = 0;
+ points.fields[0].count = 1;
+ points.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
+ points.fields[1].name = "y";
+ points.fields[1].offset = 4;
+ points.fields[1].count = 1;
+ points.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
+ points.fields[2].name = "z";
+ points.fields[2].offset = 8;
+ points.fields[2].count = 1;
+ points.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
+ points.fields[3].name = "rgb";
+ points.fields[3].offset = 12;
+ points.fields[3].count = 1;
+ points.fields[3].datatype = sensor_msgs::PointField::FLOAT32;
+ //points.is_bigendian = false; ???
+ points.point_step = 16;
+ points.row_step = points.point_step * points.width;
+ points.data.resize (points.row_step * points.height);
+ points.is_dense = false; // there may be invalid points
+
+ float bad_point = std::numeric_limits<float>::quiet_NaN ();
+ int i = 0;
+ for (int32_t u = 0; u < dense_points_.rows; ++u) {
+ for (int32_t v = 0; v < dense_points_.cols; ++v, ++i) {
+ if (isValidPoint(dense_points_(u,v))) {
+ // x,y,z,rgba
+ memcpy (&points.data[i * points.point_step + 0], &dense_points_(u,v)[0], sizeof (float));
+ memcpy (&points.data[i * points.point_step + 4], &dense_points_(u,v)[1], sizeof (float));
+ memcpy (&points.data[i * points.point_step + 8], &dense_points_(u,v)[2], sizeof (float));
+ }
+ else {
+ memcpy (&points.data[i * points.point_step + 0], &bad_point, sizeof (float));
+ memcpy (&points.data[i * points.point_step + 4], &bad_point, sizeof (float));
+ memcpy (&points.data[i * points.point_step + 8], &bad_point, sizeof (float));
+ }
+ }
+ }
+
+ // Fill in color
+ namespace enc = sensor_msgs::image_encodings;
+ i = 0;
+ if (encoding == enc::MONO8) {
+ for (int32_t u = 0; u < dense_points_.rows; ++u) {
+ for (int32_t v = 0; v < dense_points_.cols; ++v, ++i) {
+ if (isValidPoint(dense_points_(u,v))) {
+ uint8_t g = color.at<uint8_t>(u,v);
+ int32_t rgb = (g << 16) | (g << 8) | g;
+ memcpy (&points.data[i * points.point_step + 12], &rgb, sizeof (int32_t));
+ }
+ else {
+ memcpy (&points.data[i * points.point_step + 12], &bad_point, sizeof (float));
+ }
+ }
+ }
+ }
+ else if (encoding == enc::RGB8) {
+ for (int32_t u = 0; u < dense_points_.rows; ++u) {
+ for (int32_t v = 0; v < dense_points_.cols; ++v, ++i) {
+ if (isValidPoint(dense_points_(u,v))) {
+ const cv::Vec3b& rgb = color.at<cv::Vec3b>(u,v);
+ int32_t rgb_packed = (rgb[0] << 16) | (rgb[1] << 8) | rgb[2];
+ memcpy (&points.data[i * points.point_step + 12], &rgb_packed, sizeof (int32_t));
+ }
+ else {
+ memcpy (&points.data[i * points.point_step + 12], &bad_point, sizeof (float));
+ }
+ }
+ }
+ }
+ else if (encoding == enc::BGR8) {
+ for (int32_t u = 0; u < dense_points_.rows; ++u) {
+ for (int32_t v = 0; v < dense_points_.cols; ++v, ++i) {
+ if (isValidPoint(dense_points_(u,v))) {
+ const cv::Vec3b& bgr = color.at<cv::Vec3b>(u,v);
+ int32_t rgb_packed = (bgr[2] << 16) | (bgr[1] << 8) | bgr[0];
+ memcpy (&points.data[i * points.point_step + 12], &rgb_packed, sizeof (int32_t));
+ }
+ else {
+ memcpy (&points.data[i * points.point_step + 12], &bad_point, sizeof (float));
+ }
+ }
+ }
+ }
+ else {
+ ROS_WARN("Could not fill color channel of the point cloud, unrecognized encoding '%s'", encoding.c_str());
+ }
+}
+
+} //namespace stereo_image_proc
diff --git a/stereo_image_proc/src/nodelets/disparity.cpp b/stereo_image_proc/src/nodelets/disparity.cpp
new file mode 100644
index 0000000..1c39f99
--- /dev/null
+++ b/stereo_image_proc/src/nodelets/disparity.cpp
@@ -0,0 +1,250 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+#include <boost/version.hpp>
+#if ((BOOST_VERSION / 100) % 1000) >= 53
+#include <boost/thread/lock_guard.hpp>
+#endif
+
+#include <ros/ros.h>
+#include <nodelet/nodelet.h>
+#include <image_transport/image_transport.h>
+#include <image_transport/subscriber_filter.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/synchronizer.h>
+#include <message_filters/sync_policies/exact_time.h>
+#include <message_filters/sync_policies/approximate_time.h>
+
+#include <image_geometry/stereo_camera_model.h>
+#include <opencv2/calib3d/calib3d.hpp>
+#include <cv_bridge/cv_bridge.h>
+
+#include <sensor_msgs/image_encodings.h>
+#include <stereo_msgs/DisparityImage.h>
+
+#include <stereo_image_proc/DisparityConfig.h>
+#include <dynamic_reconfigure/server.h>
+
+#include <stereo_image_proc/processor.h>
+
+namespace stereo_image_proc {
+
+using namespace sensor_msgs;
+using namespace stereo_msgs;
+using namespace message_filters::sync_policies;
+
+class DisparityNodelet : public nodelet::Nodelet
+{
+ boost::shared_ptr<image_transport::ImageTransport> it_;
+
+ // Subscriptions
+ image_transport::SubscriberFilter sub_l_image_, sub_r_image_;
+ message_filters::Subscriber<CameraInfo> sub_l_info_, sub_r_info_;
+ typedef ExactTime<Image, CameraInfo, Image, CameraInfo> ExactPolicy;
+ typedef ApproximateTime<Image, CameraInfo, Image, CameraInfo> ApproximatePolicy;
+ typedef message_filters::Synchronizer<ExactPolicy> ExactSync;
+ typedef message_filters::Synchronizer<ApproximatePolicy> ApproximateSync;
+ boost::shared_ptr<ExactSync> exact_sync_;
+ boost::shared_ptr<ApproximateSync> approximate_sync_;
+ // Publications
+ boost::mutex connect_mutex_;
+ ros::Publisher pub_disparity_;
+
+ // Dynamic reconfigure
+ boost::recursive_mutex config_mutex_;
+ typedef stereo_image_proc::DisparityConfig Config;
+ typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
+ boost::shared_ptr<ReconfigureServer> reconfigure_server_;
+
+ // Processing state (note: only safe because we're single-threaded!)
+ image_geometry::StereoCameraModel model_;
+ stereo_image_proc::StereoProcessor block_matcher_; // contains scratch buffers for block matching
+
+ virtual void onInit();
+
+ void connectCb();
+
+ void imageCb(const ImageConstPtr& l_image_msg, const CameraInfoConstPtr& l_info_msg,
+ const ImageConstPtr& r_image_msg, const CameraInfoConstPtr& r_info_msg);
+
+ void configCb(Config &config, uint32_t level);
+};
+
+void DisparityNodelet::onInit()
+{
+ ros::NodeHandle &nh = getNodeHandle();
+ ros::NodeHandle &private_nh = getPrivateNodeHandle();
+
+ it_.reset(new image_transport::ImageTransport(nh));
+
+ // Synchronize inputs. Topic subscriptions happen on demand in the connection
+ // callback. Optionally do approximate synchronization.
+ int queue_size;
+ private_nh.param("queue_size", queue_size, 5);
+ bool approx;
+ private_nh.param("approximate_sync", approx, false);
+ if (approx)
+ {
+ approximate_sync_.reset( new ApproximateSync(ApproximatePolicy(queue_size),
+ sub_l_image_, sub_l_info_,
+ sub_r_image_, sub_r_info_) );
+ approximate_sync_->registerCallback(boost::bind(&DisparityNodelet::imageCb,
+ this, _1, _2, _3, _4));
+ }
+ else
+ {
+ exact_sync_.reset( new ExactSync(ExactPolicy(queue_size),
+ sub_l_image_, sub_l_info_,
+ sub_r_image_, sub_r_info_) );
+ exact_sync_->registerCallback(boost::bind(&DisparityNodelet::imageCb,
+ this, _1, _2, _3, _4));
+ }
+
+ // Set up dynamic reconfiguration
+ ReconfigureServer::CallbackType f = boost::bind(&DisparityNodelet::configCb,
+ this, _1, _2);
+ reconfigure_server_.reset(new ReconfigureServer(config_mutex_, private_nh));
+ reconfigure_server_->setCallback(f);
+
+ // Monitor whether anyone is subscribed to the output
+ ros::SubscriberStatusCallback connect_cb = boost::bind(&DisparityNodelet::connectCb, this);
+ // Make sure we don't enter connectCb() between advertising and assigning to pub_disparity_
+ boost::lock_guard<boost::mutex> lock(connect_mutex_);
+ pub_disparity_ = nh.advertise<DisparityImage>("disparity", 1, connect_cb, connect_cb);
+}
+
+// Handles (un)subscribing when clients (un)subscribe
+void DisparityNodelet::connectCb()
+{
+ boost::lock_guard<boost::mutex> lock(connect_mutex_);
+ if (pub_disparity_.getNumSubscribers() == 0)
+ {
+ sub_l_image_.unsubscribe();
+ sub_l_info_ .unsubscribe();
+ sub_r_image_.unsubscribe();
+ sub_r_info_ .unsubscribe();
+ }
+ else if (!sub_l_image_.getSubscriber())
+ {
+ ros::NodeHandle &nh = getNodeHandle();
+ // Queue size 1 should be OK; the one that matters is the synchronizer queue size.
+ /// @todo Allow remapping left, right?
+ image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle());
+ sub_l_image_.subscribe(*it_, "left/image_rect", 1, hints);
+ sub_l_info_ .subscribe(nh, "left/camera_info", 1);
+ sub_r_image_.subscribe(*it_, "right/image_rect", 1, hints);
+ sub_r_info_ .subscribe(nh, "right/camera_info", 1);
+ }
+}
+
+void DisparityNodelet::imageCb(const ImageConstPtr& l_image_msg,
+ const CameraInfoConstPtr& l_info_msg,
+ const ImageConstPtr& r_image_msg,
+ const CameraInfoConstPtr& r_info_msg)
+{
+ // Update the camera model
+ model_.fromCameraInfo(l_info_msg, r_info_msg);
+
+ // Allocate new disparity image message
+ DisparityImagePtr disp_msg = boost::make_shared<DisparityImage>();
+ disp_msg->header = l_info_msg->header;
+ disp_msg->image.header = l_info_msg->header;
+
+ // Compute window of (potentially) valid disparities
+ int border = block_matcher_.getCorrelationWindowSize() / 2;
+ int left = block_matcher_.getDisparityRange() + block_matcher_.getMinDisparity() + border - 1;
+ int wtf = (block_matcher_.getMinDisparity() >= 0) ? border + block_matcher_.getMinDisparity() : std::max(border, -block_matcher_.getMinDisparity());
+ int right = disp_msg->image.width - 1 - wtf;
+ int top = border;
+ int bottom = disp_msg->image.height - 1 - border;
+ disp_msg->valid_window.x_offset = left;
+ disp_msg->valid_window.y_offset = top;
+ disp_msg->valid_window.width = right - left;
+ disp_msg->valid_window.height = bottom - top;
+
+ // Create cv::Mat views onto all buffers
+ const cv::Mat_<uint8_t> l_image = cv_bridge::toCvShare(l_image_msg, sensor_msgs::image_encodings::MONO8)->image;
+ const cv::Mat_<uint8_t> r_image = cv_bridge::toCvShare(r_image_msg, sensor_msgs::image_encodings::MONO8)->image;
+
+ // Perform block matching to find the disparities
+ block_matcher_.processDisparity(l_image, r_image, model_, *disp_msg);
+
+ // Adjust for any x-offset between the principal points: d' = d - (cx_l - cx_r)
+ double cx_l = model_.left().cx();
+ double cx_r = model_.right().cx();
+ if (cx_l != cx_r) {
+ cv::Mat_<float> disp_image(disp_msg->image.height, disp_msg->image.width,
+ reinterpret_cast<float*>(&disp_msg->image.data[0]),
+ disp_msg->image.step);
+ cv::subtract(disp_image, cv::Scalar(cx_l - cx_r), disp_image);
+ }
+
+ pub_disparity_.publish(disp_msg);
+}
+
+void DisparityNodelet::configCb(Config &config, uint32_t level)
+{
+ // Tweak all settings to be valid
+ config.prefilter_size |= 0x1; // must be odd
+ config.correlation_window_size |= 0x1; // must be odd
+ config.disparity_range = (config.disparity_range / 16) * 16; // must be multiple of 16
+
+ // check stereo method
+ // Note: With single-threaded NodeHandle, configCb and imageCb can't be called
+ // concurrently, so this is thread-safe.
+ block_matcher_.setPreFilterCap(config.prefilter_cap);
+ block_matcher_.setCorrelationWindowSize(config.correlation_window_size);
+ block_matcher_.setMinDisparity(config.min_disparity);
+ block_matcher_.setDisparityRange(config.disparity_range);
+ block_matcher_.setUniquenessRatio(config.uniqueness_ratio);
+ block_matcher_.setSpeckleSize(config.speckle_size);
+ block_matcher_.setSpeckleRange(config.speckle_range);
+ if (config.stereo_algorithm == stereo_image_proc::Disparity_StereoBM) { // StereoBM
+ block_matcher_.setStereoType(StereoProcessor::BM);
+ block_matcher_.setPreFilterSize(config.prefilter_size);
+ block_matcher_.setTextureThreshold(config.texture_threshold);
+ }
+ else if (config.stereo_algorithm == stereo_image_proc::Disparity_StereoSGBM) { // StereoSGBM
+ block_matcher_.setStereoType(StereoProcessor::SGBM);
+ block_matcher_.setSgbmMode(config.fullDP);
+ block_matcher_.setP1(config.P1);
+ block_matcher_.setP2(config.P2);
+ block_matcher_.setDisp12MaxDiff(config.disp12MaxDiff);
+ }
+}
+
+} // namespace stereo_image_proc
+
+// Register nodelet
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(stereo_image_proc::DisparityNodelet,nodelet::Nodelet)
diff --git a/stereo_image_proc/src/nodelets/point_cloud2.cpp b/stereo_image_proc/src/nodelets/point_cloud2.cpp
new file mode 100644
index 0000000..fae8ba1
--- /dev/null
+++ b/stereo_image_proc/src/nodelets/point_cloud2.cpp
@@ -0,0 +1,272 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+#include <boost/version.hpp>
+#if ((BOOST_VERSION / 100) % 1000) >= 53
+#include <boost/thread/lock_guard.hpp>
+#endif
+
+#include <ros/ros.h>
+#include <nodelet/nodelet.h>
+#include <image_transport/image_transport.h>
+#include <image_transport/subscriber_filter.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/synchronizer.h>
+#include <message_filters/sync_policies/exact_time.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <image_geometry/stereo_camera_model.h>
+
+#include <stereo_msgs/DisparityImage.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/image_encodings.h>
+#include <sensor_msgs/point_cloud2_iterator.h>
+
+namespace stereo_image_proc {
+
+using namespace sensor_msgs;
+using namespace stereo_msgs;
+using namespace message_filters::sync_policies;
+
+class PointCloud2Nodelet : public nodelet::Nodelet
+{
+ boost::shared_ptr<image_transport::ImageTransport> it_;
+
+ // Subscriptions
+ image_transport::SubscriberFilter sub_l_image_;
+ message_filters::Subscriber<CameraInfo> sub_l_info_, sub_r_info_;
+ message_filters::Subscriber<DisparityImage> sub_disparity_;
+ typedef ExactTime<Image, CameraInfo, CameraInfo, DisparityImage> ExactPolicy;
+ typedef ApproximateTime<Image, CameraInfo, CameraInfo, DisparityImage> ApproximatePolicy;
+ typedef message_filters::Synchronizer<ExactPolicy> ExactSync;
+ typedef message_filters::Synchronizer<ApproximatePolicy> ApproximateSync;
+ boost::shared_ptr<ExactSync> exact_sync_;
+ boost::shared_ptr<ApproximateSync> approximate_sync_;
+
+ // Publications
+ boost::mutex connect_mutex_;
+ ros::Publisher pub_points2_;
+
+ // Processing state (note: only safe because we're single-threaded!)
+ image_geometry::StereoCameraModel model_;
+ cv::Mat_<cv::Vec3f> points_mat_; // scratch buffer
+
+ virtual void onInit();
+
+ void connectCb();
+
+ void imageCb(const ImageConstPtr& l_image_msg,
+ const CameraInfoConstPtr& l_info_msg,
+ const CameraInfoConstPtr& r_info_msg,
+ const DisparityImageConstPtr& disp_msg);
+};
+
+void PointCloud2Nodelet::onInit()
+{
+ ros::NodeHandle &nh = getNodeHandle();
+ ros::NodeHandle &private_nh = getPrivateNodeHandle();
+ it_.reset(new image_transport::ImageTransport(nh));
+
+ // Synchronize inputs. Topic subscriptions happen on demand in the connection
+ // callback. Optionally do approximate synchronization.
+ int queue_size;
+ private_nh.param("queue_size", queue_size, 5);
+ bool approx;
+ private_nh.param("approximate_sync", approx, false);
+ if (approx)
+ {
+ approximate_sync_.reset( new ApproximateSync(ApproximatePolicy(queue_size),
+ sub_l_image_, sub_l_info_,
+ sub_r_info_, sub_disparity_) );
+ approximate_sync_->registerCallback(boost::bind(&PointCloud2Nodelet::imageCb,
+ this, _1, _2, _3, _4));
+ }
+ else
+ {
+ exact_sync_.reset( new ExactSync(ExactPolicy(queue_size),
+ sub_l_image_, sub_l_info_,
+ sub_r_info_, sub_disparity_) );
+ exact_sync_->registerCallback(boost::bind(&PointCloud2Nodelet::imageCb,
+ this, _1, _2, _3, _4));
+ }
+
+ // Monitor whether anyone is subscribed to the output
+ ros::SubscriberStatusCallback connect_cb = boost::bind(&PointCloud2Nodelet::connectCb, this);
+ // Make sure we don't enter connectCb() between advertising and assigning to pub_points2_
+ boost::lock_guard<boost::mutex> lock(connect_mutex_);
+ pub_points2_ = nh.advertise<PointCloud2>("points2", 1, connect_cb, connect_cb);
+}
+
+// Handles (un)subscribing when clients (un)subscribe
+void PointCloud2Nodelet::connectCb()
+{
+ boost::lock_guard<boost::mutex> lock(connect_mutex_);
+ if (pub_points2_.getNumSubscribers() == 0)
+ {
+ sub_l_image_ .unsubscribe();
+ sub_l_info_ .unsubscribe();
+ sub_r_info_ .unsubscribe();
+ sub_disparity_.unsubscribe();
+ }
+ else if (!sub_l_image_.getSubscriber())
+ {
+ ros::NodeHandle &nh = getNodeHandle();
+ // Queue size 1 should be OK; the one that matters is the synchronizer queue size.
+ image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle());
+ sub_l_image_ .subscribe(*it_, "left/image_rect_color", 1, hints);
+ sub_l_info_ .subscribe(nh, "left/camera_info", 1);
+ sub_r_info_ .subscribe(nh, "right/camera_info", 1);
+ sub_disparity_.subscribe(nh, "disparity", 1);
+ }
+}
+
+inline bool isValidPoint(const cv::Vec3f& pt)
+{
+ // Check both for disparities explicitly marked as invalid (where OpenCV maps pt.z to MISSING_Z)
+ // and zero disparities (point mapped to infinity).
+ return pt[2] != image_geometry::StereoCameraModel::MISSING_Z && !std::isinf(pt[2]);
+}
+
+void PointCloud2Nodelet::imageCb(const ImageConstPtr& l_image_msg,
+ const CameraInfoConstPtr& l_info_msg,
+ const CameraInfoConstPtr& r_info_msg,
+ const DisparityImageConstPtr& disp_msg)
+{
+ // Update the camera model
+ model_.fromCameraInfo(l_info_msg, r_info_msg);
+
+ // Calculate point cloud
+ const Image& dimage = disp_msg->image;
+ const cv::Mat_<float> dmat(dimage.height, dimage.width, (float*)&dimage.data[0], dimage.step);
+ model_.projectDisparityImageTo3d(dmat, points_mat_, true);
+ cv::Mat_<cv::Vec3f> mat = points_mat_;
+
+ // Fill in new PointCloud2 message (2D image-like layout)
+ PointCloud2Ptr points_msg = boost::make_shared<PointCloud2>();
+ points_msg->header = disp_msg->header;
+ points_msg->height = mat.rows;
+ points_msg->width = mat.cols;
+ points_msg->is_bigendian = false;
+ points_msg->is_dense = false; // there may be invalid points
+
+ sensor_msgs::PointCloud2Modifier pcd_modifier(*points_msg);
+ pcd_modifier.setPointCloud2FieldsByString(2, "xyz", "rgb");
+
+ sensor_msgs::PointCloud2Iterator<float> iter_x(*points_msg, "x");
+ sensor_msgs::PointCloud2Iterator<float> iter_y(*points_msg, "y");
+ sensor_msgs::PointCloud2Iterator<float> iter_z(*points_msg, "z");
+ sensor_msgs::PointCloud2Iterator<uint8_t> iter_r(*points_msg, "r");
+ sensor_msgs::PointCloud2Iterator<uint8_t> iter_g(*points_msg, "g");
+ sensor_msgs::PointCloud2Iterator<uint8_t> iter_b(*points_msg, "b");
+
+ float bad_point = std::numeric_limits<float>::quiet_NaN ();
+ for (int v = 0; v < mat.rows; ++v)
+ {
+ for (int u = 0; u < mat.cols; ++u, ++iter_x, ++iter_y, ++iter_z)
+ {
+ if (isValidPoint(mat(v,u)))
+ {
+ // x,y,z
+ *iter_x = mat(v, u)[0];
+ *iter_y = mat(v, u)[1];
+ *iter_z = mat(v, u)[2];
+ }
+ else
+ {
+ *iter_x = *iter_y = *iter_z = bad_point;
+ }
+ }
+ }
+
+ // Fill in color
+ namespace enc = sensor_msgs::image_encodings;
+ const std::string& encoding = l_image_msg->encoding;
+ if (encoding == enc::MONO8)
+ {
+ const cv::Mat_<uint8_t> color(l_image_msg->height, l_image_msg->width,
+ (uint8_t*)&l_image_msg->data[0],
+ l_image_msg->step);
+ for (int v = 0; v < mat.rows; ++v)
+ {
+ for (int u = 0; u < mat.cols; ++u, ++iter_r, ++iter_g, ++iter_b)
+ {
+ uint8_t g = color(v,u);
+ *iter_r = *iter_g = *iter_b = g;
+ }
+ }
+ }
+ else if (encoding == enc::RGB8)
+ {
+ const cv::Mat_<cv::Vec3b> color(l_image_msg->height, l_image_msg->width,
+ (cv::Vec3b*)&l_image_msg->data[0],
+ l_image_msg->step);
+ for (int v = 0; v < mat.rows; ++v)
+ {
+ for (int u = 0; u < mat.cols; ++u, ++iter_r, ++iter_g, ++iter_b)
+ {
+ const cv::Vec3b& rgb = color(v,u);
+ *iter_r = rgb[0];
+ *iter_g = rgb[1];
+ *iter_b = rgb[2];
+ }
+ }
+ }
+ else if (encoding == enc::BGR8)
+ {
+ const cv::Mat_<cv::Vec3b> color(l_image_msg->height, l_image_msg->width,
+ (cv::Vec3b*)&l_image_msg->data[0],
+ l_image_msg->step);
+ for (int v = 0; v < mat.rows; ++v)
+ {
+ for (int u = 0; u < mat.cols; ++u, ++iter_r, ++iter_g, ++iter_b)
+ {
+ const cv::Vec3b& bgr = color(v,u);
+ *iter_r = bgr[2];
+ *iter_g = bgr[1];
+ *iter_b = bgr[0];
+ }
+ }
+ }
+ else
+ {
+ NODELET_WARN_THROTTLE(30, "Could not fill color channel of the point cloud, "
+ "unsupported encoding '%s'", encoding.c_str());
+ }
+
+ pub_points2_.publish(points_msg);
+}
+
+} // namespace stereo_image_proc
+
+// Register nodelet
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(stereo_image_proc::PointCloud2Nodelet,nodelet::Nodelet)
diff --git a/stereo_image_proc/src/nodes/stereo_image_proc.cpp b/stereo_image_proc/src/nodes/stereo_image_proc.cpp
new file mode 100644
index 0000000..df83302
--- /dev/null
+++ b/stereo_image_proc/src/nodes/stereo_image_proc.cpp
@@ -0,0 +1,149 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#include <ros/ros.h>
+#include <nodelet/loader.h>
+#include <image_proc/advertisement_checker.h>
+
+void loadMonocularNodelets(nodelet::Loader& manager, const std::string& side,
+ const XmlRpc::XmlRpcValue& rectify_params,
+ const nodelet::V_string& my_argv)
+{
+ nodelet::M_string remappings;
+
+ // Explicitly resolve global remappings (wg-ros-pkg #5055).
+ // Otherwise the internal remapping 'image_raw' -> 'left/image_raw' can hide a
+ // global remapping from the launch file or command line.
+ std::string image_raw_topic = ros::names::resolve(side + "/image_raw");
+ std::string image_mono_topic = ros::names::resolve(side + "/image_mono");
+ std::string image_color_topic = ros::names::resolve(side + "/image_color");
+ std::string image_rect_topic = ros::names::resolve(side + "/image_rect");
+ std::string image_rect_color_topic = ros::names::resolve(side + "/image_rect_color");
+ std::string camera_info_topic = ros::names::resolve(side + "/camera_info");
+
+ // Debayer nodelet: image_raw -> image_mono, image_color
+ remappings["image_raw"] = image_raw_topic;
+ remappings["image_mono"] = image_mono_topic;
+ remappings["image_color"] = image_color_topic;
+ std::string debayer_name = ros::this_node::getName() + "_debayer_" + side;
+ manager.load(debayer_name, "image_proc/debayer", remappings, my_argv);
+
+ // Rectify nodelet: image_mono -> image_rect
+ remappings.clear();
+ remappings["image_mono"] = image_mono_topic;
+ remappings["camera_info"] = camera_info_topic;
+ remappings["image_rect"] = image_rect_topic;
+ std::string rectify_mono_name = ros::this_node::getName() + "_rectify_mono_" + side;
+ if (rectify_params.valid())
+ ros::param::set(rectify_mono_name, rectify_params);
+ manager.load(rectify_mono_name, "image_proc/rectify", remappings, my_argv);
+
+ // Rectify nodelet: image_color -> image_rect_color
+ remappings.clear();
+ remappings["image_mono"] = image_color_topic;
+ remappings["camera_info"] = camera_info_topic;
+ remappings["image_rect"] = image_rect_color_topic;
+ std::string rectify_color_name = ros::this_node::getName() + "_rectify_color_" + side;
+ if (rectify_params.valid())
+ ros::param::set(rectify_color_name, rectify_params);
+ manager.load(rectify_color_name, "image_proc/rectify", remappings, my_argv);
+}
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv, "stereo_image_proc");
+
+ // Check for common user errors
+ if (ros::names::remap("camera") != "camera")
+ {
+ ROS_WARN("Remapping 'camera' has no effect! Start stereo_image_proc in the "
+ "stereo namespace instead.\nExample command-line usage:\n"
+ "\t$ ROS_NAMESPACE=%s rosrun stereo_image_proc stereo_image_proc",
+ ros::names::remap("camera").c_str());
+ }
+ if (ros::this_node::getNamespace() == "/")
+ {
+ ROS_WARN("Started in the global namespace! This is probably wrong. Start "
+ "stereo_image_proc in the stereo namespace.\nExample command-line usage:\n"
+ "\t$ ROS_NAMESPACE=my_stereo rosrun stereo_image_proc stereo_image_proc");
+ }
+
+ // Shared parameters to be propagated to nodelet private namespaces
+ ros::NodeHandle private_nh("~");
+ XmlRpc::XmlRpcValue shared_params;
+ int queue_size;
+ if (private_nh.getParam("queue_size", queue_size))
+ shared_params["queue_size"] = queue_size;
+
+ nodelet::Loader manager(false); // Don't bring up the manager ROS API
+ nodelet::M_string remappings;
+ nodelet::V_string my_argv;
+
+ // Load equivalents of image_proc for left and right cameras
+ loadMonocularNodelets(manager, "left", shared_params, my_argv);
+ loadMonocularNodelets(manager, "right", shared_params, my_argv);
+
+ // Stereo nodelets also need to know the synchronization policy
+ bool approx_sync;
+ if (private_nh.getParam("approximate_sync", approx_sync))
+ shared_params["approximate_sync"] = XmlRpc::XmlRpcValue(approx_sync);
+
+ // Disparity nodelet
+ // Inputs: left/image_rect, left/camera_info, right/image_rect, right/camera_info
+ // Outputs: disparity
+ // NOTE: Using node name for the disparity nodelet because it is the only one using
+ // dynamic_reconfigure so far, and this makes us backwards-compatible with cturtle.
+ std::string disparity_name = ros::this_node::getName();
+ manager.load(disparity_name, "stereo_image_proc/disparity", remappings, my_argv);
+
+ // PointCloud2 nodelet
+ // Inputs: left/image_rect_color, left/camera_info, right/camera_info, disparity
+ // Outputs: points2
+ std::string point_cloud2_name = ros::this_node::getName() + "_point_cloud2";
+ if (shared_params.valid())
+ ros::param::set(point_cloud2_name, shared_params);
+ manager.load(point_cloud2_name, "stereo_image_proc/point_cloud2", remappings, my_argv);
+
+ // Check for only the original camera topics
+ ros::V_string topics;
+ topics.push_back(ros::names::resolve("left/image_raw"));
+ topics.push_back(ros::names::resolve("left/camera_info"));
+ topics.push_back(ros::names::resolve("right/image_raw"));
+ topics.push_back(ros::names::resolve("right/camera_info"));
+ image_proc::AdvertisementChecker check_inputs(ros::NodeHandle(), ros::this_node::getName());
+ check_inputs.start(topics, 60.0);
+
+ ros::spin();
+ return 0;
+}
diff --git a/wiki_files/dcam-driver.svg b/wiki_files/dcam-driver.svg
new file mode 100644
index 0000000..5bbf246
--- /dev/null
+++ b/wiki_files/dcam-driver.svg
@@ -0,0 +1,249 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<!-- Created with Inkscape (http://www.inkscape.org/) -->
+<svg
+ xmlns:dc="http://purl.org/dc/elements/1.1/"
+ xmlns:cc="http://creativecommons.org/ns#"
+ xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
+ xmlns:svg="http://www.w3.org/2000/svg"
+ xmlns="http://www.w3.org/2000/svg"
+ xmlns:xlink="http://www.w3.org/1999/xlink"
+ xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
+ xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
+ width="744.09448819"
+ height="1052.3622047"
+ id="svg2"
+ sodipodi:version="0.32"
+ inkscape:version="0.46"
+ sodipodi:docname="dcam-driver.svg"
+ inkscape:output_extension="org.inkscape.output.svg.inkscape"
+ inkscape:export-filename="/u/konolige/writings/docs/dcam-driver.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90">
+ <defs
+ id="defs4">
+ <marker
+ inkscape:stockid="Arrow1Mend"
+ orient="auto"
+ refY="0.0"
+ refX="0.0"
+ id="Arrow1Mend"
+ style="overflow:visible;">
+ <path
+ id="path5073"
+ d="M 0.0,0.0 L 5.0,-5.0 L -12.5,0.0 L 5.0,5.0 L 0.0,0.0 z "
+ style="fill-rule:evenodd;stroke:#000000;stroke-width:1.0pt;marker-start:none;"
+ transform="scale(0.4) rotate(180) translate(10,0)" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow2Mend"
+ orient="auto"
+ refY="0.0"
+ refX="0.0"
+ id="Arrow2Mend"
+ style="overflow:visible;">
+ <path
+ id="path5091"
+ style="font-size:12.0;fill-rule:evenodd;stroke-width:0.62500000;stroke-linejoin:round;"
+ d="M 8.7185878,4.0337352 L -2.2072895,0.016013256 L 8.7185884,-4.0017078 C 6.9730900,-1.6296469 6.9831476,1.6157441 8.7185878,4.0337352 z "
+ transform="scale(0.6) rotate(180) translate(0,0)" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow1Mstart"
+ orient="auto"
+ refY="0.0"
+ refX="0.0"
+ id="Arrow1Mstart"
+ style="overflow:visible">
+ <path
+ id="path5070"
+ d="M 0.0,0.0 L 5.0,-5.0 L -12.5,0.0 L 5.0,5.0 L 0.0,0.0 z "
+ style="fill-rule:evenodd;stroke:#000000;stroke-width:1.0pt;marker-start:none"
+ transform="scale(0.4) translate(10,0)" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow1Lstart"
+ orient="auto"
+ refY="0.0"
+ refX="0.0"
+ id="Arrow1Lstart"
+ style="overflow:visible">
+ <path
+ id="path5064"
+ d="M 0.0,0.0 L 5.0,-5.0 L -12.5,0.0 L 5.0,5.0 L 0.0,0.0 z "
+ style="fill-rule:evenodd;stroke:#000000;stroke-width:1.0pt;marker-start:none"
+ transform="scale(0.8) translate(12.5,0)" />
+ </marker>
+ <linearGradient
+ inkscape:collect="always"
+ id="linearGradient3169">
+ <stop
+ style="stop-color:#000000;stop-opacity:1;"
+ offset="0"
+ id="stop3171" />
+ <stop
+ style="stop-color:#000000;stop-opacity:0;"
+ offset="1"
+ id="stop3173" />
+ </linearGradient>
+ <inkscape:perspective
+ sodipodi:type="inkscape:persp3d"
+ inkscape:vp_x="0 : 526.18109 : 1"
+ inkscape:vp_y="0 : 1000 : 0"
+ inkscape:vp_z="744.09448 : 526.18109 : 1"
+ inkscape:persp3d-origin="372.04724 : 350.78739 : 1"
+ id="perspective10" />
+ <radialGradient
+ inkscape:collect="always"
+ xlink:href="#linearGradient3169"
+ id="radialGradient3175"
+ cx="119.73291"
+ cy="250.98525"
+ fx="119.73291"
+ fy="250.98525"
+ r="20.210449"
+ gradientTransform="matrix(1,0,0,0.7009978,0,75.045142)"
+ gradientUnits="userSpaceOnUse" />
+ <radialGradient
+ inkscape:collect="always"
+ xlink:href="#linearGradient3169"
+ id="radialGradient5043"
+ gradientUnits="userSpaceOnUse"
+ gradientTransform="matrix(1,0,0,0.7009978,0,75.045142)"
+ cx="119.73291"
+ cy="250.98525"
+ fx="119.73291"
+ fy="250.98525"
+ r="20.210449" />
+ </defs>
+ <sodipodi:namedview
+ id="base"
+ pagecolor="#ffffff"
+ bordercolor="#666666"
+ borderopacity="1.0"
+ gridtolerance="10000"
+ guidetolerance="10"
+ objecttolerance="10"
+ inkscape:pageopacity="0.0"
+ inkscape:pageshadow="2"
+ inkscape:zoom="2.801202"
+ inkscape:cx="242.49547"
+ inkscape:cy="792.8602"
+ inkscape:document-units="px"
+ inkscape:current-layer="layer1"
+ showgrid="true"
+ inkscape:snap-global="false"
+ showguides="false"
+ inkscape:window-width="1375"
+ inkscape:window-height="1251"
+ inkscape:window-x="413"
+ inkscape:window-y="61">
+ <inkscape:grid
+ type="xygrid"
+ id="grid2383" />
+ </sodipodi:namedview>
+ <metadata
+ id="metadata7">
+ <rdf:RDF>
+ <cc:Work
+ rdf:about="">
+ <dc:format>image/svg+xml</dc:format>
+ <dc:type
+ rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
+ </cc:Work>
+ </rdf:RDF>
+ </metadata>
+ <g
+ inkscape:label="Layer 1"
+ inkscape:groupmode="layer"
+ id="layer1">
+ <g
+ id="g5021"
+ transform="translate(22.790548,-5.6640625e-2)">
+ <rect
+ ry="8.374341"
+ y="231.35966"
+ x="90.145569"
+ height="40.856945"
+ width="62.178101"
+ id="rect2385"
+ style="opacity:0.31999996;fill:#0000ff;fill-rule:evenodd;stroke:#000000;stroke-width:1.29076612;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
+ <flowRoot
+ style="font-size:14px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"
+ id="flowRoot2389"
+ xml:space="preserve"><flowRegion
+ style="fill:url(#radialGradient3175);fill-opacity:1"
+ id="flowRegion2391"><rect
+ style="font-size:14px;fill:#000000;fill-opacity:1"
+ y="233.51537"
+ x="98.755394"
+ height="64.808228"
+ width="82.296165"
+ id="rect2393" /></flowRegion><flowPara
+ style="fill:#000000;fill-opacity:1"
+ id="flowPara2395">camera </flowPara><flowPara
+ style="fill:#000000;fill-opacity:1"
+ id="flowPara2397">driver</flowPara></flowRoot> </g>
+ <g
+ id="g6362">
+ <rect
+ rx="0"
+ ry="8.3892555"
+ y="231.86771"
+ x="30.586723"
+ height="40.92971"
+ width="55.266777"
+ id="rect5031"
+ style="opacity:0.31999996;fill:#0000ff;fill-rule:evenodd;stroke:#000000;stroke-width:1.21800005;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
+ <flowRoot
+ transform="translate(-57.257396,8.4511111)"
+ style="font-size:14px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"
+ id="flowRoot5033"
+ xml:space="preserve"><flowRegion
+ style="fill:url(#radialGradient5043);fill-opacity:1"
+ id="flowRegion5035"><rect
+ style="font-size:14px;fill:#000000;fill-opacity:1"
+ y="233.51537"
+ x="98.755394"
+ height="64.808228"
+ width="82.296165"
+ id="rect5037" /></flowRegion><flowPara
+ style="fill:#000000;fill-opacity:1"
+ id="flowPara5041">Cam</flowPara></flowRoot> <path
+ sodipodi:nodetypes="cccc"
+ id="path5053"
+ d="M 10,232.36218 C 30,252.36218 30,252.36218 30,252.36218 L 10,272.36218 L 10,232.36218 z"
+ style="fill:#0000ff;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;fill-opacity:1;opacity:0.32758621" />
+ </g>
+ <path
+ style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;display:inline;opacity:1;marker-start:url(#Arrow1Mstart)"
+ d="M 112.29073,251.93773 L 86.4625,252.12633"
+ id="path5059"
+ inkscape:connector-type="polyline"
+ inkscape:connection-start="#g5021"
+ inkscape:connection-end="#rect5031" />
+ <text
+ xml:space="preserve"
+ style="font-size:10px;font-style:normal;font-weight:bold;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans;-inkscape-font-specification:Bitstream Vera Sans Bold"
+ x="175"
+ y="212.36218"
+ id="text7959"><tspan
+ sodipodi:role="line"
+ id="tspan7961"
+ x="175"
+ y="212.36218">/<camera></tspan><tspan
+ sodipodi:role="line"
+ x="175"
+ y="224.86218"
+ id="tspan2675"> /camera_info [CameraInfo]</tspan><tspan
+ sodipodi:role="line"
+ x="175"
+ y="237.36217"
+ id="tspan7963"> /image_raw [Image]</tspan></text>
+ <path
+ style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;marker-end:url(#Arrow1Mend)"
+ d="M 175.7596,251.71374 L 317.36376,251.6345"
+ id="path2699"
+ inkscape:connector-type="polyline"
+ inkscape:connection-start="#g5021" />
+ </g>
+</svg>
diff --git a/wiki_files/image_proc.svg b/wiki_files/image_proc.svg
new file mode 100644
index 0000000..cae39c1
--- /dev/null
+++ b/wiki_files/image_proc.svg
@@ -0,0 +1,335 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<!-- Created with Inkscape (http://www.inkscape.org/) -->
+<svg
+ xmlns:dc="http://purl.org/dc/elements/1.1/"
+ xmlns:cc="http://creativecommons.org/ns#"
+ xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
+ xmlns:svg="http://www.w3.org/2000/svg"
+ xmlns="http://www.w3.org/2000/svg"
+ xmlns:xlink="http://www.w3.org/1999/xlink"
+ xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
+ xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
+ width="744.09448819"
+ height="1052.3622047"
+ id="svg2"
+ sodipodi:version="0.32"
+ inkscape:version="0.46"
+ sodipodi:docname="image_proc.svg"
+ inkscape:output_extension="org.inkscape.output.svg.inkscape"
+ inkscape:export-filename="/u/mihelich/ros/pkgs/ros-pkg-trunk/stacks/image_pipeline/wiki_files/image_proc.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90">
+ <defs
+ id="defs4">
+ <marker
+ inkscape:stockid="Arrow1Mend"
+ orient="auto"
+ refY="0.0"
+ refX="0.0"
+ id="Arrow1Mend"
+ style="overflow:visible;">
+ <path
+ id="path5073"
+ d="M 0.0,0.0 L 5.0,-5.0 L -12.5,0.0 L 5.0,5.0 L 0.0,0.0 z "
+ style="fill-rule:evenodd;stroke:#000000;stroke-width:1.0pt;marker-start:none;"
+ transform="scale(0.4) rotate(180) translate(10,0)" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow2Mend"
+ orient="auto"
+ refY="0.0"
+ refX="0.0"
+ id="Arrow2Mend"
+ style="overflow:visible;">
+ <path
+ id="path5091"
+ style="font-size:12.0;fill-rule:evenodd;stroke-width:0.62500000;stroke-linejoin:round;"
+ d="M 8.7185878,4.0337352 L -2.2072895,0.016013256 L 8.7185884,-4.0017078 C 6.9730900,-1.6296469 6.9831476,1.6157441 8.7185878,4.0337352 z "
+ transform="scale(0.6) rotate(180) translate(0,0)" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow1Mstart"
+ orient="auto"
+ refY="0.0"
+ refX="0.0"
+ id="Arrow1Mstart"
+ style="overflow:visible">
+ <path
+ id="path5070"
+ d="M 0.0,0.0 L 5.0,-5.0 L -12.5,0.0 L 5.0,5.0 L 0.0,0.0 z "
+ style="fill-rule:evenodd;stroke:#000000;stroke-width:1.0pt;marker-start:none"
+ transform="scale(0.4) translate(10,0)" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow1Lstart"
+ orient="auto"
+ refY="0.0"
+ refX="0.0"
+ id="Arrow1Lstart"
+ style="overflow:visible">
+ <path
+ id="path5064"
+ d="M 0.0,0.0 L 5.0,-5.0 L -12.5,0.0 L 5.0,5.0 L 0.0,0.0 z "
+ style="fill-rule:evenodd;stroke:#000000;stroke-width:1.0pt;marker-start:none"
+ transform="scale(0.8) translate(12.5,0)" />
+ </marker>
+ <linearGradient
+ inkscape:collect="always"
+ id="linearGradient3169">
+ <stop
+ style="stop-color:#000000;stop-opacity:1;"
+ offset="0"
+ id="stop3171" />
+ <stop
+ style="stop-color:#000000;stop-opacity:0;"
+ offset="1"
+ id="stop3173" />
+ </linearGradient>
+ <inkscape:perspective
+ sodipodi:type="inkscape:persp3d"
+ inkscape:vp_x="0 : 526.18109 : 1"
+ inkscape:vp_y="0 : 1000 : 0"
+ inkscape:vp_z="744.09448 : 526.18109 : 1"
+ inkscape:persp3d-origin="372.04724 : 350.78739 : 1"
+ id="perspective10" />
+ <radialGradient
+ inkscape:collect="always"
+ xlink:href="#linearGradient3169"
+ id="radialGradient3175"
+ cx="119.73291"
+ cy="250.98525"
+ fx="119.73291"
+ fy="250.98525"
+ r="20.210449"
+ gradientTransform="matrix(1,0,0,0.7009978,0,75.045142)"
+ gradientUnits="userSpaceOnUse" />
+ <radialGradient
+ inkscape:collect="always"
+ xlink:href="#linearGradient3169"
+ id="radialGradient5043"
+ gradientUnits="userSpaceOnUse"
+ gradientTransform="matrix(1,0,0,0.7009978,0,75.045142)"
+ cx="119.73291"
+ cy="250.98525"
+ fx="119.73291"
+ fy="250.98525"
+ r="20.210449" />
+ <radialGradient
+ inkscape:collect="always"
+ xlink:href="#linearGradient3169"
+ id="radialGradient8143"
+ gradientUnits="userSpaceOnUse"
+ gradientTransform="matrix(1,0,0,0.7009978,0,75.045142)"
+ cx="119.73291"
+ cy="250.98525"
+ fx="119.73291"
+ fy="250.98525"
+ r="20.210449" />
+ </defs>
+ <sodipodi:namedview
+ id="base"
+ pagecolor="#ffffff"
+ bordercolor="#666666"
+ borderopacity="1.0"
+ gridtolerance="10000"
+ guidetolerance="10"
+ objecttolerance="10"
+ inkscape:pageopacity="0.0"
+ inkscape:pageshadow="2"
+ inkscape:zoom="1.400601"
+ inkscape:cx="218.84605"
+ inkscape:cy="792.8602"
+ inkscape:document-units="px"
+ inkscape:current-layer="g8129"
+ showgrid="true"
+ inkscape:snap-global="true"
+ showguides="false"
+ inkscape:window-width="1663"
+ inkscape:window-height="1016"
+ inkscape:window-x="93"
+ inkscape:window-y="251">
+ <inkscape:grid
+ type="xygrid"
+ id="grid2383" />
+ </sodipodi:namedview>
+ <metadata
+ id="metadata7">
+ <rdf:RDF>
+ <cc:Work
+ rdf:about="">
+ <dc:format>image/svg+xml</dc:format>
+ <dc:type
+ rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
+ </cc:Work>
+ </rdf:RDF>
+ </metadata>
+ <g
+ inkscape:label="Layer 1"
+ inkscape:groupmode="layer"
+ id="layer1">
+ <g
+ id="g4195">
+ <rect
+ ry="8.3639584"
+ y="231.32835"
+ x="112.96144"
+ height="40.806286"
+ width="67.23764"
+ id="rect2385"
+ style="opacity:0.31999996;fill:#0000ff;fill-rule:evenodd;stroke:#000000;stroke-width:1.3414228;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
+ <flowRoot
+ transform="translate(22.790548,-5.6640625e-2)"
+ style="font-size:14px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"
+ id="flowRoot2389"
+ xml:space="preserve"><flowRegion
+ style="fill:url(#radialGradient3175);fill-opacity:1"
+ id="flowRegion2391"><rect
+ style="font-size:14px;fill:#000000;fill-opacity:1"
+ y="233.51537"
+ x="98.755394"
+ height="64.808228"
+ width="82.296165"
+ id="rect2393" /></flowRegion><flowPara
+ style="fill:#000000;fill-opacity:1"
+ id="flowPara2395">camera </flowPara><flowPara
+ style="fill:#000000;fill-opacity:1"
+ id="flowPara2397">driver</flowPara></flowRoot> </g>
+ <g
+ id="g6362">
+ <rect
+ rx="0"
+ ry="8.3892555"
+ y="231.86771"
+ x="30.586723"
+ height="40.92971"
+ width="55.266777"
+ id="rect5031"
+ style="opacity:0.31999996;fill:#0000ff;fill-rule:evenodd;stroke:#000000;stroke-width:1.21800005;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
+ <flowRoot
+ transform="translate(-57.257396,8.4511111)"
+ style="font-size:14px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"
+ id="flowRoot5033"
+ xml:space="preserve"><flowRegion
+ style="fill:url(#radialGradient5043);fill-opacity:1"
+ id="flowRegion5035"><rect
+ style="font-size:14px;fill:#000000;fill-opacity:1"
+ y="233.51537"
+ x="98.755394"
+ height="64.808228"
+ width="82.296165"
+ id="rect5037" /></flowRegion><flowPara
+ style="fill:#000000;fill-opacity:1"
+ id="flowPara5041">Cam</flowPara></flowRoot> <path
+ sodipodi:nodetypes="cccc"
+ id="path5053"
+ d="M 10,232.36218 C 30,252.36218 30,252.36218 30,252.36218 L 10,272.36218 L 10,232.36218 z"
+ style="opacity:0.32758622;fill:#0000ff;fill-opacity:1;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" />
+ </g>
+ <path
+ style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;marker-start:url(#Arrow1Mstart);stroke-opacity:1;display:inline"
+ d="M 112.29073,251.93773 L 86.4625,252.12633"
+ id="path5059"
+ inkscape:connector-type="polyline"
+ inkscape:connection-end="#rect5031" />
+ <path
+ style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;marker-start:url(#Arrow1Mstart);stroke-opacity:1;display:inline"
+ d="M 112.29073,251.93773 L 86.4625,252.12633"
+ id="path8096"
+ inkscape:connector-type="polyline"
+ inkscape:connection-end="#rect5031" />
+ <g
+ id="g8129"
+ transform="matrix(1.99653,0,0,1.9203567,181.21906,-211.00774)">
+ <g
+ id="g8171"
+ transform="translate(-12.694916,-7.9273828)">
+ <g
+ id="g9861"
+ transform="translate(0.3745711,-0.5532633)">
+ <rect
+ ry="7.2690282"
+ y="231.25214"
+ x="90.043663"
+ height="35.464317"
+ width="50.283207"
+ id="rect8131"
+ style="opacity:0.31999996;fill:#0000ff;fill-rule:evenodd;stroke:#000000;stroke-width:1.08144188;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
+ <flowRoot
+ style="font-size:7.30719709px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"
+ id="flowRoot8133"
+ xml:space="preserve"
+ transform="translate(3.0138839,4.9403975)"><flowRegion
+ style="fill:url(#radialGradient8143);fill-opacity:1"
+ id="flowRegion8135"><rect
+ style="font-size:7.30719709px;fill:#000000;fill-opacity:1"
+ y="233.51537"
+ x="98.755394"
+ height="64.808228"
+ width="82.296165"
+ id="rect8137" /></flowRegion><flowPara
+ style="font-size:7.30719709px;fill:#000000;fill-opacity:1"
+ id="flowPara8167">image_</flowPara><flowPara
+ style="font-size:7.30719709px;fill:#000000;fill-opacity:1"
+ id="flowPara8169">proc</flowPara></flowRoot> </g>
+ </g>
+ </g>
+ <path
+ style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:0.96976066px;stroke-linecap:butt;stroke-linejoin:miter;marker-end:url(#Arrow1Mend);stroke-opacity:1"
+ d="M 180.10283,251.63476 L 335.38226,251.02959"
+ id="path9869"
+ inkscape:connector-type="polyline" />
+ <path
+ style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;marker-end:url(#Arrow1Mend);stroke-opacity:1"
+ d="M 437.86749,250.9947 L 534.77042,251.27752"
+ id="path10404"
+ inkscape:connector-type="polyline"
+ inkscape:connection-start="#g8129" />
+ <text
+ xml:space="preserve"
+ style="font-size:10px;font-style:normal;font-weight:bold;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans;-inkscape-font-specification:Bitstream Vera Sans Bold"
+ x="180"
+ y="202.36218"
+ id="text4177"><tspan
+ sodipodi:role="line"
+ id="tspan4179"
+ x="180"
+ y="202.36218" /><tspan
+ sodipodi:role="line"
+ x="180"
+ y="214.86218"
+ id="tspan2675">camera_info [CameraInfo]</tspan><tspan
+ sodipodi:role="line"
+ x="180"
+ y="227.36218"
+ id="tspan4182">image_raw [Image]</tspan></text>
+ <text
+ xml:space="preserve"
+ style="font-size:10px;font-style:normal;font-weight:bold;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans;-inkscape-font-specification:Bitstream Vera Sans Bold"
+ x="450"
+ y="192.36218"
+ id="text7959"
+ inkscape:export-filename="/wg/adt/mihelich/ros/pkgs/ros-pkg-trunk/stacks/image_pipeline/wiki_files/image_proc.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90"><tspan
+ sodipodi:role="line"
+ id="tspan7961"
+ x="450"
+ y="192.36218" /><tspan
+ sodipodi:role="line"
+ x="450"
+ y="204.86218"
+ id="tspan2431">image_mono [Image]</tspan><tspan
+ sodipodi:role="line"
+ x="450"
+ y="217.36218"
+ id="tspan4192">image_color [Image]</tspan><tspan
+ sodipodi:role="line"
+ x="450"
+ y="229.86218"
+ id="tspan7963">image_rect [Image]</tspan><tspan
+ sodipodi:role="line"
+ x="450"
+ y="242.36218"
+ id="tspan2429">image_rect_color [Image]</tspan></text>
+ </g>
+</svg>
diff --git a/wiki_files/image_proc_dual.svg b/wiki_files/image_proc_dual.svg
new file mode 100644
index 0000000..887bd24
--- /dev/null
+++ b/wiki_files/image_proc_dual.svg
@@ -0,0 +1,335 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<!-- Created with Inkscape (http://www.inkscape.org/) -->
+<svg
+ xmlns:dc="http://purl.org/dc/elements/1.1/"
+ xmlns:cc="http://creativecommons.org/ns#"
+ xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
+ xmlns:svg="http://www.w3.org/2000/svg"
+ xmlns="http://www.w3.org/2000/svg"
+ xmlns:xlink="http://www.w3.org/1999/xlink"
+ xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
+ xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
+ width="744.09448819"
+ height="1052.3622047"
+ id="svg2"
+ sodipodi:version="0.32"
+ inkscape:version="0.46"
+ sodipodi:docname="image_proc_dual.svg"
+ inkscape:output_extension="org.inkscape.output.svg.inkscape"
+ inkscape:export-filename="/u/konolige/writings/docs/image_proc.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90">
+ <defs
+ id="defs4">
+ <marker
+ inkscape:stockid="Arrow1Mend"
+ orient="auto"
+ refY="0.0"
+ refX="0.0"
+ id="Arrow1Mend"
+ style="overflow:visible;">
+ <path
+ id="path5073"
+ d="M 0.0,0.0 L 5.0,-5.0 L -12.5,0.0 L 5.0,5.0 L 0.0,0.0 z "
+ style="fill-rule:evenodd;stroke:#000000;stroke-width:1.0pt;marker-start:none;"
+ transform="scale(0.4) rotate(180) translate(10,0)" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow2Mend"
+ orient="auto"
+ refY="0.0"
+ refX="0.0"
+ id="Arrow2Mend"
+ style="overflow:visible;">
+ <path
+ id="path5091"
+ style="font-size:12.0;fill-rule:evenodd;stroke-width:0.62500000;stroke-linejoin:round;"
+ d="M 8.7185878,4.0337352 L -2.2072895,0.016013256 L 8.7185884,-4.0017078 C 6.9730900,-1.6296469 6.9831476,1.6157441 8.7185878,4.0337352 z "
+ transform="scale(0.6) rotate(180) translate(0,0)" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow1Mstart"
+ orient="auto"
+ refY="0.0"
+ refX="0.0"
+ id="Arrow1Mstart"
+ style="overflow:visible">
+ <path
+ id="path5070"
+ d="M 0.0,0.0 L 5.0,-5.0 L -12.5,0.0 L 5.0,5.0 L 0.0,0.0 z "
+ style="fill-rule:evenodd;stroke:#000000;stroke-width:1.0pt;marker-start:none"
+ transform="scale(0.4) translate(10,0)" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow1Lstart"
+ orient="auto"
+ refY="0.0"
+ refX="0.0"
+ id="Arrow1Lstart"
+ style="overflow:visible">
+ <path
+ id="path5064"
+ d="M 0.0,0.0 L 5.0,-5.0 L -12.5,0.0 L 5.0,5.0 L 0.0,0.0 z "
+ style="fill-rule:evenodd;stroke:#000000;stroke-width:1.0pt;marker-start:none"
+ transform="scale(0.8) translate(12.5,0)" />
+ </marker>
+ <linearGradient
+ inkscape:collect="always"
+ id="linearGradient3169">
+ <stop
+ style="stop-color:#000000;stop-opacity:1;"
+ offset="0"
+ id="stop3171" />
+ <stop
+ style="stop-color:#000000;stop-opacity:0;"
+ offset="1"
+ id="stop3173" />
+ </linearGradient>
+ <inkscape:perspective
+ sodipodi:type="inkscape:persp3d"
+ inkscape:vp_x="0 : 526.18109 : 1"
+ inkscape:vp_y="0 : 1000 : 0"
+ inkscape:vp_z="744.09448 : 526.18109 : 1"
+ inkscape:persp3d-origin="372.04724 : 350.78739 : 1"
+ id="perspective10" />
+ <radialGradient
+ inkscape:collect="always"
+ xlink:href="#linearGradient3169"
+ id="radialGradient3175"
+ cx="119.73291"
+ cy="250.98525"
+ fx="119.73291"
+ fy="250.98525"
+ r="20.210449"
+ gradientTransform="matrix(1,0,0,0.7009978,0,75.045142)"
+ gradientUnits="userSpaceOnUse" />
+ <radialGradient
+ inkscape:collect="always"
+ xlink:href="#linearGradient3169"
+ id="radialGradient5043"
+ gradientUnits="userSpaceOnUse"
+ gradientTransform="matrix(1,0,0,0.7009978,0,75.045142)"
+ cx="119.73291"
+ cy="250.98525"
+ fx="119.73291"
+ fy="250.98525"
+ r="20.210449" />
+ <radialGradient
+ inkscape:collect="always"
+ xlink:href="#linearGradient3169"
+ id="radialGradient8143"
+ gradientUnits="userSpaceOnUse"
+ gradientTransform="matrix(1,0,0,0.7009978,0,75.045142)"
+ cx="119.73291"
+ cy="250.98525"
+ fx="119.73291"
+ fy="250.98525"
+ r="20.210449" />
+ </defs>
+ <sodipodi:namedview
+ id="base"
+ pagecolor="#ffffff"
+ bordercolor="#666666"
+ borderopacity="1.0"
+ gridtolerance="10000"
+ guidetolerance="10"
+ objecttolerance="10"
+ inkscape:pageopacity="0.0"
+ inkscape:pageshadow="2"
+ inkscape:zoom="1.400601"
+ inkscape:cx="218.84605"
+ inkscape:cy="792.8602"
+ inkscape:document-units="px"
+ inkscape:current-layer="layer1"
+ showgrid="true"
+ inkscape:snap-global="true"
+ showguides="false"
+ inkscape:window-width="1236"
+ inkscape:window-height="695"
+ inkscape:window-x="133"
+ inkscape:window-y="47">
+ <inkscape:grid
+ type="xygrid"
+ id="grid2383" />
+ </sodipodi:namedview>
+ <metadata
+ id="metadata7">
+ <rdf:RDF>
+ <cc:Work
+ rdf:about="">
+ <dc:format>image/svg+xml</dc:format>
+ <dc:type
+ rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
+ </cc:Work>
+ </rdf:RDF>
+ </metadata>
+ <g
+ inkscape:label="Layer 1"
+ inkscape:groupmode="layer"
+ id="layer1">
+ <g
+ id="g4195">
+ <rect
+ ry="8.3639584"
+ y="231.32835"
+ x="112.96144"
+ height="40.806286"
+ width="67.23764"
+ id="rect2385"
+ style="opacity:0.31999996;fill:#0000ff;fill-rule:evenodd;stroke:#000000;stroke-width:1.3414228;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
+ <flowRoot
+ transform="translate(22.790548,-5.6640625e-2)"
+ style="font-size:14px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"
+ id="flowRoot2389"
+ xml:space="preserve"><flowRegion
+ style="fill:url(#radialGradient3175);fill-opacity:1"
+ id="flowRegion2391"><rect
+ style="font-size:14px;fill:#000000;fill-opacity:1"
+ y="233.51537"
+ x="98.755394"
+ height="64.808228"
+ width="82.296165"
+ id="rect2393" /></flowRegion><flowPara
+ style="fill:#000000;fill-opacity:1"
+ id="flowPara2395">camera </flowPara><flowPara
+ style="fill:#000000;fill-opacity:1"
+ id="flowPara2397">driver</flowPara></flowRoot> </g>
+ <g
+ id="g6362">
+ <rect
+ rx="0"
+ ry="8.3892555"
+ y="231.86771"
+ x="30.586723"
+ height="40.92971"
+ width="55.266777"
+ id="rect5031"
+ style="opacity:0.31999996;fill:#0000ff;fill-rule:evenodd;stroke:#000000;stroke-width:1.21800005;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
+ <flowRoot
+ transform="translate(-57.257396,8.4511111)"
+ style="font-size:14px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"
+ id="flowRoot5033"
+ xml:space="preserve"><flowRegion
+ style="fill:url(#radialGradient5043);fill-opacity:1"
+ id="flowRegion5035"><rect
+ style="font-size:14px;fill:#000000;fill-opacity:1"
+ y="233.51537"
+ x="98.755394"
+ height="64.808228"
+ width="82.296165"
+ id="rect5037" /></flowRegion><flowPara
+ style="fill:#000000;fill-opacity:1"
+ id="flowPara5041">Cam</flowPara></flowRoot> <path
+ sodipodi:nodetypes="cccc"
+ id="path5053"
+ d="M 10,232.36218 C 30,252.36218 30,252.36218 30,252.36218 L 10,272.36218 L 10,232.36218 z"
+ style="opacity:0.32758622;fill:#0000ff;fill-opacity:1;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" />
+ </g>
+ <path
+ style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;marker-start:url(#Arrow1Mstart);stroke-opacity:1;display:inline"
+ d="M 112.29073,251.93773 L 86.4625,252.12633"
+ id="path5059"
+ inkscape:connector-type="polyline"
+ inkscape:connection-end="#rect5031" />
+ <path
+ style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;marker-start:url(#Arrow1Mstart);stroke-opacity:1;display:inline"
+ d="M 112.29073,251.93773 L 86.4625,252.12633"
+ id="path8096"
+ inkscape:connector-type="polyline"
+ inkscape:connection-end="#rect5031" />
+ <g
+ id="g8129"
+ transform="matrix(1.99653,0,0,1.9203567,181.21906,-211.00774)">
+ <g
+ id="g8171"
+ transform="translate(-12.694916,-7.9273828)">
+ <g
+ id="g9861"
+ transform="translate(0.3745711,-0.5532633)">
+ <rect
+ ry="7.2690282"
+ y="231.25214"
+ x="90.043663"
+ height="35.464317"
+ width="50.283207"
+ id="rect8131"
+ style="opacity:0.31999996;fill:#0000ff;fill-rule:evenodd;stroke:#000000;stroke-width:1.08144188;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
+ <flowRoot
+ style="font-size:7.30719709px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"
+ id="flowRoot8133"
+ xml:space="preserve"
+ transform="translate(3.0138839,4.9403975)"><flowRegion
+ style="fill:url(#radialGradient8143);fill-opacity:1"
+ id="flowRegion8135"><rect
+ style="font-size:7.30719709px;fill:#000000;fill-opacity:1"
+ y="233.51537"
+ x="98.755394"
+ height="64.808228"
+ width="82.296165"
+ id="rect8137" /></flowRegion><flowPara
+ style="font-size:7.30719709px;fill:#000000;fill-opacity:1"
+ id="flowPara8167">image_</flowPara><flowPara
+ style="font-size:7.30719709px;fill:#000000;fill-opacity:1"
+ id="flowPara8169">proc</flowPara></flowRoot> </g>
+ </g>
+ </g>
+ <path
+ style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:0.96976066px;stroke-linecap:butt;stroke-linejoin:miter;marker-end:url(#Arrow1Mend);stroke-opacity:1"
+ d="M 180.10283,251.63476 L 335.38226,251.02959"
+ id="path9869"
+ inkscape:connector-type="polyline" />
+ <path
+ style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;marker-end:url(#Arrow1Mend);stroke-opacity:1"
+ d="M 437.86749,250.9947 L 534.77042,251.27752"
+ id="path10404"
+ inkscape:connector-type="polyline"
+ inkscape:connection-start="#g8129" />
+ <text
+ xml:space="preserve"
+ style="font-size:10px;font-style:normal;font-weight:bold;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans;-inkscape-font-specification:Bitstream Vera Sans Bold"
+ x="180"
+ y="202.36218"
+ id="text4177"><tspan
+ sodipodi:role="line"
+ id="tspan4179"
+ x="180"
+ y="202.36218">/<camera></tspan><tspan
+ sodipodi:role="line"
+ x="180"
+ y="214.86218"
+ id="tspan2675"> /camera_info [CameraInfo]</tspan><tspan
+ sodipodi:role="line"
+ x="180"
+ y="227.36218"
+ id="tspan4182"> /image_raw [Image]</tspan></text>
+ <text
+ xml:space="preserve"
+ style="font-size:10px;font-style:normal;font-weight:bold;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans;-inkscape-font-specification:Bitstream Vera Sans Bold"
+ x="440"
+ y="192.36218"
+ id="text7959"
+ inkscape:export-filename="/wg/stor2b/konolige/writings/docs/image_proc.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90"><tspan
+ sodipodi:role="line"
+ id="tspan7961"
+ x="440"
+ y="192.36218">/<camera></tspan><tspan
+ sodipodi:role="line"
+ x="440"
+ y="204.86218"
+ id="tspan2431"> /image_mono [Image]</tspan><tspan
+ sodipodi:role="line"
+ x="440"
+ y="217.36218"
+ id="tspan4192"> /image_color [Image]</tspan><tspan
+ sodipodi:role="line"
+ x="440"
+ y="229.86218"
+ id="tspan7963"> /image_rect [Image]</tspan><tspan
+ sodipodi:role="line"
+ x="440"
+ y="242.36218"
+ id="tspan2429"> /image_rect_color [Image]</tspan></text>
+ </g>
+</svg>
diff --git a/wiki_files/rospack_nosubdirs b/wiki_files/rospack_nosubdirs
new file mode 100644
index 0000000..e69de29
diff --git a/wiki_files/stereo_image_proc.svg b/wiki_files/stereo_image_proc.svg
new file mode 100644
index 0000000..4fa3997
--- /dev/null
+++ b/wiki_files/stereo_image_proc.svg
@@ -0,0 +1,477 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<!-- Created with Inkscape (http://www.inkscape.org/) -->
+<svg
+ xmlns:dc="http://purl.org/dc/elements/1.1/"
+ xmlns:cc="http://creativecommons.org/ns#"
+ xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
+ xmlns:svg="http://www.w3.org/2000/svg"
+ xmlns="http://www.w3.org/2000/svg"
+ xmlns:xlink="http://www.w3.org/1999/xlink"
+ xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
+ xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
+ width="744.09448819"
+ height="1052.3622047"
+ id="svg2"
+ sodipodi:version="0.32"
+ inkscape:version="0.46"
+ sodipodi:docname="stereo_image_proc.svg"
+ inkscape:output_extension="org.inkscape.output.svg.inkscape"
+ inkscape:export-filename="/u/mihelich/ros/pkgs/ros-pkg-trunk/stacks/image_pipeline/wiki_files/stereo_image_proc.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90">
+ <defs
+ id="defs4">
+ <marker
+ inkscape:stockid="Arrow1Mend"
+ orient="auto"
+ refY="0.0"
+ refX="0.0"
+ id="Arrow1Mend"
+ style="overflow:visible;">
+ <path
+ id="path5073"
+ d="M 0.0,0.0 L 5.0,-5.0 L -12.5,0.0 L 5.0,5.0 L 0.0,0.0 z "
+ style="fill-rule:evenodd;stroke:#000000;stroke-width:1.0pt;marker-start:none;"
+ transform="scale(0.4) rotate(180) translate(10,0)" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow2Mend"
+ orient="auto"
+ refY="0.0"
+ refX="0.0"
+ id="Arrow2Mend"
+ style="overflow:visible;">
+ <path
+ id="path5091"
+ style="font-size:12.0;fill-rule:evenodd;stroke-width:0.62500000;stroke-linejoin:round;"
+ d="M 8.7185878,4.0337352 L -2.2072895,0.016013256 L 8.7185884,-4.0017078 C 6.9730900,-1.6296469 6.9831476,1.6157441 8.7185878,4.0337352 z "
+ transform="scale(0.6) rotate(180) translate(0,0)" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow1Mstart"
+ orient="auto"
+ refY="0.0"
+ refX="0.0"
+ id="Arrow1Mstart"
+ style="overflow:visible">
+ <path
+ id="path5070"
+ d="M 0.0,0.0 L 5.0,-5.0 L -12.5,0.0 L 5.0,5.0 L 0.0,0.0 z "
+ style="fill-rule:evenodd;stroke:#000000;stroke-width:1.0pt;marker-start:none"
+ transform="scale(0.4) translate(10,0)" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow1Lstart"
+ orient="auto"
+ refY="0.0"
+ refX="0.0"
+ id="Arrow1Lstart"
+ style="overflow:visible">
+ <path
+ id="path5064"
+ d="M 0.0,0.0 L 5.0,-5.0 L -12.5,0.0 L 5.0,5.0 L 0.0,0.0 z "
+ style="fill-rule:evenodd;stroke:#000000;stroke-width:1.0pt;marker-start:none"
+ transform="scale(0.8) translate(12.5,0)" />
+ </marker>
+ <linearGradient
+ inkscape:collect="always"
+ id="linearGradient3169">
+ <stop
+ style="stop-color:#000000;stop-opacity:1;"
+ offset="0"
+ id="stop3171" />
+ <stop
+ style="stop-color:#000000;stop-opacity:0;"
+ offset="1"
+ id="stop3173" />
+ </linearGradient>
+ <inkscape:perspective
+ sodipodi:type="inkscape:persp3d"
+ inkscape:vp_x="0 : 526.18109 : 1"
+ inkscape:vp_y="0 : 1000 : 0"
+ inkscape:vp_z="744.09448 : 526.18109 : 1"
+ inkscape:persp3d-origin="372.04724 : 350.78739 : 1"
+ id="perspective10" />
+ <radialGradient
+ inkscape:collect="always"
+ xlink:href="#linearGradient3169"
+ id="radialGradient3175"
+ cx="119.73291"
+ cy="250.98525"
+ fx="119.73291"
+ fy="250.98525"
+ r="20.210449"
+ gradientTransform="matrix(1,0,0,0.7009978,0,75.045142)"
+ gradientUnits="userSpaceOnUse" />
+ <radialGradient
+ inkscape:collect="always"
+ xlink:href="#linearGradient3169"
+ id="radialGradient5043"
+ gradientUnits="userSpaceOnUse"
+ gradientTransform="matrix(1,0,0,0.7009978,0,75.045142)"
+ cx="119.73291"
+ cy="250.98525"
+ fx="119.73291"
+ fy="250.98525"
+ r="20.210449" />
+ <radialGradient
+ inkscape:collect="always"
+ xlink:href="#linearGradient3169"
+ id="radialGradient8108"
+ gradientUnits="userSpaceOnUse"
+ gradientTransform="matrix(1,0,0,0.7009978,0,75.045142)"
+ cx="119.73291"
+ cy="250.98525"
+ fx="119.73291"
+ fy="250.98525"
+ r="20.210449" />
+ <radialGradient
+ inkscape:collect="always"
+ xlink:href="#linearGradient3169"
+ id="radialGradient8143"
+ gradientUnits="userSpaceOnUse"
+ gradientTransform="matrix(1,0,0,0.7009978,0,75.045142)"
+ cx="119.73291"
+ cy="250.98525"
+ fx="119.73291"
+ fy="250.98525"
+ r="20.210449" />
+ <radialGradient
+ inkscape:collect="always"
+ xlink:href="#linearGradient3169"
+ id="radialGradient4422"
+ gradientUnits="userSpaceOnUse"
+ gradientTransform="matrix(1,0,0,0.7009978,0,75.045142)"
+ cx="119.73291"
+ cy="250.98525"
+ fx="119.73291"
+ fy="250.98525"
+ r="20.210449" />
+ </defs>
+ <sodipodi:namedview
+ id="base"
+ pagecolor="#ffffff"
+ bordercolor="#666666"
+ borderopacity="1.0"
+ gridtolerance="10000"
+ guidetolerance="10"
+ objecttolerance="10"
+ inkscape:pageopacity="0.0"
+ inkscape:pageshadow="2"
+ inkscape:zoom="1.980749"
+ inkscape:cx="303.85147"
+ inkscape:cy="792.8602"
+ inkscape:document-units="px"
+ inkscape:current-layer="layer1"
+ showgrid="true"
+ inkscape:snap-global="true"
+ showguides="false"
+ inkscape:window-width="1440"
+ inkscape:window-height="900"
+ inkscape:window-x="148"
+ inkscape:window-y="483">
+ <inkscape:grid
+ type="xygrid"
+ id="grid2383" />
+ </sodipodi:namedview>
+ <metadata
+ id="metadata7">
+ <rdf:RDF>
+ <cc:Work
+ rdf:about="">
+ <dc:format>image/svg+xml</dc:format>
+ <dc:type
+ rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
+ </cc:Work>
+ </rdf:RDF>
+ </metadata>
+ <g
+ inkscape:label="Layer 1"
+ inkscape:groupmode="layer"
+ id="layer1">
+ <rect
+ style="opacity:0.31999996;fill:#0000ff;fill-rule:evenodd;stroke:#000000;stroke-width:1.29076624;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
+ id="rect2385"
+ width="62.178108"
+ height="40.856945"
+ x="112.93611"
+ y="231.30302"
+ ry="8.374341" />
+ <flowRoot
+ xml:space="preserve"
+ id="flowRoot2389"
+ style="font-size:14px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"
+ transform="translate(20.524455,-0.6059364)"><flowRegion
+ id="flowRegion2391"
+ style="fill:url(#radialGradient3175);fill-opacity:1"><rect
+ id="rect2393"
+ width="82.296165"
+ height="64.808228"
+ x="98.755394"
+ y="233.51537"
+ style="font-size:14px;fill:#000000;fill-opacity:1" /></flowRegion><flowPara
+ id="flowPara2395"
+ style="fill:#000000;fill-opacity:1">camera </flowPara><flowPara
+ id="flowPara2397"
+ style="fill:#000000;fill-opacity:1">driver</flowPara></flowRoot> <g
+ id="g6362">
+ <rect
+ rx="0"
+ ry="8.3892555"
+ y="231.86771"
+ x="30.586723"
+ height="40.92971"
+ width="55.266777"
+ id="rect5031"
+ style="opacity:0.31999996;fill:#0000ff;fill-rule:evenodd;stroke:#000000;stroke-width:1.21800005;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
+ <flowRoot
+ transform="translate(-57.257396,8.4511111)"
+ style="font-size:14px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"
+ id="flowRoot5033"
+ xml:space="preserve"><flowRegion
+ style="fill:url(#radialGradient5043);fill-opacity:1"
+ id="flowRegion5035"><rect
+ style="font-size:14px;fill:#000000;fill-opacity:1"
+ y="233.51537"
+ x="98.755394"
+ height="64.808228"
+ width="82.296165"
+ id="rect5037" /></flowRegion><flowPara
+ style="fill:#000000;fill-opacity:1"
+ id="flowPara5041">Cam</flowPara></flowRoot> <path
+ sodipodi:nodetypes="cccc"
+ id="path5053"
+ d="M 10,232.36218 C 30,252.36218 30,252.36218 30,252.36218 L 10,272.36218 L 10,232.36218 z"
+ style="opacity:0.32758622;fill:#0000ff;fill-opacity:1;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" />
+ </g>
+ <path
+ style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;marker-start:url(#Arrow1Mstart);stroke-opacity:1;display:inline"
+ d="M 112.29073,251.93773 L 86.4625,252.12633"
+ id="path5059"
+ inkscape:connector-type="polyline"
+ inkscape:connection-end="#rect5031" />
+ <rect
+ style="opacity:0.31999996;fill:#0000ff;fill-rule:evenodd;stroke:#000000;stroke-width:1.29076624;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
+ id="rect8070"
+ width="62.178108"
+ height="40.856945"
+ x="112.93611"
+ y="326.30304"
+ ry="8.374341" />
+ <g
+ id="g8082"
+ transform="translate(0,95.000003)">
+ <rect
+ rx="0"
+ ry="8.3892555"
+ y="231.86771"
+ x="30.586723"
+ height="40.92971"
+ width="55.266777"
+ id="rect8084"
+ style="opacity:0.31999996;fill:#0000ff;fill-rule:evenodd;stroke:#000000;stroke-width:1.21800005;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
+ <flowRoot
+ transform="translate(-57.257396,8.4511111)"
+ style="font-size:14px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"
+ id="flowRoot8086"
+ xml:space="preserve"><flowRegion
+ style="fill:url(#radialGradient8108);fill-opacity:1"
+ id="flowRegion8088"><rect
+ style="font-size:14px;fill:#000000;fill-opacity:1"
+ y="233.51537"
+ x="98.755394"
+ height="64.808228"
+ width="82.296165"
+ id="rect8090" /></flowRegion><flowPara
+ style="fill:#000000;fill-opacity:1"
+ id="flowPara8092">Cam</flowPara></flowRoot> <path
+ sodipodi:nodetypes="cccc"
+ id="path8094"
+ d="M 10,232.36218 C 30,252.36218 30,252.36218 30,252.36218 L 10,272.36218 L 10,232.36218 z"
+ style="opacity:0.32758622;fill:#0000ff;fill-opacity:1;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" />
+ </g>
+ <path
+ style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;marker-start:url(#Arrow1Mstart);stroke-opacity:1;display:inline"
+ d="M 112.29073,251.93773 L 86.4625,252.12633"
+ id="path8096"
+ inkscape:connector-type="polyline"
+ inkscape:connection-end="#rect5031" />
+ <g
+ id="g8129"
+ transform="matrix(1.99653,0,0,1.8385633,120.44009,-148.36533)">
+ <g
+ id="g8171"
+ transform="translate(-12.694916,-7.9273828)">
+ <rect
+ style="opacity:0.31999996;fill:#0000ff;fill-rule:evenodd;stroke:#000000;stroke-width:1.21800005;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
+ id="rect8131"
+ width="55.266777"
+ height="40.92971"
+ x="90.109184"
+ y="231.32329"
+ ry="8.3892555" />
+ <flowRoot
+ transform="translate(4.1820079,3.2300212)"
+ xml:space="preserve"
+ id="flowRoot8133"
+ style="font-size:7.30719709px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"><flowRegion
+ id="flowRegion8135"
+ style="fill:url(#radialGradient8143);fill-opacity:1"><rect
+ id="rect8137"
+ width="82.296165"
+ height="64.808228"
+ x="98.755394"
+ y="233.51537"
+ style="font-size:7.30719709px;fill:#000000;fill-opacity:1" /></flowRegion><flowPara
+ id="flowPara8141"
+ style="font-size:7.30719709px;fill:#000000;fill-opacity:1">stereo_</flowPara><flowPara
+ id="flowPara8167"
+ style="font-size:7.30719709px;fill:#000000;fill-opacity:1">image_</flowPara><flowPara
+ id="flowPara8169"
+ style="font-size:7.30719709px;fill:#000000;fill-opacity:1">proc</flowPara></flowRoot> </g>
+ </g>
+ <path
+ style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1.06473207px;stroke-linecap:butt;stroke-linejoin:miter;marker-end:url(#Arrow1Mend);stroke-opacity:1"
+ d="M 175.02377,250.6733 L 274.92078,282.51599"
+ id="path8218"
+ inkscape:connector-type="polyline" />
+ <path
+ style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1.01836419px;stroke-linecap:butt;stroke-linejoin:miter;marker-end:url(#Arrow1Mend);stroke-opacity:1"
+ d="M 175.11119,342.47878 L 273.81819,313.92238"
+ id="path8220"
+ inkscape:connector-type="polyline" />
+ <path
+ style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;marker-start:url(#Arrow1Mstart);stroke-opacity:1;display:inline"
+ d="M 110.82823,347.17358 L 85,347.36218"
+ id="path8222"
+ inkscape:connector-type="polyline" />
+ <path
+ style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;marker-end:url(#Arrow1Mend)"
+ d="M 386.55766,300.04369 L 465.48049,300.12148"
+ id="path9290"
+ inkscape:connector-type="polyline"
+ inkscape:connection-start="#g8129" />
+ <text
+ xml:space="preserve"
+ style="font-size:10px;font-style:normal;font-weight:bold;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans;-inkscape-font-specification:Bitstream Vera Sans Bold"
+ x="175"
+ y="212.36218"
+ id="text4394"><tspan
+ sodipodi:role="line"
+ id="tspan4396"
+ x="175"
+ y="212.36218">left/</tspan><tspan
+ sodipodi:role="line"
+ x="175"
+ y="224.86218"
+ id="tspan2675"> camera_info [CameraInfo]</tspan><tspan
+ sodipodi:role="line"
+ x="175"
+ y="237.36218"
+ id="tspan4399"> image_raw [Image]</tspan></text>
+ <text
+ xml:space="preserve"
+ style="font-size:10px;font-style:normal;font-weight:bold;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans;-inkscape-font-specification:Bitstream Vera Sans Bold"
+ x="180"
+ y="357.36218"
+ id="text7959"><tspan
+ sodipodi:role="line"
+ id="tspan7961"
+ x="180"
+ y="357.36218">right/</tspan><tspan
+ sodipodi:role="line"
+ x="180"
+ y="369.86218"
+ id="tspan4403"> camera_info [CameraInfo]</tspan><tspan
+ sodipodi:role="line"
+ x="180"
+ y="382.36218"
+ id="tspan7963"> image_raw [Image]</tspan></text>
+ <flowRoot
+ xml:space="preserve"
+ id="flowRoot4412"
+ style="font-size:14px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"
+ transform="translate(18.339844,92.748515)"><flowRegion
+ id="flowRegion4414"
+ style="fill:url(#radialGradient4422);fill-opacity:1"><rect
+ id="rect4416"
+ width="82.296165"
+ height="64.808228"
+ x="98.755394"
+ y="233.51537"
+ style="font-size:14px;fill:#000000;fill-opacity:1" /></flowRegion><flowPara
+ id="flowPara4418"
+ style="fill:#000000;fill-opacity:1">camera </flowPara><flowPara
+ id="flowPara4420"
+ style="fill:#000000;fill-opacity:1">driver</flowPara></flowRoot> <text
+ xml:space="preserve"
+ style="font-size:10px;font-style:normal;font-weight:bold;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans;-inkscape-font-specification:Bitstream Vera Sans Bold"
+ x="395"
+ y="167.36218"
+ id="text4345"><tspan
+ sodipodi:role="line"
+ id="tspan4347"
+ x="395"
+ y="167.36218" /><tspan
+ sodipodi:role="line"
+ x="395"
+ y="179.86218"
+ id="tspan2456">left/</tspan><tspan
+ sodipodi:role="line"
+ x="395"
+ y="192.36218"
+ id="tspan2472"> image_mono [Image]</tspan><tspan
+ sodipodi:role="line"
+ x="395"
+ y="204.86218"
+ id="tspan2474"> image_color [Image]</tspan><tspan
+ sodipodi:role="line"
+ x="395"
+ y="217.36218"
+ id="tspan2476"> image_rect [Image]</tspan><tspan
+ sodipodi:role="line"
+ x="395"
+ y="229.86218"
+ id="tspan2480"> image_rect_color [Image]</tspan><tspan
+ sodipodi:role="line"
+ x="395"
+ y="242.36218"
+ id="tspan2490">right/</tspan><tspan
+ sodipodi:role="line"
+ x="395"
+ y="254.86218"
+ id="tspan2629"> image_mono [Image]</tspan><tspan
+ sodipodi:role="line"
+ x="395"
+ y="267.36218"
+ id="tspan4358"> image_color [Image]</tspan><tspan
+ sodipodi:role="line"
+ x="395"
+ y="279.86218"
+ id="tspan4360"> image_rect [Image]</tspan><tspan
+ sodipodi:role="line"
+ x="395"
+ y="292.36218"
+ id="tspan2631"> image_rect_color [Image]</tspan><tspan
+ sodipodi:role="line"
+ x="395"
+ y="304.86218"
+ id="tspan2496" /><tspan
+ sodipodi:role="line"
+ x="395"
+ y="317.36218"
+ id="tspan2498">disparity [DisparityImage]</tspan><tspan
+ sodipodi:role="line"
+ x="395"
+ y="329.86218"
+ id="tspan2486">image_disparity [Image]</tspan><tspan
+ sodipodi:role="line"
+ x="395"
+ y="342.36218"
+ id="tspan2494">points [PointCloud]</tspan><tspan
+ sodipodi:role="line"
+ x="395"
+ y="367.36218"
+ id="tspan2488" /></text>
+ </g>
+</svg>
diff --git a/wiki_files/stereo_image_proc_stereo.svg b/wiki_files/stereo_image_proc_stereo.svg
new file mode 100644
index 0000000..01fb69a
--- /dev/null
+++ b/wiki_files/stereo_image_proc_stereo.svg
@@ -0,0 +1,389 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<!-- Created with Inkscape (http://www.inkscape.org/) -->
+<svg
+ xmlns:dc="http://purl.org/dc/elements/1.1/"
+ xmlns:cc="http://creativecommons.org/ns#"
+ xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
+ xmlns:svg="http://www.w3.org/2000/svg"
+ xmlns="http://www.w3.org/2000/svg"
+ xmlns:xlink="http://www.w3.org/1999/xlink"
+ xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
+ xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
+ width="744.09448819"
+ height="1052.3622047"
+ id="svg2"
+ sodipodi:version="0.32"
+ inkscape:version="0.46"
+ sodipodi:docname="stereo_image_proc_stereo.svg"
+ inkscape:output_extension="org.inkscape.output.svg.inkscape"
+ inkscape:export-filename="/u/konolige/writings/docs/stereo_image_proc_stereo.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90">
+ <defs
+ id="defs4">
+ <radialGradient
+ inkscape:collect="always"
+ xlink:href="#linearGradient3169"
+ id="radialGradient8143"
+ gradientUnits="userSpaceOnUse"
+ gradientTransform="matrix(1,0,0,0.7009978,0,75.045142)"
+ cx="119.73291"
+ cy="250.98525"
+ fx="119.73291"
+ fy="250.98525"
+ r="20.210449" />
+ <radialGradient
+ inkscape:collect="always"
+ xlink:href="#linearGradient3169"
+ id="radialGradient8108"
+ gradientUnits="userSpaceOnUse"
+ gradientTransform="matrix(1,0,0,0.7009978,0,75.045142)"
+ cx="119.73291"
+ cy="250.98525"
+ fx="119.73291"
+ fy="250.98525"
+ r="20.210449" />
+ <radialGradient
+ inkscape:collect="always"
+ xlink:href="#linearGradient3169"
+ id="radialGradient5043"
+ gradientUnits="userSpaceOnUse"
+ gradientTransform="matrix(1,0,0,0.7009978,0,75.045142)"
+ cx="119.73291"
+ cy="250.98525"
+ fx="119.73291"
+ fy="250.98525"
+ r="20.210449" />
+ <marker
+ inkscape:stockid="Arrow1Mend"
+ orient="auto"
+ refY="0.0"
+ refX="0.0"
+ id="Arrow1Mend"
+ style="overflow:visible;">
+ <path
+ id="path5073"
+ d="M 0.0,0.0 L 5.0,-5.0 L -12.5,0.0 L 5.0,5.0 L 0.0,0.0 z "
+ style="fill-rule:evenodd;stroke:#000000;stroke-width:1.0pt;marker-start:none;"
+ transform="scale(0.4) rotate(180) translate(10,0)" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow2Mend"
+ orient="auto"
+ refY="0.0"
+ refX="0.0"
+ id="Arrow2Mend"
+ style="overflow:visible;">
+ <path
+ id="path5091"
+ style="font-size:12.0;fill-rule:evenodd;stroke-width:0.62500000;stroke-linejoin:round;"
+ d="M 8.7185878,4.0337352 L -2.2072895,0.016013256 L 8.7185884,-4.0017078 C 6.9730900,-1.6296469 6.9831476,1.6157441 8.7185878,4.0337352 z "
+ transform="scale(0.6) rotate(180) translate(0,0)" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow1Mstart"
+ orient="auto"
+ refY="0.0"
+ refX="0.0"
+ id="Arrow1Mstart"
+ style="overflow:visible">
+ <path
+ id="path5070"
+ d="M 0.0,0.0 L 5.0,-5.0 L -12.5,0.0 L 5.0,5.0 L 0.0,0.0 z "
+ style="fill-rule:evenodd;stroke:#000000;stroke-width:1.0pt;marker-start:none"
+ transform="scale(0.4) translate(10,0)" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow1Lstart"
+ orient="auto"
+ refY="0.0"
+ refX="0.0"
+ id="Arrow1Lstart"
+ style="overflow:visible">
+ <path
+ id="path5064"
+ d="M 0.0,0.0 L 5.0,-5.0 L -12.5,0.0 L 5.0,5.0 L 0.0,0.0 z "
+ style="fill-rule:evenodd;stroke:#000000;stroke-width:1.0pt;marker-start:none"
+ transform="scale(0.8) translate(12.5,0)" />
+ </marker>
+ <linearGradient
+ inkscape:collect="always"
+ id="linearGradient3169">
+ <stop
+ style="stop-color:#000000;stop-opacity:1;"
+ offset="0"
+ id="stop3171" />
+ <stop
+ style="stop-color:#000000;stop-opacity:0;"
+ offset="1"
+ id="stop3173" />
+ </linearGradient>
+ <inkscape:perspective
+ sodipodi:type="inkscape:persp3d"
+ inkscape:vp_x="0 : 526.18109 : 1"
+ inkscape:vp_y="0 : 1000 : 0"
+ inkscape:vp_z="744.09448 : 526.18109 : 1"
+ inkscape:persp3d-origin="372.04724 : 350.78739 : 1"
+ id="perspective10" />
+ <radialGradient
+ inkscape:collect="always"
+ xlink:href="#linearGradient3169"
+ id="radialGradient3175"
+ cx="119.73291"
+ cy="250.98525"
+ fx="119.73291"
+ fy="250.98525"
+ r="20.210449"
+ gradientTransform="matrix(1,0,0,0.7009978,0,75.045142)"
+ gradientUnits="userSpaceOnUse" />
+ </defs>
+ <sodipodi:namedview
+ id="base"
+ pagecolor="#ffffff"
+ bordercolor="#666666"
+ borderopacity="1.0"
+ gridtolerance="10000"
+ guidetolerance="10"
+ objecttolerance="10"
+ inkscape:pageopacity="0.0"
+ inkscape:pageshadow="2"
+ inkscape:zoom="1.400601"
+ inkscape:cx="218.84605"
+ inkscape:cy="792.8602"
+ inkscape:document-units="px"
+ inkscape:current-layer="layer1"
+ showgrid="true"
+ inkscape:snap-global="true"
+ showguides="false"
+ inkscape:window-width="1236"
+ inkscape:window-height="695"
+ inkscape:window-x="133"
+ inkscape:window-y="47">
+ <inkscape:grid
+ type="xygrid"
+ id="grid2383" />
+ </sodipodi:namedview>
+ <metadata
+ id="metadata7">
+ <rdf:RDF>
+ <cc:Work
+ rdf:about="">
+ <dc:format>image/svg+xml</dc:format>
+ <dc:type
+ rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
+ </cc:Work>
+ </rdf:RDF>
+ </metadata>
+ <g
+ inkscape:label="Layer 1"
+ inkscape:groupmode="layer"
+ id="layer1">
+ <g
+ id="g4195">
+ <rect
+ ry="8.3639584"
+ y="231.32835"
+ x="112.96144"
+ height="40.806286"
+ width="67.23764"
+ id="rect2385"
+ style="opacity:0.31999996;fill:#0000ff;fill-rule:evenodd;stroke:#000000;stroke-width:1.3414228;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
+ <flowRoot
+ transform="translate(22.790548,-5.6640625e-2)"
+ style="font-size:14px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"
+ id="flowRoot2389"
+ xml:space="preserve"><flowRegion
+ style="fill:url(#radialGradient3175);fill-opacity:1"
+ id="flowRegion2391"><rect
+ style="font-size:14px;fill:#000000;fill-opacity:1"
+ y="233.51537"
+ x="98.755394"
+ height="64.808228"
+ width="82.296165"
+ id="rect2393" /></flowRegion><flowPara
+ style="fill:#000000;fill-opacity:1"
+ id="flowPara2395">camera </flowPara><flowPara
+ style="fill:#000000;fill-opacity:1"
+ id="flowPara2397">driver</flowPara></flowRoot> </g>
+ <path
+ style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;marker-start:url(#Arrow1Mstart);stroke-opacity:1;display:inline"
+ d="M 112.29073,251.93773 L 86.4625,252.12633"
+ id="path5059"
+ inkscape:connector-type="polyline" />
+ <path
+ style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;marker-start:url(#Arrow1Mstart);stroke-opacity:1;display:inline"
+ d="M 112.29073,251.93773 L 86.4625,252.12633"
+ id="path8096"
+ inkscape:connector-type="polyline" />
+ <text
+ xml:space="preserve"
+ style="font-size:10px;font-style:normal;font-weight:bold;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans;-inkscape-font-specification:Bitstream Vera Sans Bold"
+ x="190"
+ y="182.36218"
+ id="text4177"><tspan
+ sodipodi:role="line"
+ id="tspan4179"
+ x="190"
+ y="182.36218">/<camera></tspan><tspan
+ sodipodi:role="line"
+ x="190"
+ y="194.86218"
+ id="tspan2675"> /left/camera_info [CameraInfo]</tspan><tspan
+ sodipodi:role="line"
+ x="190"
+ y="207.36218"
+ id="tspan4182"> /left/image_raw [Image]</tspan><tspan
+ sodipodi:role="line"
+ x="190"
+ y="219.86218"
+ id="tspan2543"> /right/camera_info [CameraInfo]</tspan><tspan
+ sodipodi:role="line"
+ x="190"
+ y="232.36218"
+ id="tspan2545"> /right/image_raw [Image]</tspan></text>
+ <g
+ id="g2503"
+ transform="translate(-150.24718,-49.317444)">
+ <rect
+ rx="0"
+ ry="16.730608"
+ y="261.67963"
+ x="180.24718"
+ height="81.62571"
+ width="54.772434"
+ id="rect5031"
+ style="opacity:0.31999996;fill:#0000ff;fill-rule:evenodd;stroke:#000000;stroke-width:1.71234167;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
+ <flowRoot
+ transform="translate(92.155884,38.015877)"
+ style="font-size:14px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"
+ id="flowRoot5033"
+ xml:space="preserve"><flowRegion
+ style="fill:url(#radialGradient5043);fill-opacity:1"
+ id="flowRegion5035"><rect
+ style="font-size:14px;fill:#000000;fill-opacity:1"
+ y="233.51537"
+ x="98.755394"
+ height="64.808228"
+ width="82.296165"
+ id="rect5037" /></flowRegion><flowPara
+ style="fill:#000000;fill-opacity:1"
+ id="flowPara5041">Cam</flowPara></flowRoot> <path
+ sodipodi:nodetypes="cccc"
+ id="path5053"
+ d="M 160,257.36218 C 180,277.36218 180,277.36218 180,277.36218 L 160,297.36218 L 160,257.36218 z"
+ style="opacity:0.32758622;fill:#0000ff;fill-opacity:1;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" />
+ <flowRoot
+ transform="translate(90.463867,79.709452)"
+ style="font-size:14px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"
+ id="flowRoot8086"
+ xml:space="preserve"><flowRegion
+ style="fill:url(#radialGradient8108);fill-opacity:1"
+ id="flowRegion8088"><rect
+ style="font-size:14px;fill:#000000;fill-opacity:1"
+ y="233.51537"
+ x="98.755394"
+ height="64.808228"
+ width="82.296165"
+ id="rect8090" /></flowRegion><flowPara
+ style="fill:#000000;fill-opacity:1"
+ id="flowPara8092">Cam</flowPara></flowRoot> <path
+ sodipodi:nodetypes="cccc"
+ id="path8094"
+ d="M 160,307.36218 C 180,327.36218 180,327.36218 180,327.36218 L 160,347.36218 L 160,307.36218 z"
+ style="opacity:0.32758622;fill:#0000ff;fill-opacity:1;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" />
+ </g>
+ <g
+ id="g8129"
+ transform="matrix(1.99653,0,0,1.8385633,233.88243,-196.04684)">
+ <g
+ id="g8171"
+ transform="translate(-12.694916,-7.9273828)">
+ <rect
+ style="opacity:0.31999996;fill:#0000ff;fill-rule:evenodd;stroke:#000000;stroke-width:1.21800005;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
+ id="rect8131"
+ width="55.266777"
+ height="40.92971"
+ x="90.109184"
+ y="231.32329"
+ ry="8.3892555" />
+ <flowRoot
+ transform="translate(4.1820079,3.2300212)"
+ xml:space="preserve"
+ id="flowRoot8133"
+ style="font-size:7.30719709px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"><flowRegion
+ id="flowRegion8135"
+ style="fill:url(#radialGradient8143);fill-opacity:1"><rect
+ id="rect8137"
+ width="82.296165"
+ height="64.808228"
+ x="98.755394"
+ y="233.51537"
+ style="font-size:7.30719709px;fill:#000000;fill-opacity:1" /></flowRegion><flowPara
+ id="flowPara8141"
+ style="font-size:7.30719709px;fill:#000000;fill-opacity:1">stereo_</flowPara><flowPara
+ id="flowPara8167"
+ style="font-size:7.30719709px;fill:#000000;fill-opacity:1">image_</flowPara><flowPara
+ id="flowPara8169"
+ style="font-size:7.30719709px;fill:#000000;fill-opacity:1">proc</flowPara></flowRoot> </g>
+ </g>
+ <path
+ style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;marker-end:url(#Arrow1Mend);stroke-opacity:1"
+ d="M 500,252.36218 L 578.92283,252.43997"
+ id="path9290"
+ inkscape:connector-type="polyline"
+ inkscape:connection-start="#g8129" />
+ <path
+ style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;marker-end:url(#Arrow1Mend)"
+ d="M 180.86979,251.79789 L 387.22645,252.19743"
+ id="path2871"
+ inkscape:connector-type="polyline"
+ inkscape:connection-start="#g4195"
+ inkscape:connection-end="#g8129" />
+ <text
+ xml:space="preserve"
+ style="font-size:10px;font-style:normal;font-weight:bold;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans;-inkscape-font-specification:Bitstream Vera Sans Bold"
+ x="510"
+ y="122.36218"
+ id="text4430"><tspan
+ sodipodi:role="line"
+ id="tspan4432"
+ x="510"
+ y="122.36218">/<camera></tspan><tspan
+ sodipodi:role="line"
+ x="510"
+ y="134.86218"
+ id="tspan4442"> /image_disparity [DisparityImage]</tspan><tspan
+ sodipodi:role="line"
+ x="510"
+ y="147.36218"
+ id="tspan4434"> /left/image_mono [Image]</tspan><tspan
+ sodipodi:role="line"
+ x="510"
+ y="159.86218"
+ id="tspan2625"> /left/image_color [Image]</tspan><tspan
+ sodipodi:role="line"
+ x="510"
+ y="172.36218"
+ id="tspan4436"> /left/image_rect [Image]</tspan><tspan
+ sodipodi:role="line"
+ x="510"
+ y="184.86218"
+ id="tspan2627"> /left/image_rect_color [Image]</tspan><tspan
+ sodipodi:role="line"
+ x="510"
+ y="197.36218"
+ id="tspan2629"> /right/image_mono [Image]</tspan><tspan
+ sodipodi:role="line"
+ x="510"
+ y="209.86218"
+ id="tspan4438"> /right/image_color [Image]</tspan><tspan
+ sodipodi:role="line"
+ x="510"
+ y="222.36218"
+ id="tspan4440"> /right/image_rect [Image]</tspan><tspan
+ sodipodi:role="line"
+ x="510"
+ y="234.86218"
+ id="tspan2631"> /right/image_rect_color [Image]</tspan></text>
+ </g>
+</svg>
diff --git a/wiki_files/stereocam-driver.svg b/wiki_files/stereocam-driver.svg
new file mode 100644
index 0000000..32a5057
--- /dev/null
+++ b/wiki_files/stereocam-driver.svg
@@ -0,0 +1,290 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<!-- Created with Inkscape (http://www.inkscape.org/) -->
+<svg
+ xmlns:dc="http://purl.org/dc/elements/1.1/"
+ xmlns:cc="http://creativecommons.org/ns#"
+ xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
+ xmlns:svg="http://www.w3.org/2000/svg"
+ xmlns="http://www.w3.org/2000/svg"
+ xmlns:xlink="http://www.w3.org/1999/xlink"
+ xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
+ xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
+ width="744.09448819"
+ height="1052.3622047"
+ id="svg2"
+ sodipodi:version="0.32"
+ inkscape:version="0.46"
+ sodipodi:docname="stereocam-driver.svg"
+ inkscape:output_extension="org.inkscape.output.svg.inkscape"
+ inkscape:export-filename="/u/konolige/writings/docs/stereocam-driver.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90">
+ <defs
+ id="defs4">
+ <radialGradient
+ inkscape:collect="always"
+ xlink:href="#linearGradient3169"
+ id="radialGradient8108"
+ gradientUnits="userSpaceOnUse"
+ gradientTransform="matrix(1,0,0,0.7009978,0,75.045142)"
+ cx="119.73291"
+ cy="250.98525"
+ fx="119.73291"
+ fy="250.98525"
+ r="20.210449" />
+ <radialGradient
+ inkscape:collect="always"
+ xlink:href="#linearGradient3169"
+ id="radialGradient5043"
+ gradientUnits="userSpaceOnUse"
+ gradientTransform="matrix(1,0,0,0.7009978,0,75.045142)"
+ cx="119.73291"
+ cy="250.98525"
+ fx="119.73291"
+ fy="250.98525"
+ r="20.210449" />
+ <marker
+ inkscape:stockid="Arrow1Mend"
+ orient="auto"
+ refY="0.0"
+ refX="0.0"
+ id="Arrow1Mend"
+ style="overflow:visible;">
+ <path
+ id="path5073"
+ d="M 0.0,0.0 L 5.0,-5.0 L -12.5,0.0 L 5.0,5.0 L 0.0,0.0 z "
+ style="fill-rule:evenodd;stroke:#000000;stroke-width:1.0pt;marker-start:none;"
+ transform="scale(0.4) rotate(180) translate(10,0)" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow2Mend"
+ orient="auto"
+ refY="0.0"
+ refX="0.0"
+ id="Arrow2Mend"
+ style="overflow:visible;">
+ <path
+ id="path5091"
+ style="font-size:12.0;fill-rule:evenodd;stroke-width:0.62500000;stroke-linejoin:round;"
+ d="M 8.7185878,4.0337352 L -2.2072895,0.016013256 L 8.7185884,-4.0017078 C 6.9730900,-1.6296469 6.9831476,1.6157441 8.7185878,4.0337352 z "
+ transform="scale(0.6) rotate(180) translate(0,0)" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow1Mstart"
+ orient="auto"
+ refY="0.0"
+ refX="0.0"
+ id="Arrow1Mstart"
+ style="overflow:visible">
+ <path
+ id="path5070"
+ d="M 0.0,0.0 L 5.0,-5.0 L -12.5,0.0 L 5.0,5.0 L 0.0,0.0 z "
+ style="fill-rule:evenodd;stroke:#000000;stroke-width:1.0pt;marker-start:none"
+ transform="scale(0.4) translate(10,0)" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow1Lstart"
+ orient="auto"
+ refY="0.0"
+ refX="0.0"
+ id="Arrow1Lstart"
+ style="overflow:visible">
+ <path
+ id="path5064"
+ d="M 0.0,0.0 L 5.0,-5.0 L -12.5,0.0 L 5.0,5.0 L 0.0,0.0 z "
+ style="fill-rule:evenodd;stroke:#000000;stroke-width:1.0pt;marker-start:none"
+ transform="scale(0.8) translate(12.5,0)" />
+ </marker>
+ <linearGradient
+ inkscape:collect="always"
+ id="linearGradient3169">
+ <stop
+ style="stop-color:#000000;stop-opacity:1;"
+ offset="0"
+ id="stop3171" />
+ <stop
+ style="stop-color:#000000;stop-opacity:0;"
+ offset="1"
+ id="stop3173" />
+ </linearGradient>
+ <inkscape:perspective
+ sodipodi:type="inkscape:persp3d"
+ inkscape:vp_x="0 : 526.18109 : 1"
+ inkscape:vp_y="0 : 1000 : 0"
+ inkscape:vp_z="744.09448 : 526.18109 : 1"
+ inkscape:persp3d-origin="372.04724 : 350.78739 : 1"
+ id="perspective10" />
+ <radialGradient
+ inkscape:collect="always"
+ xlink:href="#linearGradient3169"
+ id="radialGradient3175"
+ cx="119.73291"
+ cy="250.98525"
+ fx="119.73291"
+ fy="250.98525"
+ r="20.210449"
+ gradientTransform="matrix(1,0,0,0.7009978,0,75.045142)"
+ gradientUnits="userSpaceOnUse" />
+ </defs>
+ <sodipodi:namedview
+ id="base"
+ pagecolor="#ffffff"
+ bordercolor="#666666"
+ borderopacity="1.0"
+ gridtolerance="10000"
+ guidetolerance="10"
+ objecttolerance="10"
+ inkscape:pageopacity="0.0"
+ inkscape:pageshadow="2"
+ inkscape:zoom="1.400601"
+ inkscape:cx="218.84605"
+ inkscape:cy="792.8602"
+ inkscape:document-units="px"
+ inkscape:current-layer="layer1"
+ showgrid="true"
+ inkscape:snap-global="true"
+ showguides="false"
+ inkscape:window-width="1236"
+ inkscape:window-height="695"
+ inkscape:window-x="133"
+ inkscape:window-y="47">
+ <inkscape:grid
+ type="xygrid"
+ id="grid2383" />
+ </sodipodi:namedview>
+ <metadata
+ id="metadata7">
+ <rdf:RDF>
+ <cc:Work
+ rdf:about="">
+ <dc:format>image/svg+xml</dc:format>
+ <dc:type
+ rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
+ </cc:Work>
+ </rdf:RDF>
+ </metadata>
+ <g
+ inkscape:label="Layer 1"
+ inkscape:groupmode="layer"
+ id="layer1">
+ <g
+ id="g4195">
+ <rect
+ ry="8.3639584"
+ y="231.32835"
+ x="112.96144"
+ height="40.806286"
+ width="67.23764"
+ id="rect2385"
+ style="opacity:0.31999996;fill:#0000ff;fill-rule:evenodd;stroke:#000000;stroke-width:1.3414228;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
+ <flowRoot
+ transform="translate(22.790548,-5.6640625e-2)"
+ style="font-size:14px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"
+ id="flowRoot2389"
+ xml:space="preserve"><flowRegion
+ style="fill:url(#radialGradient3175);fill-opacity:1"
+ id="flowRegion2391"><rect
+ style="font-size:14px;fill:#000000;fill-opacity:1"
+ y="233.51537"
+ x="98.755394"
+ height="64.808228"
+ width="82.296165"
+ id="rect2393" /></flowRegion><flowPara
+ style="fill:#000000;fill-opacity:1"
+ id="flowPara2395">camera </flowPara><flowPara
+ style="fill:#000000;fill-opacity:1"
+ id="flowPara2397">driver</flowPara></flowRoot> </g>
+ <path
+ style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;marker-start:url(#Arrow1Mstart);stroke-opacity:1;display:inline"
+ d="M 112.29073,251.93773 L 86.4625,252.12633"
+ id="path5059"
+ inkscape:connector-type="polyline" />
+ <path
+ style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;marker-start:url(#Arrow1Mstart);stroke-opacity:1;display:inline"
+ d="M 112.29073,251.93773 L 86.4625,252.12633"
+ id="path8096"
+ inkscape:connector-type="polyline" />
+ <path
+ style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:0.96976066px;stroke-linecap:butt;stroke-linejoin:miter;marker-end:url(#Arrow1Mend);stroke-opacity:1"
+ d="M 180.10283,251.63476 L 335.38226,251.02959"
+ id="path9869"
+ inkscape:connector-type="polyline" />
+ <text
+ xml:space="preserve"
+ style="font-size:10px;font-style:normal;font-weight:bold;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans;-inkscape-font-specification:Bitstream Vera Sans Bold"
+ x="190"
+ y="182.36218"
+ id="text4177"><tspan
+ sodipodi:role="line"
+ id="tspan4179"
+ x="190"
+ y="182.36218">/<camera></tspan><tspan
+ sodipodi:role="line"
+ x="190"
+ y="194.86218"
+ id="tspan2675"> /left/camera_info [CameraInfo]</tspan><tspan
+ sodipodi:role="line"
+ x="190"
+ y="207.36218"
+ id="tspan4182"> /left/image_raw [Image]</tspan><tspan
+ sodipodi:role="line"
+ x="190"
+ y="219.86218"
+ id="tspan2543"> /right/camera_info [CameraInfo]</tspan><tspan
+ sodipodi:role="line"
+ x="190"
+ y="232.36218"
+ id="tspan2545"> /right/image_raw [Image]</tspan></text>
+ <g
+ id="g2503"
+ transform="translate(-150.24718,-49.317444)">
+ <rect
+ rx="0"
+ ry="16.730608"
+ y="261.67963"
+ x="180.24718"
+ height="81.62571"
+ width="54.772434"
+ id="rect5031"
+ style="opacity:0.31999996;fill:#0000ff;fill-rule:evenodd;stroke:#000000;stroke-width:1.71234167;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
+ <flowRoot
+ transform="translate(92.155884,38.015877)"
+ style="font-size:14px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"
+ id="flowRoot5033"
+ xml:space="preserve"><flowRegion
+ style="fill:url(#radialGradient5043);fill-opacity:1"
+ id="flowRegion5035"><rect
+ style="font-size:14px;fill:#000000;fill-opacity:1"
+ y="233.51537"
+ x="98.755394"
+ height="64.808228"
+ width="82.296165"
+ id="rect5037" /></flowRegion><flowPara
+ style="fill:#000000;fill-opacity:1"
+ id="flowPara5041">Cam</flowPara></flowRoot> <path
+ sodipodi:nodetypes="cccc"
+ id="path5053"
+ d="M 160,257.36218 C 180,277.36218 180,277.36218 180,277.36218 L 160,297.36218 L 160,257.36218 z"
+ style="opacity:0.32758622;fill:#0000ff;fill-opacity:1;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" />
+ <flowRoot
+ transform="translate(90.463867,79.709452)"
+ style="font-size:14px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"
+ id="flowRoot8086"
+ xml:space="preserve"><flowRegion
+ style="fill:url(#radialGradient8108);fill-opacity:1"
+ id="flowRegion8088"><rect
+ style="font-size:14px;fill:#000000;fill-opacity:1"
+ y="233.51537"
+ x="98.755394"
+ height="64.808228"
+ width="82.296165"
+ id="rect8090" /></flowRegion><flowPara
+ style="fill:#000000;fill-opacity:1"
+ id="flowPara8092">Cam</flowPara></flowRoot> <path
+ sodipodi:nodetypes="cccc"
+ id="path8094"
+ d="M 160,307.36218 C 180,327.36218 180,327.36218 180,327.36218 L 160,347.36218 L 160,307.36218 z"
+ style="opacity:0.32758622;fill:#0000ff;fill-opacity:1;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" />
+ </g>
+ </g>
+</svg>
diff --git a/wiki_files/stoc.svg b/wiki_files/stoc.svg
new file mode 100644
index 0000000..361c6fe
--- /dev/null
+++ b/wiki_files/stoc.svg
@@ -0,0 +1,312 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<!-- Created with Inkscape (http://www.inkscape.org/) -->
+<svg
+ xmlns:dc="http://purl.org/dc/elements/1.1/"
+ xmlns:cc="http://creativecommons.org/ns#"
+ xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
+ xmlns:svg="http://www.w3.org/2000/svg"
+ xmlns="http://www.w3.org/2000/svg"
+ xmlns:xlink="http://www.w3.org/1999/xlink"
+ xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
+ xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
+ width="744.09448819"
+ height="1052.3622047"
+ id="svg2"
+ sodipodi:version="0.32"
+ inkscape:version="0.46"
+ sodipodi:docname="stoc.svg"
+ inkscape:output_extension="org.inkscape.output.svg.inkscape"
+ inkscape:export-filename="/u/konolige/writings/docs/stoc.png"
+ inkscape:export-xdpi="90"
+ inkscape:export-ydpi="90">
+ <defs
+ id="defs4">
+ <marker
+ inkscape:stockid="Arrow1Mend"
+ orient="auto"
+ refY="0.0"
+ refX="0.0"
+ id="Arrow1Mend"
+ style="overflow:visible;">
+ <path
+ id="path5073"
+ d="M 0.0,0.0 L 5.0,-5.0 L -12.5,0.0 L 5.0,5.0 L 0.0,0.0 z "
+ style="fill-rule:evenodd;stroke:#000000;stroke-width:1.0pt;marker-start:none;"
+ transform="scale(0.4) rotate(180) translate(10,0)" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow2Mend"
+ orient="auto"
+ refY="0.0"
+ refX="0.0"
+ id="Arrow2Mend"
+ style="overflow:visible;">
+ <path
+ id="path5091"
+ style="font-size:12.0;fill-rule:evenodd;stroke-width:0.62500000;stroke-linejoin:round;"
+ d="M 8.7185878,4.0337352 L -2.2072895,0.016013256 L 8.7185884,-4.0017078 C 6.9730900,-1.6296469 6.9831476,1.6157441 8.7185878,4.0337352 z "
+ transform="scale(0.6) rotate(180) translate(0,0)" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow1Mstart"
+ orient="auto"
+ refY="0.0"
+ refX="0.0"
+ id="Arrow1Mstart"
+ style="overflow:visible">
+ <path
+ id="path5070"
+ d="M 0.0,0.0 L 5.0,-5.0 L -12.5,0.0 L 5.0,5.0 L 0.0,0.0 z "
+ style="fill-rule:evenodd;stroke:#000000;stroke-width:1.0pt;marker-start:none"
+ transform="scale(0.4) translate(10,0)" />
+ </marker>
+ <marker
+ inkscape:stockid="Arrow1Lstart"
+ orient="auto"
+ refY="0.0"
+ refX="0.0"
+ id="Arrow1Lstart"
+ style="overflow:visible">
+ <path
+ id="path5064"
+ d="M 0.0,0.0 L 5.0,-5.0 L -12.5,0.0 L 5.0,5.0 L 0.0,0.0 z "
+ style="fill-rule:evenodd;stroke:#000000;stroke-width:1.0pt;marker-start:none"
+ transform="scale(0.8) translate(12.5,0)" />
+ </marker>
+ <linearGradient
+ inkscape:collect="always"
+ id="linearGradient3169">
+ <stop
+ style="stop-color:#000000;stop-opacity:1;"
+ offset="0"
+ id="stop3171" />
+ <stop
+ style="stop-color:#000000;stop-opacity:0;"
+ offset="1"
+ id="stop3173" />
+ </linearGradient>
+ <inkscape:perspective
+ sodipodi:type="inkscape:persp3d"
+ inkscape:vp_x="0 : 526.18109 : 1"
+ inkscape:vp_y="0 : 1000 : 0"
+ inkscape:vp_z="744.09448 : 526.18109 : 1"
+ inkscape:persp3d-origin="372.04724 : 350.78739 : 1"
+ id="perspective10" />
+ <radialGradient
+ inkscape:collect="always"
+ xlink:href="#linearGradient3169"
+ id="radialGradient5043"
+ gradientUnits="userSpaceOnUse"
+ gradientTransform="matrix(1,0,0,0.7009978,0,75.045142)"
+ cx="119.73291"
+ cy="250.98525"
+ fx="119.73291"
+ fy="250.98525"
+ r="20.210449" />
+ <radialGradient
+ inkscape:collect="always"
+ xlink:href="#linearGradient3169"
+ id="radialGradient8108"
+ gradientUnits="userSpaceOnUse"
+ gradientTransform="matrix(1,0,0,0.7009978,0,75.045142)"
+ cx="119.73291"
+ cy="250.98525"
+ fx="119.73291"
+ fy="250.98525"
+ r="20.210449" />
+ <radialGradient
+ inkscape:collect="always"
+ xlink:href="#linearGradient3169"
+ id="radialGradient8143"
+ gradientUnits="userSpaceOnUse"
+ gradientTransform="matrix(1,0,0,0.7009978,0,75.045142)"
+ cx="119.73291"
+ cy="250.98525"
+ fx="119.73291"
+ fy="250.98525"
+ r="20.210449" />
+ </defs>
+ <sodipodi:namedview
+ id="base"
+ pagecolor="#ffffff"
+ bordercolor="#666666"
+ borderopacity="1.0"
+ gridtolerance="10000"
+ guidetolerance="10"
+ objecttolerance="10"
+ inkscape:pageopacity="0.0"
+ inkscape:pageshadow="2"
+ inkscape:zoom="1.980749"
+ inkscape:cx="396.24639"
+ inkscape:cy="798.31866"
+ inkscape:document-units="px"
+ inkscape:current-layer="layer1"
+ showgrid="true"
+ inkscape:snap-global="true"
+ showguides="false"
+ inkscape:window-width="1104"
+ inkscape:window-height="666"
+ inkscape:window-x="99"
+ inkscape:window-y="73">
+ <inkscape:grid
+ type="xygrid"
+ id="grid2383" />
+ </sodipodi:namedview>
+ <metadata
+ id="metadata7">
+ <rdf:RDF>
+ <cc:Work
+ rdf:about="">
+ <dc:format>image/svg+xml</dc:format>
+ <dc:type
+ rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
+ </cc:Work>
+ </rdf:RDF>
+ </metadata>
+ <g
+ inkscape:label="Layer 1"
+ inkscape:groupmode="layer"
+ id="layer1">
+ <g
+ id="g2503">
+ <rect
+ rx="0"
+ ry="16.730608"
+ y="261.67963"
+ x="180.24718"
+ height="81.62571"
+ width="54.772434"
+ id="rect5031"
+ style="opacity:0.31999996;fill:#0000ff;fill-rule:evenodd;stroke:#000000;stroke-width:1.71234167;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
+ <flowRoot
+ transform="translate(92.155884,38.015877)"
+ style="font-size:14px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"
+ id="flowRoot5033"
+ xml:space="preserve"><flowRegion
+ style="fill:url(#radialGradient5043);fill-opacity:1"
+ id="flowRegion5035"><rect
+ style="font-size:14px;fill:#000000;fill-opacity:1"
+ y="233.51537"
+ x="98.755394"
+ height="64.808228"
+ width="82.296165"
+ id="rect5037" /></flowRegion><flowPara
+ style="fill:#000000;fill-opacity:1"
+ id="flowPara5041">Cam</flowPara></flowRoot> <path
+ sodipodi:nodetypes="cccc"
+ id="path5053"
+ d="M 160,257.36218 C 180,277.36218 180,277.36218 180,277.36218 L 160,297.36218 L 160,257.36218 z"
+ style="opacity:0.32758622;fill:#0000ff;fill-opacity:1;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" />
+ <flowRoot
+ transform="translate(90.463867,79.709452)"
+ style="font-size:14px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"
+ id="flowRoot8086"
+ xml:space="preserve"><flowRegion
+ style="fill:url(#radialGradient8108);fill-opacity:1"
+ id="flowRegion8088"><rect
+ style="font-size:14px;fill:#000000;fill-opacity:1"
+ y="233.51537"
+ x="98.755394"
+ height="64.808228"
+ width="82.296165"
+ id="rect8090" /></flowRegion><flowPara
+ style="fill:#000000;fill-opacity:1"
+ id="flowPara8092">Cam</flowPara></flowRoot> <path
+ sodipodi:nodetypes="cccc"
+ id="path8094"
+ d="M 160,307.36218 C 180,327.36218 180,327.36218 180,327.36218 L 160,347.36218 L 160,307.36218 z"
+ style="opacity:0.32758622;fill:#0000ff;fill-opacity:1;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" />
+ </g>
+ <g
+ id="g8129"
+ transform="matrix(1.99653,0,0,1.8385633,120.44009,-148.36533)">
+ <g
+ id="g8171"
+ transform="translate(-12.694916,-7.9273828)">
+ <rect
+ style="opacity:0.31999996;fill:#0000ff;fill-rule:evenodd;stroke:#000000;stroke-width:1.21800005;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
+ id="rect8131"
+ width="55.266777"
+ height="40.92971"
+ x="90.109184"
+ y="231.32329"
+ ry="8.3892555" />
+ <flowRoot
+ transform="translate(4.1820079,3.2300212)"
+ xml:space="preserve"
+ id="flowRoot8133"
+ style="font-size:7.30719709px;font-style:normal;font-weight:normal;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans"><flowRegion
+ id="flowRegion8135"
+ style="fill:url(#radialGradient8143);fill-opacity:1"><rect
+ id="rect8137"
+ width="82.296165"
+ height="64.808228"
+ x="98.755394"
+ y="233.51537"
+ style="font-size:7.30719709px;fill:#000000;fill-opacity:1" /></flowRegion><flowPara
+ id="flowPara8141"
+ style="font-size:7.30719709px;fill:#000000;fill-opacity:1">stereo_</flowPara><flowPara
+ id="flowPara8167"
+ style="font-size:7.30719709px;fill:#000000;fill-opacity:1">image_</flowPara><flowPara
+ id="flowPara8169"
+ style="font-size:7.30719709px;fill:#000000;fill-opacity:1">proc</flowPara></flowRoot> </g>
+ </g>
+ <path
+ style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1.15502298px;stroke-linecap:butt;stroke-linejoin:miter;marker-start:url(#Arrow1Mstart);stroke-opacity:1;display:inline"
+ d="M 270.30311,302.36412 L 234.24928,302.54437"
+ id="path8222"
+ inkscape:connector-type="polyline" />
+ <path
+ style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;marker-end:url(#Arrow1Mend)"
+ d="M 386.55766,300.04369 L 465.48049,300.12148"
+ id="path9290"
+ inkscape:connector-type="polyline"
+ inkscape:connection-start="#g8129" />
+ <text
+ xml:space="preserve"
+ style="font-size:10px;font-style:normal;font-weight:bold;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;font-family:Bitstream Vera Sans;-inkscape-font-specification:Bitstream Vera Sans Bold"
+ x="395"
+ y="177.36218"
+ id="text4430"><tspan
+ sodipodi:role="line"
+ id="tspan4432"
+ x="395"
+ y="177.36218">/stereo</tspan><tspan
+ sodipodi:role="line"
+ x="395"
+ y="189.86218"
+ id="tspan4442"> /image_disparity [DisparityImage]</tspan><tspan
+ sodipodi:role="line"
+ x="395"
+ y="202.36218"
+ id="tspan4434"> /left/image_mono [Image]</tspan><tspan
+ sodipodi:role="line"
+ x="395"
+ y="214.86218"
+ id="tspan2625"> /left/image_color [Image]</tspan><tspan
+ sodipodi:role="line"
+ x="395"
+ y="227.36218"
+ id="tspan4436"> /left/image_rect [Image]</tspan><tspan
+ sodipodi:role="line"
+ x="395"
+ y="239.86218"
+ id="tspan2627"> /left/image_rect_color [Image]</tspan><tspan
+ sodipodi:role="line"
+ x="395"
+ y="252.36218"
+ id="tspan2629"> /right/image_mono [Image]</tspan><tspan
+ sodipodi:role="line"
+ x="395"
+ y="264.86218"
+ id="tspan4438"> /right/image_color [Image]</tspan><tspan
+ sodipodi:role="line"
+ x="395"
+ y="277.36218"
+ id="tspan4440"> /right/image_rect [Image]</tspan><tspan
+ sodipodi:role="line"
+ x="395"
+ y="289.86218"
+ id="tspan2631"> /right/image_rect_color [Image]</tspan></text>
+ </g>
+</svg>
--
Alioth's /usr/local/bin/git-commit-notice on /srv/git.debian.org/git/debian-science/packages/ros/ros-image-pipeline.git
More information about the debian-science-commits
mailing list