1243 lines
58 KiB
C++
1243 lines
58 KiB
C++
/*
|
|
Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
|
|
Licensed under the Apache License, Version 2.0 (the "License");
|
|
you may not use this file except in compliance with the License.
|
|
You may obtain a copy of the License at
|
|
http://www.apache.org/licenses/LICENSE-2.0
|
|
Unless required by applicable law or agreed to in writing, software
|
|
distributed under the License is distributed on an "AS IS" BASIS,
|
|
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
See the License for the specific language governing permissions and
|
|
limitations under the License.
|
|
*/
|
|
|
|
#include "cloud_reprojector.hpp"
|
|
#include "cloud_reprojection_processing.hpp"
|
|
|
|
#include <algorithm>
|
|
#include <array>
|
|
#include <chrono>
|
|
#include <cmath>
|
|
#include <cstdint>
|
|
#include <cstdio>
|
|
#include <filesystem>
|
|
#include <limits>
|
|
#include <map>
|
|
#include <memory>
|
|
#include <sstream>
|
|
#include <string>
|
|
#include <sys/stat.h>
|
|
#include <vector>
|
|
|
|
#include <Eigen/Dense>
|
|
#include <cv_bridge/cv_bridge.h>
|
|
#include <nav_msgs/msg/odometry.hpp>
|
|
#include <opencv2/imgcodecs.hpp>
|
|
#include <opencv2/imgproc.hpp>
|
|
#include <pcl_conversions/pcl_conversions.h>
|
|
#include <pcl/point_cloud.h>
|
|
#include <pcl/point_types.h>
|
|
#include <rclcpp/rclcpp.hpp>
|
|
#include <sensor_msgs/msg/compressed_image.hpp>
|
|
#include <sensor_msgs/msg/image.hpp>
|
|
#include <sensor_msgs/msg/point_cloud2.hpp>
|
|
#include <std_msgs/msg/color_rgba.hpp>
|
|
#include <visualization_msgs/msg/marker_array.hpp>
|
|
#include <yaml-cpp/yaml.h>
|
|
|
|
#include "odin_ros_driver/msg/target_observation.hpp"
|
|
#include "odin_ros_driver/msg/target_observation_array.hpp"
|
|
|
|
#ifdef ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION
|
|
#include "target_observation_processing.hpp"
|
|
#endif
|
|
|
|
namespace {
|
|
|
|
std::filesystem::path get_package_source_directory_from_file()
|
|
{
|
|
return std::filesystem::path(__FILE__).parent_path().parent_path();
|
|
}
|
|
|
|
std::filesystem::path get_target_prediction_root_directory()
|
|
{
|
|
return get_package_source_directory_from_file().parent_path().parent_path().parent_path().parent_path();
|
|
}
|
|
|
|
std::string resolve_camera_topic(std::string topic, bool compressed)
|
|
{
|
|
const std::string suffix = "/compressed";
|
|
if (compressed) {
|
|
if (topic.size() < suffix.size() ||
|
|
topic.compare(topic.size() - suffix.size(), suffix.size(), suffix) != 0) {
|
|
topic += suffix;
|
|
}
|
|
} else if (
|
|
topic.size() >= suffix.size() &&
|
|
topic.compare(topic.size() - suffix.size(), suffix.size(), suffix) == 0) {
|
|
topic.resize(topic.size() - suffix.size());
|
|
}
|
|
return topic;
|
|
}
|
|
|
|
std::string format_vector3f(const Eigen::Vector3f& value)
|
|
{
|
|
std::ostringstream oss;
|
|
oss.setf(std::ios::fixed);
|
|
oss.precision(2);
|
|
oss << "[" << value.x() << ", " << value.y() << ", " << value.z() << "]";
|
|
return oss.str();
|
|
}
|
|
|
|
std::string format_bbox_xyxy(const std::array<float, 4>& bbox)
|
|
{
|
|
std::ostringstream oss;
|
|
oss.setf(std::ios::fixed);
|
|
oss.precision(1);
|
|
oss << "[" << bbox[0] << ", " << bbox[1] << ", " << bbox[2] << ", " << bbox[3] << "]";
|
|
return oss.str();
|
|
}
|
|
|
|
std::string format_int_vec(const std::vector<int>& v)
|
|
{
|
|
std::ostringstream oss;
|
|
oss << "[";
|
|
for (size_t i = 0; i < v.size(); ++i) {
|
|
if (i > 0) {
|
|
oss << ",";
|
|
}
|
|
oss << v[i];
|
|
}
|
|
oss << "]";
|
|
return oss.str();
|
|
}
|
|
|
|
std::string format_reid_comparisons(
|
|
const std::vector<int>& raw_track_ids,
|
|
const std::vector<std::tuple<int, int, float>>& comparisons)
|
|
{
|
|
std::map<int, std::vector<std::pair<int, float>>> grouped;
|
|
for (const int raw_id : raw_track_ids) {
|
|
grouped.try_emplace(raw_id);
|
|
}
|
|
for (const auto& c : comparisons) {
|
|
grouped[std::get<0>(c)].emplace_back(std::get<1>(c), std::get<2>(c));
|
|
}
|
|
if (grouped.empty()) {
|
|
return "[]";
|
|
}
|
|
|
|
std::ostringstream oss;
|
|
oss.setf(std::ios::fixed);
|
|
oss.precision(3);
|
|
oss << "[";
|
|
bool first_raw = true;
|
|
for (auto& kv : grouped) {
|
|
if (!first_raw) {
|
|
oss << ",";
|
|
}
|
|
first_raw = false;
|
|
std::sort(
|
|
kv.second.begin(), kv.second.end(),
|
|
[](const auto& a, const auto& b) { return a.second > b.second; });
|
|
oss << kv.first << "->[";
|
|
for (size_t i = 0; i < kv.second.size(); ++i) {
|
|
if (i > 0) {
|
|
oss << ",";
|
|
}
|
|
oss << "sid" << kv.second[i].first << ":" << kv.second[i].second;
|
|
}
|
|
oss << "]";
|
|
}
|
|
oss << "]";
|
|
return oss.str();
|
|
}
|
|
|
|
std::string format_keypoints_xyc(
|
|
const std::array<float, 17 * 3>& keypoints_xyc,
|
|
float min_confidence = 0.5f)
|
|
{
|
|
std::ostringstream oss;
|
|
oss.setf(std::ios::fixed);
|
|
oss.precision(2);
|
|
bool first = true;
|
|
for (int i = 0; i < 17; ++i) {
|
|
const float confidence = keypoints_xyc[static_cast<size_t>(i) * 3 + 2];
|
|
if (confidence < min_confidence) {
|
|
continue;
|
|
}
|
|
if (!first) {
|
|
oss << " ";
|
|
}
|
|
first = false;
|
|
oss << i << ":["
|
|
<< keypoints_xyc[static_cast<size_t>(i) * 3 + 0] << ", "
|
|
<< keypoints_xyc[static_cast<size_t>(i) * 3 + 1] << ", "
|
|
<< confidence << "]";
|
|
}
|
|
return first ? std::string("none") : oss.str();
|
|
}
|
|
|
|
std_msgs::msg::ColorRGBA stable_track_color_rgba(int track_id, float alpha = 0.9f)
|
|
{
|
|
static const std::array<std::array<float, 3>, 8> kPalette{{
|
|
{1.0f, 0.0f, 0.0f},
|
|
{1.0f, 0.5f, 0.0f},
|
|
{1.0f, 0.85f, 0.0f},
|
|
{0.0f, 0.8f, 0.0f},
|
|
{0.0f, 1.0f, 1.0f},
|
|
{0.0f, 0.35f, 1.0f},
|
|
{0.29f, 0.0f, 0.51f},
|
|
{0.58f, 0.0f, 0.83f},
|
|
}};
|
|
std_msgs::msg::ColorRGBA color;
|
|
if (track_id < 0) {
|
|
color.r = 1.0f;
|
|
color.g = 1.0f;
|
|
color.b = 1.0f;
|
|
} else {
|
|
const auto& c = kPalette[static_cast<size_t>(track_id) % kPalette.size()];
|
|
color.r = c[0];
|
|
color.g = c[1];
|
|
color.b = c[2];
|
|
}
|
|
color.a = alpha;
|
|
return color;
|
|
}
|
|
|
|
cv::Mat build_depth_vis_from_cloud_cam(
|
|
const pcl::PointCloud<pcl::PointXYZRGB>& cloud_cam,
|
|
const CloudReprojector& reprojector)
|
|
{
|
|
const auto& cam_params = reprojector.getCameraParams();
|
|
cv::Mat img(
|
|
cam_params.image_height,
|
|
cam_params.image_width,
|
|
CV_8UC3,
|
|
cv::Scalar(255, 255, 255));
|
|
|
|
float z_min = std::numeric_limits<float>::max();
|
|
float z_max = 0.0f;
|
|
for (const auto& pt : cloud_cam) {
|
|
if (pt.z > 0.01f) {
|
|
z_min = std::min(z_min, static_cast<float>(pt.z));
|
|
z_max = std::max(z_max, static_cast<float>(pt.z));
|
|
}
|
|
}
|
|
if (!std::isfinite(z_min) || z_max <= z_min) {
|
|
return img;
|
|
}
|
|
const float z_rng = std::max(1e-4f, z_max - z_min);
|
|
|
|
for (const auto& pt : cloud_cam) {
|
|
if (pt.z <= 0.01f) {
|
|
continue;
|
|
}
|
|
const Eigen::Vector2d uv =
|
|
reprojector.projectCameraPointToPixel(Eigen::Vector3d(pt.x, pt.y, pt.z));
|
|
const int u = static_cast<int>(std::lround(uv.x()));
|
|
const int v = static_cast<int>(std::lround(uv.y()));
|
|
if (u < 0 || u >= img.cols || v < 0 || v >= img.rows) {
|
|
continue;
|
|
}
|
|
const uchar g = static_cast<uchar>(
|
|
255.0f * (static_cast<float>(pt.z) - z_min) / z_rng);
|
|
cv::circle(
|
|
img,
|
|
cv::Point(u, v),
|
|
reprojector.getPointRadius(),
|
|
cv::Scalar(g, g, g),
|
|
-1);
|
|
}
|
|
return img;
|
|
}
|
|
|
|
Eigen::Matrix4d getFixedTil()
|
|
{
|
|
Eigen::Matrix4d Til = Eigen::Matrix4d::Identity();
|
|
Til(0, 3) = -0.02663;
|
|
Til(1, 3) = 0.03447;
|
|
Til(2, 3) = 0.02174;
|
|
return Til;
|
|
}
|
|
|
|
bool fileExists(const std::string& filename)
|
|
{
|
|
struct stat buffer;
|
|
return (stat(filename.c_str(), &buffer) == 0);
|
|
}
|
|
|
|
int64_t stamp_to_ns(const builtin_interfaces::msg::Time& stamp)
|
|
{
|
|
return static_cast<int64_t>(stamp.sec) * 1000000000LL + static_cast<int64_t>(stamp.nanosec);
|
|
}
|
|
|
|
} // namespace
|
|
|
|
class CloudReprojectionSyncedRosNode : public rclcpp::Node
|
|
{
|
|
public:
|
|
CloudReprojectionSyncedRosNode()
|
|
: Node("cloud_reprojection_synced_node")
|
|
{
|
|
loadParameters();
|
|
createPublishers();
|
|
createSubscriptions();
|
|
|
|
RCLCPP_INFO_STREAM(
|
|
this->get_logger(),
|
|
"\n synced_cloud_topic: " << synced_cloud_topic_
|
|
<< "\n synced_odometry_topic: " << synced_odometry_topic_
|
|
<< "\n synced_wiwc_topic: " << synced_wiwc_topic_
|
|
<< "\n synced_image_topic: " << synced_image_topic_
|
|
<< "\n synced_image_transport: " << (synced_image_compressed_ ? "compressed" : "raw")
|
|
<< "\n output_prefix: " << sync_topic_prefix_
|
|
<< "\n overlay_compressed_topic: " << sync_overlay_image_topic_
|
|
<< "\n detection_debug_topic: " << sync_detection_debug_image_topic_
|
|
<< "\n gallery_debug_topic: " << sync_gallery_debug_topic_
|
|
<< "\n combined_compressed_topic: " << combined_compressed_topic_
|
|
<< "\n publish_combined_compressed: " << (publish_combined_compressed_ ? "on" : "off")
|
|
<< "\n send_overlay: " << (send_overlay_ ? "on" : "off")
|
|
#ifdef ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION
|
|
<< "\n process_target_observation: " << (enable_target_observation_ ? "on" : "off")
|
|
<< "\n debug: " << (debug_target_observation_ ? "on" : "off")
|
|
<< "\n target_observation_topic: " << sync_target_observation_topic_
|
|
<< "\n track_observations_topic: " << sync_track_observations_topic_
|
|
<< "\n detection_pos_cam_topic: " << sync_detection_pos_cam_topic_
|
|
<< "\n detection_pos_world_topic: " << sync_detection_pos_world_topic_
|
|
#endif
|
|
);
|
|
|
|
RCLCPP_INFO(
|
|
this->get_logger(),
|
|
"CloudReprojectionSyncedRosNode initialized (already-synced inputs, target observation only)");
|
|
}
|
|
|
|
private:
|
|
using PointCloud2 = sensor_msgs::msg::PointCloud2;
|
|
using Odometry = nav_msgs::msg::Odometry;
|
|
using Image = sensor_msgs::msg::Image;
|
|
using CompressedImage = sensor_msgs::msg::CompressedImage;
|
|
|
|
struct PendingFrame {
|
|
PointCloud2::ConstSharedPtr cloud;
|
|
Odometry::ConstSharedPtr odom;
|
|
Odometry::ConstSharedPtr wiwc;
|
|
Image::ConstSharedPtr image;
|
|
CompressedImage::ConstSharedPtr compressed_image;
|
|
};
|
|
|
|
std::string synced_cloud_topic_;
|
|
std::string synced_odometry_topic_;
|
|
std::string synced_wiwc_topic_;
|
|
std::string synced_image_topic_;
|
|
bool synced_image_compressed_ = true;
|
|
std::string sync_topic_prefix_;
|
|
std::string combined_compressed_topic_;
|
|
int combined_jpeg_quality_ = 85;
|
|
int overlay_jpeg_quality_ = 85;
|
|
bool publish_combined_compressed_ = true;
|
|
bool send_overlay_ = true;
|
|
|
|
std::string sync_overlay_image_topic_;
|
|
std::string sync_detection_debug_image_topic_;
|
|
std::string sync_gallery_debug_topic_;
|
|
std::string sync_target_observation_topic_;
|
|
std::string sync_track_observations_topic_;
|
|
std::string sync_detection_pos_cam_topic_;
|
|
std::string sync_detection_pos_world_topic_;
|
|
|
|
rclcpp::Subscription<PointCloud2>::SharedPtr cloud_sub_;
|
|
rclcpp::Subscription<Odometry>::SharedPtr odom_sub_;
|
|
rclcpp::Subscription<Odometry>::SharedPtr wiwc_sub_;
|
|
rclcpp::Subscription<Image>::SharedPtr image_sub_;
|
|
rclcpp::Subscription<CompressedImage>::SharedPtr compressed_image_sub_;
|
|
|
|
rclcpp::Publisher<CompressedImage>::SharedPtr overlay_compressed_pub_;
|
|
rclcpp::Publisher<CompressedImage>::SharedPtr detection_debug_compressed_pub_;
|
|
rclcpp::Publisher<Image>::SharedPtr gallery_debug_pub_;
|
|
rclcpp::Publisher<CompressedImage>::SharedPtr combined_pub_;
|
|
rclcpp::Publisher<odin_ros_driver::msg::TargetObservation>::SharedPtr target_observation_pub_;
|
|
rclcpp::Publisher<odin_ros_driver::msg::TargetObservationArray>::SharedPtr track_observations_pub_;
|
|
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr detection_pos_cam_pub_;
|
|
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr detection_pos_world_pub_;
|
|
|
|
std::unique_ptr<CloudReprojector> reprojector_;
|
|
#ifdef ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION
|
|
std::unique_ptr<odin_ros_driver::TargetObservationProcessor> target_observation_processor_;
|
|
bool enable_target_observation_ = false;
|
|
bool debug_target_observation_ = false;
|
|
bool debug_reid_ = false;
|
|
#endif
|
|
|
|
std::map<int64_t, PendingFrame> pending_frames_;
|
|
size_t max_pending_frames_ = 64;
|
|
|
|
void loadParameters()
|
|
{
|
|
const auto package_path = get_package_source_directory_from_file();
|
|
const auto target_prediction_root = get_target_prediction_root_directory();
|
|
const auto default_yolo_engine =
|
|
(target_prediction_root / "deploy" / "model" / "yolo26s-pose.trt").string();
|
|
const auto default_reid_engine =
|
|
(target_prediction_root / "deploy" / "model" / "target_reid_osnet_x0_25_dukemtmcreid.trt").string();
|
|
const auto default_yolo_labels =
|
|
(target_prediction_root / "thirdparty" / "YOLOs-CPP-TensorRT" / "models" / "coco.names").string();
|
|
|
|
this->declare_parameter<std::string>("synced_cloud_topic", "/odin1/sync/cloud_in_cam");
|
|
this->declare_parameter<std::string>("synced_odometry_topic", "/odin1/sync/odometry");
|
|
this->declare_parameter<std::string>("synced_wiwc_topic", "/odin1/sync/wiwc");
|
|
this->declare_parameter<std::string>("synced_image_topic", "/odin1/sync/image");
|
|
this->declare_parameter<int>("synced_image_compressed", 1);
|
|
this->declare_parameter<std::string>("register_keys.sync_topic_prefix", "/odin1/sync");
|
|
this->declare_parameter<std::string>("register_keys.combined_compressed_topic", "/odin1/combined_image/compressed");
|
|
this->declare_parameter<int>("register_keys.combined_jpeg_quality", 85);
|
|
this->declare_parameter<int>("register_keys.send_combined_compressed", 1);
|
|
this->declare_parameter<int>("register_keys.send_overlay", 1);
|
|
this->declare_parameter<int>("register_keys.overlay_jpeg_quality", 85);
|
|
#ifdef ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION
|
|
this->declare_parameter<int>("register_keys.process_target_observation", 1);
|
|
this->declare_parameter<int>("register_keys.debug", 1);
|
|
this->declare_parameter<std::string>("register_keys.target_yolo_engine", default_yolo_engine);
|
|
this->declare_parameter<std::string>("register_keys.target_yolo_labels", default_yolo_labels);
|
|
this->declare_parameter<double>("register_keys.target_yolo_conf", 0.5);
|
|
this->declare_parameter<double>("register_keys.target_yolo_nms", 0.50);
|
|
this->declare_parameter<double>("register_keys.target_min_depth", 0.5);
|
|
this->declare_parameter<double>("register_keys.target_max_depth", 12.0);
|
|
this->declare_parameter<double>("register_keys.target_search_radius_px", 25.0);
|
|
this->declare_parameter<int>("register_keys.target_lost_detection_compensation_enable", 1);
|
|
this->declare_parameter<double>("register_keys.target_lost_detection_roi_scale", 2.0);
|
|
this->declare_parameter<int>("register_keys.target_lost_detection_roi_min_size_px", 192);
|
|
this->declare_parameter<int>("register_keys.target_reid_enable", 0);
|
|
this->declare_parameter<std::string>("register_keys.target_reid_engine", default_reid_engine);
|
|
this->declare_parameter<double>("register_keys.target_reid_match_threshold", 0.65);
|
|
this->declare_parameter<double>("register_keys.target_reid_gap_threshold", 0.10);
|
|
this->declare_parameter<double>("register_keys.target_reid_min_crop_area", 2000.0);
|
|
this->declare_parameter<double>("register_keys.target_reid_max_crop_aspect_ratio", 0.90);
|
|
this->declare_parameter<int>("register_keys.target_reid_feature_update_interval", 5);
|
|
this->declare_parameter<int>("register_keys.target_reid_lost_timeout_frames", 150);
|
|
this->declare_parameter<int>("register_keys.target_reid_gallery_size", 8);
|
|
this->declare_parameter<int>("register_keys.target_reid_input_height", 256);
|
|
this->declare_parameter<int>("register_keys.target_reid_input_width", 128);
|
|
this->declare_parameter<int>("register_keys.target_reid_feature_dim", 512);
|
|
this->declare_parameter<int>("register_keys.target_reid_max_batch_size", 8);
|
|
this->declare_parameter<int>("register_keys.target_reid_crop_edge_margin_px", 0);
|
|
this->declare_parameter<int>("register_keys.target_reid_3d_fallback_enabled", 1);
|
|
this->declare_parameter<double>("register_keys.target_reid_3d_fallback_base_m", 0.75);
|
|
this->declare_parameter<double>("register_keys.target_reid_3d_fallback_per_frame_m", 0.05);
|
|
this->declare_parameter<int>("register_keys.target_reid_confirm_hits_before_allocate", 5);
|
|
this->declare_parameter<int>("register_keys.debug_reid", 0);
|
|
this->declare_parameter<int>("register_keys.target_detection_roi_enabled", 0);
|
|
this->declare_parameter<int>("register_keys.target_detection_roi_width_px", 1440);
|
|
this->declare_parameter<int>("register_keys.target_detection_roi_height_px", 1080);
|
|
this->declare_parameter<int>("register_keys.target_detection_roi_y_offset_px", 0);
|
|
#endif
|
|
|
|
synced_cloud_topic_ = this->get_parameter("synced_cloud_topic").as_string();
|
|
synced_odometry_topic_ = this->get_parameter("synced_odometry_topic").as_string();
|
|
synced_wiwc_topic_ = this->get_parameter("synced_wiwc_topic").as_string();
|
|
synced_image_compressed_ = (this->get_parameter("synced_image_compressed").as_int() != 0);
|
|
synced_image_topic_ = resolve_camera_topic(
|
|
this->get_parameter("synced_image_topic").as_string(),
|
|
synced_image_compressed_);
|
|
sync_topic_prefix_ = this->get_parameter("register_keys.sync_topic_prefix").as_string();
|
|
combined_compressed_topic_ = this->get_parameter("register_keys.combined_compressed_topic").as_string();
|
|
combined_jpeg_quality_ = std::clamp<int>(
|
|
static_cast<int>(this->get_parameter("register_keys.combined_jpeg_quality").as_int()),
|
|
1,
|
|
100);
|
|
overlay_jpeg_quality_ = std::clamp<int>(
|
|
static_cast<int>(this->get_parameter("register_keys.overlay_jpeg_quality").as_int()),
|
|
1,
|
|
100);
|
|
publish_combined_compressed_ =
|
|
(this->get_parameter("register_keys.send_combined_compressed").as_int() != 0);
|
|
send_overlay_ =
|
|
(this->get_parameter("register_keys.send_overlay").as_int() != 0);
|
|
|
|
sync_overlay_image_topic_ = sync_topic_prefix_ + "/overlay_img/compressed";
|
|
sync_detection_debug_image_topic_ = sync_topic_prefix_ + "/detection_img_debug/compressed";
|
|
sync_gallery_debug_topic_ = sync_topic_prefix_ + "/gallery_debug";
|
|
sync_target_observation_topic_ = sync_topic_prefix_ + "/target_observation";
|
|
sync_track_observations_topic_ = sync_topic_prefix_ + "/track_observations";
|
|
sync_detection_pos_cam_topic_ = sync_topic_prefix_ + "/detection_pos_cam";
|
|
sync_detection_pos_world_topic_ = sync_topic_prefix_ + "/detection_pos_world";
|
|
|
|
std::string calib_file = (package_path / "config" / "calib.yaml").string();
|
|
YAML::Node calib_config = YAML::LoadFile(calib_file);
|
|
|
|
CloudReprojector::CameraParams cam_params;
|
|
cam_params.image_width = calib_config["cam_0"]["image_width"].as<int>();
|
|
cam_params.image_height = calib_config["cam_0"]["image_height"].as<int>();
|
|
cam_params.A11 = calib_config["cam_0"]["A11"].as<double>();
|
|
cam_params.A12 = calib_config["cam_0"]["A12"].as<double>();
|
|
cam_params.A22 = calib_config["cam_0"]["A22"].as<double>();
|
|
cam_params.u0 = calib_config["cam_0"]["u0"].as<double>();
|
|
cam_params.v0 = calib_config["cam_0"]["v0"].as<double>();
|
|
cam_params.k2 = calib_config["cam_0"]["k2"].as<double>();
|
|
cam_params.k3 = calib_config["cam_0"]["k3"].as<double>();
|
|
cam_params.k4 = calib_config["cam_0"]["k4"].as<double>();
|
|
cam_params.k5 = calib_config["cam_0"]["k5"].as<double>();
|
|
cam_params.k6 = calib_config["cam_0"]["k6"].as<double>();
|
|
cam_params.k7 = calib_config["cam_0"]["k7"].as<double>();
|
|
|
|
CloudReprojector::ExtrinsicParams ext_params;
|
|
const auto Tcl_vec = calib_config["Tcl_0"].as<std::vector<double>>();
|
|
if (Tcl_vec.size() != 16) {
|
|
throw std::runtime_error("Tcl_0 has invalid size in calib.yaml");
|
|
}
|
|
for (int i = 0; i < 4; ++i) {
|
|
for (int j = 0; j < 4; ++j) {
|
|
ext_params.Tcl(i, j) = Tcl_vec[static_cast<size_t>(i * 4 + j)];
|
|
}
|
|
}
|
|
ext_params.Til = getFixedTil();
|
|
ext_params.Tic = CloudReprojector::calculateTic(ext_params.Tcl, ext_params.Til);
|
|
|
|
reprojector_ = std::make_unique<CloudReprojector>();
|
|
if (!reprojector_->initialize(cam_params, ext_params)) {
|
|
throw std::runtime_error("Failed to initialize CloudReprojector");
|
|
}
|
|
|
|
#ifdef ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION
|
|
enable_target_observation_ =
|
|
(this->get_parameter("register_keys.process_target_observation").as_int() != 0);
|
|
debug_target_observation_ =
|
|
(this->get_parameter("register_keys.debug").as_int() != 0);
|
|
debug_reid_ =
|
|
(this->get_parameter("register_keys.debug_reid").as_int() != 0);
|
|
if (enable_target_observation_) {
|
|
odin_ros_driver::TargetObservationConfig target_config;
|
|
target_config.yolo_engine_path =
|
|
this->get_parameter("register_keys.target_yolo_engine").as_string();
|
|
target_config.yolo_labels_path =
|
|
this->get_parameter("register_keys.target_yolo_labels").as_string();
|
|
target_config.yolo_conf = static_cast<float>(
|
|
this->get_parameter("register_keys.target_yolo_conf").as_double());
|
|
target_config.yolo_nms = static_cast<float>(
|
|
this->get_parameter("register_keys.target_yolo_nms").as_double());
|
|
target_config.min_depth = static_cast<float>(
|
|
this->get_parameter("register_keys.target_min_depth").as_double());
|
|
target_config.max_depth = static_cast<float>(
|
|
this->get_parameter("register_keys.target_max_depth").as_double());
|
|
target_config.search_radius_px = static_cast<float>(
|
|
this->get_parameter("register_keys.target_search_radius_px").as_double());
|
|
target_config.lost_detection_compensation_enabled =
|
|
(this->get_parameter("register_keys.target_lost_detection_compensation_enable").as_int() != 0);
|
|
target_config.lost_detection_roi_scale = static_cast<float>(
|
|
this->get_parameter("register_keys.target_lost_detection_roi_scale").as_double());
|
|
target_config.lost_detection_roi_min_size_px =
|
|
this->get_parameter("register_keys.target_lost_detection_roi_min_size_px").as_int();
|
|
target_config.reid_enabled =
|
|
(this->get_parameter("register_keys.target_reid_enable").as_int() != 0);
|
|
target_config.reid_engine_path =
|
|
this->get_parameter("register_keys.target_reid_engine").as_string();
|
|
target_config.reid_match_threshold = static_cast<float>(
|
|
this->get_parameter("register_keys.target_reid_match_threshold").as_double());
|
|
target_config.reid_gap_threshold = static_cast<float>(
|
|
this->get_parameter("register_keys.target_reid_gap_threshold").as_double());
|
|
target_config.reid_min_crop_area = static_cast<float>(
|
|
this->get_parameter("register_keys.target_reid_min_crop_area").as_double());
|
|
target_config.reid_max_crop_aspect_ratio = static_cast<float>(
|
|
this->get_parameter("register_keys.target_reid_max_crop_aspect_ratio").as_double());
|
|
target_config.reid_feature_update_interval =
|
|
this->get_parameter("register_keys.target_reid_feature_update_interval").as_int();
|
|
target_config.reid_lost_timeout_frames =
|
|
this->get_parameter("register_keys.target_reid_lost_timeout_frames").as_int();
|
|
target_config.reid_gallery_size =
|
|
this->get_parameter("register_keys.target_reid_gallery_size").as_int();
|
|
target_config.reid_input_height =
|
|
this->get_parameter("register_keys.target_reid_input_height").as_int();
|
|
target_config.reid_input_width =
|
|
this->get_parameter("register_keys.target_reid_input_width").as_int();
|
|
target_config.reid_feature_dim =
|
|
this->get_parameter("register_keys.target_reid_feature_dim").as_int();
|
|
target_config.reid_max_batch_size =
|
|
this->get_parameter("register_keys.target_reid_max_batch_size").as_int();
|
|
target_config.reid_crop_edge_margin_px =
|
|
this->get_parameter("register_keys.target_reid_crop_edge_margin_px").as_int();
|
|
target_config.reid_3d_fallback_enabled =
|
|
(this->get_parameter("register_keys.target_reid_3d_fallback_enabled").as_int() != 0);
|
|
target_config.reid_3d_fallback_base_m = static_cast<float>(
|
|
this->get_parameter("register_keys.target_reid_3d_fallback_base_m").as_double());
|
|
target_config.reid_3d_fallback_per_frame_m = static_cast<float>(
|
|
this->get_parameter("register_keys.target_reid_3d_fallback_per_frame_m").as_double());
|
|
target_config.reid_confirm_hits_before_allocate =
|
|
this->get_parameter("register_keys.target_reid_confirm_hits_before_allocate").as_int();
|
|
target_config.debug_reid = debug_reid_;
|
|
target_config.detection_roi_enabled =
|
|
(this->get_parameter("register_keys.target_detection_roi_enabled").as_int() != 0);
|
|
target_config.detection_roi_width_px =
|
|
this->get_parameter("register_keys.target_detection_roi_width_px").as_int();
|
|
target_config.detection_roi_height_px =
|
|
this->get_parameter("register_keys.target_detection_roi_height_px").as_int();
|
|
target_config.detection_roi_y_offset_px =
|
|
this->get_parameter("register_keys.target_detection_roi_y_offset_px").as_int();
|
|
target_config.debug = debug_target_observation_;
|
|
|
|
target_observation_processor_ =
|
|
std::make_unique<odin_ros_driver::TargetObservationProcessor>();
|
|
target_observation_processor_->initialize(target_config);
|
|
}
|
|
#endif
|
|
}
|
|
|
|
void createPublishers()
|
|
{
|
|
if (send_overlay_) {
|
|
overlay_compressed_pub_ =
|
|
this->create_publisher<CompressedImage>(sync_overlay_image_topic_, 10);
|
|
}
|
|
if (publish_combined_compressed_) {
|
|
combined_pub_ =
|
|
this->create_publisher<CompressedImage>(combined_compressed_topic_, 10);
|
|
}
|
|
#ifdef ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION
|
|
if (enable_target_observation_) {
|
|
target_observation_pub_ =
|
|
this->create_publisher<odin_ros_driver::msg::TargetObservation>(
|
|
sync_target_observation_topic_, 10);
|
|
track_observations_pub_ =
|
|
this->create_publisher<odin_ros_driver::msg::TargetObservationArray>(
|
|
sync_track_observations_topic_, 10);
|
|
detection_pos_cam_pub_ =
|
|
this->create_publisher<visualization_msgs::msg::MarkerArray>(
|
|
sync_detection_pos_cam_topic_, 10);
|
|
detection_pos_world_pub_ =
|
|
this->create_publisher<visualization_msgs::msg::MarkerArray>(
|
|
sync_detection_pos_world_topic_, 10);
|
|
}
|
|
if (enable_target_observation_ && debug_target_observation_) {
|
|
detection_debug_compressed_pub_ =
|
|
this->create_publisher<CompressedImage>(sync_detection_debug_image_topic_, 10);
|
|
}
|
|
if (enable_target_observation_ && debug_reid_) {
|
|
gallery_debug_pub_ =
|
|
this->create_publisher<Image>(sync_gallery_debug_topic_, 10);
|
|
}
|
|
#endif
|
|
}
|
|
|
|
void createSubscriptions()
|
|
{
|
|
cloud_sub_ = this->create_subscription<PointCloud2>(
|
|
synced_cloud_topic_, 10,
|
|
std::bind(&CloudReprojectionSyncedRosNode::cloudCallback, this, std::placeholders::_1));
|
|
odom_sub_ = this->create_subscription<Odometry>(
|
|
synced_odometry_topic_, 10,
|
|
std::bind(&CloudReprojectionSyncedRosNode::odomCallback, this, std::placeholders::_1));
|
|
wiwc_sub_ = this->create_subscription<Odometry>(
|
|
synced_wiwc_topic_, 10,
|
|
std::bind(&CloudReprojectionSyncedRosNode::wiwcCallback, this, std::placeholders::_1));
|
|
if (synced_image_compressed_) {
|
|
compressed_image_sub_ = this->create_subscription<CompressedImage>(
|
|
synced_image_topic_, 10,
|
|
std::bind(
|
|
&CloudReprojectionSyncedRosNode::compressedImageCallback,
|
|
this,
|
|
std::placeholders::_1));
|
|
} else {
|
|
image_sub_ = this->create_subscription<Image>(
|
|
synced_image_topic_, 10,
|
|
std::bind(&CloudReprojectionSyncedRosNode::imageCallback, this, std::placeholders::_1));
|
|
}
|
|
}
|
|
|
|
void prunePendingFrames()
|
|
{
|
|
while (pending_frames_.size() > max_pending_frames_) {
|
|
pending_frames_.erase(pending_frames_.begin());
|
|
}
|
|
}
|
|
|
|
void cloudCallback(const PointCloud2::ConstSharedPtr& msg)
|
|
{
|
|
const int64_t stamp_ns = stamp_to_ns(msg->header.stamp);
|
|
pending_frames_[stamp_ns].cloud = msg;
|
|
tryProcessFrame(stamp_ns);
|
|
prunePendingFrames();
|
|
}
|
|
|
|
void odomCallback(const Odometry::ConstSharedPtr& msg)
|
|
{
|
|
const int64_t stamp_ns = stamp_to_ns(msg->header.stamp);
|
|
pending_frames_[stamp_ns].odom = msg;
|
|
tryProcessFrame(stamp_ns);
|
|
prunePendingFrames();
|
|
}
|
|
|
|
void wiwcCallback(const Odometry::ConstSharedPtr& msg)
|
|
{
|
|
const int64_t stamp_ns = stamp_to_ns(msg->header.stamp);
|
|
pending_frames_[stamp_ns].wiwc = msg;
|
|
tryProcessFrame(stamp_ns);
|
|
prunePendingFrames();
|
|
}
|
|
|
|
void imageCallback(const Image::ConstSharedPtr& msg)
|
|
{
|
|
const int64_t stamp_ns = stamp_to_ns(msg->header.stamp);
|
|
pending_frames_[stamp_ns].image = msg;
|
|
tryProcessFrame(stamp_ns);
|
|
prunePendingFrames();
|
|
}
|
|
|
|
void compressedImageCallback(const CompressedImage::ConstSharedPtr& msg)
|
|
{
|
|
const int64_t stamp_ns = stamp_to_ns(msg->header.stamp);
|
|
pending_frames_[stamp_ns].compressed_image = msg;
|
|
tryProcessFrame(stamp_ns);
|
|
prunePendingFrames();
|
|
}
|
|
|
|
void tryProcessFrame(int64_t stamp_ns)
|
|
{
|
|
auto it = pending_frames_.find(stamp_ns);
|
|
if (it == pending_frames_.end()) {
|
|
return;
|
|
}
|
|
PendingFrame& frame = it->second;
|
|
if (!frame.cloud || !frame.odom || !frame.wiwc) {
|
|
return;
|
|
}
|
|
if (synced_image_compressed_) {
|
|
if (!frame.compressed_image) {
|
|
return;
|
|
}
|
|
} else if (!frame.image) {
|
|
return;
|
|
}
|
|
|
|
processFrame(frame);
|
|
pending_frames_.erase(it);
|
|
}
|
|
|
|
void processFrame(const PendingFrame& frame)
|
|
{
|
|
const auto& cloud_msg = frame.cloud;
|
|
const auto& odom_msg = frame.odom;
|
|
const auto& wiwc_msg = frame.wiwc;
|
|
|
|
cv::Mat cam_bgr;
|
|
Image sync_image_msg;
|
|
if (synced_image_compressed_) {
|
|
if (!frame.compressed_image || frame.compressed_image->data.empty()) {
|
|
RCLCPP_WARN(this->get_logger(), "Empty compressed sync image received");
|
|
return;
|
|
}
|
|
cam_bgr = odin_ros_driver::decode_compressed_to_bgr(
|
|
frame.compressed_image->data.data(),
|
|
frame.compressed_image->data.size());
|
|
if (cam_bgr.empty()) {
|
|
RCLCPP_ERROR(this->get_logger(), "cv::imdecode failed (synced compressed image)");
|
|
return;
|
|
}
|
|
sync_image_msg = *cv_bridge::CvImage(
|
|
frame.compressed_image->header, "bgr8", cam_bgr).toImageMsg();
|
|
} else {
|
|
if (!frame.image) {
|
|
return;
|
|
}
|
|
try {
|
|
cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(frame.image, "bgr8");
|
|
cam_bgr = cv_ptr->image.clone();
|
|
sync_image_msg = *frame.image;
|
|
} catch (const cv_bridge::Exception& e) {
|
|
RCLCPP_ERROR(this->get_logger(), "cv_bridge (synced image): %s", e.what());
|
|
return;
|
|
}
|
|
}
|
|
|
|
pcl::PointCloud<pcl::PointXYZRGB> cloud_cam;
|
|
pcl::fromROSMsg(*cloud_msg, cloud_cam);
|
|
if (cloud_cam.empty()) {
|
|
RCLCPP_WARN(this->get_logger(), "Empty cloud_in_cam received");
|
|
return;
|
|
}
|
|
|
|
Eigen::Matrix4d T_CL = Eigen::Matrix4d::Identity();
|
|
Eigen::Matrix4d T_IL = Eigen::Matrix4d::Identity();
|
|
for (int i = 0; i < 16; ++i) {
|
|
T_CL(i / 4, i % 4) = wiwc_msg->pose.covariance[static_cast<size_t>(i)];
|
|
T_IL(i / 4, i % 4) = wiwc_msg->twist.covariance[static_cast<size_t>(i)];
|
|
}
|
|
const bool T_CL_valid = (T_CL - Eigen::Matrix4d::Identity()).norm() > 1e-6;
|
|
const bool T_IL_valid = (T_IL - Eigen::Matrix4d::Identity()).norm() > 1e-6;
|
|
if (T_CL_valid && T_IL_valid) {
|
|
reprojector_->updateExtrinsics(T_CL, T_IL);
|
|
}
|
|
|
|
CloudReprojector::OdomPose odom_pose;
|
|
odom_pose.orientation = Eigen::Quaterniond(
|
|
odom_msg->pose.pose.orientation.w,
|
|
odom_msg->pose.pose.orientation.x,
|
|
odom_msg->pose.pose.orientation.y,
|
|
odom_msg->pose.pose.orientation.z);
|
|
odom_pose.position = Eigen::Vector3d(
|
|
odom_msg->pose.pose.position.x,
|
|
odom_msg->pose.pose.position.y,
|
|
odom_msg->pose.pose.position.z);
|
|
|
|
#ifdef ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION
|
|
odin_ros_driver::TargetObservation target_observation;
|
|
odin_ros_driver::TargetObservationDebugInfo target_debug;
|
|
if (enable_target_observation_ && target_observation_processor_) {
|
|
const auto target_start = std::chrono::steady_clock::now();
|
|
target_observation = target_observation_processor_->process(
|
|
cam_bgr,
|
|
cloud_cam,
|
|
*reprojector_,
|
|
odom_pose,
|
|
&target_debug);
|
|
const auto target_end = std::chrono::steady_clock::now();
|
|
|
|
auto format_rebind_reason = [](float sim, float dist) {
|
|
char buf[64];
|
|
if (sim >= 0.0f) {
|
|
std::snprintf(buf, sizeof(buf), "sim=%.3f fallback_dist=N/A", sim);
|
|
} else if (dist >= 0.0f) {
|
|
std::snprintf(buf, sizeof(buf), "sim=N/A fallback_dist=%.2fm", dist);
|
|
} else {
|
|
std::snprintf(buf, sizeof(buf), "sim=N/A fallback_dist=N/A");
|
|
}
|
|
return std::string(buf);
|
|
};
|
|
|
|
if (target_debug.reid_attempted ||
|
|
target_debug.reid_3d_fallback_distance >= 0.0f) {
|
|
const std::string reason = format_rebind_reason(
|
|
target_debug.reid_similarity,
|
|
target_debug.reid_3d_fallback_distance);
|
|
const std::string sims =
|
|
format_reid_comparisons(
|
|
target_debug.raw_track_ids,
|
|
target_debug.reid_comparisons);
|
|
if (target_debug.recovered_by_reid) {
|
|
RCLCPP_WARN(
|
|
this->get_logger(),
|
|
"[LazyReID] RECOVERED stable_id=%d raw_id=%d det_ind=%d %s sims={%s} gallery=%d lost=%d lost_comp_attempted=%s lost_comp_recovered=%s",
|
|
target_debug.current_target_id_before,
|
|
target_debug.selected_raw_track_id,
|
|
target_debug.detection_index,
|
|
reason.c_str(),
|
|
sims.c_str(),
|
|
target_debug.gallery_size,
|
|
target_debug.lost_frames,
|
|
target_debug.lost_detection_compensation_attempted ? "yes" : "no",
|
|
target_debug.recovered_by_lost_detection_compensation ? "yes" : "no");
|
|
} else {
|
|
RCLCPP_WARN(
|
|
this->get_logger(),
|
|
"[LazyReID] NO_MATCH prev_sid=%d %s sims=%s gallery=%d lost=%d tracked=%d detections=%d lost_comp_attempted=%s lost_comp_recovered=%s",
|
|
target_debug.current_target_id_before,
|
|
reason.c_str(),
|
|
sims.c_str(),
|
|
target_debug.gallery_size,
|
|
target_debug.lost_frames,
|
|
target_debug.tracks_count,
|
|
target_debug.poses_count,
|
|
target_debug.lost_detection_compensation_attempted ? "yes" : "no",
|
|
target_debug.recovered_by_lost_detection_compensation ? "yes" : "no");
|
|
}
|
|
}
|
|
|
|
if (debug_reid_) {
|
|
const std::string sims =
|
|
format_reid_comparisons(
|
|
target_debug.raw_track_ids,
|
|
target_debug.reid_comparisons);
|
|
RCLCPP_INFO(
|
|
this->get_logger(),
|
|
"[ReIDMatrix] prev_sid=%d bytetrack_id=%s stable_id=%s sims=%s",
|
|
target_debug.current_target_id_before,
|
|
format_int_vec(target_debug.raw_track_ids).c_str(),
|
|
format_int_vec(target_debug.row_stable_ids).c_str(),
|
|
sims.c_str());
|
|
}
|
|
|
|
if (debug_reid_ && gallery_debug_pub_ && target_observation_processor_) {
|
|
cv::Mat gallery_debug =
|
|
target_observation_processor_->render_gallery_debug_image(8, 10);
|
|
if (!gallery_debug.empty()) {
|
|
auto msg = cv_bridge::CvImage(
|
|
sync_image_msg.header, "bgr8", gallery_debug).toImageMsg();
|
|
gallery_debug_pub_->publish(*msg);
|
|
}
|
|
}
|
|
|
|
if (debug_target_observation_) {
|
|
if (!target_observation.valid) {
|
|
if (target_debug.poses_count == 0) {
|
|
RCLCPP_INFO_THROTTLE(
|
|
this->get_logger(), *this->get_clock(), 2000,
|
|
"Target observation | no detections | lost_comp_attempted=%s lost_comp_recovered=%s | yolo=%.2f ms | total=%.2f ms",
|
|
target_debug.lost_detection_compensation_attempted ? "yes" : "no",
|
|
target_debug.recovered_by_lost_detection_compensation ? "yes" : "no",
|
|
target_debug.yolo_ms,
|
|
target_debug.total_ms);
|
|
} else if (target_debug.tracks_count == 0) {
|
|
RCLCPP_INFO_THROTTLE(
|
|
this->get_logger(), *this->get_clock(), 2000,
|
|
"Target observation | detections=%d tracked=0 current_target_id=%d lost_comp_attempted=%s lost_comp_recovered=%s | yolo=%.2f ms | mot=%.2f ms | total=%.2f ms",
|
|
target_debug.poses_count,
|
|
target_debug.current_target_id_before,
|
|
target_debug.lost_detection_compensation_attempted ? "yes" : "no",
|
|
target_debug.recovered_by_lost_detection_compensation ? "yes" : "no",
|
|
target_debug.yolo_ms,
|
|
target_debug.mot_ms,
|
|
target_debug.total_ms);
|
|
} else {
|
|
RCLCPP_INFO_THROTTLE(
|
|
this->get_logger(), *this->get_clock(), 1000,
|
|
"Target observation | detections=%d tracked=%d bytetrack_id=%s stable_id=%s active=%d selected_id=%d raw_id=%d det_ind=%d center_fallback=%s lost_comp_attempted=%s lost_comp_recovered=%s reid_attempted=%s reid_recovered=%s sim=%.3f fallback_dist=%.3f gallery=%d lost=%d cloud_pts=%d depth_samples=%d | yolo=%.2f mot=%.2f cloud=0.00 bind=%.2f (depth=%.2f reid=%.2f) age=%.2f select=%.2f enrich=%.2f | total=%.2f ms",
|
|
target_debug.poses_count,
|
|
target_debug.tracks_count,
|
|
format_int_vec(target_debug.raw_track_ids).c_str(),
|
|
format_int_vec(target_debug.row_stable_ids).c_str(),
|
|
target_debug.active_tracks_count,
|
|
target_debug.selected_track_id,
|
|
target_debug.selected_raw_track_id,
|
|
target_debug.detection_index,
|
|
target_debug.selected_from_center_bootstrap ? "yes" : "no",
|
|
target_debug.lost_detection_compensation_attempted ? "yes" : "no",
|
|
target_debug.recovered_by_lost_detection_compensation ? "yes" : "no",
|
|
target_debug.reid_attempted ? "yes" : "no",
|
|
target_debug.recovered_by_reid ? "yes" : "no",
|
|
target_debug.reid_similarity,
|
|
target_debug.reid_3d_fallback_distance,
|
|
target_debug.gallery_size,
|
|
target_debug.lost_frames,
|
|
static_cast<int>(cloud_cam.size()),
|
|
target_debug.depth_sample_count,
|
|
target_debug.yolo_ms,
|
|
target_debug.mot_ms,
|
|
target_debug.bind_ms,
|
|
target_debug.depth_ms,
|
|
target_debug.reid_extract_ms,
|
|
target_debug.age_out_ms,
|
|
target_debug.select_ms,
|
|
target_debug.enrich_ms,
|
|
target_debug.total_ms);
|
|
}
|
|
} else {
|
|
RCLCPP_INFO_THROTTLE(
|
|
this->get_logger(), *this->get_clock(), 500,
|
|
"Target observation | stamp=%u.%u size=%dx%d cloud=%zu detections=%d tracked=%d bytetrack_id=%s stable_id=%s active=%d current_target_id=%d current_raw_id=%d selected_id=%d selected_raw_id=%d det_ind=%d reused=%s center_fallback=%s lost_comp_attempted=%s lost_comp_recovered=%s reid_attempted=%s reid_recovered=%s sim=%.3f fallback_dist=%.3f gate_rej=%d gate_soft=%d gallery=%d lost=%d bbox=%s depth=%.3f conf=%.3f depth_conf=%.3f pos_cam=%s pos_world=%s cloud_pts=%d depth_samples=%d | yolo=%.2f mot=%.2f cloud=0.00 bind=%.2f (depth=%.2f reid=%.2f) age=%.2f select=%.2f enrich=%.2f | total=%.2f ms | node_total=%.2f ms",
|
|
sync_image_msg.header.stamp.sec,
|
|
sync_image_msg.header.stamp.nanosec,
|
|
cam_bgr.cols,
|
|
cam_bgr.rows,
|
|
cloud_cam.size(),
|
|
target_debug.poses_count,
|
|
target_debug.tracks_count,
|
|
format_int_vec(target_debug.raw_track_ids).c_str(),
|
|
format_int_vec(target_debug.row_stable_ids).c_str(),
|
|
target_debug.active_tracks_count,
|
|
target_debug.current_target_id_before,
|
|
target_debug.current_raw_track_id_before,
|
|
target_observation.track_id,
|
|
target_debug.selected_raw_track_id,
|
|
target_observation.detection_index,
|
|
target_debug.found_existing_target ? "yes" : "no",
|
|
target_debug.selected_from_center_bootstrap ? "yes" : "no",
|
|
target_debug.lost_detection_compensation_attempted ? "yes" : "no",
|
|
target_debug.recovered_by_lost_detection_compensation ? "yes" : "no",
|
|
target_debug.reid_attempted ? "yes" : "no",
|
|
target_debug.recovered_by_reid ? "yes" : "no",
|
|
target_debug.reid_similarity,
|
|
target_debug.reid_3d_fallback_distance,
|
|
target_debug.reid_3d_gate_rejections,
|
|
target_debug.reid_3d_gate_soft_warnings,
|
|
target_debug.gallery_size,
|
|
target_debug.lost_frames,
|
|
format_bbox_xyxy(target_observation.bbox_xyxy).c_str(),
|
|
target_observation.depth,
|
|
target_observation.confidence,
|
|
target_observation.depth_confidence,
|
|
format_vector3f(target_observation.detection_pos_cam).c_str(),
|
|
format_vector3f(target_observation.detection_pos_world).c_str(),
|
|
static_cast<int>(cloud_cam.size()),
|
|
target_debug.depth_sample_count,
|
|
target_debug.yolo_ms,
|
|
target_debug.mot_ms,
|
|
target_debug.bind_ms,
|
|
target_debug.depth_ms,
|
|
target_debug.reid_extract_ms,
|
|
target_debug.age_out_ms,
|
|
target_debug.select_ms,
|
|
target_debug.enrich_ms,
|
|
target_debug.total_ms,
|
|
std::chrono::duration<double, std::milli>(target_end - target_start).count());
|
|
RCLCPP_INFO_THROTTLE(
|
|
this->get_logger(), *this->get_clock(), 500,
|
|
"Target observation keypoints: %s",
|
|
format_keypoints_xyc(target_observation.keypoints_xyc).c_str());
|
|
}
|
|
}
|
|
|
|
auto fill_observation_msg = [](
|
|
odin_ros_driver::msg::TargetObservation& msg,
|
|
const odin_ros_driver::TargetObservation& obs) {
|
|
msg.valid = obs.valid;
|
|
msg.stable_id = obs.track_id;
|
|
msg.bytetrack_id = obs.raw_track_id;
|
|
msg.detection_index = obs.detection_index;
|
|
msg.is_primary_target = obs.is_primary_target;
|
|
if (obs.valid) {
|
|
msg.confidence = obs.confidence;
|
|
msg.depth = obs.depth;
|
|
msg.depth_confidence = obs.depth_confidence;
|
|
msg.bbox_xyxy = obs.bbox_xyxy;
|
|
msg.keypoints_xyc = obs.keypoints_xyc;
|
|
msg.detection_pos_cam.x = obs.detection_pos_cam.x();
|
|
msg.detection_pos_cam.y = obs.detection_pos_cam.y();
|
|
msg.detection_pos_cam.z = obs.detection_pos_cam.z();
|
|
msg.detection_pos_world.x = obs.detection_pos_world.x();
|
|
msg.detection_pos_world.y = obs.detection_pos_world.y();
|
|
msg.detection_pos_world.z = obs.detection_pos_world.z();
|
|
} else {
|
|
msg.confidence = -1.0f;
|
|
msg.depth = -1.0f;
|
|
msg.depth_confidence = -1.0f;
|
|
msg.bbox_xyxy.fill(-1.0f);
|
|
msg.keypoints_xyc.fill(-1.0f);
|
|
msg.detection_pos_cam.x = -1.0;
|
|
msg.detection_pos_cam.y = -1.0;
|
|
msg.detection_pos_cam.z = -1.0;
|
|
msg.detection_pos_world.x = -1.0;
|
|
msg.detection_pos_world.y = -1.0;
|
|
msg.detection_pos_world.z = -1.0;
|
|
}
|
|
};
|
|
|
|
odin_ros_driver::msg::TargetObservation observation_msg;
|
|
observation_msg.header = sync_image_msg.header;
|
|
observation_msg.odometry = *odom_msg;
|
|
fill_observation_msg(observation_msg, target_observation);
|
|
target_observation_pub_->publish(observation_msg);
|
|
|
|
const std::vector<odin_ros_driver::TargetObservation> active_tracks =
|
|
target_observation_processor_->snapshot_active_tracks();
|
|
odin_ros_driver::msg::TargetObservationArray track_array_msg;
|
|
track_array_msg.header = sync_image_msg.header;
|
|
track_array_msg.primary_target_id =
|
|
target_observation_processor_->current_frame_primary_stable_id();
|
|
track_array_msg.observations.reserve(active_tracks.size());
|
|
for (const auto& obs : active_tracks) {
|
|
odin_ros_driver::msg::TargetObservation one;
|
|
one.header = sync_image_msg.header;
|
|
one.odometry = *odom_msg;
|
|
fill_observation_msg(one, obs);
|
|
track_array_msg.observations.push_back(std::move(one));
|
|
}
|
|
track_observations_pub_->publish(track_array_msg);
|
|
|
|
std_msgs::msg::Header cam_header = sync_image_msg.header;
|
|
cam_header.frame_id = cloud_msg->header.frame_id.empty()
|
|
? odom_msg->child_frame_id
|
|
: cloud_msg->header.frame_id;
|
|
std_msgs::msg::Header world_header = sync_image_msg.header;
|
|
world_header.frame_id = odom_msg->header.frame_id.empty()
|
|
? std::string("odom")
|
|
: odom_msg->header.frame_id;
|
|
|
|
const int marker_primary_id =
|
|
target_observation.valid ? target_observation.track_id : -1;
|
|
auto build_marker_array = [&active_tracks, marker_primary_id](
|
|
const std_msgs::msg::Header& header, bool world_frame) {
|
|
visualization_msgs::msg::MarkerArray out;
|
|
out.markers.reserve(1 + active_tracks.size() * 2);
|
|
visualization_msgs::msg::Marker clear_all;
|
|
clear_all.header = header;
|
|
clear_all.action = visualization_msgs::msg::Marker::DELETEALL;
|
|
out.markers.push_back(clear_all);
|
|
for (const auto& obs : active_tracks) {
|
|
if (!obs.valid) {
|
|
continue;
|
|
}
|
|
const auto& pos = world_frame ? obs.detection_pos_world
|
|
: obs.detection_pos_cam;
|
|
visualization_msgs::msg::Marker sphere;
|
|
sphere.header = header;
|
|
sphere.ns = "detection_sphere";
|
|
sphere.id = obs.track_id;
|
|
sphere.type = visualization_msgs::msg::Marker::SPHERE;
|
|
sphere.action = visualization_msgs::msg::Marker::ADD;
|
|
sphere.pose.position.x = pos.x();
|
|
sphere.pose.position.y = pos.y();
|
|
sphere.pose.position.z = pos.z();
|
|
sphere.pose.orientation.w = 1.0;
|
|
sphere.scale.x = 0.3;
|
|
sphere.scale.y = 0.3;
|
|
sphere.scale.z = 0.3;
|
|
sphere.color = stable_track_color_rgba(obs.track_id, 0.9f);
|
|
sphere.lifetime.sec = 0;
|
|
sphere.lifetime.nanosec = 300000000;
|
|
out.markers.push_back(sphere);
|
|
|
|
visualization_msgs::msg::Marker label;
|
|
label.header = header;
|
|
label.ns = "detection_label";
|
|
label.id = obs.track_id;
|
|
label.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
|
|
label.action = visualization_msgs::msg::Marker::ADD;
|
|
label.pose.position.x = pos.x();
|
|
label.pose.position.y = pos.y();
|
|
label.pose.position.z = pos.z() + 0.5;
|
|
label.pose.orientation.w = 1.0;
|
|
label.scale.z = 0.25;
|
|
label.color.r = 1.0f;
|
|
label.color.g = 1.0f;
|
|
label.color.b = 1.0f;
|
|
label.color.a = 1.0f;
|
|
label.lifetime.sec = 0;
|
|
label.lifetime.nanosec = 300000000;
|
|
label.text = (obs.track_id == marker_primary_id ? "*id=" : "id=") +
|
|
std::to_string(obs.track_id);
|
|
out.markers.push_back(label);
|
|
}
|
|
return out;
|
|
};
|
|
detection_pos_cam_pub_->publish(build_marker_array(cam_header, false));
|
|
detection_pos_world_pub_->publish(build_marker_array(world_header, true));
|
|
}
|
|
#endif
|
|
|
|
if (send_overlay_ && overlay_compressed_pub_) {
|
|
cv::Mat overlay_vis = odin_ros_driver::overlay_projected_cloud_on_image(
|
|
cam_bgr, cloud_cam, reprojector_->getCameraParams());
|
|
#ifdef ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION
|
|
if (enable_target_observation_ && target_observation_processor_) {
|
|
const cv::Rect roi = target_observation_processor_->last_detection_roi();
|
|
if (roi.area() > 0) {
|
|
cv::rectangle(overlay_vis, roi, cv::Scalar(0, 255, 255), 2);
|
|
cv::putText(
|
|
overlay_vis, "detection_roi",
|
|
cv::Point(roi.x + 8, roi.y + 22),
|
|
cv::FONT_HERSHEY_SIMPLEX, 0.55,
|
|
cv::Scalar(0, 255, 255), 2);
|
|
}
|
|
}
|
|
#endif
|
|
if (!overlay_vis.empty()) {
|
|
std::vector<uchar> obuf;
|
|
const std::vector<int> oenc = {
|
|
cv::IMWRITE_JPEG_QUALITY, overlay_jpeg_quality_};
|
|
if (cv::imencode(".jpg", overlay_vis, obuf, oenc)) {
|
|
CompressedImage omsg;
|
|
omsg.header = sync_image_msg.header;
|
|
omsg.format = "jpeg";
|
|
omsg.data.assign(obuf.begin(), obuf.end());
|
|
overlay_compressed_pub_->publish(omsg);
|
|
}
|
|
}
|
|
}
|
|
|
|
#ifdef ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION
|
|
if (enable_target_observation_ &&
|
|
debug_target_observation_ &&
|
|
target_observation_processor_ &&
|
|
detection_debug_compressed_pub_) {
|
|
cv::Mat detection_debug_vis = cam_bgr.clone();
|
|
if (!detection_debug_vis.empty()) {
|
|
std::vector<int> detection_stable_ids(
|
|
target_debug.poses.size(), -1);
|
|
for (size_t row = 0; row < target_debug.row_stable_ids.size() &&
|
|
row < target_debug.row_detection_indices.size();
|
|
++row) {
|
|
const int sid = target_debug.row_stable_ids[row];
|
|
const int det_idx = target_debug.row_detection_indices[row];
|
|
if (sid >= 0 &&
|
|
det_idx >= 0 &&
|
|
det_idx < static_cast<int>(target_debug.poses.size())) {
|
|
detection_stable_ids[static_cast<size_t>(det_idx)] = sid;
|
|
}
|
|
}
|
|
|
|
for (size_t det_idx = 0; det_idx < target_debug.poses.size(); ++det_idx) {
|
|
const auto& pose = target_debug.poses[det_idx];
|
|
const int sid = detection_stable_ids[det_idx];
|
|
const auto color = stable_track_color_rgba(sid, 1.0f);
|
|
const cv::Scalar bgr(
|
|
static_cast<int>(std::lround(color.b * 255.0f)),
|
|
static_cast<int>(std::lround(color.g * 255.0f)),
|
|
static_cast<int>(std::lround(color.r * 255.0f)));
|
|
cv::Rect bbox(
|
|
static_cast<int>(std::lround(pose.box.x)),
|
|
static_cast<int>(std::lround(pose.box.y)),
|
|
static_cast<int>(std::lround(pose.box.width)),
|
|
static_cast<int>(std::lround(pose.box.height)));
|
|
cv::rectangle(detection_debug_vis, bbox, bgr, 2, cv::LINE_AA);
|
|
}
|
|
odin_ros_driver::draw_target_observation_overlay(
|
|
detection_debug_vis, target_observation);
|
|
if (target_debug.detection_roi.area() > 0) {
|
|
cv::rectangle(
|
|
detection_debug_vis,
|
|
target_debug.detection_roi,
|
|
cv::Scalar(0, 255, 255), 2);
|
|
cv::putText(
|
|
detection_debug_vis, "detection_roi",
|
|
cv::Point(
|
|
target_debug.detection_roi.x + 8,
|
|
target_debug.detection_roi.y + 22),
|
|
cv::FONT_HERSHEY_SIMPLEX, 0.55,
|
|
cv::Scalar(0, 255, 255), 2);
|
|
}
|
|
|
|
std::vector<uchar> dbuf;
|
|
const std::vector<int> denc = {
|
|
cv::IMWRITE_JPEG_QUALITY, overlay_jpeg_quality_};
|
|
if (cv::imencode(".jpg", detection_debug_vis, dbuf, denc)) {
|
|
CompressedImage dmsg;
|
|
dmsg.header = sync_image_msg.header;
|
|
dmsg.format = "jpeg";
|
|
dmsg.data.assign(dbuf.begin(), dbuf.end());
|
|
detection_debug_compressed_pub_->publish(dmsg);
|
|
}
|
|
}
|
|
}
|
|
#endif
|
|
|
|
if (combined_pub_) {
|
|
cv::Mat depth_vis = build_depth_vis_from_cloud_cam(cloud_cam, *reprojector_);
|
|
if (!depth_vis.empty()) {
|
|
const int H = std::max(depth_vis.rows, cam_bgr.rows);
|
|
cv::Mat left = odin_ros_driver::resize_to_height(depth_vis, H);
|
|
cv::Mat right = odin_ros_driver::resize_to_height(cam_bgr, H);
|
|
cv::Mat combined;
|
|
cv::hconcat(left, right, combined);
|
|
|
|
std::vector<uchar> buf;
|
|
const std::vector<int> enc_params = {
|
|
cv::IMWRITE_JPEG_QUALITY, combined_jpeg_quality_};
|
|
if (cv::imencode(".jpg", combined, buf, enc_params)) {
|
|
CompressedImage out;
|
|
out.header = sync_image_msg.header;
|
|
out.format = "jpeg";
|
|
out.data.assign(buf.begin(), buf.end());
|
|
combined_pub_->publish(out);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
};
|
|
|
|
int main(int argc, char** argv)
|
|
{
|
|
rclcpp::init(argc, argv);
|
|
|
|
try {
|
|
auto node = std::make_shared<CloudReprojectionSyncedRosNode>();
|
|
rclcpp::spin(node);
|
|
} catch (const std::exception& e) {
|
|
auto logger = rclcpp::get_logger("cloud_reprojection_synced_ros2_node");
|
|
RCLCPP_ERROR(logger, "Failed to start synced target observation node: %s", e.what());
|
|
rclcpp::shutdown();
|
|
return 1;
|
|
}
|
|
|
|
rclcpp::shutdown();
|
|
return 0;
|
|
}
|