odin_ros_driver1/src/cloud_reprojection_ros.cpp

/*
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>

#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::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_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();
}
}  // 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  sync_* topics under: " << 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")
#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  target_pos_cam_topic: " << sync_target_pos_cam_topic_
        << "\n  target_pos_world_topic: " << sync_target_pos_world_topic_
#endif
    );

    cloud_sub_.subscribe(this, cloud_slam_topic_);
    odom_sub_.subscribe(this, odometry_topic_);
    wiwc_sub_.subscribe(this, wiwc_topic_);
    image_sub_.subscribe(this, camera_image_topic_);

    sync_ = std::make_shared<Sync>(MySyncPolicy(10), cloud_sub_, odom_sub_, wiwc_sub_, image_sub_);
    sync_->registerCallback(std::bind(&CloudReprojectionRosNode::syncCallback, this,
                                       std::placeholders::_1, std::placeholders::_2, std::placeholders::_3,
                                       std::placeholders::_4));

    sync_cloud_pub_ = this->create_publisher<PointCloud2>(sync_cloud_topic_, 10);
    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 (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);
        target_pos_cam_pub_ = this->create_publisher<geometry_msgs::msg::PointStamped>(
            sync_target_pos_cam_topic_, 10);
        target_pos_world_pub_ = this->create_publisher<geometry_msgs::msg::PointStamped>(
            sync_target_pos_world_topic_, 10);
        RCLCPP_INFO(
            this->get_logger(),
            "Target observation publishers created successfully | observation=%s | pos_cam=%s | pos_world=%s",
            sync_target_observation_topic_.c_str(),
            sync_target_pos_cam_topic_.c_str(),
            sync_target_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_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<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);
#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_topic_ = this->get_parameter("register_keys.sync_camera_topic").as_string();
    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_overlay_image_topic_ = sync_topic_prefix_ + "/overlay_img/compressed";
    sync_target_observation_topic_ = sync_topic_prefix_ + "/target_observation";
    sync_target_pos_cam_topic_ = sync_topic_prefix_ + "/target_pos_cam";
    sync_target_pos_world_topic_ = sync_topic_prefix_ + "/target_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);
    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.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 | engine=%s",
                target_config.yolo_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",
                    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);
            }
        } 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::syncCallback(
    const PointCloud2::ConstSharedPtr& cloud_msg,
    const Odometry::ConstSharedPtr& odom_msg,
    const Odometry::ConstSharedPtr& wiwc_msg,
    const Image::ConstSharedPtr& image_msg)
{
    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;
        }
    }

    cv::Mat cam_bgr;
    if (send_overlay_ || need_combined
#ifdef ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION
        || enable_target_observation_
#endif
    ) {
        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;
        }
    }

    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,
            debug_target_observation_ ? &target_debug : nullptr);
        const auto target_end = std::chrono::steady_clock::now();

        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 | yolo=%.2f ms | total=%.2f ms",
                        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 | yolo=%.2f ms | mot=%.2f ms | total=%.2f ms",
                        target_debug.poses_count,
                        target_debug.current_target_id_before,
                        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 selected_id=%d det_ind=%d cloud_pts=%d depth_samples=%d | yolo=%.2f ms | mot=%.2f ms | depth=%.2f ms | total=%.2f ms",
                        target_debug.poses_count,
                        target_debug.tracks_count,
                        target_debug.selected_track_id,
                        target_debug.detection_index,
                        target_debug.projected_cloud_points,
                        target_debug.depth_sample_count,
                        target_debug.yolo_ms,
                        target_debug.mot_ms,
                        target_debug.depth_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 current_target_id=%d selected_id=%d det_ind=%d reused=%s bbox=%s depth=%.3f conf=%.3f depth_conf=%.3f pos_cam=%s pos_world=%s cloud_pts=%d depth_samples=%d | yolo=%.2f ms | mot=%.2f ms | depth=%.2f ms | 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,
                    target_debug.current_target_id_before,
                    target_observation.track_id,
                    target_observation.detection_index,
                    target_debug.found_existing_target ? "yes" : "no",
                    format_bbox_xyxy(target_observation.bbox_xyxy).c_str(),
                    target_observation.depth,
                    target_observation.confidence,
                    target_observation.depth_confidence,
                    format_vector3f(target_observation.target_pos_cam).c_str(),
                    format_vector3f(target_observation.target_pos_world).c_str(),
                    target_debug.projected_cloud_points,
                    target_debug.depth_sample_count,
                    target_debug.yolo_ms,
                    target_debug.mot_ms,
                    target_debug.depth_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());
            }
        }

        if (target_observation.valid) {
            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;
            observation_msg.valid = target_observation.valid;
            observation_msg.track_id = target_observation.track_id;
            observation_msg.detection_index = target_observation.detection_index;
            observation_msg.confidence = target_observation.confidence;
            observation_msg.depth = target_observation.depth;
            observation_msg.depth_confidence = target_observation.depth_confidence;
            observation_msg.bbox_xyxy = target_observation.bbox_xyxy;
            observation_msg.keypoints_xyc = target_observation.keypoints_xyc;
            observation_msg.target_pos_cam.x = target_observation.target_pos_cam.x();
            observation_msg.target_pos_cam.y = target_observation.target_pos_cam.y();
            observation_msg.target_pos_cam.z = target_observation.target_pos_cam.z();
            observation_msg.target_pos_world.x = target_observation.target_pos_world.x();
            observation_msg.target_pos_world.y = target_observation.target_pos_world.y();
            observation_msg.target_pos_world.z = target_observation.target_pos_world.z();
            target_observation_pub_->publish(observation_msg);

            geometry_msgs::msg::PointStamped pos_cam_msg;
            pos_cam_msg.header = image_msg->header;
            pos_cam_msg.header.stamp = sync_stamp;
            pos_cam_msg.header.frame_id = cloud_cam_msg.header.frame_id.empty()
                ? "camera"
                : cloud_cam_msg.header.frame_id;
            pos_cam_msg.point.x = target_observation.target_pos_cam.x();
            pos_cam_msg.point.y = target_observation.target_pos_cam.y();
            pos_cam_msg.point.z = target_observation.target_pos_cam.z();
            target_pos_cam_pub_->publish(pos_cam_msg);

            geometry_msgs::msg::PointStamped pos_world_msg;
            pos_world_msg.header = image_msg->header;
            pos_world_msg.header.stamp = sync_stamp;
            pos_world_msg.header.frame_id = odom_msg->header.frame_id.empty()
                ? "odom"
                : odom_msg->header.frame_id;
            pos_world_msg.point.x = target_observation.target_pos_world.x();
            pos_world_msg.point.y = target_observation.target_pos_world.y();
            pos_world_msg.point.z = target_observation.target_pos_world.z();
            target_pos_world_pub_->publish(pos_world_msg);
        }
    }
#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 (debug_target_observation_ && target_observation_processor_) {
            target_observation_processor_->draw_detected_poses(
                overlay_vis,
                target_debug.poses);
            for (const auto& pixel : target_debug.valid_projected_pixels) {
                if (pixel.x < 0 || pixel.x >= overlay_vis.cols ||
                    pixel.y < 0 || pixel.y >= overlay_vis.rows) {
                    continue;
                }
                cv::circle(overlay_vis, pixel, 3, cv::Scalar(0, 255, 0), -1);
            }
        }
        if (target_observation.valid && debug_target_observation_) {
            odin_ros_driver::draw_target_observation_overlay(
                overlay_vis,
                target_observation);
        }
#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)) {
                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);
        }
    }

    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);
    }
}

// ==================== 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