Implemented temporal calibration.

data/config_endo_guide.yaml

@@ -5,6 +5,7 @@ sophiar_start_var: tracker_all
 
 camera_ref_transform_var: camera_ref_in_tracker
 camera_transform_var: camera_in_tracker
+camera_calib_transform_var: camera_in_camera_ref
 
 monitor:
   - name: Camera

data/sophiar_config_endo_guide.json

@@ -28,6 +28,10 @@
       "name": "camera_ref_in_tracker",
       "type": "transform_obj"
     },
+    {
+      "name": "camera_in_camera_ref",
+      "type": "transform_obj"
+    },
     {
       "name": "camera_in_tracker",
       "type": "transform_obj"
@@ -152,15 +156,7 @@
           {
             "name": "camera",
             "parent": "camera_ref",
-            "transform": [
-              19.3286001048916,
-              37.0595264763240,
-              -55.5602916285344,
-              0.505328802080145,
-              -0.493396136841779,
-              0.500875581191629,
-              0.500326599436610
-            ]
+            "transform_var_name": "camera_in_camera_ref"
           }
         ]
       },

src/image_process/impl/camera_calibrator.cpp

@@ -169,9 +169,71 @@ namespace camera_calibrator_impl {
         return dis_sum / corner_num;
     }
 
+    glm::mat4 hand_eye_calib::calib_hand_eye() {
+        // build hand-eye calibration problem
+        auto ref_r_vec = std::vector<cv::Mat>(); // tracker in reference
+        auto ref_t_vec = std::vector<cv::Mat>();
+        for (auto &info: img_pool) {
+            auto ref_mat = calc_track_mat(info.sample_ts + ts_offset);
+            ref_mat = glm::inverse(ref_mat);
+            auto r_mat = glm::mat3(ref_mat);
+            auto t_vec = glm::vec3(to_translation(ref_mat)); // fourth column of matrix
+            ref_r_vec.push_back(to_cv_mat(r_mat));
+            ref_t_vec.push_back(to_cv_mat(t_vec));
+        }
+
+        cv::Mat ret_r, ret_t;
+        cv::calibrateRobotWorldHandEye(
+                cam_r_vec, cam_t_vec, // chess board in camera
+                ref_r_vec, ref_t_vec, // tracker in reference
+                aux_r, aux_t, // tracker in chess board
+                ret_r, ret_t); // reference in camera
+        return to_transform_mat(to_vec3(ret_t), to_mat3(ret_r));
+    }
+
+    cv::Scalar hand_eye_calib::evaluate_hand_eye(const glm::mat4 &ret_mat) {
+        auto aux_mat = to_transform_mat(to_vec3(aux_t), to_mat3(aux_r));
+        auto chess_mat = glm::inverse(aux_mat);  // chess board in traker
+        auto err_mat = cv::Mat(sample_num, corner_num, CV_32FC2);
+        auto obj_ps_err = std::vector<cv::Point3f>();
+        std::ranges::transform(obj_ps, std::back_inserter(obj_ps_err), [](cv::Vec3f &p) {
+            return cv::Point3f(p[0], p[1], p[2]);
+        });
+        for (auto j = 0; j < sample_num; ++j) {
+            auto &item = img_pool[j];
+            auto proj_mat = ret_mat // reference in camera
+                            * glm::inverse(calc_track_mat(item.sample_ts + ts_offset)) // tracker in reference
+                            * chess_mat; // chess board in tracker
+            auto cam_mat = to_transform_mat( // chess board in camera
+                    to_vec3(cam_t_vec[j]), to_mat3(cam_r_vec[j]));
+            auto loop_mat = glm::inverse(proj_mat) // camera in chess board
+                            * cam_mat; // chess board in camera
+            for (auto i = 0; i < corner_num; ++i) {
+                auto p_gold = to_vec3(obj_ps[i]);
+                auto p_pred = transform_p(loop_mat, p_gold);
+                auto err_3d = glm::distance(p_gold, p_pred);
+                err_mat.at<cv::Vec2f>(j, i)[0] = err_3d;
+            }
+
+            auto proj_r = to_cv_rodigues(glm::mat3(proj_mat));
+            auto proj_t = to_cv_mat(to_translation(proj_mat));
+            auto c_pred = std::vector<cv::Point2f>();
+            cv::projectPoints(obj_ps_err, proj_r, proj_t,
+                              intrinsic_cv_mat, dist_coeffs_mat,
+                              c_pred);
+            assert(c_pred.size() == corner_num);
+            for (auto i = 0; i < corner_num; ++i) {
+                auto c_gold = to_vec2(item.corner[i]);
+                auto err_2d = glm::distance(c_gold, to_vec2(c_pred[i]));
+                err_mat.at<cv::Vec2f>(j, i)[1] = err_2d;
+            }
+        }
+        return cv::mean(err_mat);
+    }
+
     void hand_eye_calib::calc() {
-        auto sample_num = img_pool.size();
-        auto corner_num = img_pool.begin()->corner.size();
+        sample_num = img_pool.size();
+        corner_num = img_pool.begin()->corner.size();
 
         // select some images for calibrateCamera
         static constexpr auto pix_dis_threshold = 128.0f;
@@ -192,13 +254,12 @@ namespace camera_calibrator_impl {
                 std::vector<object_points_type>(select_num, obj_ps);
 
         cv::Mat r_vec_ignore, t_vec_ignore;
-        auto intrinsic_cv_mat = cv::Mat();
-        auto dist_coeffs_mat = cv::Mat();
         auto err = cv::calibrateCamera(
                 obj_ps_in, img_ps_in, img_size,
                 intrinsic_cv_mat, dist_coeffs_mat,
                 r_vec_ignore, t_vec_ignore,
                 cv::CALIB_RATIONAL_MODEL); // use k4, k5, k6
+        assert(!isnan(err));
         SPDLOG_INFO("Overall camera calibration error is {:.2f}pix", err);
         intrinsic_mat = to_mat3(intrinsic_cv_mat);
 
@@ -212,8 +273,8 @@ namespace camera_calibrator_impl {
         }
 
         // solve PnP for each image
-        auto cam_r_vec = std::vector<cv::Mat>(); // chess board in camera
-        auto cam_t_vec = std::vector<cv::Mat>();
+        cam_r_vec = std::vector<cv::Mat>(); // chess board in camera
+        cam_t_vec = std::vector<cv::Mat>();
         auto obj_ps_pnp = std::vector<cv::Point3d>();
         std::ranges::transform(obj_ps, std::back_inserter(obj_ps_pnp), [](cv::Vec3f &p) {
             return cv::Point3d(p[0], p[1], p[2]);
@@ -253,69 +314,37 @@ namespace camera_calibrator_impl {
             cam_t_vec.push_back(cam_t);
         }
 
-        static constexpr auto ts_offset = -80000; // us
-
-        // build hand-eye calibration problem
-        auto ref_r_vec = std::vector<cv::Mat>(); // tracker in reference
-        auto ref_t_vec = std::vector<cv::Mat>();
-        for (auto &info: img_pool) {
-            auto ref_mat = calc_track_mat(info.sample_ts + ts_offset);
-            ref_mat = glm::inverse(ref_mat);
-            auto r_mat = glm::mat3(ref_mat);
-            auto t_vec = glm::vec3(to_translation(ref_mat)); // fourth column of matrix
-            ref_r_vec.push_back(to_cv_mat(r_mat));
-            ref_t_vec.push_back(to_cv_mat(t_vec));
-        }
-
-        cv::Mat ret_r, ret_t;
-        cv::Mat aux_r, aux_t;
-        cv::calibrateRobotWorldHandEye(
-                cam_r_vec, cam_t_vec, // chess board in camera
-                ref_r_vec, ref_t_vec, // tracker in reference
-                aux_r, aux_t, // tracker in chess board
-                ret_r, ret_t); // reference in camera
-        auto ret_mat = to_transform_mat(to_vec3(ret_t), to_mat3(ret_r));
-        result_mat = glm::inverse(ret_mat); // camera in reference
-
-        // projection error
-        auto aux_mat = to_transform_mat(to_vec3(aux_t), to_mat3(aux_r));
-        auto chess_mat = glm::inverse(aux_mat);  // chess board in traker
-        auto err_mat = cv::Mat(sample_num, corner_num, CV_32FC2);
-        auto obj_ps_err = std::vector<cv::Point3f>();
-        std::ranges::transform(obj_ps, std::back_inserter(obj_ps_err), [](cv::Vec3f &p) {
-            return cv::Point3f(p[0], p[1], p[2]);
-        });
-        for (auto j = 0; j < sample_num; ++j) {
-            auto &item = img_pool[j];
-            auto proj_mat = ret_mat // reference in camera
-                            * glm::inverse(calc_track_mat(item.sample_ts + ts_offset)) // tracker in reference
-                            * chess_mat; // chess board in tracker
-            auto cam_mat = to_transform_mat( // chess board in camera
-                    to_vec3(cam_t_vec[j]), to_mat3(cam_r_vec[j]));
-            auto loop_mat = glm::inverse(proj_mat) // camera in chess board
-                            * cam_mat; // chess board in camera
-            for (auto i = 0; i < corner_num; ++i) {
-                auto p_gold = to_vec3(obj_ps[i]);
-                auto p_pred = transform_p(loop_mat, p_gold);
-                auto err_3d = glm::distance(p_gold, p_pred);
-                err_mat.at<cv::Vec2f>(j, i)[0] = err_3d;
-            }
-
-            auto proj_r = to_cv_rodigues(glm::mat3(proj_mat));
-            auto proj_t = to_cv_mat(to_translation(proj_mat));
-            auto c_pred = std::vector<cv::Point2f>();
-            cv::projectPoints(obj_ps_err, proj_r, proj_t,
-                              intrinsic_cv_mat, dist_coeffs_mat,
-                              c_pred);
-            assert(c_pred.size() == corner_num);
-            for (auto i = 0; i < corner_num; ++i) {
-                auto c_gold = to_vec2(item.corner[i]);
-                auto err_2d = glm::distance(c_gold, to_vec2(c_pred[i]));
-                err_mat.at<cv::Vec2f>(j, i)[1] = err_2d;
+        // find minimal error using trichotomy algorithm
+        auto offset_l = -128000; // -128ms
+        auto offset_r = 0; // 0ms
+        static constexpr auto eps = 5000; // 5ms
+        auto eval_func = [this](int offset) {
+            ts_offset = offset;
+            auto ret_mat = calib_hand_eye();
+            return evaluate_hand_eye(ret_mat);
+        };
+        auto better_op = [](const cv::Scalar &lhs, // is lhs better than rhs ?
+                            const cv::Scalar &rhs) {
+            return lhs[1] < rhs[1]; // use 2d error
+        };
+        for (; offset_r - offset_l > eps;) {
+            auto l_sec = (2 * offset_l + offset_r) / 3;
+            auto r_sec = (offset_l + 2 * offset_r) / 3;
+            auto l_err = eval_func(l_sec);
+            auto r_err = eval_func(r_sec);
+            if (better_op(l_err, r_err)) {
+                offset_r = r_sec;
+            } else {
+                offset_l = l_sec;
             }
         }
+        result_ts_offset = (offset_l + offset_r) / 2;
+
+        ts_offset = result_ts_offset;
+        result_mat = calib_hand_eye();
+        SPDLOG_INFO("Estimated temporal offset is {}ms", result_ts_offset / 1000);
 
-        auto err_val = cv::mean(err_mat);
+        auto err_val = evaluate_hand_eye(result_mat);
         obj_reproj_err = err_val[0];
         SPDLOG_INFO("Average 3D projection error is {:.2f}mm", obj_reproj_err);
         img_reproj_err = err_val[1];
@@ -341,6 +370,7 @@ void camera_calibrator::impl::start() {
     last_finish.store(nullptr);
     img_pool.clear();
     track_pool.clear();
+    img_ok_cnt = 0;
 
     assert(tp == nullptr);
     tp = std::make_unique<BS::thread_pool>(conf.num_threads);

src/image_process/impl/camera_calibrator_impl.h

@@ -62,6 +62,7 @@ namespace camera_calibrator_impl {
         using dist_coeffs_type = std::vector<float>;
         dist_coeffs_type dist_coeffs; // distortion coefficients
         glm::mat4 result_mat; // result matrix, camera in camera reference
+        int result_ts_offset; // temporal latency between tracker and camera
 
         // result error
         float obj_reproj_err; // average error of spacial corners after hand-eye calibration, in mm
@@ -88,6 +89,18 @@ namespace camera_calibrator_impl {
         static float corner_distance(const corner_type &c1,
                                      const corner_type &c2);
 
+        // do hand-eye calibration
+        glm::mat4 calib_hand_eye();
+
+        // evaluate hand-eye calibration result
+        cv::Scalar evaluate_hand_eye(const glm::mat4 &ret_mat);
+
+        size_t sample_num, corner_num;
+        cv::Mat intrinsic_cv_mat, dist_coeffs_mat;
+        int ts_offset = 0; // timestamp offset
+        std::vector<cv::Mat> cam_r_vec; // chess board in camera
+        std::vector<cv::Mat> cam_t_vec;
+        cv::Mat aux_r, aux_t; // tracker in chess board
     };
 
 }

src/impl/apps/debug/app_debug.cpp

@@ -61,7 +61,7 @@ app_debug::app_debug(const create_config &conf) {
     auto calib = std::make_unique<camera_calibrator>(calib_conf);
 
     auto sim_info = camera_calibrator::simulate_info_type{
-            .data_path = "/home/tpx/project/DepthGuide/cmake-build-debug/calib_data.txt",
+            .data_path = "/home/tpx/project/DepthGuide/cmake-build-debug/calib_data_2.txt",
             .img_size = cv::Size(1920, 1080),
     };
     calib->simulate_process(sim_info);

src/impl/apps/endo_guide/endo_guide.cpp

@@ -42,6 +42,7 @@ app_endo_guide::app_endo_guide(const create_config &_conf) {
             .stream = default_cuda_stream, .ctx = main_conf.asio_ctx,
             .sophiar_conn = sophiar_conn.get(),
             .ref_transform_var = LOAD_STR("camera_ref_transform_var"),
+            .result_transform_var = LOAD_STR("camera_calib_transform_var"),
     };
     cam_calib = std::make_unique<camera_calibrator>(calib_conf);