feat: saving and loading stereo calibration params added

feat: added tests fore new static mehods
fix: params tests - corrected length of distort coefs

packages/camera/include/params.h

@@ -16,6 +16,10 @@ struct CameraIntrinsicParameters {
 
     void storeYaml(const std::string& yaml_path) const;
     static CameraIntrinsicParameters loadFromYaml(const std::string& yaml_path, int camera_id = 0);
+    static bool saveStereoCalibration(
+        const std::string& yaml_path, cv::Mat& R, cv::Mat& T, cv::Size& image_size);
+    static void loadStereoCalibration(
+        const std::string& yaml_path, cv::Mat& R, cv::Mat& T, cv::Size& image_size);
 
     int camera_id = 0;
     cv::Size image_size{};

packages/camera/src/params.cpp

@@ -6,6 +6,7 @@
 
 #include <exception>
 #include <fstream>
+#include <iostream>
 
 namespace handy {
 
@@ -55,6 +56,45 @@ void CameraIntrinsicParameters::storeYaml(const std::string& yaml_path) const {
     output_file << config;
     output_file.close();
 }
+bool CameraIntrinsicParameters::saveStereoCalibration(
+    const std::string& yaml_path, cv::Mat& R, cv::Mat& T, cv::Size& image_size) {
+    YAML::Node config;
+    std::ifstream param_file(yaml_path);
+    if (param_file) {
+        config = YAML::Load(param_file);
+        param_file.close();
+    }
+
+    YAML::Node&& extrinsics = config["stereo_calibration"];
+
+    extrinsics["image_size"] = YAML::Node(YAML::NodeType::Sequence);
+    extrinsics["image_size"].SetStyle(YAML::EmitterStyle::Flow);
+    extrinsics["image_size"].push_back(image_size.width);
+    extrinsics["image_size"].push_back(image_size.height);
+
+    extrinsics["rotation"] = YAML::Node(YAML::NodeType::Sequence);
+    extrinsics["rotation"].SetStyle(YAML::EmitterStyle::Flow);
+    for (int i = 0; i < 3; ++i) {
+        for (int j = 0; j < 3; ++j) {
+            extrinsics["rotation"].push_back(R.at<double>(i, j));
+        }
+    }
+
+    extrinsics["translation"] = YAML::Node(YAML::NodeType::Sequence);
+    extrinsics["translation"].SetStyle(YAML::EmitterStyle::Flow);
+
+    for (int i = 0; i < 3; ++i) {
+        extrinsics["translation"].push_back(T.at<double>(i, 0));
+    }
+
+    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, int camera_id) {
@@ -91,6 +131,31 @@ CameraIntrinsicParameters CameraIntrinsicParameters::loadFromYaml(
     return result;
 }
 
+void CameraIntrinsicParameters::loadStereoCalibration(
+    const std::string& yaml_path, cv::Mat& R, cv::Mat& T, cv::Size& image_size) {
+    const YAML::Node extrinsics = YAML::LoadFile(yaml_path)["stereo_calibration"];
+
+    const auto yaml_image_size = extrinsics["image_size"].as<std::vector<int>>();
+    image_size = cv::Size(yaml_image_size[0], yaml_image_size[1]);
+
+    const auto yaml_camera_matrix = extrinsics["rotation"].as<std::vector<double>>();
+    R = cv::Mat(yaml_camera_matrix, true);
+    R = R.reshape(0, {3, 3});
+
+    const auto coefs = extrinsics["translation"].as<std::vector<double>>();
+    for (size_t i = 0; i < coefs.size(); ++i) {
+        printf("%f ", coefs[i]);
+    }
+    printf("\n");
+
+    T = cv::Mat(coefs, true);
+    T = T.reshape(0, {3, 1});
+    for (size_t i = 0; i < coefs.size(); ++i) {
+        printf("%f ", T.at<double>(i, 0));
+    }
+    printf("\n");
+}
+
 cv::Mat CameraIntrinsicParameters::undistortImage(cv::Mat& src) {
     cv::remap(
         src,

packages/camera/test/CMakeLists.txt

@@ -1,6 +1,6 @@
 find_package(ament_cmake_gtest REQUIRED)
 
-ament_add_gtest(camera_test main.cpp ../src/params.cpp)
+ament_add_gtest(camera_test test.cpp ../src/params.cpp)
 
 ament_target_dependencies(
     camera_test

packages/camera/test/main.cpp → packages/camera/test/test.cpp

@@ -11,23 +11,23 @@ TEST(camera, ParamsSingleReadWrite) {
     cv::Vec<float, 5> distort_coefs(1, 2, 3, 4, 5);
     cv::Size image_size(1920, 1080);
 
-    handy::CameraIntrinsicParameters params_1(image_size, camera_matrix, distort_coefs, 0);
+    handy::CameraIntrinsicParameters params_1(image_size, camera_matrix, distort_coefs, 1);
     params_1.storeYaml(path_to_yaml);
 
     std::ifstream test_file(path_to_yaml);
     EXPECT_TRUE(test_file);  // opened successfully
     test_file.close();
 
     handy::CameraIntrinsicParameters params_1_check =
-        handy::CameraIntrinsicParameters::loadFromYaml(path_to_yaml, 0);
+        handy::CameraIntrinsicParameters::loadFromYaml(path_to_yaml, 1);
     EXPECT_EQ(image_size, params_1_check.image_size);
     for (int i = 0; i < 3; ++i) {
         for (int j = 0; j < 3; j++) {
             EXPECT_EQ(
                 camera_matrix.at<double>(i, j), params_1_check.camera_matrix.at<double>(i, j));
         }
     }
-    for (int i = 0; i < 3; ++i) {
+    for (int i = 0; i < 5; ++i) {
         EXPECT_EQ(distort_coefs[i], params_1_check.dist_coefs[i]);
     }
 }
@@ -41,10 +41,10 @@ TEST(camera, ParamsMultipleReadWrite) {
     cv::Size image_size(1920, 1080);
 
     // store two IDs into the same file
-    handy::CameraIntrinsicParameters params_1(image_size, camera_matrix_1, distort_coefs, 0);
+    handy::CameraIntrinsicParameters params_1(image_size, camera_matrix_1, distort_coefs, 1);
     params_1.storeYaml(path_to_yaml);
 
-    handy::CameraIntrinsicParameters params_2(image_size, camera_matrix_2, distort_coefs, 1);
+    handy::CameraIntrinsicParameters params_2(image_size, camera_matrix_2, distort_coefs, 2);
     params_2.storeYaml(path_to_yaml);
 
     // check for file to exist
@@ -54,21 +54,21 @@ TEST(camera, ParamsMultipleReadWrite) {
 
     // loading and checking all params for the first ID
     handy::CameraIntrinsicParameters params_1_check =
-        handy::CameraIntrinsicParameters::loadFromYaml(path_to_yaml, 0);
+        handy::CameraIntrinsicParameters::loadFromYaml(path_to_yaml, 1);
     EXPECT_EQ(image_size, params_1_check.image_size);
     for (int i = 0; i < 3; ++i) {
         for (int j = 0; j < 3; j++) {
             EXPECT_EQ(
                 camera_matrix_1.at<double>(i, j), params_1_check.camera_matrix.at<double>(i, j));
         }
     }
-    for (int i = 0; i < 3; ++i) {
+    for (int i = 0; i < 5; ++i) {
         EXPECT_EQ(distort_coefs[i], params_1_check.dist_coefs[i]);
     }
 
     // loading and checking all params for the second ID
     handy::CameraIntrinsicParameters params_2_check =
-        handy::CameraIntrinsicParameters::loadFromYaml(path_to_yaml, 1);
+        handy::CameraIntrinsicParameters::loadFromYaml(path_to_yaml, 2);
     EXPECT_EQ(image_size, params_2_check.image_size);
 
     for (int i = 0; i < 3; ++i) {
@@ -77,11 +77,44 @@ TEST(camera, ParamsMultipleReadWrite) {
                 camera_matrix_2.at<double>(i, j), params_2_check.camera_matrix.at<double>(i, j));
         }
     }
-    for (int i = 0; i < 3; ++i) {
+    for (int i = 0; i < 5; ++i) {
         EXPECT_EQ(distort_coefs[i], params_2_check.dist_coefs[i]);
     }
 }
 
+TEST(camera, ParamsStereoCalibWrite) {
+    // create test params
+    const std::string path_to_yaml = "test_params.yaml";
+    cv::Mat R_check = (cv::Mat_<double>(3, 3) << 1., 0., 0., 0., 1., 0., 0., 0., 1.);
+    cv::Mat T_check = (cv::Mat_<double>(3, 1) << 1., 2., 3.);
+    cv::Size image_size_check(1920, 1080);
+
+    handy::CameraIntrinsicParameters::saveStereoCalibration(
+        path_to_yaml, R_check, T_check, image_size_check);
+
+    // check for file to exist
+    std::ifstream test_file(path_to_yaml);
+    EXPECT_TRUE(test_file);  // opened successfully
+    test_file.close();
+
+    // loading and checking all params for the first ID
+    cv::Mat R;
+    cv::Mat T;
+    cv::Size image_size;
+
+    handy::CameraIntrinsicParameters::loadStereoCalibration(path_to_yaml, R, T, image_size);
+
+    EXPECT_EQ(image_size, image_size_check);
+    for (int i = 0; i < 3; ++i) {
+        for (int j = 0; j < 3; j++) {
+            EXPECT_EQ(R_check.at<double>(i, j), R.at<double>(i, j));
+        }
+    }
+    for (int i = 0; i < 3; ++i) {
+        EXPECT_EQ(T_check.at<double>(i, 0), T.at<double>(i, 0));
+    }
+}
+
 int main(int argc, char** argv) {
     testing::InitGoogleTest(&argc, argv);
     return RUN_ALL_TESTS();