From c18c505588e0e6a669d56d07d86a9eaa9d2b373d Mon Sep 17 00:00:00 2001 From: Alejandro Hernandez Cordero Date: Fri, 17 Apr 2026 14:30:02 +0200 Subject: [PATCH] Added rosdoc2 documentation to camera_calibration_parsers Signed-off-by: Alejandro Hernandez Cordero --- camera_calibration_parsers/doc/api.rst | 116 ++++++++++++++++++ camera_calibration_parsers/doc/conf.py | 35 ++++++ camera_calibration_parsers/doc/overview.rst | 88 +++++++++++++ .../camera_calibration_parsers/parse.hpp | 25 ++-- .../camera_calibration_parsers/parse_ini.hpp | 45 ++++--- .../camera_calibration_parsers/parse_yml.hpp | 43 ++++--- camera_calibration_parsers/package.xml | 5 +- camera_calibration_parsers/rosdoc2.yaml | 30 +++++ 8 files changed, 334 insertions(+), 53 deletions(-) create mode 100644 camera_calibration_parsers/doc/api.rst create mode 100644 camera_calibration_parsers/doc/conf.py create mode 100644 camera_calibration_parsers/doc/overview.rst create mode 100644 camera_calibration_parsers/rosdoc2.yaml diff --git a/camera_calibration_parsers/doc/api.rst b/camera_calibration_parsers/doc/api.rst new file mode 100644 index 00000000..6bbda048 --- /dev/null +++ b/camera_calibration_parsers/doc/api.rst @@ -0,0 +1,116 @@ +API Reference +============= + +High-Level API (``parse.hpp``) +------------------------------- + +The recommended entry point for most callers. The format is inferred from +the file extension (``.yaml`` / ``.yml`` → YAML, ``.ini`` → Videre INI). + +- :ref:`exhale_function_namespacecamera__calibration__parsers_1ab1426265c50ad5f1d2812ab900e201ec` — ``writeCalibration`` +- :ref:`exhale_function_namespacecamera__calibration__parsers_1a4242ca29511127c36ea71303b13c70cb` — ``readCalibration`` +- :ref:`exhale_function_namespacecamera__calibration__parsers_1a75f5ad9712a3de93e8523ac0c81a43b7` — ``parseCalibration`` + +Browse the full namespace: +:ref:`namespace_camera_calibration_parsers`. + +YAML API (``parse_yml.hpp``) +----------------------------- + +Format-specific overloads for YAML. Use these when reading from or writing +to an arbitrary stream rather than a named file. + +- :ref:`exhale_function_namespacecamera__calibration__parsers_1afa6d0c688357d7c01d474d01377d532d` — ``writeCalibrationYml(ostream)`` +- :ref:`exhale_function_namespacecamera__calibration__parsers_1a3d98138bf170b225af61515296315ef5` — ``readCalibrationYml(istream)`` +- :ref:`exhale_function_namespacecamera__calibration__parsers_1ad818651db64b842660e74e72bfc9c714` — ``writeCalibrationYml(filename)`` +- :ref:`exhale_function_namespacecamera__calibration__parsers_1aceea3789c0817cf94a2a24a04122f320` — ``readCalibrationYml(filename)`` +- :ref:`exhale_function_namespacecamera__calibration__parsers_1ab8b17fb67c52ebe800ff2021f6e6ae95` — ``parseCalibrationYml`` + +Videre INI API (``parse_ini.hpp``) +----------------------------------- + +Format-specific overloads for the Videre INI format. + +- :ref:`exhale_function_namespacecamera__calibration__parsers_1a4c25d94528aa8e4e551e2c6e89a53520` — ``writeCalibrationIni(ostream)`` +- :ref:`exhale_function_namespacecamera__calibration__parsers_1a3e89f85a374d92d7976087ab2a3f0f68` — ``readCalibrationIni(istream)`` +- :ref:`exhale_function_namespacecamera__calibration__parsers_1a0334c84f46853d8c472132166a22e897` — ``writeCalibrationIni(filename)`` +- :ref:`exhale_function_namespacecamera__calibration__parsers_1afaae91efdebd11870baf40459c46e141` — ``readCalibrationIni(filename)`` +- :ref:`exhale_function_namespacecamera__calibration__parsers_1a7642240533c0a270f72baf3272d693a6` — ``parseCalibrationIni`` + +Type Alias +---------- + +All three headers define: + +.. code-block:: cpp + + using CameraInfo = sensor_msgs::msg::CameraInfo; + +Usage Examples +-------------- + +**Read calibration from a file (auto-detect format)** + +.. code-block:: cpp + + #include + + std::string camera_name; + sensor_msgs::msg::CameraInfo camera_info; + + if (!camera_calibration_parsers::readCalibration( + "/path/to/camera.yaml", camera_name, camera_info)) + { + RCLCPP_ERROR(logger, "Failed to load calibration"); + } + +**Write calibration to a file** + +.. code-block:: cpp + + #include + + if (!camera_calibration_parsers::writeCalibration( + "/tmp/calibration.yaml", "my_camera", camera_info)) + { + RCLCPP_ERROR(logger, "Failed to save calibration"); + } + +**Parse calibration from an in-memory string** + +.. code-block:: cpp + + #include + + std::string yaml_data = get_calibration_from_parameter_server(); + std::string camera_name; + sensor_msgs::msg::CameraInfo camera_info; + + if (!camera_calibration_parsers::parseCalibration( + yaml_data, "yml", camera_name, camera_info)) + { + RCLCPP_ERROR(logger, "Failed to parse calibration string"); + } + +**Write to a stream (YAML)** + +.. code-block:: cpp + + #include + #include + + std::ostringstream oss; + camera_calibration_parsers::writeCalibrationYml(oss, "my_camera", camera_info); + std::string yaml_string = oss.str(); + +**Read from a stream (INI)** + +.. code-block:: cpp + + #include + #include + + std::ifstream ifs("/path/to/camera.ini"); + std::string camera_name; + sensor_msgs::msg::CameraInfo camera_info; + camera_calibration_parsers::readCalibrationIni(ifs, camera_name, camera_info); diff --git a/camera_calibration_parsers/doc/conf.py b/camera_calibration_parsers/doc/conf.py new file mode 100644 index 00000000..28259d12 --- /dev/null +++ b/camera_calibration_parsers/doc/conf.py @@ -0,0 +1,35 @@ +import os +import sys + +sys.path.insert(0, os.path.abspath('.')) + +project = 'camera_calibration_parsers' +copyright = '2024, Open Robotics' +author = 'Alejandro Hernandez Cordero, Geoffrey Biggs, Michael Carroll, Patrick Mihelich' + +extensions = [ + 'sphinx.ext.autodoc', + 'sphinx.ext.intersphinx', + 'breathe', + 'myst_parser', +] + +# breathe_projects and breathe_default_project are overridden by rosdoc2 at build time. +breathe_default_project = 'camera_calibration_parsers Doxygen Project' +breathe_default_members = ('members', 'undoc-members') + +source_suffix = { + '.rst': 'restructuredtext', + '.md': 'markdown', +} + +templates_path = ['_templates'] +# Exclude root-level copies of user docs (rosdoc2 also copies them to user_docs/ +# where they are covered by the user_docs.rst glob toctree). +exclude_patterns = [ + '_build', + 'overview.rst', + 'api.rst', +] + +html_theme = 'sphinx_rtd_theme' diff --git a/camera_calibration_parsers/doc/overview.rst b/camera_calibration_parsers/doc/overview.rst new file mode 100644 index 00000000..60eedf70 --- /dev/null +++ b/camera_calibration_parsers/doc/overview.rst @@ -0,0 +1,88 @@ +Overview +======== + +``camera_calibration_parsers`` provides C++ (and Python) functions for reading +and writing camera calibration data in two human-readable file formats: + +- **YAML** — the format produced by the ``camera_calibration`` tool and the + default format for ``camera_info_manager``. +- **Videre INI** — a legacy format used by older Videre stereo cameras and + still encountered in the field. + +The high-level API in ``parse.hpp`` detects the format automatically from +the file extension, so most callers never need the format-specific headers. + +API Layers +---------- + +.. code-block:: none + + parse.hpp ← recommended: auto-detects format from file extension + ├── parse_yml.hpp ← YAML-specific stream and file overloads + └── parse_ini.hpp ← Videre INI-specific stream and file overloads + +Each layer exposes three operation types: + +- **write** — serialise a ``sensor_msgs/msg/CameraInfo`` to a stream or file. +- **read** — deserialise from a stream or file into a ``CameraInfo``. +- **parse** — deserialise from an in-memory string buffer. + +File Formats +------------ + +YAML +~~~~ + +Produced and consumed by the ROS 2 ``camera_calibration`` package. Files +typically carry a ``.yaml`` or ``.yml`` extension and look like: + +.. code-block:: yaml + + image_width: 640 + image_height: 480 + camera_name: my_camera + camera_matrix: + rows: 3 + cols: 3 + data: [fx, 0, cx, 0, fy, cy, 0, 0, 1] + distortion_model: plumb_bob + distortion_coefficients: + rows: 1 + cols: 5 + data: [k1, k2, p1, p2, k3] + rectification_matrix: + rows: 3 + cols: 3 + data: [1, 0, 0, 0, 1, 0, 0, 0, 1] + projection_matrix: + rows: 3 + cols: 4 + data: [fx', 0, cx', Tx, 0, fy', cy', Ty, 0, 0, 1, 0] + +Videre INI +~~~~~~~~~~ + +A legacy format with a ``.ini`` extension, originally used by Videre Design +stereo cameras. The parser handles the subset commonly found in open-source +camera drivers. + +Python Bindings +--------------- + +A thin Python wrapper is available via the ``camera_calibration_parsers`` +Python module: + +.. code-block:: python + + from camera_calibration_parsers import readCalibration + + result = readCalibration('/path/to/camera.yaml') + if result is not None: + camera_name, camera_info = result + +Dependencies +------------ + +- ``sensor_msgs`` — ``CameraInfo`` message type. +- ``rclcpp`` — logging. +- ``yaml_cpp_vendor`` — YAML parsing and serialisation. diff --git a/camera_calibration_parsers/include/camera_calibration_parsers/parse.hpp b/camera_calibration_parsers/include/camera_calibration_parsers/parse.hpp index ea68d712..0ebe4e33 100644 --- a/camera_calibration_parsers/include/camera_calibration_parsers/parse.hpp +++ b/camera_calibration_parsers/include/camera_calibration_parsers/parse.hpp @@ -47,9 +47,10 @@ using CameraInfo = sensor_msgs::msg::CameraInfo; * * The file name extension (.yml, .yaml, or .ini) determines the output format. * - * \param file_name File to write - * \param camera_name Name of the camera - * \param cam_info Camera parameters + * \param file_name File to write. + * \param camera_name Name of the camera. + * \param cam_info Camera parameters to serialise. + * \return \c true on success, \c false on I/O or format error. */ CAMERA_CALIBRATION_PARSERS_PUBLIC bool writeCalibration( @@ -59,11 +60,12 @@ bool writeCalibration( /** * \brief Read calibration parameters from a file. * - * The file may be YAML or INI format. + * The file format (YAML or INI) is detected automatically from the extension. * - * \param file_name File to read - * \param[out] camera_name Name of the camera - * \param[out] cam_info Camera parameters + * \param file_name File to read. + * \param[out] camera_name Camera name read from the file. + * \param[out] cam_info Camera parameters read from the file. + * \return \c true on success, \c false if the file cannot be opened or parsed. */ CAMERA_CALIBRATION_PARSERS_PUBLIC bool readCalibration( @@ -73,10 +75,11 @@ bool readCalibration( /** * \brief Parse calibration parameters from a string in memory. * - * \param buffer Calibration string - * \param format Format of calibration string, "yml" or "ini" - * \param[out] camera_name Name of the camera - * \param[out] cam_info Camera parameters + * \param buffer Calibration data as a string. + * \param format Format of the data: \c "yml" or \c "ini". + * \param[out] camera_name Camera name parsed from the buffer. + * \param[out] cam_info Camera parameters parsed from the buffer. + * \return \c true on success, \c false if the buffer cannot be parsed. */ CAMERA_CALIBRATION_PARSERS_PUBLIC bool parseCalibration( diff --git a/camera_calibration_parsers/include/camera_calibration_parsers/parse_ini.hpp b/camera_calibration_parsers/include/camera_calibration_parsers/parse_ini.hpp index d1af48ef..2a28ca97 100644 --- a/camera_calibration_parsers/include/camera_calibration_parsers/parse_ini.hpp +++ b/camera_calibration_parsers/include/camera_calibration_parsers/parse_ini.hpp @@ -43,11 +43,12 @@ namespace camera_calibration_parsers using CameraInfo = sensor_msgs::msg::CameraInfo; /** - * \brief Write calibration parameters to a file in INI format. + * \brief Write calibration parameters to a stream in Videre INI format. * - * \param out Output stream to write to - * \param camera_name Name of the camera - * \param cam_info Camera parameters + * \param out Output stream to write to. + * \param camera_name Name of the camera. + * \param cam_info Camera parameters to serialise. + * \return \c true on success, \c false on write error. */ CAMERA_CALIBRATION_PARSERS_PUBLIC bool writeCalibrationIni( @@ -55,11 +56,12 @@ bool writeCalibrationIni( const CameraInfo & cam_info); /** - * \brief Read calibration parameters from an INI file. + * \brief Read calibration parameters from a stream in Videre INI format. * - * \param in Input stream to read from - * \param[out] camera_name Name of the camera - * \param[out] cam_info Camera parameters + * \param in Input stream to read from. + * \param[out] camera_name Camera name read from the stream. + * \param[out] cam_info Camera parameters read from the stream. + * \return \c true on success, \c false if the stream cannot be parsed. */ CAMERA_CALIBRATION_PARSERS_PUBLIC bool readCalibrationIni( @@ -67,11 +69,12 @@ bool readCalibrationIni( CameraInfo & cam_info); /** - * \brief Write calibration parameters to a file in INI format. + * \brief Write calibration parameters to a file in Videre INI format. * - * \param file_name File to write - * \param camera_name Name of the camera - * \param cam_info Camera parameters + * \param file_name File path to write. + * \param camera_name Name of the camera. + * \param cam_info Camera parameters to serialise. + * \return \c true on success, \c false on I/O error. */ CAMERA_CALIBRATION_PARSERS_PUBLIC bool writeCalibrationIni( @@ -79,11 +82,12 @@ bool writeCalibrationIni( const CameraInfo & cam_info); /** - * \brief Read calibration parameters from an INI file. + * \brief Read calibration parameters from a file in Videre INI format. * - * \param file_name File to read - * \param[out] camera_name Name of the camera - * \param[out] cam_info Camera parameters + * \param file_name File path to read. + * \param[out] camera_name Camera name read from the file. + * \param[out] cam_info Camera parameters read from the file. + * \return \c true on success, \c false if the file cannot be opened or parsed. */ CAMERA_CALIBRATION_PARSERS_PUBLIC bool readCalibrationIni( @@ -91,11 +95,12 @@ bool readCalibrationIni( CameraInfo & cam_info); /** - * \brief Parse calibration parameters from a string in memory of INI format. + * \brief Parse calibration parameters from an in-memory string in Videre INI format. * - * \param buffer Calibration string - * \param[out] camera_name Name of the camera - * \param[out] cam_info Camera parameters + * \param buffer INI-format calibration data as a string. + * \param[out] camera_name Camera name parsed from the buffer. + * \param[out] cam_info Camera parameters parsed from the buffer. + * \return \c true on success, \c false if the buffer cannot be parsed. */ CAMERA_CALIBRATION_PARSERS_PUBLIC bool parseCalibrationIni( diff --git a/camera_calibration_parsers/include/camera_calibration_parsers/parse_yml.hpp b/camera_calibration_parsers/include/camera_calibration_parsers/parse_yml.hpp index 48027f13..34d76e51 100644 --- a/camera_calibration_parsers/include/camera_calibration_parsers/parse_yml.hpp +++ b/camera_calibration_parsers/include/camera_calibration_parsers/parse_yml.hpp @@ -44,11 +44,12 @@ namespace camera_calibration_parsers using CameraInfo = sensor_msgs::msg::CameraInfo; /** - * \brief Write calibration parameters to a file in YAML format. + * \brief Write calibration parameters to a stream in YAML format. * - * \param out Output stream to write to - * \param camera_name Name of the camera - * \param cam_info Camera parameters + * \param out Output stream to write to. + * \param camera_name Name of the camera. + * \param cam_info Camera parameters to serialise. + * \return \c true on success, \c false on write error. */ CAMERA_CALIBRATION_PARSERS_PUBLIC bool writeCalibrationYml( @@ -56,11 +57,12 @@ bool writeCalibrationYml( const CameraInfo & cam_info); /** - * \brief Read calibration parameters from a YAML file. + * \brief Read calibration parameters from a stream in YAML format. * - * \param in Input stream to read from - * \param[out] camera_name Name of the camera - * \param[out] cam_info Camera parameters + * \param in Input stream to read from. + * \param[out] camera_name Camera name read from the stream. + * \param[out] cam_info Camera parameters read from the stream. + * \return \c true on success, \c false if the stream cannot be parsed. */ CAMERA_CALIBRATION_PARSERS_PUBLIC bool readCalibrationYml( @@ -70,9 +72,10 @@ bool readCalibrationYml( /** * \brief Write calibration parameters to a file in YAML format. * - * \param file_name File to write - * \param camera_name Name of the camera - * \param cam_info Camera parameters + * \param file_name File path to write. + * \param camera_name Name of the camera. + * \param cam_info Camera parameters to serialise. + * \return \c true on success, \c false on I/O error. */ CAMERA_CALIBRATION_PARSERS_PUBLIC bool writeCalibrationYml( @@ -80,11 +83,12 @@ bool writeCalibrationYml( const CameraInfo & cam_info); /** - * \brief Read calibration parameters from a YAML file. + * \brief Read calibration parameters from a file in YAML format. * - * \param file_name File to read - * \param[out] camera_name Name of the camera - * \param[out] cam_info Camera parameters + * \param file_name File path to read. + * \param[out] camera_name Camera name read from the file. + * \param[out] cam_info Camera parameters read from the file. + * \return \c true on success, \c false if the file cannot be opened or parsed. */ CAMERA_CALIBRATION_PARSERS_PUBLIC bool readCalibrationYml( @@ -92,11 +96,12 @@ bool readCalibrationYml( CameraInfo & cam_info); /** - * \brief Parse calibration parameters from a string in memory of yaml format. + * \brief Parse calibration parameters from an in-memory string in YAML format. * - * \param buffer Calibration string - * \param[out] camera_name Name of the camera - * \param[out] cam_info Camera parameters + * \param buffer YAML-format calibration data as a string. + * \param[out] camera_name Camera name parsed from the buffer. + * \param[out] cam_info Camera parameters parsed from the buffer. + * \return \c true on success, \c false if the buffer cannot be parsed. */ CAMERA_CALIBRATION_PARSERS_PUBLIC bool parseCalibrationYml( diff --git a/camera_calibration_parsers/package.xml b/camera_calibration_parsers/package.xml index 29cf02a0..8de3bf04 100644 --- a/camera_calibration_parsers/package.xml +++ b/camera_calibration_parsers/package.xml @@ -1,9 +1,7 @@ camera_calibration_parsers 6.4.7 - - camera_calibration_parsers contains routines for reading and writing camera calibration parameters. - + Routines for reading and writing camera calibration parameters in YAML and Videre INI formats. Michael Carroll Patrick Mihelich @@ -29,5 +27,6 @@ ament_cmake + rosdoc2.yaml diff --git a/camera_calibration_parsers/rosdoc2.yaml b/camera_calibration_parsers/rosdoc2.yaml new file mode 100644 index 00000000..da69cb6b --- /dev/null +++ b/camera_calibration_parsers/rosdoc2.yaml @@ -0,0 +1,30 @@ +## Default configuration, generated by rosdoc2. + +## This 'attic section' self-documents this file's type and version. +type: 'rosdoc2 config' +version: 1 + +--- + +settings: + always_run_doxygen: true + +builders: + - doxygen: { + name: 'camera_calibration_parsers Public C/C++ API', + output_dir: 'generated/doxygen', + extra_doxyfile_statements: [ + 'PREDEFINED = DOXYGEN_SHOULD_SKIP_THIS CAMERA_CALIBRATION_PARSERS_PUBLIC= CAMERA_CALIBRATION_PARSERS_LOCAL= CAMERA_CALIBRATION_PARSERS_PUBLIC_TYPE= CAMERA_CALIBRATION_PARSERS_EXPORT= CAMERA_CALIBRATION_PARSERS_IMPORT=', + 'EXTRACT_ALL = YES', + 'EXTRACT_PRIVATE = NO', + 'EXTRACT_STATIC = YES', + 'ENABLE_PREPROCESSING = YES', + 'QUIET = YES' + ] + } + - sphinx: { + name: 'camera_calibration_parsers', + sphinx_sourcedir: doc, + doxygen_xml_directory: 'generated/doxygen/xml', + output_dir: '' + }