Calibration node binary (#30)

* feat: non-ROS calibration is in process

* feat: added aruco board calibration

* feat: done stereo calibration

* feat: calibration done

* feat: added mono calibration if required

* feat: saving detected points for plane calculations

* fix: formatted and cleaned calibration module
feat: added checks for missing params file

---------

Co-authored-by: Denis Bakin <denis.bakin@gmail.com>

packages/camera/include/calibration.h

@@ -1,16 +1,6 @@
 #pragma once
 
-#include "camera_srvs/srv/calibration_command.hpp"
-
-#include <cv_bridge/cv_bridge.hpp>
-#include <foxglove_msgs/msg/image_marker_array.hpp>
-#include <geometry_msgs/msg/point.hpp>
-#include <rclcpp/rclcpp.hpp>
-#include <sensor_msgs/msg/compressed_image.hpp>
-#include <std_msgs/msg/int16.hpp>
-#include <visualization_msgs/msg/image_marker.hpp>
-
-#include <opencv2/aruco/charuco.hpp>
+#include <opencv2/aruco.hpp>
 #include <opencv2/core/core.hpp>
 
 #include <boost/geometry.hpp>
@@ -21,6 +11,9 @@
 #include <optional>
 #include <string>
 #include <vector>
+#include <yaml-cpp/yaml.h>
+
+#include <params.h>
 
 namespace handy::calibration {
 
@@ -32,93 +25,52 @@ double getIou(const Polygon& first, const Polygon& second);
 
 const std::vector<Point> toBoostCorners(
     const std::vector<cv::Point2f>& corners, cv::Size pattern_size);
-sensor_msgs::msg::CompressedImage toJpegMsg(const cv_bridge::CvImage& cv_image);
 
-class CalibrationNode : public rclcpp::Node {
+class CalibrationNode {
   public:
-    CalibrationNode();
-
-    enum CalibrationState {
-        kNotCalibrated = 1,
-        kCapturing = 2,
-        kCalibrating = 3,
-        kOkCalibration = 5
-    };
-
-    enum Action { kStart = 1, kCalibrate = 2, kReset = 3 };
+    CalibrationNode(const YAML::Node& param_node);
+    bool handleFrame(const cv::Mat& image, int camera_idx);
+    void calibrate(int camera_idx);
+    void stereoCalibrate();
+    void clearDetections();
+    void clearLastDetection(int camera_idx);
 
   private:
-    void declareLaunchParams();
-    void initSignals();
-
-    void handleFrame(const sensor_msgs::msg::CompressedImage::ConstSharedPtr& msg);
-    void publishCalibrationState() const;
-
-    void onButtonClick(
-        const camera_srvs::srv::CalibrationCommand::Request::SharedPtr& request,
-        const camera_srvs::srv::CalibrationCommand::Response::SharedPtr& response);
+    void declareLaunchParams(const YAML::Node& param_node);
 
-    void calibrate();
-    void handleBadCalibration();
-    void handleResetCommand();
-
-    bool checkMaxSimilarity(std::vector<cv::Point2f>& corners) const;
-    int getImageCoverage() const;
+    bool checkMaxSimilarity(std::vector<std::vector<cv::Point2f>>& corners, int camera_idx) const;
+    int getImageCoverage(int camera_idx) const;
+    void fillImageObjectPoints(
+        std::vector<std::vector<cv::Point2f>>& image_points,
+        std::vector<std::vector<cv::Point3f>>& obj_points, int camera_idx);
 
     void initCornerMarkers();
     void appendCornerMarkers(const std::vector<cv::Point2f>& detected_corners);
-    visualization_msgs::msg::ImageMarker getCornerMarker(cv::Point2f point);
-    visualization_msgs::msg::ImageMarker getBoardMarkerFromCorners(
-        std::vector<cv::Point2f>& detected_corners, std_msgs::msg::Header& header);
-
-    struct Signals {
-        rclcpp::Publisher<foxglove_msgs::msg::ImageMarkerArray>::SharedPtr detected_boards =
-            nullptr;
-        rclcpp::Publisher<foxglove_msgs::msg::ImageMarkerArray>::SharedPtr detected_corners =
-            nullptr;
-
-        rclcpp::Publisher<std_msgs::msg::Int16>::SharedPtr calibration_state = nullptr;
-    } signal_{};
-
-    struct Slots {
-        rclcpp::Subscription<sensor_msgs::msg::CompressedImage>::SharedPtr image_sub = nullptr;
-    } slot_{};
-
-    struct Services {
-        rclcpp::Service<camera_srvs::srv::CalibrationCommand>::SharedPtr button_service = nullptr;
-    } service_{};
 
     struct Params {
-        std::string path_to_save_params = "";
+        int camera_num = 2;
+        std::string path_to_params = "";
         std::vector<cv::Point3f> square_obj_points;
-        cv::aruco::CharucoBoard charuco_board;
-
-        bool publish_preview_markers = true;
-        bool auto_calibrate = true;
+        cv::aruco::GridBoard aruco_board;
 
-        std::vector<double> marker_color = {0.0, 1.0, 0.0, 0.12};
         double min_accepted_error = 0.75;
         double iou_threshold = 0.5;
         double required_board_coverage = 0.7;
+        std::vector<int> camera_ids;
     } param_;
 
     struct State {
         std::optional<cv::Size> frame_size = std::nullopt;
 
-        std::vector<std::vector<cv::Point2f>> image_points_all;
-        std::vector<std::vector<cv::Point3f>> obj_points_all;
-        std::vector<Polygon> polygons_all;
-        foxglove_msgs::msg::ImageMarkerArray board_markers_array;
-        foxglove_msgs::msg::ImageMarkerArray board_corners_array;
+        // for each camera, each view, each detected marker vector of 4 aruco corners
+        std::vector<std::vector<std::vector<std::vector<cv::Point2f>>>> marker_corners_all;
+        std::vector<std::vector<std::vector<int>>> marker_ids_all;
+        std::vector<std::vector<Polygon>> polygons_all;
 
-        int last_marker_id = 0;
-        int calibration_state = kNotCalibrated;
+        std::vector<bool> is_calibrated;
+        std::vector<CameraIntrinsicParameters> intrinsic_params;
     } state_;
 
-    struct Timer {
-        rclcpp::TimerBase::SharedPtr calibration_state = nullptr;
-    } timer_{};
-
-    std::unique_ptr<cv::aruco::CharucoDetector> charuco_detector_ = nullptr;
+    std::unique_ptr<cv::aruco::ArucoDetector> aruco_detector_ = nullptr;
 };
 }  // namespace handy::calibration

packages/camera/include/params.h

@@ -18,7 +18,7 @@ struct CameraIntrinsicParameters {
     static CameraIntrinsicParameters loadFromYaml(const std::string& yaml_path, int camera_id = 0);
     static bool saveStereoCalibration(
         const std::string& yaml_path, cv::Mat& rotation_vector, cv::Mat& translation_vector,
-        int camera_id);
+        std::vector<std::vector<cv::Point2f>>& common_detections, int camera_id);
     static void loadStereoCalibration(
         const std::string& yaml_path, cv::Mat& rotation_vector, cv::Mat& translation_vector,
         int camera_id);

packages/camera/launch/calibration_launch.yaml

@@ -0,0 +1,13 @@
+parameters:
+    camera_num: 2
+    camera_ids: [1, 2]
+    calibration_file_path: "../../share/camera/launch/params.yaml" # only .yaml file
+    mono_source_dirs:
+        1: "/handy/datasets/TableOrange2msRecord_22_04/calibration_boards/aruco_plain_1_1/"
+        2: "/handy/datasets/TableOrange2msRecord_22_04/calibration_boards/aruco_plain_1_2/"
+    stereo_source_dirs: ["/handy/datasets/TableOrange2msRecord_22_04/calibration_boards/aruco_plain_1_1/",
+                        "/handy/datasets/TableOrange2msRecord_22_04/calibration_boards/aruco_plain_1_2/"]
+    iou_threshold: 0.5
+    min_accepted_calib_error: 0.7
+    required_board_coverage: 0.7
+    marker_color: [0.0, 1.0, 0.0, 0.12] #  [R, G, B, A], each in range 0.0-1.0