Implemented RANSAC camera calibration.

src/image_process/impl/camera_calibrator.cpp

@@ -17,6 +17,7 @@
 #include <nanovg_gl.h>
 
 #include <fstream>
+#include <random>
 #include <ranges>
 
 namespace camera_calibrator_impl {
@@ -195,7 +196,7 @@ namespace camera_calibrator_impl {
             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,
+                              cam_cv.intrinsic, cam_cv.dist_coeffs,
                               c_pred);
             assert(c_pred.size() == corner_num);
             for (auto i = 0; i < corner_num; ++i) {
@@ -207,45 +208,90 @@ namespace camera_calibrator_impl {
         return cv::mean(err_mat);
     }
 
-    void hand_eye_calib::calc() {
-        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;
+    hand_eye_calib::camera_calib_result
+    hand_eye_calib::camera_calib(const img_index_list_type &index) {
         auto img_ps_in = std::vector<corner_type>();
-        auto img_select_op = [&](corner_type &c_cand) {
-            return std::ranges::all_of(img_ps_in, [&](corner_type &c_exist) {
-                return corner_distance(c_cand, c_exist) > pix_dis_threshold;
-            });
-        };
-
-        auto img_ps_all = std::vector<corner_type>();
-        std::ranges::transform(img_pool, std::back_inserter(img_ps_all),
-                               [](const auto &item) { return item.corner; });
-        std::ranges::copy_if(img_ps_all, std::back_inserter(img_ps_in), img_select_op);
-
+        std::ranges::transform(index, std::back_inserter(img_ps_in),
+                               [this](const auto &ind) { return img_pool[ind].corner; });
         auto select_num = img_ps_in.size();
-        auto obj_ps_in =
-                std::vector<object_points_type>(select_num, obj_ps);
+        auto obj_ps_in = std::vector<object_points_type>(select_num, obj_ps);
 
+        auto ret = camera_calib_result();
         cv::Mat r_vec_ignore, t_vec_ignore;
         auto err = cv::calibrateCamera(
                 obj_ps_in, img_ps_in, img_size,
-                intrinsic_cv_mat, dist_coeffs_mat,
+                ret.intrinsic, ret.dist_coeffs,
                 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);
+        return ret;
+    }
+
+    float hand_eye_calib::reproject_error(const camera_calib_result &cam,
+                                          const corner_type &img_ps) {
+        cv::Mat r_vec, t_vec;
+        auto ok = cv::solvePnP(obj_ps, img_ps,
+                               cam.intrinsic, cam.dist_coeffs,
+                               r_vec, t_vec);
+        assert(ok);
+        corner_type proj_ps;
+        cv::projectPoints(obj_ps, r_vec, t_vec,
+                          cam.intrinsic, cam.dist_coeffs, proj_ps);
+        return corner_distance(img_ps, proj_ps);
+    }
+
+    hand_eye_calib::camera_calib_result
+    hand_eye_calib::camera_calib_ransac() {
+        static constexpr auto max_iter = 32; // TODO: make configurable
+        static constexpr auto calib_img_num = 5;
+        static constexpr auto accept_threshold = 0.7f; // pix
+        static constexpr auto valid_ratio = 0.8f; // percentage
+
+        auto rd = std::random_device();
+        auto gen = std::mt19937(rd());
+        auto sampler = std::uniform_int_distribution<size_t>(0, sample_num);
+        auto err_vec = std::vector<float>(sample_num);
+
+        auto ret = camera_calib_result();
+        auto ret_err = std::numeric_limits<float>::max();
+        auto index_list = img_index_list_type();
+        for (auto k = 0; k < max_iter; ++k) {
+            index_list.resize(calib_img_num);
+            std::ranges::generate(index_list, [&] { return sampler(gen); });
+            auto cur = camera_calib(index_list);
+            // evaluate current result
+            err_vec.clear();
+            std::ranges::transform(img_pool, std::back_inserter(err_vec),
+                                   [=, this](const auto &img) { return reproject_error(cur, img.corner); });
+            auto err_mean = std::accumulate(err_vec.begin(), err_vec.end(), 0.f) / sample_num;
+            SPDLOG_DEBUG("Iter {}, error = {:.2f}pix.", k, err_mean);
+            auto ratio = 1.f * std::ranges::count_if(
+                    err_vec, [=](auto err) { return err <= accept_threshold; }) / sample_num;
+            if (ratio < valid_ratio) continue;
+            if (err_mean < ret_err) {
+                ret = cur;
+                ret_err = err_mean;
+            }
+        }
+        assert(!ret.intrinsic.empty());
+        SPDLOG_INFO("Camera calibration error is {:.2f}pix", ret_err);
+        return ret;
+    }
+
+    void hand_eye_calib::calc() {
+        sample_num = img_pool.size();
+        corner_num = img_pool.begin()->corner.size();
+
+        cam_cv = camera_calib_ransac();
+        intrinsic_mat = to_mat3(cam_cv.intrinsic);
 
-        assert(dist_coeffs_mat.size().height == 1);
-        auto dist_coeffs_num = dist_coeffs_mat.size().width;
+        assert(cam_cv.dist_coeffs.size().height == 1);
+        auto dist_coeffs_num = cam_cv.dist_coeffs.size().width;
         assert(dist_coeffs_num >= 8); // k1-3, p1-2, k4-6
         dist_coeffs.reserve(dist_coeffs_num);
-        assert(dist_coeffs_mat.type() == CV_64FC1);
+        assert(cam_cv.dist_coeffs.type() == CV_64FC1);
         for (auto k = 0; k < 8; ++k) {
-            dist_coeffs.push_back(dist_coeffs_mat.at<double>(0, k));
+            dist_coeffs.push_back(cam_cv.dist_coeffs.at<double>(0, k));
         }
 
         // solve PnP for each image
@@ -257,12 +303,13 @@ namespace camera_calibrator_impl {
         });
         std::optional<glm::quat> last_rot_quat;
         static constexpr auto quat_threshold = 0.5f;
-        for (auto &corner: img_ps_all) {
+        for (auto &img: img_pool) {
+            auto &corner = img.corner;
             cv::Mat chess_r, chess_t; // chess board in camera
 
             for (;;) {
                 bool ok = cv::solvePnP(obj_ps_pnp, corner,
-                                       intrinsic_cv_mat, dist_coeffs_mat,
+                                       cam_cv.intrinsic, cam_cv.dist_coeffs,
                                        chess_r, chess_t);
                 assert(ok);
 

src/image_process/impl/camera_calibrator_impl.h

@@ -92,6 +92,19 @@ namespace camera_calibrator_impl {
         static float corner_distance(const corner_type &c1,
                                      const corner_type &c2);
 
+        struct camera_calib_result {
+            cv::Mat intrinsic, dist_coeffs;
+        };
+
+        using img_index_list_type = std::vector<size_t>;
+
+        camera_calib_result camera_calib(const img_index_list_type &index);
+
+        float reproject_error(const camera_calib_result &cam,
+                              const corner_type &img_ps);
+
+        camera_calib_result camera_calib_ransac();
+
         // do hand-eye calibration
         // return reference in camera
         glm::mat4 calib_hand_eye();
@@ -100,7 +113,7 @@ namespace camera_calibrator_impl {
         cv::Scalar evaluate_hand_eye(const glm::mat4 &ret_mat);
 
         size_t sample_num, corner_num;
-        cv::Mat intrinsic_cv_mat, dist_coeffs_mat;
+        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;

src/impl/apps/endo_guide/endo_guide.cpp

@@ -134,11 +134,11 @@ app_endo_guide::app_endo_guide(const create_config &_conf) {
     };
     monitor = std::make_unique<sophiar_monitor>(monitor_conf);
 
-//    auto sim_info = camera_calibrator::simulate_info_type{
-//            .data_path = "/home/tpx/project/DepthGuide/cmake-build-debug/calib_data_v2.txt",
-//            .img_size = cv::Size(1920, 1080),
-//    };
-//    cam_calib->simulate_process(sim_info);
+    auto sim_info = camera_calibrator::simulate_info_type{
+            .data_path = "/home/tpx/project/DepthGuide/cmake-build-debug/calib_data_b9.txt",
+            .img_size = cv::Size(1920, 1080),
+    };
+    cam_calib->simulate_process(sim_info);
 }
 
 void app_endo_guide::show_ui() {