rtabmap_python tests and doc (#1455)

rtabmap_python/README.md

@@ -0,0 +1,49 @@
+# rtabmap_python
+
+Python helpers for reading and writing the binary formats [RTAB-Map](https://github.com/introlab/rtabmap) uses.
+
+RTAB-Map is a C++ library, and the data it hands to ROS is not always plain ROS types. Several `rtabmap_msgs` fields — and every blob in an `.db` database — carry a matrix in RTAB-Map's own compressed encoding rather than as a `sensor_msgs/Image` or an array. This package is the Python side of that encoding, for scripts that read those fields without going through the C++ library.
+
+There are no nodes here. It is an `ament_python` package that installs one importable module.
+
+## Module
+
+`rtabmap_python.cv_compression` — a single-channel `cv::Mat` to and from bytes.
+
+| Function | Description |
+|---|---|
+| `compress(data)` | 1-D or 2-D numpy array → `bytearray`. |
+| `uncompress(data)` | those bytes → 2-D numpy array. |
+
+```python
+import numpy as np
+from rtabmap_python.cv_compression import compress, uncompress
+
+scan = np.zeros((360, 2), dtype=np.float32)
+blob = compress(scan)          # what the message field carries
+restored = uncompress(blob)    # (360, 2) float32
+```
+
+The encoding is a zlib stream followed by a 12-byte trailer holding rows, cols and the element type as three `int32`. It matches `compressData()` and `uncompressData()` in RTAB-Map's `corelib/src/Compression.cpp` byte for byte, so either side can read what the other wrote. The [module docstring](rtabmap_python/cv_compression.py) has the exact layout, and the generated [Python API reference](https://docs.ros.org/en/jazzy/p/rtabmap_python/) renders it alongside the two functions.
+
+## Things worth knowing
+
+**The result is read-only.** `uncompress` views the decompressed buffer instead of copying it, so the array it returns has `writeable=False` and assigning into it raises. Call `.copy()` if you need to modify it.
+
+**Single-channel only.** The C++ encoder packs the channel count into the type code; the tables here cover the single-channel depths `CV_8U` through `CV_64F`. A multi-channel matrix written by the C++ side raises `KeyError` rather than decoding wrongly. So does an unsupported dtype on the way in — `int64` and `float16` have no encoding.
+
+**The trailer is host-endian**, because the C++ side writes raw `int`s. A blob is not portable between machines of opposite endianness.
+
+## Building and testing
+
+```bash
+colcon build --packages-select rtabmap_python
+colcon test --packages-select rtabmap_python
+colcon test-result --verbose
+```
+
+The tests cover the round trip for every supported depth, the exact trailer bytes against the codes RTAB-Map's `serializeMatType()` produces, and each of the behaviours above. Alongside them, the standard `ament_copyright`, `ament_flake8` and `ament_pep257` linters run over the package.
+
+## License
+
+BSD-3-Clause. See the [repository root](https://github.com/introlab/rtabmap_ros#license).

rtabmap_python/package.xml

@@ -10,6 +10,8 @@
   <url type="bugtracker">https://github.com/introlab/rtabmap_ros/issues</url>
   <url type="repository">https://github.com/introlab/rtabmap_ros</url>
 
+  <depend>python3-numpy</depend>
+
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>
   <test_depend>ament_pep257</test_depend>
@@ -17,5 +19,6 @@
 
   <export>
     <build_type>ament_python</build_type>
+    <rosdoc2>rosdoc2.yaml</rosdoc2>
   </export>
 </package>

rtabmap_python/rosdoc2.yaml

@@ -0,0 +1,29 @@
+## Configuration for rosdoc2, the documentation generator used by docs.ros.org.
+## Regenerate the annotated default with:
+##   rosdoc2 default_config --package-path rtabmap_python
+## Build the docs locally with:
+##   rosdoc2 build --package-path rtabmap_python --output-directory doc_output
+
+## This 'attic section' self-documents this file's type and version.
+type: 'rosdoc2 config'
+version: 1
+
+---
+
+settings:
+    ## Generate the standard index page from package.xml (description, maintainer,
+    ## license, links) and a table of contents for the builders below.
+    generate_package_index: true
+
+    ## This is an ament_python package: there are no C/C++ headers to parse, and the
+    ## API reference comes from the module docstrings via sphinx-apidoc.
+    always_run_doxygen: false
+    always_run_sphinx_apidoc: true
+
+builders:
+    ## Sphinx renders the landing page and the autodoc pages sphinx-apidoc produces
+    ## from rtabmap_python/.
+    - sphinx: {
+        name: 'rtabmap_python',
+        output_dir: ''
+    }

rtabmap_python/rtabmap_python/cv_compression.py

@@ -27,6 +27,35 @@
 # POSSIBILITY OF SUCH DAMAGE.
 
 
+"""
+Compress numpy arrays into RTAB-Map's ``cv::Mat`` wire format.
+
+RTAB-Map stores and transmits matrices -- images, laser scans, descriptors -- as a zlib
+payload followed by a 12-byte trailer recording the shape and the element type. Database
+blobs and the compressed fields of ``rtabmap_msgs`` messages both use it.
+
+The layout is::
+
+    [ zlib stream of the elements in C order ][ rows ][ cols ][ type ]
+                                               int32   int32   int32
+
+The three trailer fields are written with ``struct`` format ``'iii'`` -- native byte order
+and size, matching the C++ side's raw ``int`` writes. That makes the encoding
+**host-endian**, so a blob does not travel between machines of opposite endianness.
+
+``type`` is the OpenCV depth of the elements: 0 ``CV_8U``, 1 ``CV_8S``, 2 ``CV_16U``,
+3 ``CV_16S``, 4 ``CV_32S``, 5 ``CV_32F``, 6 ``CV_64F``.
+
+These two functions are the Python side of that format. They match ``compressData()`` and
+``uncompressData()`` in RTAB-Map's ``corelib/src/Compression.cpp`` byte for byte, so a
+matrix written by either side can be read by the other.
+
+Single-channel matrices only. The C++ encoder packs the channel count into the type code
+alongside the depth; the tables here cover the single-channel depths ``CV_8U`` through
+``CV_64F``, which is what the codes 0 to 6 mean.
+"""
+
+
 import struct
 import zlib
 
@@ -34,6 +63,20 @@ import numpy as np
 
 
 def compress(data):
+    """
+    Compress a 1-D or 2-D array into RTAB-Map's format.
+
+    :param data: a single-channel array whose dtype is one of ``uint8``, ``int8``,
+        ``uint16``, ``int16``, ``int32``, ``float32`` or ``float64``. A 1-D array of
+        length ``n`` is recorded as a 1-by-``n`` matrix, which is the shape
+        :func:`uncompress` gives back. Any memory layout is accepted; the bytes are
+        always written in C order.
+    :returns: a ``bytearray`` holding the zlib payload followed by the trailer described
+        in the module docstring.
+    :raises AssertionError: if ``data`` has more than two dimensions.
+    :raises KeyError: if its dtype is not one of the seven above -- ``int64`` and
+        ``float16`` have no encoding in this format.
+    """
     assert data.ndim == 1 or data.ndim == 2
 
     dim1 = 1
@@ -63,6 +106,16 @@ def compress(data):
 
 
 def uncompress(data):
+    """
+    Restore an array written by :func:`compress` or by RTAB-Map's C++ side.
+
+    :param data: a bytes-like object laid out as :func:`compress` returns.
+    :returns: a 2-D array of the recorded shape and dtype. It is 2-D even when
+        :func:`compress` was handed a 1-D array, and it is **read-only**: it views the
+        decompressed buffer instead of copying it, so call ``.copy()`` before writing.
+    :raises KeyError: if the trailer's type code is not a single-channel depth 0 to 6,
+        which is what a multi-channel matrix from the C++ side encodes to.
+    """
     cvtype_to_numpy_type = {
         0: 'uint8',
         1: 'int8',

rtabmap_python/test/test_cv_compression.py

@@ -0,0 +1,178 @@
+# Copyright 2025 matlabbe
+#
+# 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 matlabbe 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 HOLDER 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.
+
+
+"""Tests for :mod:`rtabmap_python.cv_compression`."""
+
+import struct
+import zlib
+
+import numpy as np
+import pytest
+
+from rtabmap_python.cv_compression import compress, uncompress
+
+
+# The single-channel depths the format encodes, with the codes RTAB-Map's
+# serializeMatType() gives them: CV_8U, CV_8S, CV_16U, CV_16S, CV_32S, CV_32F, CV_64F.
+SUPPORTED_TYPES = [
+    ('uint8', 0),
+    ('int8', 1),
+    ('uint16', 2),
+    ('int16', 3),
+    ('int32', 4),
+    ('float32', 5),
+    ('float64', 6),
+]
+
+# Three int32: rows, cols, type code.
+TRAILER_SIZE = 3 * 4
+
+
+@pytest.mark.parametrize('dtype,code', SUPPORTED_TYPES)
+def test_roundtrip_preserves_shape_dtype_and_values(dtype, code):
+    """Every supported depth survives a compress/uncompress cycle unchanged."""
+    data = np.arange(12, dtype=dtype).reshape(3, 4)
+
+    result = uncompress(compress(data))
+
+    assert result.shape == (3, 4)
+    assert result.dtype == np.dtype(dtype)
+    assert np.array_equal(result, data)
+
+
+@pytest.mark.parametrize('dtype,code', SUPPORTED_TYPES)
+def test_trailer_records_rows_cols_and_type_code(dtype, code):
+    """The last 12 bytes are rows, cols and the type code, as the C++ side writes them."""
+    data = np.zeros((3, 4), dtype=dtype)
+
+    trailer = bytes(compress(data)[-TRAILER_SIZE:])
+
+    assert trailer == struct.pack('iii', 3, 4, code)
+
+
+def test_payload_is_plain_zlib_of_the_c_order_bytes():
+    """Everything before the trailer is a zlib stream, so the C++ side can inflate it."""
+    data = np.arange(6, dtype=np.uint8)
+
+    payload = bytes(compress(data)[:-TRAILER_SIZE])
+
+    assert zlib.decompress(payload) == data.tobytes()
+
+
+def test_compress_returns_a_bytearray():
+    """The return type is a bytearray, which is what the message fields expect."""
+    assert isinstance(compress(np.zeros(4, dtype=np.uint8)), bytearray)
+
+
+def test_one_dimensional_input_comes_back_as_a_single_row():
+    """A 1-D array is recorded as 1-by-n, so the roundtrip is not shape-preserving."""
+    data = np.arange(5, dtype=np.float32)
+
+    result = uncompress(compress(data))
+
+    assert result.shape == (1, 5)
+    assert np.array_equal(result.ravel(), data)
+
+
+@pytest.mark.parametrize('shape', [(1, 5), (5, 1), (2, 3)])
+def test_two_dimensional_shapes_are_preserved_exactly(shape):
+    """Rows and cols are recorded separately, so no 2-D shape is transposed or flattened."""
+    data = np.arange(5 if 1 in shape else 6, dtype=np.uint8).reshape(shape)
+
+    assert uncompress(compress(data)).shape == shape
+
+
+def test_non_contiguous_input_roundtrips():
+    """A transposed view is written in C order, so it reads back as the same matrix."""
+    data = np.arange(12, dtype=np.int16).reshape(3, 4).T
+    assert not data.flags.c_contiguous
+
+    result = uncompress(compress(data))
+
+    assert result.shape == (4, 3)
+    assert np.array_equal(result, data)
+
+
+def test_empty_array_roundtrips_as_an_empty_row():
+    """An empty array is not a special case; it comes back as a 1-by-0 matrix."""
+    result = uncompress(compress(np.array([], dtype=np.uint8)))
+
+    assert result.shape == (1, 0)
+    assert result.dtype == np.uint8
+
+
+def test_uncompressed_array_is_read_only():
+    """uncompress() views the decompressed buffer rather than copying it."""
+    result = uncompress(compress(np.arange(4, dtype=np.uint8)))
+
+    assert not result.flags.writeable
+    with pytest.raises(ValueError):
+        result[0, 0] = 1
+    # .copy() is the way out, as the docstring says.
+    assert result.copy().flags.writeable
+
+
+def test_accepts_bytes_as_well_as_bytearray():
+    """uncompress() reads whatever compress() produced, converted or not."""
+    data = np.arange(8, dtype=np.uint16).reshape(2, 4)
+
+    result = uncompress(bytes(compress(data)))
+
+    assert np.array_equal(result, data)
+
+
+def test_larger_matrix_roundtrips():
+    """A matrix big enough to actually exercise zlib, with non-trivial content."""
+    rng = np.random.default_rng(42)
+    data = rng.integers(0, 255, size=(120, 160), dtype=np.uint8)
+
+    assert np.array_equal(uncompress(compress(data)), data)
+
+
+def test_rejects_more_than_two_dimensions():
+    """The format has no encoding for a third dimension, so compress() refuses one."""
+    with pytest.raises(AssertionError):
+        compress(np.zeros((2, 2, 2), dtype=np.uint8))
+
+
+@pytest.mark.parametrize('dtype', ['int64', 'uint64', 'float16'])
+def test_rejects_unsupported_dtype(dtype):
+    """Depths outside CV_8U..CV_64F have no type code and raise rather than truncate."""
+    with pytest.raises(KeyError):
+        compress(np.zeros(4, dtype=dtype))
+
+
+def test_rejects_unknown_type_code():
+    """A trailer from a multi-channel C++ matrix encodes a code this table lacks."""
+    payload = bytes(compress(np.zeros((2, 2), dtype=np.uint8))[:-TRAILER_SIZE])
+    # What serializeMatType() returns for a 3-channel CV_8U matrix: depth + ((cn - 1) << 3).
+    forged = bytearray(payload) + struct.pack('iii', 2, 2, 0 + ((3 - 1) << 3))
+
+    with pytest.raises(KeyError):
+        uncompress(forged)