Params struct refactor

packages/camera/CMakeLists.txt

@@ -69,4 +69,8 @@ endif()
 
 install(DIRECTORY launch params DESTINATION share/${PROJECT_NAME})
 
+if(BUILD_TESTING)
+  add_subdirectory("test")
+endif()
+
 ament_package()

packages/camera/include/camera.h

@@ -55,6 +55,7 @@ class CameraNode : public rclcpp::Node {
   private:
     void applyParamsToCamera(int handle);
     int getCameraId(int camera_handle);
+    void initCalibParams(int camera_handle);
 
     void triggerOnTimer();
     void handleFrame(CameraHandle handle, BYTE* raw_buffer, tSdkFrameHead* frame_info);

packages/camera/include/params.h

@@ -9,17 +9,21 @@ struct CameraIntrinsicParameters {
     CameraIntrinsicParameters() = default;
 
     CameraIntrinsicParameters(
-        cv::Size size, cv::Mat camera_matrix, const cv::Vec<double, 5>& distort_coefs);
+        cv::Size size, cv::Mat camera_matrix, const cv::Vec<double, 5>& distort_coefs,
+        int cam_id = 0);
 
-    void initUndistortMaps();
     cv::Mat undistortImage(cv::Mat& src);
 
     void storeYaml(const std::string& yaml_path) const;
-    static CameraIntrinsicParameters loadFromYaml(const std::string& yaml_path);
-    static CameraIntrinsicParameters loadFromParams(
-        cv::Size param_image_size, const std::vector<double>& param_camera_matrix,
-        const std::vector<double>& param_dist_coefs);
+    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);
+    static void loadStereoCalibration(
+        const std::string& yaml_path, cv::Mat& rotation_vector, cv::Mat& translation_vector,
+        int camera_id);
 
+    int camera_id = 0;
     cv::Size image_size{};
     cv::Mat camera_matrix{};
     cv::Vec<double, 5> dist_coefs{};

packages/camera/launch/camera.yaml

@@ -53,15 +53,6 @@ launch:
             - { name: "saturation", value: 100 }           # range: 0-200
             - { name: "sharpness", value: 0 }              # range: 0-100
             - { name: "auto_exposure", value: False }
-        - name: "intrinsic_params"
-          param:
-            - name: "image_size"
-              param:
-                - { name: "width", value: 1920 }
-                - { name: "height", value: 1200 }
-            - { name: "camera_matrix", value: [2909.92107945441, 0., 582.1724561319506, 0.,
-                                              3134.621273608562, 532.0677320807733, 0., 0., 1.] }
-            - { name: "distorsion_coefs", value: [-0.7151398, 10.59934, 1.975451e-05, 0.07159046, 1.047479] }
     - name: "2"
       param:
         - name: "exposure_params"
@@ -74,9 +65,3 @@ launch:
             - { name: "saturation", value: 100 }           # range: 0-200
             - { name: "sharpness", value: 0 }              # range: 0-100
             - { name: "auto_exposure", value: False }
-        - name: "intrinsic_params"
-          param:
-            - { name: "image_size", value: [1920, 1200] }
-            - { name: "camera_matrix", value: [2909.92107945441, 0., 582.1724561319506, 0.,
-                                              3134.621273608562, 532.0677320807733, 0., 0., 1.] }
-            - { name: "distorsion_coefs", value: [-0.7151398, 10.59934, 1.975451e-05, 0.07159046, 1.047479] }

packages/camera/package.xml

@@ -25,6 +25,7 @@
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

packages/camera/src/camera.cpp

@@ -140,6 +140,7 @@ CameraNode::CameraNode() : Node("camera_node") {
         }
 
         applyParamsToCamera(state_.camera_handles[i]);
+        initCalibParams(state_.camera_handles[i]);
         abortIfNot("start", CameraPlay(state_.camera_handles[i]));
         RCLCPP_INFO(
             this->get_logger(), "inited API and started camera ID = %d", camera_internal_id);
@@ -394,21 +395,6 @@ void CameraNode::applyParamsToCamera(int handle) {
         RCLCPP_ERROR(this->get_logger(), "camera %d failed to read instrinsic params", camera_idx);
         exit(EXIT_FAILURE);
     }
-
-    // loading intrinsic parameters
-    const int64_t param_image_width = this->declare_parameter<int64_t>(prefix + "image_size.width");
-    const int64_t param_image_height =
-        this->declare_parameter<int64_t>(prefix + "image_size.height");
-    const cv::Size param_image_size(param_image_width, param_image_height);
-
-    const std::vector<double> param_camera_matrix =
-        this->declare_parameter<std::vector<double>>(prefix + "camera_matrix");
-    const std::vector<double> param_dist_coefs =
-        this->declare_parameter<std::vector<double>>(prefix + "distorsion_coefs");
-
-    state_.cameras_intrinsics.push_back(CameraIntrinsicParameters::loadFromParams(
-        param_image_size, param_camera_matrix, param_dist_coefs));
-    RCLCPP_INFO(this->get_logger(), "camera=%i loaded intrinsics", camera_idx);
 }
 
 namespace {
@@ -531,4 +517,11 @@ void CameraNode::handleQueue(int camera_idx) {
     }
 }
 
