rtabmap_conversions tests and doc (#1449)

* Initial tests

* more tests

* More in-depth deskew() testing

* slightly less verbose clamping corruption warning

* added tf buffer related tests

* added remaining tests

* Added rosdoc2, improve tests when we require sync of odom stamp and sensor stamp

* cleanup doc

* fixing ci

.github/workflows/ros2.yml

@@ -25,24 +25,24 @@ jobs:
         include:
         - ros_distro: humble
           skip_keys: ''
-          packages: 'rtabmap_ros'
+          packages: 'rtabmap_ros rtabmap_conversions'
         - ros_distro: jazzy
           skip_keys: ''
-          packages: 'rtabmap_ros'
+          packages: 'rtabmap_ros rtabmap_conversions'
         - ros_distro: kilted
           skip_keys: 'grid_map_ros'
-          packages: 'rtabmap_ros'
+          packages: 'rtabmap_ros rtabmap_conversions'
         - ros_distro: lyrical
           skip_keys: 'nav2_bringup nav2_msgs velodyne grid_map_ros realsense2_camera libpointmatcher'
           # rtabmap_costmap_plugins cannot be built, missing nav2 on lyrical, build other packages:
-          packages: 'rtabmap_launch rtabmap_demos rtabmap_python rtabmap_examples rtabmap_rviz_plugins'
+          packages: 'rtabmap_launch rtabmap_demos rtabmap_python rtabmap_examples rtabmap_rviz_plugins rtabmap_conversions'
         - ros_distro: rolling
           # libpointmatcher and gtsam are rtabmap deps not yet available on rolling
           # (same skip keys as introlab/rtabmap's cmake-ros workflow)
           skip_keys: 'nav2_bringup nav2_msgs velodyne libpointmatcher gtsam'
           use_ros2_testing: true # Rolling is using ros2-testing (nightly)
           # rtabmap_costmap_plugins cannot be built, missing nav2 on rolling, build other packages:
-          packages: 'rtabmap_launch rtabmap_demos rtabmap_python rtabmap_examples rtabmap_rviz_plugins'
+          packages: 'rtabmap_launch rtabmap_demos rtabmap_python rtabmap_examples rtabmap_rviz_plugins rtabmap_conversions'
       fail-fast: false
     container:
       image: osrf/ros:${{ matrix.ros_distro }}-desktop-full

.gitignore

@@ -1,3 +1,6 @@
 .pydevproject
 .settings
 __pycache__
+# rosdoc2 build artifacts
+docs_build
+cross_reference

rtabmap_conversions/CMakeLists.txt

@@ -122,5 +122,22 @@ install(DIRECTORY include/
    FILES_MATCHING PATTERN "*.h"
 )
 
+#############
+## Testing ##
+#############
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+
+  ament_add_gtest(test_msg_conversion test/test_msg_conversion.cpp)
+  if(TARGET test_msg_conversion)
+    target_link_libraries(test_msg_conversion rtabmap_conversions)
+    if("$ENV{ROS_DISTRO}" STRLESS "lyrical")
+      ament_target_dependencies(test_msg_conversion ${AmentLibraries})
+    else()
+      target_link_libraries(test_msg_conversion ${Libraries} ${PublicLibraries})
+    endif()
+  endif()
+endif()
+
 ament_package()
 

rtabmap_conversions/README.md

@@ -0,0 +1,83 @@
+# rtabmap_conversions
+
+Conversions between [RTAB-Map](https://github.com/introlab/rtabmap) library types and ROS 2 messages.
+
+This package is a library only — it contains no nodes, no launch files and no parameters. Every other `rtabmap_ros` package that touches a message goes through it: [`rtabmap_slam`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_slam), [`rtabmap_odom`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_odom), [`rtabmap_sync`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_sync), [`rtabmap_util`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_util), [`rtabmap_viz`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_viz) and [`rtabmap_rviz_plugins`](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_rviz_plugins).
+
+You only need it directly if you are writing your own node against RTAB-Map's C++ API and want to publish or subscribe to `rtabmap_msgs`.
+
+## Usage
+
+Add the dependency to your `package.xml` and `CMakeLists.txt`:
+
+```xml
+<depend>rtabmap_conversions</depend>
+```
+
+```cmake
+find_package(rtabmap_conversions REQUIRED)
+target_link_libraries(my_node rtabmap_conversions::rtabmap_conversions)
+```
+
+Everything lives in a single header and the `rtabmap_conversions` namespace:
+
+```cpp
+#include <rtabmap_conversions/MsgConversion.h>
+
+// A pose message to an rtabmap::Transform and back.
+rtabmap::Transform pose = rtabmap_conversions::transformFromPoseMsg(msg.pose);
+
+geometry_msgs::msg::Pose out;
+rtabmap_conversions::transformToPoseMsg(pose, out);
+```
+
+The naming is uniform: `xxxFromROS()` converts a message into an RTAB-Map type, `xxxToROS()` goes the other way. `ToROS()` functions write through a reference parameter so the message can be reused; `FromROS()` functions return by value.
+
+## What it covers
+
+| Group | Functions |
+|---|---|
+| Transforms | `transformFromTF`, `transformToTF`, `transformFromGeometryMsg`, `transformToGeometryMsg`, `transformFromPoseMsg`, `transformToPoseMsg` |
+| TF lookups | `getTransform`, `getMovingTransform` |
+| Camera models | `cameraModelFromROS`, `cameraModelToROS`, `stereoCameraModelFromROS` |
+| Images | `toCvCopy`, `toCvShare`, `rgbdImageFromROS`, `rgbdImageToROS`, `convertRGBDMsgs`, `convertStereoMsg` |
+| Laser scans | `convertScanMsg`, `convertScan3dMsg`, `deskew`, `transformPointCloud`, `sizeOfPointField` |
+| Features | `keypointFromROS`, `point2fFromROS`, `point3fFromROS`, `globalDescriptorFromROS` (+ vector and `ToROS` variants) |
+| Graph | `mapDataFromROS`, `mapGraphFromROS`, `nodeFromROS`, `linkFromROS`, `sensorDataFromROS` (+ `ToROS` variants) |
+| Misc | `infoFromROS`, `odomInfoFromROS`, `odomInfoToStatistics`, `imuFromROS`, `userDataFromROS`, `envSensorFromROS`, `landmarksFromROS`, `timestampFromROS`, `timestampToROS` |
+
+Full signatures and per-function notes are in the [API documentation](https://docs.ros.org/en/rolling/p/rtabmap_conversions/) and in [`MsgConversion.h`](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h).
+
+## Conventions worth knowing
+
+These cut across the whole API and are not obvious from the signatures. Per-function caveats — object lifetimes, which fields a given `ToROS()` fills — are documented on the functions themselves.
+
+**Null transforms.** RTAB-Map distinguishes a *null* transform (unknown) from identity. Over the wire this is encoded as an all-zero quaternion, so `transformFromGeometryMsg()` and `transformFromPoseMsg()` return a null `rtabmap::Transform` for one. Always check `isNull()` before using a result. `tf2::Transform` cannot represent this — it stores rotation as a basis matrix — so `transformToTF()` returns a `bool` instead.
+
+**`CameraInfo` matrices are fixed-size arrays.** `k`, `r` and `p` are `std::array`, so they are never "empty" — an unset matrix is all zeros. `cameraModelFromROS()` treats a zero `k[0]`/`p[0]` (the focal length) as absent.
+
+## Building and testing
+
+```bash
+colcon build --packages-select rtabmap_conversions
+colcon test --packages-select rtabmap_conversions
+colcon test-result --verbose
+```
+
+## Documentation
+
+API documentation is generated with [rosdoc2](https://github.com/ros-infrastructure/rosdoc2) from the Doxygen comments in the public header, and published to [docs.ros.org](https://docs.ros.org/en/rolling/p/rtabmap_conversions/). To build it locally:
+
+```bash
+rosdoc2 build --package-path rtabmap_conversions --output-directory doc_output
+```
+
+Besides `doc_output`, rosdoc2 writes `docs_build/` and `cross_reference/` scratch directories into the current directory. `docs_build/` contains a copy of the package manifest, so colcon then sees two packages of the same name and every later build fails with `Duplicate package names not supported`. Mark it once and the problem goes away for good — rosdoc2 leaves an existing marker in place on subsequent runs:
+
+```bash
+touch docs_build/COLCON_IGNORE
+```
+
+## License
+
+BSD-3-Clause. See the [repository root](https://github.com/introlab/rtabmap_ros#license).

rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h

@@ -71,76 +71,364 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define RCLCPP_QOS(queueSize, qos) rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qos)
 #endif
 
+/**
+ * @namespace rtabmap_conversions
+ * @brief Conversions between RTAB-Map library types and ROS 2 messages.
+ *
+ * Naming is uniform throughout: `xxxFromROS()` converts a message into an RTAB-Map
+ * type and returns it by value, `xxxToROS()` writes an RTAB-Map type into a message
+ * passed by reference so the message can be reused.
+ *
+ * @note RTAB-Map distinguishes a *null* transform (unknown) from an identity one. On
+ *       the wire a null transform is encoded as an all-zero quaternion, so results of
+ *       the `transformFromXxx()` functions should be checked with
+ *       rtabmap::Transform::isNull() before use.
+ */
 namespace rtabmap_conversions {
 
-void transformToTF(const rtabmap::Transform & transform, tf2::Transform & tfTransform);
+//============================================================================
+// Transforms
+// Conversions between rtabmap::Transform and the tf2 / geometry_msgs representations.
+//============================================================================
+
+/**
+ * @brief Convert a rtabmap::Transform into a tf2::Transform.
+ * @param[in]  transform   the transform to convert
+ * @param[out] tfTransform the converted transform, or filled with NaN if @p transform is null
+ * @return false if @p transform is null, true otherwise
+ *
+ * @note tf2::Transform stores its rotation as a basis matrix and so cannot represent
+ *       the all-zero quaternion used elsewhere to mean "null". The null case is
+ *       reported through the return value instead, and the output is poisoned with
+ *       NaN so that ignoring that return value fails loudly rather than silently
+ *       proceeding with a plausible-looking identity.
+ * @see transformFromTF()
+ */
+bool transformToTF(const rtabmap::Transform & transform, tf2::Transform & tfTransform);
+
+/**
+ * @brief Convert a tf2::Transform into a rtabmap::Transform.
+ * @param transform the transform to convert
+ * @return the converted transform, or a null transform if @p transform contains NaN
+ *         (which is how transformToTF() reports a null transform)
+ * @see transformToTF()
+ */
 rtabmap::Transform transformFromTF(const tf2::Transform & transform);
 
+/**
+ * @brief Convert a rtabmap::Transform into a geometry_msgs Transform.
+ *
+ * The quaternion is normalized. A null @p transform is encoded as an all-zero
+ * quaternion, which transformFromGeometryMsg() decodes back to null.
+ *
+ * @param[in]  transform the transform to convert
+ * @param[out] msg       the converted message
+ */
 void transformToGeometryMsg(const rtabmap::Transform & transform, geometry_msgs::msg::Transform & msg);
+
+/**
+ * @brief Convert a geometry_msgs Transform into a rtabmap::Transform.
+ * @param msg the message to convert
+ * @return the converted transform, or a null transform if the quaternion is all zeros
+ */
 rtabmap::Transform transformFromGeometryMsg(const geometry_msgs::msg::Transform & msg);
 
+/**
+ * @brief Convert a rtabmap::Transform into a geometry_msgs Pose.
+ * @param[in]  transform the transform to convert
+ * @param[out] msg       the converted message; a null @p transform gives an all-zero orientation
+ */
 void transformToPoseMsg(const rtabmap::Transform & transform, geometry_msgs::msg::Pose & msg);
+
+/**
+ * @brief Convert a geometry_msgs Pose into a rtabmap::Transform.
+ * @param msg                    the message to convert
+ * @param ignoreRotationIfNotSet if true, an all-zero orientation yields a
+ *                               translation-only transform instead of a null one
+ * @return the converted transform, or a null transform if the orientation is all zeros
+ *         and @p ignoreRotationIfNotSet is false
+ *
+ * @warning geometry_msgs::msg::Quaternion defaults to `w = 1`, not all zeros, so a
+ *          default-constructed Pose is a valid identity rotation rather than "unset".
+ */
 rtabmap::Transform transformFromPoseMsg(const geometry_msgs::msg::Pose & msg, bool ignoreRotationIfNotSet = false);
 
+
+//============================================================================
+// Images
+// Extracting OpenCV images from RGBDImage messages, and building them back.
+//============================================================================
+
+/**
+ * @brief Extract the RGB and depth images of an RGBDImage message, copying the pixels.
+ *
+ * Handles both the raw (`rgb`, `depth`) and compressed (`rgb_compressed`,
+ * `depth_compressed`) fields. Both output pointers are always valid; they hold an
+ * empty image when the corresponding field is not set.
+ *
+ * @param[in]  image the message to read
+ * @param[out] rgb   the RGB image
+ * @param[out] depth the depth image
+ * @see toCvShare() to avoid the copy
+ */
 void toCvCopy(const rtabmap_msgs::msg::RGBDImage & image, cv_bridge::CvImagePtr & rgb, cv_bridge::CvImagePtr & depth);
+
+/**
+ * @brief Extract the RGB and depth images of an RGBDImage message without copying.
+ *
+ * The returned images alias the message's buffers, so @p image must outlive them.
+ *
+ * @param[in]  image the message to read; its shared pointer keeps the buffers alive
+ * @param[out] rgb   the RGB image
+ * @param[out] depth the depth image
+ */
 void toCvShare(const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr & image, cv_bridge::CvImageConstPtr & rgb, cv_bridge::CvImageConstPtr & depth);
+
+/**
+ * @brief Extract the RGB and depth images of an RGBDImage message without copying.
+ * @param[in]  image         the message to read
+ * @param[in]  trackedObject object whose lifetime keeps the message buffers alive
+ * @param[out] rgb           the RGB image
+ * @param[out] depth         the depth image
+ */
 void toCvShare(const rtabmap_msgs::msg::RGBDImage & image, const std::shared_ptr<void const>& trackedObject, cv_bridge::CvImageConstPtr & rgb, cv_bridge::CvImageConstPtr & depth);
+
+/**
+ * @brief Fill an RGBDImage message from a SensorData.
+ *
+ * Supports a single RGB-D camera or a single stereo pair; multi-camera data cannot be
+ * represented by this message and is rejected with an error.
+ *
+ * @param[in]  data          the sensor data to convert
+ * @param[out] msg           the converted message, stamped with @p data's stamp
+ * @param[in]  sensorFrameId frame id stamped on the message and its sub-messages
+ *
+ * @note rtabmap::SensorData holds its stamp as a double, so the stamp written here is
+ *       only accurate to a few hundred nanoseconds at current epoch times and will not
+ *       compare equal to the ROS stamp the data originally came from. Callers that need
+ *       the exact original stamp assign `msg.header` after this call.
+ * @note Unlike infoToROS(), an already-stamped `msg.header` is overwritten rather than
+ *       kept: the same header is applied to every sub-message here, so preserving only
+ *       the top-level one would leave the message internally inconsistent.
+ */
 void rgbdImageToROS(const rtabmap::SensorData & data, rtabmap_msgs::msg::RGBDImage & msg, const std::string & sensorFrameId);
+
+/**
+ * @brief Build a SensorData from an RGBDImage message.
+ *
+ * The stamp is taken from the top-level `image->header`, and the camera's local
+ * transform is not carried by the message (callers resolve it from TF).
+ *
+ * @param image the message to convert
+ * @return the converted sensor data
+ *
+ * @warning The returned SensorData does **not** copy the pixels: it points into the
+ *          message's own buffers. @p image must therefore outlive it and must not be
+ *          modified meanwhile. Deep-copy the images before letting the SensorData
+ *          escape a subscription callback, because the queue recycles the message as
+ *          soon as the callback returns.
+ */
 rtabmap::SensorData rgbdImageFromROS(const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr & image);
 
-// copy data
+
+//============================================================================
+// Compressed data
+//============================================================================
+
+/**
+ * @brief Copy an already-compressed cv::Mat into a byte vector.
+ * @param[in]  compressed a 1xN CV_8UC1 matrix of compressed bytes, or an empty matrix
+ * @param[out] bytes      the bytes; cleared when @p compressed is empty
+ */
 void compressedMatToBytes(const cv::Mat & compressed, std::vector<unsigned char> & bytes);
+
+/**
+ * @brief Wrap a byte vector as a 1xN CV_8UC1 cv::Mat of compressed data.
+ * @param bytes the bytes to wrap
+ * @param copy  if false, the returned matrix aliases @p bytes, which must then outlive it
+ * @return the matrix, empty when @p bytes is empty
+ */
 cv::Mat compressedMatFromBytes(const std::vector<unsigned char> & bytes, bool copy = true);
 
+
+//============================================================================
+// Statistics
+//============================================================================
+
+/**
+ * @brief Read an Info message into RTAB-Map statistics.
+ * @param[in]  info the message to convert
+ * @param[out] stat the statistics, marked as extended
+ * @note The stamp comes from `info.header`, which infoToROS() does not set.
+ */
 void infoFromROS(const rtabmap_msgs::msg::Info & info, rtabmap::Statistics & stat);
+
+/**
+ * @brief Fill an Info message from RTAB-Map statistics.
+ * @param[in]  stats the statistics to convert
+ * @param[out] info  the converted message
+ * @note If the caller left `info.header.stamp` unset it is filled from @p stats, so that
+ *       infoFromROS() recovers a stamp. An already-stamped header is never overwritten:
+ *       rtabmap::Statistics holds its stamp as a double, so the value derived from it is
+ *       only accurate to a few hundred nanoseconds at current epoch times and will not
+ *       compare equal to the ROS stamp the data came from. Callers wanting the exact
+ *       input stamp — or a publication time unrelated to the data — stamp the header
+ *       themselves before or after this call.
+ * @warning The frame id is never set: rtabmap::Statistics does not carry one, so the
+ *          caller must always fill `info.header.frame_id` itself.
+ */
 void infoToROS(const rtabmap::Statistics & stats, rtabmap_msgs::msg::Info & info);
 
+
+//============================================================================
+// Features and landmarks
+// Keypoints, 2D/3D points, descriptors and environmental sensors.
+//============================================================================
+
+/** @brief Convert a Link message into a rtabmap::Link, including its 6x6 information matrix. */
 rtabmap::Link linkFromROS(const rtabmap_msgs::msg::Link & msg);
+/** @brief Fill a Link message from a rtabmap::Link. */
 void linkToROS(const rtabmap::Link & link, rtabmap_msgs::msg::Link & msg);
 
+/** @brief Convert a KeyPoint message into a cv::KeyPoint. */
 cv::KeyPoint keypointFromROS(const rtabmap_msgs::msg::KeyPoint & msg);
+/** @brief Fill a KeyPoint message from a cv::KeyPoint. */
 void keypointToROS(const cv::KeyPoint & kpt, rtabmap_msgs::msg::KeyPoint & msg);
 
+/** @brief Convert keypoint messages into a new vector of cv::KeyPoint. */
 std::vector<cv::KeyPoint> keypointsFromROS(const std::vector<rtabmap_msgs::msg::KeyPoint> & msg);
+
+/**
+ * @brief Append keypoint messages to an existing vector.
+ * @param[in]     msg    the messages to convert
+ * @param[in,out] kpts   vector the keypoints are appended to; existing content is kept
+ * @param[in]     xShift offset added to the x coordinate of every appended keypoint,
+ *                       used when several camera images are laid out side by side
+ */
 void keypointsFromROS(const std::vector<rtabmap_msgs::msg::KeyPoint> & msg, std::vector<cv::KeyPoint> & kpts, int xShift=0);
+
+/** @brief Fill keypoint messages from a vector of cv::KeyPoint. */
 void keypointsToROS(const std::vector<cv::KeyPoint> & kpts, std::vector<rtabmap_msgs::msg::KeyPoint> & msg);
 
+/** @brief Convert a GlobalDescriptor message, decompressing its data and info matrices. */
 rtabmap::GlobalDescriptor globalDescriptorFromROS(const rtabmap_msgs::msg::GlobalDescriptor & msg);
+/** @brief Fill a GlobalDescriptor message, compressing its data and info matrices. */
 void globalDescriptorToROS(const rtabmap::GlobalDescriptor & desc, rtabmap_msgs::msg::GlobalDescriptor & msg);
 
+/** @brief Convert global descriptor messages into RTAB-Map descriptors. */
 std::vector<rtabmap::GlobalDescriptor> globalDescriptorsFromROS(const std::vector<rtabmap_msgs::msg::GlobalDescriptor> & msg);
+/** @brief Fill global descriptor messages; @p msg is cleared first. */
 void globalDescriptorsToROS(const std::vector<rtabmap::GlobalDescriptor> & desc, std::vector<rtabmap_msgs::msg::GlobalDescriptor> & msg);
 
+/** @brief Convert an EnvSensor message into a rtabmap::EnvSensor. */
 rtabmap::EnvSensor envSensorFromROS(const rtabmap_msgs::msg::EnvSensor & msg);
+/** @brief Fill an EnvSensor message from a rtabmap::EnvSensor. */
 void envSensorToROS(const rtabmap::EnvSensor & sensor, rtabmap_msgs::msg::EnvSensor & msg);
+/** @brief Convert EnvSensor messages into a map keyed by sensor type. */
 rtabmap::EnvSensors envSensorsFromROS(const std::vector<rtabmap_msgs::msg::EnvSensor> & msg);
+/** @brief Fill EnvSensor messages from a map of sensors; @p msg is cleared first. */
 void envSensorsToROS(const rtabmap::EnvSensors & sensors, std::vector<rtabmap_msgs::msg::EnvSensor> & msg);
 
+/** @brief Convert a Point2f message into a cv::Point2f. */
 cv::Point2f point2fFromROS(const rtabmap_msgs::msg::Point2f & msg);
+/** @brief Fill a Point2f message from a cv::Point2f. */
 void point2fToROS(const cv::Point2f & kpt, rtabmap_msgs::msg::Point2f & msg);
 
+/** @brief Convert Point2f messages into a vector of cv::Point2f. */
 std::vector<cv::Point2f> points2fFromROS(const std::vector<rtabmap_msgs::msg::Point2f> & msg);
+/** @brief Fill Point2f messages from a vector of cv::Point2f. */
 void points2fToROS(const std::vector<cv::Point2f> & kpts, std::vector<rtabmap_msgs::msg::Point2f> & msg);
 
+/** @brief Convert a Point3f message into a cv::Point3f. */
 cv::Point3f point3fFromROS(const rtabmap_msgs::msg::Point3f & msg);
+/** @brief Fill a Point3f message from a cv::Point3f. */
 void point3fToROS(const cv::Point3f & kpt, rtabmap_msgs::msg::Point3f & msg);
 
+/**
+ * @brief Convert Point3f messages into a vector of cv::Point3f.
+ * @param msg       the messages to convert
+ * @param transform applied to every point; ignored when null or identity
+ * @return the converted points
+ */
 std::vector<cv::Point3f> points3fFromROS(const std::vector<rtabmap_msgs::msg::Point3f> & msg, const rtabmap::Transform & transform = rtabmap::Transform());
+
+/**
+ * @brief Append Point3f messages to an existing vector.
+ * @param[in]     msg       the messages to convert
+ * @param[in,out] points3   vector the points are appended to; existing content is kept
+ * @param[in]     transform applied to every appended point; ignored when null or identity
+ */
 void points3fFromROS(const std::vector<rtabmap_msgs::msg::Point3f> & msg, std::vector<cv::Point3f> & points3, const rtabmap::Transform & transform = rtabmap::Transform());
+
+/**
+ * @brief Fill Point3f messages from a vector of cv::Point3f.
+ * @param[in]  kpts      the points to convert
+ * @param[out] msg       the converted messages
+ * @param[in]  transform applied to every point; ignored when null or identity
+ */
 void points3fToROS(const std::vector<cv::Point3f> & kpts, std::vector<rtabmap_msgs::msg::Point3f> & msg, const rtabmap::Transform & transform = rtabmap::Transform());
 
+
+//============================================================================
+// Camera models
+//============================================================================
+
+/**
+ * @brief Convert a CameraInfo message into a rtabmap::CameraModel.
+ *
+ * Fisheye/equidistant distortion (4 coefficients) is repacked into RTAB-Map's 1x6
+ * layout. A projection matrix means the model describes an already-rectified image.
+ *
+ * @param camInfo        the message to convert
+ * @param localTransform transform from the base frame to the optical frame
+ * @return the converted model
+ *
+ * @note `k`, `r` and `p` are fixed-size arrays and so are never empty. An unset matrix
+ *       is all zeros, which is detected through the focal length (`k[0]` / `p[0]`).
+ */
 rtabmap::CameraModel cameraModelFromROS(
 		const sensor_msgs::msg::CameraInfo & camInfo,
 		const rtabmap::Transform & localTransform = rtabmap::Transform::getIdentity());
+
+/**
+ * @brief Fill a CameraInfo message from a rtabmap::CameraModel.
+ *
+ * A model carrying a projection matrix describes a rectified image, so zero distortion
+ * is reported for it. Without one, `P` is synthesized as `[K | 0]` and the raw
+ * distortion coefficients are emitted (`equidistant` for a 1x6 fisheye matrix,
+ * `rational_polynomial` above 5 coefficients, `plumb_bob` otherwise).
+ *
+ * @param[in]  model   the model to convert
+ * @param[out] camInfo the converted message; the header is not set
+ */
 void cameraModelToROS(
 		const rtabmap::CameraModel & model,
 		sensor_msgs::msg::CameraInfo & camInfo);
 
+/**
+ * @brief Build a stereo model from a pair of CameraInfo messages.
+ * @param leftCamInfo     left camera info
+ * @param rightCamInfo    right camera info; the baseline is read from its `P(0,3)`
+ * @param localTransform  transform from the base frame to the left optical frame
+ * @param stereoTransform explicit left-to-right transform, when not encoded in `P`
+ * @return the converted model
+ */
 rtabmap::StereoCameraModel stereoCameraModelFromROS(
 		const sensor_msgs::msg::CameraInfo & leftCamInfo,
 		const sensor_msgs::msg::CameraInfo & rightCamInfo,
 		const rtabmap::Transform & localTransform = rtabmap::Transform::getIdentity(),
 		const rtabmap::Transform & stereoTransform = rtabmap::Transform());
+
+/**
+ * @brief Build a stereo model, resolving the local transform from TF.
+ * @param leftCamInfo      left camera info
+ * @param rightCamInfo     right camera info
+ * @param frameId          base frame the model's local transform is expressed in
+ * @param tfBuffer         must contain @p frameId -> the left camera info's frame at
+ *                         its stamp
+ * @param waitForTransform seconds to wait for TF, 0 to not wait
+ * @return the converted model, invalid if the transform could not be resolved
+ */
 rtabmap::StereoCameraModel stereoCameraModelFromROS(
 		const sensor_msgs::msg::CameraInfo & leftCamInfo,
 		const sensor_msgs::msg::CameraInfo & rightCamInfo,
@@ -148,12 +436,34 @@ rtabmap::StereoCameraModel stereoCameraModelFromROS(
 		tf2_ros::Buffer & tfBuffer,
 		double waitForTransform);
 
+
+//============================================================================
+// Map graph
+// Poses, links, nodes and sensor data — the map serialization path.
+//============================================================================
+
+/**
+ * @brief Read a MapData message into poses, links and signatures.
+ * @param[in]  msg        the message to convert
+ * @param[out] poses      optimized poses by node id
+ * @param[out] links      constraints, keyed by their originating node id
+ * @param[out] signatures node data by node id
+ * @param[out] mapToOdom  transform from the map frame to the odometry frame
+ */
 void mapDataFromROS(
 		const rtabmap_msgs::msg::MapData & msg,
 		std::map<int, rtabmap::Transform> & poses,
 		std::multimap<int, rtabmap::Link> & links,
 		std::map<int, rtabmap::Signature> & signatures,
 		rtabmap::Transform & mapToOdom);
+/**
+ * @brief Fill a MapData message from poses, links and signatures.
+ * @param[in]  poses      optimized poses by node id
+ * @param[in]  links      constraints
+ * @param[in]  signatures node data by node id
+ * @param[in]  mapToOdom  transform from the map frame to the odometry frame
+ * @param[out] msg        the converted message; the header is not set
+ */
 void mapDataToROS(
 		const std::map<int, rtabmap::Transform> & poses,
 		const std::multimap<int, rtabmap::Link> & links,
@@ -161,40 +471,159 @@ void mapDataToROS(
 		const rtabmap::Transform & mapToOdom,
 		rtabmap_msgs::msg::MapData & msg);
 
+/**
+ * @brief Read a MapGraph message into poses and links.
+ * @param[in]  msg       the message to convert
+ * @param[out] poses     optimized poses by node id
+ * @param[out] links     constraints, keyed by their originating node id
+ * @param[out] mapToOdom transform from the map frame to the odometry frame
+ */
 void mapGraphFromROS(
 		const rtabmap_msgs::msg::MapGraph & msg,
 		std::map<int, rtabmap::Transform> & poses,
 		std::multimap<int, rtabmap::Link> & links,
 		rtabmap::Transform & mapToOdom);
+/**
+ * @brief Fill a MapGraph message from poses and links.
+ * @param[in]  poses     optimized poses by node id
+ * @param[in]  links     constraints
+ * @param[in]  mapToOdom transform from the map frame to the odometry frame
+ * @param[out] msg       the converted message; the header is not set
+ */
 void mapGraphToROS(
 		const std::map<int, rtabmap::Transform> & poses,
 		const std::multimap<int, rtabmap::Link> & links,
 		const rtabmap::Transform & mapToOdom,
 		rtabmap_msgs::msg::MapGraph & msg);
 
+/**
+ * @brief Convert a SensorData message into a rtabmap::SensorData.
+ * @param msg the message to convert
+ * @return the converted sensor data
+ * @note `ground_truth_pose` is not read here; nodeFromROS() owns that field.
+ */
 rtabmap::SensorData sensorDataFromROS(const rtabmap_msgs::msg::SensorData & msg);
+
+/**
+ * @brief Fill a SensorData message from a rtabmap::SensorData.
+ * @param[in]  signature   the sensor data to convert
+ * @param[out] msg         the converted message
+ * @param[in]  frameId     frame id stamped on the message
+ * @param[in]  copyRawData also serialize the uncompressed images and laser scan, which
+ *                         is significantly larger on the wire
+ */
 void sensorDataToROS(const rtabmap::SensorData & signature, rtabmap_msgs::msg::SensorData & msg, const std::string & frameId = "base_link", bool copyRawData = false);
 
+/**
+ * @brief Convert a Node message into a rtabmap::Signature, with its data and visual words.
+ * @param msg the message to convert
+ * @return the converted signature
+ */
 rtabmap::Signature nodeFromROS(const rtabmap_msgs::msg::Node & msg);
+
+/**
+ * @brief Fill a Node message from a rtabmap::Signature.
+ * @param[in]  signature the signature to convert
+ * @param[out] msg       the converted message
+ */
 void nodeToROS(const rtabmap::Signature & signature, rtabmap_msgs::msg::Node & msg);
 
-// DEPRECATED
+/** @deprecated Use nodeFromROS() instead. */
 rtabmap::Signature nodeDataFromROS(const rtabmap_msgs::msg::Node & msg);
+/** @deprecated Use nodeToROS() instead. */
 void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_msgs::msg::Node & msg);
 
+/** @brief Convert only the node's metadata (id, map id, weight, stamp, label, pose). */
 rtabmap::Signature nodeInfoFromROS(const rtabmap_msgs::msg::Node & msg);
+/** @brief Fill only the node's metadata (id, map id, weight, stamp, label, pose). */
 void nodeInfoToROS(const rtabmap::Signature & signature, rtabmap_msgs::msg::Node & msg);
 
+
+//============================================================================
+// Odometry
+//============================================================================
+
+/**
+ * @brief Format odometry info as the `Odometry/...` statistics published with the map.
+ * @param info the odometry info to summarize
+ * @return statistic name (with its unit) to value
+ * @note The covariance-derived entries are omitted when `info.reg.covariance` is not a
+ *       6x6 CV_64FC1 matrix, which is the case for a default-constructed OdometryInfo.
+ */
 std::map<std::string, float> odomInfoToStatistics(const rtabmap::OdometryInfo & info);
+
+/**
+ * @brief Convert an OdomInfo message into a rtabmap::OdometryInfo.
+ * @param msg        the message to convert
+ * @param ignoreData skip the heavy members (words, local map, correspondences)
+ * @return the converted odometry info
+ */
 rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_msgs::msg::OdomInfo & msg, bool ignoreData = false);
+
+/**
+ * @brief Fill an OdomInfo message from a rtabmap::OdometryInfo.
+ * @param[in]  info       the odometry info to convert
+ * @param[out] msg        the converted message
+ * @param[in]  ignoreData skip the heavy members (words, local map, correspondences)
+ */
 void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_msgs::msg::OdomInfo & msg, bool ignoreData = false);
 
+
+//============================================================================
+// User data, IMU and landmarks
+//============================================================================
+
+/**
+ * @brief Extract the payload of a UserData message.
+ * @param dataMsg the message to read
+ * @return the payload; still compressed when the message was written with compression,
+ *         in which case the caller applies rtabmap::uncompressData()
+ */
 cv::Mat userDataFromROS(const rtabmap_msgs::msg::UserData & dataMsg);
+
+/**
+ * @brief Fill a UserData message.
+ * @param[in]  data     the payload
+ * @param[out] dataMsg  the converted message
+ * @param[in]  compress compress the payload, which is then carried as a 1xN byte blob
+ */
 void userDataToROS(const cv::Mat & data, rtabmap_msgs::msg::UserData & dataMsg, bool compress);
 
+/**
+ * @brief Convert an Imu message into a rtabmap::IMU.
+ * @param msg            the message to convert
+ * @param localTransform transform from the base frame to the IMU frame
+ * @return the converted IMU sample, with its three covariance matrices
+ */
 rtabmap::IMU imuFromROS(const sensor_msgs::msg::Imu & msg, const rtabmap::Transform & localTransform = rtabmap::Transform::getIdentity());
+
+/**
+ * @brief Fill an Imu message from a rtabmap::IMU.
+ * @param[in]  imu the IMU sample to convert
+ * @param[out] msg the converted message; the header is not set
+ */
 void imuToROS(const rtabmap::IMU & imu, sensor_msgs::msg::Imu & msg);
 
+/**
+ * @brief Convert tag/landmark detections into RTAB-Map landmarks, expressed in @p frameId.
+ *
+ * Each detection is transformed from its own frame into @p frameId, then corrected for
+ * the odometry motion between @p odomStamp and the detection's stamp.
+ *
+ * @param tags               detections by landmark id, each paired with its tag size;
+ *                           ids must be > 0, others are dropped with an error
+ * @param frameId            base frame the landmarks are expressed in
+ * @param odomFrameId        fixed frame used for the odometry correction; when empty
+ *                           no correction is applied
+ * @param odomStamp          stamp the landmarks should be synchronized to
+ * @param tfBuffer           must contain @p frameId -> each detection's frame at that
+ *                           detection's stamp, and, when @p odomFrameId is set,
+ *                           @p odomFrameId -> @p frameId covering both stamps
+ * @param waitForTransform   seconds to wait for TF, 0 to not wait
+ * @param defaultLinVariance linear variance used when a detection carries no covariance
+ * @param defaultAngVariance angular variance used when a detection carries no covariance
+ * @return the landmarks, keyed by id
+ */
 rtabmap::Landmarks landmarksFromROS(
 		const std::map<int, std::pair<geometry_msgs::msg::PoseWithCovarianceStamped, float> > & tags,
 		const std::string & frameId,
@@ -205,10 +634,43 @@ rtabmap::Landmarks landmarksFromROS(
 		double defaultLinVariance,
 		double defaultAngVariance);
 
-inline double timestampFromROS(const rclcpp::Time & stamp) {return stamp.seconds();}
-inline rclcpp::Time timestampToROS(const double & t) {int32_t sec= (int32_t)floor(t); return rclcpp::Time(sec, (uint32_t)std::round((t-sec) * 1e9));}
 
-// common stuff
+//============================================================================
+// Timestamps
+//============================================================================
+
+/**
+ * @brief Convert a ROS time into seconds.
+ * @note A double holds about 15-16 significant digits, so at current epoch times
+ *       (~1.7e9 s) it resolves to roughly 400 ns. Converting back with timestampToROS()
+ *       therefore does not reproduce the original stamp exactly, and the rounding can
+ *       carry into the seconds field. Compare converted stamps with a tolerance, and
+ *       keep the original rclcpp::Time whenever exactness matters.
+ */
+inline double timestampFromROS(const rclcpp::Time & stamp) {return stamp.seconds();}
+/**
+ * @brief Convert seconds into a ROS time.
+ * @note The result uses RCL_ROS_TIME, matching how message header stamps convert. The
+ *       rclcpp::Time(sec, nsec) constructor defaults to RCL_SYSTEM_TIME instead, and
+ *       comparing times of different clock types throws.
+ */
+inline rclcpp::Time timestampToROS(const double & t) {int32_t sec= (int32_t)floor(t); return rclcpp::Time(sec, (uint32_t)std::round((t-sec) * 1e9), RCL_ROS_TIME);}
+
+
+//============================================================================
+// TF lookups
+//============================================================================
+
+/**
+ * @brief Look a static relationship between two frames up in TF.
+ * @param fromFrameId      the reference frame
+ * @param toFrameId        the target frame
+ * @param stamp            time of the lookup
+ * @param tfBuffer         buffer to query
+ * @param waitForTransform seconds to wait for TF, 0 to not wait
+ * @return the transform, or a null transform if the lookup failed (which is logged
+ *         rather than thrown)
+ */
 rtabmap::Transform getTransform(
 		const std::string & fromFrameId,
 		const std::string & toFrameId,
@@ -216,9 +678,19 @@ rtabmap::Transform getTransform(
 		tf2_ros::Buffer & tfBuffer,
 		double waitForTransform);
 
-
-// get moving transform accordingly to a fixed frame. For example get
-// transform of /base_link between two stamps accordingly to /odom frame.
+/**
+ * @brief Measure how a frame moved between two stamps, relative to a fixed frame.
+ *
+ * For example, the motion of `base_link` between two stamps as seen from `odom`.
+ *
+ * @param movingFrame      the frame whose motion is measured
+ * @param fixedFrame       the frame the motion is measured against
+ * @param stampFrom        start of the interval
+ * @param stampTo          end of the interval
+ * @param tfBuffer         buffer to query
+ * @param waitForTransform seconds to wait for TF, 0 to not wait
+ * @return the motion, or a null transform if the lookup failed
+ */
 rtabmap::Transform getMovingTransform(
 		const std::string & movingFrame,
 		const std::string & fixedFrame,
@@ -227,6 +699,54 @@ rtabmap::Transform getMovingTransform(
 		tf2_ros::Buffer & tfBuffer,
 		double waitForTransform);
 
+
+//============================================================================
+// Sensor message conversion
+// Assembling RGB-D, stereo and laser scan messages into RTAB-Map inputs.
+//============================================================================
+
+/**
+ * @brief Assemble one or more RGB-D (or RGB + right) camera streams into RTAB-Map inputs.
+ *
+ * With several cameras the images are concatenated horizontally into a single wide
+ * image and one model is produced per camera. Whether the second image is treated as a
+ * depth map or as the right image of a stereo pair is inferred from its encoding, and
+ * for `mono16` from whether the camera infos carry a baseline in `P(0,3)`.
+ *
+ * @param imageMsgs            RGB (or left) images, one per camera; may be empty
+ * @param depthMsgs            depth (or right) images, one per camera; may be empty
+ * @param cameraInfoMsgs       camera infos, one per camera; must not be empty
+ * @param depthCameraInfoMsgs  camera infos of the depth/right cameras; may be empty
+ * @param frameId              base frame the local transforms are expressed in
+ * @param odomFrameId          fixed frame the robot motion is measured against, used to
+ *                             re-express each camera pose relative to the base frame at
+ *                             @p odomStamp; empty to skip that correction entirely
+ * @param odomStamp            stamp the data is synchronized to
+ * @param[out] rgb             the assembled RGB (or left) image
+ * @param[out] depth           the assembled depth (or right) image
+ * @param[out] cameraModels    one model per camera, when the input is RGB-D
+ * @param[out] stereoCameraModels one model per camera, when the input is stereo
+ * @param tfBuffer             must contain @p frameId -> each camera's optical frame at
+ *                             that camera's stamp, and, when @p odomFrameId is set,
+ *                             @p odomFrameId -> @p frameId covering both @p odomStamp
+ *                             and the camera stamps
+ * @param waitForTransform     seconds to wait for TF, 0 to not wait
+ * @param alreadRectifiedImages whether the images are already rectified
+ * @param localKeyPointsMsgs   optional per-camera keypoints to merge
+ * @param localPoints3dMsgs    optional per-camera 3D points to merge
+ * @param localDescriptorsMsgs optional per-camera descriptors to merge
+ * @param[out] localKeyPoints  merged keypoints, shifted to the concatenated image
+ * @param[out] localPoints3d   merged 3D points
+ * @param[out] localDescriptors merged descriptors
+ * @return false on an unsupported encoding or a missing camera local transform
+ *
+ * @note The odometry correction is applied per camera, using each camera's own stamp,
+ *       and only when it differs from @p odomStamp. If that lookup fails the function
+ *       warns and carries on with an uncorrected pose — unlike a missing camera local
+ *       transform, which is fatal and returns false.
+ * @note A camera's RGB and depth stamps are assumed to be equal. Should they differ,
+ *       the depth stamp is the one used, since the geometry is what gets synchronized.
+ */
 bool convertRGBDMsgs(
 		const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
 		const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
@@ -249,6 +769,34 @@ bool convertRGBDMsgs(
 		std::vector<cv::Point3f> * localPoints3d = 0,
 		cv::Mat * localDescriptors = 0);
 
+/**
+ * @brief Convert a stereo pair into RTAB-Map inputs.
+ *
+ * The left image keeps its colour; the right image is always reduced to mono.
+ *
+ * @param leftImageMsg     left image
+ * @param rightImageMsg    right image
+ * @param leftCamInfoMsg   left camera info
+ * @param rightCamInfoMsg  right camera info; the baseline is read from its `P(0,3)`
+ * @param frameId          base frame the local transform is expressed in
+ * @param odomFrameId      fixed frame the robot motion is measured against, used to
+ *                         re-express the camera pose relative to the base frame at
+ *                         @p odomStamp; empty to skip that correction entirely
+ * @param odomStamp        stamp the data is synchronized to
+ * @param[out] left        the left image
+ * @param[out] right       the right image, as mono
+ * @param[out] stereoModel the stereo model
+ * @param tfBuffer         must contain @p frameId -> the left image's frame at the left
+ *                         image stamp, and, when @p odomFrameId is set,
+ *                         @p odomFrameId -> @p frameId covering both stamps
+ * @param waitForTransform seconds to wait for TF, 0 to not wait
+ * @param alreadyRectified whether the images are already rectified
+ * @return false on an unsupported encoding or a missing local transform
+ *
+ * @note The odometry correction is applied only when the left image stamp differs from
+ *       @p odomStamp. A failed correction lookup warns and leaves the pose uncorrected;
+ *       a missing local transform is fatal and returns false.
+ */
 bool convertStereoMsg(
 		const cv_bridge::CvImageConstPtr& leftImageMsg,
 		const cv_bridge::CvImageConstPtr& rightImageMsg,
@@ -264,6 +812,34 @@ bool convertStereoMsg(
 		double waitForTransform,
 		bool alreadyRectified);
 
+/**
+ * @brief Convert a 2D LaserScan into a rtabmap::LaserScan.
+ * @param scan2dMsg        the scan to convert
+ * @param frameId          base frame the scan's local transform is expressed in
+ * @param odomFrameId      fixed frame the robot motion is measured against, used to
+ *                         re-express the scan pose relative to the base frame at
+ *                         @p odomStamp; empty to skip that correction entirely
+ * @param odomStamp        stamp the scan is synchronized to
+ * @param[out] scan        the converted scan
+ * @param tfBuffer         must contain @p frameId -> the laser frame at the scan stamp,
+ *                         and the laser frame relative to @p odomFrameId (or @p frameId
+ *                         when that is empty) across the whole sweep, since the points
+ *                         are projected through it
+ * @param waitForTransform seconds to wait for TF, 0 to not wait
+ * @param outputInFrameId  express the points in @p frameId rather than the laser frame
+ * @return false if the scan is malformed (zero angle increment, inverted range or angle
+ *         bounds) or if a required transform is missing
+ *
+ * @note Unlike convertScan3dMsg(), this deskews the scan itself: the points are
+ *       projected with laser_geometry, which transforms each ray at its own time using
+ *       @p scan2dMsg.time_increment. That only corrects for motion if the projection
+ *       target is a fixed frame, i.e. if @p odomFrameId is set — with it empty the
+ *       target is @p frameId, which does not move relative to itself. This is also why
+ *       the laser frame must be known across the whole sweep, which the function checks
+ *       up front.
+ * @note The odometry correction is applied only when the scan stamp differs from
+ *       @p odomStamp; a failed correction lookup warns and leaves the pose uncorrected.
+ */
 bool convertScanMsg(
 		const sensor_msgs::msg::LaserScan & scan2dMsg,
 		const std::string & frameId,
@@ -274,6 +850,32 @@ bool convertScanMsg(
 		double waitForTransform,
 		bool outputInFrameId = false);
 
+/**
+ * @brief Convert a PointCloud2 into a rtabmap::LaserScan.
+ * @param scan3dMsg        the cloud to convert
+ * @param frameId          base frame the scan's local transform is expressed in
+ * @param odomFrameId      fixed frame the robot motion is measured against, used to
+ *                         re-express the scan pose relative to the base frame at
+ *                         @p odomStamp; empty to skip that correction entirely
+ * @param odomStamp        stamp the scan is synchronized to
+ * @param[out] scan        the converted scan
+ * @param tfBuffer         must contain @p frameId -> the cloud's frame at the cloud
+ *                         stamp, and, when @p odomFrameId is set, @p odomFrameId ->
+ *                         @p frameId covering both stamps
+ * @param waitForTransform seconds to wait for TF, 0 to not wait
+ * @param maxPoints        downsample to at most this many points, 0 for no limit
+ * @param maxRange         drop points beyond this range, 0 for no limit
+ * @param is2D             treat the cloud as planar
+ * @return false if the local transform could not be resolved
+ *
+ * @note The cloud is assumed to be already deskewed. A single rigid transform is applied
+ *       to the whole cloud, so any motion during the sweep is preserved as-is; call
+ *       deskew() on the message first if the sensor was moving. This is unlike
+ *       convertScanMsg(), which deskews 2D scans itself through laser_geometry.
+ * @note The odometry correction is applied only when the cloud stamp differs from
+ *       @p odomStamp; a failed correction lookup warns and leaves the pose uncorrected.
+ * @see deskew()
+ */
 bool convertScan3dMsg(
 		const sensor_msgs::msg::PointCloud2 & scan3dMsg,
 		const std::string & frameId,
@@ -286,6 +888,29 @@ bool convertScan3dMsg(
 		float maxRange = 0.0f,
 		bool is2D = false);
 
+
+//============================================================================
+// Point cloud utilities
+//============================================================================
+
+/**
+ * @brief Deskew a point cloud using TF.
+ *
+ * Corrects each point for the sensor motion during the sweep, using the per-point time
+ * channel (`t`, `time`, `stamps` or `timestamp`). See the other overload for how that
+ * channel is interpreted.
+ *
+ * @param input            the cloud to deskew
+ * @param[out] output      the deskewed cloud, expressed in the frame at input's header stamp
+ * @param fixedFrameId     frame the sensor motion is measured against
+ * @param tfBuffer         must contain the cloud's own frame relative to
+ *                         @p fixedFrameId across the whole sweep
+ * @param waitForTransform seconds to wait for TF, 0 to not wait
+ * @param slerp            interpolate between the sweep's two end poses instead of
+ *                         looking TF up for every point; one query instead of N, at the
+ *                         cost of linearizing the motion across the sweep
+ * @return false if the cloud has no usable time channel or a lookup failed
+ */
 bool deskew(
 		const sensor_msgs::msg::PointCloud2 & input,
 		sensor_msgs::msg::PointCloud2 & output,
@@ -294,21 +919,49 @@ bool deskew(
 		double waitForTransform,
 		bool slerp = false);
 
+/**
+ * @brief Deskew a point cloud using a constant velocity model.
+ *
+ * The per-point time channel may be named `t`, `time`, `stamps` or `timestamp`. Its
+ * datatype decides how it is read: `UINT32` (nanoseconds) and `FLOAT32` (seconds) are
+ * *offsets from the message header stamp*, while `FLOAT64` carries *absolute* stamps,
+ * with milliseconds/microseconds/nanoseconds detected automatically by magnitude.
+ *
+ * On success the channel is zeroed to mark the cloud as deskewed, so calling this again
+ * on the same cloud is a no-op that returns true rather than an error.
+ *
+ * @param input        cloud with a per-point time channel
+ * @param[out] output  deskewed cloud, expressed in the frame at input's header stamp
+ * @param velocity     twist of the sensor frame (m/s and rad/s)
+ * @return false if the cloud has no usable time channel or @p velocity is null
+ */
 bool deskew(
 		const sensor_msgs::msg::PointCloud2 & input,
 		sensor_msgs::msg::PointCloud2 & output,
-		double previousStamp,
 		const rtabmap::Transform & velocity);
 
-// Missing function in ros2 (from old pcl_ros)
+/**
+ * @brief Apply a rigid transform to the XYZ fields of a point cloud.
+ *
+ * Missing function in ROS 2, taken from the old pcl_ros.
+ *
+ * @param transform the transform to apply
+ * @param in        the cloud to transform
+ * @param[out] out  the transformed cloud; all other fields are copied unchanged
+ */
 void transformPointCloud (
 		const Eigen::Matrix4f &transform,
 		const sensor_msgs::msg::PointCloud2 &in,
         sensor_msgs::msg::PointCloud2 &out);
 
-/** Return the size of a datatype (which is an enum of sensor_msgs::PointField::) in bytes
- * @param datatype one of the enums of sensor_msgs::PointField::
- * Note: Missing function in ros2 (from old pcl_ros)
+/**
+ * @brief Return the size in bytes of a PointField datatype.
+ *
+ * Missing function in ROS 2, taken from the old pcl_ros.
+ *
+ * @param datatype one of the sensor_msgs::msg::PointField enums
+ * @return the size in bytes
+ * @throws std::runtime_error if @p datatype is not a known PointField type
  */
 inline int sizeOfPointField(int datatype)
 {
@@ -330,6 +983,13 @@ inline int sizeOfPointField(int datatype)
   return -1;
 }
 
+/**
+ * @brief Find the entry of a map whose key is closest to @p key.
+ * @param buffer the map to search; must not be empty
+ * @param key    the key to look for
+ * @return iterator to the closest entry, clamped to the first or last one when @p key
+ *         falls outside the map's range
+ */
 template <typename K, typename V>
 typename std::map<K, V>::const_iterator getClosestIterator(
 	const std::map<K, V> & buffer,
@@ -365,6 +1025,7 @@ typename std::map<K, V>::const_iterator getClosestIterator(
 	return iterB;
 }
 
+
 }
 
 #endif /* MSGCONVERSION_H_ */

rtabmap_conversions/package.xml

@@ -30,7 +30,10 @@
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>
 
+  <test_depend>ament_cmake_gtest</test_depend>
+
   <export>
 	<build_type>ament_cmake</build_type>
+	<rosdoc2>rosdoc2.yaml</rosdoc2>
   </export>
 </package>

rtabmap_conversions/rosdoc2.yaml

@@ -0,0 +1,35 @@
+## Configuration for rosdoc2, the documentation generator used by docs.ros.org.
+## Regenerate the annotated default with:
+##   rosdoc2 default_config --package-path rtabmap_conversions
+## Build the docs locally with:
+##   rosdoc2 build --package-path rtabmap_conversions --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_cmake package, so doxygen runs on the public headers by
+    ## default and there are no Python modules to document.
+    always_run_doxygen: false
+    always_run_sphinx_apidoc: false
+
+builders:
+    ## Doxygen parses the public C++ API out of include/.
+    - doxygen: {
+        name: 'rtabmap_conversions Public C/C++ API',
+        output_dir: 'generated/doxygen'
+    }
+    ## Sphinx renders the landing page and pulls the Doxygen XML in through
+    ## breathe/exhale so the API is browsable alongside the narrative docs.
+    - sphinx: {
+        name: 'rtabmap_conversions',
+        doxygen_xml_directory: 'generated/doxygen/xml',
+        output_dir: ''
+    }

rtabmap_conversions/src/MsgConversion.cpp

@@ -27,6 +27,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap_conversions/MsgConversion.h"
 
+#include <cmath>
+#include <limits>
+
 #include <opencv2/highgui/highgui.hpp>
 #include <zlib.h>
 #include "rclcpp/rclcpp.hpp"
@@ -60,21 +63,46 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap_conversions {
 
-void transformToTF(const rtabmap::Transform & transform, tf2::Transform & tfTransform)
+bool transformToTF(const rtabmap::Transform & transform, tf2::Transform & tfTransform)
 {
-	if(!transform.isNull())
+	if(transform.isNull())
 	{
-		geometry_msgs::msg::TransformStamped gm = tf2::eigenToTransform(transform.toEigen3d());
-		//tf2::fromMsg(gm, tfTransform);
-	}
-	else
-	{
-		tfTransform = tf2::Transform(tf2::Quaternion(0,0,0,0));
+		// tf2::Transform cannot represent a null transform: it stores its rotation as a
+		// basis matrix, so there is no equivalent of the all-zero quaternion used by the
+		// geometry_msgs conversions. Fill it with NaN so that a caller ignoring the
+		// return value corrupts its results loudly instead of silently carrying on with
+		// an identity that looks legitimate.
+		const tf2Scalar nan = std::numeric_limits<tf2Scalar>::quiet_NaN();
+		tfTransform = tf2::Transform(
+				tf2::Matrix3x3(nan, nan, nan, nan, nan, nan, nan, nan, nan),
+				tf2::Vector3(nan, nan, nan));
+		return false;
 	}
+
+	geometry_msgs::msg::Transform msg;
+	transformToGeometryMsg(transform, msg);
+	tf2::fromMsg(msg, tfTransform);
+	return true;
 }
 
 rtabmap::Transform transformFromTF(const tf2::Transform & transform)
 {
+	// transformToTF() poisons its output with NaN for a null transform, as tf2::Transform
+	// has no null representation of its own. Map that back to a null transform here so the
+	// two functions round-trip, and so a NaN coming from anywhere else does not silently
+	// propagate into the rest of the pipeline.
+	const tf2::Vector3 & origin = transform.getOrigin();
+	const tf2::Matrix3x3 & basis = transform.getBasis();
+	bool nan = std::isnan(origin.x()) || std::isnan(origin.y()) || std::isnan(origin.z());
+	for(int i=0; !nan && i<3; ++i)
+	{
+		nan = std::isnan(basis[i].x()) || std::isnan(basis[i].y()) || std::isnan(basis[i].z());
+	}
+	if(nan)
+	{
+		return rtabmap::Transform();
+	}
+
 	Eigen::Isometry3d eigenTf;
 	geometry_msgs::msg::Transform gm = tf2::toMsg(transform);
 	eigenTf = tf2::transformToEigen(gm);
@@ -244,6 +272,7 @@ void rgbdImageToROS(const rtabmap::SensorData & data, rtabmap_msgs::msg::RGBDIma
 		UERROR("Cannot convert multi-camera data to rgbd image");
 		return;
 	}
+	msg.header = header;
 	if(data.cameraModels().size() == 1)
 	{
 		//rgb+depth
@@ -558,6 +587,13 @@ void infoFromROS(const rtabmap_msgs::msg::Info & info, rtabmap::Statistics & sta
 
 void infoToROS(const rtabmap::Statistics & stats, rtabmap_msgs::msg::Info & info)
 {
+	// Fall back to the statistics' own stamp when the caller left the header unstamped.
+	// Callers that already stamped it keep their value, which may be a publication time
+	// unrelated to the data, or the exact input stamp rather than this double-derived one.
+	if(info.header.stamp.sec == 0 && info.header.stamp.nanosec == 0)
+	{
+		info.header.stamp = timestampToROS(stats.stamp());
+	}
 	info.ref_id = stats.refImageId();
 	info.loop_closure_id = stats.loopClosureId();
 	info.proximity_detection_id = stats.proximityDetectionId();
@@ -829,9 +865,12 @@ rtabmap::CameraModel cameraModelFromROS(
 		const sensor_msgs::msg::CameraInfo & camInfo,
 		const rtabmap::Transform & localTransform)
 {
+	// Note: k, r and p are fixed-size arrays in the ROS message, so they are never
+	// empty and their size is always right. An unset matrix is signalled by all-zero
+	// content instead: k[0] and p[0] hold the focal length, which is always non-zero
+	// for a valid calibration, and an unset rectification matrix is all zeros.
 	cv:: Mat K;
-	UASSERT(camInfo.k.empty() || camInfo.k.size() == 9);
-	if(!camInfo.k.empty())
+	if(camInfo.k[0] != 0.0)
 	{
 		K = cv::Mat(3, 3, CV_64FC1);
 		memcpy(K.data, camInfo.k.data(), 9*sizeof(double));
@@ -858,17 +897,22 @@ rtabmap::CameraModel cameraModelFromROS(
 		}
 	}
 
+	// R is a rotation matrix, so any of its elements can legitimately be zero: only
+	// an entirely zero matrix means "not set".
 	cv:: Mat R;
-	UASSERT(camInfo.r.empty() || camInfo.r.size() == 9);
-	if(!camInfo.r.empty())
+	bool rIsSet = false;
+	for(size_t i=0; !rIsSet && i<camInfo.r.size(); ++i)
+	{
+		rIsSet = camInfo.r[i] != 0.0;
+	}
+	if(rIsSet)
 	{
 		R = cv::Mat(3, 3, CV_64FC1);
 		memcpy(R.data, camInfo.r.data(), 9*sizeof(double));
 	}
 
 	cv:: Mat P;
-	UASSERT(camInfo.p.empty() || camInfo.p.size() == 12);
-	if(!camInfo.p.empty())
+	if(camInfo.p[0] != 0.0)
 	{
 		P = cv::Mat(3, 4, CV_64FC1);
 		memcpy(P.data, camInfo.p.data(), 12*sizeof(double));
@@ -942,8 +986,9 @@ void cameraModelToROS(
 	{
 		memset(camInfo.p.data(), 0.0, 12*sizeof(double));
 		if(!model.K_raw().empty()) {
+			// P = [K | 0]: copying K already sets the homogeneous P(2,2)=1, and the
+			// fourth column (the Tx/Ty/Tz translation) stays zero for a single camera.
 			model.K_raw().copyTo(cv::Mat(3,4,CV_64FC1, camInfo.p.data()).colRange(0,3));
-			camInfo.p.back() = 1.0;
 		}
 	}
 	else
@@ -1352,10 +1397,13 @@ void sensorDataToROS(const rtabmap::SensorData & data, rtabmap_msgs::msg::Sensor
 	{
 		pcl::PCLPointCloud2::Ptr cloud = rtabmap::util3d::laserScanToPointCloud2(data.laserScanRaw());
 		pcl_conversions::moveFromPCL(*cloud, msg.laser_scan);
-		msg.laser_scan_max_pts = data.laserScanCompressed().maxPoints();
-		msg.laser_scan_max_range = data.laserScanCompressed().rangeMax();
-		msg.laser_scan_format = data.laserScanCompressed().format();
-		transformToGeometryMsg(data.laserScanCompressed().localTransform(), msg.laser_scan_local_transform);
+		// Describe the scan we just serialized: reading these from laserScanCompressed()
+		// zeroes them whenever only the raw scan is set, and sensorDataFromROS() then
+		// fails its format assertion.
+		msg.laser_scan_max_pts = data.laserScanRaw().maxPoints();
+		msg.laser_scan_max_range = data.laserScanRaw().rangeMax();
+		msg.laser_scan_format = data.laserScanRaw().format();
+		transformToGeometryMsg(data.laserScanRaw().localTransform(), msg.laser_scan_local_transform);
 	}
 	if(!data.laserScanCompressed().empty())
 	{
@@ -1624,10 +1672,17 @@ std::map<std::string, float> odomInfoToStatistics(const rtabmap::OdometryInfo &
 	stats.insert(std::make_pair("Odometry/ICPStructuralComplexity/", info.reg.icpStructuralComplexity));
 	stats.insert(std::make_pair("Odometry/ICPStructuralDistribution/", info.reg.icpStructuralDistribution));
 	stats.insert(std::make_pair("Odometry/ICPCorrespondences/", info.reg.icpCorrespondences));
-	stats.insert(std::make_pair("Odometry/StdDevLin/", sqrt((float)info.reg.covariance.at<double>(0,0))));
-	stats.insert(std::make_pair("Odometry/StdDevAng/", sqrt((float)info.reg.covariance.at<double>(5,5))));
-	stats.insert(std::make_pair("Odometry/VarianceLin/", (float)info.reg.covariance.at<double>(0,0)));
-	stats.insert(std::make_pair("Odometry/VarianceAng/", (float)info.reg.covariance.at<double>(5,5)));
+	// RegistrationInfo leaves covariance empty by default, so only read it when the
+	// expected 6x6 matrix is actually there.
+	if(info.reg.covariance.type() == CV_64FC1 &&
+	   info.reg.covariance.rows == 6 &&
+	   info.reg.covariance.cols == 6)
+	{
+		stats.insert(std::make_pair("Odometry/StdDevLin/", sqrt((float)info.reg.covariance.at<double>(0,0))));
+		stats.insert(std::make_pair("Odometry/StdDevAng/", sqrt((float)info.reg.covariance.at<double>(5,5))));
+		stats.insert(std::make_pair("Odometry/VarianceLin/", (float)info.reg.covariance.at<double>(0,0)));
+		stats.insert(std::make_pair("Odometry/VarianceAng/", (float)info.reg.covariance.at<double>(5,5)));
+	}
 	stats.insert(std::make_pair("Odometry/TimeEstimation/ms", info.timeEstimation*1000.0f));
 	stats.insert(std::make_pair("Odometry/TimeFiltering/ms", info.timeParticleFiltering*1000.0f));
 	stats.insert(std::make_pair("Odometry/LocalMapSize/", info.localMapSize));
@@ -2832,8 +2887,7 @@ bool deskew_impl(
 		tf2_ros::Buffer * tfBuffer,
 		double waitForTransform,
 		bool slerp,
-		const rtabmap::Transform & velocity,
-		double previousStamp)
+		const rtabmap::Transform & velocity)
 {
 	if(tfBuffer != 0)
 	{
@@ -2857,12 +2911,6 @@ bool deskew_impl(
 			return false;
 		}
 
-		if(previousStamp <= 0.0)
-		{
-			UERROR("previousStamp should be >0 when constant velocity model is used!");
-			return false;
-		}
-
 		if(velocity.isNull())
 		{
 			UERROR("velocity should be valid when constant velocity model is used!");
@@ -3133,8 +3181,23 @@ bool deskew_impl(
 	}
 	else if(lastStamp == firstStamp)
 	{
-		UERROR("First and last stamps in the scan are the same (%f) (header=%f)!", timestampFromROS(lastStamp), timestampFromROS(input.header.stamp));
-		return false;
+		// There is no time spread across the scan, so there is nothing to correct. This
+		// happens when the driver doesn't fill the per-point time channel, and also when
+		// the cloud has already been deskewed: deskewing zeroes that channel to mark it.
+		// Pass the cloud through unchanged so that deskewing twice is a no-op rather than
+		// a failure that makes the caller drop the frame.
+		static bool warned = false;
+		if(!warned)
+		{
+			UWARN("First and last stamps in the scan are the same (%f) (header=%f), the "
+				  "cloud is returned unchanged. Either the time channel is not filled by "
+				  "the driver, or the cloud has already been deskewed. This warning is "
+				  "only shown once.",
+				  timestampFromROS(lastStamp), timestampFromROS(input.header.stamp));
+			warned = true;
+		}
+		output = input;
+		return true;
 	}
 	std::string errorMsg;
 	if(tfBuffer != 0 &&
@@ -3184,23 +3247,19 @@ bool deskew_impl(
 			float vx,vy,vz, vroll,vpitch,vyaw;
 			velocity.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw);
 
-			// We need three poses:
-			//  1- The pose of base frame in odom frame at first stamp
-			//  2- The pose of base frame in odom frame at msg stamp
-			//  3- The pose of base frame in odom frame at last stamp
-			UASSERT(timestampFromROS(firstStamp) >= previousStamp);
-			UASSERT(timestampFromROS(lastStamp) > previousStamp);
-			double dt1 = timestampFromROS(firstStamp) - previousStamp;
-			double dt2 = timestampFromROS(input.header.stamp) - previousStamp;
-			double dt3 = timestampFromROS(lastStamp) - previousStamp;
-
-			rtabmap::Transform p1(vx*dt1, vy*dt1, vz*dt1, vroll*dt1, vpitch*dt1, vyaw*dt1);
-			rtabmap::Transform p2(vx*dt2, vy*dt2, vz*dt2, vroll*dt2, vpitch*dt2, vyaw*dt2);
-			rtabmap::Transform p3(vx*dt3, vy*dt3, vz*dt3, vroll*dt3, vpitch*dt3, vyaw*dt3);
+			// Integrate the velocity directly from the stamp of the msg, which is the
+			// frame the deskewed cloud is expressed in. Going through a third, earlier
+			// reference pose and composing it away would give the same answer for a pure
+			// translation, but not for a rotation: Transform() scales roll/pitch/yaw
+			// linearly instead of using the twist exponential, so the composition only
+			// cancels in the small-angle limit. Keeping dt bounded by the scan duration
+			// is where that approximation is at its best.
+			double dt1 = timestampFromROS(firstStamp) - timestampFromROS(input.header.stamp);
+			double dt3 = timestampFromROS(lastStamp) - timestampFromROS(input.header.stamp);
 
 			// First and last poses are relative to stamp of the msg
-			firstPose = p2.inverse() * p1;
-			lastPose = p2.inverse() * p3;
+			firstPose = rtabmap::Transform(vx*dt1, vy*dt1, vz*dt1, vroll*dt1, vpitch*dt1, vyaw*dt1);
+			lastPose = rtabmap::Transform(vx*dt3, vy*dt3, vz*dt3, vroll*dt3, vpitch*dt3, vyaw*dt3);
 		}
 
 		if(firstPose.isNull())
@@ -3227,6 +3286,7 @@ bool deskew_impl(
 
 	output = input;
 	rclcpp::Time stamp;
+	bool clampWarned = false;   // reported once per cloud, see the clamp below
 	UTimer processingTime;
 	if(timeOnColumns)
 	{
@@ -3272,7 +3332,27 @@ bool deskew_impl(
 			rtabmap::Transform transform;
 			if(slerp)
 			{
-				transform = firstPose.interpolate((stamp-firstStamp).seconds() / scanTime, lastPose);
+				// The ordering check only compares the first and last samples, so a stamp
+				// outside [firstStamp, lastStamp] can slip through. Clamp it: extrapolating
+				// would throw the point far beyond the sweep.
+				double ratio = (stamp-firstStamp).seconds() / scanTime;
+				if(ratio < 0.0 || ratio > 1.0)
+				{
+					// Warned once per cloud rather than once per process: the timestamp
+					// channel is corrupted, which is a serious upstream problem worth
+					// reporting on every affected scan, but not once per point.
+					if(!clampWarned)
+					{
+						UWARN("A point has a stamp (%f) outside the first (%f) and last (%f) "
+							  "stamps of the scan, its correction is clamped to the closest end "
+							  "of the sweep. The timestamp channel of the input cloud is likely "
+							  "corrupted. Only the first such point of this cloud is reported.",
+							  timestampFromROS(stamp), timestampFromROS(firstStamp), timestampFromROS(lastStamp));
+						clampWarned = true;
+					}
+					ratio = ratio<0.0?0.0:1.0;
+				}
+				transform = firstPose.interpolate(float(ratio), lastPose);
 			}
 			else
 			{
@@ -3366,7 +3446,27 @@ bool deskew_impl(
 			rtabmap::Transform transform;
 			if(slerp)
 			{
-				transform = firstPose.interpolate((stamp-firstStamp).seconds() / scanTime, lastPose);
+				// The ordering check only compares the first and last samples, so a stamp
+				// outside [firstStamp, lastStamp] can slip through. Clamp it: extrapolating
+				// would throw the point far beyond the sweep.
+				double ratio = (stamp-firstStamp).seconds() / scanTime;
+				if(ratio < 0.0 || ratio > 1.0)
+				{
+					// Warned once per cloud rather than once per process: the timestamp
+					// channel is corrupted, which is a serious upstream problem worth
+					// reporting on every affected scan, but not once per point.
+					if(!clampWarned)
+					{
+						UWARN("A point has a stamp (%f) outside the first (%f) and last (%f) "
+							  "stamps of the scan, its correction is clamped to the closest end "
+							  "of the sweep. The timestamp channel of the input cloud is likely "
+							  "corrupted. Only the first such point of this cloud is reported.",
+							  timestampFromROS(stamp), timestampFromROS(firstStamp), timestampFromROS(lastStamp));
+						clampWarned = true;
+					}
+					ratio = ratio<0.0?0.0:1.0;
+				}
+				transform = firstPose.interpolate(float(ratio), lastPose);
 			}
 			else
 			{
@@ -3428,16 +3528,15 @@ bool deskew(
 		double waitForTransform,
 		bool slerp)
 {
-	return deskew_impl(input, output, fixedFrameId, &tfBuffer, waitForTransform, slerp, rtabmap::Transform(), 0);
+	return deskew_impl(input, output, fixedFrameId, &tfBuffer, waitForTransform, slerp, rtabmap::Transform());
 }
 
 bool deskew(
 		const sensor_msgs::msg::PointCloud2 & input,
 		sensor_msgs::msg::PointCloud2 & output,
-		double previousStamp,
 		const rtabmap::Transform & velocity)
 {
-	return deskew_impl(input, output, "", 0, 0, true, velocity, previousStamp);
+	return deskew_impl(input, output, "", 0, 0, true, velocity);
 }
 
 
@@ -3493,8 +3592,15 @@ transformPointCloud (
     Eigen::Vector4f pt_out;
 
     bool max_range_point = false;
-    int distance_ptr_offset = i*in.point_step + in.fields[dist_idx].offset;
-    float* distance_ptr = (dist_idx < 0 ? NULL : (float*)(&in.data[distance_ptr_offset]));
+    // Only touch in.fields[dist_idx] when the "distance" field actually exists:
+    // indexing with -1 is out of bounds and aborts on a hardened libstdc++.
+    int distance_ptr_offset = 0;
+    float* distance_ptr = NULL;
+    if (dist_idx >= 0)
+    {
+      distance_ptr_offset = i*in.point_step + in.fields[dist_idx].offset;
+      distance_ptr = (float*)(&in.data[distance_ptr_offset]);
+    }
     if (!std::isfinite (pt[0]) || !std::isfinite (pt[1]) || !std::isfinite (pt[2]))
     {
       if (distance_ptr==NULL || !std::isfinite(*distance_ptr))  // Invalid point

rtabmap_odom/src/nodelets/icp_odometry.cpp

@@ -333,7 +333,7 @@ void ICPOdometry::callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scan
 		{
 			// deskew with constant velocity model (we are in frameId)
 			sensor_msgs::msg::PointCloud2 scanOutDeskewed;
-			if(!rtabmap_conversions::deskew(scanOut, scanOutDeskewed, previousStamp(), velocityGuess()))
+			if(!rtabmap_conversions::deskew(scanOut, scanOutDeskewed, velocityGuess()))
 			{
 				RCLCPP_ERROR(this->get_logger(), "Failed to deskew input cloud, aborting odometry update!");
 				return;
@@ -362,7 +362,7 @@ void ICPOdometry::callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scan
 		{
 			// deskew with constant velocity model
 			sensor_msgs::msg::PointCloud2 scanOutDeskewed;
-			if(!rtabmap_conversions::deskew(scanOut, scanOutDeskewed, previousStamp(), velocityGuess()))
+			if(!rtabmap_conversions::deskew(scanOut, scanOutDeskewed, velocityGuess()))
 			{
 				RCLCPP_ERROR(this->get_logger(), "Failed to deskew input cloud, aborting odometry update!");
 				return;
@@ -583,7 +583,7 @@ void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr p
 			}
 
 			std::shared_ptr<sensor_msgs::msg::PointCloud2> cloudDeskewed(new sensor_msgs::msg::PointCloud2);
-			if(!rtabmap_conversions::deskew(*cloudPtr, *cloudDeskewed, previousStamp(), velocityGuess()))
+			if(!rtabmap_conversions::deskew(*cloudPtr, *cloudDeskewed, velocityGuess()))
 			{
 				RCLCPP_ERROR(this->get_logger(), "Failed to deskew input cloud, aborting odometry update!");
 				return;

rtabmap_viz/src/rgbd_image_viewer.cpp

@@ -165,6 +165,21 @@ void RGBDImageViewer::callback(
         QMetaObject::invokeMethod(warningLabel_, "clear");
     }
 
+    // rgbdImageFromROS() does not copy the pixels: the SensorData points into the ROS
+    // message buffers, which the subscription queue recycles as soon as this callback
+    // returns. The event below is posted asynchronously and so outlives the callback,
+    // therefore the images must be deep-copied first.
+    if(!data.imageRaw().empty() || !data.depthOrRightRaw().empty()) {
+        cv::Mat image = data.imageRaw().clone();
+        cv::Mat depthOrRight = data.depthOrRightRaw().clone();
+        if(!data.stereoCameraModels().empty()) {
+            data.setStereoImage(image, depthOrRight, data.stereoCameraModels());
+        }
+        else {
+            data.setRGBDImage(image, depthOrRight, data.cameraModels());
+        }
+    }
+
     this->post(new rtabmap::SensorEvent(data));
 }