1723 lines
77 KiB
C++
1723 lines
77 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_reprojection_ros_node.hpp"
|
|
#include "cloud_reprojection_processing.hpp"
|
|
|
|
#include <algorithm>
|
|
#include <chrono>
|
|
#include <filesystem>
|
|
#include <fstream>
|
|
#include <iomanip>
|
|
#include <sstream>
|
|
#include <sys/stat.h>
|
|
#include <thread>
|
|
#include <chrono>
|
|
#include <vector>
|
|
#include <cstdint>
|
|
#include <opencv2/imgcodecs.hpp>
|
|
|
|
#ifdef ROS2
|
|
#include <sensor_msgs/msg/compressed_image.hpp>
|
|
#include <geometry_msgs/msg/point_stamped.hpp>
|
|
#include <std_msgs/msg/float32_multi_array.hpp>
|
|
#include <std_msgs/msg/int32.hpp>
|
|
#include "odin_ros_driver/msg/target_observation.hpp"
|
|
#endif
|
|
|
|
namespace {
|
|
std::filesystem::path get_package_source_directory_from_file()
|
|
{
|
|
return std::filesystem::path(__FILE__).parent_path().parent_path();
|
|
}
|
|
|
|
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::Scalar stable_track_color_bgr(int track_id)
|
|
{
|
|
if (track_id < 0) {
|
|
return cv::Scalar(255, 255, 255);
|
|
}
|
|
const auto color = stable_track_color_rgba(track_id, 1.0f);
|
|
return cv::Scalar(
|
|
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)));
|
|
}
|
|
|
|
void draw_pose_with_yolo_api(
|
|
cv::Mat& image,
|
|
const yolos::pose::PoseResult& pose,
|
|
const cv::Scalar& color,
|
|
int kpt_radius = 3,
|
|
float kpt_threshold = 0.3f,
|
|
int line_thickness = 2)
|
|
{
|
|
if (image.empty()) {
|
|
return;
|
|
}
|
|
cv::Mat skeleton_vis = cv::Mat::zeros(image.rows, image.cols, CV_8UC3);
|
|
yolos::drawing::drawPoseSkeleton(
|
|
skeleton_vis,
|
|
pose.keypoints,
|
|
yolos::pose::YOLOPoseDetector::getPoseSkeleton(),
|
|
kpt_radius,
|
|
kpt_threshold,
|
|
line_thickness);
|
|
cv::Mat mask;
|
|
cv::cvtColor(skeleton_vis, mask, cv::COLOR_BGR2GRAY);
|
|
image.setTo(color, mask > 0);
|
|
}
|
|
|
|
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 format_vector3f(const Eigen::Vector3f& value)
|
|
{
|
|
std::ostringstream oss;
|
|
oss << std::fixed << std::setprecision(2)
|
|
<< "[" << value.x() << ", " << value.y() << ", " << value.z() << "]";
|
|
return oss.str();
|
|
}
|
|
|
|
std::string format_bbox_xyxy(const std::array<float, 4>& bbox)
|
|
{
|
|
std::ostringstream oss;
|
|
oss << std::fixed << std::setprecision(1)
|
|
<< "[" << 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();
|
|
}
|
|
|
|
// Format the per-row ReID comparisons as
|
|
// bytetrack_id=[2->[sid0:0.990, sid1:0.566],1->[]]
|
|
// One group per bytetrack raw_id, inside are the sim against each
|
|
// candidate stable_id's gallery. Rows with no available comparisons emit [].
|
|
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 << std::fixed << std::setprecision(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; // sort by sim desc
|
|
});
|
|
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 << std::fixed << std::setprecision(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 << "]";
|
|
}
|
|
if (first) {
|
|
return "none";
|
|
}
|
|
return oss.str();
|
|
}
|
|
|
|
std::string resolve_camera_sync_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;
|
|
}
|
|
} // namespace
|
|
|
|
#ifdef ROS2
|
|
#include <functional>
|
|
#include <yaml-cpp/yaml.h>
|
|
#include <rcpputils/filesystem_helper.hpp>
|
|
#else
|
|
#include <boost/bind.hpp>
|
|
#endif
|
|
|
|
// Fixed Til (T_imu_lidar): lidar position in imu frame, transforms from lidar to imu
|
|
// TODO: Fill in the actual Til values for your sensor setup
|
|
static 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;
|
|
}
|
|
|
|
static bool fileExists(const std::string& filename) {
|
|
struct stat buffer;
|
|
return (stat(filename.c_str(), &buffer) == 0);
|
|
}
|
|
|
|
#ifdef ROS2
|
|
static std::string get_package_source_directory() {
|
|
return get_package_source_directory_from_file().string();
|
|
}
|
|
|
|
// ==================== ROS2 Implementation ====================
|
|
|
|
CloudReprojectionRosNode::CloudReprojectionRosNode(const rclcpp::NodeOptions& options)
|
|
: Node("cloud_reprojection_node", options)
|
|
{
|
|
loadParameters();
|
|
|
|
RCLCPP_INFO_STREAM(this->get_logger(),
|
|
"\n cloud_slam_topic: " << cloud_slam_topic_
|
|
<< "\n odometry_topic: " << odometry_topic_
|
|
<< "\n wiwc_topic: " << wiwc_topic_
|
|
<< "\n camera_image_topic: " << camera_image_topic_
|
|
<< "\n camera_image_transport: " << (camera_image_compressed_ ? "compressed" : "raw")
|
|
<< "\n always_send_sync_compressed: " << (always_send_sync_compressed_ ? "on" : "off")
|
|
<< "\n always_send_sync_cloud_slam: " << (always_send_sync_cloud_slam_ ? "on" : "off")
|
|
<< "\n sync_image_topic: " << sync_image_topic_
|
|
<< "\n sync_image_compressed_topic: "
|
|
<< ((camera_image_compressed_ || always_send_sync_compressed_) ? sync_image_compressed_topic_ : "<disabled>")
|
|
<< "\n sync_* topics under: " << 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
|
|
);
|
|
|
|
cloud_sub_.subscribe(this, cloud_slam_topic_);
|
|
odom_sub_.subscribe(this, odometry_topic_);
|
|
wiwc_sub_.subscribe(this, wiwc_topic_);
|
|
if (camera_image_compressed_) {
|
|
compressed_image_sub_.subscribe(this, camera_image_topic_);
|
|
compressed_sync_ = std::make_shared<CompressedSync>(
|
|
CompressedSyncPolicy(10), cloud_sub_, odom_sub_, wiwc_sub_, compressed_image_sub_);
|
|
compressed_sync_->registerCallback(std::bind(
|
|
&CloudReprojectionRosNode::syncCallbackCompressed,
|
|
this,
|
|
std::placeholders::_1,
|
|
std::placeholders::_2,
|
|
std::placeholders::_3,
|
|
std::placeholders::_4));
|
|
} else {
|
|
image_sub_.subscribe(this, camera_image_topic_);
|
|
raw_sync_ = std::make_shared<RawSync>(
|
|
RawSyncPolicy(10), cloud_sub_, odom_sub_, wiwc_sub_, image_sub_);
|
|
raw_sync_->registerCallback(std::bind(
|
|
&CloudReprojectionRosNode::syncCallbackRaw,
|
|
this,
|
|
std::placeholders::_1,
|
|
std::placeholders::_2,
|
|
std::placeholders::_3,
|
|
std::placeholders::_4));
|
|
}
|
|
|
|
sync_cloud_pub_ = this->create_publisher<PointCloud2>(sync_cloud_topic_, 10);
|
|
if (always_send_sync_cloud_slam_) {
|
|
sync_cloud_slam_pub_ = this->create_publisher<PointCloud2>(sync_cloud_slam_topic_, 10);
|
|
}
|
|
sync_odom_pub_ = this->create_publisher<Odometry>(sync_odom_topic_, 10);
|
|
sync_wiwc_pub_ = this->create_publisher<Odometry>(sync_wiwc_topic_, 10);
|
|
sync_image_pub_ = this->create_publisher<Image>(sync_image_topic_, 10);
|
|
if (camera_image_compressed_ || always_send_sync_compressed_) {
|
|
sync_image_compressed_pub_ =
|
|
this->create_publisher<CompressedImage>(sync_image_compressed_topic_, 10);
|
|
}
|
|
|
|
if (send_overlay_) {
|
|
overlay_compressed_pub_ =
|
|
this->create_publisher<CompressedImage>(sync_overlay_image_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);
|
|
}
|
|
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);
|
|
RCLCPP_INFO(
|
|
this->get_logger(),
|
|
"Target observation publishers created | observation=%s | tracks=%s | "
|
|
"detection_pos_cam=%s | detection_pos_world=%s",
|
|
sync_target_observation_topic_.c_str(),
|
|
sync_track_observations_topic_.c_str(),
|
|
sync_detection_pos_cam_topic_.c_str(),
|
|
sync_detection_pos_world_topic_.c_str());
|
|
}
|
|
#endif
|
|
|
|
RCLCPP_INFO(this->get_logger(), "CloudReprojectionRosNode initialized (4-way sync + optional overlay + combined)");
|
|
}
|
|
|
|
void CloudReprojectionRosNode::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();
|
|
|
|
// Declare and get parameters
|
|
this->declare_parameter<std::string>("cloud_slam_topic", "/odin1/cloud_slam");
|
|
this->declare_parameter<std::string>("odometry_topic", "/odin1/odometry");
|
|
this->declare_parameter<std::string>("wiwc_topic", "/odin1/wiwc");
|
|
this->declare_parameter<std::string>("register_keys.sync_camera_topic", "/odin1/image");
|
|
this->declare_parameter<int>("register_keys.sync_camera_compressed", 0);
|
|
this->declare_parameter<int>("register_keys.always_send_sync_compressed", 0);
|
|
this->declare_parameter<int>("register_keys.always_send_sync_cloud_slam", 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
|
|
|
|
cloud_slam_topic_ = this->get_parameter("cloud_slam_topic").as_string();
|
|
odometry_topic_ = this->get_parameter("odometry_topic").as_string();
|
|
wiwc_topic_ = this->get_parameter("wiwc_topic").as_string();
|
|
camera_image_compressed_ =
|
|
(this->get_parameter("register_keys.sync_camera_compressed").as_int() != 0);
|
|
always_send_sync_compressed_ =
|
|
(this->get_parameter("register_keys.always_send_sync_compressed").as_int() != 0);
|
|
always_send_sync_cloud_slam_ =
|
|
(this->get_parameter("register_keys.always_send_sync_cloud_slam").as_int() != 0);
|
|
camera_image_topic_ = resolve_camera_sync_topic(
|
|
this->get_parameter("register_keys.sync_camera_topic").as_string(),
|
|
camera_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_ = this->get_parameter("register_keys.combined_jpeg_quality").as_int();
|
|
combined_jpeg_quality_ = std::max(1, std::min(100, combined_jpeg_quality_));
|
|
overlay_jpeg_quality_ = this->get_parameter("register_keys.overlay_jpeg_quality").as_int();
|
|
overlay_jpeg_quality_ = std::max(1, std::min(100, overlay_jpeg_quality_));
|
|
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_cloud_topic_ = sync_topic_prefix_ + "/cloud_in_cam";
|
|
sync_cloud_slam_topic_ = sync_topic_prefix_ + "/cloud_slam";
|
|
sync_odom_topic_ = sync_topic_prefix_ + "/odometry";
|
|
sync_wiwc_topic_ = sync_topic_prefix_ + "/wiwc";
|
|
sync_image_topic_ = sync_topic_prefix_ + "/image";
|
|
sync_image_compressed_topic_ = sync_topic_prefix_ + "/image/compressed";
|
|
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";
|
|
|
|
// Load camera parameters from calib.yaml file directly
|
|
std::string calib_file = (package_path / "config" / "calib.yaml").string();
|
|
|
|
YAML::Node calib_config;
|
|
try {
|
|
calib_config = YAML::LoadFile(calib_file);
|
|
} catch (const std::exception& e) {
|
|
RCLCPP_ERROR(this->get_logger(), "Failed to load calib.yaml: %s", e.what());
|
|
rclcpp::shutdown();
|
|
return;
|
|
}
|
|
|
|
CloudReprojector::CameraParams cam_params;
|
|
try {
|
|
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>();
|
|
} catch (const std::exception& e) {
|
|
RCLCPP_ERROR(this->get_logger(), "Failed to parse camera parameters: %s", e.what());
|
|
rclcpp::shutdown();
|
|
return;
|
|
}
|
|
|
|
// Load extrinsic parameters
|
|
CloudReprojector::ExtrinsicParams ext_params;
|
|
try {
|
|
auto Tcl_vec = calib_config["Tcl_0"].as<std::vector<double>>();
|
|
|
|
if (Tcl_vec.size() == 16)
|
|
{
|
|
for (int i = 0; i < 4; ++i)
|
|
for (int j = 0; j < 4; ++j)
|
|
ext_params.Tcl(i, j) = Tcl_vec[i * 4 + j];
|
|
}
|
|
else
|
|
{
|
|
RCLCPP_ERROR(this->get_logger(), "Tcl_0 has invalid size: %zu (expected 16)", Tcl_vec.size());
|
|
rclcpp::shutdown();
|
|
return;
|
|
}
|
|
} catch (const std::exception& e) {
|
|
RCLCPP_ERROR(this->get_logger(), "Failed to parse Tcl_0: %s", e.what());
|
|
rclcpp::shutdown();
|
|
return;
|
|
}
|
|
|
|
ext_params.Til = getFixedTil();
|
|
ext_params.Tic = CloudReprojector::calculateTic(ext_params.Tcl, ext_params.Til);
|
|
|
|
RCLCPP_INFO_STREAM(this->get_logger(), "Loaded Tcl (camera to lidar):\n" << ext_params.Tcl);
|
|
RCLCPP_INFO_STREAM(this->get_logger(), "Fixed Til (lidar to imu):\n" << ext_params.Til);
|
|
RCLCPP_INFO_STREAM(this->get_logger(), "Calculated Tic:\n" << ext_params.Tic);
|
|
|
|
RCLCPP_INFO(this->get_logger(), "Camera intrinsics:");
|
|
RCLCPP_INFO(this->get_logger(), "Image size: %dx%d", cam_params.image_width, cam_params.image_height);
|
|
RCLCPP_INFO(this->get_logger(), "Intrinsics: A11=%f A12=%f A22=%f u0=%f v0=%f",
|
|
cam_params.A11, cam_params.A12, cam_params.A22, cam_params.u0, cam_params.v0);
|
|
|
|
reprojector_ = std::make_unique<CloudReprojector>();
|
|
if (!reprojector_->initialize(cam_params, ext_params))
|
|
{
|
|
RCLCPP_ERROR(this->get_logger(), "Failed to initialize CloudReprojector");
|
|
rclcpp::shutdown();
|
|
}
|
|
|
|
#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_) {
|
|
try {
|
|
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);
|
|
RCLCPP_INFO(
|
|
this->get_logger(),
|
|
"Target observation enabled | yolo_engine=%s | reid=%s | reid_engine=%s",
|
|
target_config.yolo_engine_path.c_str(),
|
|
target_config.reid_enabled ? "on" : "off",
|
|
target_config.reid_engine_path.empty() ? "<disabled>" : target_config.reid_engine_path.c_str());
|
|
RCLCPP_INFO(
|
|
this->get_logger(),
|
|
"Target observation processor initialized successfully | labels=%s",
|
|
target_config.yolo_labels_path.empty() ? "<default-person>" : target_config.yolo_labels_path.c_str());
|
|
if (debug_target_observation_) {
|
|
RCLCPP_INFO(
|
|
this->get_logger(),
|
|
"Target observation debug enabled | yolo_engine=%s | yolo_labels=%s | yolo_conf=%.3f | yolo_nms=%.3f | min_depth=%.2f | max_depth=%.2f | search_radius_px=%.1f | lost_comp=%s | lost_comp_scale=%.2f | lost_comp_min=%d | reid=%s | reid_engine=%s | reid_match=%.3f | reid_gap=%.3f | reid_gallery=%d | reid_update_interval=%d | reid_timeout=%d",
|
|
target_config.yolo_engine_path.c_str(),
|
|
target_config.yolo_labels_path.empty() ? "<default-person>" : target_config.yolo_labels_path.c_str(),
|
|
target_config.yolo_conf,
|
|
target_config.yolo_nms,
|
|
target_config.min_depth,
|
|
target_config.max_depth,
|
|
target_config.search_radius_px,
|
|
target_config.lost_detection_compensation_enabled ? "on" : "off",
|
|
target_config.lost_detection_roi_scale,
|
|
target_config.lost_detection_roi_min_size_px,
|
|
target_config.reid_enabled ? "on" : "off",
|
|
target_config.reid_engine_path.empty() ? "<disabled>" : target_config.reid_engine_path.c_str(),
|
|
target_config.reid_match_threshold,
|
|
target_config.reid_gap_threshold,
|
|
target_config.reid_gallery_size,
|
|
target_config.reid_feature_update_interval,
|
|
target_config.reid_lost_timeout_frames);
|
|
}
|
|
} catch (const std::exception& e) {
|
|
enable_target_observation_ = false;
|
|
target_observation_processor_.reset();
|
|
RCLCPP_ERROR(
|
|
this->get_logger(),
|
|
"Failed to initialize target observation processor: %s",
|
|
e.what());
|
|
}
|
|
}
|
|
#endif
|
|
}
|
|
|
|
void CloudReprojectionRosNode::processSyncedData(
|
|
const PointCloud2::ConstSharedPtr& cloud_msg,
|
|
const Odometry::ConstSharedPtr& odom_msg,
|
|
const Odometry::ConstSharedPtr& wiwc_msg,
|
|
const Image& image_msg,
|
|
const cv::Mat& cam_bgr)
|
|
{
|
|
const auto sync_stamp = image_msg.header.stamp;
|
|
|
|
PointCloud2 sync_cloud_slam_msg = *cloud_msg;
|
|
sync_cloud_slam_msg.header.stamp = sync_stamp;
|
|
Odometry sync_odom_msg = *odom_msg;
|
|
sync_odom_msg.header.stamp = sync_stamp;
|
|
Odometry sync_wiwc_msg = *wiwc_msg;
|
|
sync_wiwc_msg.header.stamp = sync_stamp;
|
|
Image sync_image_msg = image_msg;
|
|
sync_image_msg.header.stamp = sync_stamp;
|
|
|
|
|
|
|
|
pcl::PointCloud<pcl::PointXYZRGB> cloud_odom;
|
|
pcl::fromROSMsg(*cloud_msg, cloud_odom);
|
|
|
|
if (cloud_odom.empty())
|
|
{
|
|
RCLCPP_WARN(this->get_logger(), "Empty cloud_slam 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[i];
|
|
T_IL(i / 4, i % 4) = wiwc_msg->twist.covariance[i];
|
|
}
|
|
|
|
bool T_CL_valid = (T_CL - Eigen::Matrix4d::Identity()).norm() > 1e-6;
|
|
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
|
|
);
|
|
|
|
pcl::PointCloud<pcl::PointXYZRGB> cloud_cam =
|
|
reprojector_->transformCloudToCamera(cloud_odom, odom_pose);
|
|
if (cloud_cam.empty())
|
|
{
|
|
RCLCPP_WARN(this->get_logger(), "Camera-frame cloud is empty after reprojection");
|
|
return;
|
|
}
|
|
|
|
PointCloud2 cloud_cam_msg;
|
|
pcl::toROSMsg(cloud_cam, cloud_cam_msg);
|
|
cloud_cam_msg.header = image_msg.header;
|
|
cloud_cam_msg.header.stamp = sync_stamp;
|
|
if (cloud_cam_msg.header.frame_id.empty()) {
|
|
// cloud_cam_msg.header.frame_id = cloud_msg->header.frame_id;
|
|
cloud_cam_msg.header.frame_id = odom_msg->child_frame_id; // odin1_base_link
|
|
}
|
|
|
|
|
|
const bool need_combined = (combined_pub_ != nullptr);
|
|
cv::Mat depth_vis;
|
|
if (need_combined) {
|
|
depth_vis = reprojector_->reprojectCloudDepth(cloud_odom, odom_pose);
|
|
if (depth_vis.empty()) {
|
|
return;
|
|
}
|
|
}
|
|
|
|
const bool need_cam_bgr =
|
|
send_overlay_ || need_combined
|
|
#ifdef ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION
|
|
|| enable_target_observation_
|
|
#endif
|
|
;
|
|
if (need_cam_bgr && cam_bgr.empty()) {
|
|
RCLCPP_ERROR(this->get_logger(), "Synced image decode/convert failed");
|
|
return;
|
|
}
|
|
|
|
if (sync_cloud_slam_pub_) {
|
|
sync_cloud_slam_pub_->publish(sync_cloud_slam_msg);
|
|
}
|
|
sync_odom_pub_->publish(sync_odom_msg);
|
|
sync_wiwc_pub_->publish(sync_wiwc_msg);
|
|
sync_cloud_pub_->publish(cloud_cam_msg);
|
|
sync_image_pub_->publish(sync_image_msg);
|
|
|
|
#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();
|
|
|
|
// Exactly one of (sim, fallback_dist) is meaningful per rebind path;
|
|
// the other stays at -1 as "N/A". Emit a compact reason string so
|
|
// operators see which mechanism fired and how confident it was.
|
|
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(),
|
|
"\033[1;33m[LazyReID]\033[0m \033[1;32mRECOVERED\033[0m 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(),
|
|
"\033[1;33m[LazyReID]\033[0m \033[1;31mNO_MATCH\033[0m 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(
|
|
image_msg.header,
|
|
"bgr8",
|
|
gallery_debug).toImageMsg();
|
|
msg->header.stamp = sync_stamp;
|
|
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=%.2f 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,
|
|
target_debug.projected_cloud_points,
|
|
target_debug.depth_sample_count,
|
|
target_debug.yolo_ms,
|
|
target_debug.mot_ms,
|
|
target_debug.cloud_project_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=%.2f bind=%.2f (depth=%.2f reid=%.2f) age=%.2f select=%.2f enrich=%.2f | total=%.2f ms | node_total=%.2f ms",
|
|
image_msg.header.stamp.sec,
|
|
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(),
|
|
target_debug.projected_cloud_points,
|
|
target_debug.depth_sample_count,
|
|
target_debug.yolo_ms,
|
|
target_debug.mot_ms,
|
|
target_debug.cloud_project_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());
|
|
}
|
|
}
|
|
|
|
// Helper: fill a TargetObservation ROS msg from the C++ struct.
|
|
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;
|
|
}
|
|
};
|
|
|
|
// Single-target topic — still published for backwards-compatible
|
|
// downstream that only cares about the follow target.
|
|
odin_ros_driver::msg::TargetObservation observation_msg;
|
|
observation_msg.header = image_msg.header;
|
|
observation_msg.header.stamp = sync_stamp;
|
|
observation_msg.odometry = sync_odom_msg;
|
|
fill_observation_msg(observation_msg, target_observation);
|
|
target_observation_pub_->publish(observation_msg);
|
|
|
|
// Multi-target topic — every retained stable track every frame.
|
|
// valid=true only when this frame has a fresh 3D estimate; otherwise
|
|
// the row is still published with valid=false so downstream dataset
|
|
// builders no longer need to synthesize placeholder observations.
|
|
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 = image_msg.header;
|
|
track_array_msg.header.stamp = sync_stamp;
|
|
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 = image_msg.header;
|
|
one.header.stamp = sync_stamp;
|
|
one.odometry = sync_odom_msg;
|
|
fill_observation_msg(one, obs);
|
|
track_array_msg.observations.push_back(std::move(one));
|
|
}
|
|
track_observations_pub_->publish(track_array_msg);
|
|
|
|
// Debug Marker topics — rendered directly in RViz. Primary target in
|
|
// red, others in cyan; short lifetime so stale markers auto-clear.
|
|
std_msgs::msg::Header cam_header = image_msg.header;
|
|
cam_header.stamp = sync_stamp;
|
|
cam_header.frame_id = cloud_cam_msg.header.frame_id.empty()
|
|
? std::string("camera")
|
|
: cloud_cam_msg.header.frame_id;
|
|
std_msgs::msg::Header world_header = image_msg.header;
|
|
world_header.stamp = sync_stamp;
|
|
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);
|
|
// DELETEALL first clears any leftover markers from prior frames.
|
|
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; // 0.3 s
|
|
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());
|
|
if (!overlay_vis.empty()) {
|
|
#ifdef ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION
|
|
// Draw the detection ROI (yellow rectangle) when it is active so
|
|
// operators can see which portion of the image the primary YOLO
|
|
// pass is actually looking at.
|
|
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
|
|
std::vector<uchar> obuf;
|
|
const std::vector<int> oenc = {cv::IMWRITE_JPEG_QUALITY, overlay_jpeg_quality_};
|
|
if (!cv::imencode(".jpg", overlay_vis, obuf, oenc)) {
|
|
RCLCPP_ERROR(this->get_logger(), "cv::imencode failed (overlay)");
|
|
return;
|
|
}
|
|
CompressedImage omsg;
|
|
omsg.header = 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 (det_idx < 0 ||
|
|
det_idx >= static_cast<int>(target_debug.poses.size())) {
|
|
continue;
|
|
}
|
|
if (sid >= 0) {
|
|
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 cv::Scalar color = stable_track_color_bgr(sid);
|
|
draw_pose_with_yolo_api(detection_debug_vis, pose, color, 3, 0.3f, 2);
|
|
const 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, color, 2, cv::LINE_AA);
|
|
}
|
|
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())) {
|
|
continue;
|
|
}
|
|
const auto& pose = target_debug.poses[static_cast<size_t>(det_idx)];
|
|
const cv::Scalar color = stable_track_color_bgr(sid);
|
|
const 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, color, 3, cv::LINE_AA);
|
|
const std::string sid_text = "sid=" + std::to_string(sid);
|
|
cv::putText(
|
|
detection_debug_vis,
|
|
sid_text,
|
|
cv::Point(bbox.x, std::max(20, bbox.y - 10)),
|
|
cv::FONT_HERSHEY_SIMPLEX,
|
|
0.6,
|
|
cv::Scalar(0, 0, 0),
|
|
3,
|
|
cv::LINE_AA);
|
|
cv::putText(
|
|
detection_debug_vis,
|
|
sid_text,
|
|
cv::Point(bbox.x, std::max(20, bbox.y - 10)),
|
|
cv::FONT_HERSHEY_SIMPLEX,
|
|
0.6,
|
|
color,
|
|
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)) {
|
|
RCLCPP_ERROR(this->get_logger(), "cv::imencode failed (detection debug)");
|
|
return;
|
|
}
|
|
CompressedImage dmsg;
|
|
dmsg.header = image_msg.header;
|
|
dmsg.format = "jpeg";
|
|
dmsg.data.assign(dbuf.begin(), dbuf.end());
|
|
detection_debug_compressed_pub_->publish(dmsg);
|
|
}
|
|
}
|
|
#endif
|
|
|
|
if (combined_pub_) {
|
|
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)) {
|
|
RCLCPP_ERROR(this->get_logger(), "cv::imencode failed");
|
|
return;
|
|
}
|
|
|
|
CompressedImage out;
|
|
out.header = image_msg.header;
|
|
out.format = "jpeg";
|
|
out.data.assign(buf.begin(), buf.end());
|
|
combined_pub_->publish(out);
|
|
}
|
|
}
|
|
|
|
void CloudReprojectionRosNode::syncCallbackRaw(
|
|
const PointCloud2::ConstSharedPtr& cloud_msg,
|
|
const Odometry::ConstSharedPtr& odom_msg,
|
|
const Odometry::ConstSharedPtr& wiwc_msg,
|
|
const Image::ConstSharedPtr& image_msg)
|
|
{
|
|
cv::Mat cam_bgr;
|
|
const bool need_cam_bgr =
|
|
send_overlay_ || (combined_pub_ != nullptr)
|
|
#ifdef ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION
|
|
|| enable_target_observation_
|
|
#endif
|
|
;
|
|
if (need_cam_bgr) {
|
|
try {
|
|
cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(image_msg, "bgr8");
|
|
cam_bgr = cv_ptr->image;
|
|
} catch (const cv_bridge::Exception& e) {
|
|
RCLCPP_ERROR(this->get_logger(), "cv_bridge (sync image): %s", e.what());
|
|
return;
|
|
}
|
|
}
|
|
if (always_send_sync_compressed_ && sync_image_compressed_pub_) {
|
|
std::vector<uchar> buf;
|
|
if (!cv::imencode(".jpg", cam_bgr, buf)) {
|
|
RCLCPP_ERROR(this->get_logger(), "cv::imencode failed (sync compressed image)");
|
|
return;
|
|
}
|
|
CompressedImage sync_compressed_msg;
|
|
sync_compressed_msg.header = image_msg->header;
|
|
sync_compressed_msg.format = "jpeg";
|
|
sync_compressed_msg.data.assign(buf.begin(), buf.end());
|
|
sync_image_compressed_pub_->publish(sync_compressed_msg);
|
|
}
|
|
processSyncedData(cloud_msg, odom_msg, wiwc_msg, *image_msg, cam_bgr);
|
|
}
|
|
|
|
void CloudReprojectionRosNode::syncCallbackCompressed(
|
|
const PointCloud2::ConstSharedPtr& cloud_msg,
|
|
const Odometry::ConstSharedPtr& odom_msg,
|
|
const Odometry::ConstSharedPtr& wiwc_msg,
|
|
const CompressedImage::ConstSharedPtr& image_msg)
|
|
{
|
|
if (image_msg->data.empty()) {
|
|
RCLCPP_WARN(this->get_logger(), "Empty compressed image received");
|
|
return;
|
|
}
|
|
const cv::Mat encoded(
|
|
1,
|
|
static_cast<int>(image_msg->data.size()),
|
|
CV_8UC1,
|
|
const_cast<unsigned char*>(image_msg->data.data()));
|
|
cv::Mat cam_bgr = cv::imdecode(encoded, cv::IMREAD_COLOR);
|
|
if (cam_bgr.empty()) {
|
|
RCLCPP_ERROR(this->get_logger(), "cv::imdecode failed (sync compressed image)");
|
|
return;
|
|
}
|
|
if (sync_image_compressed_pub_) {
|
|
CompressedImage sync_compressed_msg = *image_msg;
|
|
sync_image_compressed_pub_->publish(sync_compressed_msg);
|
|
}
|
|
auto sync_image_msg =
|
|
cv_bridge::CvImage(image_msg->header, "bgr8", cam_bgr).toImageMsg();
|
|
processSyncedData(cloud_msg, odom_msg, wiwc_msg, *sync_image_msg, cam_bgr);
|
|
}
|
|
|
|
// ==================== ROS2 Main ====================
|
|
int main(int argc, char** argv)
|
|
{
|
|
rclcpp::init(argc, argv);
|
|
|
|
auto temp_node = std::make_shared<rclcpp::Node>("cloud_reprojection_check");
|
|
|
|
// Check if reprojection is enabled from control_command.yaml
|
|
std::string package_path = get_package_source_directory();
|
|
std::string config_file = package_path + "/config/control_command.yaml";
|
|
|
|
try {
|
|
YAML::Node config = YAML::LoadFile(config_file);
|
|
std::cout << "config: " << config_file << std::endl;
|
|
if (!config["register_keys"] || !config["register_keys"]["sendreprojection"]) {
|
|
RCLCPP_INFO(temp_node->get_logger(), "sendreprojection parameter not found, cloud reprojection disabled.");
|
|
rclcpp::shutdown();
|
|
return 0;
|
|
}
|
|
|
|
int sendreprojection = config["register_keys"]["sendreprojection"].as<int>();
|
|
if (sendreprojection == 0) {
|
|
RCLCPP_INFO(temp_node->get_logger(), "Cloud reprojection will not be published.");
|
|
rclcpp::shutdown();
|
|
return 0;
|
|
}
|
|
} catch (const std::exception& e) {
|
|
RCLCPP_ERROR(temp_node->get_logger(), "Failed to read config: %s", e.what());
|
|
rclcpp::shutdown();
|
|
return 1;
|
|
}
|
|
|
|
// Wait for calib.yaml file to be generated by host_sdk_sample
|
|
std::string calib_file = package_path + "/config/calib.yaml";
|
|
RCLCPP_INFO(temp_node->get_logger(), "Waiting for calib.yaml file at: %s", calib_file.c_str());
|
|
|
|
int wait_count = 0;
|
|
while (rclcpp::ok() && !fileExists(calib_file)) {
|
|
if (wait_count % 10 == 0) {
|
|
RCLCPP_INFO(temp_node->get_logger(), "Still waiting for calib.yaml file...");
|
|
}
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(5000));
|
|
wait_count++;
|
|
|
|
// Timeout after 5 seconds
|
|
if (wait_count > 10) {
|
|
RCLCPP_ERROR(temp_node->get_logger(), "Timeout waiting for calib.yaml file");
|
|
rclcpp::shutdown();
|
|
return 1;
|
|
}
|
|
}
|
|
|
|
if (!rclcpp::ok()) {
|
|
RCLCPP_INFO(temp_node->get_logger(), "Node shutdown before calib.yaml file was found.");
|
|
return 0;
|
|
}
|
|
|
|
RCLCPP_INFO(temp_node->get_logger(), "Found calib.yaml file! Starting cloud reprojection node...");
|
|
|
|
auto node = std::make_shared<CloudReprojectionRosNode>();
|
|
|
|
RCLCPP_INFO(node->get_logger(), "CloudReprojectionRosNode started");
|
|
|
|
rclcpp::spin(node);
|
|
rclcpp::shutdown();
|
|
|
|
return 0;
|
|
}
|
|
|
|
#else
|
|
// ==================== ROS1 Implementation ====================
|
|
|
|
CloudReprojectionRosNode::CloudReprojectionRosNode(ros::NodeHandle& nh, ros::NodeHandle& pnh)
|
|
: nh_(nh), pnh_(pnh)
|
|
{
|
|
loadParameters();
|
|
|
|
ROS_INFO_STREAM("\n cloud_slam_topic: " << cloud_slam_topic_
|
|
<< "\n odometry_topic: " << odometry_topic_
|
|
<< "\n wiwc_topic: " << wiwc_topic_
|
|
<< "\n camera_image_topic: " << camera_image_topic_
|
|
<< "\n sync_prefix: " << sync_topic_prefix_
|
|
<< "\n overlay_compressed_topic: " << sync_overlay_image_topic_
|
|
<< "\n combined_compressed_topic: " << combined_compressed_topic_
|
|
<< "\n publish_combined_compressed: " << (publish_combined_compressed_ ? "on" : "off")
|
|
<< "\n send_overlay: " << (send_overlay_ ? "on" : "off"));
|
|
|
|
cloud_sub_.subscribe(nh_, cloud_slam_topic_, 1);
|
|
odom_sub_.subscribe(nh_, odometry_topic_, 1);
|
|
wiwc_sub_.subscribe(nh_, wiwc_topic_, 1);
|
|
image_sub_.subscribe(nh_, camera_image_topic_, 1);
|
|
|
|
sync_ = std::make_shared<Sync>(MySyncPolicy(10), cloud_sub_, odom_sub_, wiwc_sub_, image_sub_);
|
|
sync_->registerCallback(boost::bind(&CloudReprojectionRosNode::syncCallback, this, _1, _2, _3, _4));
|
|
|
|
sync_cloud_pub_ = nh_.advertise<sensor_msgs::PointCloud2>(sync_cloud_topic_, 1);
|
|
sync_cloud_slam_pub_ = nh_.advertise<sensor_msgs::PointCloud2>(sync_cloud_slam_topic_, 1);
|
|
sync_odom_pub_ = nh_.advertise<nav_msgs::Odometry>(sync_odom_topic_, 1);
|
|
sync_wiwc_pub_ = nh_.advertise<nav_msgs::Odometry>(sync_wiwc_topic_, 1);
|
|
sync_image_pub_ = nh_.advertise<sensor_msgs::Image>(sync_image_topic_, 1);
|
|
|
|
if (send_overlay_) {
|
|
overlay_compressed_pub_ =
|
|
nh_.advertise<sensor_msgs::CompressedImage>(sync_overlay_image_topic_, 1);
|
|
}
|
|
if (publish_combined_compressed_) {
|
|
combined_pub_ = nh_.advertise<sensor_msgs::CompressedImage>(combined_compressed_topic_, 1);
|
|
}
|
|
|
|
ROS_INFO("CloudReprojectionRosNode initialized (4-way sync + depth + combined)");
|
|
}
|
|
|
|
void CloudReprojectionRosNode::loadParameters()
|
|
{
|
|
pnh_.param<std::string>("cloud_slam_topic", cloud_slam_topic_, std::string("/odin1/cloud_slam"));
|
|
pnh_.param<std::string>("odometry_topic", odometry_topic_, std::string("/odin1/odometry"));
|
|
pnh_.param<std::string>("wiwc_topic", wiwc_topic_, std::string("/odin1/wiwc"));
|
|
pnh_.param<std::string>("register_keys/sync_camera_topic", camera_image_topic_, std::string("/odin1/image"));
|
|
pnh_.param<std::string>("register_keys/sync_topic_prefix", sync_topic_prefix_, std::string("/odin1/sync"));
|
|
pnh_.param<std::string>("register_keys/combined_compressed_topic", combined_compressed_topic_, std::string("/odin1/combined_image/compressed"));
|
|
int jq = 85;
|
|
pnh_.param<int>("register_keys/combined_jpeg_quality", jq, 85);
|
|
combined_jpeg_quality_ = std::max(1, std::min(100, jq));
|
|
int ojq = 85;
|
|
pnh_.param<int>("register_keys/overlay_jpeg_quality", ojq, 85);
|
|
overlay_jpeg_quality_ = std::max(1, std::min(100, ojq));
|
|
int send_combined = 1;
|
|
pnh_.param<int>("register_keys/send_combined_compressed", send_combined, 1);
|
|
publish_combined_compressed_ = (send_combined != 0);
|
|
int send_ov = 1;
|
|
pnh_.param<int>("register_keys/send_overlay", send_ov, 1);
|
|
send_overlay_ = (send_ov != 0);
|
|
|
|
sync_cloud_topic_ = sync_topic_prefix_ + "/cloud_in_cam";
|
|
sync_cloud_slam_topic_ = sync_topic_prefix_ + "/cloud_slam";
|
|
sync_odom_topic_ = sync_topic_prefix_ + "/odometry";
|
|
sync_wiwc_topic_ = sync_topic_prefix_ + "/wiwc";
|
|
sync_image_topic_ = sync_topic_prefix_ + "/image";
|
|
sync_overlay_image_topic_ = sync_topic_prefix_ + "/overlay_img/compressed";
|
|
|
|
// Load camera parameters
|
|
CloudReprojector::CameraParams cam_params;
|
|
pnh_.param<int>("cam_0/image_width", cam_params.image_width, 1600);
|
|
pnh_.param<int>("cam_0/image_height", cam_params.image_height, 1296);
|
|
pnh_.param<double>("cam_0/A11", cam_params.A11, 0.0);
|
|
pnh_.param<double>("cam_0/A12", cam_params.A12, 0.0);
|
|
pnh_.param<double>("cam_0/A22", cam_params.A22, 0.0);
|
|
pnh_.param<double>("cam_0/u0", cam_params.u0, 0.0);
|
|
pnh_.param<double>("cam_0/v0", cam_params.v0, 0.0);
|
|
pnh_.param<double>("cam_0/k2", cam_params.k2, 0.0);
|
|
pnh_.param<double>("cam_0/k3", cam_params.k3, 0.0);
|
|
pnh_.param<double>("cam_0/k4", cam_params.k4, 0.0);
|
|
pnh_.param<double>("cam_0/k5", cam_params.k5, 0.0);
|
|
pnh_.param<double>("cam_0/k6", cam_params.k6, 0.0);
|
|
pnh_.param<double>("cam_0/k7", cam_params.k7, 0.0);
|
|
|
|
// Load extrinsic parameters
|
|
CloudReprojector::ExtrinsicParams ext_params;
|
|
std::vector<double> Tcl_vec_param;
|
|
if (pnh_.getParam("Tcl_0", Tcl_vec_param) && Tcl_vec_param.size() == 16)
|
|
{
|
|
for (int i = 0; i < 4; ++i)
|
|
for (int j = 0; j < 4; ++j)
|
|
ext_params.Tcl(i, j) = Tcl_vec_param[i * 4 + j];
|
|
}
|
|
else
|
|
{
|
|
ROS_ERROR("Tcl_0 param missing or invalid.");
|
|
ros::shutdown();
|
|
return;
|
|
}
|
|
|
|
ext_params.Til = getFixedTil();
|
|
ext_params.Tic = CloudReprojector::calculateTic(ext_params.Tcl, ext_params.Til);
|
|
|
|
ROS_INFO_STREAM("Loaded Tcl (camera to lidar):\n" << ext_params.Tcl);
|
|
ROS_INFO_STREAM("Fixed Til (lidar to imu):\n" << ext_params.Til);
|
|
ROS_INFO_STREAM("Calculated Tic:\n" << ext_params.Tic);
|
|
|
|
ROS_INFO("Camera intrinsics:");
|
|
ROS_INFO("Image size: %dx%d", cam_params.image_width, cam_params.image_height);
|
|
ROS_INFO("Intrinsics: A11=%f A12=%f A22=%f u0=%f v0=%f",
|
|
cam_params.A11, cam_params.A12, cam_params.A22, cam_params.u0, cam_params.v0);
|
|
|
|
reprojector_ = std::make_unique<CloudReprojector>();
|
|
if (!reprojector_->initialize(cam_params, ext_params))
|
|
{
|
|
ROS_ERROR("Failed to initialize CloudReprojector");
|
|
ros::shutdown();
|
|
}
|
|
}
|
|
|
|
void CloudReprojectionRosNode::syncCallback(
|
|
const sensor_msgs::PointCloud2ConstPtr& cloud_msg,
|
|
const nav_msgs::OdometryConstPtr& odom_msg,
|
|
const nav_msgs::OdometryConstPtr& wiwc_msg,
|
|
const sensor_msgs::ImageConstPtr& image_msg)
|
|
{
|
|
sync_cloud_slam_pub_.publish(cloud_msg);
|
|
sync_odom_pub_.publish(odom_msg);
|
|
sync_wiwc_pub_.publish(wiwc_msg);
|
|
sync_image_pub_.publish(image_msg);
|
|
|
|
pcl::PointCloud<pcl::PointXYZRGB> cloud_odom;
|
|
pcl::fromROSMsg(*cloud_msg, cloud_odom);
|
|
|
|
if (cloud_odom.empty())
|
|
{
|
|
ROS_WARN("Empty cloud_slam 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[i];
|
|
T_IL(i / 4, i % 4) = wiwc_msg->twist.covariance[i];
|
|
}
|
|
|
|
bool T_CL_valid = (T_CL - Eigen::Matrix4d::Identity()).norm() > 1e-6;
|
|
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
|
|
);
|
|
|
|
pcl::PointCloud<pcl::PointXYZRGB> cloud_cam =
|
|
reprojector_->transformCloudToCamera(cloud_odom, odom_pose);
|
|
if (cloud_cam.empty())
|
|
{
|
|
ROS_WARN("Camera-frame cloud is empty after reprojection");
|
|
return;
|
|
}
|
|
|
|
sensor_msgs::PointCloud2 cloud_cam_msg;
|
|
pcl::toROSMsg(cloud_cam, cloud_cam_msg);
|
|
cloud_cam_msg.header = image_msg->header;
|
|
if (cloud_cam_msg.header.frame_id.empty()) {
|
|
cloud_cam_msg.header.frame_id = cloud_msg->header.frame_id;
|
|
}
|
|
sync_cloud_pub_.publish(cloud_cam_msg);
|
|
|
|
const bool need_combined = publish_combined_compressed_;
|
|
cv::Mat depth_vis;
|
|
if (need_combined) {
|
|
depth_vis = reprojector_->reprojectCloudDepth(cloud_odom, odom_pose);
|
|
if (depth_vis.empty()) {
|
|
return;
|
|
}
|
|
}
|
|
|
|
cv::Mat cam_bgr;
|
|
if (send_overlay_ || need_combined) {
|
|
try {
|
|
cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(image_msg, "bgr8");
|
|
cam_bgr = cv_ptr->image;
|
|
} catch (const cv_bridge::Exception& e) {
|
|
ROS_ERROR("cv_bridge (sync image): %s", e.what());
|
|
return;
|
|
}
|
|
}
|
|
|
|
if (send_overlay_) {
|
|
cv::Mat overlay_vis =
|
|
odin_ros_driver::overlay_projected_cloud_on_image(cam_bgr, cloud_cam, reprojector_->getCameraParams());
|
|
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)) {
|
|
ROS_ERROR("cv::imencode failed (overlay)");
|
|
return;
|
|
}
|
|
sensor_msgs::CompressedImage omsg;
|
|
omsg.header = image_msg->header;
|
|
omsg.format = "jpeg";
|
|
omsg.data.assign(obuf.begin(), obuf.end());
|
|
overlay_compressed_pub_.publish(omsg);
|
|
}
|
|
}
|
|
|
|
if (publish_combined_compressed_) {
|
|
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)) {
|
|
ROS_ERROR("cv::imencode failed");
|
|
return;
|
|
}
|
|
|
|
sensor_msgs::CompressedImage out;
|
|
out.header = image_msg->header;
|
|
out.format = "jpeg";
|
|
out.data.assign(buf.begin(), buf.end());
|
|
combined_pub_.publish(out);
|
|
}
|
|
}
|
|
|
|
// ==================== ROS1 Main ====================
|
|
int main(int argc, char **argv)
|
|
{
|
|
ros::init(argc, argv, "cloud_reprojection");
|
|
ros::NodeHandle nh;
|
|
ros::NodeHandle pnh("~");
|
|
|
|
// Check if reprojection is enabled
|
|
int sendreprojection = 0;
|
|
pnh.param("register_keys/sendreprojection", sendreprojection, 0);
|
|
if(sendreprojection == 0)
|
|
{
|
|
ROS_INFO("Cloud reprojection will not be published.");
|
|
return 0;
|
|
}
|
|
|
|
std::string calib_file_path;
|
|
pnh.param<std::string>("calib_file_path", calib_file_path, "");
|
|
|
|
ROS_INFO("Waiting for calib.yaml file at: %s", calib_file_path.c_str());
|
|
while(ros::ok() && !fileExists(calib_file_path))
|
|
{
|
|
ROS_INFO_THROTTLE(5, "Still waiting for calib.yaml file...");
|
|
ros::Duration(0.5).sleep();
|
|
ros::spinOnce();
|
|
}
|
|
|
|
if(!ros::ok())
|
|
{
|
|
ROS_INFO("Node shutdown before calib.yaml file was found.");
|
|
return 0;
|
|
}
|
|
|
|
ROS_INFO("Found calib.yaml file! Loading parameters...");
|
|
|
|
std::string node_name = ros::this_node::getName();
|
|
std::string rosparam_command = "rosparam load " + calib_file_path + " " + node_name;
|
|
int result = system(rosparam_command.c_str());
|
|
|
|
if(result == 0)
|
|
{
|
|
ROS_INFO("Successfully loaded parameters from calib.yaml to namespace: %s", node_name.c_str());
|
|
}
|
|
else
|
|
{
|
|
ROS_ERROR("Failed to load parameters from calib.yaml");
|
|
return 1;
|
|
}
|
|
|
|
CloudReprojectionRosNode reprojection_node(nh, pnh);
|
|
ros::spin();
|
|
return 0;
|
|
}
|
|
#endif
|