+void CameraNode::initCalibParams(int camera_handle) {
+    const int camera_id = getCameraId(camera_handle);
+    RCLCPP_INFO(this->get_logger(), "loading camera ID=%d parameters", camera_id);
+    state_.cameras_intrinsics.push_back(
+        CameraIntrinsicParameters::loadFromYaml(param_.calibration_file_path, camera_id));
+}
+
 }  // namespace handy::camera

packages/camera/src/params.cpp

@@ -6,93 +6,144 @@
 
 #include <exception>
 #include <fstream>
+#include <iostream>
 
 namespace handy {
 
 CameraIntrinsicParameters::CameraIntrinsicParameters(
-    cv::Size size, cv::Mat camera_matr, const cv::Vec<double, 5>& distort_coefs)
-    : image_size(size), camera_matrix(std::move(camera_matr)), dist_coefs(distort_coefs) {}
+    cv::Size size, cv::Mat camera_matr, const cv::Vec<double, 5>& distort_coefs, const int cam_id)
+    : image_size(size)
+    , camera_matrix(std::move(camera_matr))
+    , dist_coefs(distort_coefs)
+    , camera_id(cam_id) {}
 
 void CameraIntrinsicParameters::storeYaml(const std::string& yaml_path) const {
-    std::ofstream param_file(yaml_path);
-    if (!param_file) {
-        throw std::invalid_argument("unable to open file");
+    const std::string camera_id_str = std::to_string(camera_id);
+    YAML::Node config;
+    std::ifstream param_file(yaml_path);
+    if (param_file) {
+        config = YAML::Load(param_file);
+        param_file.close();
     }
 
-    YAML::Emitter output_yaml;
-    output_yaml << YAML::BeginMap;  // global yaml map
+    YAML::Node&& params = config["parameters"];
+    YAML::Node&& camera_id_node = params[camera_id_str];
 
-    output_yaml << YAML::Key << "image_size";
-    output_yaml << YAML::Value << YAML::Flow << YAML::BeginSeq << image_size.width
-                << image_size.height << YAML::EndSeq;
+    camera_id_node["image_size"] = YAML::Node(YAML::NodeType::Sequence);
+    camera_id_node["image_size"].SetStyle(YAML::EmitterStyle::Flow);
+    camera_id_node["image_size"].push_back(image_size.width);
+    camera_id_node["image_size"].push_back(image_size.height);
 
-    output_yaml << YAML::Key << "camera_matrix";
-    output_yaml << YAML::Value << YAML::Flow << YAML::BeginSeq;
+    camera_id_node["camera_matrix"] = YAML::Node(YAML::NodeType::Sequence);
+    camera_id_node["camera_matrix"].SetStyle(YAML::EmitterStyle::Flow);
     for (int i = 0; i < 3; ++i) {
         for (int j = 0; j < 3; ++j) {
-            output_yaml << camera_matrix.at<double>(i, j);
+            camera_id_node["camera_matrix"].push_back(camera_matrix.at<double>(i, j));
         }
     }
-    output_yaml << YAML::EndSeq;
 
-    output_yaml << YAML::Key << "distorsion_coefs";
-    output_yaml << YAML::Value << YAML::Flow << YAML::BeginSeq;
+    camera_id_node["distortion_coefs"] = YAML::Node(YAML::NodeType::Sequence);
+    camera_id_node["distortion_coefs"].SetStyle(YAML::EmitterStyle::Flow);
+
     for (int i = 0; i < 5; ++i) {
-        output_yaml << dist_coefs[i];
+        camera_id_node["distortion_coefs"].push_back(dist_coefs[i]);
+    }
+
+    std::ofstream output_file(yaml_path);
+    if (!output_file) {
+        throw std::invalid_argument("unable to open file");
+    }
+    output_file << config;
+    output_file.close();
+}
+
+bool CameraIntrinsicParameters::saveStereoCalibration(
+    const std::string& yaml_path, cv::Mat& rotation_vector, cv::Mat& translation_vector,
+    int camera_id) {
+    if (rotation_vector.cols * rotation_vector.rows != 3 ||
+        translation_vector.cols * translation_vector.rows != 3) {
+        return false;
+    }
+    YAML::Node config;
+    std::ifstream param_file(yaml_path);
+    if (param_file) {
+        config = YAML::Load(param_file);
+        param_file.close();
+    }
+
+    YAML::Node&& params = config["parameters"];
+    YAML::Node&& camera_id_node = params[std::to_string(camera_id)];
+
+    camera_id_node["rotation"] = YAML::Node(YAML::NodeType::Sequence);
+    camera_id_node["rotation"].SetStyle(YAML::EmitterStyle::Flow);
+    for (int i = 0; i < 3; ++i) {
+        camera_id_node["rotation"].push_back(rotation_vector.at<double>(i));
     }
-    output_yaml << YAML::EndSeq;
 
-    output_yaml << YAML::EndMap;  // global yaml map
+    camera_id_node["translation"] = YAML::Node(YAML::NodeType::Sequence);
+    camera_id_node["translation"].SetStyle(YAML::EmitterStyle::Flow);
+    for (int i = 0; i < 3; ++i) {
+        camera_id_node["translation"].push_back(translation_vector.at<double>(i));
+    }
 
-    param_file << output_yaml.c_str();
-    param_file.close();
+    std::ofstream output_file(yaml_path);
+    if (!output_file) {
+        throw std::invalid_argument("unable to open file");
+    }
+    output_file << config;
+    output_file.close();
+    return true;
 }
 
-CameraIntrinsicParameters CameraIntrinsicParameters::loadFromYaml(const std::string& yaml_path) {
+CameraIntrinsicParameters CameraIntrinsicParameters::loadFromYaml(
+    const std::string& yaml_path, int camera_id) {
     CameraIntrinsicParameters result{};
+    result.camera_id = camera_id;
+    const std::string camera_id_str = std::to_string(camera_id);
 
-    const YAML::Node file = YAML::LoadFile(yaml_path);
+    const YAML::Node intrinsics = YAML::LoadFile(yaml_path)["parameters"];
 
-    const auto yaml_image_size = file["image_size"].as<std::vector<int>>();
+    const auto yaml_image_size = intrinsics[camera_id_str]["image_size"].as<std::vector<int>>();
     result.image_size = cv::Size(yaml_image_size[0], yaml_image_size[1]);
 
-    const auto yaml_camera_matrix = file["camera_matrix"].as<std::vector<double>>();
+    const auto yaml_camera_matrix =
+        intrinsics[camera_id_str]["camera_matrix"].as<std::vector<double>>();
     result.camera_matrix = cv::Mat(yaml_camera_matrix, true);
+    result.camera_matrix = result.camera_matrix.reshape(0, {3, 3});
 
-    const auto coefs = file["distorsion_coefs"].as<std::vector<double>>();
+    const auto coefs = intrinsics[camera_id_str]["distortion_coefs"].as<std::vector<float>>();
     result.dist_coefs = cv::Mat(coefs, true);
 
-    result.initUndistortMaps();
+    // note that new camera matrix equals initial camera matrix
+    // because neither scaling nor cropping is used when undistoring
+    cv::initUndistortRectifyMap(
+        result.camera_matrix,
+        result.dist_coefs,
+        cv::noArray(),
+        result.camera_matrix,  // newCameraMatrix == this->camera_matrix
+        result.image_size,
+        CV_16SC2,
+        result.cached.undistort_maps.first,
+        result.cached.undistort_maps.second);
+    result.cached.undistortedImage = cv::Mat(result.image_size, CV_8UC3);
 
     return result;
 }
 
-CameraIntrinsicParameters CameraIntrinsicParameters::loadFromParams(
-    cv::Size param_image_size, const std::vector<double>& param_camera_matrix,
-    const std::vector<double>& param_dist_coefs) {
-    CameraIntrinsicParameters result{};
+void CameraIntrinsicParameters::loadStereoCalibration(
+    const std::string& yaml_path, cv::Mat& rotation_vectors, cv::Mat& translation_vectors,
+    int camera_id) {
+    const YAML::Node camera_params =
+        YAML::LoadFile(yaml_path)["parameters"][std::to_string(camera_id)];
 
-    result.image_size = param_image_size;
-    result.camera_matrix = cv::Mat(param_camera_matrix, true);
-    result.dist_coefs = cv::Mat(param_dist_coefs, true);
-    result.initUndistortMaps();
+    const auto yaml_camera_matrix = camera_params["rotation"].as<std::vector<double>>();
+    rotation_vectors = cv::Mat(yaml_camera_matrix, true);
+    rotation_vectors = rotation_vectors.reshape(0, {3, 1});
 
-    return result;
-}
+    const auto coefs = camera_params["translation"].as<std::vector<double>>();
 
-void CameraIntrinsicParameters::initUndistortMaps() {
-    // note that new camera matrix equals initial camera matrix
-    // because neither scaling nor cropping is used when undistoring
-    cv::initUndistortRectifyMap(
-        camera_matrix,
-        dist_coefs,
-        cv::noArray(),
-        camera_matrix,  // newCameraMatrix == this->camera_matrix
-        image_size,
-        CV_16SC2,
-        cached.undistort_maps.first,
-        cached.undistort_maps.second);
-    cached.undistortedImage = cv::Mat(image_size, CV_8UC3);
+    translation_vectors = cv::Mat(coefs, true);
+    translation_vectors = translation_vectors.reshape(0, {3, 1});
 }
 
 cv::Mat CameraIntrinsicParameters::undistortImage(cv::Mat& src) {

packages/camera/test/CMakeLists.txt

@@ -0,0 +1,9 @@
+find_package(ament_cmake_gtest REQUIRED)
+
+ament_add_gtest(camera_test test.cpp ../src/params.cpp)
+
+ament_target_dependencies(camera_test OpenCV yaml_cpp_vendor)
+
+target_include_directories(
+  camera_test PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/../include>"
+                     "/usr/include")