From e5f044de0dc8f1f9da214e62fb1a94cf57597ff0 Mon Sep 17 00:00:00 2001 From: cyy_mac Date: Thu, 26 Mar 2026 05:44:15 +0800 Subject: [PATCH] new camera calibraction --- src/camera_calibration/CMakeLists.txt | 81 +++ .../config/calibration_params.yaml | 22 + .../camera_calibration/calibration_node.hpp | 108 ++++ .../launch/calibration_launch.py | 43 ++ src/camera_calibration/package.xml | 29 ++ .../src/calibration_node.cpp | 493 ++++++++++++++++++ .../src/armor_pose_estimator.cpp | 26 - 7 files changed, 776 insertions(+), 26 deletions(-) create mode 100644 src/camera_calibration/CMakeLists.txt create mode 100644 src/camera_calibration/config/calibration_params.yaml create mode 100644 src/camera_calibration/include/camera_calibration/calibration_node.hpp create mode 100644 src/camera_calibration/launch/calibration_launch.py create mode 100644 src/camera_calibration/package.xml create mode 100644 src/camera_calibration/src/calibration_node.cpp diff --git a/src/camera_calibration/CMakeLists.txt b/src/camera_calibration/CMakeLists.txt new file mode 100644 index 0000000..c0ffdbe --- /dev/null +++ b/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() diff --git a/src/camera_calibration/config/calibration_params.yaml b/src/camera_calibration/config/calibration_params.yaml new file mode 100644 index 0000000..a7401ca --- /dev/null +++ b/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 diff --git a/src/camera_calibration/include/camera_calibration/calibration_node.hpp b/src/camera_calibration/include/camera_calibration/calibration_node.hpp new file mode 100644 index 0000000..5186e1a --- /dev/null +++ b/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 +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "rm_interfaces/srv/set_mode.hpp" + +namespace camera_calibration { + +// Struct to hold all calibration points +struct CalibrationData { + std::vector> all_points; +}; + +struct CalibrationObjectPoints { + std::vector> 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> coverage_grid_; + cv::Mat camera_matrix_; + cv::Mat dist_coeffs_; + bool calibration_done_; + + // ROS interfaces + rclcpp::Subscription::SharedPtr image_sub_; + rclcpp::Publisher::SharedPtr debug_image_pub_; + rclcpp::Publisher::SharedPtr progress_image_pub_; + rclcpp::Service::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 request, + const std::shared_ptr response); + + // Calibration methods + void performUniformSampling(); + bool calibrateCamera(); + void saveCalibrationResult(); + cv::Mat drawCalibrationResults(const cv::Mat& image, + const std::vector& corners, + bool found); + cv::Mat drawProgressBar(const cv::Mat& background); +}; + +} // namespace camera_calibration + +#endif // CAMERA_CALIBRATION_CALIBRATION_NODE_HPP_ diff --git a/src/camera_calibration/launch/calibration_launch.py b/src/camera_calibration/launch/calibration_launch.py new file mode 100644 index 0000000..c9130c1 --- /dev/null +++ b/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, + ]) diff --git a/src/camera_calibration/package.xml b/src/camera_calibration/package.xml new file mode 100644 index 0000000..d2cf245 --- /dev/null +++ b/src/camera_calibration/package.xml @@ -0,0 +1,29 @@ + + + + camera_calibration + 1.0.0 + Camera calibration node for detecting chessboard and saving intrinsics + Chen Youyuan + Apache-2.0 + + ament_cmake + ament_cmake_python + + rclcpp + rclcpp_components + std_msgs + sensor_msgs + image_transport + cv_bridge + opencv4 + Eigen3 + rm_interfaces + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/camera_calibration/src/calibration_node.cpp b/src/camera_calibration/src/calibration_node.cpp new file mode 100644 index 0000000..86d891f --- /dev/null +++ b/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 +#include +#include + +#include + +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(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( + 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( + 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 request, + const std::shared_ptr response) { + std::lock_guard 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(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(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>> cell_indices(grid_rows_, std::vector>(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(corners.size()); + + int col = std::min(grid_cols_ - 1, static_cast(center.x / (640.0 / grid_cols_))); + int row = std::min(grid_rows_ - 1, static_cast(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> sampled_image_points; + std::vector> 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(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 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 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(corners.size()); + + int cell_col = std::min(grid_cols_ - 1, static_cast(center.x / (image.cols / static_cast(grid_cols_)))); + int cell_row = std::min(grid_rows_ - 1, static_cast(center.y / (image.rows / static_cast(grid_rows_)))); + + // Draw and show corners + cv::Mat debug_img = drawCalibrationResults(image, corners, found); + + if (calibration_active_) { + // Create object points + std::vector 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(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(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 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(0, 0), camera_matrix_.at(1, 1), + camera_matrix_.at(0, 2), camera_matrix_.at(1, 2)); + RCLCPP_INFO(this->get_logger(), "Distortion coeffs: [k1=%.6f, k2=%.6f, p1=%.6f, p2=%.6f, k3=%.6f]", + dist_coeffs_.at(0), dist_coeffs_.at(1), + dist_coeffs_.at(2), dist_coeffs_.at(3), dist_coeffs_.at(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(0, 0) << ", " << camera_matrix_.at(0, 1) << ", " << camera_matrix_.at(0, 2) << ", " + << camera_matrix_.at(1, 0) << ", " << camera_matrix_.at(1, 1) << ", " << camera_matrix_.at(1, 2) << ", " + << camera_matrix_.at(2, 0) << ", " << camera_matrix_.at(2, 1) << ", " << camera_matrix_.at(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(0) << ", " << dist_coeffs_.at(1) << ", " + << dist_coeffs_.at(2) << ", " << dist_coeffs_.at(3) << ", " << dist_coeffs_.at(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(0, 0) << ", " << camera_matrix_.at(0, 1) << ", " << camera_matrix_.at(0, 2) << ", 0, " + << camera_matrix_.at(1, 0) << ", " << camera_matrix_.at(1, 1) << ", " << camera_matrix_.at(1, 2) << ", 0, " + << camera_matrix_.at(2, 0) << ", " << camera_matrix_.at(2, 1) << ", " << camera_matrix_.at(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& 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(board_width_ * board_height_)) { + // Draw horizontal lines + for (int i = 0; i < board_height_; i++) { + std::vector 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 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(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) diff --git a/src/rm_auto_aim/armor_detector/src/armor_pose_estimator.cpp b/src/rm_auto_aim/armor_detector/src/armor_pose_estimator.cpp index b66403d..b55109b 100644 --- a/src/rm_auto_aim/armor_detector/src/armor_pose_estimator.cpp +++ b/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(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 &rvecs, std::vector &tvecs) const {