From 947fa0c5ff00f7baec2964e44852c9ee59e416ce Mon Sep 17 00:00:00 2001
From: oneLOH <503417472@qq.com>
Date: Tue, 4 Jun 2024 17:41:12 +0800
Subject: [PATCH] self-calibration
---
src/feature/feature_processing.cc | 2 +-
src/geometry/epipolar_geometry.hpp | 2 +-
src/geometry/essential.cc | 31 +--------
src/mapper/incremental_mapper.cc | 5 +-
src/optimization/ba_solver.cc | 34 ++++++++--
src/run_reconstruction.cc | 102 ++++++++++++++++++++++++++---
src/run_triangulation.cc | 2 +-
src/utility/io_feature.hpp | 4 +-
8 files changed, 133 insertions(+), 49 deletions(-)
diff --git a/src/feature/feature_processing.cc b/src/feature/feature_processing.cc
index ec32751..4bfccc5 100644
--- a/src/feature/feature_processing.cc
+++ b/src/feature/feature_processing.cc
@@ -266,7 +266,7 @@ void FeatureMatching(const std::vector &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
}
diff --git a/src/geometry/epipolar_geometry.hpp b/src/geometry/epipolar_geometry.hpp
index b8e3257..2c525da 100644
--- a/src/geometry/epipolar_geometry.hpp
+++ b/src/geometry/epipolar_geometry.hpp
@@ -7,7 +7,7 @@
#include "ransac/loransac.h"
namespace xrsfm {
-inline void SolveFundamnetalCOLMAP(const std::vector &points1,
+inline void SolveFundamentalCOLMAP(const std::vector &points1,
const std::vector &points2,
FramePair &frame_pair) {
colmap::Options option;
diff --git a/src/geometry/essential.cc b/src/geometry/essential.cc
index faa5a50..3f67d72 100644
--- a/src/geometry/essential.cc
+++ b/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 to_matrix(const vector<9> &vec) {
+ return map(vec.data());
}
inline matrix<9, 4>
@@ -307,28 +305,6 @@ matrix3 find_essential_matrix(const std::vector &points1,
const std::vector &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 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::quiet_NaN();
- }
- matrix3 E;
- for (int i = 0; i < 3; ++i) {
- for (int j = 0; j < 3; ++j) {
- E(i, j) = cvE.at(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 &points1,
size_t iter_max = max_iteration;
for (size_t iter = 0; iter < iter_max; ++iter) {
+ std::array pts1, pts2;
// generate hypothesis
lotbox.refill_all();
- std::array 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 &points1,
}
// printf("iter_num: %d inlier_num: %d\n", iter_max, best_inlier);
return best_E;
-#endif
}
void solve_essential(const std::vector &points1,
diff --git a/src/mapper/incremental_mapper.cc b/src/mapper/incremental_mapper.cc
index 7863b94..19f4133 100644
--- a/src/mapper/incremental_mapper.cc
+++ b/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(0), true));
- // TIMING(timer.gba, ba_solver.GBA(map));
+ // TIMING(timer.gba, ba_solver.KGBA(map, std::vector(0),
+ // true));
+ TIMING(timer.gba, ba_solver.GBA(map));
TIMING(timer.fil,
p3d_processor.FilterPoints3d(map, options.th_rpe_gba,
options.th_angle_gba));
diff --git a/src/optimization/ba_solver.cc b/src/optimization/ba_solver.cc
index 5bfaf8d..52562bf 100644
--- a/src/optimization/ba_solver.cc
+++ b/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 &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 problem_camera_ids;
std::set 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 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(camera.params_.size()),
+ const_param_ids, &problem,
+ camera.params_.data());
+ }
}
}
diff --git a/src/run_reconstruction.cc b/src/run_reconstruction.cc
index 7c582ec..7034cc4 100644
--- a/src/run_reconstruction.cc
+++ b/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 &frames,
+ std::vector &frame_pairs, double w, double h,
+ double &estimated_focal) {
+ std::map 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 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 svd(A, Eigen::ComputeFullU |
+ Eigen::ComputeFullV);
+ Eigen::Vector3d s = svd.singularValues();
+ s /= s(0);
+ focal_score_map[f] += s(1);
+ }
+ count++;
+ }
+
+ std::vector> focal_vec(focal_score_map.begin(),
+ focal_score_map.end());
+ std::sort(
+ focal_vec.begin(), focal_vec.end(),
+ [](const std::pair &a, const std::pair &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 frames;
std::vector 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 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 cameras;
+ if (camera_path == "auto") {
+ std::vector 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;
diff --git a/src/run_triangulation.cc b/src/run_triangulation.cc
index 7fb5a64..14ea29e 100644
--- a/src/run_triangulation.cc
+++ b/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 new_matches;
diff --git a/src/utility/io_feature.hpp b/src/utility/io_feature.hpp
index 3876df3..5a8685e 100644
--- a/src/utility/io_feature.hpp
+++ b/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) {