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.
2026-10-12 20:49:50 +08:00 · 2026-10-10 16:05:08 -07:00
parent 82f0754bf7
commit cf7d7ab958
47 changed files with 3962 additions and 250 deletions
@@ -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
@@ -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:<maxDepth>:<quantization>"`, `".rvl:<maxDepth>:<quantization>"`, 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:<maxDepth>:<quantization>"`, `".rvl:<maxDepth>:<quantization>"` | 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:<format>"` (`"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.

@@ -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`.

@@ -244,6 +244,52 @@ protected:
 				const std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > & localKeyPoints = std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> >(),
 				const std::vector<std::vector<rtabmap_msgs::msg::Point3f> > & localPoints3d = std::vector<std::vector<rtabmap_msgs::msg::Point3f> >(),
 				const std::vector<cv::Mat> & localDescriptors = std::vector<cv::Mat>()) = 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<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)
+	{
+		(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<rtabmap_msgs::msg::GlobalDescriptor> & globalDescriptorMsgs = std::vector<rtabmap_msgs::msg::GlobalDescriptor>(),
 			const std::vector<rtabmap_msgs::msg::KeyPoint> & localKeyPoints = std::vector<rtabmap_msgs::msg::KeyPoint>(),
 			const std::vector<rtabmap_msgs::msg::Point3f> & localPoints3d = std::vector<rtabmap_msgs::msg::Point3f>(),
-			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,
@@ -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.
@@ -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.
@@ -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
@@ -27,6 +27,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 #include <rtabmap_sync/CommonDataSubscriber.h>
 #include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/core/Compression.h>
+#include <rtabmap_conversions/MsgConversion.h>

 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<rtabmap_msgs::msg::GlobalDescriptor> & globalDescriptorMsgs,
 		const std::vector<rtabmap_msgs::msg::KeyPoint> & localKeyPoints,
 		const std::vector<rtabmap_msgs::msg::Point3f> & localPoints3d,
-		const cv::Mat & localDescriptors)
+		const cv::Mat & localDescriptors,
+		const cv::Mat & compressedImage,
+		const cv::Mat & compressedDepth)
 {
 	std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > 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<cv::Mat>(1, compressedImage),
+			std::vector<cv::Mat>(1, compressedDepth));
 }

 void CommonDataSubscriber::tick(const rclcpp::Time & stamp, double targetFrequency)
@@ -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

@@ -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);
@@ -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<std::string>("rgb_image_transport", std::string("raw"));
 	std::string depthImageTransport = this->declare_parameter<std::string>("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));
 			}
@@ -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));
 			}
@@ -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,
@@ -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),
@@ -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";
@@ -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
@@ -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 --