Implemented weighted Shah algorithm.

data/config_endo_guide.yaml

@@ -34,13 +34,13 @@ probe_model: /home/tpx/project/RemoteAR3/data/models/Probe.stl
 
 registration_list:
   - name: Femur
-    model_file: /home/tpx/project/DepthGuide/data/model/Femur_3.stl
+    model_file: /home/tpx/data/KneeWithNails/Femur.stl
     collect_obj: point_picker_in_femur_ref
     collect_var: picked_point_in_femur_ref
     target_var: femur_in_femur_ref
     probe_var: probe_in_femur
   - name: Tibia
-    model_file: /home/tpx/project/DepthGuide/data/model/Tibia_3.stl
+    model_file: /home/tpx/data/KneeWithNails/Tibia.stl
     collect_obj: point_picker_in_tibia_ref
     collect_var: picked_point_in_tibia_ref
     target_var: tibia_in_tibia_ref

src/core/math_helper.hpp

@@ -28,17 +28,24 @@ inline glm::mat4 to_transform_mat(glm::vec3 t, glm::mat3 r) {
     return ret;
 }
 
-template<typename Scalar, int Mode>
-inline glm::mat4 to_mat4(const Eigen::Transform<Scalar, 3, Mode> &m) {
+template<typename DerivedT>
+inline glm::mat4 to_mat4(const Eigen::MatrixBase<DerivedT> &m) {
+    using MatType = Eigen::MatrixBase<DerivedT>;
+    static_assert(MatType::RowsAtCompileTime == 4);
+    static_assert(MatType::ColsAtCompileTime == 4);
     auto ret = glm::mat4();
-    auto &mat = m.matrix();
     for (auto j = 0; j < 4; ++j)
         for (auto i = 0; i < 4; ++i) {
-            ret[j][i] = mat(i, j);
+            ret[j][i] = m(i, j);
         }
     return ret;
 }
 
+template<typename Scalar, int Mode>
+inline glm::mat4 to_mat4(const Eigen::Transform<Scalar, 3, Mode> &m) {
+    return to_mat4(m.matrix());
+}
+
 template<typename Scalar, int Mode>
 inline auto to_eigen_transform(const glm::mat4 &m) {
     auto ret = Eigen::Transform<Scalar, 3, Mode>();
@@ -56,6 +63,23 @@ inline void to_eigen_transform(const glm::mat4 &m,
     ret = to_eigen_transform<Scalar, Mode>(m);
 }
 
+inline Eigen::Matrix3f to_eigen(const glm::mat3 &m) {
+    auto ret = Eigen::Matrix3f();
+    for (auto j = 0; j < 3; ++j)
+        for (auto i = 0; i < 3; ++i) {
+            ret(i, j) = m[j][i];
+        }
+    return ret;
+}
+
+inline Eigen::Vector3f to_eigen(const glm::vec3 &v) {
+    auto ret = Eigen::Vector3f();
+    for (auto k = 0; k < 3; ++k) {
+        ret(k) = v[k];
+    }
+    return ret;
+}
+
 template<typename T>
 concept Vec2Type = requires(T t) {
     { t.x } -> std::convertible_to<float>;
@@ -133,6 +157,27 @@ inline glm::mat4 transform_mix(const glm::mat4 &x, const glm::mat4 &y, float a)
     return to_transform_mat(zt, glm::mat3_cast(zq));
 }
 
+template<typename DerivedA, typename DerivedB>
+inline auto kron_product(const Eigen::MatrixBase<DerivedA> &a,
+                         const Eigen::MatrixBase<DerivedB> &b) {
+    using typeA = typename Eigen::MatrixBase<DerivedA>;
+    using typeB = typename Eigen::MatrixBase<DerivedB>;
+    static_assert(typeA::SizeAtCompileTime != Eigen::Dynamic);
+    static_assert(typeB::SizeAtCompileTime != Eigen::Dynamic);
+    using ScalarT = typename Eigen::MatrixBase<DerivedA>::Scalar;
+    static constexpr auto a_row = typeA::RowsAtCompileTime, a_col = typeA::ColsAtCompileTime;
+    static constexpr auto b_row = typeB::RowsAtCompileTime, b_col = typeB::ColsAtCompileTime;
+    using RetT = typename Eigen::Matrix<ScalarT, a_row * b_row, a_col * b_col>;
+
+    auto ret = RetT();
+    ret.setZero();
+    for (auto i = 0; i < a_row; ++i)
+        for (auto j = 0; j < a_col; ++j) {
+            ret.template block<b_row, b_col>(i * b_row, j * b_col) = a(i, j) * b;
+        }
+    return ret;
+}
+
 class transform_buffer {
 public:
     transform_buffer();

src/image_process/impl/camera_calibrator.cpp

@@ -11,6 +11,7 @@
 
 #include <glm/ext/quaternion_geometric.hpp>
 #include <glm/gtc/matrix_access.hpp>
+#include <glm/gtx/string_cast.hpp>
 
 #define NANOVG_GL3_IMPLEMENTATION
 
@@ -159,13 +160,67 @@ namespace camera_calibrator_impl {
             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));
+        auto inv_op = [](const cv::Mat &r, const cv::Mat &t) {
+            return glm::inverse(to_transform_mat(to_vec3(t), to_mat3(r)));
+        };
+        auto mat_A = std::vector<glm::mat4>(); // reference in tracker
+        auto mat_B = std::vector<glm::mat4>(); // camera in chess board
+        std::ranges::transform(ref_r_vec, ref_t_vec, std::back_inserter(mat_A), inv_op);
+        std::ranges::transform(cam_r_vec, cam_t_vec, std::back_inserter(mat_B), inv_op);
+
+        // weighted shah algorithm
+        using namespace Eigen;
+        using mat_T_type = Matrix<float, 9, 9>;
+        auto mat_T = mat_T_type();
+        mat_T.setZero();
+        for (auto k = 0; k < sample_num; ++k) {
+            auto Ra = glm::mat3(mat_A[k]), Rb = glm::mat3(mat_B[k]);
+            mat_T += sample_w[k] * kron_product(to_eigen(Ra), to_eigen(Rb));
+        }
+
+        JacobiSVD<mat_T_type> svdT(mat_T, ComputeFullU | ComputeFullV);
+        auto x = svdT.matrixV().col(0), y = svdT.matrixU().col(0);
+        auto shah_solve_op = [](Matrix3f x) -> Matrix3f {
+            auto x_det = x.determinant();
+            x *= (x_det >= 0 ? 1.f : -1.f)
+                 / std::pow(std::abs(x_det), 1.f / 3.f);
+            JacobiSVD <Matrix3f> svdX(x, ComputeFullU | ComputeFullV);
+            x = svdX.matrixU() * svdX.matrixV().transpose();
+            return x.transpose();
+        };
+        auto Rx = shah_solve_op(Map<const Matrix3f>(x.data()));
+        auto Ry = shah_solve_op(Map<const Matrix3f>(y.data()));
+
+        using mat_A_type = Matrix<float, Dynamic, 6>;
+        mat_A_type A = MatrixXf::Zero(3 * sample_num, 6);
+        VectorXf b = VectorXf::Zero(3 * sample_num);
+        for (int k = 0; k < sample_num; ++k) {
+            auto ta = to_eigen(to_translation(mat_A[k]));
+            auto tb = to_eigen(to_translation(mat_B[k]));
+            A.block<3, 3>(3 * k, 0) = sample_w[k] * -to_eigen(glm::mat3(mat_A[k]));
+            A.block<3, 3>(3 * k, 3) = sample_w[k] * Matrix3f::Identity();
+            b.segment<3>(3 * k) = sample_w[k] * (ta - Ry * tb);
+        }
+        using vec_t_type = Vector<float, 6>;
+        vec_t_type t = A.colPivHouseholderQr().solve(b);
+        Matrix4f X, Y;
+        X << Rx, t.segment<3>(0), 0, 0, 0, 1; // camera in reference
+        Y << Ry, t.segment<3>(3), 0, 0, 0, 1; // chess board in tracker
+
+        auto aux_mat = glm::inverse(to_mat4(Y));
+        aux_r = to_cv_mat(glm::mat3(aux_mat));
+        aux_t = to_cv_mat(to_translation(aux_mat));
+//        SPDLOG_DEBUG("Shah: {}", glm::to_string(aux_mat));
+        return glm::inverse(to_mat4(X));
+
+//        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
+//        SPDLOG_DEBUG("OpenCV: {}", glm::to_string(to_transform_mat(to_vec3(aux_t), to_mat3(aux_r))));
+//        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) {
@@ -205,7 +260,19 @@ namespace camera_calibrator_impl {
                 err_mat.at<cv::Vec2f>(j, i)[1] = err_2d;
             }
         }
-        return cv::mean(err_mat);
+
+        // calculate weighted average error
+        cv::Scalar err_sum = {};
+        float w_sum = 0.f;
+        for (auto j = 0; j < sample_num; ++j) {
+            for (auto i = 0; i < corner_num; ++i) {
+                auto err = sample_w[j] * err_mat.at<cv::Vec2f>(j, i);
+                err_sum[0] += err[0];
+                err_sum[1] += err[1];
+            }
+            w_sum += sample_w[j] * corner_num;
+        }
+        return err_sum / w_sum;
     }
 
     hand_eye_calib::camera_calib_result
