Loading files
This file is pinned to the selected release. Download the original to open it in your design tool.
#include <algorithm>
#include <array>
#include <cmath>
#include <cstdint>
#include <deque>
#include <limits>
#include <memory>
#include <mutex>
#include <stdexcept>
#include <string>
#include <vector>
#ifdef ROS_HUMBLE
#include <cv_bridge/cv_bridge.h>
#else
#include <cv_bridge/cv_bridge.hpp>
#endif
#include <kalman_interfaces/msg/instance_contour_array.hpp>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/synchronizer.h>
#include <opencv2/calib3d.hpp>
#include <opencv2/imgproc.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_components/register_node_macro.hpp>
#include <sensor_msgs/image_encodings.hpp>
#include <sensor_msgs/msg/camera_info.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <std_msgs/msg/float64_multi_array.hpp>
#include <tf2/LinearMath/Transform.hpp>
#include <vision_msgs/msg/detection2_d_array.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <yaml-cpp/yaml.h>
namespace kalman_arm2 {
class PanelRectifier : public rclcpp::Node {
public:
explicit PanelRectifier(const rclcpp::NodeOptions &options)
: Node("panel_rectifier", options) {
const std::string layout_yaml = declare_parameter<std::string>(
"layout_yaml", "panel_layout.yaml"
);
board_frame_ = declare_parameter<std::string>(
"board_frame", "aruco_board"
);
const std::string image_topic = declare_parameter<std::string>(
"image_topic", "/d455_arm_wheel/color/image_raw"
);
const std::string depth_topic = declare_parameter<std::string>(
"depth_topic", "/d455_arm_wheel/depth/image_raw"
);
const std::string camera_info_topic = declare_parameter<std::string>(
"camera_info_topic", "/d455_arm_wheel/color/camera_info"
);
const std::string output_topic = declare_parameter<std::string>(
"output_topic", "panel/image_rectified"
);
const std::string height_topic = declare_parameter<std::string>(
"height_topic", "panel/height"
);
const std::string valid_topic = declare_parameter<std::string>(
"valid_topic", "panel/valid"
);
const std::string pixel_transform_topic = declare_parameter<std::string>(
"pixel_transform_topic", "panel/homography"
);
const std::string contour_topic = declare_parameter<std::string>(
"contour_topic", "/d455_arm_wheel/yolo_contours"
);
const std::string segmentation_debug_topic =
declare_parameter<std::string>(
"segmentation_debug_topic", "panel/segmentation_debug"
);
const std::string rectified_detections_topic =
declare_parameter<std::string>(
"rectified_detections_topic", "panel/detections_rectified"
);
projection_cache_size_ = declare_parameter<int>(
"projection_cache_size", 30
);
sync_queue_size_ = declare_parameter<int>("sync_queue_size", 30);
sync_tolerance_s_ = declare_parameter<double>("sync_tolerance_s", 0.02);
pixels_per_meter_ = declare_parameter<double>("pixels_per_meter", 1000.0);
tf_timeout_s_ = declare_parameter<double>("tf_timeout_s", 0.1);
use_latest_transform_ = declare_parameter<bool>(
"use_latest_transform", true
);
min_panel_height_m_ = declare_parameter<double>(
"min_panel_height_m", -0.03
);
max_panel_height_m_ = declare_parameter<double>(
"max_panel_height_m", 0.25
);
depth_edge_threshold_m_ = declare_parameter<double>(
"depth_edge_threshold_m", 0.02
);
max_splat_radius_px_ = declare_parameter<int>(
"max_splat_radius_px", 3
);
load_layout(layout_yaml);
left_border_m_ = declare_parameter<double>(
"left_border_m", marker_size_
);
top_border_m_ = declare_parameter<double>(
"top_border_m", marker_size_
);
draw_board_outline_ = declare_parameter<bool>(
"draw_board_outline", true
);
outline_thickness_px_ = declare_parameter<int>(
"outline_thickness_px", 2
);
validate_parameters();
configure_output_geometry();
tf_buffer_ = std::make_unique<tf2_ros::Buffer>(get_clock());
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
rclcpp::QoS input_qos(5);
input_qos.durability(RMW_QOS_POLICY_DURABILITY_VOLATILE);
input_qos.reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT);
image_sub_.subscribe(this, image_topic, input_qos.get_rmw_qos_profile());
depth_sub_.subscribe(this, depth_topic, input_qos.get_rmw_qos_profile());
camera_info_sub_ = create_subscription<sensor_msgs::msg::CameraInfo>(
camera_info_topic,
input_qos,
std::bind(
&PanelRectifier::on_camera_info, this, std::placeholders::_1
)
);
synchronizer_ = std::make_shared<Synchronizer>(
SynchronizationPolicy(sync_queue_size_), image_sub_, depth_sub_
);
synchronizer_->setMaxIntervalDuration(
rclcpp::Duration::from_seconds(sync_tolerance_s_)
);
synchronizer_->registerCallback(std::bind(
&PanelRectifier::on_rgbd,
this,
std::placeholders::_1,
std::placeholders::_2
));
const auto output_qos = rclcpp::SensorDataQoS();
image_pub_ = create_publisher<sensor_msgs::msg::Image>(
output_topic, output_qos
);
height_pub_ = create_publisher<sensor_msgs::msg::Image>(
height_topic, output_qos
);
valid_pub_ = create_publisher<sensor_msgs::msg::Image>(
valid_topic, output_qos
);
segmentation_debug_pub_ = create_publisher<sensor_msgs::msg::Image>(
segmentation_debug_topic, output_qos
);
rectified_detections_pub_ =
create_publisher<vision_msgs::msg::Detection2DArray>(
rectified_detections_topic, rclcpp::QoS(10).reliable()
);
contour_sub_ = create_subscription<
kalman_interfaces::msg::InstanceContourArray>(
contour_topic,
output_qos,
std::bind(
&PanelRectifier::on_contours, this, std::placeholders::_1
)
);
auto transform_qos = rclcpp::QoS(1).reliable().transient_local();
pixel_transform_pub_ =
create_publisher<std_msgs::msg::Float64MultiArray>(
pixel_transform_topic, transform_qos
);
publish_pixel_transform();
RCLCPP_INFO(
get_logger(),
"Orthographic panel renderer: %.3f x %.3f m with left/top border "
"%.3f/%.3f m -> %d x %d px at %.1f px/m; approximate RGB-D sync "
"queue=%d tolerance=%.3f s",
board_width_,
board_height_,
left_border_m_,
top_border_m_,
output_width_,
output_height_,
pixels_per_meter_,
sync_queue_size_,
sync_tolerance_s_
);
}
private:
using SynchronizationPolicy =
message_filters::sync_policies::ApproximateTime<
sensor_msgs::msg::Image,
sensor_msgs::msg::Image>;
using Synchronizer = message_filters::Synchronizer<SynchronizationPolicy>;
struct ProjectionCacheEntry {
builtin_interfaces::msg::Time stamp;
cv::Mat image;
cv::Mat source_indices;
int source_width{0};
int source_height{0};
};
struct CalibrationData {
cv::Mat map_x;
cv::Mat map_y;
cv::Mat camera_matrix;
cv::Size image_size;
std::array<double, 9> intrinsics;
std::vector<double> distortion;
std::string distortion_model;
std::vector<float> ray_x;
std::vector<float> ray_y;
};
void load_layout(const std::string &layout_yaml) {
try {
const YAML::Node config = YAML::LoadFile(layout_yaml);
board_width_ = config["board_width"].as<double>();
board_height_ = config["board_height"].as<double>();
marker_size_ = config["marker_size"].as<double>();
} catch (const std::exception &error) {
RCLCPP_FATAL(
get_logger(),
"Failed to load panel layout '%s': %s",
layout_yaml.c_str(),
error.what()
);
throw;
}
}
void validate_parameters() const {
if (!std::isfinite(board_width_) || board_width_ <= 0.0 ||
!std::isfinite(board_height_) || board_height_ <= 0.0 ||
!std::isfinite(marker_size_) || marker_size_ <= 0.0) {
throw std::invalid_argument(
"panel dimensions and marker_size must be finite and positive"
);
}
if (!std::isfinite(pixels_per_meter_) || pixels_per_meter_ <= 0.0) {
throw std::invalid_argument("pixels_per_meter must be finite and positive");
}
if (!std::isfinite(tf_timeout_s_) || tf_timeout_s_ < 0.0) {
throw std::invalid_argument("tf_timeout_s must be finite and non-negative");
}
if (!std::isfinite(left_border_m_) || left_border_m_ < 0.0 ||
!std::isfinite(top_border_m_) || top_border_m_ < 0.0) {
throw std::invalid_argument("panel borders must be finite and non-negative");
}
if (!std::isfinite(min_panel_height_m_) ||
!std::isfinite(max_panel_height_m_) ||
min_panel_height_m_ >= max_panel_height_m_) {
throw std::invalid_argument("invalid panel height range");
}
if (!std::isfinite(depth_edge_threshold_m_) ||
depth_edge_threshold_m_ <= 0.0) {
throw std::invalid_argument(
"depth_edge_threshold_m must be finite and positive"
);
}
if (max_splat_radius_px_ < 0 || outline_thickness_px_ <= 0) {
throw std::invalid_argument("invalid rasterization pixel sizes");
}
if (projection_cache_size_ <= 0) {
throw std::invalid_argument("projection_cache_size must be positive");
}
if (sync_queue_size_ <= 0) {
throw std::invalid_argument("sync_queue_size must be positive");
}
if (!std::isfinite(sync_tolerance_s_) || sync_tolerance_s_ <= 0.0) {
throw std::invalid_argument(
"sync_tolerance_s must be finite and positive"
);
}
}
void configure_output_geometry() {
board_pixel_width_ = std::max(
2, static_cast<int>(std::lround(board_width_ * pixels_per_meter_))
);
board_pixel_height_ = std::max(
2, static_cast<int>(std::lround(board_height_ * pixels_per_meter_))
);
left_border_px_ = std::max(
0, static_cast<int>(std::lround(left_border_m_ * pixels_per_meter_))
);
top_border_px_ = std::max(
0, static_cast<int>(std::lround(top_border_m_ * pixels_per_meter_))
);
output_width_ = left_border_px_ + board_pixel_width_;
output_height_ = top_border_px_ + board_pixel_height_;
}
std::shared_ptr<const CalibrationData> calibration_for(
const sensor_msgs::msg::CameraInfo &message
) {
if (message.width == 0 || message.height == 0) {
throw std::runtime_error("CameraInfo has zero image dimensions");
}
if (calibration_ &&
calibration_->image_size.width == static_cast<int>(message.width) &&
calibration_->image_size.height == static_cast<int>(message.height) &&
calibration_->intrinsics == message.k &&
calibration_->distortion == message.d &&
calibration_->distortion_model == message.distortion_model) {
return calibration_;
}
auto data = std::make_shared<CalibrationData>();
data->camera_matrix = cv::Mat(3, 3, CV_64F);
for (size_t index = 0; index < message.k.size(); ++index) {
data->camera_matrix.at<double>(
static_cast<int>(index / 3), static_cast<int>(index % 3)
) = message.k[index];
}
const double focal_x = data->camera_matrix.at<double>(0, 0);
const double focal_y = data->camera_matrix.at<double>(1, 1);
if (focal_x <= 0.0 || focal_y <= 0.0) {
throw std::runtime_error("CameraInfo has invalid focal lengths");
}
cv::Mat distortion(
1, static_cast<int>(message.d.size()), CV_64F, cv::Scalar(0.0)
);
for (size_t index = 0; index < message.d.size(); ++index) {
distortion.at<double>(0, static_cast<int>(index)) = message.d[index];
}
data->image_size = cv::Size(
static_cast<int>(message.width), static_cast<int>(message.height)
);
data->intrinsics = message.k;
data->distortion = message.d;
data->distortion_model = message.distortion_model;
const cv::Mat identity = cv::Mat::eye(3, 3, CV_64F);
if (message.distortion_model == "equidistant") {
cv::fisheye::initUndistortRectifyMap(
data->camera_matrix,
distortion,
identity,
data->camera_matrix,
data->image_size,
CV_32FC1,
data->map_x,
data->map_y
);
} else {
cv::initUndistortRectifyMap(
data->camera_matrix,
distortion,
identity,
data->camera_matrix,
data->image_size,
CV_32FC1,
data->map_x,
data->map_y
);
}
const double center_x = data->camera_matrix.at<double>(0, 2);
const double center_y = data->camera_matrix.at<double>(1, 2);
data->ray_x.resize(message.width);
data->ray_y.resize(message.height);
for (size_t column = 0; column < message.width; ++column) {
data->ray_x[column] = static_cast<float>(
(static_cast<double>(column) - center_x) / focal_x
);
}
for (size_t row = 0; row < message.height; ++row) {
data->ray_y[row] = static_cast<float>(
(static_cast<double>(row) - center_y) / focal_y
);
}
calibration_ = data;
return data;
}
tf2::Transform board_from_camera(
const std::string &camera_frame, const builtin_interfaces::msg::Time &stamp
) const {
const rclcpp::Time transform_time = use_latest_transform_
? rclcpp::Time(
0,
0,
get_clock()->get_clock_type()
)
: rclcpp::Time(stamp);
const auto transform = tf_buffer_->lookupTransform(
board_frame_,
camera_frame,
transform_time,
rclcpp::Duration::from_seconds(tf_timeout_s_)
);
tf2::Transform result;
tf2::fromMsg(transform.transform, result);
return result;
}
cv::Mat depth_in_meters(
const sensor_msgs::msg::Image::ConstSharedPtr &message
) const {
const cv_bridge::CvImageConstPtr depth = cv_bridge::toCvShare(message);
cv::Mat meters;
if (message->encoding == sensor_msgs::image_encodings::TYPE_16UC1 ||
message->encoding == sensor_msgs::image_encodings::MONO16) {
depth->image.convertTo(meters, CV_32FC1, 0.001);
} else if (
message->encoding == sensor_msgs::image_encodings::TYPE_32FC1
) {
meters = depth->image;
} else {
throw std::runtime_error(
"unsupported depth encoding '" + message->encoding + "'"
);
}
return meters;
}
static bool valid_depth(float depth) {
return std::isfinite(depth) && depth > 0.0F;
}
bool is_depth_edge(const cv::Mat &depth, int row, int column, float value) const {
constexpr std::array<std::array<int, 2>, 4> offsets{
std::array<int, 2>{-1, 0},
std::array<int, 2>{1, 0},
std::array<int, 2>{0, -1},
std::array<int, 2>{0, 1},
};
for (const auto &offset : offsets) {
const int neighbor_row = row + offset[0];
const int neighbor_column = column + offset[1];
if (neighbor_row < 0 || neighbor_row >= depth.rows ||
neighbor_column < 0 || neighbor_column >= depth.cols) {
return true;
}
const float neighbor = depth.at<float>(neighbor_row, neighbor_column);
if (!valid_depth(neighbor) ||
std::abs(neighbor - value) > depth_edge_threshold_m_) {
return true;
}
}
return false;
}
void fill_unknown_background(cv::Mat &image) const {
constexpr int tile_size = 12;
for (int row = 0; row < image.rows; ++row) {
cv::Vec3b *pixels = image.ptr<cv::Vec3b>(row);
for (int column = 0; column < image.cols; ++column) {
const bool alternate =
((row / tile_size) + (column / tile_size)) % 2 != 0;
const unsigned char intensity = alternate ? 24 : 40;
pixels[column] = cv::Vec3b(intensity, intensity, intensity);
}
}
}
void render_points(
const cv::Mat &color,
const cv::Mat &depth,
const CalibrationData &calibration,
const tf2::Transform &board_from_camera,
cv::Mat &output,
cv::Mat &height,
cv::Mat &valid,
cv::Mat &source_indices
) const {
const double minimum_x = -0.5 * board_width_ - left_border_m_;
const double maximum_x = 0.5 * board_width_;
const double minimum_y = -0.5 * board_height_;
const double maximum_y = 0.5 * board_height_ + top_border_m_;
const double average_focal_length = 0.5 * (
calibration.camera_matrix.at<double>(0, 0) +
calibration.camera_matrix.at<double>(1, 1)
);
for (int row = 0; row < depth.rows; ++row) {
const float *depth_pixels = depth.ptr<float>(row);
const cv::Vec3b *color_pixels = color.ptr<cv::Vec3b>(row);
for (int column = 0; column < depth.cols; ++column) {
const float camera_depth = depth_pixels[column];
if (!valid_depth(camera_depth)) {
continue;
}
const tf2::Vector3 camera_point(
calibration.ray_x[static_cast<size_t>(column)] * camera_depth,
calibration.ray_y[static_cast<size_t>(row)] * camera_depth,
camera_depth
);
const tf2::Vector3 panel_point =
board_from_camera * camera_point;
if (panel_point.x() < minimum_x || panel_point.x() > maximum_x ||
panel_point.y() < minimum_y || panel_point.y() > maximum_y ||
panel_point.z() < min_panel_height_m_ ||
panel_point.z() > max_panel_height_m_) {
continue;
}
const int output_x = static_cast<int>(std::lround(
left_border_px_ +
(panel_point.x() + 0.5 * board_width_) * pixels_per_meter_
));
const int output_y = static_cast<int>(std::lround(
top_border_px_ +
(0.5 * board_height_ - panel_point.y()) * pixels_per_meter_
));
if (output_x < 0 || output_x >= output_width_ || output_y < 0 ||
output_y >= output_height_) {
continue;
}
int radius = static_cast<int>(std::ceil(
0.5 * pixels_per_meter_ * camera_depth /
average_focal_length
));
radius = std::clamp(radius, 0, max_splat_radius_px_);
if (is_depth_edge(depth, row, column, camera_depth)) {
radius = 0;
}
const float mapped_x = calibration.map_x.at<float>(row, column);
const float mapped_y = calibration.map_y.at<float>(row, column);
int raw_index = -1;
if (std::isfinite(mapped_x) && std::isfinite(mapped_y) &&
mapped_x > -0.5F &&
mapped_x < static_cast<float>(calibration.image_size.width) -
0.5F &&
mapped_y > -0.5F &&
mapped_y < static_cast<float>(calibration.image_size.height) -
0.5F) {
const int raw_x = static_cast<int>(std::lround(mapped_x));
const int raw_y = static_cast<int>(std::lround(mapped_y));
raw_index = raw_y * calibration.image_size.width + raw_x;
}
for (int target_y = std::max(0, output_y - radius);
target_y <= std::min(output_height_ - 1, output_y + radius);
++target_y) {
float *height_pixels = height.ptr<float>(target_y);
unsigned char *valid_pixels = valid.ptr<unsigned char>(target_y);
cv::Vec3b *output_pixels = output.ptr<cv::Vec3b>(target_y);
int *source_index_pixels = source_indices.ptr<int>(target_y);
for (int target_x = std::max(0, output_x - radius);
target_x <= std::min(output_width_ - 1, output_x + radius);
++target_x) {
if (valid_pixels[target_x] == 0 ||
panel_point.z() > height_pixels[target_x]) {
height_pixels[target_x] = static_cast<float>(
panel_point.z()
);
valid_pixels[target_x] = 255;
output_pixels[target_x] = color_pixels[column];
source_index_pixels[target_x] = raw_index;
}
}
}
}
}
}
static bool same_stamp(
const builtin_interfaces::msg::Time &left,
const builtin_interfaces::msg::Time &right
) {
return left.sec == right.sec && left.nanosec == right.nanosec;
}
static cv::Vec3b color_for_class(const std::string &class_id) {
uint32_t hash = 2166136261U;
for (const unsigned char byte : class_id) {
hash ^= byte;
hash *= 16777619U;
}
cv::Mat hsv(1, 1, CV_8UC3, cv::Scalar(hash % 180U, 220, 255));
cv::Mat bgr;
cv::cvtColor(hsv, bgr, cv::COLOR_HSV2BGR);
return bgr.at<cv::Vec3b>(0, 0);
}
void cache_projection(
const builtin_interfaces::msg::Time &stamp,
const cv::Mat &image,
const cv::Mat &source_indices,
int source_width,
int source_height
) {
ProjectionCacheEntry entry{
stamp,
image.clone(),
source_indices.clone(),
source_width,
source_height,
};
std::lock_guard<std::mutex> lock(projection_cache_mutex_);
projection_cache_.push_back(std::move(entry));
while (projection_cache_.size() >
static_cast<size_t>(projection_cache_size_)) {
projection_cache_.pop_front();
}
}
void on_contours(
const kalman_interfaces::msg::InstanceContourArray::ConstSharedPtr message
) {
ProjectionCacheEntry cached;
{
std::lock_guard<std::mutex> lock(projection_cache_mutex_);
const auto entry = std::find_if(
projection_cache_.begin(),
projection_cache_.end(),
[&message](const ProjectionCacheEntry &candidate) {
return same_stamp(candidate.stamp, message->header.stamp);
}
);
if (entry == projection_cache_.end()) {
RCLCPP_WARN_THROTTLE(
get_logger(),
*get_clock(),
2000,
"No orthographic projection cached for contour stamp %d.%09u",
message->header.stamp.sec,
message->header.stamp.nanosec
);
return;
}
cached = *entry;
}
if (message->image_width != static_cast<uint32_t>(cached.source_width) ||
message->image_height != static_cast<uint32_t>(cached.source_height)) {
RCLCPP_WARN_THROTTLE(
get_logger(),
*get_clock(),
2000,
"Contour dimensions %ux%u differ from cached source %dx%d",
message->image_width,
message->image_height,
cached.source_width,
cached.source_height
);
return;
}
cv::Mat labels(
cached.source_height,
cached.source_width,
CV_16UC1,
cv::Scalar(0)
);
std::vector<cv::Vec3b> label_colors(message->instances.size() + 1);
const size_t maximum_labels = std::min<size_t>(
message->instances.size(), std::numeric_limits<uint16_t>::max()
);
for (size_t index = 0; index < maximum_labels; ++index) {
const auto &instance = message->instances[index];
if (instance.points.size() < 3) {
continue;
}
std::vector<cv::Point> contour;
contour.reserve(instance.points.size());
for (const auto &point : instance.points) {
contour.emplace_back(point.x, point.y);
}
const uint16_t label = static_cast<uint16_t>(index + 1);
const std::vector<std::vector<cv::Point>> contours{std::move(contour)};
cv::fillPoly(labels, contours, cv::Scalar(label));
label_colors[label] = color_for_class(instance.class_id);
}
cv::Mat debug = cached.image.clone();
std::vector<int> minimum_x(message->instances.size() + 1, debug.cols);
std::vector<int> minimum_y(message->instances.size() + 1, debug.rows);
std::vector<int> maximum_x(message->instances.size() + 1, -1);
std::vector<int> maximum_y(message->instances.size() + 1, -1);
constexpr unsigned int overlay_weight = 128;
for (int row = 0; row < debug.rows; ++row) {
cv::Vec3b *debug_pixels = debug.ptr<cv::Vec3b>(row);
const int *source_pixels = cached.source_indices.ptr<int>(row);
for (int column = 0; column < debug.cols; ++column) {
const int source_index = source_pixels[column];
if (source_index < 0) {
continue;
}
const uint16_t label = labels.ptr<uint16_t>()[source_index];
if (label == 0 || label >= label_colors.size()) {
continue;
}
minimum_x[label] = std::min(minimum_x[label], column);
minimum_y[label] = std::min(minimum_y[label], row);
maximum_x[label] = std::max(maximum_x[label], column);
maximum_y[label] = std::max(maximum_y[label], row);
const cv::Vec3b color = label_colors[label];
for (int channel = 0; channel < 3; ++channel) {
debug_pixels[column][channel] = static_cast<unsigned char>(
(static_cast<unsigned int>(debug_pixels[column][channel]) *
(256U - overlay_weight) +
static_cast<unsigned int>(color[channel]) * overlay_weight) /
256U
);
}
}
}
std_msgs::msg::Header header = message->header;
header.frame_id = board_frame_;
vision_msgs::msg::Detection2DArray detections;
detections.header = header;
for (size_t index = 0; index < maximum_labels; ++index) {
const size_t label = index + 1;
if (maximum_x[label] < minimum_x[label] ||
maximum_y[label] < minimum_y[label]) {
continue;
}
const auto &instance = message->instances[index];
vision_msgs::msg::Detection2D detection;
detection.id = instance.id;
detection.bbox.center.position.x =
0.5 * static_cast<double>(minimum_x[label] + maximum_x[label]);
detection.bbox.center.position.y =
0.5 * static_cast<double>(minimum_y[label] + maximum_y[label]);
detection.bbox.size_x =
static_cast<double>(maximum_x[label] - minimum_x[label] + 1);
detection.bbox.size_y =
static_cast<double>(maximum_y[label] - minimum_y[label] + 1);
detection.results.resize(1);
detection.results[0].hypothesis.class_id = instance.class_id;
detection.results[0].hypothesis.score = instance.score;
detections.detections.push_back(std::move(detection));
}
rectified_detections_pub_->publish(detections);
segmentation_debug_pub_->publish(
*cv_bridge::CvImage(
header, sensor_msgs::image_encodings::BGR8, debug
)
.toImageMsg()
);
}
void publish_pixel_transform() {
std_msgs::msg::Float64MultiArray transform;
transform.layout.dim.resize(2);
transform.layout.dim[0].label = "rows";
transform.layout.dim[0].size = 3;
transform.layout.dim[0].stride = 9;
transform.layout.dim[1].label = "columns";
transform.layout.dim[1].size = 3;
transform.layout.dim[1].stride = 3;
const double meters_per_pixel = 1.0 / pixels_per_meter_;
transform.data = {
meters_per_pixel,
0.0,
-static_cast<double>(left_border_px_) * meters_per_pixel -
0.5 * board_width_,
0.0,
-meters_per_pixel,
static_cast<double>(top_border_px_) * meters_per_pixel +
0.5 * board_height_,
0.0,
0.0,
1.0,
};
pixel_transform_pub_->publish(transform);
}
void publish_outputs(
const std_msgs::msg::Header &input_header,
const cv::Mat &image,
const cv::Mat &height,
const cv::Mat &valid
) {
std_msgs::msg::Header output_header = input_header;
output_header.frame_id = board_frame_;
image_pub_->publish(
*cv_bridge::CvImage(
output_header, sensor_msgs::image_encodings::BGR8, image
)
.toImageMsg()
);
height_pub_->publish(
*cv_bridge::CvImage(
output_header, sensor_msgs::image_encodings::TYPE_32FC1, height
)
.toImageMsg()
);
valid_pub_->publish(
*cv_bridge::CvImage(
output_header, sensor_msgs::image_encodings::MONO8, valid
)
.toImageMsg()
);
// Transient-local data is not replayed to volatile rosbridge subscribers.
// Publish alongside every image so late ground-station clients receive it.
publish_pixel_transform();
}
void on_camera_info(
const sensor_msgs::msg::CameraInfo::ConstSharedPtr message
) {
std::lock_guard<std::mutex> lock(camera_info_mutex_);
latest_camera_info_ = message;
}
void on_rgbd(
const sensor_msgs::msg::Image::ConstSharedPtr &image_message,
const sensor_msgs::msg::Image::ConstSharedPtr &depth_message
) {
try {
sensor_msgs::msg::CameraInfo::ConstSharedPtr camera_info_message;
{
std::lock_guard<std::mutex> lock(camera_info_mutex_);
camera_info_message = latest_camera_info_;
}
if (!camera_info_message) {
RCLCPP_WARN_THROTTLE(
get_logger(),
*get_clock(),
2000,
"Cannot render orthographic panel before camera info arrives"
);
return;
}
if (image_message->width != depth_message->width ||
image_message->height != depth_message->height ||
image_message->width != camera_info_message->width ||
image_message->height != camera_info_message->height) {
RCLCPP_WARN_THROTTLE(
get_logger(),
*get_clock(),
2000,
"Aligned RGB-D dimensions differ: color=%ux%u depth=%ux%u "
"info=%ux%u",
image_message->width,
image_message->height,
depth_message->width,
depth_message->height,
camera_info_message->width,
camera_info_message->height
);
return;
}
const auto calibration = calibration_for(*camera_info_message);
const cv_bridge::CvImagePtr color = cv_bridge::toCvCopy(
image_message, sensor_msgs::image_encodings::BGR8
);
const cv::Mat depth_meters = depth_in_meters(depth_message);
cv::Mat rectified_color;
cv::Mat rectified_depth;
cv::remap(
color->image,
rectified_color,
calibration->map_x,
calibration->map_y,
cv::INTER_LINEAR
);
cv::remap(
depth_meters,
rectified_depth,
calibration->map_x,
calibration->map_y,
cv::INTER_NEAREST,
cv::BORDER_CONSTANT,
cv::Scalar(0.0)
);
const std::string camera_frame = image_message->header.frame_id.empty()
? camera_info_message->header.frame_id
: image_message->header.frame_id;
if (camera_frame.empty()) {
throw std::runtime_error("camera frame_id is empty");
}
const tf2::Transform board_from_camera_transform = board_from_camera(
camera_frame, image_message->header.stamp
);
cv::Mat output(
output_height_, output_width_, CV_8UC3
);
fill_unknown_background(output);
cv::Mat height(
output_height_,
output_width_,
CV_32FC1,
cv::Scalar(std::numeric_limits<float>::quiet_NaN())
);
cv::Mat valid(
output_height_, output_width_, CV_8UC1, cv::Scalar(0)
);
cv::Mat source_indices(
output_height_, output_width_, CV_32SC1, cv::Scalar(-1)
);
render_points(
rectified_color,
rectified_depth,
*calibration,
board_from_camera_transform,
output,
height,
valid,
source_indices
);
if (draw_board_outline_) {
cv::rectangle(
output,
cv::Point(left_border_px_, top_border_px_),
cv::Point(
left_border_px_ + board_pixel_width_ - 1,
top_border_px_ + board_pixel_height_ - 1
),
cv::Scalar(0, 255, 0),
outline_thickness_px_,
cv::LINE_AA
);
}
cache_projection(
image_message->header.stamp,
output,
source_indices,
static_cast<int>(image_message->width),
static_cast<int>(image_message->height)
);
publish_outputs(image_message->header, output, height, valid);
} catch (const tf2::TransformException &error) {
RCLCPP_WARN_THROTTLE(
get_logger(),
*get_clock(),
1000,
"Orthographic panel TF lookup failed: %s",
error.what()
);
} catch (const cv_bridge::Exception &error) {
RCLCPP_ERROR_THROTTLE(
get_logger(),
*get_clock(),
2000,
"RGB-D conversion failed: %s",
error.what()
);
} catch (const cv::Exception &error) {
RCLCPP_ERROR_THROTTLE(
get_logger(),
*get_clock(),
2000,
"Orthographic rendering failed: %s",
error.what()
);
} catch (const std::exception &error) {
RCLCPP_WARN_THROTTLE(
get_logger(),
*get_clock(),
2000,
"Cannot render orthographic panel: %s",
error.what()
);
}
}
std::string board_frame_;
double board_width_{0.0};
double board_height_{0.0};
double marker_size_{0.0};
double pixels_per_meter_{1000.0};
double tf_timeout_s_{0.1};
double left_border_m_{0.0};
double top_border_m_{0.0};
double min_panel_height_m_{-0.03};
double max_panel_height_m_{0.25};
double depth_edge_threshold_m_{0.02};
bool use_latest_transform_{true};
bool draw_board_outline_{true};
int max_splat_radius_px_{3};
int outline_thickness_px_{2};
int board_pixel_width_{0};
int board_pixel_height_{0};
int left_border_px_{0};
int top_border_px_{0};
int output_width_{0};
int output_height_{0};
int projection_cache_size_{30};
int sync_queue_size_{30};
double sync_tolerance_s_{0.02};
std::shared_ptr<CalibrationData> calibration_;
std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
message_filters::Subscriber<sensor_msgs::msg::Image> image_sub_;
message_filters::Subscriber<sensor_msgs::msg::Image> depth_sub_;
rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr camera_info_sub_;
std::shared_ptr<Synchronizer> synchronizer_;
std::mutex camera_info_mutex_;
sensor_msgs::msg::CameraInfo::ConstSharedPtr latest_camera_info_;
rclcpp::Subscription<kalman_interfaces::msg::InstanceContourArray>::SharedPtr
contour_sub_;
std::mutex projection_cache_mutex_;
std::deque<ProjectionCacheEntry> projection_cache_;
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr image_pub_;
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr height_pub_;
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr valid_pub_;
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr segmentation_debug_pub_;
rclcpp::Publisher<vision_msgs::msg::Detection2DArray>::SharedPtr
rectified_detections_pub_;
rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr
pixel_transform_pub_;
};
} // namespace kalman_arm2
RCLCPP_COMPONENTS_REGISTER_NODE(kalman_arm2::PanelRectifier)
d060b5a20This file is pinned to the selected release. Download the original to open it in your design tool.