Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

feat: extend type adaptation to support cv::cuda::GpuMat #519

Open
wants to merge 1 commit into
base: rolling
Choose a base branch
from
Open

feat: extend type adaptation to support cv::cuda::GpuMat #519

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
92 changes: 51 additions & 41 deletions cv_bridge/include/cv_bridge/cv_mat_sensor_msgs_image_type_adapter.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,17 +17,18 @@

#include <cstddef>
#include <memory>
#include <optional>
#include <variant>
#include <string>

#include "opencv2/core/mat.hpp"
#include "opencv2/core/cuda.hpp"

#include "rclcpp/type_adapter.hpp"
#include "sensor_msgs/msg/image.hpp"

#include "cv_bridge/visibility_control.h"

#include <optional>

namespace cv_bridge
{
namespace detail
Expand Down Expand Up @@ -94,28 +95,48 @@ class ROSCvMatContainer
std::shared_ptr<sensor_msgs::msg::Image>
>;

using CvImageStorageType = std::variant<
std::nullptr_t,
cv::Mat,
cv::cuda::GpuMat
>;

using AdaptedType = rclcpp::TypeAdapter<ROSCvMatContainer, sensor_msgs::msg::Image>;

CV_BRIDGE_PUBLIC
ROSCvMatContainer() = default;

CV_BRIDGE_PUBLIC
explicit ROSCvMatContainer(const ROSCvMatContainer & other)
: header_(other.header_), frame_(other.frame_.clone()), is_bigendian_(other.is_bigendian_)
: header_(other.header_), is_bigendian_(other.is_bigendian_)
{
if (std::holds_alternative<std::shared_ptr<sensor_msgs::msg::Image>>(other.storage_)) {
storage_ = std::get<std::shared_ptr<sensor_msgs::msg::Image>>(other.storage_);
} else if (std::holds_alternative<std::unique_ptr<sensor_msgs::msg::Image>>(other.storage_)) {
storage_ = std::make_unique<sensor_msgs::msg::Image>(
*std::get<std::unique_ptr<sensor_msgs::msg::Image>>(other.storage_));
}

if (std::holds_alternative<cv::Mat>(other.frame_)) {
frame_ = std::get<cv::Mat>(other.frame_).clone();
} else if (std::holds_alternative<cv::cuda::GpuMat>(other.frame_)) {
frame_ = std::get<cv::cuda::GpuMat>(other.frame_).clone();
}
}

CV_BRIDGE_PUBLIC
ROSCvMatContainer & operator=(const ROSCvMatContainer & other)
{
if (this != &other) {
header_ = other.header_;
frame_ = other.frame_.clone();
is_bigendian_ = other.is_bigendian_;

if (std::holds_alternative<cv::Mat>(other.frame_)) {
frame_ = std::get<cv::Mat>(other.frame_).clone();
} else if (std::holds_alternative<cv::cuda::GpuMat>(other.frame_)) {
frame_ = std::get<cv::cuda::GpuMat>(other.frame_).clone();
}

if (std::holds_alternative<std::shared_ptr<sensor_msgs::msg::Image>>(other.storage_)) {
storage_ = std::get<std::shared_ptr<sensor_msgs::msg::Image>>(other.storage_);
} else if (std::holds_alternative<std::unique_ptr<sensor_msgs::msg::Image>>(other.storage_)) {
Expand All @@ -136,18 +157,18 @@ class ROSCvMatContainer
CV_BRIDGE_PUBLIC
explicit ROSCvMatContainer(std::shared_ptr<sensor_msgs::msg::Image> shared_sensor_msgs_image);

/// Shallow copy the given cv::Mat into this class, but do not own the data directly.
/// Move the given cv::Mat if passed by r-value reference, otherwise shallow copy it.
CV_BRIDGE_PUBLIC
ROSCvMatContainer(
const cv::Mat & mat_frame,
cv::Mat mat_frame,
const std_msgs::msg::Header & header,
bool is_bigendian = is_bigendian_system,
std::optional<std::string> encoding_override = std::nullopt);

/// Move the given cv::Mat into this class.
/// Move the given cv::cuda::GpuMat if passed by r-value reference, otherwise shallow copy it.
CV_BRIDGE_PUBLIC
ROSCvMatContainer(
cv::Mat && mat_frame,
cv::cuda::GpuMat mat_frame,
const std_msgs::msg::Header & header,
bool is_bigendian = is_bigendian_system,
std::optional<std::string> encoding_override = std::nullopt);
Expand All @@ -165,6 +186,15 @@ class ROSCvMatContainer
bool
is_owning() const;

/// Return true if this class held cv type matches passed type
template<class T>
CV_BRIDGE_PUBLIC
bool
holds_cv_type() const
{
return std::holds_alternative<T>(frame_);
}

/// Const access the cv::Mat in this class.
CV_BRIDGE_PUBLIC
const cv::Mat &
Expand All @@ -179,6 +209,16 @@ class ROSCvMatContainer
cv::Mat
cv_mat();

/// Const access the cv::Mat in this class.
CV_BRIDGE_PUBLIC
const cv::cuda::GpuMat &
cv_gpu_mat() const;

/// Get a shallow copy of the cv::cuda::Mat that is in this class.
CV_BRIDGE_PUBLIC
cv::cuda::GpuMat
cv_gpu_mat();

/// Const access the ROS Header.
CV_BRIDGE_PUBLIC
const std_msgs::msg::Header &
Expand Down Expand Up @@ -213,15 +253,15 @@ class ROSCvMatContainer
CV_BRIDGE_PUBLIC
bool
is_bigendian() const;

/// Return the encoding override if provided.
CV_BRIDGE_PUBLIC
std::optional<std::string>
encoding_override() const;

private:
std_msgs::msg::Header header_;
cv::Mat frame_;
CvImageStorageType frame_;
SensorMsgsImageStorageType storage_;
bool is_bigendian_;
std::optional<std::string> encoding_override_;
Expand All @@ -242,37 +282,7 @@ struct rclcpp::TypeAdapter<cv_bridge::ROSCvMatContainer, sensor_msgs::msg::Image
const custom_type & source,
ros_message_type & destination)
{
destination.height = source.cv_mat().rows;
destination.width = source.cv_mat().cols;
const auto& encoding_override = source.encoding_override();
if (encoding_override.has_value() && !encoding_override.value().empty())
{
destination.encoding = encoding_override.value();
}
else
{
switch (source.cv_mat().type()) {
case CV_8UC1:
destination.encoding = "mono8";
break;
case CV_8UC3:
destination.encoding = "bgr8";
break;
case CV_16SC1:
destination.encoding = "mono16";
break;
case CV_8UC4:
destination.encoding = "rgba8";
break;
default:
throw std::runtime_error("unsupported encoding type");
}
}
destination.step = static_cast<sensor_msgs::msg::Image::_step_type>(source.cv_mat().step);
size_t size = source.cv_mat().step * source.cv_mat().rows;
destination.data.resize(size);
memcpy(&destination.data[0], source.cv_mat().data, size);
destination.header = source.header();
source.get_sensor_msgs_msg_image_copy(destination);
}

static
Expand Down
76 changes: 47 additions & 29 deletions cv_bridge/src/cv_mat_sensor_msgs_image_type_adapter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -68,6 +68,16 @@ struct NotNull
const T * pointer;
};

cv::Mat
matFromImage(sensor_msgs::msg::Image & image)
{
return {static_cast<int>(image.height),
static_cast<int>(image.width),
encoding2mat_type(image.encoding),
image.data.data(),
image.step};
}

} // namespace

ROSCvMatContainer::ROSCvMatContainer(
Expand All @@ -76,46 +86,36 @@ ROSCvMatContainer::ROSCvMatContainer(
unique_sensor_msgs_image.get(),
"unique_sensor_msgs_image cannot be nullptr"
).pointer->header),
frame_(
unique_sensor_msgs_image->height,
unique_sensor_msgs_image->width,
encoding2mat_type(unique_sensor_msgs_image->encoding),
unique_sensor_msgs_image->data.data(),
unique_sensor_msgs_image->step),
frame_(matFromImage(*unique_sensor_msgs_image)),
storage_(std::move(unique_sensor_msgs_image))
{}

ROSCvMatContainer::ROSCvMatContainer(
std::shared_ptr<sensor_msgs::msg::Image> shared_sensor_msgs_image)
: header_(shared_sensor_msgs_image->header),
frame_(
shared_sensor_msgs_image->height,
shared_sensor_msgs_image->width,
encoding2mat_type(shared_sensor_msgs_image->encoding),
shared_sensor_msgs_image->data.data(),
shared_sensor_msgs_image->step),
frame_(matFromImage(*shared_sensor_msgs_image)),
storage_(shared_sensor_msgs_image)
{}

ROSCvMatContainer::ROSCvMatContainer(
const cv::Mat & mat_frame,
cv::Mat mat_frame,
const std_msgs::msg::Header & header,
bool is_bigendian,
std::optional<std::string> encoding_override)
: header_(header),
frame_(mat_frame),
frame_(std::move(mat_frame)),
storage_(nullptr),
is_bigendian_(is_bigendian),
encoding_override_(encoding_override)
{}

ROSCvMatContainer::ROSCvMatContainer(
cv::Mat && mat_frame,
cv::cuda::GpuMat mat_frame,
const std_msgs::msg::Header & header,
bool is_bigendian,
std::optional<std::string> encoding_override)
: header_(header),
frame_(std::forward<cv::Mat>(mat_frame)),
frame_(std::move(mat_frame)),
storage_(nullptr),
is_bigendian_(is_bigendian),
encoding_override_(encoding_override)
Expand All @@ -135,13 +135,25 @@ ROSCvMatContainer::is_owning() const
const cv::Mat &
ROSCvMatContainer::cv_mat() const
{
return frame_;
return std::get<cv::Mat>(frame_);
}

cv::Mat
ROSCvMatContainer::cv_mat()
{
return frame_;
return std::get<cv::Mat>(frame_);
}

cv::cuda::GpuMat
ROSCvMatContainer::cv_gpu_mat()
{
return std::get<cv::cuda::GpuMat>(frame_);
}

const cv::cuda::GpuMat &
ROSCvMatContainer::cv_gpu_mat() const
{
return std::get<cv::cuda::GpuMat>(frame_);
}

const std_msgs::msg::Header &
Expand Down Expand Up @@ -177,15 +189,21 @@ void
ROSCvMatContainer::get_sensor_msgs_msg_image_copy(
sensor_msgs::msg::Image & sensor_msgs_image) const
{
sensor_msgs_image.height = frame_.rows;
sensor_msgs_image.width = frame_.cols;
if (encoding_override_.has_value() && !encoding_override_.value().empty())
{
sensor_msgs_image.encoding = encoding_override_.value();
cv::Mat sourceMat;
if (holds_cv_type<cv::Mat>()) {
sourceMat = cv_mat();
} else if (holds_cv_type<cv::cuda::GpuMat>()) {
cv_gpu_mat().download(sourceMat);
} else {
throw std::runtime_error("unsupported cv type");
}
else
{
switch (frame_.type()) {

sensor_msgs_image.height = sourceMat.rows;
sensor_msgs_image.width = sourceMat.cols;
if (encoding_override_.has_value() && !encoding_override_.value().empty()) {
sensor_msgs_image.encoding = encoding_override_.value();
} else {
switch (sourceMat.type()) {
case CV_8UC1:
sensor_msgs_image.encoding = "mono8";
break;
Expand All @@ -202,10 +220,10 @@ ROSCvMatContainer::get_sensor_msgs_msg_image_copy(
throw std::runtime_error("unsupported encoding type");
}
}
sensor_msgs_image.step = static_cast<sensor_msgs::msg::Image::_step_type>(frame_.step);
size_t size = frame_.step * frame_.rows;
sensor_msgs_image.step = static_cast<sensor_msgs::msg::Image::_step_type>(sourceMat.step);
size_t size = sourceMat.step * sourceMat.rows;
sensor_msgs_image.data.resize(size);
memcpy(&sensor_msgs_image.data[0], frame_.data, size);
memcpy(&sensor_msgs_image.data[0], sourceMat.data, size);
sensor_msgs_image.header = header_;
}

Expand Down