mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-12 12:39: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
@@ -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); });
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
{
|
||||
|
||||
Reference in New Issue
Block a user