diff --git a/.github/workflows/ros2.yml b/.github/workflows/ros2.yml index f0be82e9..0723b173 100644 --- a/.github/workflows/ros2.yml +++ b/.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 diff --git a/.gitignore b/.gitignore index 5fb548e9..8f56a6c7 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,6 @@ .pydevproject .settings __pycache__ +# rosdoc2 build artifacts +docs_build +cross_reference diff --git a/rtabmap_conversions/CMakeLists.txt b/rtabmap_conversions/CMakeLists.txt index bb6aa06e..7300b3ea 100644 --- a/rtabmap_conversions/CMakeLists.txt +++ b/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() diff --git a/rtabmap_conversions/README.md b/rtabmap_conversions/README.md new file mode 100644 index 00000000..9b187a02 --- /dev/null +++ b/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 +rtabmap_conversions +``` + +```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 + +// 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). diff --git a/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h b/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h index 4e82e388..a87aa2a3 100644 --- a/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h +++ b/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& 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 & 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 & 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 keypointsFromROS(const std::vector & 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 & msg, std::vector & kpts, int xShift=0); + +/** @brief Fill keypoint messages from a vector of cv::KeyPoint. */ void keypointsToROS(const std::vector & kpts, std::vector & 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 globalDescriptorsFromROS(const std::vector & msg); +/** @brief Fill global descriptor messages; @p msg is cleared first. */ void globalDescriptorsToROS(const std::vector & desc, std::vector & 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 & msg); +/** @brief Fill EnvSensor messages from a map of sensors; @p msg is cleared first. */ void envSensorsToROS(const rtabmap::EnvSensors & sensors, std::vector & 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 points2fFromROS(const std::vector & msg); +/** @brief Fill Point2f messages from a vector of cv::Point2f. */ void points2fToROS(const std::vector & kpts, std::vector & 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 points3fFromROS(const std::vector & 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 & msg, std::vector & 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 & kpts, std::vector & 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 & poses, std::multimap & links, std::map & 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 & poses, const std::multimap & 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 & poses, std::multimap & 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 & poses, const std::multimap & 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 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 > & 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 & imageMsgs, const std::vector & depthMsgs, @@ -249,6 +769,34 @@ bool convertRGBDMsgs( std::vector * 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 std::map::const_iterator getClosestIterator( const std::map & buffer, @@ -365,6 +1025,7 @@ typename std::map::const_iterator getClosestIterator( return iterB; } + } #endif /* MSGCONVERSION_H_ */ diff --git a/rtabmap_conversions/package.xml b/rtabmap_conversions/package.xml index 9ad6cadd..9484d9e8 100644 --- a/rtabmap_conversions/package.xml +++ b/rtabmap_conversions/package.xml @@ -30,7 +30,10 @@ tf2_geometry_msgs tf2_ros + ament_cmake_gtest + ament_cmake + rosdoc2.yaml diff --git a/rtabmap_conversions/rosdoc2.yaml b/rtabmap_conversions/rosdoc2.yaml new file mode 100644 index 00000000..bcd0f339 --- /dev/null +++ b/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: '' + } diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp index 39dabb6e..e252a617 100644 --- a/rtabmap_conversions/src/MsgConversion.cpp +++ b/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 +#include + #include #include #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::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 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(0,0)))); - stats.insert(std::make_pair("Odometry/StdDevAng/", sqrt((float)info.reg.covariance.at(5,5)))); - stats.insert(std::make_pair("Odometry/VarianceLin/", (float)info.reg.covariance.at(0,0))); - stats.insert(std::make_pair("Odometry/VarianceAng/", (float)info.reg.covariance.at(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(0,0)))); + stats.insert(std::make_pair("Odometry/StdDevAng/", sqrt((float)info.reg.covariance.at(5,5)))); + stats.insert(std::make_pair("Odometry/VarianceLin/", (float)info.reg.covariance.at(0,0))); + stats.insert(std::make_pair("Odometry/VarianceAng/", (float)info.reg.covariance.at(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 diff --git a/rtabmap_conversions/test/test_msg_conversion.cpp b/rtabmap_conversions/test/test_msg_conversion.cpp new file mode 100644 index 00000000..0b911720 --- /dev/null +++ b/rtabmap_conversions/test/test_msg_conversion.cpp @@ -0,0 +1,3456 @@ +/* +Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the Universite de Sherbrooke 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 AUTHOR BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#include + +#include +#include +#include + +#include + +#include +#include +#include + +using namespace rtabmap_conversions; + +namespace { + +// A transform with translation and rotation on all three axes, so that a +// round-trip that drops or swaps a component cannot pass by accident. +rtabmap::Transform sampleTransform() +{ + return rtabmap::Transform(1.0f, -2.0f, 3.0f, 0.1f, -0.2f, 0.3f); +} + +void expectTransformNear( + const rtabmap::Transform & actual, + const rtabmap::Transform & expected, + float epsilon = 1e-5f) +{ + ASSERT_FALSE(actual.isNull()) << "expected " << expected.prettyPrint(); + for(int i=0; i<12; ++i) + { + EXPECT_NEAR(actual.data()[i], expected.data()[i], epsilon) + << "at index " << i + << "\n actual: " << actual.prettyPrint() + << "\n expected: " << expected.prettyPrint(); + } +} + +} // namespace + +///////////////////////// +// Transform <-> geometry_msgs +///////////////////////// + +TEST(MsgConversion, transformGeometryMsgRoundTrip) +{ + const rtabmap::Transform in = sampleTransform(); + + geometry_msgs::msg::Transform msg; + transformToGeometryMsg(in, msg); + + expectTransformNear(transformFromGeometryMsg(msg), in); +} + +TEST(MsgConversion, transformGeometryMsgQuaternionIsNormalized) +{ + geometry_msgs::msg::Transform msg; + transformToGeometryMsg(sampleTransform(), msg); + + const double norm = std::sqrt( + msg.rotation.x * msg.rotation.x + + msg.rotation.y * msg.rotation.y + + msg.rotation.z * msg.rotation.z + + msg.rotation.w * msg.rotation.w); + EXPECT_NEAR(norm, 1.0, 1e-9); +} + +TEST(MsgConversion, transformGeometryMsgNullRoundTrip) +{ + geometry_msgs::msg::Transform msg; + transformToGeometryMsg(rtabmap::Transform(), msg); + + // A null transform is encoded as an all-zero quaternion. + EXPECT_EQ(msg.rotation.x, 0.0); + EXPECT_EQ(msg.rotation.y, 0.0); + EXPECT_EQ(msg.rotation.z, 0.0); + EXPECT_EQ(msg.rotation.w, 0.0); + EXPECT_TRUE(transformFromGeometryMsg(msg).isNull()); +} + +TEST(MsgConversion, transformGeometryMsgIdentityIsNotNull) +{ + geometry_msgs::msg::Transform msg; + transformToGeometryMsg(rtabmap::Transform::getIdentity(), msg); + + const rtabmap::Transform out = transformFromGeometryMsg(msg); + EXPECT_FALSE(out.isNull()); + EXPECT_TRUE(out.isIdentity()); +} + +///////////////////////// +// Transform <-> tf2 +///////////////////////// + +TEST(MsgConversion, transformTFRoundTrip) +{ + const rtabmap::Transform in = sampleTransform(); + + tf2::Transform tf; + EXPECT_TRUE(transformToTF(in, tf)); + + expectTransformNear(transformFromTF(tf), in); +} + +TEST(MsgConversion, transformTFIdentityRoundTrip) +{ + tf2::Transform tf; + EXPECT_TRUE(transformToTF(rtabmap::Transform::getIdentity(), tf)) + << "an identity transform is not a null transform"; + + const rtabmap::Transform out = transformFromTF(tf); + EXPECT_FALSE(out.isNull()); + EXPECT_TRUE(out.isIdentity()); +} + +TEST(MsgConversion, transformToTFWritesTranslationAndRotation) +{ + // Guards against the output being left untouched: seed it with a value that + // differs from the expected result, then check it was actually overwritten. + tf2::Transform tf(tf2::Quaternion(0, 0, 0, 1), tf2::Vector3(99, 99, 99)); + EXPECT_TRUE(transformToTF(sampleTransform(), tf)); + + EXPECT_NEAR(tf.getOrigin().x(), 1.0, 1e-5); + EXPECT_NEAR(tf.getOrigin().y(), -2.0, 1e-5); + EXPECT_NEAR(tf.getOrigin().z(), 3.0, 1e-5); + + geometry_msgs::msg::Transform expected; + transformToGeometryMsg(sampleTransform(), expected); + EXPECT_NEAR(tf.getRotation().x(), expected.rotation.x, 1e-5); + EXPECT_NEAR(tf.getRotation().y(), expected.rotation.y, 1e-5); + EXPECT_NEAR(tf.getRotation().z(), expected.rotation.z, 1e-5); + EXPECT_NEAR(tf.getRotation().w(), expected.rotation.w, 1e-5); +} + +TEST(MsgConversion, transformToTFNullReturnsFalseAndNaN) +{ + tf2::Transform tf(tf2::Quaternion(0, 0, 0, 1), tf2::Vector3(99, 99, 99)); + + EXPECT_FALSE(transformToTF(rtabmap::Transform(), tf)); + + // tf2::Transform cannot carry a null sentinel, so the output is poisoned with NaN + // on purpose: a caller that ignores the return value must fail loudly rather than + // silently proceed with a plausible-looking identity. + for(int i=0; i<3; ++i) + { + EXPECT_TRUE(std::isnan(tf.getBasis()[i].x())) << "basis row " << i; + EXPECT_TRUE(std::isnan(tf.getBasis()[i].y())) << "basis row " << i; + EXPECT_TRUE(std::isnan(tf.getBasis()[i].z())) << "basis row " << i; + } + EXPECT_TRUE(std::isnan(tf.getOrigin().x())); + EXPECT_TRUE(std::isnan(tf.getOrigin().y())); + EXPECT_TRUE(std::isnan(tf.getOrigin().z())); + + const tf2::Quaternion q = tf.getRotation(); + EXPECT_TRUE(std::isnan(q.x())); + EXPECT_TRUE(std::isnan(q.y())); + EXPECT_TRUE(std::isnan(q.z())); + EXPECT_TRUE(std::isnan(q.w())); +} + +TEST(MsgConversion, transformToTFNullPoisonsComposition) +{ + // The point of the NaN: it propagates through downstream math instead of + // quietly producing a wrong-but-finite answer. + tf2::Transform tf; + EXPECT_FALSE(transformToTF(rtabmap::Transform(), tf)); + + const tf2::Transform composed = + tf * tf2::Transform(tf2::Quaternion(0, 0, 0, 1), tf2::Vector3(1, 2, 3)); + + EXPECT_TRUE(std::isnan(composed.getOrigin().x())); + EXPECT_TRUE(std::isnan(composed.getOrigin().y())); + EXPECT_TRUE(std::isnan(composed.getOrigin().z())); +} + +TEST(MsgConversion, transformFromTFDetectsNaN) +{ + const tf2Scalar nan = std::numeric_limits::quiet_NaN(); + + // NaN anywhere in the rotation basis... + EXPECT_TRUE(transformFromTF(tf2::Transform( + tf2::Matrix3x3(nan, nan, nan, nan, nan, nan, nan, nan, nan), + tf2::Vector3(0, 0, 0))).isNull()) << "NaN basis"; + + // ...or in the translation alone must yield a null transform. + EXPECT_TRUE(transformFromTF(tf2::Transform( + tf2::Quaternion(0, 0, 0, 1), + tf2::Vector3(nan, 0, 0))).isNull()) << "NaN origin"; +} + +TEST(MsgConversion, transformTFNullRoundTrip) +{ + // The pair round-trips a null transform: toTF poisons with NaN and reports + // false, fromTF maps that back to null. + tf2::Transform tf; + EXPECT_FALSE(transformToTF(rtabmap::Transform(), tf)); + EXPECT_TRUE(transformFromTF(tf).isNull()); +} + +TEST(MsgConversion, transformFromTFAcceptsValidTransforms) +{ + // The NaN guard must not reject legitimate values, including zeros. + EXPECT_FALSE(transformFromTF(tf2::Transform( + tf2::Quaternion(0, 0, 0, 1), tf2::Vector3(0, 0, 0))).isNull()); + EXPECT_FALSE(transformFromTF(tf2::Transform( + tf2::Quaternion(0, 0, 0, 1), tf2::Vector3(-1, 2, -3))).isNull()); +} + +///////////////////////// +// Transform <-> Pose +///////////////////////// + +TEST(MsgConversion, transformPoseMsgRoundTrip) +{ + const rtabmap::Transform in = sampleTransform(); + + geometry_msgs::msg::Pose msg; + transformToPoseMsg(in, msg); + + expectTransformNear(transformFromPoseMsg(msg), in); +} + +TEST(MsgConversion, transformPoseMsgNullIsNull) +{ + geometry_msgs::msg::Pose msg; + transformToPoseMsg(rtabmap::Transform(), msg); + + EXPECT_TRUE(transformFromPoseMsg(msg).isNull()); +} + +TEST(MsgConversion, transformPoseMsgIgnoreRotationIfNotSet) +{ + // Note: geometry_msgs::msg::Quaternion defaults to w=1.0, so an "unset" + // orientation has to be zeroed explicitly to reach the branch under test. + geometry_msgs::msg::Pose msg; + msg.position.x = 1.0; + msg.position.y = 2.0; + msg.position.z = 3.0; + msg.orientation.w = 0.0; + + // An all-zero orientation normally yields a null transform... + + EXPECT_TRUE(transformFromPoseMsg(msg, false).isNull()); + + // ...but with ignoreRotationIfNotSet the translation is kept with no rotation. + expectTransformNear( + transformFromPoseMsg(msg, true), + rtabmap::Transform(1.0f, 2.0f, 3.0f, 0.0f, 0.0f, 0.0f)); +} + +///////////////////////// +// Points and keypoints +///////////////////////// + +TEST(MsgConversion, point2fRoundTrip) +{ + const cv::Point2f in(1.5f, -2.5f); + + rtabmap_msgs::msg::Point2f msg; + point2fToROS(in, msg); + const cv::Point2f out = point2fFromROS(msg); + + EXPECT_FLOAT_EQ(out.x, in.x); + EXPECT_FLOAT_EQ(out.y, in.y); +} + +TEST(MsgConversion, points2fVectorRoundTrip) +{ + const std::vector in = {{1.0f, 2.0f}, {-3.0f, 4.5f}}; + + std::vector msg; + points2fToROS(in, msg); + const std::vector out = points2fFromROS(msg); + + ASSERT_EQ(out.size(), in.size()); + for(size_t i=0; i in = {{1.0f, 2.0f, 3.0f}, {-4.0f, 5.0f, -6.0f}}; + + std::vector msg; + points3fToROS(in, msg); + const std::vector out = points3fFromROS(msg); + + ASSERT_EQ(out.size(), in.size()); + for(size_t i=0; i in = {{1.0f, 2.0f, 3.0f}}; + const rtabmap::Transform t = sampleTransform(); + + // Applying t on the way out and t.inverse() on the way in must cancel. + std::vector msg; + points3fToROS(in, msg, t); + const std::vector out = points3fFromROS(msg, t.inverse()); + + ASSERT_EQ(out.size(), in.size()); + EXPECT_NEAR(out[0].x, in[0].x, 1e-4); + EXPECT_NEAR(out[0].y, in[0].y, 1e-4); + EXPECT_NEAR(out[0].z, in[0].z, 1e-4); + + // ...and the intermediate message really is the transformed point. + const cv::Point3f expected = rtabmap::util3d::transformPoint(in[0], t); + EXPECT_NEAR(msg[0].x, expected.x, 1e-4); + EXPECT_NEAR(msg[0].y, expected.y, 1e-4); + EXPECT_NEAR(msg[0].z, expected.z, 1e-4); +} + +TEST(MsgConversion, points3fFromROSAppendsToExistingVector) +{ + std::vector msg(2); + msg[0].x = 1.0f; + msg[1].x = 2.0f; + + std::vector points = {{9.0f, 9.0f, 9.0f}}; + points3fFromROS(msg, points); + + ASSERT_EQ(points.size(), 3u); + EXPECT_FLOAT_EQ(points[0].x, 9.0f) << "existing content must be preserved"; + EXPECT_FLOAT_EQ(points[1].x, 1.0f); + EXPECT_FLOAT_EQ(points[2].x, 2.0f); +} + +TEST(MsgConversion, keypointRoundTrip) +{ + const cv::KeyPoint in(cv::Point2f(10.0f, 20.0f), 7.0f, 45.0f, 0.5f, 2, 3); + + rtabmap_msgs::msg::KeyPoint msg; + keypointToROS(in, msg); + const cv::KeyPoint out = keypointFromROS(msg); + + EXPECT_FLOAT_EQ(out.pt.x, in.pt.x); + EXPECT_FLOAT_EQ(out.pt.y, in.pt.y); + EXPECT_FLOAT_EQ(out.size, in.size); + EXPECT_FLOAT_EQ(out.angle, in.angle); + EXPECT_FLOAT_EQ(out.response, in.response); + EXPECT_EQ(out.octave, in.octave); + EXPECT_EQ(out.class_id, in.class_id); +} + +TEST(MsgConversion, keypointsFromROSAppendsAndAppliesXShift) +{ + const std::vector in = { + cv::KeyPoint(cv::Point2f(10.0f, 20.0f), 7.0f), + cv::KeyPoint(cv::Point2f(30.0f, 40.0f), 7.0f)}; + + std::vector msg; + keypointsToROS(in, msg); + ASSERT_EQ(msg.size(), in.size()); + + std::vector kpts = {cv::KeyPoint(cv::Point2f(1.0f, 1.0f), 1.0f)}; + keypointsFromROS(msg, kpts, /*xShift=*/100); + + ASSERT_EQ(kpts.size(), 3u); + EXPECT_FLOAT_EQ(kpts[0].pt.x, 1.0f) << "existing content must be preserved"; + EXPECT_FLOAT_EQ(kpts[1].pt.x, 110.0f); + EXPECT_FLOAT_EQ(kpts[2].pt.x, 130.0f); + EXPECT_FLOAT_EQ(kpts[1].pt.y, 20.0f) << "xShift must not touch y"; +} + +///////////////////////// +// Timestamps +///////////////////////// + +TEST(MsgConversion, timestampRoundTrip) +{ + // Not exact on purpose: a double resolves to a few hundred nanoseconds at this + // magnitude, so the round trip is only good to about a microsecond. + const double in = 1234567890.123456; + EXPECT_NEAR(timestampFromROS(timestampToROS(in)), in, 1e-6); +} + +TEST(MsgConversion, timestampToROSUsesRosClock) +{ + // Message header stamps convert to RCL_ROS_TIME, while rclcpp::Time(sec, nsec) + // defaults to RCL_SYSTEM_TIME. Comparing two different clock types throws, so a + // timestamp built here must be comparable with one taken from a message -- several + // conversions do exactly that when syncing to an odometry stamp. + const rclcpp::Time built = timestampToROS(1000.0); + EXPECT_EQ(built.get_clock_type(), RCL_ROS_TIME); + + builtin_interfaces::msg::Time asMsg = timestampToROS(1000.5); + const rclcpp::Time fromMsg(asMsg); + EXPECT_EQ(fromMsg.get_clock_type(), RCL_ROS_TIME); + + EXPECT_NO_THROW({ volatile bool differ = (built != fromMsg); (void)differ; }) + << "a built stamp must be comparable with a message-derived one"; +} + +TEST(MsgConversion, timestampZeroRoundTrip) +{ + EXPECT_EQ(timestampFromROS(timestampToROS(0.0)), 0.0); +} + +///////////////////////// +// sizeOfPointField +///////////////////////// + +TEST(MsgConversion, sizeOfPointField) +{ + EXPECT_EQ(sizeOfPointField(sensor_msgs::msg::PointField::INT8), 1); + EXPECT_EQ(sizeOfPointField(sensor_msgs::msg::PointField::UINT8), 1); + EXPECT_EQ(sizeOfPointField(sensor_msgs::msg::PointField::INT16), 2); + EXPECT_EQ(sizeOfPointField(sensor_msgs::msg::PointField::UINT16), 2); + EXPECT_EQ(sizeOfPointField(sensor_msgs::msg::PointField::INT32), 4); + EXPECT_EQ(sizeOfPointField(sensor_msgs::msg::PointField::UINT32), 4); + EXPECT_EQ(sizeOfPointField(sensor_msgs::msg::PointField::FLOAT32), 4); + EXPECT_EQ(sizeOfPointField(sensor_msgs::msg::PointField::FLOAT64), 8); +} + +TEST(MsgConversion, sizeOfPointFieldThrowsOnUnknownType) +{ + EXPECT_THROW(sizeOfPointField(42), std::runtime_error); +} + +///////////////////////// +// getClosestIterator +///////////////////////// + +TEST(MsgConversion, getClosestIterator) +{ + const std::map buffer = {{1.0, 10}, {2.0, 20}, {3.0, 30}}; + + EXPECT_EQ(getClosestIterator(buffer, 1.0)->second, 10) << "exact match"; + EXPECT_EQ(getClosestIterator(buffer, 2.0)->second, 20) << "exact match"; + EXPECT_EQ(getClosestIterator(buffer, 1.4)->second, 10) << "closer to lower"; + EXPECT_EQ(getClosestIterator(buffer, 1.6)->second, 20) << "closer to upper"; + EXPECT_EQ(getClosestIterator(buffer, 0.0)->second, 10) << "clamped below range"; + EXPECT_EQ(getClosestIterator(buffer, 99.0)->second, 30) << "clamped above range"; +} + +TEST(MsgConversion, getClosestIteratorSingleEntry) +{ + const std::map buffer = {{5.0, 50}}; + + EXPECT_EQ(getClosestIterator(buffer, 0.0)->second, 50); + EXPECT_EQ(getClosestIterator(buffer, 99.0)->second, 50); +} + +///////////////////////// +// compressedMat <-> bytes +///////////////////////// + +TEST(MsgConversion, compressedMatRoundTrip) +{ + const cv::Mat in = (cv::Mat_(1, 5) << 1, 2, 3, 250, 255); + + std::vector bytes; + compressedMatToBytes(in, bytes); + ASSERT_EQ(bytes.size(), 5u); + + const cv::Mat out = compressedMatFromBytes(bytes); + ASSERT_EQ(out.type(), CV_8UC1); + ASSERT_EQ(out.total(), in.total()); + EXPECT_EQ(cv::countNonZero(out.reshape(1, 1) != in.reshape(1, 1)), 0); +} + +TEST(MsgConversion, compressedMatEmptyRoundTrip) +{ + std::vector bytes = {1, 2, 3}; + compressedMatToBytes(cv::Mat(), bytes); + + EXPECT_TRUE(bytes.empty()) << "output must be cleared"; + EXPECT_TRUE(compressedMatFromBytes(bytes).empty()); +} + +TEST(MsgConversion, compressedMatFromBytesCopyFlag) +{ + std::vector bytes = {1, 2, 3}; + + const cv::Mat shared = compressedMatFromBytes(bytes, /*copy=*/false); + const cv::Mat copied = compressedMatFromBytes(bytes, /*copy=*/true); + + bytes[0] = 99; + EXPECT_EQ(shared.at(0, 0), 99) << "copy=false must alias the input"; + EXPECT_EQ(copied.at(0, 0), 1) << "copy=true must be independent"; +} + +///////////////////////// +// EnvSensor +///////////////////////// + +TEST(MsgConversion, envSensorRoundTrip) +{ + const rtabmap::EnvSensor in( + rtabmap::EnvSensor::kAmbientTemperature, 21.5, 1234567890.5); + + rtabmap_msgs::msg::EnvSensor msg; + envSensorToROS(in, msg); + const rtabmap::EnvSensor out = envSensorFromROS(msg); + + EXPECT_EQ(out.type(), in.type()); + EXPECT_DOUBLE_EQ(out.value(), in.value()); + EXPECT_NEAR(out.stamp(), in.stamp(), 1e-6); +} + +TEST(MsgConversion, envSensorsRoundTripKeyedByType) +{ + rtabmap::EnvSensors in; + in.insert(std::make_pair( + rtabmap::EnvSensor::kAmbientTemperature, + rtabmap::EnvSensor(rtabmap::EnvSensor::kAmbientTemperature, 21.5, 1.0))); + in.insert(std::make_pair( + rtabmap::EnvSensor::kAmbientLight, + rtabmap::EnvSensor(rtabmap::EnvSensor::kAmbientLight, 300.0, 2.0))); + + std::vector msg; + envSensorsToROS(in, msg); + ASSERT_EQ(msg.size(), in.size()); + + const rtabmap::EnvSensors out = envSensorsFromROS(msg); + ASSERT_EQ(out.size(), in.size()); + for(rtabmap::EnvSensors::const_iterator iter=in.begin(); iter!=in.end(); ++iter) + { + rtabmap::EnvSensors::const_iterator found = out.find(iter->first); + ASSERT_NE(found, out.end()) << "missing type " << iter->first; + EXPECT_DOUBLE_EQ(found->second.value(), iter->second.value()); + } +} + +///////////////////////// +// Link +///////////////////////// + +TEST(MsgConversion, linkRoundTrip) +{ + cv::Mat information = cv::Mat::eye(6, 6, CV_64FC1) * 3.0; + const rtabmap::Link in( + 1, 2, rtabmap::Link::kGlobalClosure, sampleTransform(), information); + + rtabmap_msgs::msg::Link msg; + linkToROS(in, msg); + const rtabmap::Link out = linkFromROS(msg); + + EXPECT_EQ(out.from(), in.from()); + EXPECT_EQ(out.to(), in.to()); + EXPECT_EQ(out.type(), in.type()); + expectTransformNear(out.transform(), in.transform()); + + ASSERT_EQ(out.infMatrix().rows, 6); + ASSERT_EQ(out.infMatrix().cols, 6); + for(int i=0; i<6; ++i) + { + for(int j=0; j<6; ++j) + { + EXPECT_DOUBLE_EQ( + out.infMatrix().at(i, j), + in.infMatrix().at(i, j)) << "at " << i << "," << j; + } + } +} + +///////////////////////// +// CameraModel +///////////////////////// + +TEST(MsgConversion, cameraModelFromROSReadsIntrinsics) +{ + sensor_msgs::msg::CameraInfo in; + in.width = 640; + in.height = 480; + in.distortion_model = "plumb_bob"; + in.d = {0.1, 0.2, 0.3, 0.4, 0.5}; + in.k = {525.0, 0.0, 320.0, 0.0, 525.0, 240.0, 0.0, 0.0, 1.0}; + in.r = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0}; + in.p = {525.0, 0.0, 320.0, 0.0, 0.0, 525.0, 240.0, 0.0, 0.0, 0.0, 1.0, 0.0}; + + const rtabmap::Transform localTransform(0.0f, 0.0f, 0.1f, 0.0f, 0.0f, 0.0f); + const rtabmap::CameraModel model = cameraModelFromROS(in, localTransform); + + EXPECT_EQ(model.imageWidth(), 640); + EXPECT_EQ(model.imageHeight(), 480); + EXPECT_NEAR(model.fx(), 525.0, 1e-9); + EXPECT_NEAR(model.fy(), 525.0, 1e-9); + EXPECT_NEAR(model.cx(), 320.0, 1e-9); + EXPECT_NEAR(model.cy(), 240.0, 1e-9); + expectTransformNear(model.localTransform(), localTransform); + + // The raw distortion coefficients are kept verbatim. + ASSERT_EQ(model.D_raw().cols, 5); + for(size_t i=0; i(0, i), in.d[i], 1e-9) << "d at " << i; + } +} + +TEST(MsgConversion, cameraModelToROSRectifiedHasNoDistortion) +{ + sensor_msgs::msg::CameraInfo in; + in.width = 640; + in.height = 480; + in.distortion_model = "plumb_bob"; + in.d = {0.1, 0.2, 0.3, 0.4, 0.5}; + in.k = {525.0, 0.0, 320.0, 0.0, 525.0, 240.0, 0.0, 0.0, 1.0}; + in.r = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0}; + in.p = {525.0, 0.0, 320.0, 0.0, 0.0, 525.0, 240.0, 0.0, 0.0, 0.0, 1.0, 0.0}; + + sensor_msgs::msg::CameraInfo out; + cameraModelToROS(cameraModelFromROS(in, rtabmap::Transform::getIdentity()), out); + + EXPECT_EQ(out.width, in.width); + EXPECT_EQ(out.height, in.height); + + // A model carrying a projection matrix describes an already-rectified image, + // so cameraModelToROS deliberately emits zero distortion rather than echoing + // back the raw coefficients. K and P do round-trip unchanged. + EXPECT_EQ(out.distortion_model, "plumb_bob"); + ASSERT_EQ(out.d.size(), 5u); + for(size_t i=0; i(3, 3) << + 525.0, 0.0, 320.0, 0.0, 525.0, 240.0, 0.0, 0.0, 1.0); + cv::Mat D = cv::Mat::zeros(1, 6, CV_64FC1); + D.at(0, 0) = 0.1; + D.at(0, 1) = 0.2; + D.at(0, 4) = 0.3; + D.at(0, 5) = 0.4; + + const rtabmap::CameraModel model( + "fisheye", cv::Size(640, 480), K, D, cv::Mat(), cv::Mat(), + rtabmap::Transform::getIdentity()); + + sensor_msgs::msg::CameraInfo out; + cameraModelToROS(model, out); + + EXPECT_EQ(out.distortion_model, "equidistant"); + ASSERT_EQ(out.d.size(), 4u); + EXPECT_NEAR(out.d[0], 0.1, 1e-9); + EXPECT_NEAR(out.d[1], 0.2, 1e-9); + EXPECT_NEAR(out.d[2], 0.3, 1e-9); + EXPECT_NEAR(out.d[3], 0.4, 1e-9); +} + +TEST(MsgConversion, cameraModelToROSRationalPolynomialDistortion) +{ + cv::Mat K = (cv::Mat_(3, 3) << + 525.0, 0.0, 320.0, 0.0, 525.0, 240.0, 0.0, 0.0, 1.0); + cv::Mat D = (cv::Mat_(1, 8) << + 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8); + + const rtabmap::CameraModel model( + "rational", cv::Size(640, 480), K, D, cv::Mat(), cv::Mat(), + rtabmap::Transform::getIdentity()); + + sensor_msgs::msg::CameraInfo out; + cameraModelToROS(model, out); + + EXPECT_EQ(out.distortion_model, "rational_polynomial"); + ASSERT_EQ(out.d.size(), 8u); + for(size_t i=0; i model -> message round trip. + sensor_msgs::msg::CameraInfo in; + in.width = 640; + in.height = 480; + in.distortion_model = "equidistant"; + in.d = {0.1, 0.2, 0.3, 0.4}; + in.k = {525.0, 0.0, 320.0, 0.0, 525.0, 240.0, 0.0, 0.0, 1.0}; + + sensor_msgs::msg::CameraInfo out; + cameraModelToROS(cameraModelFromROS(in, rtabmap::Transform::getIdentity()), out); + + EXPECT_EQ(out.distortion_model, "equidistant"); + ASSERT_EQ(out.d.size(), 4u); + for(size_t i=0; i identity = {1., 0., 0., 0., 1., 0., 0., 0., 1.}; + for(size_t i=0; i(3, 3) << + 525.0, 0.0, 320.0, 0.0, 525.0, 240.0, 0.0, 0.0, 1.0); + + const rtabmap::CameraModel model( + "raw", cv::Size(640, 480), K, cv::Mat(), cv::Mat(), cv::Mat(), + rtabmap::Transform::getIdentity()); + + sensor_msgs::msg::CameraInfo out; + cameraModelToROS(model, out); + + const std::array expected = { + 525.0, 0.0, 320.0, 0.0, + 0.0, 525.0, 240.0, 0.0, + 0.0, 0.0, 1.0, 0.0}; + for(size_t i=0; i(1, 4) << 1.0f, 2.0f, 3.0f, 4.0f); + cv::Mat info = (cv::Mat_(1, 2) << 9.0f, 8.0f); + const rtabmap::GlobalDescriptor in(7, data, info); + + rtabmap_msgs::msg::GlobalDescriptor msg; + globalDescriptorToROS(in, msg); + const rtabmap::GlobalDescriptor out = globalDescriptorFromROS(msg); + + EXPECT_EQ(out.type(), in.type()); + ASSERT_EQ(out.data().total(), in.data().total()); + for(size_t i=0; i(0, i), in.data().at(0, i)) << "data at " << i; + } + ASSERT_EQ(out.info().total(), in.info().total()); + for(size_t i=0; i(0, i), in.info().at(0, i)) << "info at " << i; + } +} + +TEST(MsgConversion, globalDescriptorsVectorRoundTrip) +{ + std::vector in; + in.push_back(rtabmap::GlobalDescriptor(1, (cv::Mat_(1, 2) << 1.0f, 2.0f))); + in.push_back(rtabmap::GlobalDescriptor(2, (cv::Mat_(1, 2) << 3.0f, 4.0f))); + + std::vector msg; + globalDescriptorsToROS(in, msg); + ASSERT_EQ(msg.size(), in.size()); + + const std::vector out = globalDescriptorsFromROS(msg); + ASSERT_EQ(out.size(), in.size()); + for(size_t i=0; i(0, 0), in[i].data().at(0, 0)) << "at " << i; + } +} + +TEST(MsgConversion, globalDescriptorsEmptyRoundTrip) +{ + std::vector msg(3); + globalDescriptorsToROS(std::vector(), msg); + + EXPECT_TRUE(msg.empty()) << "output must be cleared"; + EXPECT_TRUE(globalDescriptorsFromROS(msg).empty()); +} + +///////////////////////// +// UserData +///////////////////////// + +TEST(MsgConversion, userDataUncompressedRoundTrip) +{ + const cv::Mat in = (cv::Mat_(2, 3) << 1, 2, 3, 4, 5, 6); + + rtabmap_msgs::msg::UserData msg; + userDataToROS(in, msg, /*compress=*/false); + + EXPECT_EQ(msg.rows, in.rows); + EXPECT_EQ(msg.cols, in.cols); + EXPECT_EQ(msg.type, in.type()); + + const cv::Mat out = userDataFromROS(msg); + ASSERT_EQ(out.rows, in.rows); + ASSERT_EQ(out.cols, in.cols); + ASSERT_EQ(out.type(), in.type()); + EXPECT_EQ(cv::countNonZero(out != in), 0); +} + +TEST(MsgConversion, userDataCompressedRoundTrip) +{ + const cv::Mat in = (cv::Mat_(2, 3) << 1, 2, 3, 4, 5, 6); + + rtabmap_msgs::msg::UserData msg; + userDataToROS(in, msg, /*compress=*/true); + + // Compressed payloads travel as a 1xN byte blob. + EXPECT_EQ(msg.rows, 1); + EXPECT_EQ(msg.type, CV_8UC1); + EXPECT_EQ((size_t)msg.cols, msg.data.size()); + + // userDataFromROS hands back the still-compressed blob; the caller uncompresses. + const cv::Mat blob = userDataFromROS(msg); + ASSERT_FALSE(blob.empty()); + const cv::Mat out = rtabmap::uncompressData(blob); + + ASSERT_EQ(out.rows, in.rows); + ASSERT_EQ(out.cols, in.cols); + ASSERT_EQ(out.type(), in.type()); + EXPECT_EQ(cv::countNonZero(out != in), 0); +} + +TEST(MsgConversion, userDataEmpty) +{ + rtabmap_msgs::msg::UserData msg; + userDataToROS(cv::Mat(), msg, /*compress=*/false); + EXPECT_TRUE(msg.data.empty()); + EXPECT_TRUE(userDataFromROS(msg).empty()); +} + +///////////////////////// +// StereoCameraModel +///////////////////////// + +TEST(MsgConversion, stereoCameraModelFromROS) +{ + const double fx = 525.0; + const double baseline = 0.12; + + sensor_msgs::msg::CameraInfo left; + left.width = 640; + left.height = 480; + left.k = {fx, 0.0, 320.0, 0.0, fx, 240.0, 0.0, 0.0, 1.0}; + left.r = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0}; + left.p = {fx, 0.0, 320.0, 0.0, 0.0, fx, 240.0, 0.0, 0.0, 0.0, 1.0, 0.0}; + + // The right camera carries the baseline in P(0,3) = -fx * baseline. + sensor_msgs::msg::CameraInfo right = left; + right.p[3] = -fx * baseline; + + const rtabmap::StereoCameraModel model = stereoCameraModelFromROS( + left, right, rtabmap::Transform::getIdentity()); + + EXPECT_NEAR(model.left().fx(), fx, 1e-9); + EXPECT_NEAR(model.right().fx(), fx, 1e-9); + EXPECT_NEAR(model.baseline(), baseline, 1e-9); + EXPECT_TRUE(model.isValidForProjection()); +} + +///////////////////////// +// OdometryInfo +///////////////////////// + +TEST(MsgConversion, odomInfoRoundTrip) +{ + rtabmap::OdometryInfo in; + in.lost = false; + in.features = 500; + in.localMapSize = 1000; + in.localScanMapSize = 2000; + in.localKeyFrames = 5; + in.keyFrameAdded = true; + in.timeEstimation = 0.02f; + in.interval = 0.033; + in.distanceTravelled = 12.5f; + in.reg.matches = 300; + in.reg.inliers = 250; + in.transform = sampleTransform(); + + rtabmap_msgs::msg::OdomInfo msg; + odomInfoToROS(in, msg); + const rtabmap::OdometryInfo out = odomInfoFromROS(msg); + + EXPECT_EQ(out.lost, in.lost); + EXPECT_EQ(out.features, in.features); + EXPECT_EQ(out.localMapSize, in.localMapSize); + EXPECT_EQ(out.localScanMapSize, in.localScanMapSize); + EXPECT_EQ(out.localKeyFrames, in.localKeyFrames); + EXPECT_EQ(out.keyFrameAdded, in.keyFrameAdded); + EXPECT_FLOAT_EQ(out.timeEstimation, in.timeEstimation); + EXPECT_NEAR(out.interval, in.interval, 1e-6); + EXPECT_FLOAT_EQ(out.distanceTravelled, in.distanceTravelled); + EXPECT_EQ(out.reg.matches, in.reg.matches); + EXPECT_EQ(out.reg.inliers, in.reg.inliers); + expectTransformNear(out.transform, in.transform); +} + +TEST(MsgConversion, odomInfoIgnoreDataDropsHeavyMembers) +{ + rtabmap::OdometryInfo in; + in.features = 500; + in.reg.inliers = 250; + in.words.insert(std::make_pair(1, cv::KeyPoint(cv::Point2f(1, 2), 3))); + in.localMap.insert(std::make_pair(1, cv::Point3f(1, 2, 3))); + + rtabmap_msgs::msg::OdomInfo full; + odomInfoToROS(in, full, /*ignoreData=*/false); + EXPECT_FALSE(full.words_keys.empty()); + + rtabmap_msgs::msg::OdomInfo light; + odomInfoToROS(in, light, /*ignoreData=*/true); + EXPECT_TRUE(light.words_keys.empty()) << "heavy members must be dropped"; + + // The scalar statistics survive either way. + EXPECT_EQ(odomInfoFromROS(light).features, in.features); + EXPECT_EQ(odomInfoFromROS(light).reg.inliers, in.reg.inliers); +} + +TEST(MsgConversion, odomInfoToStatistics) +{ + rtabmap::OdometryInfo info; + info.features = 400; + info.reg.inliers = 100; + info.reg.matches = 200; + info.localMapSize = 1234; + + const std::map stats = odomInfoToStatistics(info); + + ASSERT_TRUE(stats.find("Odometry/Features/") != stats.end()); + EXPECT_FLOAT_EQ(stats.at("Odometry/Features/"), 400.0f); + EXPECT_FLOAT_EQ(stats.at("Odometry/Matches/"), 200.0f); + EXPECT_FLOAT_EQ(stats.at("Odometry/Inliers/"), 100.0f); + EXPECT_FLOAT_EQ(stats.at("Odometry/LocalMapSize/"), 1234.0f); + // MatchesRatio is inliers/features, and must not divide by zero. + EXPECT_FLOAT_EQ(stats.at("Odometry/MatchesRatio/"), 100.0f/400.0f); +} + +TEST(MsgConversion, odomInfoToStatisticsEmptyCovariance) +{ + // RegistrationInfo does not initialize covariance, so a plain OdometryInfo has + // an empty matrix. Reading it must not be attempted. + rtabmap::OdometryInfo info; + ASSERT_TRUE(info.reg.covariance.empty()) << "precondition"; + + const std::map stats = odomInfoToStatistics(info); + + EXPECT_TRUE(stats.find("Odometry/StdDevLin/") == stats.end()) + << "covariance-derived stats must be omitted, not read out of bounds"; + EXPECT_TRUE(stats.find("Odometry/VarianceAng/") == stats.end()); + // The rest of the statistics are still produced. + EXPECT_TRUE(stats.find("Odometry/Features/") != stats.end()); +} + +TEST(MsgConversion, odomInfoToStatisticsWithCovariance) +{ + rtabmap::OdometryInfo info; + info.reg.covariance = cv::Mat::eye(6, 6, CV_64FC1) * 4.0; + + const std::map stats = odomInfoToStatistics(info); + + ASSERT_TRUE(stats.find("Odometry/VarianceLin/") != stats.end()); + EXPECT_FLOAT_EQ(stats.at("Odometry/VarianceLin/"), 4.0f); + EXPECT_FLOAT_EQ(stats.at("Odometry/StdDevLin/"), 2.0f); + EXPECT_FLOAT_EQ(stats.at("Odometry/VarianceAng/"), 4.0f); + EXPECT_FLOAT_EQ(stats.at("Odometry/StdDevAng/"), 2.0f); +} + +TEST(MsgConversion, odomInfoToStatisticsNoFeatures) +{ + rtabmap::OdometryInfo info; + info.features = 0; + info.reg.inliers = 10; + + EXPECT_FLOAT_EQ(odomInfoToStatistics(info).at("Odometry/MatchesRatio/"), 0.0f) + << "must not divide by zero"; +} + +///////////////////////// +// MapGraph / MapData +///////////////////////// + +TEST(MsgConversion, mapGraphRoundTrip) +{ + std::map poses; + poses.insert(std::make_pair(1, rtabmap::Transform(1, 0, 0, 0, 0, 0))); + poses.insert(std::make_pair(2, sampleTransform())); + + std::multimap links; + links.insert(std::make_pair(1, rtabmap::Link( + 1, 2, rtabmap::Link::kNeighbor, sampleTransform(), + cv::Mat::eye(6, 6, CV_64FC1) * 2.0))); + links.insert(std::make_pair(2, rtabmap::Link( + 2, 1, rtabmap::Link::kGlobalClosure, rtabmap::Transform::getIdentity(), + cv::Mat::eye(6, 6, CV_64FC1)))); + + const rtabmap::Transform mapToOdom(0.5f, -0.5f, 0.0f, 0.0f, 0.0f, 0.1f); + + rtabmap_msgs::msg::MapGraph msg; + mapGraphToROS(poses, links, mapToOdom, msg); + ASSERT_EQ(msg.poses.size(), poses.size()); + ASSERT_EQ(msg.poses_id.size(), poses.size()); + ASSERT_EQ(msg.links.size(), links.size()); + + std::map outPoses; + std::multimap outLinks; + rtabmap::Transform outMapToOdom; + mapGraphFromROS(msg, outPoses, outLinks, outMapToOdom); + + ASSERT_EQ(outPoses.size(), poses.size()); + for(std::map::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter) + { + ASSERT_TRUE(outPoses.find(iter->first) != outPoses.end()) << "missing pose " << iter->first; + expectTransformNear(outPoses.at(iter->first), iter->second); + } + + ASSERT_EQ(outLinks.size(), links.size()); + for(std::multimap::const_iterator iter=links.begin(); iter!=links.end(); ++iter) + { + std::multimap::const_iterator found = outLinks.find(iter->first); + ASSERT_TRUE(found != outLinks.end()) << "missing link from " << iter->first; + EXPECT_EQ(found->second.from(), iter->second.from()); + EXPECT_EQ(found->second.to(), iter->second.to()); + EXPECT_EQ(found->second.type(), iter->second.type()); + } + + expectTransformNear(outMapToOdom, mapToOdom); +} + +TEST(MsgConversion, mapGraphEmptyRoundTrip) +{ + rtabmap_msgs::msg::MapGraph msg; + mapGraphToROS(std::map(), std::multimap(), + rtabmap::Transform(), msg); + + EXPECT_TRUE(msg.poses.empty()); + EXPECT_TRUE(msg.links.empty()); + + std::map poses; + std::multimap links; + rtabmap::Transform mapToOdom; + mapGraphFromROS(msg, poses, links, mapToOdom); + + EXPECT_TRUE(poses.empty()); + EXPECT_TRUE(links.empty()); + EXPECT_TRUE(mapToOdom.isNull()) << "a null map_to_odom must survive as null"; +} + +TEST(MsgConversion, mapDataRoundTrip) +{ + std::map poses; + poses.insert(std::make_pair(1, sampleTransform())); + + std::multimap links; + links.insert(std::make_pair(1, rtabmap::Link( + 1, 2, rtabmap::Link::kNeighbor, sampleTransform()))); + + std::map signatures; + rtabmap::Signature sig(1, 0, 3, 1234.5, "my_label", sampleTransform()); + signatures.insert(std::make_pair(1, sig)); + + const rtabmap::Transform mapToOdom = rtabmap::Transform::getIdentity(); + + rtabmap_msgs::msg::MapData msg; + mapDataToROS(poses, links, signatures, mapToOdom, msg); + ASSERT_EQ(msg.nodes.size(), signatures.size()); + ASSERT_EQ(msg.graph.poses.size(), poses.size()); + + std::map outPoses; + std::multimap outLinks; + std::map outSignatures; + rtabmap::Transform outMapToOdom; + mapDataFromROS(msg, outPoses, outLinks, outSignatures, outMapToOdom); + + EXPECT_EQ(outPoses.size(), poses.size()); + EXPECT_EQ(outLinks.size(), links.size()); + ASSERT_EQ(outSignatures.size(), signatures.size()); + ASSERT_TRUE(outSignatures.find(1) != outSignatures.end()); + EXPECT_EQ(outSignatures.at(1).id(), sig.id()); + EXPECT_EQ(outSignatures.at(1).getLabel(), sig.getLabel()); + EXPECT_EQ(outSignatures.at(1).getWeight(), sig.getWeight()); + EXPECT_NEAR(outSignatures.at(1).getStamp(), sig.getStamp(), 1e-6); +} + +///////////////////////// +// Node / Signature +///////////////////////// + +namespace { + +rtabmap::Signature sampleSignature() +{ + rtabmap::Signature s(7, 2, 3, 1234.5, "node_label", sampleTransform()); + + std::multimap words; + std::vector kpts; + std::vector pts3; + cv::Mat descriptors(2, 4, CV_32FC1); + for(int i=0; i<2; ++i) + { + words.insert(std::make_pair(100 + i, i)); + kpts.push_back(cv::KeyPoint(cv::Point2f(10.0f * i, 20.0f * i), 7.0f)); + pts3.push_back(cv::Point3f(1.0f * i, 2.0f * i, 3.0f * i)); + for(int j=0; j<4; ++j) + { + descriptors.at(i, j) = float(i * 4 + j); + } + } + s.setWords(words, kpts, pts3, descriptors); + return s; +} + +} // namespace + +TEST(MsgConversion, nodeRoundTrip) +{ + const rtabmap::Signature in = sampleSignature(); + + rtabmap_msgs::msg::Node msg; + nodeToROS(in, msg); + const rtabmap::Signature out = nodeFromROS(msg); + + EXPECT_EQ(out.id(), in.id()); + EXPECT_EQ(out.mapId(), in.mapId()); + EXPECT_EQ(out.getWeight(), in.getWeight()); + EXPECT_NEAR(out.getStamp(), in.getStamp(), 1e-6); + EXPECT_EQ(out.getLabel(), in.getLabel()); + expectTransformNear(out.getPose(), in.getPose()); + + // Visual words: ids, keypoints, 3D points and descriptors. + ASSERT_EQ(out.getWords().size(), in.getWords().size()); + EXPECT_TRUE(std::equal(out.getWords().begin(), out.getWords().end(), in.getWords().begin())); + + ASSERT_EQ(out.getWordsKpts().size(), in.getWordsKpts().size()); + for(size_t i=0; i(3, 3) << + 525.0, 0.0, 320.0, 0.0, 525.0, 240.0, 0.0, 0.0, 1.0); + const rtabmap::CameraModel model( + "cam", cv::Size(640, 480), K, cv::Mat(), cv::Mat(), cv::Mat(), + rtabmap::Transform(0.0f, 0.0f, 0.1f, 0.0f, 0.0f, 0.0f)); + + rtabmap::SensorData in(cv::Mat(), cv::Mat(), model, 42, 1234.5); + in.setGroundTruth(sampleTransform()); + in.setGPS(rtabmap::GPS(1234.5, -71.9, 45.4, 100.0, 5.0, 90.0)); + + rtabmap_msgs::msg::SensorData msg; + sensorDataToROS(in, msg, "base_link"); + EXPECT_EQ(msg.header.frame_id, "base_link"); + + const rtabmap::SensorData out = sensorDataFromROS(msg); + + EXPECT_NEAR(out.stamp(), in.stamp(), 1e-6); + + // sensorDataToROS writes ground_truth_pose into the message, but sensorDataFromROS + // deliberately does not read it back: the ground truth is owned by the enclosing + // Node conversion (nodeFromROS feeds it to the Signature constructor). See + // nodeGroundTruthRoundTrip for the round trip that does preserve it. + EXPECT_FALSE(transformFromPoseMsg(msg.ground_truth_pose).isNull()) + << "the message must still carry the ground truth for nodeFromROS"; + EXPECT_TRUE(out.groundTruth().isNull()) + << "sensorDataFromROS does not restore the ground truth"; + + ASSERT_EQ(out.cameraModels().size(), 1u); + EXPECT_NEAR(out.cameraModels()[0].fx(), 525.0, 1e-9); + EXPECT_NEAR(out.cameraModels()[0].cx(), 320.0, 1e-9); + expectTransformNear( + out.cameraModels()[0].localTransform(), model.localTransform()); + + EXPECT_NEAR(out.gps().longitude(), in.gps().longitude(), 1e-9); + EXPECT_NEAR(out.gps().latitude(), in.gps().latitude(), 1e-9); + EXPECT_NEAR(out.gps().altitude(), in.gps().altitude(), 1e-9); + EXPECT_NEAR(out.gps().bearing(), in.gps().bearing(), 1e-9); +} + +TEST(MsgConversion, sensorDataUserDataRoundTrip) +{ + rtabmap::SensorData in; + in.setStamp(10.0); + in.setUserData((cv::Mat_(1, 3) << 7, 8, 9)); + + rtabmap_msgs::msg::SensorData msg; + sensorDataToROS(in, msg); + const rtabmap::SensorData out = sensorDataFromROS(msg); + + const cv::Mat data = out.userDataRaw().empty() + ? rtabmap::uncompressData(out.userDataCompressed()) + : out.userDataRaw(); + ASSERT_FALSE(data.empty()); + ASSERT_EQ(data.cols, 3); + EXPECT_EQ(data.at(0, 0), 7); + EXPECT_EQ(data.at(0, 2), 9); +} + +///////////////////////// +// Statistics / Info +///////////////////////// + +TEST(MsgConversion, infoRoundTrip) +{ + rtabmap::Statistics in; + in.setExtended(true); + in.setRefImageId(5); + in.setLoopClosureId(9); + in.setProximityDetectionId(11); + in.setStamp(1234.5); + in.setLoopClosureTransform(sampleTransform()); + in.setWmState(std::vector{1, 2, 3}); + + std::map posterior; + posterior.insert(std::make_pair(1, 0.25f)); + posterior.insert(std::make_pair(2, 0.75f)); + in.setPosterior(posterior); + + std::map weights; + weights.insert(std::make_pair(1, 10)); + in.setWeights(weights); + + std::map labels; + labels.insert(std::make_pair(1, "kitchen")); + in.setLabels(labels); + + in.addStatistic("Some/Stat/", 3.5f); + + rtabmap_msgs::msg::Info msg; + infoToROS(in, msg); + + // An unstamped header is filled from the statistics, so infoFromROS recovers the + // stamp without the caller doing anything. Only to double precision, though. + EXPECT_NEAR(timestampFromROS(msg.header.stamp), in.stamp(), 1e-6); + EXPECT_TRUE(msg.header.frame_id.empty()) << "the frame id is always the caller's job"; + + rtabmap::Statistics out; + infoFromROS(msg, out); + + EXPECT_EQ(out.refImageId(), in.refImageId()); + EXPECT_EQ(out.loopClosureId(), in.loopClosureId()); + EXPECT_EQ(out.proximityDetectionId(), in.proximityDetectionId()); + EXPECT_NEAR(out.stamp(), in.stamp(), 1e-6); + expectTransformNear(out.loopClosureTransform(), in.loopClosureTransform()); + EXPECT_EQ(out.wmState(), in.wmState()); + + ASSERT_EQ(out.posterior().size(), in.posterior().size()); + EXPECT_FLOAT_EQ(out.posterior().at(1), 0.25f); + EXPECT_FLOAT_EQ(out.posterior().at(2), 0.75f); + + ASSERT_EQ(out.weights().size(), in.weights().size()); + EXPECT_EQ(out.weights().at(1), 10); + + ASSERT_EQ(out.labels().size(), in.labels().size()); + EXPECT_EQ(out.labels().at(1), "kitchen"); + + ASSERT_TRUE(out.data().find("Some/Stat/") != out.data().end()); + EXPECT_FLOAT_EQ(out.data().at("Some/Stat/"), 3.5f); +} + +///////////////////////// +// PointCloud2 helpers +///////////////////////// + +namespace { + +/// Builds a dense, unorganized XYZ float cloud from the given points. +sensor_msgs::msg::PointCloud2 makeXYZCloud(const std::vector & points) +{ + sensor_msgs::msg::PointCloud2 cloud; + cloud.height = 1; + cloud.width = points.size(); + cloud.is_bigendian = false; + cloud.is_dense = true; + cloud.fields.resize(3); + const char * names[3] = {"x", "y", "z"}; + for(int i=0; i<3; ++i) + { + cloud.fields[i].name = names[i]; + cloud.fields[i].offset = 4 * i; + cloud.fields[i].datatype = sensor_msgs::msg::PointField::FLOAT32; + cloud.fields[i].count = 1; + } + cloud.point_step = 12; + cloud.row_step = cloud.point_step * cloud.width; + cloud.data.resize(cloud.row_step * cloud.height); + for(size_t i=0; i(&cloud.data[i * cloud.point_step]); + p[0] = points[i].x; + p[1] = points[i].y; + p[2] = points[i].z; + } + return cloud; +} + +cv::Point3f readXYZ(const sensor_msgs::msg::PointCloud2 & cloud, size_t index) +{ + const float * p = reinterpret_cast(&cloud.data[index * cloud.point_step]); + return cv::Point3f(p[0], p[1], p[2]); +} + +} // namespace + +TEST(MsgConversion, transformPointCloudTranslation) +{ + const std::vector points = {{1.0f, 2.0f, 3.0f}, {-1.0f, 0.0f, 1.0f}}; + const sensor_msgs::msg::PointCloud2 in = makeXYZCloud(points); + + Eigen::Matrix4f t = Eigen::Matrix4f::Identity(); + t(0, 3) = 10.0f; + t(1, 3) = 20.0f; + t(2, 3) = 30.0f; + + sensor_msgs::msg::PointCloud2 out; + transformPointCloud(t, in, out); + + ASSERT_EQ(out.width, in.width); + ASSERT_EQ(out.point_step, in.point_step); + for(size_t i=0; i points = {{1.0f, 0.0f, 0.0f}}; + const sensor_msgs::msg::PointCloud2 in = makeXYZCloud(points); + + const Eigen::Matrix4f t = + rtabmap::Transform(0, 0, 0, 0, 0, M_PI/2.0).toEigen4f(); + + sensor_msgs::msg::PointCloud2 out; + transformPointCloud(t, in, out); + + const cv::Point3f p = readXYZ(out, 0); + EXPECT_NEAR(p.x, 0.0f, 1e-5); + EXPECT_NEAR(p.y, 1.0f, 1e-5); + EXPECT_NEAR(p.z, 0.0f, 1e-5); +} + +TEST(MsgConversion, transformPointCloudIdentityPreservesMetadata) +{ + const sensor_msgs::msg::PointCloud2 in = makeXYZCloud({{1.0f, 2.0f, 3.0f}}); + + sensor_msgs::msg::PointCloud2 out; + transformPointCloud(Eigen::Matrix4f::Identity(), in, out); + + EXPECT_EQ(out.height, in.height); + EXPECT_EQ(out.width, in.width); + EXPECT_EQ(out.point_step, in.point_step); + EXPECT_EQ(out.row_step, in.row_step); + EXPECT_EQ(out.is_dense, in.is_dense); + ASSERT_EQ(out.fields.size(), in.fields.size()); + for(size_t i=0; i last +constexpr float kWallDistance = 5.0f; // m, distance to the wall at the first point +constexpr float kSpeed = 1.0f; // m/s forward (+x) + +/// How the per-point time channel is encoded. deskew() accepts three datatypes, and +/// FLOAT64 differs from the other two: it carries ABSOLUTE stamps (with an automatic +/// ms/us/ns unit guess), while UINT32 and FLOAT32 carry offsets from the header stamp. +enum TimeEncoding +{ + kOffsetSecFloat32, ///< FLOAT32 seconds, relative to header.stamp + kOffsetNsecUint32, ///< UINT32 nanoseconds, relative to header.stamp + kAbsoluteSecFloat64, ///< FLOAT64 absolute seconds + kAbsoluteMsecFloat64 ///< FLOAT64 absolute milliseconds (auto-scaled by deskew) +}; + +/// Organized-cloud layout. deskew() picks its traversal from width>height, so the two +/// orderings exercise different loops. +enum ScanLayout +{ + kTimeOnColumns, ///< Ouster style: width=time samples, height=rings + kTimeOnRows ///< Velodyne style: height=time samples, width=rings +}; + +/** + * Builds the raw (skewed) scan of a flat wall captured while moving forward. + * + * Each time sample is taken 1 ms after the previous one, by which time the robot has + * closed in on the wall by kSpeed * elapsed. Expressed in the sensor frame at capture + * time, the wall therefore appears to slide towards the robot: a straight wall is + * recorded as a slanted line. Deskewing must undo exactly that. + * + * @param headerStamp absolute stamp put in the message header + * @param firstPointOffset time of the first sample relative to the header stamp + * @param encoding how to write the time channel + * @param layout whether time runs along columns or rows + * @param rings number of rings (the non-time dimension) + * @param fieldName name of the time channel + * @param descendingTime emit the samples newest-first, which deskew has to detect + * @param displacement distance travelled as a function of time since the first + * sample; defaults to the constant-velocity kSpeed * elapsed + */ +sensor_msgs::msg::PointCloud2 makeSkewedWallScan( + double headerStamp, + double firstPointOffset, + TimeEncoding encoding = kOffsetSecFloat32, + ScanLayout layout = kTimeOnColumns, + size_t rings = 1, + const std::string & fieldName = "t", + bool descendingTime = false, + const std::function & displacement = nullptr) +{ + const bool timeIs64Bit = + encoding == kAbsoluteSecFloat64 || encoding == kAbsoluteMsecFloat64; + // Keep the 8-byte time channel aligned: x,y,z then 4 bytes of padding. + const uint32_t timeOffset = timeIs64Bit ? 16 : 12; + const uint32_t pointStep = timeIs64Bit ? 24 : 16; + + sensor_msgs::msg::PointCloud2 cloud; + cloud.header.stamp = timestampToROS(headerStamp); + cloud.header.frame_id = "base_link"; + cloud.is_bigendian = false; + cloud.is_dense = true; + if(layout == kTimeOnColumns) + { + cloud.width = kScanPoints; + cloud.height = rings; + } + else + { + cloud.width = rings; + cloud.height = kScanPoints; + } + + cloud.fields.resize(4); + const char * xyz[3] = {"x", "y", "z"}; + for(int i=0; i<3; ++i) + { + cloud.fields[i].name = xyz[i]; + cloud.fields[i].offset = 4 * i; + cloud.fields[i].datatype = sensor_msgs::msg::PointField::FLOAT32; + cloud.fields[i].count = 1; + } + cloud.fields[3].name = fieldName; + cloud.fields[3].offset = timeOffset; + cloud.fields[3].datatype = + encoding == kOffsetNsecUint32 ? sensor_msgs::msg::PointField::UINT32 : + timeIs64Bit ? sensor_msgs::msg::PointField::FLOAT64 : + sensor_msgs::msg::PointField::FLOAT32; + cloud.fields[3].count = 1; + + cloud.point_step = pointStep; + cloud.row_step = cloud.point_step * cloud.width; + cloud.data.resize(cloud.row_step * cloud.height); + + for(size_t i=0; i(base); + // The robot has closed in on the wall by this much when the sample was taken. + const double travelled = displacement ? displacement(elapsed) : kSpeed * elapsed; + p[0] = kWallDistance - float(travelled); // the skew + p[1] = -1.0f + 2.0f * float(sample) / float(kScanPoints - 1); + p[2] = 0.1f * float(r); // one plane per ring + + switch(encoding) + { + case kOffsetSecFloat32: + *reinterpret_cast(base + timeOffset) = float(offset); + break; + case kOffsetNsecUint32: + *reinterpret_cast(base + timeOffset) = + uint32_t(std::llround(offset * 1e9)); + break; + case kAbsoluteSecFloat64: + *reinterpret_cast(base + timeOffset) = absolute; + break; + case kAbsoluteMsecFloat64: + *reinterpret_cast(base + timeOffset) = absolute * 1e3; + break; + } + } + } + return cloud; +} + +/// Reads x of the point at (time sample, ring) for the given layout. +float readWallX(const sensor_msgs::msg::PointCloud2 & cloud, size_t sample, size_t ring, + ScanLayout layout) +{ + const size_t row = (layout == kTimeOnColumns) ? ring : sample; + const size_t col = (layout == kTimeOnColumns) ? sample : ring; + return *reinterpret_cast( + &cloud.data[row * cloud.row_step + col * cloud.point_step]); +} + +float readField(const sensor_msgs::msg::PointCloud2 & cloud, size_t index, size_t field) +{ + return *reinterpret_cast( + &cloud.data[index * cloud.point_step + cloud.fields[field].offset]); +} + +} // namespace + +TEST(MsgConversion, deskewConstantVelocityHeaderAtFirstPoint) +{ + const double firstPointStamp = 1000.0; + + // Header stamped at the first point, so "t" runs 0 .. +0.100 s. + const sensor_msgs::msg::PointCloud2 in = makeSkewedWallScan(firstPointStamp, 0.0); + ASSERT_NEAR(readField(in, 0, 0), kWallDistance, 1e-4) << "first point is unskewed"; + ASSERT_NEAR(readField(in, kScanPoints-1, 0), kWallDistance - float(kScanSpan), 1e-4) + << "last point is skewed by v*0.099s = 9.9 cm"; + + sensor_msgs::msg::PointCloud2 out; + ASSERT_TRUE(deskew(in, out, rtabmap::Transform(kSpeed, 0, 0, 0, 0, 0))); + + // Everything collapses back onto the wall at its original distance. + for(size_t i=0; i height=4 rings. + expectDeskewRecoversWall(kOffsetSecFloat32, kTimeOnColumns, 4, 1000.0); +} + +TEST(MsgConversion, deskewTimeOnRowsWithMultipleRings) +{ + // Velodyne layout: height=101 samples > width=4 rings, which takes the other loop. + expectDeskewRecoversWall(kOffsetSecFloat32, kTimeOnRows, 4, 1000.0); +} + +TEST(MsgConversion, deskewLayoutsAgree) +{ + // The same scan expressed in either layout must deskew to the same geometry. + const double headerStamp = 1000.0; + const size_t rings = 4; + + sensor_msgs::msg::PointCloud2 byColumns, byRows; + ASSERT_TRUE(deskew(makeSkewedWallScan(headerStamp, 0.0, kOffsetSecFloat32, kTimeOnColumns, rings), + byColumns, rtabmap::Transform(kSpeed, 0, 0, 0, 0, 0))); + ASSERT_TRUE(deskew(makeSkewedWallScan(headerStamp, 0.0, kOffsetSecFloat32, kTimeOnRows, rings), + byRows, rtabmap::Transform(kSpeed, 0, 0, 0, 0, 0))); + + for(size_t i=0; i(&c.data[i * c.point_step]); + return std::make_pair(p[0], p[1]); + }; + + for(size_t i=0; i a = xy(in, i); + const std::pair b = xy(out, i); + const double dt = double(i) * kScanStep; // sample 0 sits at the header stamp + + EXPECT_NEAR(std::hypot(b.first, b.second), std::hypot(a.first, a.second), 1e-4) + << "a rotation must preserve the range of sample " << i; + EXPECT_NEAR(std::atan2(b.second, b.first) - std::atan2(a.second, a.first), + yawRate * dt, 1e-4) + << "sample " << i << " must be rotated by yawRate*dt"; + } + + // Spelling out the i=0 case: dt is zero there, so that sample is untouched. + EXPECT_FLOAT_EQ(xy(out, 0).first, xy(in, 0).first); + EXPECT_FLOAT_EQ(xy(out, 0).second, xy(in, 0).second); +} + +TEST(MsgConversion, deskewPassesThroughWhenThereIsNoTimeSpread) +{ + // A driver that leaves the time channel at zero gives a scan with no time spread. + // There is nothing to correct, so the cloud must come back unchanged rather than + // being reported as a failure -- callers abort the frame on false. + sensor_msgs::msg::PointCloud2 in = makeSkewedWallScan(1000.0, 0.0); + for(size_t i=0; i(&in.data[i * in.point_step + in.fields[3].offset]) = 0.0f; + } + + sensor_msgs::msg::PointCloud2 out; + ASSERT_TRUE(deskew(in, out, rtabmap::Transform(kSpeed, 0, 0, 0, 0, 0))); + EXPECT_EQ(out.data, in.data) << "the cloud must be returned untouched"; +} + +TEST(MsgConversion, deskewIsIdempotent) +{ + // Deskewing zeroes the time channel to mark the cloud as done, so running deskew a + // second time (e.g. lidar_deskewing feeding icp_odometry) must be a silent no-op. + const sensor_msgs::msg::PointCloud2 in = makeSkewedWallScan(1000.0, 0.0); + const rtabmap::Transform velocity(kSpeed, 0, 0, 0, 0, 0); + + sensor_msgs::msg::PointCloud2 once; + ASSERT_TRUE(deskew(in, once, velocity)); + for(size_t i=0; i( + &once.data[i * once.point_step + once.fields[3].offset]), 0.0f) + << "deskewing must zero the time channel, sample " << i; + } + + sensor_msgs::msg::PointCloud2 twice; + ASSERT_TRUE(deskew(once, twice, velocity)) << "a second pass must not fail"; + EXPECT_EQ(twice.data, once.data) << "a second pass must change nothing"; +} + +TEST(MsgConversion, deskewClampsSamplesOutsideTheSweep) +{ + // The ordering check only inspects the first and last samples, so a corrupt stamp in + // the middle is not detected. It must be clamped to the end of the sweep rather than + // extrapolated, which would fling the point far past the wall. + const double headerStamp = 1000.0; + sensor_msgs::msg::PointCloud2 in = makeSkewedWallScan(headerStamp, 0.0); + const size_t corrupt = kScanPoints / 2; + *reinterpret_cast( + &in.data[corrupt * in.point_step + in.fields[3].offset]) = 0.5f; // 5x the sweep + + sensor_msgs::msg::PointCloud2 out; + ASSERT_TRUE(deskew(in, out, rtabmap::Transform(kSpeed, 0, 0, 0, 0, 0))); + + // Clamped to the last sample's correction, so it lands within the sweep's own range + // rather than metres away. Every other sample is unaffected. + const float x = readWallX(out, corrupt, 0, kTimeOnColumns); + EXPECT_GE(x, kWallDistance - 1e-3f); + EXPECT_LE(x, kWallDistance + float(kSpeed * kScanSpan) + 1e-3f) + << "an unclamped ratio of ~5 would put this point ~0.45 m past the wall"; + + for(size_t i=0; i(3, 3) << + 525.0, 0.0, 320.0, 0.0, 525.0, 240.0, 0.0, 0.0, 1.0); + const rtabmap::CameraModel model( + "cam", cv::Size(4, 4), K, cv::Mat(), cv::Mat(), cv::Mat(), + rtabmap::Transform(0.0f, 0.0f, 0.1f, 0.0f, 0.0f, 0.0f)); + + cv::Mat rgb(4, 4, CV_8UC3, cv::Scalar(10, 20, 30)); + cv::Mat depth(4, 4, CV_16UC1, cv::Scalar(1000)); + rtabmap::SensorData in(rgb, depth, model, 1, 1234.5); + + rtabmap_msgs::msg::RGBDImage msg; + rgbdImageToROS(in, msg, "camera_link"); + + EXPECT_EQ(msg.rgb_camera_info.header.frame_id, "camera_link"); + EXPECT_NEAR(timestampFromROS(msg.rgb_camera_info.header.stamp), 1234.5, 1e-6); + + // The top-level header is stamped too, so rgbdImageFromROS recovers the stamp + // without the caller having to fill it in. + EXPECT_EQ(msg.header.frame_id, "camera_link"); + EXPECT_NEAR(timestampFromROS(msg.header.stamp), 1234.5, 1e-6); + + // The returned SensorData shallow-references the message buffers, so the message + // must outlive it -- see rgbdImageFromROSAliasesTheMessage. + const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr held = + std::make_shared(msg); + const rtabmap::SensorData out = rgbdImageFromROS(held); + + EXPECT_NEAR(out.stamp(), in.stamp(), 1e-6); + ASSERT_EQ(out.cameraModels().size(), 1u); + EXPECT_NEAR(out.cameraModels()[0].fx(), 525.0, 1e-9); + + // The local transform is not carried by the message (CameraInfo has no such + // field); callers resolve it from TF, so it comes back as the default identity. + EXPECT_TRUE(out.cameraModels()[0].localTransform().isIdentity()) + << out.cameraModels()[0].localTransform().prettyPrint(); + + ASSERT_FALSE(out.imageRaw().empty()); + EXPECT_EQ(out.imageRaw().type(), CV_8UC3); + EXPECT_EQ(cv::countNonZero(out.imageRaw().reshape(1) != rgb.reshape(1)), 0); + + ASSERT_FALSE(out.depthRaw().empty()); + EXPECT_EQ(out.depthRaw().type(), CV_16UC1); + EXPECT_EQ(cv::countNonZero(out.depthRaw() != depth), 0); +} + +TEST(MsgConversion, rgbdImageFromROSAliasesTheMessage) +{ + // rgbdImageFromROS deliberately avoids copying the pixels: the SensorData it returns + // points into the message's own buffers. Mutating the message is visible through the + // SensorData. Callers must therefore keep the message alive and unchanged for as long + // as they use the result -- and must deep-copy before letting the SensorData outlive + // the subscription callback, since the ROS queue recycles the message once it + // returns. + cv::Mat rgb(4, 4, CV_8UC3, cv::Scalar(10, 20, 30)); + cv::Mat depth(4, 4, CV_16UC1, cv::Scalar(1000)); + + auto msg = std::make_shared(); + msg->rgb_camera_info.width = 4; + msg->rgb_camera_info.height = 4; + msg->rgb_camera_info.k = {525.0, 0.0, 2.0, 0.0, 525.0, 2.0, 0.0, 0.0, 1.0}; + cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", rgb).toImageMsg(msg->rgb); + cv_bridge::CvImage(std_msgs::msg::Header(), "16UC1", depth).toImageMsg(msg->depth); + + const rtabmap::SensorData data = rgbdImageFromROS(msg); + ASSERT_FALSE(data.imageRaw().empty()); + ASSERT_EQ(data.imageRaw().at(0, 0), cv::Vec3b(10, 20, 30)); + + // Writing through the message is observable in the SensorData: no copy was made. + msg->rgb.data[0] = 99; + EXPECT_EQ(data.imageRaw().at(0, 0)[0], 99) + << "SensorData is expected to alias the message buffer"; +} + +TEST(MsgConversion, toCvCopyReadsRawImages) +{ + cv::Mat rgb(4, 4, CV_8UC3, cv::Scalar(10, 20, 30)); + cv::Mat depth(4, 4, CV_16UC1, cv::Scalar(1000)); + + rtabmap_msgs::msg::RGBDImage msg; + cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", rgb).toImageMsg(msg.rgb); + cv_bridge::CvImage(std_msgs::msg::Header(), "16UC1", depth).toImageMsg(msg.depth); + + cv_bridge::CvImagePtr rgbPtr, depthPtr; + toCvCopy(msg, rgbPtr, depthPtr); + + ASSERT_TRUE(rgbPtr && depthPtr); + EXPECT_EQ(cv::countNonZero(rgbPtr->image.reshape(1) != rgb.reshape(1)), 0); + EXPECT_EQ(cv::countNonZero(depthPtr->image != depth), 0); + + // The copy must be independent of the message buffer. + rgbPtr->image.at(0, 0) = cv::Vec3b(0, 0, 0); + EXPECT_EQ(rgb.at(0, 0), cv::Vec3b(10, 20, 30)); +} + +TEST(MsgConversion, toCvCopyEmptyImageYieldsEmptyPtr) +{ + rtabmap_msgs::msg::RGBDImage msg; + + cv_bridge::CvImagePtr rgbPtr, depthPtr; + toCvCopy(msg, rgbPtr, depthPtr); + + ASSERT_TRUE(rgbPtr && depthPtr) << "pointers must be valid even with no image"; + EXPECT_TRUE(rgbPtr->image.empty()); + EXPECT_TRUE(depthPtr->image.empty()); +} + +TEST(MsgConversion, toCvShareAliasesRawImages) +{ + cv::Mat rgb(4, 4, CV_8UC3, cv::Scalar(10, 20, 30)); + cv::Mat depth(4, 4, CV_16UC1, cv::Scalar(1000)); + + rtabmap_msgs::msg::RGBDImage msg; + cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", rgb).toImageMsg(msg.rgb); + cv_bridge::CvImage(std_msgs::msg::Header(), "16UC1", depth).toImageMsg(msg.depth); + + cv_bridge::CvImageConstPtr rgbPtr, depthPtr; + toCvShare(msg, std::shared_ptr(), rgbPtr, depthPtr); + + ASSERT_TRUE(rgbPtr && depthPtr); + ASSERT_FALSE(rgbPtr->image.empty()); + EXPECT_EQ(cv::countNonZero(rgbPtr->image.reshape(1) != rgb.reshape(1)), 0); + EXPECT_EQ(cv::countNonZero(depthPtr->image != depth), 0); +} + +///////////////////////// +// Compressed images +///////////////////////// + +namespace { + +/// Builds a depth image compressed the way rtabmap does it (not a jpg/png CompressedImage). +sensor_msgs::msg::CompressedImage makeRtabmapCompressedDepth(const cv::Mat & depth) +{ + sensor_msgs::msg::CompressedImage msg; + msg.format = ""; // anything but "jpg" takes the rtabmap::uncompressImage path + msg.data = rtabmap::compressImage(depth, ".png"); + return msg; +} + +} // namespace + +TEST(MsgConversion, toCvCopyReadsCompressedDepth) +{ + const cv::Mat depth(4, 4, CV_16UC1, cv::Scalar(1234)); + + rtabmap_msgs::msg::RGBDImage msg; + msg.depth_compressed = makeRtabmapCompressedDepth(depth); + + cv_bridge::CvImagePtr rgbPtr, depthPtr; + toCvCopy(msg, rgbPtr, depthPtr); + + ASSERT_TRUE(depthPtr); + ASSERT_FALSE(depthPtr->image.empty()); + EXPECT_EQ(depthPtr->image.type(), CV_16UC1); + EXPECT_EQ(depthPtr->encoding, sensor_msgs::image_encodings::TYPE_16UC1); + EXPECT_EQ(cv::countNonZero(depthPtr->image != depth), 0); +} + +TEST(MsgConversion, toCvShareReadsCompressedDepth) +{ + const cv::Mat depth(4, 4, CV_32FC1, cv::Scalar(1.5f)); + + rtabmap_msgs::msg::RGBDImage msg; + msg.depth_compressed = makeRtabmapCompressedDepth(depth); + + cv_bridge::CvImageConstPtr rgbPtr, depthPtr; + toCvShare(msg, std::shared_ptr(), rgbPtr, depthPtr); + + ASSERT_TRUE(depthPtr); + ASSERT_FALSE(depthPtr->image.empty()); + EXPECT_EQ(depthPtr->image.type(), CV_32FC1); + EXPECT_EQ(depthPtr->encoding, sensor_msgs::image_encodings::TYPE_32FC1); + EXPECT_EQ(cv::countNonZero(depthPtr->image != depth), 0); +} + +TEST(MsgConversion, toCvCopyReadsCompressedRgb) +{ + const cv::Mat rgb(8, 8, CV_8UC3, cv::Scalar(10, 20, 30)); + + rtabmap_msgs::msg::RGBDImage msg; + msg.rgb_compressed.format = "png"; + msg.rgb_compressed.data = rtabmap::compressImage(rgb, ".png"); + + cv_bridge::CvImagePtr rgbPtr, depthPtr; + toCvCopy(msg, rgbPtr, depthPtr); + + ASSERT_TRUE(rgbPtr); + ASSERT_FALSE(rgbPtr->image.empty()); + EXPECT_EQ(rgbPtr->image.type(), CV_8UC3); + EXPECT_EQ(cv::countNonZero(rgbPtr->image.reshape(1) != rgb.reshape(1)), 0); +} + +///////////////////////// +// SensorData: raw copies, laser scans, stereo +///////////////////////// + +TEST(MsgConversion, sensorDataToROSCopyRawDataCarriesImages) +{ + cv::Mat K = (cv::Mat_(3, 3) << + 525.0, 0.0, 4.0, 0.0, 525.0, 4.0, 0.0, 0.0, 1.0); + const rtabmap::CameraModel model("cam", cv::Size(8, 8), K, cv::Mat(), cv::Mat(), cv::Mat()); + + const cv::Mat rgb(8, 8, CV_8UC3, cv::Scalar(10, 20, 30)); + const cv::Mat depth(8, 8, CV_16UC1, cv::Scalar(2000)); + rtabmap::SensorData in(rgb, depth, model, 1, 1000.0); + + // Without copyRawData the raw images are not serialized... + rtabmap_msgs::msg::SensorData without; + sensorDataToROS(in, without, "base_link", /*copyRawData=*/false); + EXPECT_TRUE(without.left.data.empty()); + EXPECT_TRUE(without.right.data.empty()); + + // ...with it, they are. + rtabmap_msgs::msg::SensorData with; + sensorDataToROS(in, with, "base_link", /*copyRawData=*/true); + ASSERT_FALSE(with.left.data.empty()); + ASSERT_FALSE(with.right.data.empty()); + EXPECT_EQ(with.left.encoding, sensor_msgs::image_encodings::BGR8); + EXPECT_EQ(with.right.encoding, sensor_msgs::image_encodings::TYPE_16UC1); + + const rtabmap::SensorData out = sensorDataFromROS(with); + ASSERT_FALSE(out.imageRaw().empty()); + EXPECT_EQ(cv::countNonZero(out.imageRaw().reshape(1) != rgb.reshape(1)), 0); + ASSERT_FALSE(out.depthRaw().empty()); + EXPECT_EQ(cv::countNonZero(out.depthRaw() != depth), 0); +} + +TEST(MsgConversion, sensorDataLaserScanRoundTrip) +{ + cv::Mat points(1, 3, CV_32FC3); + points.at(0, 0) = cv::Vec3f(1.0f, 0.0f, 0.0f); + points.at(0, 1) = cv::Vec3f(0.0f, 2.0f, 0.0f); + points.at(0, 2) = cv::Vec3f(0.0f, 0.0f, 3.0f); + + const rtabmap::Transform localTransform(0.0f, 0.0f, 0.3f, 0.0f, 0.0f, 0.0f); + const rtabmap::LaserScan scan(points, /*maxPoints=*/100, /*maxRange=*/40.0f, + rtabmap::LaserScan::kXYZ, localTransform); + + rtabmap::SensorData in; + in.setStamp(1000.0); + in.setLaserScan(scan); + + rtabmap_msgs::msg::SensorData msg; + sensorDataToROS(in, msg, "base_link", /*copyRawData=*/true); + + EXPECT_EQ(msg.laser_scan_max_pts, 100); + EXPECT_FLOAT_EQ(msg.laser_scan_max_range, 40.0f); + EXPECT_EQ(msg.laser_scan_format, (int)rtabmap::LaserScan::kXYZ); + expectTransformNear(transformFromGeometryMsg(msg.laser_scan_local_transform), + localTransform, 1e-4f); + + const rtabmap::SensorData out = sensorDataFromROS(msg); + const rtabmap::LaserScan & outScan = out.laserScanRaw().empty() + ? out.laserScanCompressed() : out.laserScanRaw(); + EXPECT_EQ(outScan.size(), scan.size()); + EXPECT_EQ(outScan.maxPoints(), scan.maxPoints()); + EXPECT_FLOAT_EQ(outScan.rangeMax(), scan.rangeMax()); + expectTransformNear(outScan.localTransform(), localTransform, 1e-4f); +} + +TEST(MsgConversion, sensorDataStereoModelRoundTrip) +{ + const double fx = 525.0; + const double baseline = 0.12; + const rtabmap::Transform localTransform(0.0f, 0.0f, 0.1f, 0.0f, 0.0f, 0.0f); + + const rtabmap::StereoCameraModel stereo( + fx, fx, 320.0, 240.0, baseline, localTransform, cv::Size(640, 480)); + ASSERT_TRUE(stereo.isValidForProjection()) << "precondition"; + + rtabmap::SensorData in; + in.setStamp(1000.0); + in.setStereoImage(cv::Mat(), cv::Mat(), stereo); + + rtabmap_msgs::msg::SensorData msg; + sensorDataToROS(in, msg, "base_link"); + + // The stereo branch fills BOTH camera infos, unlike the monocular one. + ASSERT_EQ(msg.left_camera_info.size(), 1u); + ASSERT_EQ(msg.right_camera_info.size(), 1u); + + const rtabmap::SensorData out = sensorDataFromROS(msg); + ASSERT_EQ(out.stereoCameraModels().size(), 1u); + EXPECT_TRUE(out.cameraModels().empty()) << "must not be read back as monocular"; + EXPECT_NEAR(out.stereoCameraModels()[0].left().fx(), fx, 1e-9); + EXPECT_NEAR(out.stereoCameraModels()[0].baseline(), baseline, 1e-6); + expectTransformNear(out.stereoCameraModels()[0].localTransform(), localTransform, 1e-4f); +} + +TEST(MsgConversion, nodeWithStereoModelRoundTrip) +{ + const rtabmap::StereoCameraModel stereo( + 525.0, 525.0, 320.0, 240.0, 0.12, + rtabmap::Transform::getIdentity(), cv::Size(640, 480)); + + rtabmap::Signature in(3, 0, 1, 1000.0, "stereo_node", sampleTransform()); + in.sensorData().setStereoImage(cv::Mat(), cv::Mat(), stereo); + + rtabmap_msgs::msg::Node msg; + nodeToROS(in, msg); + const rtabmap::Signature out = nodeFromROS(msg); + + EXPECT_EQ(out.id(), in.id()); + ASSERT_EQ(out.sensorData().stereoCameraModels().size(), 1u); + EXPECT_NEAR(out.sensorData().stereoCameraModels()[0].baseline(), 0.12, 1e-6); +} + +TEST(MsgConversion, infoToROSKeepsACallerSuppliedStamp) +{ + // CoreWrapper stamps the message before calling infoToROS, sometimes with a + // publication time unrelated to the data. That must not be overwritten. + rtabmap::Statistics in; + in.setExtended(true); + in.setStamp(1234.5); + + rtabmap_msgs::msg::Info msg; + msg.header.stamp = timestampToROS(9999.0); + msg.header.frame_id = "map"; + infoToROS(in, msg); + + EXPECT_NEAR(timestampFromROS(msg.header.stamp), 9999.0, 1e-6) + << "a caller-supplied stamp must win over the statistics stamp"; + EXPECT_EQ(msg.header.frame_id, "map"); +} + +TEST(MsgConversion, infoOdomCacheRoundTrip) +{ + // Statistics carries a whole MapGraph for the odometry cache in localization mode. + std::map poses; + poses.insert(std::make_pair(1, sampleTransform())); + poses.insert(std::make_pair(2, rtabmap::Transform(1, 2, 3, 0, 0, 0))); + + std::multimap links; + links.insert(std::make_pair(1, rtabmap::Link( + 1, 2, rtabmap::Link::kNeighbor, sampleTransform()))); + + rtabmap::Statistics in; + in.setExtended(true); + in.setOdomCachePoses(poses); + in.setOdomCacheConstraints(links); + + rtabmap_msgs::msg::Info msg; + infoToROS(in, msg); + ASSERT_EQ(msg.odom_cache.poses.size(), poses.size()); + ASSERT_EQ(msg.odom_cache.links.size(), links.size()); + + rtabmap::Statistics out; + infoFromROS(msg, out); + + ASSERT_EQ(out.odomCachePoses().size(), poses.size()); + expectTransformNear(out.odomCachePoses().at(1), poses.at(1)); + expectTransformNear(out.odomCachePoses().at(2), poses.at(2)); + EXPECT_EQ(out.odomCacheConstraints().size(), links.size()); +} + +///////////////////////// +// TF-based conversions +///////////////////////// + +namespace { + +/// A tf2 buffer needs a clock, but neither a node nor a listener: transforms can be +/// injected directly, which makes every TF-based conversion an ordinary unit test. +std::shared_ptr makeTfBuffer() +{ + std::shared_ptr buffer = + std::make_shared(std::make_shared(RCL_ROS_TIME)); + // Transforms are injected synchronously before the lookups, so tell tf2 not to warn + // about waiting for a listener thread that will never exist. + buffer->setUsingDedicatedThread(true); + return buffer; +} + +void addTf(tf2_ros::Buffer & buffer, + const std::string & parent, const std::string & child, + const rtabmap::Transform & t, double stamp, bool isStatic = true) +{ + geometry_msgs::msg::TransformStamped msg; + msg.header.stamp = timestampToROS(stamp); + msg.header.frame_id = parent; + msg.child_frame_id = child; + transformToGeometryMsg(t, msg.transform); + ASSERT_TRUE(buffer.setTransform(msg, "unit_test", isStatic)); +} + +} // namespace + +TEST(MsgConversion, getTransformReadsTheBuffer) +{ + const std::shared_ptr buffer = makeTfBuffer(); + const rtabmap::Transform baseToCamera(0.1f, 0.0f, 0.2f, 0.0f, 0.0f, 0.0f); + addTf(*buffer, "base_link", "camera_link", baseToCamera, 1000.0); + + const rtabmap::Transform out = + getTransform("base_link", "camera_link", timestampToROS(1000.0), *buffer, 0.0); + + expectTransformNear(out, baseToCamera); +} + +TEST(MsgConversion, getTransformReturnsNullWhenUnknown) +{ + const std::shared_ptr buffer = makeTfBuffer(); + addTf(*buffer, "base_link", "camera_link", rtabmap::Transform::getIdentity(), 1000.0); + + // An unrelated frame must not throw; it must come back as a null transform. + EXPECT_TRUE(getTransform("base_link", "lidar_link", timestampToROS(1000.0), *buffer, 0.0) + .isNull()); +} + +TEST(MsgConversion, getTransformIsInvertedByFrameOrder) +{ + const std::shared_ptr buffer = makeTfBuffer(); + const rtabmap::Transform baseToCamera(0.1f, 0.2f, 0.3f, 0.0f, 0.0f, 0.5f); + addTf(*buffer, "base_link", "camera_link", baseToCamera, 1000.0); + + const rtabmap::Transform forward = + getTransform("base_link", "camera_link", timestampToROS(1000.0), *buffer, 0.0); + const rtabmap::Transform backward = + getTransform("camera_link", "base_link", timestampToROS(1000.0), *buffer, 0.0); + + expectTransformNear(backward, forward.inverse(), 1e-4f); +} + +TEST(MsgConversion, getMovingTransformMeasuresMotionBetweenStamps) +{ + // base_link drives 1 m along x of odom between t=1000 and t=1001. + const std::shared_ptr buffer = makeTfBuffer(); + addTf(*buffer, "odom", "base_link", rtabmap::Transform(0, 0, 0, 0, 0, 0), 1000.0, false); + addTf(*buffer, "odom", "base_link", rtabmap::Transform(1, 0, 0, 0, 0, 0), 1001.0, false); + + // Motion of base_link from t=1000 to t=1001, seen in the fixed odom frame. + const rtabmap::Transform motion = getMovingTransform( + "base_link", "odom", timestampToROS(1000.0), timestampToROS(1001.0), *buffer, 0.0); + + ASSERT_FALSE(motion.isNull()); + EXPECT_NEAR(motion.x(), 1.0, 1e-4); + EXPECT_NEAR(motion.y(), 0.0, 1e-4); + EXPECT_NEAR(motion.z(), 0.0, 1e-4); +} + +TEST(MsgConversion, getMovingTransformInterpolatesBetweenStamps) +{ + const std::shared_ptr buffer = makeTfBuffer(); + addTf(*buffer, "odom", "base_link", rtabmap::Transform(0, 0, 0, 0, 0, 0), 1000.0, false); + addTf(*buffer, "odom", "base_link", rtabmap::Transform(1, 0, 0, 0, 0, 0), 1001.0, false); + + // Halfway through, so half the motion. + const rtabmap::Transform half = getMovingTransform( + "base_link", "odom", timestampToROS(1000.0), timestampToROS(1000.5), *buffer, 0.0); + + ASSERT_FALSE(half.isNull()); + EXPECT_NEAR(half.x(), 0.5, 1e-4); +} + +TEST(MsgConversion, getMovingTransformIsNullWithoutAFixedFrame) +{ + const std::shared_ptr buffer = makeTfBuffer(); + addTf(*buffer, "odom", "base_link", rtabmap::Transform::getIdentity(), 1000.0, false); + + EXPECT_TRUE(getMovingTransform("base_link", "map", + timestampToROS(1000.0), timestampToROS(1001.0), *buffer, 0.0).isNull()); +} + +TEST(MsgConversion, convertScanMsgProducesALaserScan) +{ + const std::shared_ptr buffer = makeTfBuffer(); + const rtabmap::Transform baseToLaser(0.2f, 0.0f, 0.1f, 0.0f, 0.0f, 0.0f); + addTf(*buffer, "base_link", "laser", baseToLaser, 1000.0); + + sensor_msgs::msg::LaserScan msg; + msg.header.stamp = timestampToROS(1000.0); + msg.header.frame_id = "laser"; + msg.angle_min = -1.0f; + msg.angle_max = 1.0f; + msg.angle_increment = 0.1f; + msg.time_increment = 0.0f; + msg.range_min = 0.1f; + msg.range_max = 30.0f; + msg.ranges.assign(21, 5.0f); + + rtabmap::LaserScan scan; + ASSERT_TRUE(convertScanMsg(msg, "base_link", "", timestampToROS(1000.0), + scan, *buffer, 0.0)); + + EXPECT_FALSE(scan.empty()); + EXPECT_EQ(scan.size(), (int)msg.ranges.size()); + expectTransformNear(scan.localTransform(), baseToLaser, 1e-4f); +} + +TEST(MsgConversion, convertScanMsgRejectsMalformedScans) +{ + const std::shared_ptr buffer = makeTfBuffer(); + addTf(*buffer, "base_link", "laser", rtabmap::Transform::getIdentity(), 1000.0); + + sensor_msgs::msg::LaserScan base; + base.header.stamp = timestampToROS(1000.0); + base.header.frame_id = "laser"; + base.angle_min = -1.0f; + base.angle_max = 1.0f; + base.angle_increment = 0.1f; + base.range_min = 0.1f; + base.range_max = 30.0f; + base.ranges.assign(21, 5.0f); + + rtabmap::LaserScan scan; + + sensor_msgs::msg::LaserScan zeroIncrement = base; + zeroIncrement.angle_increment = 0.0f; + EXPECT_FALSE(convertScanMsg(zeroIncrement, "base_link", "", timestampToROS(1000.0), + scan, *buffer, 0.0)) << "angle_increment of 0 would divide by zero"; + + sensor_msgs::msg::LaserScan invertedRange = base; + invertedRange.range_min = 40.0f; + EXPECT_FALSE(convertScanMsg(invertedRange, "base_link", "", timestampToROS(1000.0), + scan, *buffer, 0.0)) << "range_min > range_max"; + + sensor_msgs::msg::LaserScan invertedAngle = base; + invertedAngle.angle_min = 1.0f; + invertedAngle.angle_max = -1.0f; + EXPECT_FALSE(convertScanMsg(invertedAngle, "base_link", "", timestampToROS(1000.0), + scan, *buffer, 0.0)) << "positive increment with angle_max < angle_min"; +} + +TEST(MsgConversion, convertScanMsgFailsWithoutTf) +{ + const std::shared_ptr buffer = makeTfBuffer(); // empty + + sensor_msgs::msg::LaserScan msg; + msg.header.stamp = timestampToROS(1000.0); + msg.header.frame_id = "laser"; + msg.angle_min = -1.0f; + msg.angle_max = 1.0f; + msg.angle_increment = 0.1f; + msg.range_min = 0.1f; + msg.range_max = 30.0f; + msg.ranges.assign(21, 5.0f); + + rtabmap::LaserScan scan; + EXPECT_FALSE(convertScanMsg(msg, "base_link", "", timestampToROS(1000.0), + scan, *buffer, 0.0)); +} + +TEST(MsgConversion, convertScan3dMsgKeepsLocalTransformAndLimits) +{ + const std::shared_ptr buffer = makeTfBuffer(); + const rtabmap::Transform baseToLidar(0.0f, 0.0f, 0.5f, 0.0f, 0.0f, 0.0f); + addTf(*buffer, "base_link", "lidar", baseToLidar, 1000.0); + + sensor_msgs::msg::PointCloud2 msg = + makeXYZCloud({{1.0f, 0.0f, 0.0f}, {2.0f, 0.0f, 0.0f}, {3.0f, 0.0f, 0.0f}}); + msg.header.stamp = timestampToROS(1000.0); + msg.header.frame_id = "lidar"; + + rtabmap::LaserScan scan; + ASSERT_TRUE(convertScan3dMsg(msg, "base_link", "", timestampToROS(1000.0), + scan, *buffer, 0.0)); + + EXPECT_EQ(scan.size(), 3); + expectTransformNear(scan.localTransform(), baseToLidar, 1e-4f); + EXPECT_EQ(scan.rangeMax(), 0.0f) << "no max range requested"; + + rtabmap::LaserScan limited; + ASSERT_TRUE(convertScan3dMsg(msg, "base_link", "", timestampToROS(1000.0), + limited, *buffer, 0.0, /*maxPoints=*/10, /*maxRange=*/2.5f)); + EXPECT_EQ(limited.maxPoints(), 10); + EXPECT_FLOAT_EQ(limited.rangeMax(), 2.5f); +} + +TEST(MsgConversion, convertScan3dMsgFailsWithoutTf) +{ + const std::shared_ptr buffer = makeTfBuffer(); // empty + + sensor_msgs::msg::PointCloud2 msg = makeXYZCloud({{1.0f, 0.0f, 0.0f}}); + msg.header.stamp = timestampToROS(1000.0); + msg.header.frame_id = "lidar"; + + rtabmap::LaserScan scan; + EXPECT_FALSE(convertScan3dMsg(msg, "base_link", "", timestampToROS(1000.0), + scan, *buffer, 0.0)); +} + +TEST(MsgConversion, landmarksFromROSAppliesTfAndDefaultVariance) +{ + const std::shared_ptr buffer = makeTfBuffer(); + const rtabmap::Transform baseToCamera(0.5f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f); + addTf(*buffer, "base_link", "camera_link", baseToCamera, 1000.0); + + geometry_msgs::msg::PoseWithCovarianceStamped tag; + tag.header.stamp = timestampToROS(1000.0); + tag.header.frame_id = "camera_link"; + tag.pose.pose.position.x = 2.0; // 2 m in front of the camera + tag.pose.pose.orientation.w = 1.0; + // covariance left at zero -> the defaults must be substituted + + std::map > tags; + tags.insert(std::make_pair(7, std::make_pair(tag, 0.15f))); + + const rtabmap::Landmarks landmarks = landmarksFromROS( + tags, "base_link", "", timestampToROS(1000.0), *buffer, 0.0, + /*defaultLinVariance=*/0.01, /*defaultAngVariance=*/0.02); + + ASSERT_EQ(landmarks.size(), 1u); + ASSERT_TRUE(landmarks.find(7) != landmarks.end()); + + // The tag pose must come back in base_link: 0.5 (base->camera) + 2.0 (camera->tag). + EXPECT_NEAR(landmarks.at(7).pose().x(), 2.5, 1e-4); + + const cv::Mat cov = landmarks.at(7).covariance(); + ASSERT_EQ(cov.rows, 6); + EXPECT_NEAR(cov.at(0,0), 0.01, 1e-9) << "linear default"; + EXPECT_NEAR(cov.at(3,3), 0.02, 1e-9) << "angular default"; +} + +TEST(MsgConversion, landmarksFromROSCorrectsForOdometryMotion) +{ + // The tag is seen 1 s after the odometry stamp, during which the robot drives 1 m. + // landmarksFromROS must fold that motion in, otherwise the landmark is placed where + // the robot would have seen it had it not moved. + const std::shared_ptr buffer = makeTfBuffer(); + const rtabmap::Transform baseToCamera(0.5f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f); + addTf(*buffer, "base_link", "camera_link", baseToCamera, 1000.0); + addTf(*buffer, "odom", "base_link", rtabmap::Transform(0, 0, 0, 0, 0, 0), 1000.0, false); + addTf(*buffer, "odom", "base_link", rtabmap::Transform(1, 0, 0, 0, 0, 0), 1001.0, false); + + geometry_msgs::msg::PoseWithCovarianceStamped tag; + tag.header.stamp = timestampToROS(1001.0); // observed at t=1001 + tag.header.frame_id = "camera_link"; + tag.pose.pose.position.x = 2.0; + tag.pose.pose.orientation.w = 1.0; + + std::map > tags; + tags.insert(std::make_pair(7, std::make_pair(tag, 0.15f))); + + // odomStamp is 1000, one second BEFORE the observation. + const rtabmap::Landmarks corrected = landmarksFromROS( + tags, "base_link", "odom", timestampToROS(1000.0), *buffer, 0.0, 0.01, 0.02); + + ASSERT_EQ(corrected.size(), 1u); + // 0.5 (base->camera) + 2.0 (camera->tag) + 1.0 (odometry motion since odomStamp). + EXPECT_NEAR(corrected.at(7).pose().x(), 3.5, 1e-3); + + // Without an odom frame the correction cannot be looked up, and the landmark stays + // in the frame at the observation stamp. + const rtabmap::Landmarks uncorrected = landmarksFromROS( + tags, "base_link", "", timestampToROS(1000.0), *buffer, 0.0, 0.01, 0.02); + ASSERT_EQ(uncorrected.size(), 1u); + EXPECT_NEAR(uncorrected.at(7).pose().x(), 2.5, 1e-3); +} + +TEST(MsgConversion, landmarksFromROSKeepsProvidedCovariance) +{ + const std::shared_ptr buffer = makeTfBuffer(); + addTf(*buffer, "base_link", "camera_link", rtabmap::Transform::getIdentity(), 1000.0); + + geometry_msgs::msg::PoseWithCovarianceStamped tag; + tag.header.stamp = timestampToROS(1000.0); + tag.header.frame_id = "camera_link"; + tag.pose.pose.position.x = 1.0; + tag.pose.pose.orientation.w = 1.0; + for(size_t i=0; i<6; ++i) + { + tag.pose.covariance[i*6 + i] = 0.5; // a real, finite covariance + } + + std::map > tags; + tags.insert(std::make_pair(1, std::make_pair(tag, 0.1f))); + + const rtabmap::Landmarks landmarks = landmarksFromROS( + tags, "base_link", "", timestampToROS(1000.0), *buffer, 0.0, + /*defaultLinVariance=*/0.01, /*defaultAngVariance=*/0.02); + + ASSERT_EQ(landmarks.size(), 1u); + EXPECT_NEAR(landmarks.at(1).covariance().at(0,0), 0.5, 1e-9) + << "a provided covariance must not be replaced by the default"; + EXPECT_NEAR(landmarks.at(1).covariance().at(3,3), 0.5, 1e-9); +} + +TEST(MsgConversion, landmarksFromROSRejectsNonPositiveIds) +{ + const std::shared_ptr buffer = makeTfBuffer(); + addTf(*buffer, "base_link", "camera_link", rtabmap::Transform::getIdentity(), 1000.0); + + geometry_msgs::msg::PoseWithCovarianceStamped tag; + tag.header.stamp = timestampToROS(1000.0); + tag.header.frame_id = "camera_link"; + tag.pose.pose.orientation.w = 1.0; + + std::map > tags; + tags.insert(std::make_pair(0, std::make_pair(tag, 0.1f))); + tags.insert(std::make_pair(-3, std::make_pair(tag, 0.1f))); + tags.insert(std::make_pair(5, std::make_pair(tag, 0.1f))); + + const rtabmap::Landmarks landmarks = landmarksFromROS( + tags, "base_link", "", timestampToROS(1000.0), *buffer, 0.0, 0.01, 0.02); + + EXPECT_EQ(landmarks.size(), 1u) << "ids <= 0 must be dropped"; + EXPECT_TRUE(landmarks.find(5) != landmarks.end()); +} + +void expectTfDeskewRecoversWall(bool slerp) +{ + SCOPED_TRACE(slerp ? "slerp=true" : "slerp=false"); + + // base_link advances 0.1 m along odom over the sweep -- the same motion the constant + // velocity tests apply at 1 m/s. With slerp the correction is interpolated between + // the two end poses; without it, every sample gets its own TF lookup. + const std::shared_ptr buffer = makeTfBuffer(); + addTf(*buffer, "odom", "base_link", rtabmap::Transform(0, 0, 0, 0, 0, 0), 1000.0, false); + addTf(*buffer, "odom", "base_link", + rtabmap::Transform(float(kSpeed * kScanSpan), 0, 0, 0, 0, 0), + 1000.0 + kScanSpan, false); + + sensor_msgs::msg::PointCloud2 in = makeSkewedWallScan(1000.0, 0.0); + in.header.frame_id = "base_link"; + + sensor_msgs::msg::PointCloud2 out; + ASSERT_TRUE(deskew(in, out, "odom", *buffer, 0.0, slerp)); + + for(size_t i=0; i b = makeTfBuffer(); + geometry_msgs::msg::TransformStamped m; + m.header.frame_id = "odom"; + m.child_frame_id = "base_link"; + m.transform.rotation.w = 1.0; + for(double elapsed : {0.0, kneeTime, kScanSpan}) + { + m.header.stamp = timestampToROS(1000.0 + elapsed); + m.transform.translation.x = travelled(elapsed); + b->setTransform(m, "unit_test", false); + } + return b; + }; + + sensor_msgs::msg::PointCloud2 in = makeSkewedWallScan( + 1000.0, 0.0, kOffsetSecFloat32, kTimeOnColumns, 1, "t", false, travelled); + in.header.frame_id = "base_link"; + + sensor_msgs::msg::PointCloud2 slerped, perPoint; + const std::shared_ptr b1 = buildBuffer(); + const std::shared_ptr b2 = buildBuffer(); + ASSERT_TRUE(deskew(in, slerped, "odom", *b1, 0.0, /*slerp=*/true)); + ASSERT_TRUE(deskew(in, perPoint, "odom", *b2, 0.0, /*slerp=*/false)); + + // Per-point lookups follow the real motion, so they reconstruct the wall exactly. + for(size_t i=0; i buffer = makeTfBuffer(); // empty + + sensor_msgs::msg::PointCloud2 in = makeSkewedWallScan(1000.0, 0.0); + in.header.frame_id = "base_link"; + + sensor_msgs::msg::PointCloud2 out; + EXPECT_FALSE(deskew(in, out, "odom", *buffer, 0.0, true)); +} + +///////////////////////// +// convertRGBDMsgs / convertStereoMsg +///////////////////////// + +namespace { + +/// A rectified pinhole CameraInfo. tx is P(0,3): 0 for the left/depth camera, and +/// -fx*baseline for the right camera of a stereo pair. +sensor_msgs::msg::CameraInfo makeCameraInfo( + const std::string & frameId, double stamp, int width, int height, + double tx = 0.0, double fx = 100.0) +{ + sensor_msgs::msg::CameraInfo info; + info.header.stamp = timestampToROS(stamp); + info.header.frame_id = frameId; + info.width = width; + info.height = height; + info.distortion_model = "plumb_bob"; + info.d = {0.0, 0.0, 0.0, 0.0, 0.0}; + info.k = {fx, 0.0, width/2.0, 0.0, fx, height/2.0, 0.0, 0.0, 1.0}; + info.r = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0}; + info.p = {fx, 0.0, width/2.0, tx, 0.0, fx, height/2.0, 0.0, 0.0, 0.0, 1.0, 0.0}; + return info; +} + +cv_bridge::CvImageConstPtr makeImage( + const std::string & frameId, double stamp, + const cv::Mat & image, const std::string & encoding) +{ + std_msgs::msg::Header header; + header.stamp = timestampToROS(stamp); + header.frame_id = frameId; + return std::make_shared(header, encoding, image); +} + +} // namespace + +TEST(MsgConversion, convertRGBDMsgsSingleCamera) +{ + const std::shared_ptr buffer = makeTfBuffer(); + const rtabmap::Transform baseToCamera(0.1f, 0.0f, 0.2f, 0.0f, 0.0f, 0.0f); + addTf(*buffer, "base_link", "camera_link", baseToCamera, 1000.0); + + const cv::Mat rgbImage(8, 8, CV_8UC3, cv::Scalar(10, 20, 30)); + const cv::Mat depthImage(8, 8, CV_16UC1, cv::Scalar(1500)); + + const std::vector images = + {makeImage("camera_link", 1000.0, rgbImage, "bgr8")}; + const std::vector depths = + {makeImage("camera_link", 1000.0, depthImage, "16UC1")}; + const std::vector infos = + {makeCameraInfo("camera_link", 1000.0, 8, 8)}; + + cv::Mat rgb, depth; + std::vector models; + std::vector stereoModels; + ASSERT_TRUE(convertRGBDMsgs(images, depths, infos, {}, "base_link", "", + timestampToROS(1000.0), rgb, depth, models, stereoModels, + *buffer, 0.0, /*alreadyRectifiedImages=*/true)); + + EXPECT_TRUE(stereoModels.empty()) << "a depth image must not produce a stereo model"; + ASSERT_EQ(models.size(), 1u); + EXPECT_NEAR(models[0].fx(), 100.0, 1e-9); + expectTransformNear(models[0].localTransform(), baseToCamera, 1e-4f); + + ASSERT_EQ(rgb.cols, 8); + ASSERT_EQ(rgb.rows, 8); + EXPECT_EQ(depth.type(), CV_16UC1); + EXPECT_EQ(depth.at(0, 0), 1500); +} + +TEST(MsgConversion, convertRGBDMsgsMultiCameraSideBySide) +{ + // Two cameras are concatenated horizontally into one wide image, one model each. + const std::shared_ptr buffer = makeTfBuffer(); + addTf(*buffer, "base_link", "cam0", rtabmap::Transform(0.1f, 0.1f, 0, 0, 0, 0), 1000.0); + addTf(*buffer, "base_link", "cam1", rtabmap::Transform(0.1f, -0.1f, 0, 0, 0, 0), 1000.0); + + const cv::Mat rgb0(8, 8, CV_8UC3, cv::Scalar(10, 0, 0)); + const cv::Mat rgb1(8, 8, CV_8UC3, cv::Scalar(0, 20, 0)); + const cv::Mat depth0(8, 8, CV_16UC1, cv::Scalar(1000)); + const cv::Mat depth1(8, 8, CV_16UC1, cv::Scalar(2000)); + + const std::vector images = { + makeImage("cam0", 1000.0, rgb0, "bgr8"), + makeImage("cam1", 1000.0, rgb1, "bgr8")}; + const std::vector depths = { + makeImage("cam0", 1000.0, depth0, "16UC1"), + makeImage("cam1", 1000.0, depth1, "16UC1")}; + const std::vector infos = { + makeCameraInfo("cam0", 1000.0, 8, 8), + makeCameraInfo("cam1", 1000.0, 8, 8)}; + + cv::Mat rgb, depth; + std::vector models; + std::vector stereoModels; + ASSERT_TRUE(convertRGBDMsgs(images, depths, infos, {}, "base_link", "", + timestampToROS(1000.0), rgb, depth, models, stereoModels, + *buffer, 0.0, true)); + + ASSERT_EQ(models.size(), 2u); + EXPECT_EQ(rgb.cols, 16) << "the two 8-wide images must be side by side"; + EXPECT_EQ(rgb.rows, 8); + EXPECT_EQ(depth.cols, 16); + + // Each half keeps its own camera's data. + EXPECT_EQ(depth.at(0, 0), 1000); + EXPECT_EQ(depth.at(0, 8), 2000); + EXPECT_NEAR(models[0].localTransform().y(), 0.1, 1e-4); + EXPECT_NEAR(models[1].localTransform().y(), -0.1, 1e-4); +} + +namespace { + +/// base_link sits at odom origin at t=1000 and 1 m along x at t=1001. +void addOdomMotion(tf2_ros::Buffer & buffer) +{ + geometry_msgs::msg::TransformStamped m; + m.header.frame_id = "odom"; + m.child_frame_id = "base_link"; + m.transform.rotation.w = 1.0; + m.header.stamp = timestampToROS(1000.0); + m.transform.translation.x = 0.0; + ASSERT_TRUE(buffer.setTransform(m, "unit_test", false)); + m.header.stamp = timestampToROS(1001.0); + m.transform.translation.x = 1.0; + ASSERT_TRUE(buffer.setTransform(m, "unit_test", false)); +} + +} // namespace + +TEST(MsgConversion, convertRGBDMsgsSyncsToOdomStamp) +{ + // The image is captured at t=1001 but must be expressed relative to the base frame + // at odomStamp=1000, one metre back. + const std::shared_ptr buffer = makeTfBuffer(); + const rtabmap::Transform baseToCamera(0.1f, 0.0f, 0.2f, 0.0f, 0.0f, 0.0f); + addTf(*buffer, "base_link", "camera_link", baseToCamera, 1000.0); + addOdomMotion(*buffer); + + const cv::Mat rgbImage(8, 8, CV_8UC3, cv::Scalar(10, 20, 30)); + const cv::Mat depthImage(8, 8, CV_16UC1, cv::Scalar(1500)); + const std::vector images = + {makeImage("camera_link", 1001.0, rgbImage, "bgr8")}; + const std::vector depths = + {makeImage("camera_link", 1001.0, depthImage, "16UC1")}; + const std::vector infos = + {makeCameraInfo("camera_link", 1001.0, 8, 8)}; + + cv::Mat rgb, depth; + std::vector corrected; + std::vector stereoModels; + ASSERT_TRUE(convertRGBDMsgs(images, depths, infos, {}, "base_link", "odom", + timestampToROS(1000.0), rgb, depth, corrected, stereoModels, *buffer, 0.0, true)); + ASSERT_EQ(corrected.size(), 1u); + EXPECT_NEAR(corrected[0].localTransform().x(), 1.1, 1e-3) + << "0.1 base->camera plus 1.0 of odometry motion"; + + // Without an odom frame the motion is not folded in. + std::vector uncorrected; + std::vector stereoModels2; + ASSERT_TRUE(convertRGBDMsgs(images, depths, infos, {}, "base_link", "", + timestampToROS(1000.0), rgb, depth, uncorrected, stereoModels2, *buffer, 0.0, true)); + ASSERT_EQ(uncorrected.size(), 1u); + EXPECT_NEAR(uncorrected[0].localTransform().x(), 0.1, 1e-3); +} + +TEST(MsgConversion, convertRGBDMsgsSyncsEachCameraAtItsOwnStamp) +{ + // Two cameras captured 1 s apart, on a robot moving 1 m/s along x. Each must be + // corrected by its OWN elapsed motion, not by a single shared one. + const std::shared_ptr buffer = makeTfBuffer(); + addTf(*buffer, "base_link", "cam0", rtabmap::Transform(0.1f, 0.1f, 0, 0, 0, 0), 1000.0); + addTf(*buffer, "base_link", "cam1", rtabmap::Transform(0.1f, -0.1f, 0, 0, 0, 0), 1000.0); + addOdomMotion(*buffer); // x=0 @1000, x=1 @1001 + geometry_msgs::msg::TransformStamped m; // extend to x=2 @1002 + m.header.frame_id = "odom"; + m.child_frame_id = "base_link"; + m.transform.rotation.w = 1.0; + m.header.stamp = timestampToROS(1002.0); + m.transform.translation.x = 2.0; + ASSERT_TRUE(buffer->setTransform(m, "unit_test", false)); + + const cv::Mat rgb0(8, 8, CV_8UC3, cv::Scalar(10, 0, 0)); + const cv::Mat rgb1(8, 8, CV_8UC3, cv::Scalar(0, 20, 0)); + const cv::Mat depth0(8, 8, CV_16UC1, cv::Scalar(1000)); + const cv::Mat depth1(8, 8, CV_16UC1, cv::Scalar(2000)); + + const std::vector images = { + makeImage("cam0", 1001.0, rgb0, "bgr8"), + makeImage("cam1", 1002.0, rgb1, "bgr8")}; + const std::vector depths = { + makeImage("cam0", 1001.0, depth0, "16UC1"), + makeImage("cam1", 1002.0, depth1, "16UC1")}; + const std::vector infos = { + makeCameraInfo("cam0", 1001.0, 8, 8), + makeCameraInfo("cam1", 1002.0, 8, 8)}; + + cv::Mat rgb, depth; + std::vector models; + std::vector stereoModels; + ASSERT_TRUE(convertRGBDMsgs(images, depths, infos, {}, "base_link", "odom", + timestampToROS(1000.0), rgb, depth, models, stereoModels, *buffer, 0.0, true)); + + ASSERT_EQ(models.size(), 2u); + // cam0 is 1 s after odomStamp, cam1 is 2 s after. + EXPECT_NEAR(models[0].localTransform().x(), 1.1, 1e-3) << "0.1 + 1.0 of motion"; + EXPECT_NEAR(models[1].localTransform().x(), 2.1, 1e-3) << "0.1 + 2.0 of motion"; + // The corrections must differ, which is the whole point of per-camera stamps. + EXPECT_GT(models[1].localTransform().x() - models[0].localTransform().x(), 0.5); + // The lateral offsets are untouched by a purely forward motion. + EXPECT_NEAR(models[0].localTransform().y(), 0.1, 1e-3); + EXPECT_NEAR(models[1].localTransform().y(), -0.1, 1e-3); +} + +TEST(MsgConversion, convertRGBDMsgsPrefersTheDepthStampWhenTheyDiffer) +{ + // The RGB and depth stamps of a camera are assumed to be equal. This pins the + // tie-break for when they are not: the depth stamp prevails, since it is the one the + // geometry is synchronized to. Not a behaviour to rely on -- a camera whose two + // stamps disagree is already outside the contract. + const std::shared_ptr buffer = makeTfBuffer(); + addTf(*buffer, "base_link", "camera_link", rtabmap::Transform(0.1f, 0, 0, 0, 0, 0), 1000.0); + addOdomMotion(*buffer); + + const cv::Mat rgbImage(8, 8, CV_8UC3, cv::Scalar(10, 20, 30)); + const cv::Mat depthImage(8, 8, CV_16UC1, cv::Scalar(1500)); + + // RGB stamped at odomStamp (no motion), depth stamped 1 s later (1 m of motion). + const std::vector images = + {makeImage("camera_link", 1000.0, rgbImage, "bgr8")}; + const std::vector depths = + {makeImage("camera_link", 1001.0, depthImage, "16UC1")}; + const std::vector infos = + {makeCameraInfo("camera_link", 1000.0, 8, 8)}; + + cv::Mat rgb, depth; + std::vector models; + std::vector stereoModels; + ASSERT_TRUE(convertRGBDMsgs(images, depths, infos, {}, "base_link", "odom", + timestampToROS(1000.0), rgb, depth, models, stereoModels, *buffer, 0.0, true)); + + ASSERT_EQ(models.size(), 1u); + EXPECT_NEAR(models[0].localTransform().x(), 1.1, 1e-3) + << "the depth stamp (1001) prevails over the rgb stamp (1000)"; +} + +TEST(MsgConversion, convertRGBDMsgsMultiStereoBuildsOneModelPerPair) +{ + // mono8 "right" images make convertRGBDMsgs take the stereo branch and produce + // StereoCameraModels instead of CameraModels. The odometry sync is not re-tested + // here: it happens in the shared loop before the depth/stereo split, so + // convertRGBDMsgsSyncsEachCameraAtItsOwnStamp already covers it for both. + const double fx = 100.0; + const double baseline0 = 0.15; + const double baseline1 = 0.20; + + const std::shared_ptr buffer = makeTfBuffer(); + addTf(*buffer, "base_link", "left0", rtabmap::Transform(0.1f, 0.1f, 0, 0, 0, 0), 1000.0); + addTf(*buffer, "base_link", "left1", rtabmap::Transform(0.1f, -0.1f, 0, 0, 0, 0), 1000.0); + + const cv::Mat left0(8, 8, CV_8UC1, cv::Scalar(40)); + const cv::Mat left1(8, 8, CV_8UC1, cv::Scalar(60)); + const cv::Mat right(8, 8, CV_8UC1, cv::Scalar(50)); + + const std::vector images = { + makeImage("left0", 1000.0, left0, "mono8"), + makeImage("left1", 1000.0, left1, "mono8")}; + const std::vector rights = { + makeImage("right0", 1000.0, right, "mono8"), + makeImage("right1", 1000.0, right, "mono8")}; + const std::vector leftInfos = { + makeCameraInfo("left0", 1000.0, 8, 8, 0.0, fx), + makeCameraInfo("left1", 1000.0, 8, 8, 0.0, fx)}; + const std::vector rightInfos = { + makeCameraInfo("right0", 1000.0, 8, 8, -fx*baseline0, fx), + makeCameraInfo("right1", 1000.0, 8, 8, -fx*baseline1, fx)}; + + cv::Mat rgb, depth; + std::vector models; + std::vector stereoModels; + ASSERT_TRUE(convertRGBDMsgs(images, rights, leftInfos, rightInfos, "base_link", "", + timestampToROS(1000.0), rgb, depth, models, stereoModels, *buffer, 0.0, true)); + + EXPECT_TRUE(models.empty()) << "mono8 right images must give stereo models"; + ASSERT_EQ(stereoModels.size(), 2u); + + // Each pair keeps its own baseline and its own local transform. + EXPECT_NEAR(stereoModels[0].baseline(), baseline0, 1e-6); + EXPECT_NEAR(stereoModels[1].baseline(), baseline1, 1e-6); + EXPECT_NEAR(stereoModels[0].localTransform().y(), 0.1, 1e-3); + EXPECT_NEAR(stereoModels[1].localTransform().y(), -0.1, 1e-3); + + // The two left images are laid out side by side, as in the RGB-D case. + EXPECT_EQ(rgb.cols, 16); + EXPECT_EQ(depth.cols, 16); +} + +TEST(MsgConversion, convertRGBDMsgsSurvivesAFailedOdomLookup) +{ + // A missing odom frame must only warn: the data is still converted, uncorrected. + const std::shared_ptr buffer = makeTfBuffer(); + addTf(*buffer, "base_link", "camera_link", rtabmap::Transform(0.1f, 0, 0.2f, 0, 0, 0), 1001.0); + + const cv::Mat rgbImage(8, 8, CV_8UC3, cv::Scalar(10, 20, 30)); + const std::vector images = + {makeImage("camera_link", 1001.0, rgbImage, "bgr8")}; + const std::vector infos = + {makeCameraInfo("camera_link", 1001.0, 8, 8)}; + + cv::Mat rgb, depth; + std::vector models; + std::vector stereoModels; + ASSERT_TRUE(convertRGBDMsgs(images, {}, infos, {}, "base_link", "odom", + timestampToROS(1000.0), rgb, depth, models, stereoModels, *buffer, 0.0, true)) + << "a failed odometry correction must not be fatal"; + ASSERT_EQ(models.size(), 1u); + EXPECT_NEAR(models[0].localTransform().x(), 0.1, 1e-3) << "left uncorrected"; +} + +TEST(MsgConversion, convertStereoMsgSyncsToOdomStamp) +{ + const std::shared_ptr buffer = makeTfBuffer(); + addTf(*buffer, "base_link", "left_link", rtabmap::Transform(0.1f, 0, 0.2f, 0, 0, 0), 1000.0); + addOdomMotion(*buffer); + + const cv::Mat mono(8, 8, CV_8UC1, cv::Scalar(40)); + + cv::Mat left, right; + rtabmap::StereoCameraModel model; + ASSERT_TRUE(convertStereoMsg( + makeImage("left_link", 1001.0, mono, "mono8"), + makeImage("right_link", 1001.0, mono, "mono8"), + makeCameraInfo("left_link", 1001.0, 8, 8, 0.0), + makeCameraInfo("right_link", 1001.0, 8, 8, -15.0), + "base_link", "odom", timestampToROS(1000.0), + left, right, model, *buffer, 0.0, true)); + + EXPECT_NEAR(model.localTransform().x(), 1.1, 1e-3); +} + +TEST(MsgConversion, convertScan3dMsgSyncsToOdomStamp) +{ + const std::shared_ptr buffer = makeTfBuffer(); + addTf(*buffer, "base_link", "lidar", rtabmap::Transform(0.0f, 0, 0.5f, 0, 0, 0), 1000.0); + addOdomMotion(*buffer); + + sensor_msgs::msg::PointCloud2 msg = makeXYZCloud({{1.0f, 0.0f, 0.0f}}); + msg.header.stamp = timestampToROS(1001.0); + msg.header.frame_id = "lidar"; + + rtabmap::LaserScan scan; + ASSERT_TRUE(convertScan3dMsg(msg, "base_link", "odom", timestampToROS(1000.0), + scan, *buffer, 0.0)); + + EXPECT_NEAR(scan.localTransform().x(), 1.0, 1e-3) << "0.0 base->lidar plus 1.0 motion"; + EXPECT_NEAR(scan.localTransform().z(), 0.5, 1e-3); +} + +TEST(MsgConversion, convertScanMsgSyncsToOdomStamp) +{ + const std::shared_ptr buffer = makeTfBuffer(); + addTf(*buffer, "base_link", "laser", rtabmap::Transform(0.2f, 0, 0.1f, 0, 0, 0), 1000.0); + addOdomMotion(*buffer); + + sensor_msgs::msg::LaserScan msg; + msg.header.stamp = timestampToROS(1001.0); + msg.header.frame_id = "laser"; + msg.angle_min = -1.0f; + msg.angle_max = 1.0f; + msg.angle_increment = 0.1f; + msg.time_increment = 0.0f; + msg.range_min = 0.1f; + msg.range_max = 30.0f; + msg.ranges.assign(21, 5.0f); + + rtabmap::LaserScan scan; + ASSERT_TRUE(convertScanMsg(msg, "base_link", "odom", timestampToROS(1000.0), + scan, *buffer, 0.0)); + + EXPECT_NEAR(scan.localTransform().x(), 1.2, 1e-3) << "0.2 base->laser plus 1.0 motion"; +} + +TEST(MsgConversion, convertRGBDMsgsRejectsBadEncoding) +{ + const std::shared_ptr buffer = makeTfBuffer(); + addTf(*buffer, "base_link", "camera_link", rtabmap::Transform::getIdentity(), 1000.0); + + // 32FC1 is a valid depth encoding but not a valid rgb/left one. + const cv::Mat bad(8, 8, CV_32FC1, cv::Scalar(1.0f)); + const std::vector images = + {makeImage("camera_link", 1000.0, bad, "32FC1")}; + const std::vector infos = + {makeCameraInfo("camera_link", 1000.0, 8, 8)}; + + cv::Mat rgb, depth; + std::vector models; + std::vector stereoModels; + EXPECT_FALSE(convertRGBDMsgs(images, {}, infos, {}, "base_link", "", + timestampToROS(1000.0), rgb, depth, models, stereoModels, + *buffer, 0.0, true)); +} + +TEST(MsgConversion, convertRGBDMsgsFailsWithoutTf) +{ + const std::shared_ptr buffer = makeTfBuffer(); // empty + + const cv::Mat rgbImage(8, 8, CV_8UC3, cv::Scalar(10, 20, 30)); + const std::vector images = + {makeImage("camera_link", 1000.0, rgbImage, "bgr8")}; + const std::vector infos = + {makeCameraInfo("camera_link", 1000.0, 8, 8)}; + + cv::Mat rgb, depth; + std::vector models; + std::vector stereoModels; + EXPECT_FALSE(convertRGBDMsgs(images, {}, infos, {}, "base_link", "", + timestampToROS(1000.0), rgb, depth, models, stereoModels, + *buffer, 0.0, true)); +} + +TEST(MsgConversion, convertRGBDMsgsCarriesLocalFeatures) +{ + const std::shared_ptr buffer = makeTfBuffer(); + addTf(*buffer, "base_link", "camera_link", rtabmap::Transform::getIdentity(), 1000.0); + + const cv::Mat rgbImage(8, 8, CV_8UC3, cv::Scalar(10, 20, 30)); + const cv::Mat depthImage(8, 8, CV_16UC1, cv::Scalar(1500)); + const std::vector images = + {makeImage("camera_link", 1000.0, rgbImage, "bgr8")}; + const std::vector depths = + {makeImage("camera_link", 1000.0, depthImage, "16UC1")}; + const std::vector infos = + {makeCameraInfo("camera_link", 1000.0, 8, 8)}; + + std::vector kptMsgs(2); + kptMsgs[0].pt.x = 1.0f; kptMsgs[0].pt.y = 2.0f; kptMsgs[0].size = 7.0f; + kptMsgs[1].pt.x = 3.0f; kptMsgs[1].pt.y = 4.0f; kptMsgs[1].size = 7.0f; + std::vector ptMsgs(2); + ptMsgs[0].x = 1.0f; ptMsgs[1].x = 2.0f; + cv::Mat descriptors = cv::Mat::ones(2, 4, CV_32FC1); + + std::vector outKpts; + std::vector outPts; + cv::Mat outDescriptors; + + cv::Mat rgb, depth; + std::vector models; + std::vector stereoModels; + ASSERT_TRUE(convertRGBDMsgs(images, depths, infos, {}, "base_link", "", + timestampToROS(1000.0), rgb, depth, models, stereoModels, + *buffer, 0.0, true, + {kptMsgs}, {ptMsgs}, {descriptors}, + &outKpts, &outPts, &outDescriptors)); + + ASSERT_EQ(outKpts.size(), 2u); + EXPECT_FLOAT_EQ(outKpts[0].pt.x, 1.0f); + EXPECT_FLOAT_EQ(outKpts[1].pt.x, 3.0f); + ASSERT_EQ(outPts.size(), 2u); + EXPECT_FLOAT_EQ(outPts[1].x, 2.0f); + EXPECT_EQ(outDescriptors.rows, 2); +} + +TEST(MsgConversion, convertStereoMsgProducesAStereoModel) +{ + const std::shared_ptr buffer = makeTfBuffer(); + const rtabmap::Transform baseToLeft(0.1f, 0.0f, 0.2f, 0.0f, 0.0f, 0.0f); + addTf(*buffer, "base_link", "left_link", baseToLeft, 1000.0); + + const double fx = 100.0; + const double baseline = 0.15; + const cv::Mat leftImage(8, 8, CV_8UC1, cv::Scalar(40)); + const cv::Mat rightImage(8, 8, CV_8UC1, cv::Scalar(50)); + + cv::Mat left, right; + rtabmap::StereoCameraModel model; + ASSERT_TRUE(convertStereoMsg( + makeImage("left_link", 1000.0, leftImage, "mono8"), + makeImage("right_link", 1000.0, rightImage, "mono8"), + makeCameraInfo("left_link", 1000.0, 8, 8, /*tx=*/0.0, fx), + makeCameraInfo("right_link", 1000.0, 8, 8, /*tx=*/-fx*baseline, fx), + "base_link", "", timestampToROS(1000.0), + left, right, model, *buffer, 0.0, /*alreadyRectified=*/true)); + + EXPECT_NEAR(model.baseline(), baseline, 1e-6); + EXPECT_NEAR(model.left().fx(), fx, 1e-9); + expectTransformNear(model.localTransform(), baseToLeft, 1e-4f); + + ASSERT_EQ(left.type(), CV_8UC1); + ASSERT_EQ(right.type(), CV_8UC1); + EXPECT_EQ(left.at(0, 0), 40); + EXPECT_EQ(right.at(0, 0), 50); +} + +TEST(MsgConversion, convertStereoMsgConvertsColourToMono) +{ + const std::shared_ptr buffer = makeTfBuffer(); + addTf(*buffer, "base_link", "left_link", rtabmap::Transform::getIdentity(), 1000.0); + + const cv::Mat colour(8, 8, CV_8UC3, cv::Scalar(10, 20, 30)); + const cv::Mat mono(8, 8, CV_8UC1, cv::Scalar(50)); + + cv::Mat left, right; + rtabmap::StereoCameraModel model; + ASSERT_TRUE(convertStereoMsg( + makeImage("left_link", 1000.0, colour, "bgr8"), + makeImage("right_link", 1000.0, mono, "mono8"), + makeCameraInfo("left_link", 1000.0, 8, 8, 0.0), + makeCameraInfo("right_link", 1000.0, 8, 8, -15.0), + "base_link", "", timestampToROS(1000.0), + left, right, model, *buffer, 0.0, true)); + + // The left image is kept in colour; the right is always reduced to mono. + EXPECT_EQ(left.type(), CV_8UC3); + EXPECT_EQ(right.type(), CV_8UC1); +} + +TEST(MsgConversion, convertStereoMsgRejectsBadEncoding) +{ + const std::shared_ptr buffer = makeTfBuffer(); + addTf(*buffer, "base_link", "left_link", rtabmap::Transform::getIdentity(), 1000.0); + + const cv::Mat bad(8, 8, CV_32FC1, cv::Scalar(1.0f)); + const cv::Mat mono(8, 8, CV_8UC1, cv::Scalar(50)); + + cv::Mat left, right; + rtabmap::StereoCameraModel model; + EXPECT_FALSE(convertStereoMsg( + makeImage("left_link", 1000.0, bad, "32FC1"), + makeImage("right_link", 1000.0, mono, "mono8"), + makeCameraInfo("left_link", 1000.0, 8, 8, 0.0), + makeCameraInfo("right_link", 1000.0, 8, 8, -15.0), + "base_link", "", timestampToROS(1000.0), + left, right, model, *buffer, 0.0, true)); +} + +TEST(MsgConversion, convertStereoMsgFailsWithoutTf) +{ + const std::shared_ptr buffer = makeTfBuffer(); // empty + const cv::Mat mono(8, 8, CV_8UC1, cv::Scalar(50)); + + cv::Mat left, right; + rtabmap::StereoCameraModel model; + EXPECT_FALSE(convertStereoMsg( + makeImage("left_link", 1000.0, mono, "mono8"), + makeImage("right_link", 1000.0, mono, "mono8"), + makeCameraInfo("left_link", 1000.0, 8, 8, 0.0), + makeCameraInfo("right_link", 1000.0, 8, 8, -15.0), + "base_link", "", timestampToROS(1000.0), + left, right, model, *buffer, 0.0, true)); +} + +///////////////////////// +// IMU +///////////////////////// + +TEST(MsgConversion, imuRoundTrip) +{ + sensor_msgs::msg::Imu in; + in.orientation.x = 0.0; + in.orientation.y = 0.0; + in.orientation.z = 0.0; + in.orientation.w = 1.0; + in.angular_velocity.x = 0.1; + in.angular_velocity.y = 0.2; + in.angular_velocity.z = 0.3; + in.linear_acceleration.x = 1.0; + in.linear_acceleration.y = 2.0; + in.linear_acceleration.z = 9.81; + for(size_t i=0; i<9; ++i) + { + in.orientation_covariance[i] = 0.01 * (i + 1); + in.angular_velocity_covariance[i] = 0.02 * (i + 1); + in.linear_acceleration_covariance[i] = 0.03 * (i + 1); + } + + const rtabmap::IMU imu = imuFromROS(in, rtabmap::Transform::getIdentity()); + + sensor_msgs::msg::Imu out; + imuToROS(imu, out); + + EXPECT_DOUBLE_EQ(out.orientation.w, in.orientation.w); + EXPECT_DOUBLE_EQ(out.angular_velocity.x, in.angular_velocity.x); + EXPECT_DOUBLE_EQ(out.angular_velocity.y, in.angular_velocity.y); + EXPECT_DOUBLE_EQ(out.angular_velocity.z, in.angular_velocity.z); + EXPECT_DOUBLE_EQ(out.linear_acceleration.x, in.linear_acceleration.x); + EXPECT_DOUBLE_EQ(out.linear_acceleration.y, in.linear_acceleration.y); + EXPECT_DOUBLE_EQ(out.linear_acceleration.z, in.linear_acceleration.z); + for(size_t i=0; i<9; ++i) + { + EXPECT_DOUBLE_EQ(out.orientation_covariance[i], in.orientation_covariance[i]) + << "orientation covariance at " << i; + EXPECT_DOUBLE_EQ(out.angular_velocity_covariance[i], in.angular_velocity_covariance[i]) + << "angular velocity covariance at " << i; + EXPECT_DOUBLE_EQ(out.linear_acceleration_covariance[i], in.linear_acceleration_covariance[i]) + << "linear acceleration covariance at " << i; + } +} diff --git a/rtabmap_odom/src/nodelets/icp_odometry.cpp b/rtabmap_odom/src/nodelets/icp_odometry.cpp index 990976ec..581b5316 100644 --- a/rtabmap_odom/src/nodelets/icp_odometry.cpp +++ b/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 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; diff --git a/rtabmap_viz/src/rgbd_image_viewer.cpp b/rtabmap_viz/src/rgbd_image_viewer.cpp index d9fd1953..0cc36c81 100644 --- a/rtabmap_viz/src/rgbd_image_viewer.cpp +++ b/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)); }