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_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<cv_bridge::CvImageConstPtr> & imageMsgs,
+			const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
+			const std::vector<sensor_msgs::msg::CameraInfo> & cameraInfoMsgs,
+			const std::vector<sensor_msgs::msg::CameraInfo> & depthCameraInfoMsgs,
+			const sensor_msgs::msg::LaserScan & scanMsg,
+			const sensor_msgs::msg::PointCloud2 & scan3dMsg,
+			const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr & odomInfoMsg,
+			const std::vector<rtabmap_msgs::msg::GlobalDescriptor> & globalDescriptorMsgs,
+			const std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > & localKeyPoints,
+			const std::vector<std::vector<rtabmap_msgs::msg::Point3f> > & localPoints3d,
+			const std::vector<cv::Mat> & localDescriptors,
+			const std::vector<cv::Mat> & compressedImages,
+			const std::vector<cv::Mat> & 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,

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 <rtabmap_msgs/msg/rgbd_images.hpp>
+#include <rtabmap_conversions/MsgConversion.h>
 
 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<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbd =
+			advertise<rtabmap_msgs::msg::RGBDImage>("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<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbd =
+			advertise<rtabmap_msgs::msg::RGBDImage>("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<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbd =
+			advertise<rtabmap_msgs::msg::RGBDImage>("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<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbd =
+			advertise<rtabmap_msgs::msg::RGBDImage>("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<RGBDInputs> {};
+
+}  // 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<rclcpp::Parameter> 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<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbd =
+			advertise<rtabmap_msgs::msg::RGBDImage>("rgbd_image");
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom;
+	if(inputs.odom)
+	{
+		odom = advertise<nav_msgs::msg::Odometry>("odom");
+	}
+	rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr scan;
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloud;
+	rclcpp::Publisher<rtabmap_msgs::msg::ScanDescriptor>::SharedPtr descriptor;
+	rclcpp::Publisher<rtabmap_msgs::msg::OdomInfo>::SharedPtr odomInfo;
+	if(inputs.extra == "scan")
+	{
+		scan = advertise<sensor_msgs::msg::LaserScan>("scan");
+	}
+	else if(inputs.extra == "scan_cloud")
+	{
+		cloud = advertise<sensor_msgs::msg::PointCloud2>("scan_cloud");
+	}
+	else if(inputs.extra == "scan_descriptor")
+	{
+		descriptor = advertise<rtabmap_msgs::msg::ScanDescriptor>("scan_descriptor");
+	}
+	else if(inputs.extra == "odom_info")
+	{
+		odomInfo = advertise<rtabmap_msgs::msg::OdomInfo>("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<RGBDInputs> & info) { return info.param.name; });
+
 TEST_F(CommonDataSubscriberSyncTest, RGBDModeCarriesAScanAlongside)
 {
 	start({rclcpp::Parameter("subscribe_rgbd", true),

rtabmap_sync/test/test_rgb_sync.cpp

@@ -9,6 +9,7 @@ All rights reserved. (BSD-3-Clause, see the repository root.)
 #include <rtabmap_sync/rgb_sync.hpp>
 
 #include <rtabmap/core/Compression.h>
+#include <rtabmap_conversions/MsgConversion.h>
 
 #include <string>
 #include <vector>
@@ -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";

rtabmap_sync/test/test_rgbd_sync.cpp

@@ -9,6 +9,7 @@ All rights reserved. (BSD-3-Clause, see the repository root.)
 #include <rtabmap_sync/rgbd_sync.hpp>
 
 #include <rtabmap/core/Compression.h>
+#include <rtabmap_conversions/MsgConversion.h>
 
 #include <cmath>
 
@@ -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<uint16_t>(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<float>(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<uint16_t>(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<float>(0, 0), 2.0004f);
+	EXPECT_EQ(rtabmap_conversions::uncompressDepthImage(got)->image.at<float>(0, 0), 2.0004f);
+}
+
 TEST_F(RGBDSyncTest, CompressedRateThrottlesTheCompressedOutputOnly)
 {
 	// Compression is expensive and the compressed topic usually feeds a slow link, so

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 --