self-calibration

src/feature/feature_processing.cc

@@ -266,7 +266,7 @@ void FeatureMatching(const std::vector<Frame> &frames,
                 points1.push_back(frame1.points[match.id1]);
                 points2.push_back(frame2.points[match.id2]);
             }
-            SolveFundamnetalCOLMAP(points1, points2, frame_pair);
+            SolveFundamentalCOLMAP(points1, points2, frame_pair);
         } else {
             CHECK(false); // TODO it only work with known camera parameters
         }

src/geometry/epipolar_geometry.hpp

@@ -7,7 +7,7 @@
 #include "ransac/loransac.h"
 
 namespace xrsfm {
-inline void SolveFundamnetalCOLMAP(const std::vector<Eigen::Vector2d> &points1,
+inline void SolveFundamentalCOLMAP(const std::vector<Eigen::Vector2d> &points1,
                                    const std::vector<Eigen::Vector2d> &points2,
                                    FramePair &frame_pair) {
     colmap::Options option;

src/geometry/essential.cc

@@ -96,10 +96,8 @@ Polynomial operator*(const double &scale, const Polynomial &poly) {
     return Polynomial(scale * poly.coefficients());
 }
 
-inline matrix3 to_matrix(const vector<9> &vec) {
-    return (matrix3() << vec.segment<3>(0), vec.segment<3>(3),
-            vec.segment<3>(6))
-        .finished();
+inline map<matrix3> to_matrix(const vector<9> &vec) {
+    return map<matrix3>(vec.data());
 }
 
 inline matrix<9, 4>
@@ -307,28 +305,6 @@ matrix3 find_essential_matrix(const std::vector<vector2> &points1,
                               const std::vector<vector2> &points2,
                               double threshold, double confidence,
                               size_t max_iteration, int seed) {
-    // Diagnosis::TimingItem timing =
-    // Diagnosis::time(DI_F_ESSENTIAL_RANSAC_TIME);
-#if 0
-        ITSLAM_UNUSED_EXPR(max_iteration);
-    std::vector<cv::Point2f> cvPoints1, cvPoints2;
-    for (size_t i = 0; i < points1.size(); ++i) {
-        cvPoints1.emplace_back(float(points1[i].x()), float(points1[i].y()));
-        cvPoints2.emplace_back(float(points2[i].x()), float(points2[i].y()));
-    }
-    cv::Mat cvE = cv::findEssentialMat(cvPoints1, cvPoints2, cv::Mat::eye(3, 3, CV_32FC1), cv::RANSAC, confidence, threshold, cv::noArray());
-    // workaround: fuck opencv
-    if (cvE.rows != 3 || cvE.cols != 3) {
-        return matrix3::Identity() * std::numeric_limits<double>::quiet_NaN();
-    }
-    matrix3 E;
-    for (int i = 0; i < 3; ++i) {
-        for (int j = 0; j < 3; ++j) {
-            E(i, j) = cvE.at<double>(i, j);
-        }
-    }
-    return E;
-#else
     LotBox lotbox(points1.size());
     lotbox.seed(seed);
     double K = log(1 - confidence);
@@ -341,9 +317,9 @@ matrix3 find_essential_matrix(const std::vector<vector2> &points1,
 
     size_t iter_max = max_iteration;
     for (size_t iter = 0; iter < iter_max; ++iter) {
+        std::array<vector2, 5> pts1, pts2;
         // generate hypothesis
         lotbox.refill_all();
-        std::array<vector2, 5> pts1, pts2;
         for (size_t i = 0; i < 5; ++i) {
             size_t sample_id = lotbox.draw_without_replacement();
             pts1[i] = points1[sample_id];
@@ -383,7 +359,6 @@ matrix3 find_essential_matrix(const std::vector<vector2> &points1,
     }
     // printf("iter_num: %d inlier_num: %d\n", iter_max, best_inlier);
     return best_E;
-#endif
 }
 
 void solve_essential(const std::vector<vector2> &points1,

src/mapper/incremental_mapper.cc

@@ -77,8 +77,9 @@ void IncrementalMapper::Reconstruct(Map &map) {
             TIMING(timer.che, p3d_processor.CheckTrackDepth(map));
             p3d_processor.CheckFramesMeasurement(map, options.th_rpe_lba,
                                                  options.th_angle_lba);
-            TIMING(timer.gba, ba_solver.KGBA(map, std::vector<int>(0), true));
-            // TIMING(timer.gba, ba_solver.GBA(map));
+            // TIMING(timer.gba, ba_solver.KGBA(map, std::vector<int>(0),
+            // true));
+            TIMING(timer.gba, ba_solver.GBA(map));
             TIMING(timer.fil,
                    p3d_processor.FilterPoints3d(map, options.th_rpe_gba,
                                                 options.th_angle_gba));

src/optimization/ba_solver.cc

@@ -605,18 +605,44 @@ void BASolver::LBA(int frame_id, Map &map) {
     ceres::Solve(solver_options, &problem, &summary);
 }
 
+inline void SetSubsetManifold(int size, const std::vector<int> &constant_params,
+                              ceres::Problem *problem, double *params) {
+#if CERES_VERSION_MAJOR >= 3 ||                                                \
+    (CERES_VERSION_MAJOR == 2 && CERES_VERSION_MINOR >= 1)
+    problem->SetManifold(params,
+                         new ceres::SubsetManifold(size, constant_params));
+#else
+    problem->SetParameterization(
+        params, new ceres::SubsetParameterization(size, constant_params));
+#endif
+}
+
 void BASolver::GBA(Map &map, bool accurate, bool fix_all_frames) {
     // set up problem
     ceres::Problem problem;
+    std::set<int> problem_camera_ids;
     std::set<int> fixed_camera_ids;
     for (auto &frame : map.frames_) {
         if (!frame.registered)
             continue;
         SetUp(problem, map, frame);
-        if (fixed_camera_ids.count(frame.camera_id) == 0) {
-            fixed_camera_ids.insert(frame.camera_id);
-            problem.SetParameterBlockConstant(
-                map.Camera(frame.camera_id).params_.data());
+        problem_camera_ids.insert(frame.camera_id);
+    }
+    // ParameterizeCameras
+    bool fix_camera = false;
+    for (const int camera_id : problem_camera_ids) {
+        auto &camera = map.Camera(camera_id);
+        if (fix_camera) {
+            problem.SetParameterBlockConstant(camera.params_.data());
+        } else {
+            std::vector<int> const_param_ids;
+            const_param_ids.push_back(index_cx(camera.model_id_));
+            const_param_ids.push_back(index_cy(camera.model_id_));
+            if (const_param_ids.size() > 0) {
+                SetSubsetManifold(static_cast<int>(camera.params_.size()),
+                                  const_param_ids, &problem,
+                                  camera.params_.data());
+            }
         }
     }
 

src/run_reconstruction.cc

@@ -6,25 +6,80 @@
 #include "mapper/incremental_mapper.h"
 #include "utility/io_ecim.hpp"
 #include "utility/io_feature.hpp"
+#include "geometry/epipolar_geometry.hpp"
 
 using namespace xrsfm;
 
+bool self_calibration(std::vector<Frame> &frames,
+                      std::vector<FramePair> &frame_pairs, double w, double h,
+                      double &estimated_focal) {
+    std::map<float, float> focal_score_map;
+
+    for (int k = 0; k < 100; ++k) {
+        const double f = 0.25 * w + 2 * w * k / 100;
+        focal_score_map[f] = 0;
+    }
+
+    int count = 0;
+    for (const auto &fp : frame_pairs) {
+        if (fp.inlier_num < 200)
+            continue;
+
+        std::vector<Eigen::Vector2d> points1, points2;
+        for (const auto &match : fp.matches) {
+            points1.push_back(frames.at(fp.id1).points[match.id1]);
+            points2.push_back(frames.at(fp.id2).points[match.id2]);
+        }
+        double distance = 0;
+        for (int i = 0; i < points1.size(); ++i) {
+            points1.at(i).x() -= 0.5 * w;
+            points1.at(i).y() -= 0.5 * h;
+            points2.at(i).x() -= 0.5 * w;
+            points2.at(i).y() -= 0.5 * h;
+            distance = (points2.at(i) - points1.at(i)).norm();
+        }
+        distance /= points1.size();
+        FramePair tmp_fp;
+        SolveFundamentalCOLMAP(points1, points2, tmp_fp);
+
+        for (int k = 0; k < 100; ++k) {
+            const double f = 0.25 * w + 2 * w * k / 100;
+            Eigen::Matrix3d A = tmp_fp.F;
+            A(0, 2) /= f;
+            A(1, 2) /= f;
+            A(2, 0) /= f;
+            A(2, 1) /= f;
+            A(2, 2) /= (f * f);
+            Eigen::JacobiSVD<Eigen::Matrix3d> svd(A, Eigen::ComputeFullU |
+                                                         Eigen::ComputeFullV);
+            Eigen::Vector3d s = svd.singularValues();
+            s /= s(0);
+            focal_score_map[f] += s(1);
+        }
+        count++;
+    }
+
+    std::vector<std::pair<float, float>> focal_vec(focal_score_map.begin(),
+                                                   focal_score_map.end());
+    std::sort(
+        focal_vec.begin(), focal_vec.end(),
+        [](const std::pair<float, float> &a, const std::pair<float, float> &b) {
+            return a.second > b.second;
+        });
+
+    estimated_focal = focal_vec.at(0).first;
+    std::cout << focal_vec.at(0).first << " " << focal_vec.at(0).second / count
+              << std::endl;
+
+    return true;
+}
+
 void PreProcess(const std::string dir_path, const std::string camera_path,
                 Map &map) {
     std::vector<Frame> frames;
     std::vector<FramePair> frame_pairs;
     ReadFeatures(dir_path + "ftr.bin", frames, true);
     ReadFramePairs(dir_path + "fp.bin", frame_pairs);
-    std::cout << "ReadFramePairs\n";
-
-    // set cameras & image name
-    std::map<int, Camera> cameras = ReadCamerasText(camera_path);
-    CHECK_EQ(cameras.size(), 1);
-    const int camera_id = cameras.begin()->first;
-
-    for (auto &frame : frames) {
-        frame.camera_id = camera_id;
-    }
 
     // convert keypoint to points(for reconstruction)
     for (auto &frame : frames) {
@@ -38,6 +93,33 @@ void PreProcess(const std::string dir_path, const std::string camera_path,
         }
     }
 
+    // set cameras & image name
+    std::map<int, Camera> cameras;
+    if (camera_path == "auto") {
+        std::vector<ImageSize> image_size;
+        LoadImageSize(dir_path + "size.bin", image_size);
+        const int w = image_size.at(0).width;
+        const int h = image_size.at(0).height;
+
+        double focal = 0.5 * w;
+        self_calibration(frames, frame_pairs, w, h, focal);
+
+        Camera seq(0, "SIMPLE_RADIAL", w, h);
+        seq.params_ = {focal, 0.5 * w, 0.5 * h, 0};
+        cameras[0] = seq;
+    } else {
+        cameras = ReadCamerasText(camera_path);
+    }
+
+    CHECK_EQ(cameras.size(), 1);
+    const int camera_id = cameras.begin()->first;
+
+    for (auto &frame : frames) {
+        frame.camera_id = camera_id;
+    }
+
+    // TODO check single camera
+
     map.camera_map_ = cameras;
     map.frames_ = frames;
     map.frame_pairs_ = frame_pairs;

src/run_triangulation.cc

@@ -85,7 +85,7 @@ void PreProcess(const std::string bin_path, const std::string feature_path,
             points1.push_back(frame1.points[match.id1]);
             points2.push_back(frame2.points[match.id2]);
         }
-        SolveFundamnetalCOLMAP(points1, points2, frame_pair);
+        SolveFundamentalCOLMAP(points1, points2, frame_pair);
         if (frame_pair.inlier_num < 30)
             continue;
         std::vector<Match> new_matches;

src/utility/io_feature.hpp

@@ -41,7 +41,7 @@ inline void ReadFeatures(const std::string &file_name,
         std::cerr << "Error: can not open " << file_name << "\n";
         return;
     }
-    // points track_ids_ keypoints_ uint_descs_
+
     int num_frames = -1;
     read_data(file, num_frames);
     if (init_frames) {
@@ -79,7 +79,7 @@ inline void SaveFeatures(const std::string &file_name,
         std::cerr << "Error: can not open " << file_name << "\n";
         return;
     }
-    // points track_ids_ keypoints_ uint_descs_
+
     int num_frames = frames.size();
     write_data(file, num_frames);
     for (const auto &frame : frames) {