From cf7d7ab9587b670d8833b981eb26d5058c157cf7 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 10 Oct 2026 16:05:08 -0700 Subject: [PATCH] Supporting image compression formats passthrough (#1461) * Supporting ros depth compression formats passthrough * Added image_compression_format (support jpg,png). Use Compression.h constants. * Bridging gap with #1457 * CoreWrapper: decode_images_on_demand * rgbd_sync->rgbd_split encoding/decoding test with image_transport validation. rgbd_split color passthrough * more tests * fixing ci on kilted+ * fixing test * fixing lyrical tests * fixing rolling ci * Improved test coverage * fixing some tf waitForTransform deadlocks when sim time is used in tests * CI: build rtabmap_slam on rolling rtabmap_slam builds without nav2_msgs, which is not on resolute yet: skip its key instead of leaving the package out. Only rtabmap_costmap_plugins (and the rtabmap_ros metapackage pulling it in) still need nav2. --- rtabmap_conversions/CMakeLists.txt | 7 +- .../rtabmap_conversions/MsgConversion.h | 172 ++++- rtabmap_conversions/src/MsgConversion.cpp | 549 +++++++++++++- .../test/test_msg_conversion.cpp | 685 ++++++++++++++++++ rtabmap_msgs/msg/GlobalDescriptor.msg | 2 +- rtabmap_msgs/msg/Node.msg | 2 +- rtabmap_msgs/msg/RGBDImage.msg | 14 +- rtabmap_msgs/msg/SensorData.msg | 14 +- rtabmap_odom/README.md | 15 +- .../include/rtabmap_odom/OdometryROS.h | 1 + rtabmap_odom/src/OdometryROS.cpp | 24 +- rtabmap_odom/test/test_icp_odometry.cpp | 23 + rtabmap_odom/test/test_odometry_ros.cpp | 20 + rtabmap_slam/doc/rtabmap.md | 8 + .../include/rtabmap_slam/CoreWrapper.h | 27 +- rtabmap_slam/src/CoreWrapper.cpp | 71 +- rtabmap_slam/test/core_wrapper_fixture.hpp | 42 +- .../test/test_core_wrapper_inputs.cpp | 326 +++++++++ .../test/test_core_wrapper_planning.cpp | 58 +- rtabmap_sync/doc/rgb_sync.md | 3 +- rtabmap_sync/doc/rgbd_sync.md | 23 +- rtabmap_sync/doc/stereo_sync.md | 7 +- .../rtabmap_sync/CommonDataSubscriber.h | 58 +- .../include/rtabmap_sync/rgb_sync.hpp | 2 + .../include/rtabmap_sync/rgbd_sync.hpp | 3 + .../include/rtabmap_sync/stereo_sync.hpp | 2 + rtabmap_sync/src/CommonDataSubscriber.cpp | 56 +- .../src/impl/CommonDataSubscriberRGBD.cpp | 124 +++- rtabmap_sync/src/nodelets/rgb_sync.cpp | 11 +- rtabmap_sync/src/nodelets/rgbd_sync.cpp | 20 +- rtabmap_sync/src/nodelets/stereo_sync.cpp | 10 +- .../test/common_data_subscriber_fixture.hpp | 42 ++ .../test/test_common_data_subscriber_sync.cpp | 230 ++++++ rtabmap_sync/test/test_rgb_sync.cpp | 15 +- rtabmap_sync/test/test_rgbd_sync.cpp | 102 ++- rtabmap_sync/test/test_stereo_sync.cpp | 13 + rtabmap_util/CMakeLists.txt | 8 + rtabmap_util/doc/rgbd_relay.md | 8 +- rtabmap_util/doc/rgbd_split.md | 30 + .../include/rtabmap_util/rgbd_relay.hpp | 3 + .../include/rtabmap_util/rgbd_split.hpp | 27 + rtabmap_util/package.xml | 5 + rtabmap_util/src/nodelets/rgbd_relay.cpp | 54 +- rtabmap_util/src/nodelets/rgbd_split.cpp | 343 +++++++-- rtabmap_util/test/test_rgbd_pipeline.cpp | 417 +++++++++++ rtabmap_util/test/test_rgbd_relay.cpp | 102 ++- rtabmap_util/test/test_rgbd_split.cpp | 434 +++++++++++ 47 files changed, 3962 insertions(+), 250 deletions(-) create mode 100644 rtabmap_util/test/test_rgbd_pipeline.cpp diff --git a/rtabmap_conversions/CMakeLists.txt b/rtabmap_conversions/CMakeLists.txt index af0a93d4..685004b5 100644 --- a/rtabmap_conversions/CMakeLists.txt +++ b/rtabmap_conversions/CMakeLists.txt @@ -30,7 +30,7 @@ find_package(tf2_eigen REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) -find_package(RTABMap 0.23.13 REQUIRED) +find_package(RTABMap 0.24.0 REQUIRED) # libraries SET(Libraries @@ -79,6 +79,11 @@ IF("$ENV{ROS_DISTRO}" STRLESS "kilted") add_definitions(-DPRE_ROS_KILTED) ENDIF() +# compressed_depth_image_transport decodes RVL since Jazzy +IF("$ENV{ROS_DISTRO}" STRLESS "jazzy") + add_definitions(-DPRE_ROS_JAZZY) +ENDIF() + ########### ## Build ## ########### diff --git a/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h b/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h index 666cdb38..9d20bb96 100644 --- a/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h +++ b/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h @@ -36,6 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include @@ -208,7 +209,17 @@ void toCvShare(const rtabmap_msgs::msg::RGBDImage & image, const std::shared_ptr * @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 + * @param[in] legacyDepthCompression if @p data carries only compressed images, keep its + * compressed depth in rtabmap's format as is (readable by + * rtabmap_ros < 0.24) instead of converting it to + * compressed_depth_image_transport's format (see + * compressDepthImage(), "legacy" format) * + * @note Compressed images (as loaded from a database) are copied without being + * decompressed when possible: color and right images as is, depth converted with + * rtabmapToCompressedDepthTransport(). Only depth in rtabmap's legacy 32FC1 format, + * which has no compressed_depth_image_transport equivalent, is decompressed and + * re-compressed in millimeters. * @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 @@ -217,7 +228,7 @@ void toCvShare(const rtabmap_msgs::msg::RGBDImage & image, const std::shared_ptr * 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); +void rgbdImageToROS(const rtabmap::SensorData & data, rtabmap_msgs::msg::RGBDImage & msg, const std::string & sensorFrameId, bool legacyDepthCompression = false); /** * @brief Build a SensorData from an RGBDImage message. @@ -228,6 +239,10 @@ void rgbdImageToROS(const rtabmap::SensorData & data, rtabmap_msgs::msg::RGBDIma * The depth image is optional: a message carrying only the color image and its camera * info gives a SensorData with no depth, which is valid. * + * The local features (key points, their 3D points and descriptors) and the global + * descriptor of the message are set too; the 3D points stay in the camera frame, as the + * local transform is left to identity. + * * @param image the message to convert * @return the converted sensor data, empty (SensorData::isValid() false) if the message * carries no color image or an unsupported encoding @@ -260,6 +275,161 @@ void compressedMatToBytes(const cv::Mat & compressed, std::vector */ cv::Mat compressedMatFromBytes(const std::vector & bytes, bool copy = true); +/** + * @brief Convert a compressed_depth_image_transport message into a depth image compressed + * the way rtabmap does it, without decompressing it. + * + * Only the header is converted, the compressed payload is copied as is: 32FC1 depth + * images become rtabmap's inverse depth format (e.g., ".png:10:100"), 16UC1 (or mono16) + * depth images become rtabmap's ".png" or ".rvl" format. The result can be decoded with + * rtabmap::uncompressImage(), or set as compressed depth of a rtabmap::SensorData. + * + * @param msg a message published by compressed_depth_image_transport, with a format like + * "32FC1; compressedDepth png" or "16UC1; compressedDepth rvl" + * @return a 1xN CV_8UC1 matrix of compressed bytes, empty if the format is not supported + * (an error is logged) + * @see rtabmapToCompressedDepthTransport() + */ +cv::Mat compressedDepthTransportToRtabmap(const sensor_msgs::msg::CompressedImage & msg); + +/** + * @brief Convert a depth image compressed the way rtabmap does it into a + * compressed_depth_image_transport message, without decompressing it. + * + * Only the header is converted, the compressed payload is copied as is. Supported are + * 16UC1 depth images compressed in ".png" or ".rvl" and 32FC1 depth images compressed + * in inverse depth format (e.g., ".rvl:10:100", see Mem/DepthCompressionFormat). 32FC1 + * depth images compressed in the legacy ".png" format (4 channels) have no equivalent. + * + * Before ROS Jazzy, compressed_depth_image_transport cannot decode RVL: RVL payloads are + * then decompressed and re-compressed as PNG (lossless, the 16 bits values are the same). + * + * @param[in] compressed a 1xN CV_8UC1 matrix of compressed bytes (e.g., + * rtabmap::SensorData::depthOrRightCompressed()) + * @param[out] msg the message; only `format` and `data` are set, the header is kept + * @param[in] logErrors log an error if @p compressed cannot be converted + * @return false if the compressed depth image cannot be converted, in which case @p msg + * is not modified + * @see compressedDepthTransportToRtabmap() + */ +bool rtabmapToCompressedDepthTransport(const cv::Mat & compressed, sensor_msgs::msg::CompressedImage & msg, bool logErrors = true); + +/** + * @brief Decompress the `depth_compressed` field of an RGBDImage message. + * + * Accepts every format that field can hold: a depth image compressed by rtabmap + * (rtabmap::compressImage(), e.g., ".png", ".rvl" or ".rvl:10:100"), a message published + * by compressed_depth_image_transport (format "; compressedDepth ", see + * compressedDepthTransportToRtabmap()), or a right stereo image compressed as JPEG or PNG. + * + * @param msg the compressed image + * @return always valid; the decompressed image with its encoding (32FC1, 16UC1, mono8 or + * bgr8) and the header of @p msg, or an empty image if @p msg is empty or could + * not be decompressed (an error is logged) + */ +cv_bridge::CvImagePtr uncompressDepthImage(const sensor_msgs::msg::CompressedImage & msg); + +/** + * @brief Get the compressed images of an RGBDImage in rtabmap's format, without + * decompressing them, e.g., to store them as is in rtabmap::SensorData. + * + * `rgb_compressed` (JPEG or PNG) is copied as is. `depth_compressed` is converted with + * compressedDepthTransportToRtabmap() if in compressed_depth_image_transport's format, + * otherwise copied as is (rtabmap's format, or JPEG/PNG right image). A field is only + * returned if the message has no raw image for it: the raw image is then the one decoded + * from it. + * + * @param[in] msg the message + * @param[out] rgb 1xN CV_8UC1 compressed color/left image, or empty + * @param[out] depth 1xN CV_8UC1 compressed depth/right image, or empty + */ +void rgbdImageCompressedToRtabmap(const rtabmap_msgs::msg::RGBDImage & msg, cv::Mat & rgb, cv::Mat & depth); + +/** + * @brief Whether rtabmap can use compressed images as they are, decoding them itself + * only when needed (see rtabmap::Memory), without any conversion of the decoded + * images: checked from their headers, without decoding them. + * + * @param rgb compressed color or left image in rtabmap's format (see + * rgbdImageCompressedToRtabmap()), or empty: gray or color JPEG or 8 bits PNG + * @param depth compressed depth or right image in rtabmap's format, or empty. Depth: + * 16UC1 PNG or RVL, 32FC1 inverse depth, or rtabmap's legacy 32FC1 PNG. + * Right image: gray JPEG or 8 bits PNG. + * @param stereo whether @p depth is the right image of a stereo pair + * @return false if both are empty, or if one of them is not supported + */ +bool isCompressedRGBDSupportedByRtabmap(const cv::Mat & rgb, const cv::Mat & depth, bool stereo = false); + +/** + * @return true if @p format is a valid format for compressDepthImage(): ".png" or ".rvl", + * optionally followed by ":[:]", optionally prefixed by + * "legacy:", or "legacy". + */ +bool isValidDepthCompressionFormat(const std::string & format); + +/** + * @brief Compress a depth image (16UC1 or 32FC1) into the `depth_compressed` field of an + * RGBDImage message, in compressed_depth_image_transport's format. + * + * The result is readable by the "compressedDepth" image_transport plugin: + * - 16UC1: compressed with the codec, losslessly ("16UC1; compressedDepth png"). + * - 32FC1 without depth parameters (".png", ".rvl"): rounded to millimeters (16UC1, values + * over 65.535 m become 0) and compressed with the codec ("16UC1; compressedDepth png"). + * - 32FC1 with depth parameters (".png:[:]"): quantized as 16 bits + * inverse depth, like compressed_depth_image_transport ("32FC1; compressedDepth png"), + * see Mem/DepthCompressionFormat. + * - "legacy:" ("legacy" is "legacy:.png"): rtabmap's own format, the same as in + * its database (see Mem/DepthCompressionFormat and rtabmap::compressImage()), not + * readable by image_transport plugins: 16UC1 with the codec, 32FC1 without depth + * parameters losslessly as 4 channels PNG, 32FC1 with depth parameters as inverse depth. + * `format` is set to the format without its leading dot, e.g., "png", "rvl", + * "rvl:10:100". Readable by rtabmap_ros before 0.24, except inverse depth. Use it to + * keep 32FC1 depth lossless, or for RVL before Jazzy. + * + * Before ROS Jazzy, ".rvl" gives PNG data, as compressed_depth_image_transport cannot + * decode RVL (see rtabmapToCompressedDepthTransport()). + * + * @param[in] depth the depth image + * @param[in] format see above + * @param[out] msg the message; `data` and `format` are set, the header is kept + * @return false if the image could not be compressed (an error is logged), in which case + * @p msg is not modified + * @see uncompressDepthImage() + */ +bool compressDepthImage(const cv::Mat & depth, const std::string & format, sensor_msgs::msg::CompressedImage & msg); + +/** + * @brief Compress an image the way compressed_image_transport does, so that any + * "compressed" image_transport subscriber can read it. + * + * Like cv_bridge::CvImage::toCompressedImageMsg(), bgr8, bgra8, mono8 and mono16 images + * are compressed as is and other color images are converted to bgr8 (bgra8 with alpha), + * but `format` also says which encoding the data is in, e.g. "bgr8; jpeg compressed bgr8", + * instead of only "jpg", from which subscribers have to guess it. + * + * @param[in] image the image to compress + * @param[in] format "jpeg" or "png", or as rtabmap names them, ".jpg" or ".png" + * @param[out] msg the message; `header`, `format` and `data` are set + * @return false if the image could not be compressed (an error is logged) + */ +bool toCompressedImageMsg(const cv_bridge::CvImage & image, const std::string & format, sensor_msgs::msg::CompressedImage & msg); + +/** + * @return true if @p format is a valid format for toCompressedImageMsg(): ".jpg", ".png", + * "jpeg" or "png". + */ +bool isValidImageCompressionFormat(const std::string & format); + +/** + * @brief compressed_image_transport's format of a JPEG or PNG image, e.g., + * "bgr8; jpeg compressed bgr8", with the encoding read from its header, without + * decoding it. + * @param data the compressed image + * @return the format, or empty if @p data is not a JPEG or PNG image whose encoding is + * known (mono8, mono16, bgr8, bgr16, bgra8 or bgra16) + */ +std::string compressedImageTransportFormat(const std::vector & data); + //============================================================================ // Statistics diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp index 03d608ec..18542fde 100644 --- a/rtabmap_conversions/src/MsgConversion.cpp +++ b/rtabmap_conversions/src/MsgConversion.cpp @@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include #include #include @@ -65,6 +66,102 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap_conversions { +namespace { + +// PNG file signature. rtabmap's own depth layouts are in rtabmap/core/Compression.h. +const unsigned char kPngSignature[8] = {0x89, 'P', 'N', 'G', '\r', '\n', 0x1A, '\n'}; + +// compressed_depth_image_transport::ConfigHeader, at the start of the message data +struct CompressedDepthConfigHeader +{ + int32_t format; // 0: INV_DEPTH + float depthParam[2]; +}; +static_assert(sizeof(CompressedDepthConfigHeader) == 12, "Unexpected compressedDepth header size"); + +bool hasSignature(const unsigned char * bytes, size_t size, const void * signature) +{ + return size >= 8 && memcmp(bytes, signature, 8) == 0; +} + + +// compressed_image_transport's format of a JPEG or PNG image ("; +// compressed "), with the encoding read from its header. Only the codec name +// if the encoding cannot be read from it. +std::string compressedImageFormat(const unsigned char * d, size_t size) +{ + std::string codec; + std::string encoding; + if(size > 25 && hasSignature(d, size, kPngSignature)) + { + codec = "png"; + // IHDR: bit depth, color type. OpenCV decodes color as BGR. + const bool depth16 = d[24] == 16; + switch(d[25]) + { + case 0: encoding = depth16 ? sensor_msgs::image_encodings::MONO16 : sensor_msgs::image_encodings::MONO8; break; + case 2: encoding = depth16 ? sensor_msgs::image_encodings::BGR16 : sensor_msgs::image_encodings::BGR8; break; + case 6: encoding = depth16 ? sensor_msgs::image_encodings::BGRA16 : sensor_msgs::image_encodings::BGRA8; break; + default: break; + } + if(d[24] != 8 && !depth16) + { + encoding.clear(); // e.g., 1 bit per pixel, decoded as 8 bits by OpenCV + } + } + else if(size > 2 && d[0] == 0xFF && d[1] == 0xD8) + { + codec = "jpeg"; + // Number of components in the start of frame segment + size_t i = 2; + while(i + 4 <= size && d[i] == 0xFF) + { + const unsigned char marker = d[i+1]; + if(marker == 0xFF) + { + ++i; // fill byte + continue; + } + if(marker == 0x01 || (marker >= 0xD0 && marker <= 0xD8)) + { + i += 2; // no length + continue; + } + const size_t length = (size_t(d[i+2]) << 8) | d[i+3]; + if(marker >= 0xC0 && marker <= 0xCF && marker != 0xC4 && marker != 0xC8 && marker != 0xCC) + { + if(i + 9 < size) + { + encoding = d[i+9] == 1 ? sensor_msgs::image_encodings::MONO8 : + d[i+9] == 3 ? sensor_msgs::image_encodings::BGR8 : ""; + } + break; + } + i += 2 + length; + } + } + if(codec.empty() || encoding.empty()) + { + return codec; + } + return encoding + "; " + codec + " compressed " + encoding; +} + +std::string compressedImageFormat(const std::vector & d) +{ + return compressedImageFormat(d.data(), d.size()); +} + +// Encoding of a JPEG or PNG image read from its header, empty if unknown +std::string compressedImageEncoding(const cv::Mat & bytes) +{ + const std::string format = compressedImageFormat(bytes.data, bytes.total()); + const size_t split = format.find(';'); + return split == std::string::npos ? std::string() : format.substr(0, split); +} + +} // namespace + bool transformToTF(const rtabmap::Transform & transform, tf2::Transform & tfTransform) { if(transform.isNull()) @@ -178,6 +275,28 @@ rtabmap::Transform transformFromPoseMsg(const geometry_msgs::msg::Pose & msg, bo return rtabmap::Transform::fromEigen3d(tfPose); } +namespace { + +// cv_bridge::toCvCopy() labels a decoded 16 bits image mono8/bgr8 (it only looks at the +// number of channels): fix the encoding so that it can be converted downstream. +cv_bridge::CvImagePtr toCvCopyCompressedImage(const sensor_msgs::msg::CompressedImage & msg) +{ + cv_bridge::CvImagePtr ptr = cv_bridge::toCvCopy(msg); + if(ptr && ptr->image.depth() == CV_16U) + { + switch(ptr->image.channels()) + { + case 1: ptr->encoding = sensor_msgs::image_encodings::MONO16; break; + case 3: ptr->encoding = sensor_msgs::image_encodings::BGR16; break; + case 4: ptr->encoding = sensor_msgs::image_encodings::BGRA16; break; + default: break; + } + } + return ptr; +} + +} // namespace + void toCvCopy(const rtabmap_msgs::msg::RGBDImage & image, cv_bridge::CvImagePtr & rgb, cv_bridge::CvImagePtr & depth) { if(!image.rgb.data.empty()) @@ -186,7 +305,7 @@ void toCvCopy(const rtabmap_msgs::msg::RGBDImage & image, cv_bridge::CvImagePtr } else if(!image.rgb_compressed.data.empty()) { - rgb = cv_bridge::toCvCopy(image.rgb_compressed); + rgb = toCvCopyCompressedImage(image.rgb_compressed); } else { @@ -200,12 +319,7 @@ void toCvCopy(const rtabmap_msgs::msg::RGBDImage & image, cv_bridge::CvImagePtr } else if(!image.depth_compressed.data.empty()) { - cv_bridge::CvImagePtr ptr = std::make_unique(); - ptr->header = image.depth_compressed.header; - ptr->image = rtabmap::uncompressImage(image.depth_compressed.data); - UASSERT(ptr->image.empty() || ptr->image.type() == CV_32FC1 || ptr->image.type() == CV_16UC1); - ptr->encoding = ptr->image.empty()?"":ptr->image.type() == CV_32FC1?sensor_msgs::image_encodings::TYPE_32FC1:sensor_msgs::image_encodings::TYPE_16UC1; - depth = ptr; + depth = uncompressDepthImage(image.depth_compressed); } else { @@ -229,7 +343,7 @@ void toCvShare(const rtabmap_msgs::msg::RGBDImage & image, const std::shared_ptr } else if(!image.rgb_compressed.data.empty()) { - rgb = cv_bridge::toCvCopy(image.rgb_compressed); + rgb = toCvCopyCompressedImage(image.rgb_compressed); } else { @@ -243,19 +357,7 @@ void toCvShare(const rtabmap_msgs::msg::RGBDImage & image, const std::shared_ptr } else if(!image.depth_compressed.data.empty()) { - if(image.depth_compressed.format.compare("jpg")==0) - { - depth = cv_bridge::toCvCopy(image.depth_compressed); - } - else - { - cv_bridge::CvImagePtr ptr = std::make_shared(); - ptr->header = image.depth_compressed.header; - ptr->image = rtabmap::uncompressImage(image.depth_compressed.data); - UASSERT(ptr->image.empty() || ptr->image.type() == CV_32FC1 || ptr->image.type() == CV_16UC1); - ptr->encoding = ptr->image.empty()?"":ptr->image.type() == CV_32FC1?sensor_msgs::image_encodings::TYPE_32FC1:sensor_msgs::image_encodings::TYPE_16UC1; - depth = ptr; - } + depth = uncompressDepthImage(image.depth_compressed); } else { @@ -268,7 +370,7 @@ void toCvShare(const rtabmap_msgs::msg::RGBDImage & image, const std::shared_ptr } } -void rgbdImageToROS(const rtabmap::SensorData & data, rtabmap_msgs::msg::RGBDImage & msg, const std::string & sensorFrameId) +void rgbdImageToROS(const rtabmap::SensorData & data, rtabmap_msgs::msg::RGBDImage & msg, const std::string & sensorFrameId, bool legacyDepthCompression) { std_msgs::msg::Header header; header.frame_id = sensorFrameId; @@ -308,7 +410,10 @@ void rgbdImageToROS(const rtabmap::SensorData & data, rtabmap_msgs::msg::RGBDIma } else if(!data.imageCompressed().empty()) { - UERROR("Conversion of compressed SensorData to RGBDImage is not implemented..."); + // JPEG or PNG, read as is by compressed_image_transport + msg.rgb_compressed.header = header; + compressedMatToBytes(data.imageCompressed(), msg.rgb_compressed.data); + msg.rgb_compressed.format = compressedImageFormat(msg.rgb_compressed.data); } if(!data.depthOrRightRaw().empty()) @@ -322,7 +427,29 @@ void rgbdImageToROS(const rtabmap::SensorData & data, rtabmap_msgs::msg::RGBDIma } else if(!data.depthOrRightCompressed().empty()) { - UERROR("Conversion of compressed SensorData to RGBDImage is not implemented..."); + msg.depth_compressed.header = header; + if(!data.stereoCameraModels().empty()) + { + // Right image: JPEG or PNG, read as is by compressed_image_transport + compressedMatToBytes(data.depthOrRightCompressed(), msg.depth_compressed.data); + msg.depth_compressed.format = compressedImageFormat(msg.depth_compressed.data); + } + else if(legacyDepthCompression) + { + // rtabmap's format, see uncompressDepthImage() + compressedMatToBytes(data.depthOrRightCompressed(), msg.depth_compressed.data); + msg.depth_compressed.format = rtabmap::compressedDepthFormat(data.depthOrRightCompressed()).substr(1); + } + else if(!rtabmapToCompressedDepthTransport(data.depthOrRightCompressed(), msg.depth_compressed, false)) + { + // Legacy 32FC1 format (4 channels PNG), no compressedDepth equivalent + const cv::Mat depth = rtabmap::uncompressImage(data.depthOrRightCompressed()); + if(depth.type() != CV_32FC1 || !compressDepthImage(depth, ".png", msg.depth_compressed)) + { + UERROR("Could not convert compressed depth image (%d bytes) to compressedDepth format.", + (int)data.depthOrRightCompressed().total()); + } + } } //convert features @@ -501,6 +628,22 @@ rtabmap::SensorData rgbdImageFromROS(const rtabmap_msgs::msg::RGBDImage::ConstSh rtabmap_conversions::timestampFromROS(image->header.stamp)); } + if(data.isValid()) + { + // Features, in the camera frame like the local transform (identity here) + if(!image->key_points.empty()) + { + data.setFeatures( + keypointsFromROS(image->key_points), + points3fFromROS(image->points), + rtabmap::uncompressData(image->descriptors)); + } + if(!image->global_descriptor.data.empty()) + { + data.addGlobalDescriptor(globalDescriptorFromROS(image->global_descriptor)); + } + } + return data; } @@ -529,6 +672,348 @@ cv::Mat compressedMatFromBytes(const std::vector & bytes, bool co return out; } +cv::Mat compressedDepthTransportToRtabmap(const sensor_msgs::msg::CompressedImage & msg) +{ + // Same parsing as compressed_depth_image_transport + const std::string & format = msg.format; + const size_t split = format.find(';'); + const std::string encoding = format.substr(0, split); + const bool rvl = split != std::string::npos && format.find("compressedDepth rvl", split) != std::string::npos; + if(split != std::string::npos && !rvl && + format.find("compressedDepth png", split) == std::string::npos && + (format.find("compressedDepth", split) == std::string::npos || format.find("compressedDepth ", split) != std::string::npos)) + { + UERROR("Unsupported compressedDepth format \"%s\".", format.c_str()); + return cv::Mat(); + } + const bool inverseDepth = encoding == sensor_msgs::image_encodings::TYPE_32FC1; + if(!inverseDepth && + encoding != sensor_msgs::image_encodings::TYPE_16UC1 && + encoding != sensor_msgs::image_encodings::MONO16) + { + UERROR("Unsupported compressedDepth encoding \"%s\" (format=\"%s\"), only 32FC1, 16UC1 and mono16 are supported.", + encoding.c_str(), format.c_str()); + return cv::Mat(); + } + // RVL payload starts with uint32 cols, uint32 rows + if(msg.data.size() <= sizeof(CompressedDepthConfigHeader) + (rvl?8:0)) + { + UERROR("compressedDepth data is truncated (%d bytes).", (int)msg.data.size()); + return cv::Mat(); + } + CompressedDepthConfigHeader header; + memcpy(&header, msg.data.data(), sizeof(CompressedDepthConfigHeader)); + if(inverseDepth && header.format != 0) + { + UERROR("Unsupported compressedDepth compression format %d (only 0=INV_DEPTH is supported).", header.format); + return cv::Mat(); + } + const unsigned char * payload = msg.data.data() + sizeof(CompressedDepthConfigHeader); + const size_t payloadSize = msg.data.size() - sizeof(CompressedDepthConfigHeader); + + cv::Mat bytes(1, (int)((inverseDepth?rtabmap::kCompressedDepthInvHeaderSize:0) + (rvl?8:0) + payloadSize), CV_8UC1); + unsigned char * out = bytes.data; + if(inverseDepth) + { + memcpy(out, rtabmap::kCompressedDepthInvSignature, 8); + memcpy(out+8, &header.depthParam[0], 4); + memcpy(out+12, &header.depthParam[1], 4); + out += rtabmap::kCompressedDepthInvHeaderSize; + } + if(rvl) + { + memcpy(out, rtabmap::kCompressedDepthRvlSignature, 8); + out += 8; + } + memcpy(out, payload, payloadSize); + return bytes; +} + +bool rtabmapToCompressedDepthTransport(const cv::Mat & compressed, sensor_msgs::msg::CompressedImage & msg, bool logErrors) +{ + UASSERT(compressed.empty() || compressed.type() == CV_8UC1); + const unsigned char * bytes = compressed.data; + size_t size = compressed.total(); + + CompressedDepthConfigHeader header; + header.format = 0; // INV_DEPTH, also set for 16UC1 by compressed_depth_image_transport + header.depthParam[0] = header.depthParam[1] = 0.0f; + std::string encoding = sensor_msgs::image_encodings::TYPE_16UC1; + if(hasSignature(bytes, size, rtabmap::kCompressedDepthInvSignature) && size > rtabmap::kCompressedDepthInvHeaderSize) + { + encoding = sensor_msgs::image_encodings::TYPE_32FC1; + memcpy(&header.depthParam[0], bytes+8, 4); + memcpy(&header.depthParam[1], bytes+12, 4); + bytes += rtabmap::kCompressedDepthInvHeaderSize; + size -= rtabmap::kCompressedDepthInvHeaderSize; + } + + std::string codec; + std::vector png; + if(hasSignature(bytes, size, rtabmap::kCompressedDepthRvlSignature) && size >= rtabmap::kCompressedDepthRvlHeaderSize) + { +#ifdef PRE_ROS_JAZZY + // compressed_depth_image_transport decodes RVL only since Jazzy: re-compress + // the 16 bits image as PNG, losslessly. + png = rtabmap::compressImage(rtabmap::uncompressImage(bytes, size), ".png"); + bytes = png.data(); + size = png.size(); + codec = "png"; +#else + codec = "rvl"; + bytes += 8; // keep uint32 cols, uint32 rows, data + size -= 8; +#endif + } + else if(hasSignature(bytes, size, kPngSignature) && size > 25 && + bytes[24] == 16 && bytes[25] == 0) // IHDR: 16 bits grayscale + { + codec = "png"; + } + if(codec.empty() || size == 0) + { + if(logErrors) + { + UERROR("Compressed depth image (%d bytes) cannot be converted to compressedDepth format, " + "only 16UC1 depth images compressed in PNG or RVL and 32FC1 depth images " + "compressed in inverse depth format are supported (legacy 32FC1 PNG format " + "is not supported, see Mem/DepthCompressionFormat).", (int)compressed.total()); + } + return false; + } + + msg.format = encoding + "; compressedDepth " + codec; + msg.data.resize(sizeof(CompressedDepthConfigHeader) + size); + memcpy(msg.data.data(), &header, sizeof(CompressedDepthConfigHeader)); + memcpy(msg.data.data() + sizeof(CompressedDepthConfigHeader), bytes, size); + return true; +} + +std::string compressedImageTransportFormat(const std::vector & data) +{ + const std::string format = compressedImageFormat(data); + return format.find(';') == std::string::npos ? std::string() : format; +} + +bool isValidImageCompressionFormat(const std::string & format) +{ + return format == ".jpg" || format == ".png" || format == "jpeg" || format == "png"; +} + +bool toCompressedImageMsg(const cv_bridge::CvImage & image, const std::string & inputFormat, sensor_msgs::msg::CompressedImage & msg) +{ + if(!isValidImageCompressionFormat(inputFormat)) + { + UERROR("Unsupported compressed image format \"%s\" (should be \".jpg\", \".png\", \"jpeg\" or \"png\").", inputFormat.c_str()); + return false; + } + // compressed_image_transport's names + const std::string format = inputFormat == ".jpg" || inputFormat == "jpeg" ? "jpeg" : "png"; + try + { + image.toCompressedImageMsg(msg, format == "jpeg" ? cv_bridge::JPEG : cv_bridge::PNG); + } + catch(const std::exception & e) + { + UERROR("Could not compress image (encoding=\"%s\") as %s: %s", image.encoding.c_str(), format.c_str(), e.what()); + return false; + } + if(msg.data.empty()) + { + UERROR("Could not compress image (encoding=\"%s\") as %s.", image.encoding.c_str(), format.c_str()); + return false; + } + // Encoding of what was compressed, see cv_bridge::CvImage::toCompressedImageMsg() + std::string encoding = image.encoding; + if(encoding != sensor_msgs::image_encodings::BGR8 && + encoding != sensor_msgs::image_encodings::BGRA8 && + encoding != sensor_msgs::image_encodings::MONO8 && + encoding != sensor_msgs::image_encodings::MONO16) + { + encoding = sensor_msgs::image_encodings::hasAlpha(encoding) ? + sensor_msgs::image_encodings::BGRA8 : sensor_msgs::image_encodings::BGR8; + } + msg.format = encoding + "; " + format + " compressed " + encoding; + return true; +} + +cv_bridge::CvImagePtr uncompressDepthImage(const sensor_msgs::msg::CompressedImage & msg) +{ + cv_bridge::CvImagePtr ptr = std::make_shared(); + ptr->header = msg.header; + if(msg.data.empty()) + { + return ptr; + } + if(msg.format.find("compressedDepth") != std::string::npos) + { + ptr->image = rtabmap::uncompressImage(compressedDepthTransportToRtabmap(msg)); + } + else + { + // rtabmap's formats, or JPEG/PNG right image + ptr->image = rtabmap::uncompressImage(msg.data); + } + // Pick the encoding from what actually came out: the format string cannot tell + // a 16 bits depth PNG from a right image compressed as PNG. + switch(ptr->image.empty() ? -1 : ptr->image.type()) + { + case CV_32FC1: ptr->encoding = sensor_msgs::image_encodings::TYPE_32FC1; break; + case CV_16UC1: ptr->encoding = sensor_msgs::image_encodings::TYPE_16UC1; break; + case CV_8UC1: ptr->encoding = sensor_msgs::image_encodings::MONO8; break; + case CV_8UC3: ptr->encoding = sensor_msgs::image_encodings::BGR8; break; + default: + ptr->image = cv::Mat(); + break; + } + if(ptr->image.empty()) + { + UERROR("Could not decompress the depth/right image (format=\"%s\", %d bytes).", + msg.format.c_str(), (int)msg.data.size()); + } + return ptr; +} + +namespace { + +// rtabmap's format of "legacy" or "legacy:", empty if @p format is not legacy +std::string legacyDepthCompressionFormat(const std::string & format) +{ + if(format == "legacy") + { + return ".png"; + } + if(format.compare(0, 7, "legacy:") == 0) + { + return format.substr(7); + } + return ""; +} + +} // namespace + +void rgbdImageCompressedToRtabmap(const rtabmap_msgs::msg::RGBDImage & msg, cv::Mat & rgb, cv::Mat & depth) +{ + rgb = msg.rgb.data.empty() && !msg.rgb_compressed.data.empty() ? + compressedMatFromBytes(msg.rgb_compressed.data) : cv::Mat(); + depth = cv::Mat(); + if(msg.depth.data.empty() && !msg.depth_compressed.data.empty()) + { + depth = msg.depth_compressed.format.find("compressedDepth") != std::string::npos ? + compressedDepthTransportToRtabmap(msg.depth_compressed) : + compressedMatFromBytes(msg.depth_compressed.data); + } +} + +bool isCompressedRGBDSupportedByRtabmap(const cv::Mat & rgb, const cv::Mat & depth, bool stereo) +{ + if(rgb.empty() && depth.empty()) + { + return false; + } + if(!rgb.empty()) + { + UASSERT(rgb.type() == CV_8UC1); + const std::string encoding = compressedImageEncoding(rgb); + if(encoding != sensor_msgs::image_encodings::MONO8 && encoding != sensor_msgs::image_encodings::BGR8) + { + return false; + } + } + if(!depth.empty()) + { + UASSERT(depth.type() == CV_8UC1); + if(stereo) + { + // Right image, used in gray + return compressedImageEncoding(depth) == sensor_msgs::image_encodings::MONO8; + } + const unsigned char * d = depth.data; + const size_t size = depth.total(); + const bool rtabmapFormat = + hasSignature(d, size, rtabmap::kCompressedDepthRvlSignature) || + hasSignature(d, size, rtabmap::kCompressedDepthInvSignature); + // IHDR: 16 bits gray (16UC1), or 8 bits RGBA (legacy 32FC1) + const bool png = size > 25 && hasSignature(d, size, kPngSignature) && + ((d[24] == 16 && d[25] == 0) || (d[24] == 8 && d[25] == 6)); + if(!rtabmapFormat && !png) + { + return false; + } + } + return true; +} + +bool isValidDepthCompressionFormat(const std::string & format) +{ + const std::string legacy = legacyDepthCompressionFormat(format); + std::string codec; + float maxDepth, quantization; + return rtabmap::parseImageCompressionFormat(legacy.empty() ? format : legacy, codec, maxDepth, quantization) && + (codec == ".png" || codec == ".rvl"); +} + +bool compressDepthImage(const cv::Mat & depth, const std::string & format, sensor_msgs::msg::CompressedImage & msg) +{ + if(depth.type() != CV_16UC1 && depth.type() != CV_32FC1) + { + UERROR("Depth image should be 16UC1 or 32FC1 (type=%d).", depth.type()); + return false; + } + if(!isValidDepthCompressionFormat(format)) + { + UERROR("Invalid depth compression format \"%s\".", format.c_str()); + return false; + } + + const std::string legacy = legacyDepthCompressionFormat(format); + if(!legacy.empty()) + { + // rtabmap's own format, as Mem/DepthCompressionFormat: 32FC1 without depth + // parameters losslessly as 4 channels PNG. Readable by rtabmap_ros before 0.24, + // except inverse depth. + std::vector bytes = rtabmap::compressImage(depth, legacy); + if(bytes.empty()) + { + UERROR("Could not compress depth image with format \"%s\".", format.c_str()); + return false; + } + msg.format = rtabmap::compressedDepthFormat(bytes).substr(1); + msg.data = std::move(bytes); + return true; + } + + std::string codec; + float maxDepth, quantization; + rtabmap::parseImageCompressionFormat(format, codec, maxDepth, quantization); + cv::Mat image = depth; + if(depth.type() == CV_32FC1 && maxDepth <= 0.0f) + { + // No inverse depth parameters: rounded to millimeters, 0 (invalid) over 65.535 m + image = cv::Mat(depth.size(), CV_16UC1); + for(int i=0; i(i); + uint16_t * out = image.ptr(i); + for(int j=0; j= 1.0f && mm < 65536.0f ? (uint16_t)mm : 0; // false for NaN + } + } + } + const cv::Mat bytes = rtabmap::compressImage2(image, image.type() == CV_32FC1 ? format : codec); + sensor_msgs::msg::CompressedImage out; + if(bytes.empty() || !rtabmapToCompressedDepthTransport(bytes, out)) + { + UERROR("Could not compress depth image with format \"%s\".", format.c_str()); + return false; + } + msg.format = std::move(out.format); + msg.data = std::move(out.data); + return true; +} + void infoFromROS(const rtabmap_msgs::msg::Info & info, rtabmap::Statistics & stat) { stat.setExtended(true); // Extended @@ -1274,7 +1759,9 @@ rtabmap::SensorData sensorDataFromROS(const rtabmap_msgs::msg::SensorData & msg) compressedMatFromBytes(msg.left_compressed), compressedMatFromBytes(msg.right_compressed), stereoModels); - if(!left.empty() && !right.empty()) + // Raw images override their compressed ones; an image without its raw + // version stays compressed only. + if(!left.empty() || !right.empty()) { s.setStereoImage(left, right, stereoModels, false); } @@ -1285,7 +1772,9 @@ rtabmap::SensorData sensorDataFromROS(const rtabmap_msgs::msg::SensorData & msg) compressedMatFromBytes(msg.left_compressed), compressedMatFromBytes(msg.right_compressed), models); - if(!left.empty() && !right.empty()) + // Raw images override their compressed ones; an image without its raw + // version stays compressed only. + if(!left.empty() || !right.empty()) { s.setRGBDImage(left, right, models, false); } @@ -2184,7 +2673,13 @@ bool convertRGBDMsgs( int depthWidth = depthMsgs.size()?depthMsgs[0]->image.cols:0; int depthHeight = depthMsgs.size()?depthMsgs[0]->image.rows:0; - bool isDepth = depthMsgs.empty() || (depthMsgs[0].get() != 0 && ( + // Without a decoded depth/right image (e.g., left compressed, see + // CommonDataSubscriber::imagesDecodedOnDemand()), the camera infos tell: a stereo pair + // has its baseline in P(0,3) of one of them. + bool isDepth = depthMsgs.empty() ? + !(depthCameraInfoMsgs.size() == cameraInfoMsgs.size() && + (cameraInfoMsgs[0].p[3] != 0.0 || depthCameraInfoMsgs[0].p[3] != 0.0)) : + (depthMsgs[0].get() != 0 && ( depthMsgs[0]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 || depthMsgs[0]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 || depthMsgs[0]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)); diff --git a/rtabmap_conversions/test/test_msg_conversion.cpp b/rtabmap_conversions/test/test_msg_conversion.cpp index 098a3d87..91f88418 100644 --- a/rtabmap_conversions/test/test_msg_conversion.cpp +++ b/rtabmap_conversions/test/test_msg_conversion.cpp @@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include #include #include @@ -2275,6 +2276,25 @@ TEST(MsgConversion, rgbdImageFromROSWithoutDepthKeepsTheColorImage) EXPECT_NEAR(data.cameraModels()[0].fx(), 525.0, 1e-9); } +/// cv_bridge labels a decoded 16 bits image by its number of channels only (mono8); +/// toCvCopy() and toCvShare() must say mono16, so that it is converted, not misread. +TEST(MsgConversion, toCvCopyLabels16BitsCompressedRgb) +{ + cv::Mat gray16(6, 8, CV_16UC1); + cv::randu(gray16, 0, 65535); + rtabmap_msgs::msg::RGBDImage::SharedPtr msg = std::make_shared(); + ASSERT_TRUE(toCompressedImageMsg(cv_bridge::CvImage(std_msgs::msg::Header(), "mono16", gray16), "png", msg->rgb_compressed)); + + cv_bridge::CvImagePtr rgb, depth; + toCvCopy(*msg, rgb, depth); + EXPECT_EQ(rgb->encoding, "mono16"); + EXPECT_EQ(rgb->image.type(), CV_16UC1); + + cv_bridge::CvImageConstPtr rgbShared, depthShared; + toCvShare(msg, rgbShared, depthShared); + EXPECT_EQ(rgbShared->encoding, "mono16"); +} + TEST(MsgConversion, toCvCopyReadsCompressedRgb) { const cv::Mat rgb(8, 8, CV_8UC3, cv::Scalar(10, 20, 30)); @@ -3681,3 +3701,668 @@ TEST(MsgConversion, imuRoundTrip) << "linear acceleration covariance at " << i; } } + +//============================================================================ +// compressedDepthTransportToRtabmap / rtabmapToCompressedDepthTransport +//============================================================================ + +namespace { + +cv::Mat makeFloatDepthRamp(int rows, int cols, float minDepth, float maxDepth) +{ + cv::Mat depth(rows, cols, CV_32FC1); + for(int r = 0; r < rows; ++r) + { + for(int c = 0; c < cols; ++c) + { + depth.at(r, c) = minDepth + (maxDepth - minDepth) * float(r * cols + c) / float(rows * cols); + } + } + return depth; +} + +// What compressed_depth_image_transport publishes for a 32FC1 or 16UC1 depth image. +sensor_msgs::msg::CompressedImage encodeCompressedDepth(const cv::Mat & depth, const std::string & codec, float maxDepth, float quantization) +{ + struct { int32_t format; float depthParam[2]; } header = {0, {0.0f, 0.0f}}; + cv::Mat image = depth; + if(depth.type() == CV_32FC1) + { + const float A = quantization * (quantization + 1.0f); + const float B = 1.0f - A / maxDepth; + image = cv::Mat(depth.size(), CV_16UC1); + for(int r = 0; r < depth.rows; ++r) + { + for(int c = 0; c < depth.cols; ++c) + { + const float d = depth.at(r, c); + image.at(r, c) = d < maxDepth ? (uint16_t)(A / d + B) : 0; + } + } + header.depthParam[0] = A; + header.depthParam[1] = B; + } + std::vector payload; + if(codec == "png") + { + cv::imencode(".png", image, payload); + } + else + { + const std::vector rtabmapRvl = rtabmap::compressImage(image, ".rvl"); + payload.assign(rtabmapRvl.begin() + 8, rtabmapRvl.end()); // without "DEPTHRVL" + } + sensor_msgs::msg::CompressedImage msg; + msg.format = (depth.type() == CV_32FC1 ? "32FC1" : "16UC1") + std::string("; compressedDepth ") + codec; + msg.data.resize(sizeof(header)); + memcpy(msg.data.data(), &header, sizeof(header)); + msg.data.insert(msg.data.end(), payload.begin(), payload.end()); + return msg; +} + +std::vector toBytes(const cv::Mat & compressed) +{ + return std::vector(compressed.data, compressed.data + compressed.total()); +} + +} // namespace + +TEST(MsgConversion, compressedDepthRoundTrip) +{ + const cv::Mat depth = makeFloatDepthRamp(48, 64, 0.2f, 9.9f); + cv::Mat depth16; + depth.convertTo(depth16, CV_16UC1, 1000.0); + const float A = 100.0f * 101.0f; + + struct Case { cv::Mat image; std::string codec; }; + const Case cases[] = {{depth, "png"}, {depth, "rvl"}, {depth16, "png"}, {depth16, "rvl"}}; + for(const Case & cs : cases) + { + const sensor_msgs::msg::CompressedImage rosMsg = encodeCompressedDepth(cs.image, cs.codec, 10.0f, 100.0f); + SCOPED_TRACE(rosMsg.format); + const bool isFloat = cs.image.type() == CV_32FC1; + + const cv::Mat compressed = compressedDepthTransportToRtabmap(rosMsg); + ASSERT_FALSE(compressed.empty()); + ASSERT_EQ(compressed.type(), CV_8UC1); + EXPECT_EQ(rtabmap::compressedDepthFormat(compressed), isFloat ? "." + cs.codec + ":10:100" : "." + cs.codec); + // Only the header changed, the payload is the same + const size_t payloadSize = rosMsg.data.size() - 12; + ASSERT_GE(compressed.total(), payloadSize); + EXPECT_EQ(memcmp(compressed.data + compressed.total() - payloadSize, rosMsg.data.data() + 12, payloadSize), 0); + + const cv::Mat restored = rtabmap::uncompressImage(compressed); + ASSERT_EQ(restored.size(), cs.image.size()); + ASSERT_EQ(restored.type(), cs.image.type()); + if(isFloat) + { + for(int r = 0; r < depth.rows; ++r) + { + for(int c = 0; c < depth.cols; ++c) + { + // compressed_depth_image_transport truncates: up to one quantization step of error + const float d = depth.at(r, c); + ASSERT_NEAR(restored.at(r, c), d, 1.02f * d * d / A + 1e-6f); + } + } + } + else + { + EXPECT_EQ(cv::countNonZero(restored != depth16), 0); + } + + // And back to ROS + sensor_msgs::msg::CompressedImage rosMsgOut; + rosMsgOut.header.frame_id = "camera"; + ASSERT_TRUE(rtabmapToCompressedDepthTransport(compressed, rosMsgOut)); + EXPECT_EQ(rosMsgOut.header.frame_id, "camera"); +#ifdef PRE_ROS_JAZZY + if(cs.codec == "rvl") + { + // compressed_depth_image_transport cannot decode RVL before Jazzy: re-compressed + // as PNG, with the same 16 bits values + EXPECT_EQ(rosMsgOut.format, (isFloat ? "32FC1" : "16UC1") + std::string("; compressedDepth png")); + EXPECT_EQ(cv::countNonZero(rtabmap::uncompressImage(compressedDepthTransportToRtabmap(rosMsgOut)) != restored), 0); + continue; + } +#endif + EXPECT_EQ(rosMsgOut.format, rosMsg.format); + EXPECT_EQ(rosMsgOut.data, rosMsg.data); + + // Depth images compressed by rtabmap can be published as compressedDepth + const cv::Mat rtabmapCompressed = rtabmap::compressImage2(cs.image, "." + cs.codec + ":10:100"); + ASSERT_TRUE(rtabmapToCompressedDepthTransport(rtabmapCompressed, rosMsgOut)); + EXPECT_EQ(rosMsgOut.format, rosMsg.format); + EXPECT_EQ(toBytes(compressedDepthTransportToRtabmap(rosMsgOut)), toBytes(rtabmapCompressed)); + } +} + +TEST(MsgConversion, compressedDepthTransportToRtabmapOldFormatIsPng) +{ + sensor_msgs::msg::CompressedImage msg = encodeCompressedDepth(makeFloatDepthRamp(8, 8, 0.5f, 5.0f), "png", 10.0f, 100.0f); + msg.format = "32FC1; compressedDepth"; + EXPECT_EQ(rtabmap::compressedDepthFormat(compressedDepthTransportToRtabmap(msg)), ".png:10:100"); + msg.format = "mono16; compressedDepth png"; + EXPECT_EQ(rtabmap::compressedDepthFormat(compressedDepthTransportToRtabmap(msg)), ".png"); +} + +TEST(MsgConversion, compressedDepthUnsupported) +{ + const cv::Mat depth = makeFloatDepthRamp(8, 8, 0.5f, 5.0f); + sensor_msgs::msg::CompressedImage msg = encodeCompressedDepth(depth, "png", 10.0f, 100.0f); + + sensor_msgs::msg::CompressedImage bad = msg; + bad.format = "32FC1; compressedDepth zstd"; + EXPECT_TRUE(compressedDepthTransportToRtabmap(bad).empty()); + bad.format = "bgr8; compressedDepth png"; + EXPECT_TRUE(compressedDepthTransportToRtabmap(bad).empty()); + bad = msg; + bad.data.resize(12); + EXPECT_TRUE(compressedDepthTransportToRtabmap(bad).empty()); + EXPECT_TRUE(compressedDepthTransportToRtabmap(sensor_msgs::msg::CompressedImage()).empty()); + + // Legacy 32FC1 (4 channels PNG), 8 bits and empty images have no compressedDepth equivalent + sensor_msgs::msg::CompressedImage out; + out.format = "unchanged"; + EXPECT_FALSE(rtabmapToCompressedDepthTransport(rtabmap::compressImage2(depth, ".png"), out)); + EXPECT_FALSE(rtabmapToCompressedDepthTransport(rtabmap::compressImage2(cv::Mat::ones(8, 8, CV_8UC1), ".png"), out)); + EXPECT_FALSE(rtabmapToCompressedDepthTransport(cv::Mat(), out)); + EXPECT_EQ(out.format, "unchanged"); + EXPECT_TRUE(out.data.empty()); +} + +//============================================================================ +// uncompressDepthImage / compressDepthImage +//============================================================================ + +TEST(MsgConversion, uncompressDepthImageAcceptsEveryFormat) +{ + const cv::Mat depth32F = makeFloatDepthRamp(6, 8, 0.5f, 8.0f); + cv::Mat depth16U; + depth32F.convertTo(depth16U, CV_16UC1, 1000.0); + const cv::Mat gray(6, 8, CV_8UC1, cv::Scalar(77)); + const cv::Mat bgr(6, 8, CV_8UC3, cv::Scalar(1, 2, 3)); + + struct Case + { + std::string name; + sensor_msgs::msg::CompressedImage msg; + cv::Mat expected; + std::string encoding; + float tolerance; + }; + std::vector cases; + const auto rtabmapMsg = [](const cv::Mat & image, const std::string & format) { + sensor_msgs::msg::CompressedImage msg; + msg.header.frame_id = "camera"; + EXPECT_TRUE(compressDepthImage(image, format, msg)) << format; + return msg; + }; + cases.push_back({"png 16UC1", rtabmapMsg(depth16U, ".png"), depth16U, "16UC1", 0.0f}); + cases.push_back({"rvl 16UC1", rtabmapMsg(depth16U, ".rvl"), depth16U, "16UC1", 0.0f}); + cases.push_back({"32FC1 in millimeters", rtabmapMsg(depth32F, ".png"), depth16U, "16UC1", 1.0f}); + cases.push_back({"inverse depth", rtabmapMsg(depth32F, ".rvl:10:100"), depth32F, "32FC1", 0.004f}); + cases.push_back({"legacy 16UC1", rtabmapMsg(depth16U, "legacy"), depth16U, "16UC1", 0.0f}); + cases.push_back({"legacy 32FC1", rtabmapMsg(depth32F, "legacy"), depth32F, "32FC1", 0.0f}); + { + // rtabmap_ros < 0.24 (rtabmap's bytes with a format of rtabmap's codec) + sensor_msgs::msg::CompressedImage msg; + msg.header.frame_id = "camera"; + msg.format = "rvl"; + msg.data = rtabmap::compressImage(depth16U, ".rvl"); + cases.push_back({"rtabmap rvl", msg, depth16U, "16UC1", 0.0f}); + msg.format = "png:10:100"; + msg.data = rtabmap::compressImage(depth32F, ".png:10:100"); + cases.push_back({"rtabmap inverse depth", msg, depth32F, "32FC1", 0.004f}); + } + { + sensor_msgs::msg::CompressedImage msg = encodeCompressedDepth(depth32F, "png", 10.0f, 100.0f); + msg.header.frame_id = "camera"; + cases.push_back({"compressedDepth 32FC1", msg, depth32F, "32FC1", 0.008f}); + } + { + sensor_msgs::msg::CompressedImage msg = encodeCompressedDepth(depth16U, "rvl", 10.0f, 100.0f); + msg.header.frame_id = "camera"; + cases.push_back({"compressedDepth 16UC1", msg, depth16U, "16UC1", 0.0f}); + } + { + sensor_msgs::msg::CompressedImage msg; + cv_bridge::CvImage(std_msgs::msg::Header(), "mono8", gray).toCompressedImageMsg(msg, cv_bridge::PNG); + msg.header.frame_id = "camera"; + cases.push_back({"png right image", msg, gray, "mono8", 0.0f}); + } + { + sensor_msgs::msg::CompressedImage msg; + cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", bgr).toCompressedImageMsg(msg, cv_bridge::JPG); + msg.header.frame_id = "camera"; + cases.push_back({"jpeg right image", msg, bgr, "bgr8", 2.0f}); + } + + for(const Case & cs : cases) + { + SCOPED_TRACE(cs.name + " (format=\"" + cs.msg.format + "\")"); + const cv_bridge::CvImagePtr out = uncompressDepthImage(cs.msg); + ASSERT_TRUE(out); + ASSERT_EQ(out->image.type(), cs.expected.type()); + EXPECT_EQ(out->encoding, cs.encoding); + EXPECT_EQ(out->header.frame_id, "camera"); + EXPECT_LE(cv::norm(out->image, cs.expected, cv::NORM_INF), cs.tolerance); + } +} + +TEST(MsgConversion, uncompressDepthImageEmptyOrInvalid) +{ + sensor_msgs::msg::CompressedImage msg; + msg.header.frame_id = "camera"; + cv_bridge::CvImagePtr out = uncompressDepthImage(msg); + ASSERT_TRUE(out); + EXPECT_TRUE(out->image.empty()); + EXPECT_EQ(out->header.frame_id, "camera"); + + msg.format = "png"; + msg.data = {1, 2, 3, 4, 5}; + out = uncompressDepthImage(msg); + ASSERT_TRUE(out); + EXPECT_TRUE(out->image.empty()); + EXPECT_TRUE(out->encoding.empty()); + + msg.format = "32FC1; compressedDepth png"; + out = uncompressDepthImage(msg); + ASSERT_TRUE(out); + EXPECT_TRUE(out->image.empty()); +} + +TEST(MsgConversion, compressDepthImageSetsTheFormat) +{ + const cv::Mat depth32F = makeFloatDepthRamp(4, 4, 0.5f, 3.0f); + const cv::Mat depth16U(4, 4, CV_16UC1, cv::Scalar(1000)); + sensor_msgs::msg::CompressedImage msg; + msg.header.frame_id = "camera"; + +#ifdef PRE_ROS_JAZZY + const std::string rvl = "png"; // compressed_depth_image_transport cannot decode RVL before Jazzy +#else + const std::string rvl = "rvl"; +#endif + ASSERT_TRUE(compressDepthImage(depth32F, ".rvl:10:100", msg)); + EXPECT_EQ(msg.format, "32FC1; compressedDepth " + rvl); + EXPECT_EQ(msg.header.frame_id, "camera") << "the header is kept"; + ASSERT_TRUE(compressDepthImage(depth32F, ".png", msg)); + EXPECT_EQ(msg.format, "16UC1; compressedDepth png") << "32FC1 without depth parameters: millimeters"; + ASSERT_TRUE(compressDepthImage(depth16U, ".rvl:10:100", msg)); + EXPECT_EQ(msg.format, "16UC1; compressedDepth " + rvl) << "16UC1 ignores the inverse depth parameters"; + ASSERT_TRUE(compressDepthImage(depth32F, "legacy", msg)); + EXPECT_EQ(msg.format, "png"); + EXPECT_EQ(rtabmap::compressedDepthFormat(msg.data), ".png"); + const cv::Mat legacy = rtabmap::uncompressImage(msg.data); + ASSERT_EQ(legacy.type(), CV_32FC1); + EXPECT_EQ(memcmp(legacy.data, depth32F.data, depth32F.total() * depth32F.elemSize()), 0) << "lossless"; + + const std::vector previous = msg.data; + EXPECT_FALSE(compressDepthImage(cv::Mat(4, 4, CV_8UC1, cv::Scalar(1)), ".png", msg)); + EXPECT_FALSE(compressDepthImage(depth32F, ".jpg:10", msg)); + EXPECT_FALSE(compressDepthImage(depth32F, "legacy:10", msg)); + EXPECT_EQ(msg.data, previous) << "not modified on error"; + EXPECT_EQ(msg.format, "png"); + + EXPECT_TRUE(isValidDepthCompressionFormat(".png")); + EXPECT_TRUE(isValidDepthCompressionFormat(".rvl:10:100")); + EXPECT_TRUE(isValidDepthCompressionFormat("legacy")); + EXPECT_TRUE(isValidDepthCompressionFormat("legacy:.rvl")); + EXPECT_TRUE(isValidDepthCompressionFormat("legacy:.png:10:100")); + EXPECT_FALSE(isValidDepthCompressionFormat(".jpg")); + EXPECT_FALSE(isValidDepthCompressionFormat("png")); + EXPECT_FALSE(isValidDepthCompressionFormat("legacy:")); + EXPECT_FALSE(isValidDepthCompressionFormat("legacy:.jpg")); + EXPECT_FALSE(isValidDepthCompressionFormat("legacy:rvl")); +} + +TEST(MsgConversion, compressDepthImageLegacyIsRtabmapFormat) +{ + const cv::Mat depth32F = makeFloatDepthRamp(6, 8, 0.5f, 8.0f); + cv::Mat depth16U; + depth32F.convertTo(depth16U, CV_16UC1, 1000.0); + + struct Case { cv::Mat depth; std::string format; std::string expected; }; + const Case cases[] = { + {depth16U, "legacy", "png"}, + {depth16U, "legacy:.rvl", "rvl"}, + {depth16U, "legacy:.rvl:10:100", "rvl"}, + {depth32F, "legacy", "png"}, + {depth32F, "legacy:.rvl", "png"}, // RVL is 16 bits only: lossless 4 channels PNG + {depth32F, "legacy:.rvl:10:100", "rvl:10:100"}}; + for(const Case & cs : cases) + { + SCOPED_TRACE(cs.format + (cs.depth.type() == CV_32FC1 ? " 32FC1" : " 16UC1")); + sensor_msgs::msg::CompressedImage msg; + ASSERT_TRUE(compressDepthImage(cs.depth, cs.format, msg)); + EXPECT_EQ(msg.format, cs.expected); + // Same bytes as rtabmap stores in its database with Mem/DepthCompressionFormat + const std::string rtabmapFormat = cs.format == "legacy" ? ".png" : cs.format.substr(7); + EXPECT_EQ(msg.data, rtabmap::compressImage(cs.depth, rtabmapFormat)); + const cv_bridge::CvImagePtr out = uncompressDepthImage(msg); + ASSERT_EQ(out->image.type(), cs.depth.type()); + EXPECT_LE(cv::norm(out->image, cs.depth, cv::NORM_INF), cs.expected == "rvl:10:100" ? 0.004 : 0.0); + } +} + +TEST(MsgConversion, compressDepthImageInMillimeters) +{ + cv::Mat depth(1, 8, CV_32FC1); + const float values[] = {1.2344f, 1.2346f, 0.0f, -1.0f, 65.6f, + std::numeric_limits::quiet_NaN(), std::numeric_limits::infinity(), 0.0004f}; + const uint16_t expected[] = {1234, 1235, 0, 0, 0, 0, 0, 0}; + for(int i = 0; i < 8; ++i) + { + depth.at(0, i) = values[i]; + } + sensor_msgs::msg::CompressedImage msg; + ASSERT_TRUE(compressDepthImage(depth, ".png", msg)); + const cv_bridge::CvImagePtr out = uncompressDepthImage(msg); + ASSERT_EQ(out->encoding, "16UC1"); + for(int i = 0; i < 8; ++i) + { + EXPECT_EQ(out->image.at(0, i), expected[i]) << "input " << values[i]; + } +} + +TEST(MsgConversion, rgbdImageToROSConvertsCompressedDepth) +{ + const rtabmap::CameraModel model(500.0, 500.0, 4.0, 3.0, rtabmap::Transform::getIdentity()); + const cv::Mat rgb = rtabmap::compressImage2(cv::Mat(6, 8, CV_8UC3, cv::Scalar(1, 2, 3)), ".jpg"); + const cv::Mat depth32F = makeFloatDepthRamp(6, 8, 0.5f, 8.0f); + cv::Mat depth16U; + depth32F.convertTo(depth16U, CV_16UC1, 1000.0); +#ifdef PRE_ROS_JAZZY + const std::string rvl = "png"; +#else + const std::string rvl = "rvl"; +#endif + + struct Case { std::string name; cv::Mat compressed; std::string format; std::string legacyFormat; }; + const Case cases[] = { + {"16UC1 rvl", rtabmap::compressImage2(depth16U, ".rvl"), "16UC1; compressedDepth " + rvl, "rvl"}, + {"inverse depth", rtabmap::compressImage2(depth32F, ".png:10:100"), "32FC1; compressedDepth png", "png:10:100"}, + {"legacy 32FC1", rtabmap::compressImage2(depth32F, ".png"), "16UC1; compressedDepth png", "png"}}; + for(const Case & cs : cases) + { + SCOPED_TRACE(cs.name); + const rtabmap::SensorData data(rgb, cs.compressed, model, 1, 1.0); + for(bool legacy : {false, true}) + { + rtabmap_msgs::msg::RGBDImage msg; + rgbdImageToROS(data, msg, "camera", legacy); + EXPECT_EQ(msg.depth_compressed.format, legacy ? cs.legacyFormat : cs.format); + if(legacy) + { + EXPECT_EQ(msg.depth_compressed.data, toBytes(cs.compressed)) << "copied as is"; + } + const cv_bridge::CvImagePtr out = uncompressDepthImage(msg.depth_compressed); + ASSERT_FALSE(out->image.empty()); + } + } +} + +TEST(MsgConversion, rgbdImageToROSKeepsCompressedImages) +{ + 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); + const rtabmap::CameraModel model( + "cam", cv::Size(8, 6), K, cv::Mat(), cv::Mat(), cv::Mat(), + rtabmap::Transform(0.0f, 0.0f, 0.1f, 0.0f, 0.0f, 0.0f)); + const cv::Mat rgb(6, 8, CV_8UC3, cv::Scalar(10, 20, 30)); + const cv::Mat depth = makeFloatDepthRamp(6, 8, 0.5f, 8.0f); + + for(const std::string rgbFormat : {".jpg", ".png"}) + { + SCOPED_TRACE(rgbFormat); + // Compressed only, as loaded from a database + const cv::Mat rgbCompressed = rtabmap::compressImage2(rgb, rgbFormat); + const cv::Mat depthCompressed = rtabmap::compressImage2(depth, ".rvl:10:100"); + const rtabmap::SensorData in(rgbCompressed, depthCompressed, model, 1, 1234.5); + ASSERT_TRUE(in.imageRaw().empty()); + ASSERT_TRUE(in.depthRaw().empty()); + + rtabmap_msgs::msg::RGBDImage msg; + rgbdImageToROS(in, msg, "camera_link"); + EXPECT_TRUE(msg.rgb.data.empty()); + EXPECT_TRUE(msg.depth.data.empty()); + EXPECT_EQ(msg.rgb_compressed.format, rgbFormat == ".jpg" ? "bgr8; jpeg compressed bgr8" : "bgr8; png compressed bgr8"); + EXPECT_EQ(msg.rgb_compressed.data, toBytes(rgbCompressed)) << "not re-compressed"; +#ifdef PRE_ROS_JAZZY + EXPECT_EQ(msg.depth_compressed.format, "32FC1; compressedDepth png"); +#else + EXPECT_EQ(msg.depth_compressed.format, "32FC1; compressedDepth rvl"); + EXPECT_EQ(toBytes(compressedDepthTransportToRtabmap(msg.depth_compressed)), toBytes(depthCompressed)) << "not re-compressed"; +#endif + EXPECT_EQ(msg.depth_compressed.header.frame_id, "camera_link"); + + cv_bridge::CvImagePtr rgbOut, depthOut; + toCvCopy(msg, rgbOut, depthOut); + ASSERT_EQ(rgbOut->image.type(), CV_8UC3); + EXPECT_LE(cv::norm(rgbOut->image, rgb, cv::NORM_INF), rgbFormat == ".jpg" ? 3.0 : 0.0); + ASSERT_EQ(depthOut->encoding, "32FC1"); + EXPECT_LE(cv::norm(depthOut->image, depth, cv::NORM_INF), 0.004); + } +} + +//============================================================================ +// rgb_compressed: compressed_image_transport's format +//============================================================================ + +TEST(MsgConversion, toCompressedImageMsgSetsTheTransportFormat) +{ + struct Case { std::string encoding; cv::Mat image; std::string format; std::string expected; }; + const Case cases[] = { + {"bgr8", cv::Mat(6, 8, CV_8UC3, cv::Scalar(10, 20, 30)), "jpeg", "bgr8; jpeg compressed bgr8"}, + {"rgb8", cv::Mat(6, 8, CV_8UC3, cv::Scalar(10, 20, 30)), "jpeg", "bgr8; jpeg compressed bgr8"}, + {"mono8", cv::Mat(6, 8, CV_8UC1, cv::Scalar(40)), "jpeg", "mono8; jpeg compressed mono8"}, + {"bgr8", cv::Mat(6, 8, CV_8UC3, cv::Scalar(10, 20, 30)), "png", "bgr8; png compressed bgr8"}, + {"bgra8", cv::Mat(6, 8, CV_8UC4, cv::Scalar(10, 20, 30, 40)), "png", "bgra8; png compressed bgra8"}, + {"mono16", cv::Mat(6, 8, CV_16UC1, cv::Scalar(1234)), "png", "mono16; png compressed mono16"}}; + for(const Case & cs : cases) + { + SCOPED_TRACE(cs.encoding + " " + cs.format); + cv_bridge::CvImage image; + image.header.frame_id = "camera"; + image.encoding = cs.encoding; + image.image = cs.image; + sensor_msgs::msg::CompressedImage msg; + ASSERT_TRUE(toCompressedImageMsg(image, cs.format, msg)); + EXPECT_EQ(msg.format, cs.expected); + EXPECT_EQ(msg.header.frame_id, "camera"); + + // The encoding announced is the one of the data + const cv_bridge::CvImagePtr decoded = cv_bridge::toCvCopy(msg); + const std::string announced = msg.format.substr(0, msg.format.find(';')); + EXPECT_EQ(decoded->image.channels(), sensor_msgs::image_encodings::numChannels(announced)); + EXPECT_EQ(decoded->image.elemSize1() * 8, (size_t)sensor_msgs::image_encodings::bitDepth(announced)); + + // Same format as rgbdImageToROS() reads from the header of the compressed bytes + const rtabmap::CameraModel model(500.0, 500.0, 4.0, 3.0, rtabmap::Transform::getIdentity()); + const rtabmap::SensorData data(compressedMatFromBytes(msg.data), cv::Mat(), model, 1, 1.0); + rtabmap_msgs::msg::RGBDImage rgbd; + rgbdImageToROS(data, rgbd, "camera"); + EXPECT_EQ(rgbd.rgb_compressed.format, cs.expected); + } + + cv_bridge::CvImage image(std_msgs::msg::Header(), "bgr8", cv::Mat(6, 8, CV_8UC3, cv::Scalar(1, 2, 3))); + sensor_msgs::msg::CompressedImage msg; + msg.format = "unchanged"; + EXPECT_FALSE(toCompressedImageMsg(image, "jpg", msg)) << "\"jpeg\" or \".jpg\""; + EXPECT_FALSE(toCompressedImageMsg(image, "tiff", msg)); + EXPECT_FALSE(toCompressedImageMsg(image, ".bmp", msg)); + EXPECT_EQ(msg.format, "unchanged"); + + // rtabmap's names + ASSERT_TRUE(toCompressedImageMsg(image, ".jpg", msg)); + EXPECT_EQ(msg.format, "bgr8; jpeg compressed bgr8"); + ASSERT_TRUE(toCompressedImageMsg(image, ".png", msg)); + EXPECT_EQ(msg.format, "bgr8; png compressed bgr8"); +} + +/// 16 bits color PNGs: the format and the decoded encoding come from the PNG header. +TEST(MsgConversion, compressedImageTransportFormatReads16BitsColor) +{ + struct Case { cv::Mat image; std::string encoding; }; + const Case cases[] = { + {cv::Mat(6, 8, CV_16UC3, cv::Scalar(1000, 2000, 3000)), "bgr16"}, + {cv::Mat(6, 8, CV_16UC4, cv::Scalar(1000, 2000, 3000, 4000)), "bgra16"}}; + for(const Case & cs : cases) + { + SCOPED_TRACE(cs.encoding); + rtabmap_msgs::msg::RGBDImage msg; + ASSERT_TRUE(cv::imencode(".png", cs.image, msg.rgb_compressed.data)); + EXPECT_EQ(compressedImageTransportFormat(msg.rgb_compressed.data), + cs.encoding + "; png compressed " + cs.encoding); + msg.rgb_compressed.format = "png"; + + cv_bridge::CvImagePtr rgb, depth; + toCvCopy(msg, rgb, depth); + ASSERT_TRUE(rgb); + EXPECT_EQ(rgb->encoding, cs.encoding) << "not the 8 bits encoding cv_bridge gives"; + EXPECT_EQ(cv::norm(rgb->image, cs.image, cv::NORM_INF), 0.0); + } +} + +/// PNGs OpenCV does not decode to one of the supported encodings: no transport format. +TEST(MsgConversion, compressedImageTransportFormatRejectsUnsupportedPngs) +{ + std::vector png; + ASSERT_TRUE(cv::imencode(".png", cv::Mat(6, 8, CV_8UC1, cv::Scalar(40)), png)); + ASSERT_EQ(compressedImageTransportFormat(png), "mono8; png compressed mono8") << "precondition"; + + // IHDR: bit depth at 24, color type at 25 + std::vector palette = png; + palette[25] = 3; + EXPECT_EQ(compressedImageTransportFormat(palette), ""); + std::vector grayAlpha = png; + grayAlpha[25] = 4; + EXPECT_EQ(compressedImageTransportFormat(grayAlpha), ""); + std::vector oneBit = png; + oneBit[24] = 1; + EXPECT_EQ(compressedImageTransportFormat(oneBit), "") << "decoded as 8 bits by OpenCV"; + + png.resize(20); + EXPECT_EQ(compressedImageTransportFormat(png), "") << "truncated before IHDR"; +} + +/// The number of components of a JPEG is found past the markers before its frame header. +TEST(MsgConversion, compressedImageTransportFormatSkipsJpegMarkers) +{ + for(const bool color : {false, true}) + { + SCOPED_TRACE(color ? "bgr8" : "mono8"); + const std::string expected = color ? "bgr8; jpeg compressed bgr8" : "mono8; jpeg compressed mono8"; + std::vector jpeg; + ASSERT_TRUE(cv::imencode(".jpg", color ? + cv::Mat(6, 8, CV_8UC3, cv::Scalar(10, 20, 30)) : cv::Mat(6, 8, CV_8UC1, cv::Scalar(40)), jpeg)); + ASSERT_EQ(compressedImageTransportFormat(jpeg), expected) << "precondition"; + + // After the start of image: a restart marker (no length), then fill bytes before + // the next marker + std::vector markers(jpeg.begin(), jpeg.begin() + 2); + markers.insert(markers.end(), {0xFF, 0xD0, 0xFF, 0xFF}); + markers.insert(markers.end(), jpeg.begin() + 2, jpeg.end()); + EXPECT_EQ(compressedImageTransportFormat(markers), expected); + } + + // Start of image only, no frame header: the codec is known, not the encoding + const std::vector noFrame = {0xFF, 0xD8, 0xFF, 0xD9, 0x00, 0x00}; + EXPECT_EQ(compressedImageTransportFormat(noFrame), ""); + EXPECT_EQ(compressedImageTransportFormat({0xFF, 0xD8}), ""); + EXPECT_EQ(compressedImageTransportFormat({0x00, 0x01, 0x02, 0x03}), "") << "not an image"; +} + +/// The right image of a compressed stereo pair is published as is, in +/// compressed_image_transport's format. +TEST(MsgConversion, rgbdImageToROSKeepsACompressedStereoPair) +{ + const rtabmap::StereoCameraModel stereo( + 525.0, 525.0, 4.0, 3.0, 0.12, rtabmap::Transform::getIdentity(), cv::Size(8, 6)); + const cv::Mat left(6, 8, CV_8UC3, cv::Scalar(10, 20, 30)); + const cv::Mat right(6, 8, CV_8UC1, cv::Scalar(40)); + for(const std::string format : {".jpg", ".png"}) + { + SCOPED_TRACE(format); + const cv::Mat leftCompressed = rtabmap::compressImage2(left, format); + const cv::Mat rightCompressed = rtabmap::compressImage2(right, format); + rtabmap::SensorData in; + in.setStereoImage(leftCompressed, rightCompressed, stereo, false); + ASSERT_TRUE(in.imageRaw().empty()); + ASSERT_TRUE(in.rightRaw().empty()); + + rtabmap_msgs::msg::RGBDImage msg; + rgbdImageToROS(in, msg, "camera_link"); + EXPECT_TRUE(msg.depth.data.empty()); + const std::string codec = format == ".jpg" ? "jpeg" : "png"; + EXPECT_EQ(msg.depth_compressed.format, "mono8; " + codec + " compressed mono8"); + EXPECT_EQ(msg.depth_compressed.data, toBytes(rightCompressed)) << "not re-compressed"; + EXPECT_EQ(msg.depth_compressed.header.frame_id, "camera_link"); + EXPECT_EQ(msg.rgb_compressed.data, toBytes(leftCompressed)) << "not re-compressed"; + + const rtabmap::SensorData out = rgbdImageFromROS(std::make_shared(msg)); + ASSERT_EQ(out.stereoCameraModels().size(), 1u); + cv::Mat leftOut, rightOut; + out.uncompressDataConst(&leftOut, &rightOut); + ASSERT_EQ(rightOut.type(), CV_8UC1); + EXPECT_LE(cv::norm(rightOut, right, cv::NORM_INF), format == ".jpg" ? 3.0 : 0.0); + } +} + +//============================================================================ +// Mixed raw and compressed images, features of RGBDImage +//============================================================================ + +/// A SensorData message may carry one image raw and the other compressed: both are kept. +TEST(MsgConversion, sensorDataFromROSKeepsARawImageNextToACompressedOne) +{ + const rtabmap::CameraModel model(50.0, 50.0, 4.0, 3.0, rtabmap::CameraModel::opticalRotation(), 0.0, cv::Size(8, 6)); + const cv::Mat rgb(6, 8, CV_8UC3, cv::Scalar(10, 20, 30)); + const cv::Mat depth(6, 8, CV_16UC1, cv::Scalar(1500)); + + for(bool rawColor : {true, false}) + { + SCOPED_TRACE(rawColor ? "raw color, compressed depth" : "compressed color, raw depth"); + rtabmap::SensorData in(rawColor ? rgb : rtabmap::compressImage2(rgb, ".png"), + rawColor ? rtabmap::compressImage2(depth, ".png") : depth, model, 1, 1.0); + rtabmap_msgs::msg::SensorData msg; + sensorDataToROS(in, msg, "base_link", true); + + const rtabmap::SensorData out = sensorDataFromROS(msg); + EXPECT_EQ(out.imageRaw().empty(), !rawColor); + EXPECT_EQ(out.depthRaw().empty(), rawColor); + EXPECT_EQ(out.imageCompressed().empty(), rawColor); + EXPECT_EQ(out.depthOrRightCompressed().empty(), !rawColor); + const cv::Mat outRgb = rawColor ? out.imageRaw() : rtabmap::uncompressImage(out.imageCompressed()); + const cv::Mat outDepth = rawColor ? rtabmap::uncompressImage(out.depthOrRightCompressed()) : out.depthRaw(); + EXPECT_EQ(cv::norm(outRgb, rgb, cv::NORM_INF), 0.0); + EXPECT_EQ(cv::countNonZero(outDepth != depth), 0); + } +} + +/// rgbdImageFromROS() keeps the features and the global descriptor of the message. +TEST(MsgConversion, rgbdImageFromROSKeepsFeatures) +{ + const rtabmap::CameraModel model(50.0, 50.0, 4.0, 3.0, rtabmap::Transform::getIdentity(), 0.0, cv::Size(8, 6)); + rtabmap::SensorData in(cv::Mat(6, 8, CV_8UC3, cv::Scalar(1, 2, 3)), cv::Mat(6, 8, CV_16UC1, cv::Scalar(1000)), model, 1, 1.0); + const std::vector kpts = {cv::KeyPoint(1, 2, 3), cv::KeyPoint(4, 5, 6)}; + const std::vector pts = {cv::Point3f(0.1f, 0.2f, 1.0f), cv::Point3f(0.3f, 0.4f, 2.0f)}; + const cv::Mat descriptors = (cv::Mat_(2, 3) << 1, 2, 3, 4, 5, 6); + in.setFeatures(kpts, pts, descriptors); + in.addGlobalDescriptor(rtabmap::GlobalDescriptor(1, (cv::Mat_(1, 4) << 0.1f, 0.2f, 0.3f, 0.4f))); + + rtabmap_msgs::msg::RGBDImage::SharedPtr msg = std::make_shared(); + rgbdImageToROS(in, *msg, "camera"); + + const rtabmap::SensorData out = rgbdImageFromROS(msg); + ASSERT_EQ(out.keypoints().size(), 2u); + EXPECT_FLOAT_EQ(out.keypoints()[1].pt.x, 4.0f); + ASSERT_EQ(out.keypoints3D().size(), 2u); + EXPECT_NEAR(out.keypoints3D()[1].z, 2.0f, 1e-6); + ASSERT_EQ(out.descriptors().rows, 2); + EXPECT_EQ(cv::norm(out.descriptors(), descriptors, cv::NORM_INF), 0.0); + ASSERT_EQ(out.globalDescriptors().size(), 1u); + EXPECT_EQ(out.globalDescriptors()[0].type(), 1); + EXPECT_EQ(cv::norm(out.globalDescriptors()[0].data(), in.globalDescriptors()[0].data(), cv::NORM_INF), 0.0); +} diff --git a/rtabmap_msgs/msg/GlobalDescriptor.msg b/rtabmap_msgs/msg/GlobalDescriptor.msg index 704fe41d..31afb7af 100644 --- a/rtabmap_msgs/msg/GlobalDescriptor.msg +++ b/rtabmap_msgs/msg/GlobalDescriptor.msg @@ -2,7 +2,7 @@ std_msgs/Header header # compressed global descriptor -# use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h" +# use rtabmap::uncompressData() from "rtabmap/core/Compression.h" int32 type uint8[] info uint8[] data \ No newline at end of file diff --git a/rtabmap_msgs/msg/Node.msg b/rtabmap_msgs/msg/Node.msg index fd6d5ebd..429a793b 100644 --- a/rtabmap_msgs/msg/Node.msg +++ b/rtabmap_msgs/msg/Node.msg @@ -17,7 +17,7 @@ int32[] word_id_values KeyPoint[] word_kpts Point3f[] word_pts # compressed descriptors -# use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h" +# use rtabmap::uncompressData() from "rtabmap/core/Compression.h" uint8[] word_descriptors SensorData data diff --git a/rtabmap_msgs/msg/RGBDImage.msg b/rtabmap_msgs/msg/RGBDImage.msg index 51425327..ee44d8c6 100644 --- a/rtabmap_msgs/msg/RGBDImage.msg +++ b/rtabmap_msgs/msg/RGBDImage.msg @@ -12,6 +12,18 @@ sensor_msgs/Image rgb sensor_msgs/Image depth # Compressed +# rgb_compressed: JPEG or PNG in compressed_image_transport's format (e.g., +# "bgr8; jpeg compressed bgr8"), readable by the "compressed" image_transport plugin. +# depth_compressed: right image (stereo) in compressed_image_transport's format, or depth +# image in compressed_depth_image_transport's format (e.g., "16UC1; compressedDepth png", +# "32FC1; compressedDepth rvl"), readable by the "compressedDepth" image_transport +# plugin (RVL since Jazzy). +# Exception: depth compressed in rtabmap's own format (rtabmap_ros < 0.24, or +# depth_compression_format "legacy[:]", e.g., to keep 32FC1 depth lossless), with format +# "png", "rvl" or "png::" / "rvl::", which +# image_transport plugins cannot read. rtabmap_conversions::uncompressDepthImage() +# decodes every format, and rgbd_split republishes them on a compressedDepth +# image_transport topic. sensor_msgs/CompressedImage rgb_compressed sensor_msgs/CompressedImage depth_compressed @@ -19,7 +31,7 @@ sensor_msgs/CompressedImage depth_compressed KeyPoint[] key_points Point3f[] points # compressed descriptors -# use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h" +# use rtabmap::uncompressData() from "rtabmap/core/Compression.h" uint8[] descriptors GlobalDescriptor global_descriptor diff --git a/rtabmap_msgs/msg/SensorData.msg b/rtabmap_msgs/msg/SensorData.msg index 129d8a38..600c71a6 100644 --- a/rtabmap_msgs/msg/SensorData.msg +++ b/rtabmap_msgs/msg/SensorData.msg @@ -9,7 +9,11 @@ sensor_msgs/Image left sensor_msgs/Image right # Compressed images -# use rtabmap::util3d::uncompressImage() from "rtabmap/core/util3d.h" +# use rtabmap::uncompressImage() from "rtabmap/core/Compression.h" +# right_compressed holds a depth image in rtabmap's format (see Mem/DepthCompressionFormat), +# not readable by image_transport plugins: see +# rtabmap_conversions::rtabmapToCompressedDepthTransport() to convert it without +# decompression to compressed_depth_image_transport's format. uint8[] left_compressed uint8[] right_compressed @@ -23,7 +27,7 @@ geometry_msgs/Transform[] local_transform # raw 2d or 3D laser scan sensor_msgs/PointCloud2 laser_scan # compressed 2D or 3D laser scan -# use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h" +# use rtabmap::uncompressData() from "rtabmap/core/Compression.h" uint8[] laser_scan_compressed int32 laser_scan_max_pts float32 laser_scan_max_range @@ -32,11 +36,11 @@ int32 laser_scan_format geometry_msgs/Transform laser_scan_local_transform # compressed user data -# use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h" +# use rtabmap::uncompressData() from "rtabmap/core/Compression.h" uint8[] user_data # compressed occupancy grid -# use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h" +# use rtabmap::uncompressData() from "rtabmap/core/Compression.h" uint8[] grid_ground uint8[] grid_obstacles uint8[] grid_empty_cells @@ -47,7 +51,7 @@ Point3f grid_view_point KeyPoint[] key_points Point3f[] points # compressed descriptors -# use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h" +# use rtabmap::uncompressData() from "rtabmap/core/Compression.h" uint8[] descriptors GlobalDescriptor[] global_descriptors diff --git a/rtabmap_odom/README.md b/rtabmap_odom/README.md index 8ebd0f3d..3dbc8b73 100644 --- a/rtabmap_odom/README.md +++ b/rtabmap_odom/README.md @@ -19,6 +19,7 @@ SLAM needs a pose for every measurement it maps. These nodes produce one by regi - [Services](#services) - [Published topics](#published-topics) - [Outputting filtered scans and features](#outputting-filtered-scans-and-features) + - [Compressed sensor data](#compressed-sensor-data) - [Diagnostics](#diagnostics) - [License](#license) @@ -254,7 +255,7 @@ Common to all three nodes. **Every one of them, `odom` included, is published on | `odom_local_scan_map` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | The scan map, the ICP path's equivalent of `odom_local_map`. | | `odom_last_frame` | [`sensor_msgs/msg/PointCloud2`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/PointCloud2.html) | The current frame's **features**, in the odom frame — not its scan or its pixels. Visual paths only, for the same reason as `odom_local_map`; for the filtered scan see `odom_sensor_data/*`. | | `odom_rgbd_image` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | The frame **as odometry processed it**, not the input as it arrived. See [Outputting filtered scans and features](#outputting-filtered-scans-and-features). | -| `odom_sensor_data/raw`, `/features`, `/compressed` | [`rtabmap_msgs/msg/SensorData`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/SensorData.html) | The same frame as `SensorData`. `/features` strips the images and scan and keeps only the extracted features; `/compressed` carries JPEG/PNG images instead of raw. | +| `odom_sensor_data/raw`, `/features`, `/compressed` | [`rtabmap_msgs/msg/SensorData`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/SensorData.html) | The same frame as `SensorData`. `/features` strips the images and scan and keeps only the extracted features; `/compressed` carries compressed images instead of raw, see [Compressed sensor data](#compressed-sensor-data). | ### Outputting filtered scans and features @@ -264,6 +265,18 @@ Common to all three nodes. **Every one of them, `odom` included, is published on - **The scan is the filtered one.** `icp_odometry` builds the frame after deskewing, voxelization, range filtering and normal estimation, so what comes out here is the decimated cloud ICP saw — not the raw sweep the lidar published. Subscribe to the driver's topic if you want the original. - **Images are converted.** `rgbd_odometry` hands over grayscale unless `keep_color` is set, so that is what these carry too. +### Compressed sensor data + +`odom_sensor_data/compressed` carries the frame with its images and scan compressed, for a slow link. It is only built when something subscribes to it. + +| Parameter | Type | Default | Description | +|---|---|---|---| +| `sensor_data_compression_format` | `string` | `".jpg"` | Format of the color image, and of the right image of a stereo pair: `".jpg"` or `".png"`. | +| `sensor_data_depth_compression_format` | `string` | `".rvl"` | Format of the depth image: `".rvl"` (lossless, faster) or `".png"` (lossless, smaller), optionally followed by `":[:]"` (e.g. `".rvl:10:100"`) to compress 32FC1 depth as 16 bits inverse depth, as `Mem/DepthCompressionFormat`. That is lossy -- depth over `maxDepth` is lost and the error grows with the square of the depth (~0.05 mm at 1 m and 5 mm at 10 m with a quantization of 100) -- but much smaller than the lossless 32FC1 PNG. 16UC1 depth is always lossless. An invalid value falls back to `".rvl"` with an error. | +| `sensor_data_parallel_compression` | `bool` | `true` | Compress the images and the scan in parallel threads. | + +The depth is in rtabmap's format (`rtabmap::uncompressImage()` reads it), not in `compressed_depth_image_transport`'s; `rtabmap_conversions::rtabmapToCompressedDepthTransport()` converts it without decompressing it. + ## Diagnostics All three publish to `/diagnostics`: the input rate, the output rate, and how many frames were processed versus dropped. A healthy input rate with a low output rate means frames are arriving but not registering — check `odom_info` before touching anything else. diff --git a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h index b0e8328c..8b36c214 100644 --- a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h +++ b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h @@ -270,6 +270,7 @@ private: double minUpdateRate_; bool alwaysProcessMostRecentFrame_; std::string compressionImgFormat_; + std::string compressionDepthFormat_; bool compressionParallelized_; int odomStrategy_; bool waitIMUToinit_; diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp index fa128131..e1408548 100644 --- a/rtabmap_odom/src/OdometryROS.cpp +++ b/rtabmap_odom/src/OdometryROS.cpp @@ -138,6 +138,7 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o minUpdateRate_(0.0), alwaysProcessMostRecentFrame_(true), compressionImgFormat_(".jpg"), + compressionDepthFormat_(".rvl"), compressionParallelized_(true), odomStrategy_(Parameters::defaultOdomStrategy()), waitIMUToinit_(false), @@ -194,6 +195,18 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o alwaysProcessMostRecentFrame_ = this->declare_parameter("always_process_most_recent_frame", alwaysProcessMostRecentFrame_); compressionImgFormat_ = this->declare_parameter("sensor_data_compression_format", compressionImgFormat_); + compressionDepthFormat_ = this->declare_parameter("sensor_data_depth_compression_format", compressionDepthFormat_); + { + std::string codec; + float maxDepth, quantization; + if(!rtabmap::parseImageCompressionFormat(compressionDepthFormat_, codec, maxDepth, quantization) || + (codec != ".png" && codec != ".rvl")) + { + RCLCPP_ERROR(this->get_logger(), "Invalid sensor_data_depth_compression_format \"%s\" (should be \".png\" or \".rvl\", " + "optionally followed by \":maxDepth[:quantization]\"), using \".rvl\".", compressionDepthFormat_.c_str()); + compressionDepthFormat_ = ".rvl"; + } + } compressionParallelized_ = this->declare_parameter("sensor_data_parallel_compression", compressionParallelized_); waitIMUToinit_ = this->declare_parameter("wait_imu_to_init", waitIMUToinit_); @@ -268,6 +281,7 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o RCLCPP_INFO(this->get_logger(), "Odometry: wait_imu_to_init = %s", waitIMUToinit_?"true":"false"); RCLCPP_INFO(this->get_logger(), "Odometry: always_check_imu_tf = %s", alwaysCheckImuTf_?"true":"false"); RCLCPP_INFO(this->get_logger(), "Odometry: sensor_data_compression_format = %s", compressionImgFormat_.c_str()); + RCLCPP_INFO(this->get_logger(), "Odometry: sensor_data_depth_compression_format = %s", compressionDepthFormat_.c_str()); RCLCPP_INFO(this->get_logger(), "Odometry: sensor_data_parallel_compression = %s", compressionParallelized_?"true":"false"); } @@ -1300,7 +1314,11 @@ void OdometryROS::processData() postProcessData(data, header); - if(!data.imageRaw().empty() && odomRgbdImagePub_->get_subscription_count()>0) + // Any frame with a camera, also with features only or compressed images only (lidar + // odometry sets an invalid CameraModel() as placeholder) + if(((!data.cameraModels().empty() && data.cameraModels()[0].isValidForProjection()) || + (!data.stereoCameraModels().empty() && data.stereoCameraModels()[0].isValidForProjection())) && + odomRgbdImagePub_->get_subscription_count()>0) { if(!header.frame_id.empty()) { @@ -1344,7 +1362,7 @@ void OdometryROS::processData() if(compressionParallelized_) { rtabmap::CompressionThread ctImage(data.imageRaw(), compressionImgFormat_); - rtabmap::CompressionThread ctDepth(data.depthOrRightRaw(), data.depthOrRightRaw().type() == CV_32FC1 || data.depthOrRightRaw().type() == CV_16UC1?std::string(".png"):compressionImgFormat_); + rtabmap::CompressionThread ctDepth(data.depthOrRightRaw(), data.depthOrRightRaw().type() == CV_32FC1 || data.depthOrRightRaw().type() == CV_16UC1?compressionDepthFormat_:compressionImgFormat_); rtabmap::CompressionThread ctLaserScan(data.laserScanRaw().data()); if(!data.imageRaw().empty()) { @@ -1369,7 +1387,7 @@ void OdometryROS::processData() else { compressedImage = compressImage2(data.imageRaw(), compressionImgFormat_); - compressedDepth = compressImage2(data.depthOrRightRaw(), data.depthOrRightRaw().type() == CV_32FC1 || data.depthOrRightRaw().type() == CV_16UC1?std::string(".png"):compressionImgFormat_); + compressedDepth = compressImage2(data.depthOrRightRaw(), data.depthOrRightRaw().type() == CV_32FC1 || data.depthOrRightRaw().type() == CV_16UC1?compressionDepthFormat_:compressionImgFormat_); compressedScan = compressData2(data.laserScanRaw().data()); } if(!compressedImage.empty() && !data.stereoCameraModels().empty()) diff --git a/rtabmap_odom/test/test_icp_odometry.cpp b/rtabmap_odom/test/test_icp_odometry.cpp index eeac9099..d3c9bd33 100644 --- a/rtabmap_odom/test/test_icp_odometry.cpp +++ b/rtabmap_odom/test/test_icp_odometry.cpp @@ -4,6 +4,7 @@ All rights reserved. (BSD-3-Clause, see the repository root.) */ #include +#include #include @@ -584,6 +585,28 @@ TEST_F(IcpOdometryTest, publishes_an_identity_pose_for_the_first_scan_cloud) EXPECT_NEAR(0.0, msg.pose.pose.position.z, 1e-6); } +/// Without a camera there is no RGBDImage to republish: odom_rgbd_image stays silent, +/// while the odometry itself is published. +TEST_F(IcpOdometryTest, does_not_publish_an_rgbd_image_without_a_camera) +{ + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + std::shared_ptr> frames = + collect("odom_rgbd_image"); + makeNode(icpTestParameters()); + + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("scan_cloud", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + ASSERT_TRUE(waitForPublisher(frames->subscription)); + + pub->publish(makeXYZCloud("lidar", 1.0, corner3D())); + ASSERT_TRUE(spinUntil([&]() { return !odom->empty(); })); + spinFor(std::chrono::milliseconds(200)); + EXPECT_TRUE(frames->empty()); +} + /// A 2D lidar goes in on `scan` instead of `scan_cloud`, and reaches the same odometry. TEST_F(IcpOdometryTest, accepts_a_laser_scan) { diff --git a/rtabmap_odom/test/test_odometry_ros.cpp b/rtabmap_odom/test/test_odometry_ros.cpp index 0e20c00e..5c4104c0 100644 --- a/rtabmap_odom/test/test_odometry_ros.cpp +++ b/rtabmap_odom/test/test_odometry_ros.cpp @@ -987,6 +987,26 @@ TEST_F(OdometryRosTest, publishes_compressed_sensor_data_when_asked) << "the raw scan should have been replaced by the compressed one"; } +/// Same, compressing in the odometry thread instead of one thread per sensor. +TEST_F(OdometryRosTest, publishes_compressed_sensor_data_without_parallel_compression) +{ + publishSensorTf(); + std::shared_ptr> odom = + collect("odom"); + std::shared_ptr> compressed = + collect("odom_sensor_data/compressed"); + makeNode({rclcpp::Parameter("publish_compressed_sensor_data", true), + rclcpp::Parameter("sensor_data_parallel_compression", false)}); + + rclcpp::Publisher::SharedPtr pub = scanPublisher(); + ASSERT_TRUE(waitForSubscriber(pub)); + feedFrames(pub, odom, 2); + ASSERT_TRUE(spinUntil([&]() { return !compressed->empty(); })) + << "nothing was published on odom_sensor_data/compressed"; + EXPECT_GT(compressed->back().laser_scan_compressed.size(), 0u); + EXPECT_EQ(0u, compressed->back().laser_scan.data.size()); +} + /** * The whole recovery story, as a wheeled robot would live it: a guess frame it trusts, diff --git a/rtabmap_slam/doc/rtabmap.md b/rtabmap_slam/doc/rtabmap.md index 8c91d16b..49715a8d 100644 --- a/rtabmap_slam/doc/rtabmap.md +++ b/rtabmap_slam/doc/rtabmap.md @@ -37,6 +37,7 @@ Loop closures are found two ways: - [User data and environment sensors](#user-data-and-environment-sensors) - [Intermediate odometry](#intermediate-odometry) - [Deriving missing data](#deriving-missing-data) +- [Compressed images](#compressed-images) - [Localization](#localization) - [Planning](#planning) - [Diagnostics](#diagnostics) @@ -426,6 +427,7 @@ The node's own ROS parameters, with their real types. The ones about frames are | `gen_depth_fill_iterations` | `int` | `1` | Hole-filling passes. | | `gen_depth_fill_holes_error` | `double` | `0.1` | Maximum depth difference, in meters, across a hole for it to be filled. | | `stereo_to_depth` | `bool` | `false` | Compute a depth image from the stereo pair (with the `StereoBM/*` parameters) and map it as RGB-D. | +| `decode_images_on_demand` | `bool` | `true` | Leave compressed images of `rgbd_image` to RTAB-Map to decode, only if it needs them. See [Compressed images](#compressed-images). | | `scan_cloud_max_points` | `int` | `0` | Points in a full `scan_cloud` sweep, for an organized or fixed-size cloud; used by ICP as the reference for its correspondence ratio. `0` takes each cloud's own size. | | `scan_cloud_is_2d` | `bool` | `false` | `scan_cloud` is a 2D lidar published as a cloud. | | `odom_sensor_sync` | `bool` | `true` | Place each sensor where the robot was at that sensor's stamp, and deskew 2D scans ray by ray, using the odometry in TF. See [Sensors not stamped together](#sensors-not-stamped-together). | @@ -563,6 +565,12 @@ With `subscribe_inter_odom_info`, `inter_odom` is synchronized by exact stamp wi **`stereo_to_depth`** computes a dense depth image from a stereo pair, so a stereo camera is mapped like an RGB-D one — denser grids and clouds, at the cost of the disparity computation. +## Compressed images + +An `rgbd_image` (`subscribe_rgbd`, single camera) carrying only compressed images, as [rgbd_sync](https://docs.ros.org/en/jazzy/p/rtabmap_sync/)'s `rgbd_image/compressed` does, is not decoded by this node: the compressed images are given to RTAB-Map as they are, which decodes them only if it needs the pixels -- to extract features, build a grid from depth or rectify the images. With features already extracted by odometry (`Mem/UseOdomFeatures`), they are often never decoded at all. Either way, they are stored in the database as received, not compressed again. + +The images are decoded here instead when this node needs them (`gen_scan`, `gen_depth`, `stereo_to_depth`), when RTAB-Map could not use them as they are (e.g., 16 bits color images, a color right image, which are converted first), and for several cameras. `decode_images_on_demand: false` always decodes them here, as before 0.24: a fallback if something needs the decoded images that this node does not know of. + ## Localization `localization_pose` is the robot's pose in the map frame — `map` → `odom` composed with the odometry — after each update, with RTAB-Map's covariance. While mapping, that is the odometry covariance accumulated along the graph, growing with distance until a loop closure brings it down. In localization mode before the first loop closure, it is `9999`: the robot is not localized yet. diff --git a/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h b/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h index 4dc26b4e..ad8e7fc8 100644 --- a/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h +++ b/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h @@ -178,6 +178,28 @@ private: const std::vector > & localKeyPoints = std::vector >(), const std::vector > & localPoints3d = std::vector >(), const std::vector & localDescriptors = std::vector()); + /// Compressed images are left to rtabmap to decode when it needs them, unless + /// something here needs the pixels (stereo_to_depth, gen_scan, gen_depth). + virtual bool imagesDecodedOnDemand() const override + { + return decodeImagesOnDemand_ && !stereoToDepth_ && !genScan_ && !genDepth_; + } + virtual void commonMultiCameraCallbackWithCompressed( + const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg, + const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg, + const std::vector & imageMsgs, + const std::vector & depthMsgs, + const std::vector & cameraInfoMsgs, + const std::vector & depthCameraInfoMsgs, + const sensor_msgs::msg::LaserScan & scanMsg, + const sensor_msgs::msg::PointCloud2 & scan3dMsg, + const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg, + const std::vector & globalDescriptorMsgs, + const std::vector > & localKeyPoints, + const std::vector > & localPoints3d, + const std::vector & localDescriptors, + const std::vector & compressedImages, + const std::vector & compressedDepths) override; // Callback called from sync thread void commonMultiCameraCallbackImpl( const std::string & odomFrameId, @@ -192,7 +214,9 @@ private: const std::vector & globalDescriptorMsgs, const std::vector > & localKeyPoints, const std::vector > & localPoints3d, - const std::vector & localDescriptors); + const std::vector & localDescriptors, + const std::vector & compressedImages = std::vector(), + const std::vector & compressedDepths = std::vector()); // Callback called from sync thread virtual void commonLaserScanCallback( const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg, @@ -371,6 +395,7 @@ private: double genScanMaxDepth_; double genScanMinDepth_; bool genDepth_; + bool decodeImagesOnDemand_; int genDepthDecimation_; int genDepthFillHolesSize_; int genDepthFillIterations_; diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index ee6b9a60..d47f9b5b 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -128,6 +128,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : genScanMaxDepth_(4.0), genScanMinDepth_(0.0), genDepth_(false), + decodeImagesOnDemand_(true), genDepthDecimation_(1), genDepthFillHolesSize_(0), genDepthFillIterations_(1), @@ -223,6 +224,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : genScanMaxDepth_ = this->declare_parameter("gen_scan_max_depth", genScanMaxDepth_); genScanMinDepth_ = this->declare_parameter("gen_scan_min_depth", genScanMinDepth_); genDepth_ = this->declare_parameter("gen_depth", genDepth_); + decodeImagesOnDemand_ = this->declare_parameter("decode_images_on_demand", decodeImagesOnDemand_); genDepthDecimation_ = this->declare_parameter("gen_depth_decimation", genDepthDecimation_); genDepthFillHolesSize_ = this->declare_parameter("gen_depth_fill_holes_size", genDepthFillHolesSize_); genDepthFillIterations_ = this->declare_parameter("gen_depth_fill_iterations", genDepthFillIterations_); @@ -268,6 +270,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : } RCLCPP_INFO(get_logger(), "rtabmap: gen_depth = %s", genDepth_?"true":"false"); + RCLCPP_INFO(get_logger(), "rtabmap: decode_images_on_demand = %s", decodeImagesOnDemand_?"true":"false"); if(genDepth_) { RCLCPP_INFO(get_logger(), "rtabmap: gen_depth_decimation = %d", genDepthDecimation_); @@ -1348,6 +1351,29 @@ void CoreWrapper::commonMultiCameraCallback( const std::vector > & localKeyPoints, const std::vector > & localPoints3d, const std::vector & localDescriptors) +{ + commonMultiCameraCallbackWithCompressed(odomMsg, userDataMsg, imageMsgs, depthMsgs, + cameraInfoMsgs, depthCameraInfoMsgs, scan2dMsg, scan3dMsg, odomInfoMsg, + globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors, + std::vector(), std::vector()); +} + +void CoreWrapper::commonMultiCameraCallbackWithCompressed( + const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg, + const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg, + const std::vector & imageMsgs, + const std::vector & depthMsgs, + const std::vector & cameraInfoMsgs, + const std::vector & depthCameraInfoMsgs, + const sensor_msgs::msg::LaserScan & scan2dMsg, + const sensor_msgs::msg::PointCloud2 & scan3dMsg, + const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg, + const std::vector & globalDescriptorMsgs, + const std::vector > & localKeyPoints, + const std::vector > & localPoints3d, + const std::vector & localDescriptors, + const std::vector & compressedImages, + const std::vector & compressedDepths) { std::string odomFrameId; if(odomMsg.get()) @@ -1418,7 +1444,9 @@ void CoreWrapper::commonMultiCameraCallback( globalDescriptorMsgs, localKeyPoints, localPoints3d, - localDescriptors); + localDescriptors, + compressedImages, + compressedDepths); if(syncData_.valid) { syncTimer_->reset(); @@ -1439,7 +1467,9 @@ void CoreWrapper::commonMultiCameraCallbackImpl( const std::vector & globalDescriptorMsgs, const std::vector > & localKeyPointsMsgs, const std::vector > & localPoints3dMsgs, - const std::vector & localDescriptorsMsgs) + const std::vector & localDescriptorsMsgs, + const std::vector & compressedImages, + const std::vector & compressedDepths) { UTimer timerConversion; cv::Mat rgb; @@ -1696,6 +1726,43 @@ void CoreWrapper::commonMultiCameraCallbackImpl( userData); } + // Keep the compressed images the images come from, so that rtabmap stores them as is + // instead of compressing them again (see Memory). Only for a single camera, and only if + // the images were not changed on the way (color conversion, generated depth). + if(imageMsgs.empty() && depthMsgs.empty() && compressedImages.size() == 1 && compressedDepths.size() == 1) + { + // Not decoded (see imagesDecodedOnDemand()): rtabmap decodes them if it needs to + if(!stereoCameraModels.empty()) + { + syncData_.data.setStereoImage(compressedImages[0], compressedDepths[0], stereoCameraModels, false); + } + else + { + syncData_.data.setRGBDImage(compressedImages[0], compressedDepths[0], cameraModels, false); + } + } + else if(imageMsgs.size() == 1 && compressedImages.size() == 1 && compressedDepths.size() == 1) + { + const bool sameImage = !compressedImages[0].empty() && imageMsgs[0].get() && + rgb.type() == imageMsgs[0]->image.type() && rgb.size() == imageMsgs[0]->image.size(); + const bool sameDepth = !compressedDepths[0].empty() && !genDepth_ && + depthMsgs.size() == 1 && depthMsgs[0].get() && + depth.type() == depthMsgs[0]->image.type() && depth.size() == depthMsgs[0]->image.size(); + if(sameImage || sameDepth) + { + const cv::Mat compressedImage = sameImage ? compressedImages[0] : cv::Mat(); + const cv::Mat compressedDepth = sameDepth ? compressedDepths[0] : cv::Mat(); + if(!stereoCameraModels.empty()) + { + syncData_.data.setStereoImage(compressedImage, compressedDepth, stereoCameraModels, false); + } + else + { + syncData_.data.setRGBDImage(compressedImage, compressedDepth, cameraModels, false); + } + } + } + OdometryInfo odomInfo; if(odomInfoMsg.get()) { diff --git a/rtabmap_slam/test/core_wrapper_fixture.hpp b/rtabmap_slam/test/core_wrapper_fixture.hpp index 382337a4..ddaf8a1c 100644 --- a/rtabmap_slam/test/core_wrapper_fixture.hpp +++ b/rtabmap_slam/test/core_wrapper_fixture.hpp @@ -223,9 +223,9 @@ protected: if(!tfPub_) { tfPub_ = helper()->create_publisher("/tf", rclcpp::QoS(100)); - // The node's listener and nothing else: publishing before it is matched loses + // The node's listener (and tfReceived_): publishing before it is matched loses // the transform. - waitForSubscriber(tfPub_); + waitForSubscriber(tfPub_, tfReceived_ ? 2 : 1); } tf2_msgs::msg::TFMessage msg; msg.transforms.push_back(tf); @@ -244,9 +244,47 @@ protected: double variance = 0.001) { publishTf(makeTransform("odom", "base_link", stamp, x, y, yaw)); + if(tfReceived_) + { + EXPECT_TRUE(spinUntil([&]() { return hasTransform(*tfReceived_, "base_link", stamp); })) + << "odom -> base_link at " << stamp << " was not received"; + } pub->publish(makeOdometry(stamp, x, y, yaw, variance)); } + /// Whether @p tf received the transform of @p child at @p stamp. + static bool hasTransform(const Collector & tf, + const std::string & child, double stamp) + { + for(const tf2_msgs::msg::TFMessage::ConstSharedPtr & msg : tf.messages) + { + for(const geometry_msgs::msg::TransformStamped & t : msg->transforms) + { + if(t.child_frame_id == child && rclcpp::Time(t.header.stamp) == stampOf(stamp)) + { + return true; + } + } + } + return false; + } + + /** + * @brief Makes sendOdom() wait for each odom -> base_link transform to be received + * before publishing the odometry. + * + * To use with wait_for_transform=0 when the node is on simulated time: since Lyrical, + * tf2_ros waits for a transform on the node's clock, which does not advance while the + * test, the only /clock publisher, is blocked in the node's callback waiting for it. + */ + void waitForTfBeforeOdom() + { + tfReceived_ = collect("/tf", rclcpp::QoS(100)); + } + + /// The test's own /tf subscription, see waitForTfBeforeOdom(). + std::shared_ptr> tfReceived_; + rclcpp::Publisher::SharedPtr odomPublisher() { rclcpp::Publisher::SharedPtr pub = diff --git a/rtabmap_slam/test/test_core_wrapper_inputs.cpp b/rtabmap_slam/test/test_core_wrapper_inputs.cpp index 07003f98..5135fdbb 100644 --- a/rtabmap_slam/test/test_core_wrapper_inputs.cpp +++ b/rtabmap_slam/test/test_core_wrapper_inputs.cpp @@ -23,6 +23,7 @@ All rights reserved. (BSD-3-Clause, see the repository root.) #include #include +#include #include #include @@ -570,6 +571,331 @@ TEST_F(CoreWrapperInputsTest, maps_sensor_data) EXPECT_EQ(2u, getGraph().graph.poses_id.size()); } +/** + * Compressed images of rtabmap_msgs/SensorData (what odom_sensor_data/compressed carries) + * are stored as received, not decompressed and re-compressed: the formats below differ + * from Mem/ImageCompressionFormat (".jpg") and Mem/DepthCompressionFormat (".rvl"), so + * re-compressed images would not be the same bytes. + */ +class CoreWrapperCompressedSensorDataTest : + public CoreWrapperInputsTest, + public ::testing::WithParamInterface> +{ +protected: + static constexpr int kWidth = 64; + static constexpr int kHeight = 48; + + /// A textured color image and a depth image of @p depthType, compressed by rtabmap. + static rtabmap::SensorData makeCompressedData(int depthType, const std::string & depthFormat, double stamp) + { + cv::Mat rgb(kHeight, kWidth, CV_8UC3); + cv::randu(rgb, 0, 255); + cv::Mat depth(kHeight, kWidth, depthType); + if(depthType == CV_16UC1) + { + cv::randu(depth, 1000, 3000); + } + else + { + cv::randu(depth, 1.0f, 3.0f); + } + const rtabmap::CameraModel model(50.0, 50.0, kWidth/2.0, kHeight/2.0, + rtabmap::CameraModel::opticalRotation(), 0.0, cv::Size(kWidth, kHeight)); + return rtabmap::SensorData( + rtabmap::compressImage2(rgb, ".png"), + rtabmap::compressImage2(depth, depthFormat), + model, 0, stamp); + } + + /// Publishes @p data (compressed only, or with its raw images too) and returns the + /// node it became. + rtabmap_msgs::msg::Node map(const rtabmap::SensorData & data, bool withRaw, + const std::vector & params = {}) + { + std::vector all = { + rclcpp::Parameter("subscribe_sensor_data", true), + rclcpp::Parameter("Mem/ImageCompressionFormat", std::string(".jpg")), + rclcpp::Parameter("Mem/DepthCompressionFormat", std::string(".rvl"))}; + all.insert(all.end(), params.begin(), params.end()); + makeNode(all); + std::shared_ptr> info = collectInfo(); + rclcpp::Publisher::SharedPtr odom = odomPublisher(); + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("sensor_data", 10); + EXPECT_TRUE(waitForSubscriber(pub)); + + rtabmap::SensorData copy = data; + if(withRaw) + { + copy.uncompressData(); + EXPECT_FALSE(copy.imageRaw().empty()); + EXPECT_FALSE(copy.depthRaw().empty()); + } + rtabmap_msgs::msg::SensorData msg; + rtabmap_conversions::sensorDataToROS(copy, msg, "base_link", withRaw); + msg.header.stamp = stampOf(data.stamp()); + sendOdom(odom, data.stamp(), 0.0); + pub->publish(msg); + EXPECT_TRUE(spinUntil([&]() { return !info->empty(); })); + return getNode(1); + } + + static std::vector bytes(const cv::Mat & compressed) + { + return std::vector(compressed.data, compressed.data + compressed.total()); + } +}; + +TEST_P(CoreWrapperCompressedSensorDataTest, stores_compressed_images_as_received) +{ + const std::string depthFormat = std::get<0>(GetParam()); + const bool withRaw = std::get<1>(GetParam()); + const int depthType = depthFormat == ".png" ? CV_16UC1 : CV_32FC1; + const rtabmap::SensorData data = makeCompressedData(depthType, depthFormat, 1.0); + + const rtabmap_msgs::msg::Node node = map(data, withRaw); + ASSERT_EQ(1, node.id); + EXPECT_EQ(node.data.left_compressed, bytes(data.imageCompressed())) << "color not re-compressed"; + EXPECT_EQ(node.data.right_compressed, bytes(data.depthOrRightCompressed())) << "depth not re-compressed"; +} + +INSTANTIATE_TEST_SUITE_P( + Formats, + CoreWrapperCompressedSensorDataTest, + ::testing::Combine( + // 16UC1 PNG, 32FC1 lossless PNG (4 channels), 32FC1 inverse depth + ::testing::Values(std::string(".png"), std::string(".png:10:100")), + ::testing::Bool()), // compressed only, or raw images too + [](const ::testing::TestParamInfo> & info) { + const std::string & f = std::get<0>(info.param); + return std::string(f == ".png" ? "png16" : "inverse_depth") + + (std::get<1>(info.param) ? "_with_raw" : "_compressed_only"); + }); + +/// The legacy lossless 32FC1 format is stored as received too. +TEST_F(CoreWrapperCompressedSensorDataTest, stores_legacy_float_depth_as_received) +{ + const rtabmap::SensorData data = makeCompressedData(CV_32FC1, ".png", 1.0); + ASSERT_EQ(rtabmap::compressedDepthFormat(data.depthOrRightCompressed()), ".png"); + const rtabmap_msgs::msg::Node node = map(data, false); + ASSERT_EQ(1, node.id); + EXPECT_EQ(node.data.right_compressed, bytes(data.depthOrRightCompressed())); +} + +/// When Memory changes an image (here the color image, by Mem/ImagePostDecimation), it +/// compresses it itself, with its own format; the images it leaves as is (here depth, +/// decimated only by Mem/ImagePreDecimation) are still stored as received. +TEST_F(CoreWrapperCompressedSensorDataTest, recompresses_only_the_images_it_changes) +{ + const rtabmap::SensorData data = makeCompressedData(CV_16UC1, ".png", 1.0); + const rtabmap_msgs::msg::Node node = map(data, false, + {rclcpp::Parameter("Mem/ImagePostDecimation", std::string("2"))}); + ASSERT_EQ(1, node.id); + const cv::Mat rgb = rtabmap::uncompressImage(node.data.left_compressed); + EXPECT_EQ(rgb.cols, kWidth/2); + ASSERT_GE(node.data.left_compressed.size(), 2u); + EXPECT_EQ(node.data.left_compressed[0], 0xFF) << "JPEG, Mem/ImageCompressionFormat"; + EXPECT_EQ(node.data.right_compressed, bytes(data.depthOrRightCompressed())); +} + +/** + * The same for rtabmap_msgs/RGBDImage: images received compressed only are stored as + * received (depth in compressed_depth_image_transport's format only converted to rtabmap's + * format, without decompression), unless they had to be changed on the way. + */ +class CoreWrapperCompressedRGBDTest : public CoreWrapperInputsTest +{ +protected: + static constexpr int kWidth = 64; + static constexpr int kHeight = 48; + + /// @p msg with only its images and camera infos left to set. + static rtabmap_msgs::msg::RGBDImage makeMsg() + { + rtabmap_msgs::msg::RGBDImage msg; + msg.header.frame_id = "camera"; + msg.header.stamp = stampOf(1.0); + msg.rgb_camera_info = makeCameraInfo("camera", 1.0, kWidth, kHeight); + msg.depth_camera_info = msg.rgb_camera_info; + return msg; + } + + static cv::Mat colorImage() + { + return cv_bridge::toCvCopy(makeTexturedImage("camera", 1.0, kWidth, kHeight))->image; + } + + static cv::Mat floatDepth() + { + cv::Mat depth(kHeight, kWidth, CV_32FC1); + cv::randu(depth, 1.0f, 3.0f); + return depth; + } + + static sensor_msgs::msg::CompressedImage compressedColor(const cv::Mat & image, const std::string & encoding) + { + sensor_msgs::msg::CompressedImage msg; + EXPECT_TRUE(rtabmap_conversions::toCompressedImageMsg( + cv_bridge::CvImage(makeMsg().header, encoding, image), "png", msg)); + return msg; + } + + static sensor_msgs::msg::CompressedImage compressedDepth(const cv::Mat & depth, const std::string & format) + { + sensor_msgs::msg::CompressedImage msg; + msg.header = makeMsg().header; + EXPECT_TRUE(rtabmap_conversions::compressDepthImage(depth, format, msg)); + return msg; + } + + /// Maps @p msg (Mem/ImageCompressionFormat=".jpg", Mem/DepthCompressionFormat=".rvl", + /// so that re-compressed images are not the same bytes) and returns its node. + rtabmap_msgs::msg::Node map(const rtabmap_msgs::msg::RGBDImage & msg, + const std::vector & params = {}) + { + std::vector all = { + rclcpp::Parameter("subscribe_rgbd", true), + rclcpp::Parameter("Mem/ImageCompressionFormat", std::string(".jpg")), + rclcpp::Parameter("Mem/DepthCompressionFormat", std::string(".rvl"))}; + all.insert(all.end(), params.begin(), params.end()); + publishStaticTf("camera"); + makeNode(all); + std::shared_ptr> info = collectInfo(); + rclcpp::Publisher::SharedPtr odom = odomPublisher(); + rclcpp::Publisher::SharedPtr rgbd = + helper()->create_publisher("rgbd_image", 10); + EXPECT_TRUE(waitForSubscriber(rgbd)); + sendOdom(odom, 1.0, 0.0); + rgbd->publish(msg); + EXPECT_TRUE(spinUntil([&]() { return !info->empty(); })); + return getNode(1); + } + + /// Maps a color PNG and @p depth, both compressed only, and checks they are stored as is. + void checkStoredAsReceived(const sensor_msgs::msg::CompressedImage & depth, const std::string & storedFormat) + { + rtabmap_msgs::msg::RGBDImage msg = makeMsg(); + msg.rgb_compressed = compressedColor(colorImage(), "bgr8"); + msg.depth_compressed = depth; + + const rtabmap_msgs::msg::Node node = map(msg); + ASSERT_EQ(1, node.id); + EXPECT_EQ(node.data.left_compressed, msg.rgb_compressed.data) << "color not re-compressed"; + const cv::Mat expected = depth.format.find("compressedDepth") != std::string::npos ? + rtabmap_conversions::compressedDepthTransportToRtabmap(depth) : + rtabmap_conversions::compressedMatFromBytes(depth.data); + EXPECT_EQ(node.data.right_compressed, std::vector(expected.data, expected.data + expected.total())) + << "depth not re-compressed"; + EXPECT_EQ(rtabmap::compressedDepthFormat(node.data.right_compressed), storedFormat); + } + + /// Maps a compressed stereo pair and checks it is stored as is. + void checkStereoStoredAsReceived(bool decodeOnDemand) + { + rtabmap_msgs::msg::RGBDImage msg = makeMsg(); + msg.depth_camera_info.p[3] = -msg.depth_camera_info.p[0] * 0.1; // 10 cm baseline + cv::Mat left, right; + cv::cvtColor(colorImage(), left, cv::COLOR_BGR2GRAY); + cv::flip(left, right, 1); + msg.rgb_compressed = compressedColor(left, "mono8"); + msg.depth_compressed = compressedColor(right, "mono8"); + const rtabmap_msgs::msg::Node node = map(msg, {rclcpp::Parameter("decode_images_on_demand", decodeOnDemand)}); + ASSERT_EQ(1, node.id); + ASSERT_EQ(node.data.right_camera_info.size(), 1u) << "stored as a stereo pair"; + EXPECT_EQ(node.data.left_compressed, msg.rgb_compressed.data); + EXPECT_EQ(node.data.right_compressed, msg.depth_compressed.data); + } + + static bool isJpeg(const std::vector & bytes) + { + return bytes.size() >= 2 && bytes[0] == 0xFF && bytes[1] == 0xD8; + } +}; + +TEST_F(CoreWrapperCompressedRGBDTest, stores_16bits_compressed_depth_as_received) +{ + cv::Mat depth16U; + floatDepth().convertTo(depth16U, CV_16UC1, 1000.0); + checkStoredAsReceived(compressedDepth(depth16U, ".png"), ".png"); +} + +TEST_F(CoreWrapperCompressedRGBDTest, stores_inverse_depth_as_received) +{ + checkStoredAsReceived(compressedDepth(floatDepth(), ".png:10:100"), ".png:10:100"); +} + +TEST_F(CoreWrapperCompressedRGBDTest, stores_legacy_float_depth_as_received) +{ + checkStoredAsReceived(compressedDepth(floatDepth(), "legacy"), ".png"); +} + +/// Decoding on demand can be disabled: images are then decoded here, and still stored as +/// received. +TEST_F(CoreWrapperCompressedRGBDTest, stores_compressed_images_as_received_when_decoded_here) +{ + rtabmap_msgs::msg::RGBDImage msg = makeMsg(); + msg.rgb_compressed = compressedColor(colorImage(), "bgr8"); + msg.depth_compressed = compressedDepth(floatDepth(), ".png:10:100"); + const rtabmap_msgs::msg::Node node = map(msg, {rclcpp::Parameter("decode_images_on_demand", false)}); + ASSERT_EQ(1, node.id); + EXPECT_EQ(node.data.left_compressed, msg.rgb_compressed.data); + EXPECT_EQ(rtabmap::compressedDepthFormat(node.data.right_compressed), ".png:10:100"); +} + +/// gen_scan needs the depth pixels: the images are decoded here, and the scan generated. +TEST_F(CoreWrapperCompressedRGBDTest, decodes_images_to_generate_a_scan) +{ + rtabmap_msgs::msg::RGBDImage msg = makeMsg(); + msg.rgb_compressed = compressedColor(colorImage(), "bgr8"); + msg.depth_compressed = compressedDepth(floatDepth(), ".png:10:100"); + const rtabmap_msgs::msg::Node node = map(msg, {rclcpp::Parameter("gen_scan", true)}); + ASSERT_EQ(1, node.id); + EXPECT_FALSE(node.data.laser_scan_compressed.empty()) << "scan generated from the depth image"; + EXPECT_EQ(node.data.left_compressed, msg.rgb_compressed.data) << "still stored as received"; +} + +/// A compressed stereo pair (gray right image) is stored as received too, decoded on +/// demand or here. +TEST_F(CoreWrapperCompressedRGBDTest, stores_a_compressed_stereo_pair_as_received) +{ + checkStereoStoredAsReceived(true); +} + +TEST_F(CoreWrapperCompressedRGBDTest, stores_a_compressed_stereo_pair_as_received_when_decoded_here) +{ + checkStereoStoredAsReceived(false); +} + +/// A raw image is compressed by rtabmap; the compressed one next to it is still stored as is. +TEST_F(CoreWrapperCompressedRGBDTest, stores_compressed_depth_with_raw_color) +{ + rtabmap_msgs::msg::RGBDImage msg = makeMsg(); + msg.rgb = makeTexturedImage("camera", 1.0, kWidth, kHeight); + msg.depth_compressed = compressedDepth(floatDepth(), ".png:10:100"); + const rtabmap_msgs::msg::Node node = map(msg); + ASSERT_EQ(1, node.id); + EXPECT_TRUE(isJpeg(node.data.left_compressed)) << "Mem/ImageCompressionFormat"; + const cv::Mat expected = rtabmap_conversions::compressedDepthTransportToRtabmap(msg.depth_compressed); + EXPECT_EQ(node.data.right_compressed, std::vector(expected.data, expected.data + expected.total())); +} + +/// A color image that has to be converted (here mono16 to mono8) is not the image of its +/// compressed bytes anymore: it is compressed again by rtabmap. +TEST_F(CoreWrapperCompressedRGBDTest, recompresses_converted_images) +{ + rtabmap_msgs::msg::RGBDImage msg = makeMsg(); + cv::Mat gray16(kHeight, kWidth, CV_16UC1); + cv::randu(gray16, 0, 65535); + msg.rgb_compressed = compressedColor(gray16, "mono16"); + ASSERT_EQ(msg.rgb_compressed.format, "mono16; png compressed mono16"); + msg.depth_compressed = compressedDepth(floatDepth(), ".png:10:100"); + const rtabmap_msgs::msg::Node node = map(msg); + ASSERT_EQ(1, node.id); + EXPECT_TRUE(isJpeg(node.data.left_compressed)) << "converted to mono8, then Mem/ImageCompressionFormat"; + EXPECT_EQ(rtabmap::uncompressImage(node.data.left_compressed).type(), CV_8UC1); + EXPECT_EQ(rtabmap::compressedDepthFormat(node.data.right_compressed), ".png:10:100") << "depth still as received"; +} + //========================================================================================== // Synchronization of sensors that are not stamped together //========================================================================================== diff --git a/rtabmap_slam/test/test_core_wrapper_planning.cpp b/rtabmap_slam/test/test_core_wrapper_planning.cpp index 7dabc860..dc0f3d12 100644 --- a/rtabmap_slam/test/test_core_wrapper_planning.cpp +++ b/rtabmap_slam/test/test_core_wrapper_planning.cpp @@ -50,14 +50,11 @@ protected: ASSERT_TRUE(waitForPublisher(goalReached_->subscription)); ASSERT_TRUE(waitForPublisher(globalPath_->subscription)); ASSERT_TRUE(waitForPublisher(globalPathNodes_->subscription)); - // Nodes 1..5 at x = 0, 0.5, 1.0, 1.5, 2.0. - for(int i=0; i<5; ++i) + if(simulatedTime()) { - const size_t before = info_->size(); - sendUpdate(1.0 + i, 0.5 * i); - ASSERT_TRUE(spinUntil([&]() { return info_->size() > before; })) - << "update " << i << " was not processed"; + waitForTfBeforeOdom(); } + driveStraight(odom_, info_, 5); // nodes 1..5 at x = 0, 0.5, 1.0, 1.5, 2.0 nextStamp_ = 6.0; } @@ -71,18 +68,13 @@ protected: } virtual std::vector nodeParameters() { return {}; } - - /// Sends one odometry update stamped @p stamp, the robot at (@p x, 0, @p yaw). - virtual void sendUpdate(double stamp, double x, double yaw = 0.0) - { - sendOdom(odom_, stamp, x, 0.0, yaw); - } + virtual bool simulatedTime() const { return false; } /// Moves the robot to @p x and waits for the update to be processed. bool moveTo(double x) { const size_t before = info_->size(); - sendUpdate(nextStamp_, x); + sendOdom(odom_, nextStamp_, x); nextStamp_ += 1.0; return spinUntil([&]() { return info_->size() > before; }); } @@ -257,29 +249,22 @@ TEST_F(CoreWrapperPlanningTest, plans_through_the_graph_beyond_the_local_radius) /** * The same corridor, with the node on the tests' own clock (use_sim_time): map -> odom is * then stamped in the odometry's time base, and with tf_tolerance at 0, exactly at the - * clock's time. The clock is set to each update's stamp before it is sent. A TF lookup - * waits on that clock (tf2_ros sleeps on it since Rolling/Lyrical): frozen, a lookup that - * has to wait never returns, even once its transform is there. Each update's transform is - * then sent first, the clock and some time for the node's listener after it, the odometry - * last, so that its lookup never has to wait. Only for tests whose lookups find their - * transform: one for a transform that is not there -- a goal in an unknown frame, say -- - * would wait forever. + * clock's time. Only for tests whose TF lookups never have to wait: with a clock that only + * moves when told to, a lookup waiting for a transform that is not there -- a goal in an + * unknown frame, say -- would wait forever. */ class CoreWrapperPlanningSimTimeTest : public CoreWrapperPlanningTest { protected: std::vector nodeParameters() override { + // wait_for_transform=0: the clock is advanced by the test only, a wait for a + // transform on it would never end (see waitForTfBeforeOdom()). return {rclcpp::Parameter("use_sim_time", true), - rclcpp::Parameter("tf_tolerance", 0.0)}; - } - - void sendUpdate(double stamp, double x, double yaw = 0.0) override - { - publishTf(makeTransform("odom", "base_link", stamp, x, 0.0, yaw)); - setClock(stamp); - odom_->publish(makeOdometry(stamp, x, 0.0, yaw)); + rclcpp::Parameter("tf_tolerance", 0.0), + rclcpp::Parameter("wait_for_transform", 0.0)}; } + bool simulatedTime() const override { return true; } /// Sets the node's clock to @p seconds. void setClock(double seconds) @@ -302,7 +287,7 @@ TEST_F(CoreWrapperPlanningSimTimeTest, transforms_a_goal_in_the_robot_frame_to_t { // Turn the robot to face +y where it stands, at x = 2. const size_t before = info_->size(); - sendUpdate(nextStamp_, 2.0, M_PI/2.0); + sendOdom(odom_, nextStamp_, 2.0, 0.0, M_PI/2.0); ASSERT_TRUE(spinUntil([&]() { return info_->size() > before; })); rclcpp::Publisher::SharedPtr goal = @@ -312,21 +297,8 @@ TEST_F(CoreWrapperPlanningSimTimeTest, transforms_a_goal_in_the_robot_frame_to_t // The goal is looked up through map -> odom -> base_link at its stamp: bring the clock // to the turn's stamp, and wait for map -> odom to be published at it. const double stamp = nextStamp_; - std::shared_ptr> tf = - collect("/tf", rclcpp::QoS(100)); setClock(stamp); - ASSERT_TRUE(spinUntil([&]() { - for(const tf2_msgs::msg::TFMessage::ConstSharedPtr & msg : tf->messages) - { - for(const geometry_msgs::msg::TransformStamped & t : msg->transforms) - { - if(t.child_frame_id == "odom" && rclcpp::Time(t.header.stamp) == stampOf(stamp)) - { - return true; - } - } - } - return false; })); + ASSERT_TRUE(spinUntil([&]() { return hasTransform(*tfReceived_, "odom", stamp); })); // 1 m straight ahead of the robot, facing where it faces. geometry_msgs::msg::PoseStamped pose; diff --git a/rtabmap_sync/doc/rgb_sync.md b/rtabmap_sync/doc/rgb_sync.md index 58ec0a7a..2c0b2584 100644 --- a/rtabmap_sync/doc/rgb_sync.md +++ b/rtabmap_sync/doc/rgb_sync.md @@ -59,7 +59,7 @@ ComposableNode( | Topic | Type | Description | |---|---|---| | `rgbd_image` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | The image and its calibration. The depth slot is left empty unless `fill_empty_depth`. Published only when someone is subscribed. | -| `rgbd_image/compressed` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | The same frame with the image as JPEG. Published only when someone is subscribed. | +| `rgbd_image/compressed` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | The same frame with the image compressed, JPEG by default (see `image_compression_format`). Published only when someone is subscribed. | The output's `header.frame_id` comes from the camera_info; its `header.stamp` is the image's. @@ -77,6 +77,7 @@ The output's `header.frame_id` comes from the camera_info; its `header.stamp` is | `qos_camera_info` | `int` | value of `qos` | Reliability of the `rgb/camera_info` subscription alone. | | `fill_empty_depth` | `bool` | `false` | Add an all-zero depth image the size of the color one. See below. | | `compressed_rate` | `double` | `0.0` | Maximum rate, in Hz, of `rgbd_image/compressed`. `0` means every frame. | +| `image_compression_format` | `string` | `".jpg"` | Format of the image in `rgbd_image/compressed`: `".jpg"` (lossy, smaller) or `".png"` (lossless, larger). An invalid value falls back to `".jpg"` with an error. | | `image_transport` | `string` | `"raw"` | Transport for `rgb/image`, e.g. `compressed`. | ## fill_empty_depth diff --git a/rtabmap_sync/doc/rgbd_sync.md b/rtabmap_sync/doc/rgbd_sync.md index 2259a985..2bc492b2 100644 --- a/rtabmap_sync/doc/rgbd_sync.md +++ b/rtabmap_sync/doc/rgbd_sync.md @@ -111,7 +111,7 @@ ComposableNode( | Topic | Type | Description | |---|---|---| | `rgbd_image` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | The three inputs, raw. Published only when someone is subscribed. | -| `rgbd_image/compressed` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | The same frame with JPEG color and PNG depth instead of raw images. Published only when someone is subscribed. | +| `rgbd_image/compressed` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | The same frame with compressed color (JPEG by default) and depth (PNG by default) instead of raw images. Published only when someone is subscribed. | The output's `header.frame_id` is taken from the **camera_info**, which is the frame the calibration is expressed in — so make sure the images and the `camera_info` carry the same `frame_id`. If they disagree, the output is labelled with the calibration's frame while the pixels were measured in another, and every point projected out of them lands somewhere else. @@ -132,6 +132,8 @@ The output's `header.stamp` is the **later** of the color and depth stamps, so t | `depth_scale` | `double` | `1.0` | Multiplies every depth pixel. See [Depth units](#depth-units). | | `decimation` | `int` | `1` | Downsample both images by this factor, scaling the calibration to match. Must divide the depth image size exactly, or it is ignored. | | `compressed_rate` | `double` | `0.0` | Maximum rate, in Hz, of `rgbd_image/compressed`. `0` means every frame. Does not affect `rgbd_image`. | +| `image_compression_format` | `string` | `".jpg"` | Format of the color image in `rgbd_image/compressed`: `".jpg"` (lossy, smaller) or `".png"` (lossless, larger). An invalid value falls back to `".jpg"` with an error. | +| `depth_compression_format` | `string` | `".png"` | Format of the depth in `rgbd_image/compressed`: `".png"`, `".rvl"`, `".png::"`, `".rvl::"`, any of them prefixed by `"legacy:"`, or `"legacy"`. See [Compressing for a slow link](#compressing-for-a-slow-link). | | `image_transport` | `string` | `"raw"` | Transport for `rgb/image`, e.g. `compressed`. | | `depth_transport` | `string` | `"raw"` | Transport for `depth/image`, e.g. `compressedDepth`. | | `rgb_image_transport` | `string` | — | **Deprecated**, renamed to `image_transport`. | @@ -160,7 +162,24 @@ RTAB-Map reads `16UC1` depth as millimeters and `32FC1` as meters. A driver that ## Compressing for a slow link -`rgbd_image/compressed` carries the same frame with the color image as **JPEG** and the depth image as **PNG**. Depth stays lossless deliberately: JPEG artifacts in a depth image are not blur, they are invented geometry. +`rgbd_image/compressed` carries the same frame with the color image as **JPEG** and the depth image as **PNG**. Depth stays lossless deliberately: JPEG artifacts in a depth image are not blur, they are invented geometry. `image_compression_format: ".png"` makes the color image lossless too, at a few times the size. + +The images are in the formats of `compressed_image_transport` (e.g. `"bgr8; jpeg compressed bgr8"`) and `compressed_depth_image_transport` (e.g. `"16UC1; compressedDepth png"`), so the `compressed` and `compressedDepth` image_transport plugins can read them. + +`depth_compression_format` changes the depth format: + +| Value | 16UC1 depth | 32FC1 depth | +|---|---|---| +| `".png"` (default) | Lossless PNG | Rounded to millimeters (16UC1, ±0.5 mm, depth over 65.5 m lost), PNG | +| `".rvl"` | Lossless RVL: several times faster to compress than PNG, but larger. PNG before Jazzy, as `compressed_depth_image_transport` cannot decode RVL there. | Millimeters, RVL | +| `".png::"`, `".rvl::"` | Same as without the depth parameters | Quantized on 16 bits as inverse depth, like `compressed_depth_image_transport` (e.g. `".png:10:100"`, its default): smaller, stays 32FC1, but depth over `maxDepth` is lost and the error grows with the square of the depth (~0.05 mm at 1 m and 5 mm at 10 m with a quantization of 100). | +| `"legacy:"` (`"legacy"` is `"legacy:.png"`) | With the codec | Without depth parameters: **lossless**, 4 channels PNG of the float bytes, whatever the codec. With depth parameters: inverse depth. | + +`"legacy:"` keeps rtabmap's own format, the one of its database (`Mem/DepthCompressionFormat`) and of rtabmap_ros before 0.24, instead of `compressed_depth_image_transport`'s. image_transport plugins cannot read it (format `"png"`, `"rvl"` or `"rvl:10:100"`), but rtabmap_ros decodes it, and [rgbd_split](https://docs.ros.org/en/jazzy/p/rtabmap_util/)'s `compressedDepth` output converts it. Use it to: + +- keep 32FC1 depth lossless when compressed (`"legacy"`); +- send RVL before Jazzy without it being re-compressed as PNG (`"legacy:.rvl"`); +- feed rtabmap_ros nodes older than 0.24 (`"legacy"` or `"legacy:.rvl"`; they cannot decode inverse depth). Neither output is produced unless it has a subscriber, so the compression costs nothing until something subscribes. diff --git a/rtabmap_sync/doc/stereo_sync.md b/rtabmap_sync/doc/stereo_sync.md index 33d6c9f8..a70b9091 100644 --- a/rtabmap_sync/doc/stereo_sync.md +++ b/rtabmap_sync/doc/stereo_sync.md @@ -92,7 +92,7 @@ As with `rgbd_sync`, compose it into the driver's process where you can: this no | Topic | Type | Description | |---|---|---| | `rgbd_image` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | Left image in the color slot, right image in the depth slot. Published only when someone is subscribed. | -| `rgbd_image/compressed` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | The same pair, both images JPEG. Published only when someone is subscribed. | +| `rgbd_image/compressed` | [`rtabmap_msgs/msg/RGBDImage`](https://docs.ros.org/en/jazzy/p/rtabmap_msgs/msg/RGBDImage.html) | The same pair, both images compressed, JPEG by default (see `image_compression_format`). Published only when someone is subscribed. | The output's `header.frame_id` comes from the **left** camera_info, and its `header.stamp` is the later of the two image stamps. @@ -109,6 +109,7 @@ The output's `header.frame_id` comes from the **left** camera_info, and its `hea | `qos` | `int` | `0` | Reliability of the subscriptions and the publishers: `0` system default, `1` reliable, `2` best effort. | | `qos_camera_info` | `int` | value of `qos` | Reliability of the two `camera_info` subscriptions alone. | | `compressed_rate` | `double` | `0.0` | Maximum rate, in Hz, of `rgbd_image/compressed`. `0` means every frame. | +| `image_compression_format` | `string` | `".jpg"` | Format of the left and right images in `rgbd_image/compressed`: `".jpg"` (lossy, smaller) or `".png"` (lossless, larger). An invalid value falls back to `".jpg"` with an error. | | `image_transport` | `string` | `"raw"` | Transport for both images, e.g. `compressed`. | ## How a stereo pair travels in an RGBDImage @@ -136,9 +137,9 @@ ros2 topic echo --once /stereo/right/image_rect --field header.stamp ## Compressing for a slow link -`rgbd_image/compressed` carries both images as **JPEG**. Unlike [rgbd_sync](rgbd_sync.md), there is no lossless path: both halves of a stereo pair are ordinary camera images, and neither is depth. +`rgbd_image/compressed` carries both images as **JPEG** by default: both halves of a stereo pair are ordinary camera images, and neither is depth. -JPEG artifacts do affect stereo matching, so a pipeline that computes odometry from the compressed stream will match slightly fewer features than one on the raw images. For sending frames to an operator, that does not matter; for running odometry at the far end of a link, prefer a higher JPEG quality over a lower frame rate. +JPEG artifacts do affect stereo matching, so a pipeline that computes odometry from the compressed stream will match slightly fewer features than one on the raw images. For sending frames to an operator, that does not matter; for running odometry at the far end of a link, set `image_compression_format: ".png"` to keep both images lossless, at a few times the size, and lower `compressed_rate` if the link cannot carry it. `compressed_rate` caps the compressed topic without touching `rgbd_image`. diff --git a/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h b/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h index 1282a4c6..1bb66eed 100644 --- a/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h +++ b/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h @@ -244,6 +244,52 @@ protected: const std::vector > & localKeyPoints = std::vector >(), const std::vector > & localPoints3d = std::vector >(), const std::vector & localDescriptors = std::vector()) = 0; + /** + * @brief Same as commonMultiCameraCallback(), with the compressed images the decoded + * images come from, called instead of it when there are some. + * + * For a single `RGBDImage` carrying compressed images only, @p compressedImages and + * @p compressedDepths hold them in rtabmap's format (see + * rtabmap_conversions::rgbdImageCompressedToRtabmap()), so that they can be stored as + * is instead of being compressed again. An element is empty when the corresponding + * image was raw. + * + * The default implementation ignores them and calls commonMultiCameraCallback(). + */ + /** + * @brief Whether the subclass accepts images left compressed, not decoded. + * + * When true, a single `RGBDImage` (RGB-D or stereo) carrying only compressed images + * that rtabmap can use as they are (see + * rtabmap_conversions::isCompressedRGBDSupportedByRtabmap()) is not decoded: + * commonMultiCameraCallbackWithCompressed() is called with no decoded image, only the + * compressed ones, and the camera infos. The default is false: images are always + * decoded. + */ + virtual bool imagesDecodedOnDemand() const { return false; } + virtual void commonMultiCameraCallbackWithCompressed( + const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg, + const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg, + const std::vector & imageMsgs, + const std::vector & depthMsgs, + const std::vector & cameraInfoMsgs, + const std::vector & depthCameraInfoMsgs, + const sensor_msgs::msg::LaserScan& scanMsg, + const sensor_msgs::msg::PointCloud2& scan3dMsg, + const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg, + const std::vector & globalDescriptorMsgs, + const std::vector > & localKeyPoints, + const std::vector > & localPoints3d, + const std::vector & localDescriptors, + const std::vector & compressedImages, + const std::vector & compressedDepths) + { + (void)compressedImages; + (void)compressedDepths; + commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, + cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, + globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors); + } /** * @brief Called with one synchronized scan, when no camera is subscribed. * @@ -314,7 +360,17 @@ private: const std::vector & globalDescriptorMsgs = std::vector(), const std::vector & localKeyPoints = std::vector(), const std::vector & localPoints3d = std::vector(), - const cv::Mat & localDescriptors = cv::Mat()); + const cv::Mat & localDescriptors = cv::Mat(), + const cv::Mat & compressedImage = cv::Mat(), + const cv::Mat & compressedDepth = cv::Mat()); + /// The images of @p msg, decoded unless imagesDecodedOnDemand() allows not to (then + /// @p rgb and @p depth are null), and its compressed images in rtabmap's format. + void convertRGBDImage( + const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr & msg, + cv_bridge::CvImageConstPtr & rgb, + cv_bridge::CvImageConstPtr & depth, + cv::Mat & compressedImage, + cv::Mat & compressedDepth) const; void processSyncData(); void setupDepthCallbacks( rclcpp::Node & node, diff --git a/rtabmap_sync/include/rtabmap_sync/rgb_sync.hpp b/rtabmap_sync/include/rtabmap_sync/rgb_sync.hpp index 1215834f..584a87a9 100644 --- a/rtabmap_sync/include/rtabmap_sync/rgb_sync.hpp +++ b/rtabmap_sync/include/rtabmap_sync/rgb_sync.hpp @@ -69,6 +69,8 @@ public: private: double compressedRate_; + /// Format of the compressed color (and right) images: ".jpg" or ".png". + std::string imageCompressionFormat_ = ".jpg"; bool fillEmptyDepth_; /// Stamp of the last compressed message published, for compressed_rate throttling. diff --git a/rtabmap_sync/include/rtabmap_sync/rgbd_sync.hpp b/rtabmap_sync/include/rtabmap_sync/rgbd_sync.hpp index 8c58a0c6..f24d0109 100644 --- a/rtabmap_sync/include/rtabmap_sync/rgbd_sync.hpp +++ b/rtabmap_sync/include/rtabmap_sync/rgbd_sync.hpp @@ -73,6 +73,9 @@ private: double depthScale_; int decimation_; double compressedRate_; + std::string depthCompressionFormat_; + /// Format of the compressed color (and right) images: ".jpg" or ".png". + std::string imageCompressionFormat_ = ".jpg"; double approxSyncMaxInterval_; /// Stamp of the last compressed message published, for compressed_rate throttling. diff --git a/rtabmap_sync/include/rtabmap_sync/stereo_sync.hpp b/rtabmap_sync/include/rtabmap_sync/stereo_sync.hpp index bf5dc942..b82ee1a4 100644 --- a/rtabmap_sync/include/rtabmap_sync/stereo_sync.hpp +++ b/rtabmap_sync/include/rtabmap_sync/stereo_sync.hpp @@ -70,6 +70,8 @@ public: const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoRight); private: double compressedRate_; + /// Format of the compressed color (and right) images: ".jpg" or ".png". + std::string imageCompressionFormat_ = ".jpg"; double approxSyncMaxInterval_; /// Stamp of the last compressed message published, for compressed_rate throttling. /// Explicitly on the ROS clock: the default is the system clock, and rclcpp refuses diff --git a/rtabmap_sync/src/CommonDataSubscriber.cpp b/rtabmap_sync/src/CommonDataSubscriber.cpp index c8cbfb06..cefe347b 100644 --- a/rtabmap_sync/src/CommonDataSubscriber.cpp +++ b/rtabmap_sync/src/CommonDataSubscriber.cpp @@ -27,6 +27,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include +#include namespace rtabmap_sync { @@ -1060,6 +1062,32 @@ CommonDataSubscriber::~CommonDataSubscriber() rgbdSubs_.clear(); } +void CommonDataSubscriber::convertRGBDImage( + const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr & msg, + cv_bridge::CvImageConstPtr & rgb, + cv_bridge::CvImageConstPtr & depth, + cv::Mat & compressedImage, + cv::Mat & compressedDepth) const +{ + rtabmap_conversions::rgbdImageCompressedToRtabmap(*msg, compressedImage, compressedDepth); + const bool compressedOnly = + msg->rgb.data.empty() && msg->depth.data.empty() && + (!compressedImage.empty() || !compressedDepth.empty()) && + (msg->rgb_compressed.data.empty() || !compressedImage.empty()) && + (msg->depth_compressed.data.empty() || !compressedDepth.empty()); + // A stereo pair has its baseline in P(0,3) of one of its camera infos + const bool stereo = msg->rgb_camera_info.p[3] != 0.0 || msg->depth_camera_info.p[3] != 0.0; + if(compressedOnly && + imagesDecodedOnDemand() && + rtabmap_conversions::isCompressedRGBDSupportedByRtabmap(compressedImage, compressedDepth, stereo)) + { + rgb.reset(); + depth.reset(); + return; + } + rtabmap_conversions::toCvShare(msg, rgb, depth); +} + void CommonDataSubscriber::commonSingleCameraCallback( const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg, const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg, @@ -1073,7 +1101,9 @@ void CommonDataSubscriber::commonSingleCameraCallback( const std::vector & globalDescriptorMsgs, const std::vector & localKeyPoints, const std::vector & localPoints3d, - const cv::Mat & localDescriptors) + const cv::Mat & localDescriptors, + const cv::Mat & compressedImage, + const cv::Mat & compressedDepth) { std::vector > localKeyPointsMsgs; localKeyPointsMsgs.push_back(localKeyPoints); @@ -1096,7 +1126,25 @@ void CommonDataSubscriber::commonSingleCameraCallback( } cameraInfoMsgs.push_back(rgbCameraInfoMsg); depthCameraInfoMsgs.push_back(depthCameraInfoMsg); - commonMultiCameraCallback( + if(compressedImage.empty() && compressedDepth.empty()) + { + commonMultiCameraCallback( + odomMsg, + userDataMsg, + imageMsgs, + depthMsgs, + cameraInfoMsgs, + depthCameraInfoMsgs, + scanMsg, + scan3dMsg, + odomInfoMsg, + globalDescriptorMsgs, + localKeyPointsMsgs, + localPoints3dMsgs, + localDescriptorsMsgs); + return; + } + commonMultiCameraCallbackWithCompressed( odomMsg, userDataMsg, imageMsgs, @@ -1109,7 +1157,9 @@ void CommonDataSubscriber::commonSingleCameraCallback( globalDescriptorMsgs, localKeyPointsMsgs, localPoints3dMsgs, - localDescriptorsMsgs); + localDescriptorsMsgs, + std::vector(1, compressedImage), + std::vector(1, compressedDepth)); } void CommonDataSubscriber::tick(const rclcpp::Time & stamp, double targetFrequency) diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD.cpp index 9d9c11ca..32833818 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD.cpp @@ -38,7 +38,8 @@ void CommonDataSubscriber::rgbdCallback( { if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; - rtabmap_conversions::toCvShare(image1Msg, rgb, depth); + cv::Mat compressedImage, compressedDepth; + convertRGBDImage(image1Msg, rgb, depth, compressedImage, compressedDepth); rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null @@ -56,7 +57,8 @@ void CommonDataSubscriber::rgbdCallback( depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, image1Msg->key_points, image1Msg->points, - rtabmap::uncompressData(image1Msg->descriptors)); + rtabmap::uncompressData(image1Msg->descriptors), + compressedImage, compressedDepth); } void CommonDataSubscriber::rgbdScan2dCallback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg, @@ -64,7 +66,8 @@ void CommonDataSubscriber::rgbdScan2dCallback( { if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; - rtabmap_conversions::toCvShare(image1Msg, rgb, depth); + cv::Mat compressedImage, compressedDepth; + convertRGBDImage(image1Msg, rgb, depth, compressedImage, compressedDepth); rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null @@ -81,7 +84,8 @@ void CommonDataSubscriber::rgbdScan2dCallback( depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, image1Msg->key_points, image1Msg->points, - rtabmap::uncompressData(image1Msg->descriptors)); + rtabmap::uncompressData(image1Msg->descriptors), + compressedImage, compressedDepth); } void CommonDataSubscriber::rgbdScan3dCallback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg, @@ -89,7 +93,8 @@ void CommonDataSubscriber::rgbdScan3dCallback( { if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; - rtabmap_conversions::toCvShare(image1Msg, rgb, depth); + cv::Mat compressedImage, compressedDepth; + convertRGBDImage(image1Msg, rgb, depth, compressedImage, compressedDepth); rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null @@ -106,7 +111,8 @@ void CommonDataSubscriber::rgbdScan3dCallback( depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, image1Msg->key_points, image1Msg->points, - rtabmap::uncompressData(image1Msg->descriptors)); + rtabmap::uncompressData(image1Msg->descriptors), + compressedImage, compressedDepth); } void CommonDataSubscriber::rgbdScanDescCallback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg, @@ -114,7 +120,8 @@ void CommonDataSubscriber::rgbdScanDescCallback( { if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; - rtabmap_conversions::toCvShare(image1Msg, rgb, depth); + cv::Mat compressedImage, compressedDepth; + convertRGBDImage(image1Msg, rgb, depth, compressedImage, compressedDepth); rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null @@ -125,12 +132,17 @@ void CommonDataSubscriber::rgbdScanDescCallback( { globalDescriptorMsgs.push_back(image1Msg->global_descriptor); } + if(!scanDescMsg->global_descriptor.data.empty()) + { + globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor); + } commonSingleCameraCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, image1Msg->key_points, image1Msg->points, - rtabmap::uncompressData(image1Msg->descriptors)); + rtabmap::uncompressData(image1Msg->descriptors), + compressedImage, compressedDepth); } void CommonDataSubscriber::rgbdInfoCallback( const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg, @@ -138,7 +150,8 @@ void CommonDataSubscriber::rgbdInfoCallback( { if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; - rtabmap_conversions::toCvShare(image1Msg, rgb, depth); + cv::Mat compressedImage, compressedDepth; + convertRGBDImage(image1Msg, rgb, depth, compressedImage, compressedDepth); rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null @@ -155,7 +168,8 @@ void CommonDataSubscriber::rgbdInfoCallback( depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, image1Msg->key_points, image1Msg->points, - rtabmap::uncompressData(image1Msg->descriptors)); + rtabmap::uncompressData(image1Msg->descriptors), + compressedImage, compressedDepth); } // 1 RGBD camera + Odom @@ -165,7 +179,8 @@ void CommonDataSubscriber::rgbdOdomCallback( { if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; - rtabmap_conversions::toCvShare(image1Msg, rgb, depth); + cv::Mat compressedImage, compressedDepth; + convertRGBDImage(image1Msg, rgb, depth, compressedImage, compressedDepth); rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null sensor_msgs::msg::LaserScan scanMsg; // Null @@ -182,7 +197,8 @@ void CommonDataSubscriber::rgbdOdomCallback( depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, image1Msg->key_points, image1Msg->points, - rtabmap::uncompressData(image1Msg->descriptors)); + rtabmap::uncompressData(image1Msg->descriptors), + compressedImage, compressedDepth); } void CommonDataSubscriber::rgbdOdomScan2dCallback( const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg, @@ -191,7 +207,8 @@ void CommonDataSubscriber::rgbdOdomScan2dCallback( { if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; - rtabmap_conversions::toCvShare(image1Msg, rgb, depth); + cv::Mat compressedImage, compressedDepth; + convertRGBDImage(image1Msg, rgb, depth, compressedImage, compressedDepth); rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // Null @@ -207,7 +224,8 @@ void CommonDataSubscriber::rgbdOdomScan2dCallback( depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, image1Msg->key_points, image1Msg->points, - rtabmap::uncompressData(image1Msg->descriptors)); + rtabmap::uncompressData(image1Msg->descriptors), + compressedImage, compressedDepth); } void CommonDataSubscriber::rgbdOdomScan3dCallback( const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg, @@ -216,7 +234,8 @@ void CommonDataSubscriber::rgbdOdomScan3dCallback( { if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; - rtabmap_conversions::toCvShare(image1Msg, rgb, depth); + cv::Mat compressedImage, compressedDepth; + convertRGBDImage(image1Msg, rgb, depth, compressedImage, compressedDepth); rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null sensor_msgs::msg::LaserScan scanMsg; // Null @@ -232,7 +251,8 @@ void CommonDataSubscriber::rgbdOdomScan3dCallback( depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, image1Msg->key_points, image1Msg->points, - rtabmap::uncompressData(image1Msg->descriptors)); + rtabmap::uncompressData(image1Msg->descriptors), + compressedImage, compressedDepth); } void CommonDataSubscriber::rgbdOdomScanDescCallback( const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg, @@ -241,7 +261,8 @@ void CommonDataSubscriber::rgbdOdomScanDescCallback( { if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; - rtabmap_conversions::toCvShare(image1Msg, rgb, depth); + cv::Mat compressedImage, compressedDepth; + convertRGBDImage(image1Msg, rgb, depth, compressedImage, compressedDepth); rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null @@ -260,7 +281,8 @@ void CommonDataSubscriber::rgbdOdomScanDescCallback( depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, image1Msg->key_points, image1Msg->points, - rtabmap::uncompressData(image1Msg->descriptors)); + rtabmap::uncompressData(image1Msg->descriptors), + compressedImage, compressedDepth); } void CommonDataSubscriber::rgbdOdomInfoCallback( const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg, @@ -269,7 +291,8 @@ void CommonDataSubscriber::rgbdOdomInfoCallback( { if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; - rtabmap_conversions::toCvShare(image1Msg, rgb, depth); + cv::Mat compressedImage, compressedDepth; + convertRGBDImage(image1Msg, rgb, depth, compressedImage, compressedDepth); rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null sensor_msgs::msg::LaserScan scanMsg; // Null @@ -285,7 +308,8 @@ void CommonDataSubscriber::rgbdOdomInfoCallback( depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, image1Msg->key_points, image1Msg->points, - rtabmap::uncompressData(image1Msg->descriptors)); + rtabmap::uncompressData(image1Msg->descriptors), + compressedImage, compressedDepth); } #ifdef RTABMAP_SYNC_USER_DATA @@ -296,7 +320,8 @@ void CommonDataSubscriber::rgbdDataCallback( { if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; - rtabmap_conversions::toCvShare(image1Msg, rgb, depth); + cv::Mat compressedImage, compressedDepth; + convertRGBDImage(image1Msg, rgb, depth, compressedImage, compressedDepth); nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null sensor_msgs::msg::LaserScan scanMsg; // Null @@ -313,7 +338,8 @@ void CommonDataSubscriber::rgbdDataCallback( depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, image1Msg->key_points, image1Msg->points, - rtabmap::uncompressData(image1Msg->descriptors)); + rtabmap::uncompressData(image1Msg->descriptors), + compressedImage, compressedDepth); } void CommonDataSubscriber::rgbdDataScan2dCallback( const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg, @@ -322,7 +348,8 @@ void CommonDataSubscriber::rgbdDataScan2dCallback( { if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; - rtabmap_conversions::toCvShare(image1Msg, rgb, depth); + cv::Mat compressedImage, compressedDepth; + convertRGBDImage(image1Msg, rgb, depth, compressedImage, compressedDepth); nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // Null @@ -338,7 +365,8 @@ void CommonDataSubscriber::rgbdDataScan2dCallback( depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, image1Msg->key_points, image1Msg->points, - rtabmap::uncompressData(image1Msg->descriptors)); + rtabmap::uncompressData(image1Msg->descriptors), + compressedImage, compressedDepth); } void CommonDataSubscriber::rgbdDataScan3dCallback( const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg, @@ -347,7 +375,8 @@ void CommonDataSubscriber::rgbdDataScan3dCallback( { if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; - rtabmap_conversions::toCvShare(image1Msg, rgb, depth); + cv::Mat compressedImage, compressedDepth; + convertRGBDImage(image1Msg, rgb, depth, compressedImage, compressedDepth); nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null sensor_msgs::msg::LaserScan scanMsg; // Null @@ -363,7 +392,8 @@ void CommonDataSubscriber::rgbdDataScan3dCallback( depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, image1Msg->key_points, image1Msg->points, - rtabmap::uncompressData(image1Msg->descriptors)); + rtabmap::uncompressData(image1Msg->descriptors), + compressedImage, compressedDepth); } void CommonDataSubscriber::rgbdDataScanDescCallback( const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg, @@ -372,7 +402,8 @@ void CommonDataSubscriber::rgbdDataScanDescCallback( { if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; - rtabmap_conversions::toCvShare(image1Msg, rgb, depth); + cv::Mat compressedImage, compressedDepth; + convertRGBDImage(image1Msg, rgb, depth, compressedImage, compressedDepth); nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null @@ -391,7 +422,8 @@ void CommonDataSubscriber::rgbdDataScanDescCallback( depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, image1Msg->key_points, image1Msg->points, - rtabmap::uncompressData(image1Msg->descriptors)); + rtabmap::uncompressData(image1Msg->descriptors), + compressedImage, compressedDepth); } void CommonDataSubscriber::rgbdDataInfoCallback( const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg, @@ -400,7 +432,8 @@ void CommonDataSubscriber::rgbdDataInfoCallback( { if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; - rtabmap_conversions::toCvShare(image1Msg, rgb, depth); + cv::Mat compressedImage, compressedDepth; + convertRGBDImage(image1Msg, rgb, depth, compressedImage, compressedDepth); nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null sensor_msgs::msg::LaserScan scanMsg; // Null @@ -416,7 +449,8 @@ void CommonDataSubscriber::rgbdDataInfoCallback( depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, image1Msg->key_points, image1Msg->points, - rtabmap::uncompressData(image1Msg->descriptors)); + rtabmap::uncompressData(image1Msg->descriptors), + compressedImage, compressedDepth); } // 1 RGBD camera + Odom + User Data @@ -427,7 +461,8 @@ void CommonDataSubscriber::rgbdOdomDataCallback( { if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; - rtabmap_conversions::toCvShare(image1Msg, rgb, depth); + cv::Mat compressedImage, compressedDepth; + convertRGBDImage(image1Msg, rgb, depth, compressedImage, compressedDepth); sensor_msgs::msg::LaserScan scanMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // Null @@ -443,7 +478,8 @@ void CommonDataSubscriber::rgbdOdomDataCallback( depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, image1Msg->key_points, image1Msg->points, - rtabmap::uncompressData(image1Msg->descriptors)); + rtabmap::uncompressData(image1Msg->descriptors), + compressedImage, compressedDepth); } void CommonDataSubscriber::rgbdOdomDataScan2dCallback( const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg, @@ -453,7 +489,8 @@ void CommonDataSubscriber::rgbdOdomDataScan2dCallback( { if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; - rtabmap_conversions::toCvShare(image1Msg, rgb, depth); + cv::Mat compressedImage, compressedDepth; + convertRGBDImage(image1Msg, rgb, depth, compressedImage, compressedDepth); sensor_msgs::msg::PointCloud2 scan3dMsg; // Null rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null @@ -468,7 +505,8 @@ void CommonDataSubscriber::rgbdOdomDataScan2dCallback( depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, image1Msg->key_points, image1Msg->points, - rtabmap::uncompressData(image1Msg->descriptors)); + rtabmap::uncompressData(image1Msg->descriptors), + compressedImage, compressedDepth); } void CommonDataSubscriber::rgbdOdomDataScan3dCallback( const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg, @@ -478,7 +516,8 @@ void CommonDataSubscriber::rgbdOdomDataScan3dCallback( { if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; - rtabmap_conversions::toCvShare(image1Msg, rgb, depth); + cv::Mat compressedImage, compressedDepth; + convertRGBDImage(image1Msg, rgb, depth, compressedImage, compressedDepth); sensor_msgs::msg::LaserScan scanMsg; // Null rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null @@ -493,7 +532,8 @@ void CommonDataSubscriber::rgbdOdomDataScan3dCallback( depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, image1Msg->key_points, image1Msg->points, - rtabmap::uncompressData(image1Msg->descriptors)); + rtabmap::uncompressData(image1Msg->descriptors), + compressedImage, compressedDepth); } void CommonDataSubscriber::rgbdOdomDataScanDescCallback( const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg, @@ -503,7 +543,8 @@ void CommonDataSubscriber::rgbdOdomDataScanDescCallback( { if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; - rtabmap_conversions::toCvShare(image1Msg, rgb, depth); + cv::Mat compressedImage, compressedDepth; + convertRGBDImage(image1Msg, rgb, depth, compressedImage, compressedDepth); rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null @@ -521,7 +562,8 @@ void CommonDataSubscriber::rgbdOdomDataScanDescCallback( depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, image1Msg->key_points, image1Msg->points, - rtabmap::uncompressData(image1Msg->descriptors)); + rtabmap::uncompressData(image1Msg->descriptors), + compressedImage, compressedDepth); } void CommonDataSubscriber::rgbdOdomDataInfoCallback( const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg, @@ -531,7 +573,8 @@ void CommonDataSubscriber::rgbdOdomDataInfoCallback( { if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} cv_bridge::CvImageConstPtr rgb, depth; - rtabmap_conversions::toCvShare(image1Msg, rgb, depth); + cv::Mat compressedImage, compressedDepth; + convertRGBDImage(image1Msg, rgb, depth, compressedImage, compressedDepth); sensor_msgs::msg::LaserScan scanMsg; // Null sensor_msgs::msg::PointCloud2 scan3dMsg; // Null @@ -545,7 +588,8 @@ void CommonDataSubscriber::rgbdOdomDataInfoCallback( depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, image1Msg->key_points, image1Msg->points, - rtabmap::uncompressData(image1Msg->descriptors)); + rtabmap::uncompressData(image1Msg->descriptors), + compressedImage, compressedDepth); } #endif diff --git a/rtabmap_sync/src/nodelets/rgb_sync.cpp b/rtabmap_sync/src/nodelets/rgb_sync.cpp index 3da278d3..1745d225 100644 --- a/rtabmap_sync/src/nodelets/rgb_sync.cpp +++ b/rtabmap_sync/src/nodelets/rgb_sync.cpp @@ -77,6 +77,12 @@ RGBSync::RGBSync(const rclcpp::NodeOptions & options) : qos = this->declare_parameter("qos", qos); int qosCaminfo = this->declare_parameter("qos_camera_info", qos); compressedRate_ = this->declare_parameter("compressed_rate", compressedRate_); + imageCompressionFormat_ = this->declare_parameter("image_compression_format", imageCompressionFormat_); + if(!rtabmap_conversions::isValidImageCompressionFormat(imageCompressionFormat_)) + { + RCLCPP_ERROR(this->get_logger(), "Invalid image_compression_format \"%s\" (should be \".jpg\" or \".png\"), using \".jpg\".", imageCompressionFormat_.c_str()); + imageCompressionFormat_ = ".jpg"; + } std::string imageTransport = this->declare_parameter("image_transport", std::string("raw")); fillEmptyDepth_ = this->declare_parameter("fill_empty_depth", fillEmptyDepth_); @@ -191,13 +197,12 @@ void RGBSync::callback( rtabmap_msgs::msg::RGBDImage msgCompressed = msg; cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image); - imagePtr->toCompressedImageMsg(msgCompressed.rgb_compressed, cv_bridge::JPG); + rtabmap_conversions::toCompressedImageMsg(*imagePtr, imageCompressionFormat_, msgCompressed.rgb_compressed); if(fillEmptyDepth_) { msgCompressed.depth_compressed.header = image->header; - msgCompressed.depth_compressed.data = rtabmap::compressImage(fakeDepthImage.image, ".png"); - msgCompressed.depth_compressed.format = "png"; + rtabmap_conversions::compressDepthImage(fakeDepthImage.image, ".png", msgCompressed.depth_compressed); } rgbdImageCompressedPub_->publish(msgCompressed); diff --git a/rtabmap_sync/src/nodelets/rgbd_sync.cpp b/rtabmap_sync/src/nodelets/rgbd_sync.cpp index 5528f194..083fd21f 100644 --- a/rtabmap_sync/src/nodelets/rgbd_sync.cpp +++ b/rtabmap_sync/src/nodelets/rgbd_sync.cpp @@ -50,6 +50,7 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) : depthScale_(1.0), decimation_(1), compressedRate_(0), + depthCompressionFormat_(".png"), approxSyncMaxInterval_(0.0), lastCompressedPublished_(0, 0, RCL_ROS_TIME), approxSyncDepth_(0), @@ -78,6 +79,19 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) : depthScale_ = this->declare_parameter("depth_scale", depthScale_); decimation_ = this->declare_parameter("decimation", decimation_); compressedRate_ = this->declare_parameter("compressed_rate", compressedRate_); + imageCompressionFormat_ = this->declare_parameter("image_compression_format", imageCompressionFormat_); + if(!rtabmap_conversions::isValidImageCompressionFormat(imageCompressionFormat_)) + { + RCLCPP_ERROR(this->get_logger(), "Invalid image_compression_format \"%s\" (should be \".jpg\" or \".png\"), using \".jpg\".", imageCompressionFormat_.c_str()); + imageCompressionFormat_ = ".jpg"; + } + depthCompressionFormat_ = this->declare_parameter("depth_compression_format", depthCompressionFormat_); + if(!rtabmap_conversions::isValidDepthCompressionFormat(depthCompressionFormat_)) + { + RCLCPP_ERROR(this->get_logger(), "Invalid depth_compression_format \"%s\" (should be \".png\" or \".rvl\", " + "optionally followed by \":maxDepth[:quantization]\", optionally prefixed by \"legacy:\", or \"legacy\"), using \".png\".", depthCompressionFormat_.c_str()); + depthCompressionFormat_ = ".png"; + } std::string rgbImageTransport = this->declare_parameter("rgb_image_transport", std::string("raw")); std::string depthImageTransport = this->declare_parameter("depth_image_transport", std::string("raw")); if(rgbImageTransport != "raw") { @@ -272,12 +286,10 @@ void RGBDSync::callback( cvImg.header = image->header; cvImg.image = rgbMat; cvImg.encoding = image->encoding; - cvImg.toCompressedImageMsg(msgCompressed->rgb_compressed, cv_bridge::JPG); + rtabmap_conversions::toCompressedImageMsg(cvImg, imageCompressionFormat_, msgCompressed->rgb_compressed); msgCompressed->depth_compressed.header = imageDepthPtr->header; - msgCompressed->depth_compressed.data = rtabmap::compressImage(depthMat, ".png"); - - msgCompressed->depth_compressed.format = "png"; + rtabmap_conversions::compressDepthImage(depthMat, depthCompressionFormat_, msgCompressed->depth_compressed); rgbdImageCompressedPub_->publish(std::move(msgCompressed)); } diff --git a/rtabmap_sync/src/nodelets/stereo_sync.cpp b/rtabmap_sync/src/nodelets/stereo_sync.cpp index aec5002e..6b00e008 100644 --- a/rtabmap_sync/src/nodelets/stereo_sync.cpp +++ b/rtabmap_sync/src/nodelets/stereo_sync.cpp @@ -73,6 +73,12 @@ StereoSync::StereoSync(const rclcpp::NodeOptions & options) : qos = this->declare_parameter("qos", qos); int qosCamInfo = this->declare_parameter("qos_camera_info", qos); compressedRate_ = this->declare_parameter("compressed_rate", compressedRate_); + imageCompressionFormat_ = this->declare_parameter("image_compression_format", imageCompressionFormat_); + if(!rtabmap_conversions::isValidImageCompressionFormat(imageCompressionFormat_)) + { + RCLCPP_ERROR(this->get_logger(), "Invalid image_compression_format \"%s\" (should be \".jpg\" or \".png\"), using \".jpg\".", imageCompressionFormat_.c_str()); + imageCompressionFormat_ = ".jpg"; + } std::string imageTransport = this->declare_parameter("image_transport", std::string("raw")); RCLCPP_INFO(this->get_logger(), "%s: approx_sync = %s", get_name(), approxSync?"true":"false"); @@ -201,7 +207,7 @@ void StereoSync::callback( catch(cv::Exception& e) { UFATAL("Fatal error while converting left image (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what()); } - imagePtr->toCompressedImageMsg(msgCompressed->rgb_compressed, cv_bridge::JPG); + rtabmap_conversions::toCompressedImageMsg(*imagePtr, imageCompressionFormat_, msgCompressed->rgb_compressed); cv_bridge::CvImageConstPtr imageDepthPtr; try { @@ -210,7 +216,7 @@ void StereoSync::callback( catch(cv::Exception& e) { UFATAL("Fatal error while converting right image (do you have multiple opencv versions? if so, make sure cv_bridge is loading the right opencv libraries on runtime): %s", e.what()); } - imageDepthPtr->toCompressedImageMsg(msgCompressed->depth_compressed, cv_bridge::JPG); + rtabmap_conversions::toCompressedImageMsg(*imageDepthPtr, imageCompressionFormat_, msgCompressed->depth_compressed); rgbdImageCompressedPub_->publish(std::move(msgCompressed)); } diff --git a/rtabmap_sync/test/common_data_subscriber_fixture.hpp b/rtabmap_sync/test/common_data_subscriber_fixture.hpp index 0b01f22f..31ca0372 100644 --- a/rtabmap_sync/test/common_data_subscriber_fixture.hpp +++ b/rtabmap_sync/test/common_data_subscriber_fixture.hpp @@ -48,9 +48,17 @@ public: bool hasScan2d = false; ///< a non-empty LaserScan reached the callback bool hasScan3d = false; ///< a non-empty PointCloud2 reached the callback size_t globalDescriptors = 0; + size_t compressedImages = 0; ///< compressed color images passed along, not empty + size_t compressedDepths = 0; ///< compressed depth images passed along, not empty std::string frameId; }; + /// What imagesDecodedOnDemand() returns: whether images may be left compressed. + bool decodeOnDemand = false; + /// Leaves commonMultiCameraCallbackWithCompressed() to CommonDataSubscriber's default, + /// like a subclass that does not override it. + bool defaultCompressedCallback = false; + /** * @param options ROS options; the subscribe_* parameters go in here * @param gui the flag the real subclasses pass: false for the SLAM node, true for @@ -69,6 +77,40 @@ public: const Record & back() const { return records_.back(); } protected: + bool imagesDecodedOnDemand() const override { return decodeOnDemand; } + + void commonMultiCameraCallbackWithCompressed( + const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg, + const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg, + const std::vector & imageMsgs, + const std::vector & depthMsgs, + const std::vector & cameraInfoMsgs, + const std::vector & depthCameraInfoMsgs, + const sensor_msgs::msg::LaserScan & scanMsg, + const sensor_msgs::msg::PointCloud2 & scan3dMsg, + const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr & odomInfoMsg, + const std::vector & globalDescriptorMsgs, + const std::vector > & localKeyPoints, + const std::vector > & localPoints3d, + const std::vector & localDescriptors, + const std::vector & compressedImages, + const std::vector & compressedDepths) override + { + if(defaultCompressedCallback) + { + CommonDataSubscriber::commonMultiCameraCallbackWithCompressed(odomMsg, userDataMsg, + imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, + odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, + localDescriptors, compressedImages, compressedDepths); + return; + } + commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, + depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, + localKeyPoints, localPoints3d, localDescriptors); + for(const cv::Mat & m : compressedImages) { records_.back().compressedImages += m.empty()?0:1; } + for(const cv::Mat & m : compressedDepths) { records_.back().compressedDepths += m.empty()?0:1; } + } + void commonMultiCameraCallback( const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg, const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg, diff --git a/rtabmap_sync/test/test_common_data_subscriber_sync.cpp b/rtabmap_sync/test/test_common_data_subscriber_sync.cpp index ba2d0f63..309112aa 100644 --- a/rtabmap_sync/test/test_common_data_subscriber_sync.cpp +++ b/rtabmap_sync/test/test_common_data_subscriber_sync.cpp @@ -6,6 +6,7 @@ All rights reserved. (BSD-3-Clause, see the repository root.) #include "common_data_subscriber_fixture.hpp" #include +#include using namespace rtabmap_sync_test; @@ -267,6 +268,235 @@ TEST_F(CommonDataSubscriberSyncTest, RGBDModeUnpacksTheMessageIntoImages) EXPECT_EQ(got.frameId, "camera_link"); } +namespace { + +/// An RGBDImage with its images compressed only, in compressed_image_transport's and +/// compressed_depth_image_transport's formats. +rtabmap_msgs::msg::RGBDImage makeCompressedRGBDImage( + const std::string & frameId, double stamp, const std::string & colorEncoding = "bgr8") +{ + rtabmap_msgs::msg::RGBDImage msg = makeRGBDImage(frameId, stamp); + const cv::Mat color = colorEncoding == "mono16" ? + cv::Mat(8, 8, CV_16UC1, cv::Scalar(1000)) : cv_bridge::toCvCopy(msg.rgb)->image; + EXPECT_TRUE(rtabmap_conversions::toCompressedImageMsg( + cv_bridge::CvImage(msg.rgb.header, colorEncoding, color), ".png", msg.rgb_compressed)); + msg.depth_compressed.header = msg.depth.header; + EXPECT_TRUE(rtabmap_conversions::compressDepthImage( + cv_bridge::toCvCopy(msg.depth)->image, ".png", msg.depth_compressed)); + msg.rgb = sensor_msgs::msg::Image(); + msg.depth = sensor_msgs::msg::Image(); + return msg; +} + +} // namespace + +/// A subclass accepting it gets compressed images left compressed, not decoded. +TEST_F(CommonDataSubscriberSyncTest, RGBDModeLeavesCompressedImagesCompressedWhenAccepted) +{ + start({rclcpp::Parameter("subscribe_rgbd", true), + rclcpp::Parameter("subscribe_odom", false)}); + sub_->decodeOnDemand = true; + rclcpp::Publisher::SharedPtr rgbd = + advertise("rgbd_image"); + + rgbd->publish(makeCompressedRGBDImage("camera_link", 1000.0)); + ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); })); + EXPECT_EQ(sub_->back().images, 0u) << "not decoded"; + EXPECT_EQ(sub_->back().depths, 0u) << "not decoded"; + EXPECT_EQ(sub_->back().compressedImages, 1u); + EXPECT_EQ(sub_->back().compressedDepths, 1u); + EXPECT_EQ(sub_->back().cameraInfos, 1u); + EXPECT_EQ(sub_->back().frameId, "camera_link"); +} + +/// A stereo pair (baseline in the right camera info) with a gray right image is left +/// compressed too. +TEST_F(CommonDataSubscriberSyncTest, RGBDModeLeavesACompressedStereoPairCompressedWhenAccepted) +{ + start({rclcpp::Parameter("subscribe_rgbd", true), + rclcpp::Parameter("subscribe_odom", false)}); + sub_->decodeOnDemand = true; + rclcpp::Publisher::SharedPtr rgbd = + advertise("rgbd_image"); + + rtabmap_msgs::msg::RGBDImage stereo = makeCompressedRGBDImage("camera_link", 1000.0); + stereo.depth_camera_info.p[3] = -5.0; + EXPECT_TRUE(rtabmap_conversions::toCompressedImageMsg( + cv_bridge::CvImage(stereo.rgb_compressed.header, "mono8", cv::Mat(8, 8, CV_8UC1, cv::Scalar(60))), + ".jpg", stereo.depth_compressed)); + + rgbd->publish(stereo); + ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); })); + EXPECT_EQ(sub_->back().images, 0u) << "not decoded"; + EXPECT_EQ(sub_->back().depths, 0u) << "not decoded"; + EXPECT_EQ(sub_->back().compressedImages, 1u); + EXPECT_EQ(sub_->back().compressedDepths, 1u); +} + +/// By default, or when rtabmap could not use them as they are, images are decoded. +TEST_F(CommonDataSubscriberSyncTest, RGBDModeDecodesCompressedImagesOtherwise) +{ + start({rclcpp::Parameter("subscribe_rgbd", true), + rclcpp::Parameter("subscribe_odom", false)}); + rclcpp::Publisher::SharedPtr rgbd = + advertise("rgbd_image"); + size_t received = 0; + const auto publish = [&](const rtabmap_msgs::msg::RGBDImage & msg) { + rgbd->publish(msg); + EXPECT_TRUE(spinUntil([&]() { return sub_->size() > received; })); + received = sub_->size(); + return sub_->back(); + }; + + // Not accepted by the subclass (the default) + RecordingSubscriber::Record got = publish(makeCompressedRGBDImage("camera_link", 1000.0)); + EXPECT_EQ(got.images, 1u); + EXPECT_EQ(got.depths, 1u); + EXPECT_EQ(got.compressedImages, 1u) << "still passed along, to be stored as is"; + + sub_->decodeOnDemand = true; + + // 16 bits color: rtabmap needs it converted to 8 bits + got = publish(makeCompressedRGBDImage("camera_link", 1001.0, "mono16")); + EXPECT_EQ(got.images, 1u); + + // Stereo pair with a color right image: rtabmap needs it converted to gray + rtabmap_msgs::msg::RGBDImage stereo = makeCompressedRGBDImage("camera_link", 1002.0); + stereo.depth_camera_info.p[3] = -5.0; + stereo.depth_compressed = stereo.rgb_compressed; + got = publish(stereo); + EXPECT_EQ(got.images, 1u); + EXPECT_EQ(got.depths, 1u); + + // A raw image next to a compressed one + rtabmap_msgs::msg::RGBDImage mixed = makeCompressedRGBDImage("camera_link", 1003.0); + mixed.rgb = makeRGBDImage("camera_link", 1003.0).rgb; + got = publish(mixed); + EXPECT_EQ(got.images, 1u); + EXPECT_EQ(got.depths, 1u); +} + +/// A subclass not overriding commonMultiCameraCallbackWithCompressed() gets the decoded +/// images in commonMultiCameraCallback(), the compressed ones are dropped. +TEST_F(CommonDataSubscriberSyncTest, RGBDModeFallsBackOnTheDecodedImagesCallback) +{ + start({rclcpp::Parameter("subscribe_rgbd", true), + rclcpp::Parameter("subscribe_odom", false)}); + sub_->defaultCompressedCallback = true; + rclcpp::Publisher::SharedPtr rgbd = + advertise("rgbd_image"); + + rgbd->publish(makeCompressedRGBDImage("camera_link", 1000.0)); + ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); })); + EXPECT_EQ(sub_->back().images, 1u); + EXPECT_EQ(sub_->back().depths, 1u); + EXPECT_EQ(sub_->back().compressedImages, 0u); + EXPECT_EQ(sub_->back().compressedDepths, 0u); + EXPECT_EQ(sub_->back().frameId, "camera_link"); +} + +namespace { + +/// What is synchronized with the RGBDImage: each combination has its own callback. +struct RGBDInputs +{ + std::string name; + bool odom; + std::string extra; ///< "", "scan", "scan_cloud", "scan_descriptor" or "odom_info" +}; + +std::ostream & operator<<(std::ostream & os, const RGBDInputs & inputs) { return os << inputs.name; } + +class RGBDModeInputsTest : + public CommonDataSubscriberTest, + public ::testing::WithParamInterface {}; + +} // namespace + +/// Every callback of a single RGBDImage passes its compressed images along, and what is +/// synchronized with it. +TEST_P(RGBDModeInputsTest, PassesCompressedImagesAlong) +{ + const RGBDInputs & inputs = GetParam(); + std::vector params = { + rclcpp::Parameter("subscribe_rgbd", true), + rclcpp::Parameter("subscribe_odom", inputs.odom)}; + if(!inputs.extra.empty()) + { + params.push_back(rclcpp::Parameter("subscribe_" + inputs.extra, true)); + } + start(params); + sub_->decodeOnDemand = true; + + rclcpp::Publisher::SharedPtr rgbd = + advertise("rgbd_image"); + rclcpp::Publisher::SharedPtr odom; + if(inputs.odom) + { + odom = advertise("odom"); + } + rclcpp::Publisher::SharedPtr scan; + rclcpp::Publisher::SharedPtr cloud; + rclcpp::Publisher::SharedPtr descriptor; + rclcpp::Publisher::SharedPtr odomInfo; + if(inputs.extra == "scan") + { + scan = advertise("scan"); + } + else if(inputs.extra == "scan_cloud") + { + cloud = advertise("scan_cloud"); + } + else if(inputs.extra == "scan_descriptor") + { + descriptor = advertise("scan_descriptor"); + } + else if(inputs.extra == "odom_info") + { + odomInfo = advertise("odom_info"); + } + + rgbd->publish(makeCompressedRGBDImage("camera_link", 1000.0)); + if(odom) { odom->publish(makeOdometry("odom", 1000.0, 1.5)); } + if(scan) { scan->publish(makeLaserScan("base_scan", 1000.0)); } + if(cloud) { cloud->publish(makeScanCloud("lidar_link", 1000.0)); } + if(descriptor) + { + descriptor->publish(makeScanDescriptor("base_scan", 1000.0, + /*with2d=*/true, /*with3d=*/false, /*withGlobalDescriptor=*/true)); + } + if(odomInfo) { odomInfo->publish(makeOdomInfo("odom", 1000.0)); } + ASSERT_TRUE(spinUntil([&]() { return !sub_->empty(); })); + + const RecordingSubscriber::Record & got = sub_->back(); + EXPECT_EQ(got.kind, RecordingSubscriber::Record::kMultiCamera); + EXPECT_EQ(got.images, 0u) << "not decoded"; + EXPECT_EQ(got.depths, 0u) << "not decoded"; + EXPECT_EQ(got.compressedImages, 1u); + EXPECT_EQ(got.compressedDepths, 1u); + EXPECT_EQ(got.frameId, "camera_link"); + EXPECT_EQ(got.hasOdom, inputs.odom); + EXPECT_EQ(got.hasScan2d, inputs.extra == "scan" || inputs.extra == "scan_descriptor"); + EXPECT_EQ(got.hasScan3d, inputs.extra == "scan_cloud"); + EXPECT_EQ(got.globalDescriptors, inputs.extra == "scan_descriptor" ? 1u : 0u); + EXPECT_EQ(got.hasOdomInfo, inputs.extra == "odom_info"); +} + +INSTANTIATE_TEST_SUITE_P( + Inputs, RGBDModeInputsTest, + ::testing::Values( + RGBDInputs{"rgbd", false, ""}, + RGBDInputs{"scan", false, "scan"}, + RGBDInputs{"scan_cloud", false, "scan_cloud"}, + RGBDInputs{"scan_descriptor", false, "scan_descriptor"}, + RGBDInputs{"odom_info", false, "odom_info"}, + RGBDInputs{"odom", true, ""}, + RGBDInputs{"odom_scan", true, "scan"}, + RGBDInputs{"odom_scan_cloud", true, "scan_cloud"}, + RGBDInputs{"odom_scan_descriptor", true, "scan_descriptor"}, + RGBDInputs{"odom_odom_info", true, "odom_info"}), + [](const ::testing::TestParamInfo & info) { return info.param.name; }); + TEST_F(CommonDataSubscriberSyncTest, RGBDModeCarriesAScanAlongside) { start({rclcpp::Parameter("subscribe_rgbd", true), diff --git a/rtabmap_sync/test/test_rgb_sync.cpp b/rtabmap_sync/test/test_rgb_sync.cpp index cb2902a3..73b0586d 100644 --- a/rtabmap_sync/test/test_rgb_sync.cpp +++ b/rtabmap_sync/test/test_rgb_sync.cpp @@ -9,6 +9,7 @@ All rights reserved. (BSD-3-Clause, see the repository root.) #include #include +#include #include #include @@ -174,6 +175,16 @@ TEST_F(RGBSyncTest, CompressesColorAsJpeg) << "without fill_empty_depth there is nothing to compress on the depth side"; } +TEST_F(RGBSyncTest, ImageCompressionFormatAppliesToTheColorImage) +{ + start({rclcpp::Parameter("image_compression_format", std::string(".png"))}); + collectCompressed(); + + publish(1000.0); + ASSERT_TRUE(spinUntil([&]() { return !compressed_->empty(); })); + EXPECT_EQ(compressed_->back().rgb_compressed.format, "bgr8; png compressed bgr8"); +} + TEST_F(RGBSyncTest, CompressesTheFakeDepthAsPng) { start({rclcpp::Parameter("fill_empty_depth", true)}); @@ -184,8 +195,8 @@ TEST_F(RGBSyncTest, CompressesTheFakeDepthAsPng) const rtabmap_msgs::msg::RGBDImage & got = compressed_->back(); ASSERT_FALSE(got.depth_compressed.data.empty()); - EXPECT_EQ(got.depth_compressed.format, "png"); - const cv::Mat depth = rtabmap::uncompressImage(got.depth_compressed.data); + EXPECT_EQ(got.depth_compressed.format, "16UC1; compressedDepth png"); + const cv::Mat depth = rtabmap_conversions::uncompressDepthImage(got.depth_compressed)->image; ASSERT_FALSE(depth.empty()); EXPECT_EQ(depth.type(), CV_16UC1); EXPECT_EQ(cv::countNonZero(depth), 0) << "the fake depth is all zeros"; diff --git a/rtabmap_sync/test/test_rgbd_sync.cpp b/rtabmap_sync/test/test_rgbd_sync.cpp index cc6c5c8d..74fe3985 100644 --- a/rtabmap_sync/test/test_rgbd_sync.cpp +++ b/rtabmap_sync/test/test_rgbd_sync.cpp @@ -9,6 +9,7 @@ All rights reserved. (BSD-3-Clause, see the repository root.) #include #include +#include #include @@ -70,6 +71,18 @@ protected: } /// Publishes one hardware-synchronized set: every input carries the same stamp. + /// Publishes the three inputs with a 32FC1 depth image of @p meters. + void publishFloatDepth(double stamp, float meters) + { + sensor_msgs::msg::Image depth; + cv_bridge::CvImage(makeRgbImage("camera_link", stamp).header, + sensor_msgs::image_encodings::TYPE_32FC1, + cv::Mat(8, 8, CV_32FC1, cv::Scalar(meters))).toImageMsg(depth); + rgbPub_->publish(makeRgbImage("camera_link", stamp)); + depthPub_->publish(depth); + infoPub_->publish(makeCameraInfo("camera_link", stamp)); + } + void publish(double stamp, int width = 8, int height = 8, uint16_t depthMillimeters = 1500) { @@ -287,14 +300,42 @@ TEST_F(RGBDSyncTest, CompressesColorAsJpegAndDepthAsPng) const rtabmap_msgs::msg::RGBDImage & got = compressed_->back(); EXPECT_FALSE(got.rgb_compressed.data.empty()); EXPECT_FALSE(got.depth_compressed.data.empty()); - EXPECT_EQ(got.depth_compressed.format, "png") << "depth must stay lossless"; + EXPECT_EQ(got.depth_compressed.format, "16UC1; compressedDepth png") + << "depth must stay lossless, in compressed_depth_image_transport's format"; EXPECT_NE(got.rgb_compressed.format.find("jp"), std::string::npos) << "expected a jpeg format, got \"" << got.rgb_compressed.format << "\""; + EXPECT_EQ(got.rgb_compressed.format, "bgr8; jpeg compressed bgr8") + << "compressed_image_transport's format, with the encoding"; EXPECT_TRUE(got.rgb.data.empty()) << "the compressed output carries no raw images"; EXPECT_TRUE(got.depth.data.empty()); EXPECT_EQ(got.header.frame_id, "camera_link"); } +TEST_F(RGBDSyncTest, ImageCompressionFormatAppliesToTheColorImage) +{ + start({rclcpp::Parameter("image_compression_format", std::string(".png"))}); + collectCompressed(); + + publish(1000.0); + ASSERT_TRUE(spinUntil([&]() { return !compressed_->empty(); })); + const rtabmap_msgs::msg::RGBDImage & got = compressed_->back(); + EXPECT_EQ(got.rgb_compressed.format, "bgr8; png compressed bgr8"); + // Lossless: the color is the input's + const cv::Mat rgb = cv_bridge::toCvCopy(got.rgb_compressed)->image; + const cv::Mat input = cv_bridge::toCvCopy(makeRgbImage("camera_link", 1000.0))->image; + EXPECT_EQ(cv::norm(rgb, input, cv::NORM_INF), 0.0); +} + +TEST_F(RGBDSyncTest, InvalidImageCompressionFormatFallsBackToJpeg) +{ + start({rclcpp::Parameter("image_compression_format", std::string("bmp"))}); + collectCompressed(); + + publish(1000.0); + ASSERT_TRUE(spinUntil([&]() { return !compressed_->empty(); })); + EXPECT_EQ(compressed_->back().rgb_compressed.format, "bgr8; jpeg compressed bgr8"); +} + TEST_F(RGBDSyncTest, TheCompressedDepthDecompressesBackToTheInput) { start(); @@ -304,7 +345,7 @@ TEST_F(RGBDSyncTest, TheCompressedDepthDecompressesBackToTheInput) ASSERT_TRUE(spinUntil([&]() { return !compressed_->empty(); })); const cv::Mat depth = - rtabmap::uncompressImage(compressed_->back().depth_compressed.data); + rtabmap_conversions::uncompressDepthImage(compressed_->back().depth_compressed)->image; ASSERT_FALSE(depth.empty()); EXPECT_EQ(depth.type(), CV_16UC1); EXPECT_EQ(depth.cols, 8); @@ -313,6 +354,63 @@ TEST_F(RGBDSyncTest, TheCompressedDepthDecompressesBackToTheInput) << "png is lossless, so the value must survive the round trip exactly"; } +TEST_F(RGBDSyncTest, DepthCompressionFormatAppliesToTheCompressedDepth) +{ + start({rclcpp::Parameter("depth_compression_format", std::string(".rvl:10:100"))}); + collectCompressed(); + + // 16UC1: the codec is used, losslessly; the inverse depth parameters are ignored. + // (RVL is re-compressed as PNG before Jazzy, compressed_depth_image_transport cannot + // decode it.) + publish(1000.0, 8, 8, /*depthMillimeters=*/1234); + ASSERT_TRUE(spinUntil([&]() { return !compressed_->empty(); })); + EXPECT_EQ(compressed_->back().depth_compressed.format.rfind("16UC1; compressedDepth ", 0), 0u) + << compressed_->back().depth_compressed.format; + cv::Mat depth = rtabmap_conversions::uncompressDepthImage(compressed_->back().depth_compressed)->image; + ASSERT_EQ(depth.type(), CV_16UC1); + EXPECT_EQ(depth.at(0, 0), 1234); + + // 32FC1: 16 bits inverse depth + const size_t received = compressed_->size(); + publishFloatDepth(1001.0, 2.0f); + ASSERT_TRUE(spinUntil([&]() { return compressed_->size() > received; })); + EXPECT_EQ(compressed_->back().depth_compressed.format.rfind("32FC1; compressedDepth ", 0), 0u) + << compressed_->back().depth_compressed.format; + depth = rtabmap_conversions::uncompressDepthImage(compressed_->back().depth_compressed)->image; + ASSERT_EQ(depth.type(), CV_32FC1); + EXPECT_NEAR(depth.at(0, 0), 2.0f, 0.001f); +} + +TEST_F(RGBDSyncTest, FloatDepthIsCompressedInMillimetersByDefault) +{ + start(); + collectCompressed(); + publishFloatDepth(1000.0, 2.0004f); + ASSERT_TRUE(spinUntil([&]() { return !compressed_->empty(); })); + + const sensor_msgs::msg::CompressedImage & got = compressed_->back().depth_compressed; + EXPECT_EQ(got.format, "16UC1; compressedDepth png"); + const cv::Mat depth = rtabmap_conversions::uncompressDepthImage(got)->image; + ASSERT_EQ(depth.type(), CV_16UC1); + EXPECT_EQ(depth.at(0, 0), 2000); +} + +TEST_F(RGBDSyncTest, LegacyKeepsFloatDepthLossless) +{ + // rtabmap's format before 0.24: 32FC1 kept losslessly, readable by older versions + start({rclcpp::Parameter("depth_compression_format", std::string("legacy"))}); + collectCompressed(); + publishFloatDepth(1000.0, 2.0004f); + ASSERT_TRUE(spinUntil([&]() { return !compressed_->empty(); })); + + const sensor_msgs::msg::CompressedImage & got = compressed_->back().depth_compressed; + EXPECT_EQ(got.format, "png"); + const cv::Mat depth = rtabmap::uncompressImage(got.data); + ASSERT_EQ(depth.type(), CV_32FC1); + EXPECT_EQ(depth.at(0, 0), 2.0004f); + EXPECT_EQ(rtabmap_conversions::uncompressDepthImage(got)->image.at(0, 0), 2.0004f); +} + TEST_F(RGBDSyncTest, CompressedRateThrottlesTheCompressedOutputOnly) { // Compression is expensive and the compressed topic usually feeds a slow link, so diff --git a/rtabmap_sync/test/test_stereo_sync.cpp b/rtabmap_sync/test/test_stereo_sync.cpp index 384d194a..9fb095bf 100644 --- a/rtabmap_sync/test/test_stereo_sync.cpp +++ b/rtabmap_sync/test/test_stereo_sync.cpp @@ -199,6 +199,19 @@ TEST_F(StereoSyncTest, ApproxSyncMaxIntervalRejectsDistantFrames) EXPECT_TRUE(spinUntil([&]() { return !out_->empty(); })); } +TEST_F(StereoSyncTest, ImageCompressionFormatAppliesToBothImages) +{ + start({rclcpp::Parameter("image_compression_format", std::string(".png"))}); + collectCompressed(); + + publish(1000.0); + ASSERT_TRUE(spinUntil([&]() { return !compressed_->empty(); })); + + const rtabmap_msgs::msg::RGBDImage & got = compressed_->back(); + EXPECT_EQ(got.rgb_compressed.format, "mono8; png compressed mono8"); + EXPECT_EQ(got.depth_compressed.format, "mono8; png compressed mono8"); +} + TEST_F(StereoSyncTest, CompressesBothImagesAsJpeg) { // Both halves of a stereo pair are ordinary images, so both take the lossy path -- diff --git a/rtabmap_util/CMakeLists.txt b/rtabmap_util/CMakeLists.txt index 9104af74..cb5f493d 100644 --- a/rtabmap_util/CMakeLists.txt +++ b/rtabmap_util/CMakeLists.txt @@ -305,6 +305,8 @@ install(DIRECTORY include/ ############# if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) + find_package(ament_index_cpp REQUIRED) + find_package(class_loader REQUIRED) # Each node gets its own test binary: a crash or a stuck executor in one node cannot # take the others down, and every binary starts with a clean DDS graph. @@ -334,6 +336,12 @@ if(BUILD_TESTING) rtabmap_util_add_node_test(test_disparity_to_depth) rtabmap_util_add_node_test(test_rgbd_relay) rtabmap_util_add_node_test(test_rgbd_split) + rtabmap_util_add_node_test(test_rgbd_pipeline) + # Loads rtabmap_sync's components by hand: since Kilted, rclcpp_components no longer + # exports these to its dependents. + if(TARGET test_rgbd_pipeline) + target_link_libraries(test_rgbd_pipeline ament_index_cpp::ament_index_cpp class_loader::class_loader) + endif() rtabmap_util_add_node_test(test_lidar_deskewing) rtabmap_util_add_node_test(test_obstacles_detection) rtabmap_util_add_node_test(test_point_cloud_aggregator) diff --git a/rtabmap_util/doc/rgbd_relay.md b/rtabmap_util/doc/rgbd_relay.md index f3574b7d..3dbb1ded 100644 --- a/rtabmap_util/doc/rgbd_relay.md +++ b/rtabmap_util/doc/rgbd_relay.md @@ -76,8 +76,10 @@ flowchart LR | Parameter | Type | Default | Description | |---|---|---|---| -| `compress` | `bool` | `false` | Fill the compressed fields of the output. Color becomes JPEG; depth becomes PNG, or JPEG when the message carries a stereo pair rather than depth. Fields already compressed on input are passed through as-is. | -| `uncompress` | `bool` | `false` | Fill the raw fields of the output by decoding the compressed ones. Fields already raw on input are passed through as-is. | +| `compress` | `bool` | `false` | Fill the compressed fields of the output. Color becomes JPEG; depth becomes PNG, or JPEG when the message carries a stereo pair rather than depth (see `image_compression_format` and `depth_compression_format`). Fields already compressed on input are passed through as-is. | +| `image_compression_format` | `string` | `".jpg"` | With `compress`, format of the color image, and of the right image of a stereo pair: `".jpg"` (lossy, smaller) or `".png"` (lossless, larger). An invalid value falls back to `".jpg"` with an error. | +| `uncompress` | `bool` | `false` | Fill the raw fields of the output by decoding the compressed ones. Fields already raw on input are passed through as-is. Depth compressed by rtabmap or by `compressed_depth_image_transport` is accepted. | +| `depth_compression_format` | `string` | `".png"` | With `compress`, format of the compressed depth, see [rgbd_sync](https://docs.ros.org/en/jazzy/p/rtabmap_sync/)'s parameter of the same name: `".png"` or `".rvl"` (32FC1 converted to millimeters), optionally followed by `":[:]"` (32FC1 as inverse depth), optionally prefixed by `"legacy:"` (rtabmap's own format, 32FC1 lossless without depth parameters), or `"legacy"` (`"legacy:.png"`). | | `qos` | `int` | `0` | Reliability of both sides: `0` system default, `1` reliable, `2` best effort. | | `qos_sub` | `int` | value of `qos` | Reliability of the `rgbd_image` subscription alone. | | `qos_pub` | `int` | value of `qos` | Reliability of the `rgbd_image_relay` publisher alone. | @@ -118,4 +120,4 @@ Setting neither `compress` nor `uncompress` forwards the message unchanged and s Setting both is allowed and produces a message carrying each image twice, raw and compressed. That is rarely what you want. -Depth is compressed as **PNG**, a stereo right image as **JPEG**. +Depth is compressed as **PNG** by default (see `depth_compression_format`), in `compressed_depth_image_transport`'s format, color and stereo right images as **JPEG** by default (see `image_compression_format`), in `compressed_image_transport`'s format. diff --git a/rtabmap_util/doc/rgbd_split.md b/rtabmap_util/doc/rgbd_split.md index fac1b1b2..2217d6d6 100644 --- a/rtabmap_util/doc/rgbd_split.md +++ b/rtabmap_util/doc/rgbd_split.md @@ -13,6 +13,7 @@ It is the inverse of [rtabmap_sync](https://docs.ros.org/en/jazzy/p/rtabmap_sync - [Published Topics](#published-topics) - [Parameters](#parameters) - [Stereo messages](#stereo-messages) +- [Compressed images](#compressed-images) - [Notes](#notes) ## Usage @@ -57,6 +58,8 @@ The output topics are named after the **resolved** input topic, so remapping `rg | `/rgb/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | | | `/depth/image` | [`sensor_msgs/msg/Image`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/Image.html) | The depth image, or the right image of a stereo pair. | | `/depth/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | For a stereo pair this is the right camera, and its `P(0,3)` carries the baseline. | +| `/rgb/image/compressed` | [`sensor_msgs/msg/CompressedImage`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CompressedImage.html) | The color image in [`compressed_image_transport`](https://github.com/ros-perception/image_transport_plugins)'s format, published by the node itself unless `compressed_passthrough` is false. Same for `/right/image/compressed` with `stereo: true`. See [Compressed images](#compressed-images). | +| `/depth/image/compressedDepth` | [`sensor_msgs/msg/CompressedImage`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CompressedImage.html) | The depth image in [`compressed_depth_image_transport`](https://github.com/ros-perception/image_transport_plugins)'s format, published by the node itself unless `compressed_depth_passthrough` is false. See [Compressed images](#compressed-images). | Each half is only unpacked if something is subscribed to it, so subscribing to color alone does not pay for depth decompression. @@ -70,6 +73,12 @@ Each half is only unpacked if something is subscribed to it, so subscribing to c | `queue_sub` | `int` | `5` | Queue depth of the `rgbd_image` subscription. Must be at least 1. | | `queue_pub` | `int` | `1` | Queue depth of every publisher. Must be at least 1. | | `stereo` | `bool` | `false` | Name the outputs `left`/`right` instead of `rgb`/`depth`. See [Stereo messages](#stereo-messages). | +| `compressed_passthrough` | `bool` | `true` | Publish the `compressed` topics (color, left and right images) from the compressed images of the input without decompressing them, in place of the `compressed` image_transport plugin. See [Compressed images](#compressed-images). | +| `compressed_depth_passthrough` | `bool` | `compressed_passthrough` | Same for the `compressedDepth` topic (depth), in place of the `compressedDepth` image_transport plugin. | +| `.compressed.format` | `string` | `"jpeg"` | `jpeg` or `png`. With `compressed_passthrough`, used only when an image (color, left or right) has to be compressed. `` is the color (or left) topic with `.` instead of `/`, e.g. `rgbd_image.rgb.image`. | +| `.compressedDepth.format` | `string` | `"png"` | `png` or `rvl` (Jazzy and later). With `compressed_depth_passthrough`, used only when the depth has to be compressed. `` is the depth topic with `.` instead of `/`, e.g. `rgbd_image.depth.image`. | +| `.compressedDepth.depth_max` | `double` | `10.0` | Same, maximum depth (m) of a 32FC1 depth image. | +| `.compressedDepth.depth_quantization` | `double` | `100.0` | Same, depth quantization of a 32FC1 depth image. | ## Stereo messages @@ -92,6 +101,27 @@ Only the names change — the message contents and the order of the two halves a The node checks the setting against what actually arrives, going by the encoding of the second half: `16UC1`, `32FC1` and `mono16` are depth, anything else is an image. If the two disagree it logs a warning **once** and keeps forwarding — a mismatch makes the topic name misleading, not the data wrong, so it is never worth dropping a frame over. +## Compressed images + +`rgb_compressed` of an `RGBDImage` holds a JPEG or PNG image, and so does `depth_compressed` for the right image of a stereo pair. With `compressed_passthrough` (the default), the node republishes them as they are on `/rgb/image/compressed` (and `/right/image/compressed`), readable with the `compressed` transport. Their format is set from the image header, with the encoding (e.g. `"bgr8; jpeg compressed bgr8"`), which producers before 0.24 did not set. Raw images, or images whose header cannot be read, are compressed with `.compressed.format`. + +`depth_compressed` of an `RGBDImage` holds depth in `compressed_depth_image_transport`'s format, or, from rtabmap_ros before 0.24 or with `depth_compression_format: "legacy"`, in rtabmap's own format (`png`, `rvl`, `png::`, `rvl::`), which image_transport plugins cannot read. The node republishes both on `/depth/image/compressedDepth`, readable with the `compressedDepth` transport: + +```bash +ros2 run image_transport republish compressedDepth raw --ros-args \ + -r in/compressedDepth:=/camera/rgbd_image/depth/image/compressedDepth -r out:=/depth +``` + +With `compressed_depth_passthrough` (the default), the depth is not decompressed when that can be avoided: + +| Input depth | `compressedDepth` output | +|---|---| +| `compressed_depth_image_transport` format | Republished as is (RVL re-compressed as PNG before Jazzy). | +| rtabmap's legacy `png` (16UC1), `rvl`, `png::`, `rvl::` | Only the header is converted, the compressed payload is copied. Before Jazzy, `compressed_depth_image_transport` cannot decode RVL, so RVL payloads are re-compressed as PNG, losslessly. | +| Raw, or rtabmap's legacy `png` of 32FC1 depth (4 channels) | Compressed with the `.compressedDepth.*` parameters. | + +The passthrough does not support the plugins' `jpeg_quality` and `png_level` parameters: to set them, set `compressed_passthrough: false` (`compressed` plugin) or `compressed_depth_passthrough: false` (`compressedDepth` plugin), and the plugin decompresses and re-compresses every frame instead. `compressed_depth_passthrough` follows `compressed_passthrough` unless set, so e.g. `compressed_passthrough: false` with `compressed_depth_passthrough: true` sets `jpeg_quality` of the color image while still passing the depth through. + ## Notes If a message has no `frame_id` on one of its sub-messages, the node fills it in from the other one so the output is always usable by TF. diff --git a/rtabmap_util/include/rtabmap_util/rgbd_relay.hpp b/rtabmap_util/include/rtabmap_util/rgbd_relay.hpp index f2972f26..17782d81 100644 --- a/rtabmap_util/include/rtabmap_util/rgbd_relay.hpp +++ b/rtabmap_util/include/rtabmap_util/rgbd_relay.hpp @@ -45,6 +45,9 @@ private: private: bool compress_; bool uncompress_; + std::string depthCompressionFormat_; + /// Format of the compressed color (and right) images: ".jpg" or ".png". + std::string imageCompressionFormat_ = ".jpg"; rclcpp::Subscription::SharedPtr rgbdImageSub_; rclcpp::Publisher::SharedPtr rgbdImagePub_; }; diff --git a/rtabmap_util/include/rtabmap_util/rgbd_split.hpp b/rtabmap_util/include/rtabmap_util/rgbd_split.hpp index 6478dc5e..be264fc5 100644 --- a/rtabmap_util/include/rtabmap_util/rgbd_split.hpp +++ b/rtabmap_util/include/rtabmap_util/rgbd_split.hpp @@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include "rtabmap_msgs/msg/rgbd_image.hpp" +#include namespace rtabmap_util { @@ -55,6 +56,32 @@ private: image_transport::Publisher rgbPub_; image_transport::Publisher depthPub_; + /// Replace the "compressed" image_transport plugin of rgbPub_, and of depthPub_ with + /// "stereo" (right image), so that images already compressed in the RGBDImage are + /// republished without being decompressed. Null if "compressed_passthrough" is false. + rclcpp::Publisher::SharedPtr compressedRgbPub_; + rclcpp::Publisher::SharedPtr compressedRightPub_; + /// Format used when an image has to be compressed: "jpeg" or "png". + std::string compressedImageFormat_; + /// Same for the "compressedDepth" plugin of depthPub_, without "stereo". Null if + /// "compressed_depth_passthrough" is false. + rclcpp::Publisher::SharedPtr compressedDepthPub_; + /// Format used when depth has to be compressed (raw input, or compressed in a format + /// that compressed_depth_image_transport cannot read), same parameters as the plugin. + std::string compressedDepthFormat_; + double compressedDepthMax_; + double compressedDepthQuantization_; + + /// Publishes the image (color, left or right) of @p raw or @p compressed on @p rawPub + /// and on @p compressedPub, without decompressing it when possible. + void publishImage( + const sensor_msgs::msg::Image & raw, + const sensor_msgs::msg::CompressedImage & compressed, + const std_msgs::msg::Header & defaultHeader, + const rtabmap_msgs::msg::RGBDImage::SharedPtr & input, + const image_transport::Publisher & rawPub, + const rclcpp::Publisher::SharedPtr & compressedPub, + sensor_msgs::msg::Image & outputImage) const; rclcpp::Publisher::SharedPtr rgbInfoPub_; rclcpp::Publisher::SharedPtr depthInfoPub_; }; diff --git a/rtabmap_util/package.xml b/rtabmap_util/package.xml index 964485e0..a8dee302 100644 --- a/rtabmap_util/package.xml +++ b/rtabmap_util/package.xml @@ -37,6 +37,11 @@ grid_map_ros ament_cmake_gtest + ament_index_cpp + class_loader + + compressed_image_transport + compressed_depth_image_transport ament_cmake diff --git a/rtabmap_util/src/nodelets/rgbd_relay.cpp b/rtabmap_util/src/nodelets/rgbd_relay.cpp index d3bcb301..45d025f6 100644 --- a/rtabmap_util/src/nodelets/rgbd_relay.cpp +++ b/rtabmap_util/src/nodelets/rgbd_relay.cpp @@ -51,7 +51,8 @@ namespace rtabmap_util RGBDRelay::RGBDRelay(const rclcpp::NodeOptions & options) : Node("rgbd_relay", options), compress_(false), - uncompress_(false) + uncompress_(false), + depthCompressionFormat_(".png") { int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; qos = this->declare_parameter("qos", qos); @@ -63,6 +64,19 @@ RGBDRelay::RGBDRelay(const rclcpp::NodeOptions & options) : int queuePub = this->declare_parameter("queue_pub", 1); compress_ = this->declare_parameter("compress", compress_); uncompress_ = this->declare_parameter("uncompress", uncompress_); + imageCompressionFormat_ = this->declare_parameter("image_compression_format", imageCompressionFormat_); + if(!rtabmap_conversions::isValidImageCompressionFormat(imageCompressionFormat_)) + { + RCLCPP_ERROR(this->get_logger(), "Invalid image_compression_format \"%s\" (should be \".jpg\" or \".png\"), using \".jpg\".", imageCompressionFormat_.c_str()); + imageCompressionFormat_ = ".jpg"; + } + depthCompressionFormat_ = this->declare_parameter("depth_compression_format", depthCompressionFormat_); + if(!rtabmap_conversions::isValidDepthCompressionFormat(depthCompressionFormat_)) + { + RCLCPP_ERROR(this->get_logger(), "Invalid depth_compression_format \"%s\" (should be \".png\" or \".rvl\", " + "optionally followed by \":maxDepth[:quantization]\", optionally prefixed by \"legacy:\", or \"legacy\"), using \".png\".", depthCompressionFormat_.c_str()); + depthCompressionFormat_ = ".png"; + } UASSERT_MSG(queueSub >= 1 && queuePub >= 1, uFormat("queue_sub (%d) and queue_pub (%d) must be at least 1", queueSub, queuePub).c_str()); @@ -103,7 +117,7 @@ void RGBDRelay::callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) co else if(!input->rgb.data.empty()) { cv_bridge::CvImageConstPtr rgb = cv_bridge::toCvShare(input->rgb, input); - rgb->toCompressedImageMsg(output->rgb_compressed, cv_bridge::JPG); + rtabmap_conversions::toCompressedImageMsg(*rgb, imageCompressionFormat_, output->rgb_compressed); } if(!input->depth_compressed.data.empty()) @@ -117,14 +131,14 @@ void RGBDRelay::callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) co { // right stereo image cv_bridge::CvImageConstPtr imageRightPtr = cv_bridge::toCvShare(input->depth, input); - imageRightPtr->toCompressedImageMsg(output->depth_compressed, cv_bridge::JPG); + rtabmap_conversions::toCompressedImageMsg(*imageRightPtr, imageCompressionFormat_, output->depth_compressed); } else { // depth image cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(input->depth, input); - output->depth_compressed.data = rtabmap::compressImage(imageDepthPtr->image, ".png"); - output->depth_compressed.format = "png"; + output->depth_compressed.header = input->depth.header; + rtabmap_conversions::compressDepthImage(imageDepthPtr->image, depthCompressionFormat_, output->depth_compressed); } } } @@ -147,34 +161,10 @@ void RGBDRelay::callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) co } else if(!input->depth_compressed.data.empty()) { - // Decode first, then pick the encoding from what actually came out. - // Branching on the "jpg"/"png" format string instead would abort on a - // right image compressed as PNG, which nothing forbids. - auto cvImg = std::make_unique(); - cvImg->header = input->depth_compressed.header; - cvImg->image = rtabmap::uncompressImage(input->depth_compressed.data); - if(cvImg->image.empty()) + cv_bridge::CvImagePtr cvImg = rtabmap_conversions::uncompressDepthImage(input->depth_compressed); + if(!cvImg->image.empty()) { - RCLCPP_ERROR(this->get_logger(), "Could not decompress the depth/right image of \"%s\" (format=\"%s\").", - rgbdImageSub_->get_topic_name(), input->depth_compressed.format.c_str()); - } - else - { - switch(cvImg->image.type()) - { - case CV_32FC1: cvImg->encoding = sensor_msgs::image_encodings::TYPE_32FC1; break; - case CV_16UC1: cvImg->encoding = sensor_msgs::image_encodings::TYPE_16UC1; break; - case CV_8UC1: cvImg->encoding = sensor_msgs::image_encodings::MONO8; break; - case CV_8UC3: cvImg->encoding = sensor_msgs::image_encodings::BGR8; break; - default: - RCLCPP_ERROR(this->get_logger(), "Unsupported decompressed depth/right image type %d.", cvImg->image.type()); - cvImg->image = cv::Mat(); - break; - } - if(!cvImg->image.empty()) - { - cvImg->toImageMsg(output->depth); - } + cvImg->toImageMsg(output->depth); } } } diff --git a/rtabmap_util/src/nodelets/rgbd_split.cpp b/rtabmap_util/src/nodelets/rgbd_split.cpp index 7d3ad63e..8d3bb288 100644 --- a/rtabmap_util/src/nodelets/rgbd_split.cpp +++ b/rtabmap_util/src/nodelets/rgbd_split.cpp @@ -27,9 +27,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include #include #include #include +#include +#include #ifdef PRE_ROS_IRON #include @@ -42,7 +45,11 @@ namespace rtabmap_util RGBDSplit::RGBDSplit(const rclcpp::NodeOptions & options) : Node("rgbd_split", options), - stereo_(false) + stereo_(false), + compressedImageFormat_("jpeg"), + compressedDepthFormat_("png"), + compressedDepthMax_(10.0), + compressedDepthQuantization_(100.0) { int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; qos = this->declare_parameter("qos", qos); @@ -55,11 +62,18 @@ RGBDSplit::RGBDSplit(const rclcpp::NodeOptions & options) : // A stereo RGBDImage carries the right image in the depth slot, so name the outputs // left/right instead of rgb/depth to say what they really are. stereo_ = this->declare_parameter("stereo", false); + // Republish compressed images on the compressed and compressedDepth topics without + // decompressing them. Depth can be set apart, e.g., to use the jpeg_quality parameter + // of the compressed plugin while still passing the depth through. + bool compressedPassthrough = this->declare_parameter("compressed_passthrough", true); + bool compressedDepthPassthrough = this->declare_parameter("compressed_depth_passthrough", compressedPassthrough); RCLCPP_INFO(this->get_logger(), "%s: qos = %d", get_name(), qos); RCLCPP_INFO(this->get_logger(), "%s: queue_sub = %d", get_name(), queueSub); RCLCPP_INFO(this->get_logger(), "%s: queue_pub = %d", get_name(), queuePub); RCLCPP_INFO(this->get_logger(), "%s: stereo = %s", get_name(), stereo_?"true":"false"); + RCLCPP_INFO(this->get_logger(), "%s: compressed_passthrough = %s", get_name(), compressedPassthrough?"true":"false"); + RCLCPP_INFO(this->get_logger(), "%s: compressed_depth_passthrough = %s", get_name(), compressedDepthPassthrough?"true":"false"); UASSERT_MSG(queueSub >= 1 && queuePub >= 1, uFormat("queue_sub (%d) and queue_pub (%d) must be at least 1", queueSub, queuePub).c_str()); @@ -71,12 +85,107 @@ RGBDSplit::RGBDSplit(const rclcpp::NodeOptions & options) : const std::string secondName = stereo_?"/right":"/depth"; const rclcpp::QoS pubQos = rclcpp::QoS(queuePub).reliability((rmw_qos_reliability_policy_t)qosPub); + const std::string rgbTopic = base + firstName + "/image"; + const std::string depthTopic = base + secondName + "/image"; + + // Parameter namespace image_transport gives to a topic (see + // image_transport::Publisher), e.g., "rgbd_image.depth.image". + const auto paramBaseOf = [this](const std::string & topic) { + std::string paramBase = topic.substr(std::min(topic.size(), this->get_effective_namespace().size())); + std::replace(paramBase.begin(), paramBase.end(), '/', '.'); + if(!paramBase.empty() && paramBase.front() == '.') + { + paramBase = paramBase.substr(1); + } + return paramBase; + }; + // Disables the image_transport @p transport plugin of @p topic, as we publish on its + // topic instead. image_transport reads the list of plugins from this parameter when + // the publisher is created: the plugin is removed from it even if set by the user, + // otherwise both would publish on the same topic. + const auto removePlugin = [this, ¶mBaseOf](const std::string & topic, const std::string & transport, const std::string & passthroughParam) { + const std::string name = paramBaseOf(topic) + ".enable_pub_plugins"; + std::vector plugins; + for(const std::string & loadable : image_transport::getLoadableTransports()) + { + if(loadable != transport) + { + plugins.push_back(loadable); + } + } + plugins = this->declare_parameter(name, plugins); + const auto plugin = std::find(plugins.begin(), plugins.end(), transport); + if(plugin != plugins.end()) + { + RCLCPP_WARN(this->get_logger(), "%s: %s is removed from %s, as %s is true: the node " + "publishes that topic itself. Set %s to false to use the plugin.", + get_name(), transport.c_str(), name.c_str(), passthroughParam.c_str(), passthroughParam.c_str()); + plugins.erase(plugin); + this->set_parameter(rclcpp::Parameter(name, plugins)); + } + }; + + if(compressedPassthrough) + { + const std::string kCompressed = "image_transport/compressed"; + // Same parameter as compressed_image_transport's publisher ("jpeg_quality" and + // "png_level" are not supported), read from the color topic. + const std::string cBase = paramBaseOf(rgbTopic) + ".compressed."; + compressedImageFormat_ = this->declare_parameter(cBase + "format", compressedImageFormat_); + if(compressedImageFormat_ != "jpeg" && compressedImageFormat_ != "png") + { + RCLCPP_ERROR(this->get_logger(), "%sformat should be \"jpeg\" or \"png\" (\"%s\"), using \"jpeg\".", + cBase.c_str(), compressedImageFormat_.c_str()); + compressedImageFormat_ = "jpeg"; + } + removePlugin(rgbTopic, kCompressed, "compressed_passthrough"); + compressedRgbPub_ = this->create_publisher(rgbTopic + "/compressed", pubQos); + if(stereo_) + { + removePlugin(depthTopic, kCompressed, "compressed_passthrough"); + compressedRightPub_ = this->create_publisher(depthTopic + "/compressed", pubQos); + } + } + + if(!stereo_ && compressedDepthPassthrough) + { + removePlugin(depthTopic, "image_transport/compressedDepth", "compressed_depth_passthrough"); + // Same parameters as compressed_depth_image_transport's publisher + // ("png_level" is not supported). + const std::string cdBase = paramBaseOf(depthTopic) + ".compressedDepth."; + compressedDepthFormat_ = this->declare_parameter(cdBase + "format", compressedDepthFormat_); + compressedDepthMax_ = this->declare_parameter(cdBase + "depth_max", compressedDepthMax_); + compressedDepthQuantization_ = this->declare_parameter(cdBase + "depth_quantization", compressedDepthQuantization_); +#ifdef PRE_ROS_JAZZY + if(compressedDepthFormat_ == "rvl") + { + RCLCPP_ERROR(this->get_logger(), "%sformat \"rvl\" cannot be decoded by compressed_depth_image_transport " + "before ROS Jazzy, using \"png\".", cdBase.c_str()); + compressedDepthFormat_ = "png"; + } +#endif + if(compressedDepthFormat_ != "png" && compressedDepthFormat_ != "rvl") + { + RCLCPP_ERROR(this->get_logger(), "%sformat should be \"png\" or \"rvl\" (\"%s\"), using \"png\".", + cdBase.c_str(), compressedDepthFormat_.c_str()); + compressedDepthFormat_ = "png"; + } + if(compressedDepthMax_ <= 0.0 || compressedDepthQuantization_ <= 0.0) + { + RCLCPP_ERROR(this->get_logger(), "%sdepth_max (%f) and %sdepth_quantization (%f) should be positive, using 10 and 100.", + cdBase.c_str(), compressedDepthMax_, cdBase.c_str(), compressedDepthQuantization_); + compressedDepthMax_ = 10.0; + compressedDepthQuantization_ = 100.0; + } + compressedDepthPub_ = this->create_publisher(depthTopic + "/compressedDepth", pubQos); + } + #ifdef PRE_ROS_LYRICAL - rgbPub_ = image_transport::create_publisher(this, base + firstName + "/image", pubQos.get_rmw_qos_profile()); - depthPub_ = image_transport::create_publisher(this, base + secondName + "/image", pubQos.get_rmw_qos_profile()); + rgbPub_ = image_transport::create_publisher(this, rgbTopic, pubQos.get_rmw_qos_profile()); + depthPub_ = image_transport::create_publisher(this, depthTopic, pubQos.get_rmw_qos_profile()); #else - rgbPub_ = image_transport::create_publisher(*this, base + firstName + "/image", pubQos); - depthPub_ = image_transport::create_publisher(*this, base + secondName + "/image", pubQos); + rgbPub_ = image_transport::create_publisher(*this, rgbTopic, pubQos); + depthPub_ = image_transport::create_publisher(*this, depthTopic, pubQos); #endif rgbInfoPub_ = this->create_publisher(base + firstName + "/camera_info", pubQos); depthInfoPub_ = this->create_publisher(base + secondName + "/camera_info", pubQos); @@ -93,79 +202,173 @@ RGBDSplit::RGBDSplit(const rclcpp::NodeOptions & options) : } -void RGBDSplit::callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) const +void RGBDSplit::publishImage( + const sensor_msgs::msg::Image & raw, + const sensor_msgs::msg::CompressedImage & compressed, + const std_msgs::msg::Header & defaultHeader, + const rtabmap_msgs::msg::RGBDImage::SharedPtr & input, + const image_transport::Publisher & rawPub, + const rclcpp::Publisher::SharedPtr & compressedPub, + sensor_msgs::msg::Image & outputImage) const { - if(rgbPub_.getNumSubscribers()) - { - sensor_msgs::msg::Image outputImage; - sensor_msgs::msg::CameraInfo outputCameraInfo; - outputImage.header = outputCameraInfo.header = input->header; - outputCameraInfo = input->rgb_camera_info; + const bool rawSubscribed = rawPub.getNumSubscribers() > 0; + const bool compressedSubscribed = compressedPub && compressedPub->get_subscription_count() > 0; - if(!input->rgb.data.empty()) + // compressed without decompression, when compressed_image_transport can read it + sensor_msgs::msg::CompressedImage outputCompressed; + bool compressedReady = false; + if(compressedSubscribed && raw.data.empty() && !compressed.data.empty()) + { + const std::string format = rtabmap_conversions::compressedImageTransportFormat(compressed.data); + if(!format.empty()) + { + outputCompressed = compressed; + outputCompressed.format = format; // with the encoding, which older producers did not set + compressedReady = true; + } + } + + outputImage.header = defaultHeader; + cv_bridge::CvImageConstPtr image; + if(!raw.data.empty()) + { + if(rawSubscribed) { // already raw, just copy pointer - outputImage = input->rgb; + outputImage = raw; } - else if(!input->rgb_compressed.data.empty()) + else { -#ifdef CV_BRIDGE_HYDRO - ROS_ERROR("Unsupported compressed image copy, please upgrade at least to ROS Indigo to use this."); -#else - cv_bridge::toCvCopy(input->rgb_compressed)->toImageMsg(outputImage); -#endif + outputImage.header = raw.header; } - rgbPub_.publish(outputImage); + if(compressedSubscribed) + { + image = cv_bridge::toCvShare(raw, input); + } + } + else if(!compressed.data.empty() && (rawSubscribed || !compressedReady)) + { + cv_bridge::CvImagePtr decoded = cv_bridge::toCvCopy(compressed); + decoded->toImageMsg(outputImage); + image = decoded; + } + else if(!compressed.data.empty()) + { + outputImage.header = compressed.header; + } + if(outputImage.header.frame_id.empty()) + { + outputImage.header = defaultHeader; + } + + if(rawSubscribed) + { + rawPub.publish(outputImage); + } + if(compressedSubscribed) + { + if(!compressedReady && image) + { + compressedReady = rtabmap_conversions::toCompressedImageMsg(*image, compressedImageFormat_, outputCompressed); + } + if(compressedReady) + { + outputCompressed.header = outputImage.header; + compressedPub->publish(outputCompressed); + } + } +} + +void RGBDSplit::callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) const +{ + if(rgbPub_.getNumSubscribers() || (compressedRgbPub_ && compressedRgbPub_->get_subscription_count())) + { + sensor_msgs::msg::Image outputImage; + const sensor_msgs::msg::CameraInfo outputCameraInfo = input->rgb_camera_info; + publishImage(input->rgb, input->rgb_compressed, input->header, input, rgbPub_, compressedRgbPub_, outputImage); rgbInfoPub_->publish(outputCameraInfo); } - if(depthPub_.getNumSubscribers()) + const bool rawDepthSubscribed = depthPub_.getNumSubscribers() > 0; + const bool compressedDepthSubscribed = compressedDepthPub_ && compressedDepthPub_->get_subscription_count() > 0; + const bool compressedRightSubscribed = compressedRightPub_ && compressedRightPub_->get_subscription_count() > 0; + if(rawDepthSubscribed || compressedDepthSubscribed || compressedRightSubscribed) { sensor_msgs::msg::Image outputImage; sensor_msgs::msg::CameraInfo outputCameraInfo; outputCameraInfo = input->depth_camera_info; - if(!input->depth.data.empty()) + cv::Mat depth; + sensor_msgs::msg::CompressedImage outputCompressedDepth; + bool compressedDepthReady = false; + if(compressedRightPub_) { - // already raw, just copy pointer - outputImage = input->depth; + // Right image of a stereo pair + publishImage(input->depth, input->depth_compressed, input->header, input, depthPub_, compressedRightPub_, outputImage); } - else if(!input->depth_compressed.data.empty()) + else { -#ifdef CV_BRIDGE_HYDRO - ROS_ERROR("Unsupported compressed image copy, please upgrade at least to ROS Indigo to use this."); -#else - // Decode first, then pick the encoding from what actually came out. Going - // by the "jpg"/"png" format string instead would mislabel a depth PNG as - // mono8 (cv_bridge cannot infer 16-bit from it), and would abort outright on - // a right image compressed as PNG, which nothing forbids. - cv_bridge::CvImage cvImg; - cvImg.header = input->depth_compressed.header; - cvImg.image = rtabmap::uncompressImage(input->depth_compressed.data); - if(cvImg.image.empty()) + // compressedDepth without decompression, when the depth is already compressed + // in a format compressed_depth_image_transport can read. + if(compressedDepthSubscribed && input->depth.data.empty() && !input->depth_compressed.data.empty()) { - RCLCPP_ERROR(this->get_logger(), "Could not decompress the depth/right image of \"%s\" (format=\"%s\").", - rgbdImageSub_->get_topic_name(), input->depth_compressed.format.c_str()); + const bool rosFormat = input->depth_compressed.format.find("compressedDepth") != std::string::npos; +#ifdef PRE_ROS_JAZZY + const bool rvlSupported = false; +#else + const bool rvlSupported = true; +#endif + if(rosFormat && (rvlSupported || input->depth_compressed.format.find(" rvl") == std::string::npos)) + { + outputCompressedDepth = input->depth_compressed; + compressedDepthReady = true; + } + else + { + // RVL is re-compressed as PNG before Jazzy. Legacy 32FC1 format (4 channels + // PNG) and right images have no compressedDepth equivalent, they are + // re-compressed below. + const cv::Mat bytes = rosFormat ? + rtabmap_conversions::compressedDepthTransportToRtabmap(input->depth_compressed) : + rtabmap_conversions::compressedMatFromBytes(input->depth_compressed.data, false); + outputCompressedDepth.header = input->depth_compressed.header; + compressedDepthReady = rtabmap_conversions::rtabmapToCompressedDepthTransport(bytes, outputCompressedDepth, false); + } + } + + if(!input->depth.data.empty()) + { + if(rawDepthSubscribed) + { + // already raw, just copy pointer + outputImage = input->depth; + } + else + { + outputImage.header = input->depth.header; + } + if(compressedDepthSubscribed) + { + depth = cv_bridge::toCvShare(input->depth, input)->image; + } + } + else if(!input->depth_compressed.data.empty() && (rawDepthSubscribed || !compressedDepthReady)) + { +#ifdef CV_BRIDGE_HYDRO + ROS_ERROR("Unsupported compressed image copy, please upgrade at least to ROS Indigo to use this."); +#else + cv_bridge::CvImagePtr cvImg = rtabmap_conversions::uncompressDepthImage(input->depth_compressed); + if(!cvImg->image.empty()) + { + cvImg->toImageMsg(outputImage); + depth = cvImg->image; + } +#endif } else { - switch(cvImg.image.type()) - { - case CV_32FC1: cvImg.encoding = sensor_msgs::image_encodings::TYPE_32FC1; break; - case CV_16UC1: cvImg.encoding = sensor_msgs::image_encodings::TYPE_16UC1; break; - case CV_8UC1: cvImg.encoding = sensor_msgs::image_encodings::MONO8; break; - case CV_8UC3: cvImg.encoding = sensor_msgs::image_encodings::BGR8; break; - default: - RCLCPP_ERROR(this->get_logger(), "Unsupported decompressed depth/right image type %d.", cvImg.image.type()); - cvImg.image = cv::Mat(); - break; - } + outputImage.header = input->depth_compressed.header; } - if(!cvImg.image.empty()) - { - cvImg.toImageMsg(outputImage); - } -#endif } if(outputCameraInfo.header.frame_id.empty()) { if(outputImage.header.frame_id.empty()) { @@ -214,7 +417,35 @@ void RGBDSplit::callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) co } } - depthPub_.publish(outputImage); + if(!compressedRightPub_) // else already published + { + if(rawDepthSubscribed) + { + depthPub_.publish(outputImage); + } + if(compressedDepthSubscribed) + { + if(!compressedDepthReady && (depth.type() == CV_32FC1 || depth.type() == CV_16UC1)) + { + const std::string format = depth.type() == CV_32FC1 ? + uFormat(".%s:%g:%g", compressedDepthFormat_.c_str(), compressedDepthMax_, compressedDepthQuantization_) : + "." + compressedDepthFormat_; + // Same as compressed_depth_image_transport: inverse depth for 32FC1 + compressedDepthReady = rtabmap_conversions::compressDepthImage(depth, format, outputCompressedDepth); + } + else if(!compressedDepthReady && !depth.empty()) + { + RCLCPP_WARN_ONCE(this->get_logger(), "Cannot publish \"%s\" as compressedDepth, it is " + "not a depth image (type=%d). (This warning is printed only once)", + compressedDepthPub_->get_topic_name(), depth.type()); + } + if(compressedDepthReady) + { + outputCompressedDepth.header = outputImage.header; + compressedDepthPub_->publish(outputCompressedDepth); + } + } + } depthInfoPub_->publish(outputCameraInfo); } } diff --git a/rtabmap_util/test/test_rgbd_pipeline.cpp b/rtabmap_util/test/test_rgbd_pipeline.cpp new file mode 100644 index 00000000..11914fc4 --- /dev/null +++ b/rtabmap_util/test/test_rgbd_pipeline.cpp @@ -0,0 +1,417 @@ +/* +Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke +All rights reserved. (BSD-3-Clause, see the repository root.) +*/ + +/** + * Round trip of raw images through rgbd_sync (compressed output) and rgbd_split, read + * back through image_transport (raw, and the "compressed" and "compressedDepth" plugins), + * compared to the original images, for each image and depth compression format. Same + * for a stereo pair through stereo_sync and rgbd_split ("stereo"). + */ + +#include "node_test_utils.hpp" +#include "msg_builders.hpp" + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +using namespace rtabmap_util_test; + +namespace { +::testing::Environment * const kEnv = registerRclcppEnvironment(); + +constexpr int kWidth = 64; +constexpr int kHeight = 48; + +struct PipelineCase +{ + std::string name; + int depthType; ///< CV_16UC1 (mm) or CV_32FC1 (m) + std::string imageFormat; ///< rgbd_sync's image_compression_format + std::string depthFormat; ///< rgbd_sync's depth_compression_format +}; + +/** + * Creates a node registered as a component (RCLCPP_COMPONENTS_REGISTER_NODE) by another + * package, the way component_container does: rtabmap_sync does not install the headers + * of its nodes. + */ +rclcpp::Node::SharedPtr loadComponent( + const std::string & package, const std::string & className, const rclcpp::NodeOptions & options) +{ + std::string content, basePath; +#ifdef PRE_ROS_LYRICAL + if(!ament_index_cpp::get_resource("rclcpp_components", package, content, &basePath)) + { + return nullptr; + } +#else + // The overload above is deprecated in Lyrical and removed in Rolling + const ament_index_cpp::PathWithResource resource = ament_index_cpp::get_resource("rclcpp_components", package); + if(!resource.resourcePath) + { + return nullptr; + } + content = resource.contents; + basePath = resource.resourcePath->string(); +#endif + std::istringstream lines(content); + std::string line; + while(std::getline(lines, line)) + { + const size_t split = line.find(';'); + if(split == std::string::npos || line.substr(0, split) != className) + { + continue; + } + // Never unloaded: the node may outlive any owner of the loader in the test. + static std::vector loaders; + loaders.push_back(new class_loader::ClassLoader(basePath + "/" + line.substr(split + 1))); + std::shared_ptr factory = + loaders.back()->createInstance( + "rclcpp_components::NodeFactoryTemplate<" + className + ">"); + rclcpp_components::NodeInstanceWrapper wrapper = factory->create_node_instance(options); + // The component class derives from rclcpp::Node only, at the start of the object. + return std::static_pointer_cast(wrapper.get_node_instance()); + } + return nullptr; +} + +/// A smooth color gradient, so that JPEG stays close to it. +cv::Mat colorImage() +{ + cv::Mat image(kHeight, kWidth, CV_8UC3); + for(int r = 0; r < kHeight; ++r) + { + for(int c = 0; c < kWidth; ++c) + { + image.at(r, c) = cv::Vec3b(uchar(4*c), uchar(5*r), uchar(2*(r+c))); + } + } + return image; +} + +/// A depth ramp from 0.5 to 8 m, with invalid (0) pixels on the first row. +cv::Mat depthImage(int type) +{ + cv::Mat depth(kHeight, kWidth, type); + for(int r = 0; r < kHeight; ++r) + { + for(int c = 0; c < kWidth; ++c) + { + const float meters = r == 0 && c < 8 ? 0.0f : + 0.5f + 7.5f * float(r * kWidth + c) / float(kWidth * kHeight); + if(type == CV_16UC1) + { + depth.at(r, c) = uint16_t(meters * 1000.0f + 0.5f); + } + else + { + depth.at(r, c) = meters; + } + } + } + return depth; +} + +/// Depth in meters at (r, c), 0 when invalid (0 or NaN). +float meters(const cv::Mat & depth, int r, int c) +{ + const float d = depth.type() == CV_16UC1 ? depth.at(r, c) * 0.001f : depth.at(r, c); + return std::isfinite(d) ? d : 0.0f; +} + +class RGBDPipelineTest : public NodeTest, public ::testing::WithParamInterface +{ +protected: + using ImagePtr = sensor_msgs::msg::Image::ConstSharedPtr; + + image_transport::Subscriber subscribe(const std::string & topic, const std::string & transport, std::vector & out) + { + const auto callback = [&out](const ImagePtr & msg) { out.push_back(msg); }; +#ifdef PRE_ROS_LYRICAL + return image_transport::create_subscription(helper().get(), topic, callback, transport); +#else + return image_transport::create_subscription(*helper(), topic, callback, transport, rclcpp::QoS(10), rclcpp::SubscriptionOptions()); +#endif + } + + /// What reaches the consumer of rgbd_split for the original images. + void run(const PipelineCase & cs) + { + rclcpp::Node::SharedPtr sync = loadComponent("rtabmap_sync", "rtabmap_sync::RGBDSync", + rclcpp::NodeOptions().parameter_overrides({ + rclcpp::Parameter("approx_sync", false), + rclcpp::Parameter("image_compression_format", cs.imageFormat), + rclcpp::Parameter("depth_compression_format", cs.depthFormat)})); + ASSERT_TRUE(sync) << "rtabmap_sync::RGBDSync component not found"; + addNode(sync); + addNode(std::make_shared(rclcpp::NodeOptions() + .arguments({"--ros-args", "-r", "rgbd_image:=rgbd_image/compressed"}))); + + const std::string rgbTopic = "rgbd_image/compressed/rgb/image"; + const std::string depthTopic = "rgbd_image/compressed/depth/image"; + image_transport::Subscriber subs[] = { + subscribe(rgbTopic, "raw", rgbRaw_), + subscribe(rgbTopic, "compressed", rgbCompressed_), + subscribe(depthTopic, "raw", depthRaw_), + subscribe(depthTopic, "compressedDepth", depthCompressed_)}; + + rclcpp::Publisher::SharedPtr rgbPub = + helper()->create_publisher("rgb/image", 10); + rclcpp::Publisher::SharedPtr depthPub = + helper()->create_publisher("depth/image", 10); + rclcpp::Publisher::SharedPtr infoPub = + helper()->create_publisher("rgb/camera_info", 10); + ASSERT_TRUE(waitForSubscriber(rgbPub)); + ASSERT_TRUE(waitForSubscriber(depthPub)); + ASSERT_TRUE(waitForSubscriber(infoPub)); + for(const image_transport::Subscriber & sub : subs) + { + ASSERT_TRUE(spinUntil([&]() { return sub.getNumPublishers() > 0; })) << sub.getTopic(); + } + + color_ = colorImage(); + depth_ = depthImage(cs.depthType); + rgbPub->publish(makeImage("camera_link", 1000.0, color_, sensor_msgs::image_encodings::BGR8)); + depthPub->publish(makeImage("camera_link", 1000.0, depth_, + cs.depthType == CV_16UC1 ? sensor_msgs::image_encodings::TYPE_16UC1 : sensor_msgs::image_encodings::TYPE_32FC1)); + infoPub->publish(makeCameraInfo("camera_link", 1000.0, kWidth, kHeight)); + + ASSERT_TRUE(spinUntil([&]() { + return !rgbRaw_.empty() && !rgbCompressed_.empty() && !depthRaw_.empty() && !depthCompressed_.empty(); + })); + } + + /// The color image read back is the original, exactly in PNG, closely in JPEG. + void expectColor(const ImagePtr & msg, const std::string & imageFormat) + { + SCOPED_TRACE(msg->encoding); + const cv::Mat image = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8)->image; + ASSERT_EQ(image.size(), color_.size()); + if(imageFormat == ".png") + { + EXPECT_EQ(cv::norm(image, color_, cv::NORM_INF), 0.0) << "lossless"; + } + else + { + EXPECT_LT(cv::norm(image, color_, cv::NORM_L1) / double(image.total() * image.channels()), 3.0) + << "mean error of JPEG"; + } + } + + /// The depth image read back is the original within @p tolerance(meters), with invalid + /// pixels (0 or NaN) where the original is invalid, and in @p type. + template + void expectDepth(const ImagePtr & msg, int type, Tolerance tolerance) + { + SCOPED_TRACE(msg->encoding); + const cv::Mat depth = cv_bridge::toCvCopy(msg)->image; + ASSERT_EQ(depth.type(), type); + ASSERT_EQ(depth.size(), depth_.size()); + for(int r = 0; r < kHeight; ++r) + { + for(int c = 0; c < kWidth; ++c) + { + const float expected = meters(depth_, r, c); + const float got = meters(depth, r, c); + if(expected == 0.0f) + { + ASSERT_EQ(got, 0.0f) << "invalid at " << r << "," << c; + } + else + { + ASSERT_NEAR(got, expected, tolerance(expected)) << "at " << r << "," << c; + } + } + } + } + + cv::Mat color_; + cv::Mat depth_; + std::vector rgbRaw_; + std::vector rgbCompressed_; + std::vector depthRaw_; + std::vector depthCompressed_; +}; + +} // namespace + +TEST_P(RGBDPipelineTest, ImagesReadBackAsSent) +{ + const PipelineCase & cs = GetParam(); + ASSERT_NO_FATAL_FAILURE(run(cs)); + + expectColor(rgbRaw_.back(), cs.imageFormat); + expectColor(rgbCompressed_.back(), cs.imageFormat); + + const auto exact = [](float) { return 1e-6f; }; + const auto millimeters = [](float) { return 0.0005f + 1e-5f; }; + // Inverse depth quantization: half a step, or a whole one when truncated (the + // compressed_depth_image_transport plugin, used by rgbd_split to re-compress) + const auto inverseDepth = [](float d) { return 1.02f * d * d / (100.0f * 101.0f) + 1e-6f; }; + + // "legacy:": rtabmap's own format, with or without inverse depth parameters + const bool legacy = cs.depthFormat.compare(0, 6, "legacy") == 0; + const std::string format = !legacy ? cs.depthFormat : cs.depthFormat == "legacy" ? ".png" : cs.depthFormat.substr(7); + const bool inverse = format.find(':') != std::string::npos; + if(cs.depthType == CV_16UC1) + { + // Always lossless + expectDepth(depthRaw_.back(), CV_16UC1, exact); + expectDepth(depthCompressed_.back(), CV_16UC1, exact); + } + else if(inverse) + { + expectDepth(depthRaw_.back(), CV_32FC1, inverseDepth); + expectDepth(depthCompressed_.back(), CV_32FC1, inverseDepth); + } + else if(legacy) + { + // Lossless in rtabmap's legacy format, which rgbd_split has to compress again for + // compressedDepth, with its default inverse depth quantization (10 m, 100) + expectDepth(depthRaw_.back(), CV_32FC1, exact); + expectDepth(depthCompressed_.back(), CV_32FC1, inverseDepth); + } + else + { + // 32FC1 compressed in millimeters + expectDepth(depthRaw_.back(), CV_16UC1, millimeters); + expectDepth(depthCompressed_.back(), CV_16UC1, millimeters); + } +} + +INSTANTIATE_TEST_SUITE_P( + Formats, + RGBDPipelineTest, + ::testing::Values( + PipelineCase{"defaults_16UC1", CV_16UC1, ".jpg", ".png"}, + PipelineCase{"defaults_32FC1", CV_32FC1, ".jpg", ".png"}, + PipelineCase{"png_color", CV_16UC1, ".png", ".png"}, + PipelineCase{"rvl_16UC1", CV_16UC1, ".png", ".rvl"}, + PipelineCase{"rvl_32FC1", CV_32FC1, ".png", ".rvl"}, + PipelineCase{"inverse_depth_png", CV_32FC1, ".png", ".png:10:100"}, + PipelineCase{"inverse_depth_rvl", CV_32FC1, ".png", ".rvl:10:100"}, + PipelineCase{"legacy_16UC1", CV_16UC1, ".png", "legacy"}, + PipelineCase{"legacy_32FC1", CV_32FC1, ".png", "legacy"}, + PipelineCase{"legacy_rvl_16UC1", CV_16UC1, ".png", "legacy:.rvl"}, + PipelineCase{"legacy_rvl_32FC1", CV_32FC1, ".png", "legacy:.rvl"}, + PipelineCase{"legacy_inverse_depth", CV_32FC1, ".png", "legacy:.png:10:100"}), + [](const ::testing::TestParamInfo & info) { return info.param.name; }); + +namespace { + +/// The same round trip for a stereo pair: stereo_sync -> rgbd_split ("stereo"), with a +/// color left image and a gray right image. +class StereoPipelineTest : public NodeTest, public ::testing::WithParamInterface +{ +protected: + using ImagePtr = sensor_msgs::msg::Image::ConstSharedPtr; + + image_transport::Subscriber subscribe(const std::string & topic, const std::string & transport, std::vector & out) + { + const auto callback = [&out](const ImagePtr & msg) { out.push_back(msg); }; +#ifdef PRE_ROS_LYRICAL + return image_transport::create_subscription(helper().get(), topic, callback, transport); +#else + return image_transport::create_subscription(*helper(), topic, callback, transport, rclcpp::QoS(10), rclcpp::SubscriptionOptions()); +#endif + } + + /// @p msg is @p original, exactly in PNG, closely in JPEG. + static void expectImage(const ImagePtr & msg, const cv::Mat & original, const std::string & imageFormat) + { + SCOPED_TRACE(msg->encoding); + const cv::Mat image = cv_bridge::toCvCopy(msg)->image; + ASSERT_EQ(image.type(), original.type()); + ASSERT_EQ(image.size(), original.size()); + if(imageFormat == ".png") + { + EXPECT_EQ(cv::norm(image, original, cv::NORM_INF), 0.0) << "lossless"; + } + else + { + EXPECT_LT(cv::norm(image, original, cv::NORM_L1) / double(image.total() * image.channels()), 3.0) + << "mean error of JPEG"; + } + } +}; + +} // namespace + +TEST_P(StereoPipelineTest, ImagesReadBackAsSent) +{ + const std::string imageFormat = GetParam(); + rclcpp::Node::SharedPtr sync = loadComponent("rtabmap_sync", "rtabmap_sync::StereoSync", + rclcpp::NodeOptions().parameter_overrides({ + rclcpp::Parameter("approx_sync", false), + rclcpp::Parameter("image_compression_format", imageFormat)})); + ASSERT_TRUE(sync) << "rtabmap_sync::StereoSync component not found"; + addNode(sync); + addNode(std::make_shared(rclcpp::NodeOptions() + .arguments({"--ros-args", "-r", "rgbd_image:=rgbd_image/compressed"}) + .parameter_overrides({rclcpp::Parameter("stereo", true)}))); + + std::vector leftRaw, leftCompressed, rightRaw, rightCompressed; + const std::string leftTopic = "rgbd_image/compressed/left/image"; + const std::string rightTopic = "rgbd_image/compressed/right/image"; + image_transport::Subscriber subs[] = { + subscribe(leftTopic, "raw", leftRaw), + subscribe(leftTopic, "compressed", leftCompressed), + subscribe(rightTopic, "raw", rightRaw), + subscribe(rightTopic, "compressed", rightCompressed)}; + + rclcpp::Publisher::SharedPtr leftPub = + helper()->create_publisher("left/image_rect", 10); + rclcpp::Publisher::SharedPtr rightPub = + helper()->create_publisher("right/image_rect", 10); + rclcpp::Publisher::SharedPtr leftInfoPub = + helper()->create_publisher("left/camera_info", 10); + rclcpp::Publisher::SharedPtr rightInfoPub = + helper()->create_publisher("right/camera_info", 10); + ASSERT_TRUE(waitForSubscriber(leftPub)); + ASSERT_TRUE(waitForSubscriber(rightPub)); + ASSERT_TRUE(waitForSubscriber(leftInfoPub)); + ASSERT_TRUE(waitForSubscriber(rightInfoPub)); + for(const image_transport::Subscriber & sub : subs) + { + ASSERT_TRUE(spinUntil([&]() { return sub.getNumPublishers() > 0; })) << sub.getTopic(); + } + + const cv::Mat left = colorImage(); + cv::Mat right; + cv::cvtColor(left, right, cv::COLOR_BGR2GRAY); + cv::flip(right, right, 1); + leftPub->publish(makeImage("camera_link", 1000.0, left, sensor_msgs::image_encodings::BGR8)); + rightPub->publish(makeImage("camera_link", 1000.0, right, sensor_msgs::image_encodings::MONO8)); + leftInfoPub->publish(makeCameraInfo("camera_link", 1000.0, kWidth, kHeight)); + rightInfoPub->publish(makeCameraInfo("camera_link", 1000.0, kWidth, kHeight, -10.0)); // 10 cm baseline + + ASSERT_TRUE(spinUntil([&]() { + return !leftRaw.empty() && !leftCompressed.empty() && !rightRaw.empty() && !rightCompressed.empty(); + })); + expectImage(leftRaw.back(), left, imageFormat); + expectImage(leftCompressed.back(), left, imageFormat); + expectImage(rightRaw.back(), right, imageFormat); + expectImage(rightCompressed.back(), right, imageFormat); + EXPECT_EQ(rightCompressed.back()->encoding, "mono8"); +} + +INSTANTIATE_TEST_SUITE_P( + Formats, + StereoPipelineTest, + ::testing::Values(std::string(".jpg"), std::string(".png")), + [](const ::testing::TestParamInfo & info) { return info.param.substr(1); }); diff --git a/rtabmap_util/test/test_rgbd_relay.cpp b/rtabmap_util/test/test_rgbd_relay.cpp index 6489bef3..74cc39f1 100644 --- a/rtabmap_util/test/test_rgbd_relay.cpp +++ b/rtabmap_util/test/test_rgbd_relay.cpp @@ -9,6 +9,7 @@ All rights reserved. (BSD-3-Clause, see the repository root.) #include #include +#include #include using namespace rtabmap_util_test; @@ -21,12 +22,15 @@ class RGBDRelayTest : public NodeTest { protected: /// Starts the node, wires up the input publisher and the output collector. - void start(bool compress, bool uncompress) + void start(bool compress, bool uncompress, const std::string & depthCompressionFormat = ".png", + const std::string & imageCompressionFormat = ".jpg") { addNode(std::make_shared(rclcpp::NodeOptions() .parameter_overrides({ rclcpp::Parameter("compress", compress), - rclcpp::Parameter("uncompress", uncompress)}))); + rclcpp::Parameter("uncompress", uncompress), + rclcpp::Parameter("depth_compression_format", depthCompressionFormat), + rclcpp::Parameter("image_compression_format", imageCompressionFormat)}))); out_ = collect("rgbd_image_relay"); pub_ = helper()->create_publisher("rgbd_image", 10); @@ -65,8 +69,9 @@ TEST_F(RGBDRelayTest, CompressesRawImagesWhenAsked) const rtabmap_msgs::msg::RGBDImage & got = out_->back(); EXPECT_FALSE(got.rgb_compressed.data.empty()) << "rgb must be compressed"; EXPECT_FALSE(got.depth_compressed.data.empty()) << "depth must be compressed"; - // Depth is lossless png; color is jpg. - EXPECT_EQ(got.depth_compressed.format, "png"); + // Depth is lossless png in compressed_depth_image_transport's format; color is jpeg. + EXPECT_EQ(got.depth_compressed.format, "16UC1; compressedDepth png"); + EXPECT_EQ(got.rgb_compressed.format, "bgr8; jpeg compressed bgr8"); EXPECT_TRUE(got.rgb.data.empty()) << "the raw image is not carried as well"; } @@ -81,13 +86,26 @@ TEST_F(RGBDRelayTest, CompressesAStereoPairAsJpeg) const rtabmap_msgs::msg::RGBDImage & got = out_->back(); ASSERT_FALSE(got.depth_compressed.data.empty()); - EXPECT_NE(got.depth_compressed.format, "png") - << "a stereo right image must not take the depth PNG path"; + EXPECT_EQ(got.depth_compressed.format, "mono8; jpeg compressed mono8") + << "a stereo right image must not take the depth path"; EXPECT_NE(got.depth_compressed.format.find("jp"), std::string::npos) << "expected a jpeg format, got \"" << got.depth_compressed.format << "\""; EXPECT_LT(got.depth_camera_info.p[3], 0.0) << "the baseline must survive the relay"; } +TEST_F(RGBDRelayTest, ImageCompressionFormatAppliesToColorAndRightImages) +{ + start(/*compress=*/true, /*uncompress=*/false, ".png", ".png"); + + pub_->publish(makeRGBDImage("camera_link", 1000.0)); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + EXPECT_EQ(out_->back().rgb_compressed.format, "bgr8; png compressed bgr8"); + + pub_->publish(makeStereoRGBDImage("camera_link", 1001.0)); + ASSERT_TRUE(spinUntil([&]() { return out_->size() > 1; })); + EXPECT_EQ(out_->back().depth_compressed.format, "mono8; png compressed mono8") << "right image"; +} + TEST_F(RGBDRelayTest, CompressesDepthAsLosslessPng) { // The same call with no baseline is treated as color + depth instead. @@ -98,7 +116,7 @@ TEST_F(RGBDRelayTest, CompressesDepthAsLosslessPng) const rtabmap_msgs::msg::RGBDImage & got = out_->back(); ASSERT_FALSE(got.depth_compressed.data.empty()); - EXPECT_EQ(got.depth_compressed.format, "png") << "depth must stay lossless"; + EXPECT_EQ(got.depth_compressed.format, "16UC1; compressedDepth png") << "depth must stay lossless"; EXPECT_DOUBLE_EQ(got.depth_camera_info.p[3], 0.0) << "no baseline: not stereo"; } @@ -146,6 +164,76 @@ TEST_F(RGBDRelayTest, UncompressRestoresRawImages) EXPECT_EQ(got.depth.height, 8u); } +TEST_F(RGBDRelayTest, CompressesFloatDepthAsInverseDepthWhenAsked) +{ + start(/*compress=*/true, /*uncompress=*/false, ".rvl:10:100"); + + rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0); + const cv::Mat depth(8, 8, CV_32FC1, cv::Scalar(1.5f)); + cv_bridge::CvImage(in.header, sensor_msgs::image_encodings::TYPE_32FC1, depth).toImageMsg(in.depth); + + pub_->publish(in); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + const rtabmap_msgs::msg::RGBDImage & got = out_->back(); + ASSERT_FALSE(got.depth_compressed.data.empty()); + EXPECT_EQ(got.depth_compressed.format.rfind("32FC1; compressedDepth ", 0), 0u) << got.depth_compressed.format; + EXPECT_EQ(got.depth_compressed.header.frame_id, in.depth.header.frame_id); + const cv::Mat restored = rtabmap_conversions::uncompressDepthImage(got.depth_compressed)->image; + ASSERT_EQ(restored.type(), CV_32FC1); + EXPECT_LT(cv::norm(restored, depth, cv::NORM_INF), 0.001); +} + +TEST_F(RGBDRelayTest, InvalidDepthCompressionFormatFallsBackToPng) +{ + start(/*compress=*/true, /*uncompress=*/false, ".jpg:10"); + + pub_->publish(makeRGBDImage("camera_link", 1000.0)); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + EXPECT_EQ(out_->back().depth_compressed.format, "16UC1; compressedDepth png"); +} + +TEST_F(RGBDRelayTest, LegacyDepthCompressionKeepsFloatDepthLossless) +{ + start(/*compress=*/true, /*uncompress=*/false, "legacy"); + + rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0); + const cv::Mat depth(8, 8, CV_32FC1, cv::Scalar(1.2345f)); + cv_bridge::CvImage(in.header, sensor_msgs::image_encodings::TYPE_32FC1, depth).toImageMsg(in.depth); + + pub_->publish(in); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + const rtabmap_msgs::msg::RGBDImage & got = out_->back(); + EXPECT_EQ(got.depth_compressed.format, "png"); + const cv::Mat restored = rtabmap::uncompressImage(got.depth_compressed.data); + ASSERT_EQ(restored.type(), CV_32FC1); + EXPECT_EQ(cv::norm(restored, depth, cv::NORM_INF), 0.0) << "lossless"; +} + +TEST_F(RGBDRelayTest, UncompressRestoresRosCompressedDepth) +{ + // depth_compressed filled by compressed_depth_image_transport ("32FC1; compressedDepth png") + start(/*compress=*/false, /*uncompress=*/true); + + rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0); + const cv::Mat depth(8, 8, CV_32FC1, cv::Scalar(2.5f)); + in.depth = sensor_msgs::msg::Image(); + in.depth_compressed.header = in.header; + ASSERT_TRUE(rtabmap_conversions::rtabmapToCompressedDepthTransport( + rtabmap::compressImage2(depth, ".png:10:100"), in.depth_compressed)); + ASSERT_EQ(in.depth_compressed.format, "32FC1; compressedDepth png"); + + pub_->publish(in); + ASSERT_TRUE(spinUntil([&]() { return !out_->empty(); })); + + const rtabmap_msgs::msg::RGBDImage & got = out_->back(); + ASSERT_FALSE(got.depth.data.empty()) << "depth must be decompressed"; + EXPECT_EQ(got.depth.encoding, sensor_msgs::image_encodings::TYPE_32FC1); + const cv::Mat restored = cv_bridge::toCvCopy(got.depth)->image; + EXPECT_LT(cv::norm(restored, depth, cv::NORM_INF), 0.001); +} + TEST_F(RGBDRelayTest, UncompressPrefersTheRawImageOverTheCompressedOne) { // A message may carry both. The raw image is already usable, so decompressing the diff --git a/rtabmap_util/test/test_rgbd_split.cpp b/rtabmap_util/test/test_rgbd_split.cpp index f032d998..3783f13b 100644 --- a/rtabmap_util/test/test_rgbd_split.cpp +++ b/rtabmap_util/test/test_rgbd_split.cpp @@ -8,7 +8,11 @@ All rights reserved. (BSD-3-Clause, see the repository root.) #include +#include +#include + #include +#include #include using namespace rtabmap_util_test; @@ -136,6 +140,436 @@ TEST_F(RGBDSplitTest, DecompressesDepthWithTheCorrectEncoding) << "and the values must survive the round trip"; } +TEST_F(RGBDSplitTest, DecompressesRosCompressedDepthAndInverseDepth) +{ + // depth_compressed filled by compressed_depth_image_transport, or by rtabmap with an + // inverse depth format (Mem/DepthCompressionFormat=".rvl:10:100"). + addNode(std::make_shared(rclcpp::NodeOptions())); + + std::shared_ptr> depth = + collect("rgbd_image/depth/image"); + rclcpp::Publisher::SharedPtr pub = + helper()->create_publisher("rgbd_image", 10); + ASSERT_TRUE(waitForSubscriber(pub)); + ASSERT_TRUE(waitForPublisher(depth->subscription)); + + const cv::Mat original(8, 8, CV_32FC1, cv::Scalar(3.0f)); + const cv::Mat compressed = rtabmap::compressImage2(original, ".rvl:10:100"); + for(bool ros : {true, false}) + { + SCOPED_TRACE(ros ? "compressedDepth" : "rtabmap"); + rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0); + in.depth = sensor_msgs::msg::Image(); + in.depth_compressed.header = in.header; + if(ros) + { + ASSERT_TRUE(rtabmap_conversions::rtabmapToCompressedDepthTransport(compressed, in.depth_compressed)); + // RVL re-compressed as PNG before Jazzy + ASSERT_EQ(in.depth_compressed.format.rfind("32FC1; compressedDepth ", 0), 0u); + } + else + { + in.depth_compressed.format = "rvl:10:100"; + in.depth_compressed.data.assign(compressed.data, compressed.data + compressed.total()); + } + + const size_t received = depth->size(); + pub->publish(in); + ASSERT_TRUE(spinUntil([&]() { return depth->size() > received; })); + + const sensor_msgs::msg::Image & got = depth->back(); + EXPECT_EQ(got.encoding, sensor_msgs::image_encodings::TYPE_32FC1); + EXPECT_EQ(got.width, 8u); + EXPECT_EQ(got.height, 8u); + ASSERT_EQ(got.step, 32u); + EXPECT_NEAR(*reinterpret_cast(&got.data[0]), 3.0f, 0.001f); + } +} + +/// The compressedDepth topic of depth/image, published by rgbd_split itself (see +/// "compressed_passthrough") and read here through the real +/// compressed_depth_image_transport plugin, as any ROS consumer would. +class RGBDSplitCompressedDepthTest : public NodeTest +{ +protected: + void start(const std::vector & params = {}) + { + addNode(std::make_shared( + rclcpp::NodeOptions().parameter_overrides(params))); + compressed_ = collect("rgbd_image/depth/image/compressedDepth"); + const auto callback = [this](const sensor_msgs::msg::Image::ConstSharedPtr & msg) { decoded_.push_back(msg); }; +#ifdef PRE_ROS_LYRICAL + decodedSub_ = image_transport::create_subscription(helper().get(), "rgbd_image/depth/image", callback, "compressedDepth"); +#else + decodedSub_ = image_transport::create_subscription(*helper(), "rgbd_image/depth/image", callback, "compressedDepth", rclcpp::QoS(10), rclcpp::SubscriptionOptions()); +#endif + pub_ = helper()->create_publisher("rgbd_image", 10); + ASSERT_TRUE(waitForSubscriber(pub_)); + ASSERT_TRUE(waitForPublisher(compressed_->subscription)); + } + + /// Publishes @p in and waits for both the raw compressedDepth message and its decoding. + void publishAndWait(const rtabmap_msgs::msg::RGBDImage & in) + { + const size_t received = compressed_->size(); + const size_t decoded = decoded_.size(); + pub_->publish(in); + ASSERT_TRUE(spinUntil([&]() { return compressed_->size() > received && decoded_.size() > decoded; })); + } + + static rtabmap_msgs::msg::RGBDImage withCompressedDepth(const cv::Mat & compressed, const std::string & format) + { + rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0); + in.depth = sensor_msgs::msg::Image(); + in.depth_compressed.header = in.header; + in.depth_compressed.format = format; + in.depth_compressed.data.assign(compressed.data, compressed.data + compressed.total()); + return in; + } + + void expectDecodedDepth(float expected, float tolerance) + { + ASSERT_FALSE(decoded_.empty()); + const sensor_msgs::msg::Image & got = *decoded_.back(); + ASSERT_EQ(got.encoding, sensor_msgs::image_encodings::TYPE_32FC1); + ASSERT_EQ(got.width, 8u); + ASSERT_EQ(got.height, 8u); + EXPECT_NEAR(*reinterpret_cast(&got.data[0]), expected, tolerance); + EXPECT_EQ(got.header.frame_id, "camera_link"); + } + + std::shared_ptr> compressed_; + std::vector decoded_; + image_transport::Subscriber decodedSub_; + rclcpp::Publisher::SharedPtr pub_; +}; + +TEST_F(RGBDSplitCompressedDepthTest, RepublishesRosCompressedDepthAsIs) +{ + start(); + rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0); + in.depth = sensor_msgs::msg::Image(); + in.depth_compressed.header = in.header; + ASSERT_TRUE(rtabmap_conversions::rtabmapToCompressedDepthTransport( + rtabmap::compressImage2(cv::Mat(8, 8, CV_32FC1, cv::Scalar(4.0f)), ".png:20:50"), in.depth_compressed)); + + publishAndWait(in); + EXPECT_EQ(compressed_->back().format, in.depth_compressed.format); + EXPECT_EQ(compressed_->back().data, in.depth_compressed.data) << "not decompressed and re-compressed"; + expectDecodedDepth(4.0f, 0.01f); +} + +TEST_F(RGBDSplitCompressedDepthTest, ConvertsRtabmapInverseDepthWithoutDecompressingIt) +{ + start(); + const cv::Mat compressed = rtabmap::compressImage2(cv::Mat(8, 8, CV_32FC1, cv::Scalar(3.0f)), ".png:10:100"); + const rtabmap_msgs::msg::RGBDImage in = withCompressedDepth(compressed, "png:10:100"); + + publishAndWait(in); + EXPECT_EQ(compressed_->back().format, "32FC1; compressedDepth png"); + // Same payload: only the header changed + const std::vector & data = compressed_->back().data; + ASSERT_EQ(data.size(), compressed.total() - 16 + 12); + EXPECT_EQ(memcmp(data.data() + 12, compressed.data + 16, compressed.total() - 16), 0); + expectDecodedDepth(3.0f, 0.001f); +} + +TEST_F(RGBDSplitCompressedDepthTest, Converts16BitsRvl) +{ + start(); + const cv::Mat compressed = rtabmap::compressImage2(cv::Mat(8, 8, CV_16UC1, cv::Scalar(1234)), ".rvl"); + publishAndWait(withCompressedDepth(compressed, "rvl")); +#ifdef PRE_ROS_JAZZY + // compressed_depth_image_transport cannot decode RVL before Jazzy: re-compressed as PNG + EXPECT_EQ(compressed_->back().format, "16UC1; compressedDepth png"); +#else + // without decompressing it + EXPECT_EQ(compressed_->back().format, "16UC1; compressedDepth rvl"); + EXPECT_EQ(compressed_->back().data.size(), compressed.total() - 8 + 12); +#endif + const sensor_msgs::msg::Image & got = *decoded_.back(); + ASSERT_EQ(got.encoding, sensor_msgs::image_encodings::TYPE_16UC1); + EXPECT_EQ(*reinterpret_cast(&got.data[0]), 1234); +} + +TEST_F(RGBDSplitCompressedDepthTest, CompressesLegacyFloatDepthWithTheTransportParameters) +{ + // The legacy 32FC1 format (4 channels PNG) has no compressedDepth equivalent. + start({rclcpp::Parameter("rgbd_image.depth.image.compressedDepth.format", std::string("rvl")), + rclcpp::Parameter("rgbd_image.depth.image.compressedDepth.depth_max", 5.0)}); + const cv::Mat compressed = rtabmap::compressImage2(cv::Mat(8, 8, CV_32FC1, cv::Scalar(2.0f)), ".png"); + ASSERT_EQ(rtabmap::compressedDepthFormat(compressed), ".png"); + + publishAndWait(withCompressedDepth(compressed, "png")); +#ifdef PRE_ROS_JAZZY + const std::string codec = "png"; // RVL not supported by compressed_depth_image_transport +#else + const std::string codec = "rvl"; +#endif + EXPECT_EQ(compressed_->back().format, "32FC1; compressedDepth " + codec); + EXPECT_EQ(rtabmap::compressedDepthFormat(rtabmap_conversions::compressedDepthTransportToRtabmap(compressed_->back())), "." + codec + ":5:100"); + expectDecodedDepth(2.0f, 0.001f); +} + +TEST_F(RGBDSplitCompressedDepthTest, ConvertsRtabmapRvlInverseDepth) +{ + start(); + const cv::Mat compressed = rtabmap::compressImage2(cv::Mat(8, 8, CV_32FC1, cv::Scalar(6.0f)), ".rvl:10:100"); + publishAndWait(withCompressedDepth(compressed, "rvl:10:100")); + const cv::Mat bytes = rtabmap_conversions::compressedDepthTransportToRtabmap(compressed_->back()); +#ifdef PRE_ROS_JAZZY + EXPECT_EQ(compressed_->back().format, "32FC1; compressedDepth png"); + EXPECT_EQ(rtabmap::compressedDepthFormat(bytes), ".png:10:100"); +#else + EXPECT_EQ(compressed_->back().format, "32FC1; compressedDepth rvl"); + EXPECT_EQ(rtabmap::compressedDepthFormat(bytes), ".rvl:10:100"); +#endif + // Same quantized values either way + const cv::Mat restored = rtabmap::uncompressImage(bytes); + EXPECT_EQ(cv::countNonZero(restored != rtabmap::uncompressImage(compressed)), 0); + expectDecodedDepth(6.0f, 0.002f); +} + +TEST_F(RGBDSplitCompressedDepthTest, CompressesRawDepth) +{ + start(); + rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0); + cv_bridge::CvImage(in.header, sensor_msgs::image_encodings::TYPE_32FC1, + cv::Mat(8, 8, CV_32FC1, cv::Scalar(1.5f))).toImageMsg(in.depth); + + publishAndWait(in); + EXPECT_EQ(compressed_->back().format, "32FC1; compressedDepth png"); + expectDecodedDepth(1.5f, 0.001f); +} + +/// What compressed_depth_image_transport publishes for a 16UC1 depth image in RVL (since +/// Jazzy): its config header, then rtabmap's RVL payload without its signature. +TEST_F(RGBDSplitCompressedDepthTest, RepublishesRosRvlDepthWhereItCanBeDecoded) +{ + start(); + const cv::Mat rvl = rtabmap::compressImage2(cv::Mat(8, 8, CV_16UC1, cv::Scalar(2345)), ".rvl"); + rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0); + in.depth = sensor_msgs::msg::Image(); + in.depth_compressed.header = in.header; + in.depth_compressed.format = "16UC1; compressedDepth rvl"; + in.depth_compressed.data.assign(12, 0); // format 0 (INV_DEPTH), depthParam unused for 16UC1 + in.depth_compressed.data.insert(in.depth_compressed.data.end(), rvl.data + 8, rvl.data + rvl.total()); + + publishAndWait(in); +#ifdef PRE_ROS_JAZZY + // compressed_depth_image_transport cannot decode RVL before Jazzy: re-compressed as PNG + EXPECT_EQ(compressed_->back().format, "16UC1; compressedDepth png"); +#else + EXPECT_EQ(compressed_->back().format, in.depth_compressed.format); + EXPECT_EQ(compressed_->back().data, in.depth_compressed.data) << "not decompressed and re-compressed"; +#endif + const sensor_msgs::msg::Image & got = *decoded_.back(); + ASSERT_EQ(got.encoding, sensor_msgs::image_encodings::TYPE_16UC1); + EXPECT_EQ(*reinterpret_cast(&got.data[0]), 2345); +} + +/// A raw depth image compressed here keeps its own header, not the RGBDImage's. +TEST_F(RGBDSplitCompressedDepthTest, CompressesRawDepthWithItsOwnHeader) +{ + start(); + rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0); + std_msgs::msg::Header depthHeader = in.header; + depthHeader.frame_id = "depth_optical"; + cv_bridge::CvImage(depthHeader, sensor_msgs::image_encodings::TYPE_32FC1, + cv::Mat(8, 8, CV_32FC1, cv::Scalar(1.5f))).toImageMsg(in.depth); + + publishAndWait(in); + EXPECT_EQ(compressed_->back().header.frame_id, "depth_optical"); + EXPECT_EQ(decoded_.back()->header.frame_id, "depth_optical"); +} + +TEST_F(RGBDSplitCompressedDepthTest, PluginIsUsedWhenPassthroughIsDisabled) +{ + start({rclcpp::Parameter("compressed_passthrough", false)}); + const cv::Mat compressed = rtabmap::compressImage2(cv::Mat(8, 8, CV_32FC1, cv::Scalar(3.0f)), ".png:10:100"); + + publishAndWait(withCompressedDepth(compressed, "png:10:100")); + expectDecodedDepth(3.0f, 0.01f); + // Re-compressed by the plugin ("32FC1; compressedDepth" before Jazzy, without the codec) + EXPECT_EQ(compressed_->back().format.rfind("32FC1; compressedDepth", 0), 0u) << compressed_->back().format; + EXPECT_EQ(rtabmap::compressedDepthFormat(rtabmap_conversions::compressedDepthTransportToRtabmap(compressed_->back())), ".png:10:100"); +} + +/// compressed_depth_passthrough follows compressed_passthrough unless set: the depth can +/// still be passed through with the color compressed by the plugin (e.g., for jpeg_quality). +TEST_F(RGBDSplitCompressedDepthTest, DepthPassthroughCanBeKeptWithoutColorPassthrough) +{ + start({rclcpp::Parameter("compressed_passthrough", false), + rclcpp::Parameter("compressed_depth_passthrough", true)}); + const cv::Mat compressed = rtabmap::compressImage2(cv::Mat(8, 8, CV_32FC1, cv::Scalar(3.0f)), ".png:10:100"); + + publishAndWait(withCompressedDepth(compressed, "png:10:100")); + EXPECT_EQ(compressed_->back().format, "32FC1; compressedDepth png"); + // Same payload: only the header changed + const std::vector & data = compressed_->back().data; + ASSERT_EQ(data.size(), compressed.total() - 16 + 12); + EXPECT_EQ(memcmp(data.data() + 12, compressed.data + 16, compressed.total() - 16), 0); + expectDecodedDepth(3.0f, 0.001f); +} + +/// The compressed topic of the color (or left/right) image, published by rgbd_split +/// itself too, read through the real compressed_image_transport plugin. +class RGBDSplitCompressedImageTest : public NodeTest +{ +protected: + void start(const std::string & topic, const std::vector & params = {}) + { + split_ = addNode(std::make_shared( + rclcpp::NodeOptions().parameter_overrides(params))); + compressed_ = collect(topic + "/compressed"); + const auto callback = [this](const sensor_msgs::msg::Image::ConstSharedPtr & msg) { decoded_.push_back(msg); }; +#ifdef PRE_ROS_LYRICAL + decodedSub_ = image_transport::create_subscription(helper().get(), topic, callback, "compressed"); +#else + decodedSub_ = image_transport::create_subscription(*helper(), topic, callback, "compressed", rclcpp::QoS(10), rclcpp::SubscriptionOptions()); +#endif + pub_ = helper()->create_publisher("rgbd_image", 10); + ASSERT_TRUE(waitForSubscriber(pub_)); + ASSERT_TRUE(waitForPublisher(compressed_->subscription)); + } + + void publishAndWait(const rtabmap_msgs::msg::RGBDImage & in) + { + const size_t received = compressed_->size(); + const size_t decoded = decoded_.size(); + pub_->publish(in); + ASSERT_TRUE(spinUntil([&]() { return compressed_->size() > received && decoded_.size() > decoded; })); + } + + std::shared_ptr split_; + std::shared_ptr> compressed_; + std::vector decoded_; + image_transport::Subscriber decodedSub_; + rclcpp::Publisher::SharedPtr pub_; +}; + +TEST_F(RGBDSplitCompressedImageTest, RepublishesCompressedColorAsIs) +{ + start("rgbd_image/rgb/image"); + rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0); + const cv::Mat color = cv_bridge::toCvCopy(in.rgb)->image; + in.rgb = sensor_msgs::msg::Image(); + ASSERT_TRUE(rtabmap_conversions::toCompressedImageMsg( + cv_bridge::CvImage(in.header, "bgr8", color), ".png", in.rgb_compressed)); + + publishAndWait(in); + EXPECT_EQ(compressed_->back().data, in.rgb_compressed.data) << "not decompressed and re-compressed"; + EXPECT_EQ(compressed_->back().format, "bgr8; png compressed bgr8"); + EXPECT_EQ(cv::norm(cv_bridge::toCvCopy(decoded_.back())->image, color, cv::NORM_INF), 0.0); +} + +/// Older producers set only "jpg" as format: the encoding is added from the image header. +TEST_F(RGBDSplitCompressedImageTest, AddsTheEncodingToAnOldFormat) +{ + start("rgbd_image/rgb/image"); + rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0); + cv_bridge::toCvCopy(in.rgb)->toCompressedImageMsg(in.rgb_compressed, cv_bridge::JPG); + in.rgb = sensor_msgs::msg::Image(); + ASSERT_EQ(in.rgb_compressed.format, "jpg"); + + publishAndWait(in); + EXPECT_EQ(compressed_->back().data, in.rgb_compressed.data); + EXPECT_EQ(compressed_->back().format, "bgr8; jpeg compressed bgr8"); + EXPECT_EQ(decoded_.back()->encoding, "bgr8"); +} + +/// A raw color image is compressed with ".compressed.format". +TEST_F(RGBDSplitCompressedImageTest, CompressesRawColorWithTheTransportParameter) +{ + start("rgbd_image/rgb/image", {rclcpp::Parameter("rgbd_image.rgb.image.compressed.format", std::string("png"))}); + const rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0); + + publishAndWait(in); + EXPECT_EQ(compressed_->back().format, "bgr8; png compressed bgr8"); + EXPECT_EQ(cv::norm(cv_bridge::toCvCopy(decoded_.back())->image, cv_bridge::toCvCopy(in.rgb)->image, cv::NORM_INF), 0.0); +} + +/// With compressed_passthrough, the compressed plugin is removed even if the user enabled +/// it: only the node publishes on the topic, the image as is. +TEST_F(RGBDSplitCompressedImageTest, RemovesTheCompressedPluginEnabledByTheUser) +{ + const std::string name = "rgbd_image.rgb.image.enable_pub_plugins"; + start("rgbd_image/rgb/image", {rclcpp::Parameter(name, + std::vector{"image_transport/raw", "image_transport/compressed"})}); + const std::vector plugins = split_->get_parameter(name).as_string_array(); + EXPECT_EQ(plugins, std::vector{"image_transport/raw"}); + + rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0); + const cv::Mat color = cv_bridge::toCvCopy(in.rgb)->image; + in.rgb = sensor_msgs::msg::Image(); + ASSERT_TRUE(rtabmap_conversions::toCompressedImageMsg( + cv_bridge::CvImage(in.header, "bgr8", color), ".png", in.rgb_compressed)); + + publishAndWait(in); + spinFor(std::chrono::milliseconds(200)); + EXPECT_EQ(compressed_->size(), 1u) << "published by the node only"; + EXPECT_EQ(compressed_->back().data, in.rgb_compressed.data) << "not decompressed and re-compressed"; +} + +/// compressed_depth_passthrough does not change the color image's passthrough. +TEST_F(RGBDSplitCompressedImageTest, ColorPassthroughCanBeKeptWithoutDepthPassthrough) +{ + start("rgbd_image/rgb/image", {rclcpp::Parameter("compressed_depth_passthrough", false)}); + rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0); + const cv::Mat color = cv_bridge::toCvCopy(in.rgb)->image; + in.rgb = sensor_msgs::msg::Image(); + ASSERT_TRUE(rtabmap_conversions::toCompressedImageMsg( + cv_bridge::CvImage(in.header, "bgr8", color), ".png", in.rgb_compressed)); + + publishAndWait(in); + EXPECT_EQ(compressed_->back().data, in.rgb_compressed.data) << "not decompressed and re-compressed"; +} + +/// A raw color image compressed here keeps its own header, not the RGBDImage's. +TEST_F(RGBDSplitCompressedImageTest, CompressesRawColorWithItsOwnHeader) +{ + start("rgbd_image/rgb/image"); + rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0); + in.rgb.header.frame_id = "rgb_optical"; + + publishAndWait(in); + EXPECT_EQ(compressed_->back().header.frame_id, "rgb_optical"); + EXPECT_EQ(decoded_.back()->header.frame_id, "rgb_optical"); +} + +/// A compressed color image without a frame takes the RGBDImage's header. +TEST_F(RGBDSplitCompressedImageTest, RepublishesCompressedColorWithTheInputHeaderIfItHasNone) +{ + start("rgbd_image/rgb/image"); + rtabmap_msgs::msg::RGBDImage in = makeRGBDImage("camera_link", 1000.0); + const cv::Mat color = cv_bridge::toCvCopy(in.rgb)->image; + in.rgb = sensor_msgs::msg::Image(); + ASSERT_TRUE(rtabmap_conversions::toCompressedImageMsg( + cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", color), ".png", in.rgb_compressed)); + ASSERT_TRUE(in.rgb_compressed.header.frame_id.empty()); + + publishAndWait(in); + EXPECT_EQ(compressed_->back().data, in.rgb_compressed.data) << "not decompressed and re-compressed"; + EXPECT_EQ(compressed_->back().header.frame_id, "camera_link"); + EXPECT_EQ(compressed_->back().header.stamp, in.header.stamp); +} + +/// With "stereo", the right image is republished as is on right/image/compressed. +TEST_F(RGBDSplitCompressedImageTest, RepublishesACompressedRightImageAsIs) +{ + start("rgbd_image/right/image", {rclcpp::Parameter("stereo", true)}); + rtabmap_msgs::msg::RGBDImage in = makeStereoRGBDImage("camera_link", 1000.0); + const cv::Mat right = cv_bridge::toCvCopy(in.depth)->image; + in.depth = sensor_msgs::msg::Image(); + ASSERT_TRUE(rtabmap_conversions::toCompressedImageMsg( + cv_bridge::CvImage(in.header, "mono8", right), ".jpg", in.depth_compressed)); + + publishAndWait(in); + EXPECT_EQ(compressed_->back().data, in.depth_compressed.data); + EXPECT_EQ(compressed_->back().format, "mono8; jpeg compressed mono8"); + EXPECT_EQ(decoded_.back()->encoding, "mono8"); +} + /// Feeds a compressed right image in @p format and returns what lands on depth/image. class RGBDSplitRightImageTest : public NodeTest {