mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-12 20:49:50 +08:00
Supporting image compression formats passthrough (#1461)
* Supporting ros depth compression formats passthrough * Added image_compression_format (support jpg,png). Use Compression.h constants. * Bridging gap with #1457 * CoreWrapper: decode_images_on_demand * rgbd_sync->rgbd_split encoding/decoding test with image_transport validation. rgbd_split color passthrough * more tests * fixing ci on kilted+ * fixing test * fixing lyrical tests * fixing rolling ci * Improved test coverage * fixing some tf waitForTransform deadlocks when sim time is used in tests * CI: build rtabmap_slam on rolling rtabmap_slam builds without nav2_msgs, which is not on resolute yet: skip its key instead of leaving the package out. Only rtabmap_costmap_plugins (and the rtabmap_ros metapackage pulling it in) still need nav2.
This commit is contained in:
matlabbe
@@ -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 ##
|
||||
###########
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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));
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user