@@ -286,6 +353,24 @@ namespace camera_calibrator_impl {
         return ret;
     }
 
+    void hand_eye_calib::calc_sample_weights() {
+        auto cam_p = std::vector<glm::vec3>();
+        auto cam_p_op = [](const cv::Mat &r, const cv::Mat &t) {
+            auto t_mat = to_transform_mat(to_vec3(t), to_mat3(r));
+            return to_translation(t_mat);
+        };
+        std::ranges::transform(cam_r_vec, cam_t_vec,
+                               std::back_inserter(cam_p), cam_p_op);
+
+        sample_w.clear();
+        auto w_op = [&](glm::vec3 x) {
+            return 1.f / std::ranges::count_if(cam_p, [=](glm::vec3 y) {
+                return glm::distance(x, y) <= sample_neighbor_radius;
+            });
+        };
+        std::ranges::transform(cam_p, std::back_inserter(sample_w), w_op);
+    }
+
     void hand_eye_calib::calc() {
         sample_num = img_pool.size();
         corner_num = img_pool.begin()->corner.size();
@@ -341,6 +426,7 @@ namespace camera_calibrator_impl {
             cam_r_vec.push_back(cam_r);
             cam_t_vec.push_back(cam_t);
         }
+        calc_sample_weights();
 
         // find minimal error using trichotomy algorithm
         auto offset_l = -128000; // -128ms
@@ -450,11 +536,13 @@ void camera_calibrator::impl::process() {
         conf.sophiar_conn->update_transform_variable(
                 conf.result_transform_var, ret);
     }
-    conf.cb_func(result_type{
-            .cam_in = calib->intrinsic_v0(),
-            .transform = calib->result_mat,
-            .time_offset = calib->result_ts_offset,
-    });
+    if (conf.cb_func) {
+        conf.cb_func(result_type{
+                .cam_in = calib->intrinsic_v0(),
+                .transform = calib->result_mat,
+                .time_offset = calib->result_ts_offset,
+        });
+    }
 }
 
 void camera_calibrator::impl::simulate_process(const simulate_info_type &info) {

src/image_process/impl/camera_calibrator_impl.h

@@ -20,6 +20,7 @@ namespace camera_calibrator_impl {
     // TODO: make configurable
     static constexpr auto sharpness_threshold = 4.f;
     static constexpr auto track_err_threshold = 0.25f;
+    static constexpr auto sample_neighbor_radius = 5.f;
 
     using corner_type = std::vector<cv::Point2f>;
 
@@ -112,12 +113,15 @@ namespace camera_calibrator_impl {
         // evaluate hand-eye calibration result
         cv::Scalar evaluate_hand_eye(const glm::mat4 &ret_mat);
 
+        void calc_sample_weights();
+
         size_t sample_num, corner_num;
         camera_calib_result cam_cv;
         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
+        std::vector<float> sample_w; // sample weights
     };
 
 }

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/exp-20240605/calib/calib_data_c5.txt",
             .img_size = cv::Size(1920, 1080),
     };
     calib->simulate_process(sim_info);

src/impl/apps/endo_guide/endo_guide.cpp

@@ -155,7 +155,7 @@ app_endo_guide::app_endo_guide(const create_config &_conf) {
     });
 
     auto sim_info = camera_calibrator::simulate_info_type{
-            .data_path = "/home/tpx/project/DepthGuide/cmake-build-debug/exp-20240605/calib/calib_data_c5.txt",
+            .data_path = "/home/tpx/project/DepthGuide/cmake-build-debug/exp-20240605/calib/calib_data_c1.txt",
             .img_size = cv::Size(1920, 1080),
     };
     cam_calib->simulate_process(sim_info);