new camera calibraction

src/camera_calibration/CMakeLists.txt

@@ -0,0 +1,81 @@
+cmake_minimum_required(VERSION 3.8)
+project(camera_calibration)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# Find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(image_transport REQUIRED)
+find_package(cv_bridge REQUIRED)
+find_package(OpenCV REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(rm_interfaces REQUIRED)
+
+include_directories(
+  include
+  ${OpenCV_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIRS}
+)
+
+# Node library
+add_library(camera_calibration_lib SHARED
+  src/calibration_node.cpp
+)
+
+ament_target_dependencies(camera_calibration_lib
+  rclcpp
+  std_msgs
+  sensor_msgs
+  image_transport
+  cv_bridge
+  OpenCV
+  Eigen3
+  rm_interfaces
+)
+
+# Component library for component container
+rclcpp_components_register_node(camera_calibration_lib
+  PLUGIN "camera_calibration::CalibrationNode"
+  EXECUTABLE camera_calibration_node
+)
+
+install(TARGETS camera_calibration_lib
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION lib
+)
+
+install(DIRECTORY include/
+  DESTINATION include
+)
+
+install(DIRECTORY launch/
+  DESTINATION share/${PROJECT_NAME}/launch
+)
+
+install(DIRECTORY config/
+  DESTINATION share/${PROJECT_NAME}/config
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_export_dependencies(
+  rclcpp
+  std_msgs
+  sensor_msgs
+  image_transport
+  cv_bridge
+  OpenCV
+  Eigen3
+)
+
+ament_package()

src/camera_calibration/config/calibration_params.yaml

@@ -0,0 +1,22 @@
+/**:
+  ros__parameters:
+    # Chessboard parameters
+    board_width: 11
+    board_height: 8
+    square_size: 0.025
+
+    # Camera info
+    camera_name: "camera"
+    save_path: "/home/jetson/calibration.yaml"
+
+    # Topic names
+    image_topic: "image_raw"
+    debug_image_topic: "calibration/debug_image"
+    progress_image_topic: "calibration/progress_image"
+    control_service_name: "calibration/control"
+
+    # Coverage grid parameters (for thorough calibration)
+    grid_cols: 6
+    grid_rows: 5
+    min_samples_per_cell: 1
+    max_total_samples: 50

src/camera_calibration/include/camera_calibration/calibration_node.hpp

@@ -0,0 +1,108 @@
+// Copyright (C) FYT Vision Group. All rights reserved.
+//
+// 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.
+
+#ifndef CAMERA_CALIBRATION_CALIBRATION_NODE_HPP_
+#define CAMERA_CALIBRATION_CALIBRATION_NODE_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+#include <mutex>
+
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/bool.hpp>
+#include <std_msgs/msg/string.hpp>
+#include <sensor_msgs/msg/camera_info.hpp>
+#include <sensor_msgs/msg/image.hpp>
+#include <image_transport/image_transport.hpp>
+
+#include <opencv2/opencv.hpp>
+#include <cv_bridge/cv_bridge.h>
+
+#include "rm_interfaces/srv/set_mode.hpp"
+
+namespace camera_calibration {
+
+// Struct to hold all calibration points
+struct CalibrationData {
+  std::vector<std::vector<cv::Point2f>> all_points;
+};
+
+struct CalibrationObjectPoints {
+  std::vector<std::vector<cv::Point3f>> all_points;
+};
+
+class CalibrationNode : public rclcpp::Node {
+public:
+  explicit CalibrationNode(const rclcpp::NodeOptions& options);
+  ~CalibrationNode();
+
+private:
+  // Parameters
+  int board_width_;
+  int board_height_;
+  double square_size_;
+  std::string camera_name_;
+  std::string save_path_;
+
+  // Topic names (configurable via parameters)
+  std::string image_topic_;
+  std::string debug_image_topic_;
+  std::string progress_image_topic_;
+  std::string control_service_name_;
+
+  // Grid parameters for coverage tracking
+  int grid_cols_;
+  int grid_rows_;
+  int min_samples_per_cell_;
+  int max_total_samples_;
+
+  // State
+  bool calibration_active_;
+  CalibrationData image_points_;
+  CalibrationObjectPoints object_points_;
+  std::vector<std::vector<int>> coverage_grid_;
+  cv::Mat camera_matrix_;
+  cv::Mat dist_coeffs_;
+  bool calibration_done_;
+
+  // ROS interfaces
+  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_sub_;
+  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr debug_image_pub_;
+  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr progress_image_pub_;
+  rclcpp::Service<rm_interfaces::srv::SetMode>::SharedPtr control_srv_;
+
+  // Camera info
+  sensor_msgs::msg::CameraInfo::SharedPtr cam_info_;
+  std::mutex mutex_;
+
+  // Callbacks
+  void imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr& msg);
+  void controlCallback(
+      const std::shared_ptr<rm_interfaces::srv::SetMode::Request> request,
+      const std::shared_ptr<rm_interfaces::srv::SetMode::Response> response);
+
+  // Calibration methods
+  void performUniformSampling();
+  bool calibrateCamera();
+  void saveCalibrationResult();
+  cv::Mat drawCalibrationResults(const cv::Mat& image,
+                                  const std::vector<cv::Point2f>& corners,
+                                  bool found);
+  cv::Mat drawProgressBar(const cv::Mat& background);
+};
+
+}  // namespace camera_calibration
+
+#endif  // CAMERA_CALIBRATION_CALIBRATION_NODE_HPP_

src/camera_calibration/launch/calibration_launch.py

@@ -0,0 +1,43 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from ament_index_python.packages import get_package_share_directory
+import os
+
+
+def generate_launch_description():
+    # Get config file path
+    config_file = os.path.join(
+        get_package_share_directory('camera_calibration'),
+        'config',
+        'calibration_params.yaml'
+    )
+
+    # Declare launch arguments
+    declared_arguments = [
+        DeclareLaunchArgument('image_topic', default_value='image_raw'),
+        DeclareLaunchArgument('board_width', default_value='11'),
+        DeclareLaunchArgument('board_height', default_value='8'),
+        DeclareLaunchArgument('square_size', default_value='0.025'),
+        DeclareLaunchArgument('grid_cols', default_value='6'),
+        DeclareLaunchArgument('grid_rows', default_value='5'),
+        DeclareLaunchArgument('min_samples_per_cell', default_value='1'),
+    ]
+
+    # Create node
+    camera_calibration_node = Node(
+        package='camera_calibration',
+        executable='camera_calibration_node',
+        name='camera_calibration',
+        output='screen',
+        parameters=[config_file],
+        remappings=[
+            ('image_raw', LaunchConfiguration('image_topic')),
+        ],
+    )
+
+    return LaunchDescription([
+        *declared_arguments,
+        camera_calibration_node,
+    ])

src/camera_calibration/package.xml

@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>camera_calibration</name>
+  <version>1.0.0</version>
+  <description>Camera calibration node for detecting chessboard and saving intrinsics</description>
+  <maintainer email="cyy@example.com">Chen Youyuan</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>ament_cmake_python</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>std_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>image_transport</depend>
+  <depend>cv_bridge</depend>
+  <depend>opencv4</depend>
+  <depend>Eigen3</depend>
+  <depend>rm_interfaces</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/camera_calibration/src/calibration_node.cpp

@@ -0,0 +1,493 @@
+// Copyright (C) FYT Vision Group. All rights reserved.
+//
+// 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 "camera_calibration/calibration_node.hpp"
+
+#include <fstream>
+#include <iomanip>
+#include <iostream>
+
+#include <cv_bridge/cv_bridge.h>
+
+namespace camera_calibration {
+
+CalibrationNode::CalibrationNode(const rclcpp::NodeOptions& options)
+    : Node("camera_calibration", options) {
+  // Declare parameters
+  board_width_ = this->declare_parameter("board_width", 11);
+  board_height_ = this->declare_parameter("board_height", 8);
+  square_size_ = this->declare_parameter("square_size", 0.025);  // meters
+  camera_name_ = this->declare_parameter("camera_name", "camera");
+  save_path_ = this->declare_parameter("save_path", "/home/jetson/calibration.yaml");
+
+  // Configurable topic names
+  image_topic_ = this->declare_parameter("image_topic", "image_raw");
+  debug_image_topic_ = this->declare_parameter("debug_image_topic", "calibration/debug_image");
+  progress_image_topic_ = this->declare_parameter("progress_image_topic", "calibration/progress_image");
+  control_service_name_ = this->declare_parameter("control_service_name", "calibration/control");
+
+  // Grid parameters for coverage tracking
+  grid_cols_ = this->declare_parameter("grid_cols", 6);
+  grid_rows_ = this->declare_parameter("grid_rows", 5);
+  min_samples_per_cell_ = this->declare_parameter("min_samples_per_cell", 1);
+  max_total_samples_ = this->declare_parameter("max_total_samples", 50);
+
+  // Initialize
+  calibration_active_ = false;
+  calibration_done_ = false;
+  coverage_grid_.resize(grid_rows_, std::vector<int>(grid_cols_, 0));
+
+  RCLCPP_INFO(this->get_logger(), "Camera Calibration Node Started");
+  RCLCPP_INFO(this->get_logger(), "Board size: %dx%d, Square size: %.3fm",
+              board_width_, board_height_, square_size_);
+  RCLCPP_INFO(this->get_logger(), "Coverage grid: %dx%d (%d cells), min samples per cell: %d",
+              grid_cols_, grid_rows_, grid_cols_ * grid_rows_, min_samples_per_cell_);
+
+  // Create subscriptions using configurable topic name
+  image_sub_ = this->create_subscription<sensor_msgs::msg::Image>(
+      image_topic_, rclcpp::SensorDataQoS(),
+      std::bind(&CalibrationNode::imageCallback, this, std::placeholders::_1));
+
+  // Create publishers
+  debug_image_pub_ = image_transport::create_publisher(this, debug_image_topic_);
+  progress_image_pub_ = image_transport::create_publisher(this, progress_image_topic_);
+
+  // Create services using configurable service name
+  control_srv_ = this->create_service<rm_interfaces::srv::SetMode>(
+      control_service_name_,
+      std::bind(&CalibrationNode::controlCallback, this,
+                std::placeholders::_1, std::placeholders::_2));
+
+  RCLCPP_INFO(this->get_logger(), "Subscribing to image topic: %s", image_topic_.c_str());
+  RCLCPP_INFO(this->get_logger(), "Publishing debug image to: %s", debug_image_topic_.c_str());
+  RCLCPP_INFO(this->get_logger(), "Publishing progress image to: %s", progress_image_topic_.c_str());
+  RCLCPP_INFO(this->get_logger(), "Control service: %s", control_service_name_.c_str());
+  RCLCPP_INFO(this->get_logger(), "Use service '%s' with mode=1(start) or mode=0(stop&calibrate)",
+              control_service_name_.c_str());
+}
+
+CalibrationNode::~CalibrationNode() {
+  if (calibration_done_) {
+    saveCalibrationResult();
+  }
+}
+
+void CalibrationNode::controlCallback(
+    const std::shared_ptr<rm_interfaces::srv::SetMode::Request> request,
+    const std::shared_ptr<rm_interfaces::srv::SetMode::Response> response) {
+  std::lock_guard<std::mutex> lock(mutex_);
+
+  if (request->mode == 1) {
+    // Start calibration
+    calibration_active_ = true;
+    image_points_.clear();
+    object_points_.all_points.clear();
+    coverage_grid_.assign(grid_rows_, std::vector<int>(grid_cols_, 0));
+    calibration_done_ = false;
+    RCLCPP_INFO(this->get_logger(), "=== Started Calibration Mode ===");
+    RCLCPP_INFO(this->get_logger(), "Move chessboard to cover all areas of the image!");
+    RCLCPP_INFO(this->get_logger(), "Coverage grid: %dx%d (%d cells), need %d samples each",
+                grid_cols_, grid_rows_, grid_cols_ * grid_rows_, min_samples_per_cell_);
+    response->message = "Calibration started. Cover all areas with chessboard.";
+  } else if (request->mode == 0) {
+    // Stop and calibrate with uniform sampling
+    calibration_active_ = false;
+    RCLCPP_INFO(this->get_logger(), "=== Stopped Calibration Mode ===");
+
+    if (image_points_.all_points.size() < static_cast<size_t>(grid_cols_ * grid_rows_ * min_samples_per_cell_)) {
+      RCLCPP_WARN(this->get_logger(), "Not enough samples collected!");
+      response->message = "Not enough samples. Need to cover all grid cells.";
+      return;
+    }
+
+    // Uniform sampling
+    performUniformSampling();
+
+    if (calibrateCamera()) {
+      saveCalibrationResult();
+      response->message = "Calibration successful!";
+    } else {
+      response->message = "Calibration failed!";
+    }
+  } else {
+    response->message = "Unknown mode. Use 1=start, 0=stop";
+  }
+  response->success = true;
+}
+
+void CalibrationNode::performUniformSampling() {
+  RCLCPP_INFO(this->get_logger(), "Performing uniform sampling...");
+
+  // Calculate how many samples to keep from each cell
+  std::vector<std::vector<std::vector<size_t>>> cell_indices(grid_rows_, std::vector<std::vector<size_t>>(grid_cols_));
+
+  // Group samples by cell
+  for (size_t i = 0; i < image_points_.all_points.size(); i++) {
+    const auto& corners = image_points_.all_points[i];
+    cv::Point2f center;
+    for (const auto& c : corners) center += c;
+    center /= static_cast<float>(corners.size());
+
+    int col = std::min(grid_cols_ - 1, static_cast<int>(center.x / (640.0 / grid_cols_)));
+    int row = std::min(grid_rows_ - 1, static_cast<int>(center.y / (480.0 / grid_rows_)));
+    col = std::max(0, std::min(col, grid_cols_ - 1));
+    row = std::max(0, std::min(row, grid_rows_ - 1));
+    cell_indices[row][col].push_back(i);
+  }
+
+  // Select uniform samples
+  std::vector<std::vector<cv::Point2f>> sampled_image_points;
+  std::vector<std::vector<cv::Point3f>> sampled_object_points;
+
+  int samples_per_cell = min_samples_per_cell_;
+
+  for (int r = 0; r < grid_rows_; r++) {
+    for (int c = 0; c < grid_cols_; c++) {
+      auto& indices = cell_indices[r][c];
+      if (indices.empty()) {
+        RCLCPP_WARN(this->get_logger(), "Cell [%d,%d] has no samples!", r, c);
+        continue;
+      }
+
+      // Select evenly spaced samples from this cell
+      int count = std::min(static_cast<int>(indices.size()), samples_per_cell);
+      int step = indices.size() / count;
+      for (int i = 0; i < count; i++) {
+        size_t idx = indices[i * step];
+        sampled_image_points.push_back(image_points_.all_points[idx]);
+        sampled_object_points.push_back(object_points_.all_points[idx]);
+      }
+    }
+  }
+
+  // Use sampled points
+  image_points_.all_points = sampled_image_points;
+  object_points_.all_points = sampled_object_points;
+
+  RCLCPP_INFO(this->get_logger(), "Uniform sampling complete: %zu samples selected", image_points_.all_points.size());
+}
+
+void CalibrationNode::imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr& msg) {
+  std::lock_guard<std::mutex> lock(mutex_);
+
+  cv::Mat image = cv_bridge::toCvShare(msg, "bgr8")->image;
+
+  if (image.empty()) {
+    RCLCPP_ERROR(this->get_logger(), "Empty image received!");
+    return;
+  }
+
+  std::vector<cv::Point2f> corners;
+
+  // Find chessboard corners
+  bool found = cv::findChessboardCorners(
+      image,
+      cv::Size(board_width_, board_height_),
+      corners,
+      cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_NORMALIZE_IMAGE);
+
+  if (found) {
+    // Refine corner positions
+    cv::Mat gray;
+    cv::cvtColor(image, gray, cv::COLOR_BGR2GRAY);
+    cv::cornerSubPix(gray, corners, cv::Size(11, 11), cv::Size(-1, -1),
+                     cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::MAX_ITER, 30, 0.1));
+
+    // Calculate center of detected corners
+    cv::Point2f center;
+    for (const auto& c : corners) center += c;
+    center /= static_cast<float>(corners.size());
+
+    int cell_col = std::min(grid_cols_ - 1, static_cast<int>(center.x / (image.cols / static_cast<float>(grid_cols_))));
+    int cell_row = std::min(grid_rows_ - 1, static_cast<int>(center.y / (image.rows / static_cast<float>(grid_rows_))));
+
+    // Draw and show corners
+    cv::Mat debug_img = drawCalibrationResults(image, corners, found);
+
+    if (calibration_active_) {
+      // Create object points
+      std::vector<cv::Point3f> objp;
+      for (int i = 0; i < board_height_; i++) {
+        for (int j = 0; j < board_width_; j++) {
+          objp.emplace_back(j * square_size_, i * square_size_, 0);
+        }
+      }
+
+      // Check if this cell needs more samples
+      if (coverage_grid_[cell_row][cell_col] < min_samples_per_cell_) {
+        coverage_grid_[cell_row][cell_col]++;
+        image_points_.all_points.push_back(corners);
+        object_points_.all_points.push_back(objp);
+        RCLCPP_INFO(this->get_logger(), "Corner collected! Cell[%d,%d] count: %d, Total: %zu",
+                    cell_row, cell_col, coverage_grid_[cell_row][cell_col],
+                    image_points_.all_points.size());
+      }
+
+      // Publish progress visualization
+      cv::Mat progress_img = drawProgressBar(image);
+      sensor_msgs::msg::Image::SharedPtr progress_msg =
+          cv_bridge::CvImage(msg->header, "bgr8", progress_img).toImageMsg();
+      progress_image_pub_.publish(progress_msg);
+    }
+
+    // Publish debug image
+    sensor_msgs::msg::Image::SharedPtr output_msg =
+        cv_bridge::CvImage(msg->header, "bgr8", debug_img).toImageMsg();
+    debug_image_pub_.publish(output_msg);
+  } else {
+    // No chessboard found
+    if (calibration_active_) {
+      cv::Mat progress_img = drawProgressBar(image);
+      sensor_msgs::msg::Image::SharedPtr progress_msg =
+          cv_bridge::CvImage(msg->header, "bgr8", progress_img).toImageMsg();
+      progress_image_pub_.publish(progress_msg);
+    }
+
+    sensor_msgs::msg::Image::SharedPtr output_msg =
+        cv_bridge::CvImage(msg->header, "bgr8", image).toImageMsg();
+    debug_image_pub_.publish(output_msg);
+  }
+}
+
+cv::Mat CalibrationNode::drawProgressBar(const cv::Mat& background) {
+  cv::Mat output = background.clone();
+  int img_h = output.rows;
+  int img_w = output.cols;
+  int cell_w = img_w / grid_cols_;
+  int cell_h = img_h / grid_rows_;
+
+  // Draw grid cells
+  for (int r = 0; r < grid_rows_; r++) {
+    for (int c = 0; c < grid_cols_; c++) {
+      cv::Rect cell_rect(c * cell_w, r * cell_h, cell_w, cell_h);
+      int count = coverage_grid_[r][c];
+
+      // Color based on coverage level
+      cv::Scalar color;
+      if (count == 0) {
+        color = cv::Scalar(50, 50, 50);  // Gray - no samples
+      } else if (count < min_samples_per_cell_) {
+        // Yellow to green gradient based on count
+        int alpha = 128 + (count * 127 / min_samples_per_cell_);
+        color = cv::Scalar(0, alpha, 255 - alpha);  // BGR
+      } else {
+        color = cv::Scalar(0, 255, 0);  // Green - enough samples
+      }
+
+      cv::rectangle(output, cell_rect, color, -1);
+
+      // Draw border
+      cv::rectangle(output, cell_rect, cv::Scalar(255, 255, 255), 2);
+
+      // Draw count text
+      std::string count_text = std::to_string(count);
+      cv::putText(output, count_text,
+                  cv::Point(cell_rect.x + cell_w/2 - 10, cell_rect.y + cell_h/2 + 10),
+                  cv::FONT_HERSHEY_SIMPLEX, 0.8, cv::Scalar(255, 255, 255), 2);
+    }
+  }
+
+  // Draw separator lines between cells
+  for (int c = 1; c < grid_cols_; c++) {
+    cv::line(output, cv::Point(c * cell_w, 0), cv::Point(c * cell_w, img_h),
+             cv::Scalar(255, 0, 0), 3);
+  }
+  for (int r = 1; r < grid_rows_; r++) {
+    cv::line(output, cv::Point(0, r * cell_h), cv::Point(img_w, r * cell_h),
+             cv::Scalar(255, 0, 0), 3);
+  }
+
+  // Calculate overall progress
+  int total_covered = 0;
+  int total_needed = grid_rows_ * grid_cols_ * min_samples_per_cell_;
+  for (int r = 0; r < grid_rows_; r++) {
+    for (int c = 0; c < grid_cols_; c++) {
+      total_covered += std::min(coverage_grid_[r][c], min_samples_per_cell_);
+    }
+  }
+  float progress = static_cast<float>(total_covered) / total_needed;
+
+  // Draw overall progress bar at bottom
+  int bar_height = 40;
+  int bar_y = img_h - bar_height;
+  cv::rectangle(output, cv::Point(0, bar_y), cv::Point(img_w, img_h), cv::Scalar(30, 30, 30), -1);
+
+  // Progress fill
+  int progress_width = static_cast<int>(img_w * progress);
+  cv::rectangle(output, cv::Point(0, bar_y), cv::Point(progress_width, img_h),
+                cv::Scalar(0, 200, 100), -1);
+
+  // Progress text
+  std::string progress_text = cv::format("Progress: %.1f%% (%d/%d cells covered, total samples: %zu)",
+                                         progress * 100, total_covered / min_samples_per_cell_,
+                                         grid_cols_ * grid_rows_, image_points_.all_points.size());
+  cv::putText(output, progress_text, cv::Point(10, bar_y + 28),
+              cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(255, 255, 255), 1);
+
+  // Draw legend
+  cv::putText(output, "LEGEND: Gray=empty, Yellow=partial, Green=complete",
+              cv::Point(10, 30), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(200, 200, 200), 1);
+
+  return output;
+}
+
+bool CalibrationNode::calibrateCamera() {
+  if (image_points_.all_points.empty() || object_points_.all_points.empty()) {
+    RCLCPP_ERROR(this->get_logger(), "No calibration data!");
+    return false;
+  }
+
+  RCLCPP_INFO(this->get_logger(), "Starting camera calibration with %zu samples...",
+              image_points_.all_points.size());
+
+  cv::Size image_size;
+  if (cam_info_) {
+    image_size = cv::Size(cam_info_->width, cam_info_->height);
+  } else {
+    image_size = cv::Size(640, 480);
+  }
+
+  std::vector<cv::Mat> rvecs, tvecs;
+
+  double rms = cv::calibrateCamera(
+      object_points_.all_points,
+      image_points_.all_points,
+      image_size,
+      camera_matrix_,
+      dist_coeffs_,
+      rvecs,
+      tvecs,
+      cv::CALIB_FIX_K3);
+
+  RCLCPP_INFO(this->get_logger(), "Calibration done with RMS error: %.4f", rms);
+  RCLCPP_INFO(this->get_logger(), "Camera matrix:\n  fx=%.2f, fy=%.2f, cx=%.2f, cy=%.2f",
+              camera_matrix_.at<double>(0, 0), camera_matrix_.at<double>(1, 1),
+              camera_matrix_.at<double>(0, 2), camera_matrix_.at<double>(1, 2));
+  RCLCPP_INFO(this->get_logger(), "Distortion coeffs: [k1=%.6f, k2=%.6f, p1=%.6f, p2=%.6f, k3=%.6f]",
+              dist_coeffs_.at<double>(0), dist_coeffs_.at<double>(1),
+              dist_coeffs_.at<double>(2), dist_coeffs_.at<double>(3), dist_coeffs_.at<double>(4));
+
+  calibration_done_ = true;
+  return true;
+}
+
+void CalibrationNode::saveCalibrationResult() {
+  if (!calibration_done_) {
+    RCLCPP_WARN(this->get_logger(), "No calibration results to save!");
+    return;
+  }
+
+  RCLCPP_INFO(this->get_logger(), "Saving calibration to: %s", save_path_.c_str());
+
+  std::ofstream ofs(save_path_);
+  if (!ofs.is_open()) {
+    RCLCPP_ERROR(this->get_logger(), "Failed to open file for writing: %s", save_path_.c_str());
+    return;
+  }
+
+  // YAML format compatible with ROS camera_calibration_parsers
+  ofs << "%YAML:1.0" << std::endl;
+  ofs << "camera_name: " << camera_name_ << std::endl;
+  ofs << "image_width: " << (cam_info_ ? cam_info_->width : 640) << std::endl;
+  ofs << "image_height: " << (cam_info_ ? cam_info_->height : 480) << std::endl;
+  ofs << "camera_matrix:" << std::endl;
+  ofs << "  rows: 3" << std::endl;
+  ofs << "  cols: 3" << std::endl;
+  ofs << "  data: ["
+      << camera_matrix_.at<double>(0, 0) << ", " << camera_matrix_.at<double>(0, 1) << ", " << camera_matrix_.at<double>(0, 2) << ", "
+      << camera_matrix_.at<double>(1, 0) << ", " << camera_matrix_.at<double>(1, 1) << ", " << camera_matrix_.at<double>(1, 2) << ", "
+      << camera_matrix_.at<double>(2, 0) << ", " << camera_matrix_.at<double>(2, 1) << ", " << camera_matrix_.at<double>(2, 2) << "]"
+      << std::endl;
+  ofs << "distortion_model: plumb_bob" << std::endl;
+  ofs << "distortion_coefficients:" << std::endl;
+  ofs << "  rows: 1" << std::endl;
+  ofs << "  cols: 5" << std::endl;
+  ofs << "  data: ["
+      << dist_coeffs_.at<double>(0) << ", " << dist_coeffs_.at<double>(1) << ", "
+      << dist_coeffs_.at<double>(2) << ", " << dist_coeffs_.at<double>(3) << ", " << dist_coeffs_.at<double>(4) << "]"
+      << std::endl;
+  ofs << "rectification_matrix:" << std::endl;
+  ofs << "  rows: 3" << std::endl;
+  ofs << "  cols: 3" << std::endl;
+  ofs << "  data: [1, 0, 0, 0, 1, 0, 0, 0, 1]" << std::endl;
+  ofs << "projection_matrix:" << std::endl;
+  ofs << "  rows: 3" << std::endl;
+  ofs << "  cols: 4" << std::endl;
+  ofs << "  data: ["
+      << camera_matrix_.at<double>(0, 0) << ", " << camera_matrix_.at<double>(0, 1) << ", " << camera_matrix_.at<double>(0, 2) << ", 0, "
+      << camera_matrix_.at<double>(1, 0) << ", " << camera_matrix_.at<double>(1, 1) << ", " << camera_matrix_.at<double>(1, 2) << ", 0, "
+      << camera_matrix_.at<double>(2, 0) << ", " << camera_matrix_.at<double>(2, 1) << ", " << camera_matrix_.at<double>(2, 2) << ", 0]"
+      << std::endl;
+
+  ofs.close();
+  RCLCPP_INFO(this->get_logger(), "Calibration saved successfully!");
+}
+
+cv::Mat CalibrationNode::drawCalibrationResults(const cv::Mat& image,
+                                               const std::vector<cv::Point2f>& corners,
+                                               bool found) {
+  cv::Mat output = image.clone();
+
+  if (!found || corners.empty()) {
+    cv::putText(output, "No chessboard detected", cv::Point(20, 40),
+                cv::FONT_HERSHEY_SIMPLEX, 1.0, cv::Scalar(0, 0, 255), 2);
+    return output;
+  }
+
+  // Draw chessboard corners
+  cv::drawChessboardCorners(output, cv::Size(board_width_, board_height_), corners, true);
+
+  // Draw grid lines connecting corners
+  if (corners.size() == static_cast<size_t>(board_width_ * board_height_)) {
+    // Draw horizontal lines
+    for (int i = 0; i < board_height_; i++) {
+      std::vector<cv::Point> line_pts;
+      for (int j = 0; j < board_width_; j++) {
+        line_pts.push_back(corners[i * board_width_ + j]);
+      }
+      cv::polylines(output, line_pts, false, cv::Scalar(0, 255, 0), 2);
+    }
+
+    // Draw vertical lines
+    for (int j = 0; j < board_width_; j++) {
+      std::vector<cv::Point> line_pts;
+      for (int i = 0; i < board_height_; i++) {
+        line_pts.push_back(corners[i * board_width_ + j]);
+      }
+      cv::polylines(output, line_pts, false, cv::Scalar(0, 255, 0), 2);
+    }
+  }
+
+  // Draw corner markers and numbers
+  for (size_t i = 0; i < corners.size(); i++) {
+    cv::circle(output, corners[i], 5, cv::Scalar(0, 0, 255), -1);
+  }
+
+  // Calculate and display center
+  cv::Point2f center;
+  for (const auto& c : corners) center += c;
+  center /= static_cast<float>(corners.size());
+  cv::circle(output, center, 8, cv::Scalar(255, 255, 0), 2);
+  cv::putText(output, cv::format("(%.0f, %.0f)", center.x, center.y),
+              center + cv::Point2f(10, -10),
+              cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255, 255, 0), 1);
+
+  return output;
+}
+
+}  // namespace camera_calibration
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+RCLCPP_COMPONENTS_REGISTER_NODE(camera_calibration::CalibrationNode)

src/rm_auto_aim/armor_detector/src/armor_pose_estimator.cpp

@@ -19,9 +19,6 @@
 #include "rm_utils/math/utils.hpp"
 
 namespace fyt::auto_aim {
-
-// Forward declaration for forceRPY
-Eigen::Matrix3d forceRPY(const Eigen::Matrix3d& R, double forced_roll_deg, double forced_pitch_deg);
 ArmorPoseEstimator::ArmorPoseEstimator(sensor_msgs::msg::CameraInfo::SharedPtr camera_info) {
   // Setup pnp solver
   pnp_solver_ = std::make_unique<PnPSolver>(camera_info->k, camera_info->d);
@@ -102,29 +99,6 @@ Eigen::Vector3d ArmorPoseEstimator::rotationMatrixToRPY(const Eigen::Matrix3d &R
   return rpy;
 }
 
-// Force rotation to have roll=0 and pitch=forced_degrees, keeping original yaw
-// Uses quaternion to avoid gimbal lock issues with Euler angles
-Eigen::Matrix3d forceRPY(const Eigen::Matrix3d& R, double forced_roll_deg, double forced_pitch_deg) {
-  // Extract current RPY using tf2 (standard ZYX convention)
-  Eigen::Quaterniond q(R);
-  tf2::Quaternion tf_q(q.x(), q.y(), q.z(), q.w());
-  double r, p, y;
-  tf2::Matrix3x3(tf_q).getRPY(r, p, y);
-
-  // Keep original yaw, force roll=0 and pitch=forced_pitch_deg
-  double roll = forced_roll_deg * M_PI / 180.0;
-  double pitch = forced_pitch_deg * M_PI / 180.0;
-
-  // Reconstruct using quaternion to avoid gimbal lock
-  tf2::Quaternion q_new;
-  q_new.setRPY(roll, pitch, y);
-
-  Eigen::Matrix3d R_new;
-  R_new = Eigen::Matrix3d(q_new);
-
-  return R_new;
-}
-
 void ArmorPoseEstimator::sortPnPResult(const Armor &armor,
                                        std::vector<cv::Mat> &rvecs,
                                        std::vector<cv::Mat> &tvecs) const {