i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-11 20:19:50 +08:00
Code Issues Packages Projects Releases Wiki Activity

Supporting image compression formats passthrough (#1461)

Browse Source 
* 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.
This commit is contained in:
matlabbe
2026-10-10 16:05:08 -07:00
committed by GitHub
parent 82f0754bf7
commit cf7d7ab958
47 changed files with 3962 additions and 250 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_conversions/CMakeLists.txt
+6 -1
View File
@@ -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 ##
###########
rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h
+171 -1
View File
@@ -36,6 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <sensor_msgs/msg/camera_info.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <sensor_msgs/msg/compressed_image.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <opencv2/opencv.hpp>
@@ -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<unsigned char>
*/
cv::Mat compressedMatFromBytes(const std::vector<unsigned char> & 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 "<encoding>; compressedDepth <codec>", 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 ":<maxDepth>[:<quantization>]", 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:<maxDepth>[:<quantization>]"): quantized as 16 bits
* inverse depth, like compressed_depth_image_transport ("32FC1; compressedDepth png"),
* see Mem/DepthCompressionFormat.
* - "legacy:<format>" ("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<unsigned char> & data);
//============================================================================
// Statistics
rtabmap_conversions/src/MsgConversion.cpp
+522 -27
View File
@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <algorithm>
#include <cmath>
#include <cstring>
#include <limits>
#include <opencv2/highgui/highgui.hpp>
@@ -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 ("<encoding>; <codec>
// compressed <encoding>"), 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<unsigned char> & 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<cv_bridge::CvImage>();
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<cv_bridge::CvImage>();
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<unsigned char> & 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<unsigned char> 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<unsigned char> & 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<cv_bridge::CvImage>();
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:<format>", 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<unsigned char> 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<depth.rows; ++i)
{
const float * in = depth.ptr<float>(i);
uint16_t * out = image.ptr<uint16_t>(i);
for(int j=0; j<depth.cols; ++j)
{
const float mm = in[j] * 1000.0f + 0.5f;
out[j] = mm >= 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));
rtabmap_conversions/test/test_msg_conversion.cpp
+685
View File
@@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <gtest/gtest.h>
#include <cmath>
#include <cstring>
#include <functional>
#include <limits>
@@ -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<rtabmap_msgs::msg::RGBDImage>();
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<float>(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<float>(r, c);
image.at<uint16_t>(r, c) = d < maxDepth ? (uint16_t)(A / d + B) : 0;
}
}
header.depthParam[0] = A;
header.depthParam[1] = B;
}
std::vector<unsigned char> payload;
if(codec == "png")
{
cv::imencode(".png", image, payload);
}
else
{
const std::vector<unsigned char> 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<unsigned char> toBytes(const cv::Mat & compressed)
{
return std::vector<unsigned char>(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<float>(r, c);
ASSERT_NEAR(restored.at<float>(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<Case> 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<unsigned char> 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<float>::quiet_NaN(), std::numeric_limits<float>::infinity(), 0.0004f};
const uint16_t expected[] = {1234, 1235, 0, 0, 0, 0, 0, 0};
for(int i = 0; i < 8; ++i)
{
depth.at<float>(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<uint16_t>(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_<double>(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<unsigned char> 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<unsigned char> palette = png;
palette[25] = 3;
EXPECT_EQ(compressedImageTransportFormat(palette), "");
std::vector<unsigned char> grayAlpha = png;
grayAlpha[25] = 4;
EXPECT_EQ(compressedImageTransportFormat(grayAlpha), "");
std::vector<unsigned char> 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<unsigned char> 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<unsigned char> 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<unsigned char> 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<rtabmap_msgs::msg::RGBDImage>(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<cv::KeyPoint> kpts = {cv::KeyPoint(1, 2, 3), cv::KeyPoint(4, 5, 6)};
const std::vector<cv::Point3f> pts = {cv::Point3f(0.1f, 0.2f, 1.0f), cv::Point3f(0.3f, 0.4f, 2.0f)};
const cv::Mat descriptors = (cv::Mat_<float>(2, 3) << 1, 2, 3, 4, 5, 6);
in.setFeatures(kpts, pts, descriptors);
in.addGlobalDescriptor(rtabmap::GlobalDescriptor(1, (cv::Mat_<float>(1, 4) << 0.1f, 0.2f, 0.3f, 0.4f)));
rtabmap_msgs::msg::RGBDImage::SharedPtr msg = std::make_shared<rtabmap_msgs::msg::RGBDImage>();
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);
}
rtabmap_msgs/msg/GlobalDescriptor.msg
+1 -1
View File
@@ -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
rtabmap_msgs/msg/Node.msg
+1 -1
View File
@@ -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
rtabmap_msgs/msg/RGBDImage.msg
+13 -1
View File
@@ -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[:<format>]", e.g., to keep 32FC1 depth lossless), with format
# "png", "rvl" or "png:<max>:<quantization>" / "rvl:<max>:<quantization>", 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
rtabmap_msgs/msg/SensorData.msg
+9 -5
View File
@@ -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
rtabmap_odom/README.md
+14 -1
View File
@@ -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 `":<maxDepth>[:<quantization>]"` (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.
rtabmap_odom/include/rtabmap_odom/OdometryROS.h
+1
View File
@@ -270,6 +270,7 @@ private:
double minUpdateRate_;
bool alwaysProcessMostRecentFrame_;
std::string compressionImgFormat_;
std::string compressionDepthFormat_;
bool compressionParallelized_;
int odomStrategy_;
bool waitIMUToinit_;
rtabmap_odom/src/OdometryROS.cpp
+21 -3
View File
@@ -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())
rtabmap_odom/test/test_icp_odometry.cpp
+23
View File
@@ -4,6 +4,7 @@ All rights reserved. (BSD-3-Clause, see the repository root.)
*/
#include <gtest/gtest.h>
#include <rtabmap_msgs/msg/rgbd_image.hpp>
#include <tf2_ros/static_transform_broadcaster.hpp>
@@ -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<Collector<nav_msgs::msg::Odometry>> odom =
collect<nav_msgs::msg::Odometry>("odom");
std::shared_ptr<Collector<rtabmap_msgs::msg::RGBDImage>> frames =
collect<rtabmap_msgs::msg::RGBDImage>("odom_rgbd_image");
makeNode(icpTestParameters());
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub =
helper()->create_publisher<sensor_msgs::msg::PointCloud2>("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)
{
rtabmap_odom/test/test_odometry_ros.cpp
+20
View File
@@ -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<Collector<nav_msgs::msg::Odometry>> odom =
collect<nav_msgs::msg::Odometry>("odom");
std::shared_ptr<Collector<rtabmap_msgs::msg::SensorData>> compressed =
collect<rtabmap_msgs::msg::SensorData>("odom_sensor_data/compressed");
makeNode({rclcpp::Parameter("publish_compressed_sensor_data", true),
rclcpp::Parameter("sensor_data_parallel_compression", false)});
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::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,
rtabmap_slam/doc/rtabmap.md
+8
View File
@@ -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.
rtabmap_slam/include/rtabmap_slam/CoreWrapper.h
+26 -1
View File
@@ -178,6 +178,28 @@ private:
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>());
/// 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<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;
// Callback called from sync thread
void commonMultiCameraCallbackImpl(
const std::string & odomFrameId,
@@ -192,7 +214,9 @@ private:
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> & localDescriptors,
const std::vector<cv::Mat> & compressedImages = std::vector<cv::Mat>(),
const std::vector<cv::Mat> & compressedDepths = std::vector<cv::Mat>());
// 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_;
rtabmap_slam/src/CoreWrapper.cpp
+69 -2
View File
@@ -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<std::vector<rtabmap_msgs::msg::KeyPoint> > & localKeyPoints,
const std::vector<std::vector<rtabmap_msgs::msg::Point3f> > & localPoints3d,
const std::vector<cv::Mat> & localDescriptors)
{
commonMultiCameraCallbackWithCompressed(odomMsg, userDataMsg, imageMsgs, depthMsgs,
cameraInfoMsgs, depthCameraInfoMsgs, scan2dMsg, scan3dMsg, odomInfoMsg,
globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors,
std::vector<cv::Mat>(), std::vector<cv::Mat>());
}
void CoreWrapper::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 & scan2dMsg,
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)
{
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<rtabmap_msgs::msg::GlobalDescriptor> & globalDescriptorMsgs,
const std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > & localKeyPointsMsgs,
const std::vector<std::vector<rtabmap_msgs::msg::Point3f> > & localPoints3dMsgs,
const std::vector<cv::Mat> & localDescriptorsMsgs)
const std::vector<cv::Mat> & localDescriptorsMsgs,
const std::vector<cv::Mat> & compressedImages,
const std::vector<cv::Mat> & 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())
{
rtabmap_slam/test/core_wrapper_fixture.hpp
+40 -2
View File
@@ -223,9 +223,9 @@ protected:
if(!tfPub_)
{
tfPub_ = helper()->create_publisher<tf2_msgs::msg::TFMessage>("/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<tf2_msgs::msg::TFMessage> & 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<tf2_msgs::msg::TFMessage>("/tf", rclcpp::QoS(100));
}
/// The test's own /tf subscription, see waitForTfBeforeOdom().
std::shared_ptr<Collector<tf2_msgs::msg::TFMessage>> tfReceived_;
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odomPublisher()
{
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pub =
rtabmap_slam/test/test_core_wrapper_inputs.cpp
+326
View File
@@ -23,6 +23,7 @@ All rights reserved. (BSD-3-Clause, see the repository root.)
#include <rtabmap/core/Parameters.h>
#include <rtabmap/core/Compression.h>
#include <opencv2/imgproc.hpp>
#include <rtabmap/core/SensorData.h>
#include <rtabmap_conversions/MsgConversion.h>
@@ -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<std::tuple<std::string, bool>>
{
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<rclcpp::Parameter> & params = {})
{
std::vector<rclcpp::Parameter> 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<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
rclcpp::Publisher<rtabmap_msgs::msg::SensorData>::SharedPtr pub =
helper()->create_publisher<rtabmap_msgs::msg::SensorData>("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<unsigned char> bytes(const cv::Mat & compressed)
{
return std::vector<unsigned char>(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<std::tuple<std::string, bool>> & 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<rclcpp::Parameter> & params = {})
{
std::vector<rclcpp::Parameter> 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<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbd =
helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("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<unsigned char>(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<unsigned char> & 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<unsigned char>(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
//==========================================================================================
rtabmap_slam/test/test_core_wrapper_planning.cpp
+15 -43
View File
@@ -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<rclcpp::Parameter> 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<rclcpp::Parameter> 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<geometry_msgs::msg::PoseStamped>::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<Collector<tf2_msgs::msg::TFMessage>> tf =
collect<tf2_msgs::msg::TFMessage>("/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;
rtabmap_sync/doc/rgb_sync.md
+2 -1
View File
@@ -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
rtabmap_sync/doc/rgbd_sync.md
+21 -2
View File
@@ -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.
rtabmap_sync/doc/stereo_sync.md
+4 -3
View File
@@ -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`.
rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h
+57 -1
View File
@@ -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,
rtabmap_sync/include/rtabmap_sync/rgb_sync.hpp
+2
View File
@@ -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.
rtabmap_sync/include/rtabmap_sync/rgbd_sync.hpp
+3
View File
@@ -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.
rtabmap_sync/include/rtabmap_sync/stereo_sync.hpp
+2
View File
@@ -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
rtabmap_sync/src/CommonDataSubscriber.cpp
+53 -3
View File
@@ -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)
rtabmap_sync/src/impl/CommonDataSubscriberRGBD.cpp
+84 -40
View File
@@ -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
rtabmap_sync/src/nodelets/rgb_sync.cpp
+8 -3
View File
@@ -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);
rtabmap_sync/src/nodelets/rgbd_sync.cpp
+16 -4
View File
@@ -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));
}
rtabmap_sync/src/nodelets/stereo_sync.cpp
+8 -2
View File
@@ -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));
}
rtabmap_sync/test/common_data_subscriber_fixture.hpp
+42
View File
@@ -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
+230
View File
@@ -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
+13 -2
View File
@@ -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
+100 -2
View File
@@ -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
+13
View File
@@ -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 --
rtabmap_util/CMakeLists.txt
+8
View File
@@ -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)
rtabmap_util/doc/rgbd_relay.md
+5 -3
View File
@@ -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 `":<maxDepth>[:<quantization>]"` (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.
rtabmap_util/doc/rgbd_split.md
+30
View File
@@ -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
| `<rgbd_image>/rgb/camera_info` | [`sensor_msgs/msg/CameraInfo`](https://docs.ros.org/en/jazzy/p/sensor_msgs/msg/CameraInfo.html) | |
| `<rgbd_image>/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. |
| `<rgbd_image>/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. |
| `<rgbd_image>/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 `<rgbd_image>/right/image/compressed` with `stereo: true`. See [Compressed images](#compressed-images). |
| `<rgbd_image>/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. |
| `<color topic>.compressed.format` | `string` | `"jpeg"` | `jpeg` or `png`. With `compressed_passthrough`, used only when an image (color, left or right) has to be compressed. `<color topic>` is the color (or left) topic with `.` instead of `/`, e.g. `rgbd_image.rgb.image`. |
| `<depth topic>.compressedDepth.format` | `string` | `"png"` | `png` or `rvl` (Jazzy and later). With `compressed_depth_passthrough`, used only when the depth has to be compressed. `<depth topic>` is the depth topic with `.` instead of `/`, e.g. `rgbd_image.depth.image`. |
| `<depth topic>.compressedDepth.depth_max` | `double` | `10.0` | Same, maximum depth (m) of a 32FC1 depth image. |
| `<depth topic>.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 `<rgbd_image>/rgb/image/compressed` (and `<rgbd_image>/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 `<color topic>.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:<max>:<q>`, `rvl:<max>:<q>`), which image_transport plugins cannot read. The node republishes both on `<rgbd_image>/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:<max>:<q>`, `rvl:<max>:<q>` | 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 `<depth topic>.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.
rtabmap_util/include/rtabmap_util/rgbd_relay.hpp
+3
View File
@@ -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<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdImageSub_;
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdImagePub_;
};
rtabmap_util/include/rtabmap_util/rgbd_split.hpp
+27
View File
@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <image_transport/image_transport.hpp>
#include "rtabmap_msgs/msg/rgbd_image.hpp"
#include <sensor_msgs/msg/compressed_image.hpp>
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<sensor_msgs::msg::CompressedImage>::SharedPtr compressedRgbPub_;
rclcpp::Publisher<sensor_msgs::msg::CompressedImage>::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<sensor_msgs::msg::CompressedImage>::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<sensor_msgs::msg::CompressedImage>::SharedPtr & compressedPub,
sensor_msgs::msg::Image & outputImage) const;
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr rgbInfoPub_;
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr depthInfoPub_;
};
rtabmap_util/package.xml
+5
View File
@@ -37,6 +37,11 @@
<depend>grid_map_ros</depend>
<test_depend>ament_cmake_gtest</test_depend>
<test_depend>ament_index_cpp</test_depend>
<test_depend>class_loader</test_depend>
<!-- image_transport plugins the tests read rgbd_split's outputs with -->
<test_depend>compressed_image_transport</test_depend>
<test_depend>compressed_depth_image_transport</test_depend>
<export>
<build_type>ament_cmake</build_type>
rtabmap_util/src/nodelets/rgbd_relay.cpp
+22 -32
View File
@@ -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<cv_bridge::CvImage>();
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);
}
}
}
rtabmap_util/src/nodelets/rgbd_split.cpp
+287 -56
View File
@@ -27,9 +27,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap_util/rgbd_split.hpp>
#include <rtabmap/core/Compression.h>
#include <rtabmap_conversions/MsgConversion.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UConversion.h>
#include <sensor_msgs/image_encodings.hpp>
#include <algorithm>
#include <cstring>
#ifdef PRE_ROS_IRON
#include <cv_bridge/cv_bridge.h>
@@ -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, &paramBaseOf](const std::string & topic, const std::string & transport, const std::string & passthroughParam) {
const std::string name = paramBaseOf(topic) + ".enable_pub_plugins";
std::vector<std::string> 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<sensor_msgs::msg::CompressedImage>(rgbTopic + "/compressed", pubQos);
if(stereo_)
{
removePlugin(depthTopic, kCompressed, "compressed_passthrough");
compressedRightPub_ = this->create_publisher<sensor_msgs::msg::CompressedImage>(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<sensor_msgs::msg::CompressedImage>(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<sensor_msgs::msg::CameraInfo>(base + firstName + "/camera_info", pubQos);
depthInfoPub_ = this->create_publisher<sensor_msgs::msg::CameraInfo>(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<sensor_msgs::msg::CompressedImage>::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);
}
}
rtabmap_util/test/test_rgbd_pipeline.cpp
+417
View File
@@ -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 <rtabmap_util/rgbd_split.hpp>
#include <ament_index_cpp/get_resource.hpp>
#include <class_loader/class_loader.hpp>
#include <image_transport/image_transport.hpp>
#include <rclcpp_components/node_factory.hpp>
#include <sensor_msgs/image_encodings.hpp>
#include <opencv2/imgproc.hpp>
#include <cmath>
#include <limits>
#include <sstream>
#include <string>
#include <vector>
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<class_loader::ClassLoader *> loaders;
loaders.push_back(new class_loader::ClassLoader(basePath + "/" + line.substr(split + 1)));
std::shared_ptr<rclcpp_components::NodeFactory> factory =
loaders.back()->createInstance<rclcpp_components::NodeFactory>(
"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<rclcpp::Node>(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<cv::Vec3b>(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<uint16_t>(r, c) = uint16_t(meters * 1000.0f + 0.5f);
}
else
{
depth.at<float>(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<uint16_t>(r, c) * 0.001f : depth.at<float>(r, c);
return std::isfinite(d) ? d : 0.0f;
}
class RGBDPipelineTest : public NodeTest, public ::testing::WithParamInterface<PipelineCase>
{
protected:
using ImagePtr = sensor_msgs::msg::Image::ConstSharedPtr;
image_transport::Subscriber subscribe(const std::string & topic, const std::string & transport, std::vector<ImagePtr> & 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<rtabmap_util::RGBDSplit>(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<sensor_msgs::msg::Image>::SharedPtr rgbPub =
helper()->create_publisher<sensor_msgs::msg::Image>("rgb/image", 10);
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depthPub =
helper()->create_publisher<sensor_msgs::msg::Image>("depth/image", 10);
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr infoPub =
helper()->create_publisher<sensor_msgs::msg::CameraInfo>("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<typename Tolerance>
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<ImagePtr> rgbRaw_;
std::vector<ImagePtr> rgbCompressed_;
std::vector<ImagePtr> depthRaw_;
std::vector<ImagePtr> 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:<format>": 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<PipelineCase> & 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<std::string>
{
protected:
using ImagePtr = sensor_msgs::msg::Image::ConstSharedPtr;
image_transport::Subscriber subscribe(const std::string & topic, const std::string & transport, std::vector<ImagePtr> & 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<rtabmap_util::RGBDSplit>(rclcpp::NodeOptions()
.arguments({"--ros-args", "-r", "rgbd_image:=rgbd_image/compressed"})
.parameter_overrides({rclcpp::Parameter("stereo", true)})));
std::vector<ImagePtr> 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<sensor_msgs::msg::Image>::SharedPtr leftPub =
helper()->create_publisher<sensor_msgs::msg::Image>("left/image_rect", 10);
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr rightPub =
helper()->create_publisher<sensor_msgs::msg::Image>("right/image_rect", 10);
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr leftInfoPub =
helper()->create_publisher<sensor_msgs::msg::CameraInfo>("left/camera_info", 10);
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr rightInfoPub =
helper()->create_publisher<sensor_msgs::msg::CameraInfo>("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<std::string> & info) { return info.param.substr(1); });
rtabmap_util/test/test_rgbd_relay.cpp
+95 -7
View File
@@ -9,6 +9,7 @@ All rights reserved. (BSD-3-Clause, see the repository root.)
#include <rtabmap_util/rgbd_relay.hpp>
#include <rtabmap/core/Compression.h>
#include <rtabmap_conversions/MsgConversion.h>
#include <rtabmap/utilite/UException.h>
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<rtabmap_util::RGBDRelay>(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<rtabmap_msgs::msg::RGBDImage>("rgbd_image_relay");
pub_ = helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("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
rtabmap_util/test/test_rgbd_split.cpp
+434
View File
@@ -8,7 +8,11 @@ All rights reserved. (BSD-3-Clause, see the repository root.)
#include <rtabmap_util/rgbd_split.hpp>
#include <image_transport/image_transport.hpp>
#include <cstring>
#include <rtabmap/core/Compression.h>
#include <rtabmap_conversions/MsgConversion.h>
#include <rtabmap/utilite/UException.h>
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<rtabmap_util::RGBDSplit>(rclcpp::NodeOptions()));
std::shared_ptr<Collector<sensor_msgs::msg::Image>> depth =
collect<sensor_msgs::msg::Image>("rgbd_image/depth/image");
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr pub =
helper()->create_publisher<rtabmap_msgs::msg::RGBDImage>("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<const float *>(&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<rclcpp::Parameter> & params = {})
{
addNode(std::make_shared<rtabmap_util::RGBDSplit>(
rclcpp::NodeOptions().parameter_overrides(params)));
compressed_ = collect<sensor_msgs::msg::CompressedImage>("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<rtabmap_msgs::msg::RGBDImage>("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<const float *>(&got.data[0]), expected, tolerance);
EXPECT_EQ(got.header.frame_id, "camera_link");
}
std::shared_ptr<Collector<sensor_msgs::msg::CompressedImage>> compressed_;
std::vector<sensor_msgs::msg::Image::ConstSharedPtr> decoded_;
image_transport::Subscriber decodedSub_;
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::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<unsigned char> & 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<const uint16_t *>(&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<const uint16_t *>(&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<unsigned char> & 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<rclcpp::Parameter> & params = {})
{
split_ = addNode(std::make_shared<rtabmap_util::RGBDSplit>(
rclcpp::NodeOptions().parameter_overrides(params)));
compressed_ = collect<sensor_msgs::msg::CompressedImage>(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<rtabmap_msgs::msg::RGBDImage>("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<rtabmap_util::RGBDSplit> split_;
std::shared_ptr<Collector<sensor_msgs::msg::CompressedImage>> compressed_;
std::vector<sensor_msgs::msg::Image::ConstSharedPtr> decoded_;
image_transport::Subscriber decodedSub_;
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::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 "<topic>.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<std::string>{"image_transport/raw", "image_transport/compressed"})});
const std::vector<std::string> plugins = split_->get_parameter(name).as_string_array();
EXPECT_EQ(plugins, std::vector<std::string>{"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
{