diff --git a/README.md b/README.md
index e5f4455b..5c388af3 100644
--- a/README.md
+++ b/README.md
@@ -20,6 +20,7 @@ If you use this project for your research, please cite
year={2024},
}
```
+To use the seperate rotation averaging module, refer to [this README](docs/rotation_averager.md).
## Getting Started
diff --git a/docs/rotation_averager.md b/docs/rotation_averager.md
new file mode 100644
index 00000000..5c4e9947
--- /dev/null
+++ b/docs/rotation_averager.md
@@ -0,0 +1,62 @@
+# Gravity-aligned Rotation Averaging with Circular Regression
+
+[Project page](https://lpanaf.github.io/eccv24_ra1dof/) | [Paper](https://www.ecva.net/papers/eccv_2024/papers_ECCV/papers/05651.pdf) | [Supp.](https://lpanaf.github.io/assets/pdf/eccv24_ra1dof_sm.pdf)
+---
+
+## About
+
+This project aims at solving the rotation averaging problem with gravity prior.
+To achieve this, circular regression is leveraged.
+
+If you use this project for your research, please cite
+```
+@inproceedings{pan2024ra1dof,
+ author={Pan, Linfei and Pollefeys, Marc and Barath, Daniel},
+ title={{Gravity-aligned Rotation Averaging with Circular Regression}},
+ booktitle={European Conference on Computer Vision (ECCV)},
+ year={2024},
+}
+```
+
+## Getting Started
+Install GLOMAP as instrcucted in [README](../README.md).
+Then, call the rotation averager (3 degree-of-freedom) via
+```
+glomap rotation_averager --relpose_path RELPOSE_PATH --output_path OUTPUT_PATH
+```
+
+If gravity directions are available, call the rotation averager (1 degree-of-freedom) via
+```
+glomap rotation_averager \
+ --relpose_path RELPOSE_PATH \
+ --output_path OUTPUT_PATH \
+ --gravity_path GRAVTIY PATH
+```
+It is recommended to set `--use_stratified=1` if only a subset of images have gravity direction.
+If gravity measurements are subject to i.i.d. noise, they can be refined by setting `--refine_gravity=1`.
+
+
+## File Formats
+### Relative Pose
+The relative pose file is expected to be of the following format
+```
+IMAGE_NAME_1 IMAGE_NAME_2 QW QX QY QZ TX TY TZ
+```
+Only images contained in at least one relative pose will be included in the following procedure.
+The relative pose should be 2R1 x1 + 2t1 = x2.
+
+### Gravity Direction
+The gravity direction file is expected to be of the following format
+```
+IMAGE_NAME GX GY GZ
+```
+The gravity direction $g$ should $[0, 1, 0]$ if the image is parallel to the ground plane, and the estimated rotation would have the property that $R_i \cdot [0, 1, 0]^\top = g$.
+If is acceptable if only a subset of all images have gravity direciton.
+If the specified image name does not match any known image name from relative pose, it is ignored.
+
+### Output
+The estimated global rotation will be in the following format
+```
+IMAGE_NAME QW QX QY QZ
+```
+Any images that are not within the largest connected component of the view-graph formed by the relative pose will be ignored.
diff --git a/glomap/CMakeLists.txt b/glomap/CMakeLists.txt
index 7d60ee17..e191049a 100644
--- a/glomap/CMakeLists.txt
+++ b/glomap/CMakeLists.txt
@@ -1,6 +1,7 @@
set(SOURCES
controllers/global_mapper.cc
controllers/option_manager.cc
+ controllers/rotation_averager.cc
controllers/track_establishment.cc
controllers/track_retriangulation.cc
estimators/bundle_adjustment.cc
@@ -11,7 +12,7 @@ set(SOURCES
estimators/view_graph_calibration.cc
io/colmap_converter.cc
io/colmap_io.cc
- io/gravity_io.cc
+ io/pose_io.cc
math/gravity.cc
math/rigid3d.cc
math/tree.cc
@@ -29,6 +30,7 @@ set(SOURCES
set(HEADERS
controllers/global_mapper.h
controllers/option_manager.h
+ controllers/rotation_averager.h
controllers/track_establishment.h
controllers/track_retriangulation.h
estimators/bundle_adjustment.h
@@ -41,7 +43,7 @@ set(HEADERS
estimators/view_graph_calibration.h
io/colmap_converter.h
io/colmap_io.h
- io/gravity_io.h
+ io/pose_io.h
math/gravity.h
math/l1_solver.h
math/rigid3d.h
@@ -101,7 +103,10 @@ endif()
add_executable(glomap_main
glomap.cc
exe/global_mapper.h
- exe/global_mapper.cc)
+ exe/global_mapper.cc
+ exe/rotation_averager.h
+ exe/rotation_averager.cc
+)
target_link_libraries(glomap_main glomap)
set_target_properties(glomap_main PROPERTIES OUTPUT_NAME glomap)
@@ -111,6 +116,7 @@ install(TARGETS glomap_main DESTINATION bin)
if(TESTS_ENABLED)
add_executable(glomap_test
controllers/global_mapper_test.cc
+ controllers/rotation_averager_test.cc
)
target_link_libraries(
glomap_test
diff --git a/glomap/controllers/option_manager.cc b/glomap/controllers/option_manager.cc
index d1e03042..5912441d 100644
--- a/glomap/controllers/option_manager.cc
+++ b/glomap/controllers/option_manager.cc
@@ -1,6 +1,7 @@
#include "option_manager.h"
#include "glomap/controllers/global_mapper.h"
+#include "glomap/estimators/gravity_refinement.h"
#include
#include
@@ -14,6 +15,7 @@ OptionManager::OptionManager(bool add_project_options) {
image_path = std::make_shared();
mapper = std::make_shared();
+ gravity_refiner = std::make_shared();
Reset();
desc_->add_options()("help,h", "");
@@ -248,6 +250,18 @@ void OptionManager::AddInlierThresholdOptions() {
&mapper->inlier_thresholds.max_rotation_error);
}
+void OptionManager::AddGravityRefinerOptions() {
+ if (added_gravity_refiner_options_) {
+ return;
+ }
+ added_gravity_refiner_options_ = true;
+ AddAndRegisterDefaultOption("GravityRefiner.max_outlier_ratio",
+ &gravity_refiner->max_outlier_ratio);
+ AddAndRegisterDefaultOption("GravityRefiner.max_gravity_error",
+ &gravity_refiner->max_gravity_error);
+ AddAndRegisterDefaultOption("GravityRefiner.min_num_neighbors",
+ &gravity_refiner->min_num_neighbors);
+}
void OptionManager::Reset() {
const bool kResetPaths = true;
ResetOptions(kResetPaths);
@@ -276,6 +290,7 @@ void OptionManager::ResetOptions(const bool reset_paths) {
*image_path = "";
}
*mapper = GlobalMapperOptions();
+ *gravity_refiner = GravityRefinerOptions();
}
void OptionManager::Parse(const int argc, char** argv) {
diff --git a/glomap/controllers/option_manager.h b/glomap/controllers/option_manager.h
index 0926387d..030d38e3 100644
--- a/glomap/controllers/option_manager.h
+++ b/glomap/controllers/option_manager.h
@@ -18,6 +18,7 @@ struct GlobalPositionerOptions;
struct BundleAdjusterOptions;
struct TriangulatorOptions;
struct InlierThresholdOptions;
+struct GravityRefinerOptions;
class OptionManager {
public:
@@ -37,6 +38,7 @@ class OptionManager {
void AddBundleAdjusterOptions();
void AddTriangulatorOptions();
void AddInlierThresholdOptions();
+ void AddGravityRefinerOptions();
template
void AddRequiredOption(const std::string& name,
@@ -56,6 +58,7 @@ class OptionManager {
std::shared_ptr image_path;
std::shared_ptr mapper;
+ std::shared_ptr gravity_refiner;
private:
template
@@ -88,6 +91,7 @@ class OptionManager {
bool added_bundle_adjustment_options_ = false;
bool added_triangulation_options_ = false;
bool added_inliers_options_ = false;
+ bool added_gravity_refiner_options_ = false;
};
template
diff --git a/glomap/controllers/rotation_averager.cc b/glomap/controllers/rotation_averager.cc
new file mode 100644
index 00000000..09039e84
--- /dev/null
+++ b/glomap/controllers/rotation_averager.cc
@@ -0,0 +1,61 @@
+#include "glomap/controllers/rotation_averager.h"
+
+namespace glomap {
+
+bool SolveRotationAveraging(ViewGraph& view_graph,
+ std::unordered_map& images,
+ const RotationAveragerOptions& options) {
+ view_graph.KeepLargestConnectedComponents(images);
+
+ bool solve_1dof_system = options.use_gravity && options.use_stratified;
+
+ ViewGraph view_graph_grav;
+ image_pair_t total_pairs = 0;
+ image_pair_t grav_pairs = 0;
+ if (solve_1dof_system) {
+ // Prepare two sets: ones all with gravity, and one does not have gravity.
+ // Solve them separately first, then solve them in a single system
+ for (const auto& [pair_id, image_pair] : view_graph.image_pairs) {
+ if (!image_pair.is_valid) continue;
+
+ image_t image_id1 = image_pair.image_id1;
+ image_t image_id2 = image_pair.image_id2;
+
+ Image& image1 = images[image_id1];
+ Image& image2 = images[image_id2];
+
+ if (!image1.is_registered || !image2.is_registered) continue;
+
+ total_pairs++;
+
+ if (image1.gravity_info.has_gravity && image2.gravity_info.has_gravity) {
+ view_graph_grav.image_pairs.emplace(
+ pair_id,
+ ImagePair(image_id1, image_id2, image_pair.cam2_from_cam1));
+ grav_pairs++;
+ }
+ }
+ }
+
+ // If there is no image pairs with gravity or most image pairs are with
+ // gravity, then just run the 3dof version
+ bool status = (grav_pairs == 0) || (grav_pairs > total_pairs * 0.95);
+ solve_1dof_system = solve_1dof_system && (!status);
+
+ if (solve_1dof_system) {
+ // Run the 1dof optimization
+ LOG(INFO) << "Solving subset 1DoF rotation averaging problem in the mixed "
+ "prior system";
+ int num_img_grv = view_graph_grav.KeepLargestConnectedComponents(images);
+ RotationEstimator rotation_estimator_grav(options);
+ if (!rotation_estimator_grav.EstimateRotations(view_graph_grav, images)) {
+ return false;
+ }
+ view_graph.KeepLargestConnectedComponents(images);
+ }
+
+ RotationEstimator rotation_estimator(options);
+ return rotation_estimator.EstimateRotations(view_graph, images);
+}
+
+} // namespace glomap
\ No newline at end of file
diff --git a/glomap/controllers/rotation_averager.h b/glomap/controllers/rotation_averager.h
new file mode 100644
index 00000000..cdf73893
--- /dev/null
+++ b/glomap/controllers/rotation_averager.h
@@ -0,0 +1,15 @@
+#pragma once
+
+#include "glomap/estimators/global_rotation_averaging.h"
+
+namespace glomap {
+
+struct RotationAveragerOptions : public RotationEstimatorOptions {
+ bool use_stratified = true;
+};
+
+bool SolveRotationAveraging(ViewGraph& view_graph,
+ std::unordered_map& images,
+ const RotationAveragerOptions& options);
+
+} // namespace glomap
\ No newline at end of file
diff --git a/glomap/controllers/rotation_averager_test.cc b/glomap/controllers/rotation_averager_test.cc
new file mode 100644
index 00000000..dbab164e
--- /dev/null
+++ b/glomap/controllers/rotation_averager_test.cc
@@ -0,0 +1,236 @@
+#include "glomap/controllers/rotation_averager.h"
+
+#include "glomap/controllers/global_mapper.h"
+#include "glomap/estimators/gravity_refinement.h"
+#include "glomap/io/colmap_io.h"
+#include "glomap/math/rigid3d.h"
+#include "glomap/types.h"
+
+#include
+#include
+#include
+
+#include
+
+namespace glomap {
+namespace {
+
+void CreateRandomRotation(const double stddev, Eigen::Quaterniond& q) {
+ std::random_device rd{};
+ std::mt19937 gen{rd()};
+
+ // Construct a random axis
+ double theta = double(rand()) / RAND_MAX * 2 * M_PI;
+ double phi = double(rand()) / RAND_MAX * M_PI;
+ Eigen::Vector3d axis(std::cos(theta) * std::sin(phi),
+ std::sin(theta) * std::sin(phi),
+ std::cos(phi));
+
+ // Construct a random angle
+ std::normal_distribution d{0, stddev};
+ double angle = d(gen);
+ q = Eigen::AngleAxisd(angle, axis);
+}
+
+void PrepareGravity(const colmap::Reconstruction& gt,
+ std::unordered_map& images,
+ double stddev_gravity = 0.0,
+ double outlier_ratio = 0.0) {
+ for (auto& image_id : gt.RegImageIds()) {
+ Eigen::Vector3d gravity =
+ gt.Image(image_id).CamFromWorld().rotation * Eigen::Vector3d(0, 1, 0);
+
+ if (stddev_gravity > 0.0) {
+ Eigen::Quaterniond q;
+ CreateRandomRotation(DegToRad(stddev_gravity), q);
+ gravity = q * gravity;
+ }
+
+ if (outlier_ratio > 0.0 && double(rand()) / RAND_MAX < outlier_ratio) {
+ Eigen::Quaterniond q;
+ CreateRandomRotation(1., q);
+ gravity =
+ Rigid3dToAngleAxis(Rigid3d(q, Eigen::Vector3d::Zero())).normalized();
+ }
+ images[image_id].gravity_info.SetGravity(gravity);
+ }
+}
+
+GlobalMapperOptions CreateMapperTestOptions() {
+ GlobalMapperOptions options;
+ options.skip_view_graph_calibration = false;
+ options.skip_relative_pose_estimation = false;
+ options.skip_rotation_averaging = true;
+ options.skip_track_establishment = true;
+ options.skip_global_positioning = true;
+ options.skip_bundle_adjustment = true;
+ options.skip_retriangulation = true;
+
+ return options;
+}
+
+RotationAveragerOptions CreateRATestOptions(bool use_gravity = false) {
+ RotationAveragerOptions options;
+ options.skip_initialization = true;
+ options.use_gravity = use_gravity;
+ return options;
+}
+
+void ExpectEqualRotations(const colmap::Reconstruction& gt,
+ const colmap::Reconstruction& computed,
+ const double max_rotation_error_deg) {
+ const std::set reg_image_ids_set = gt.RegImageIds();
+ std::vector reg_image_ids(reg_image_ids_set.begin(),
+ reg_image_ids_set.end());
+ for (size_t i = 0; i < reg_image_ids.size(); i++) {
+ const image_t image_id1 = reg_image_ids[i];
+ for (size_t j = 0; j < reg_image_ids.size(); j++) {
+ if (i == j) continue;
+ const image_t image_id2 = reg_image_ids[j];
+
+ const Rigid3d cam2_from_cam1 =
+ computed.Image(image_id2).CamFromWorld() *
+ colmap::Inverse(computed.Image(image_id1).CamFromWorld());
+
+ const Rigid3d cam2_from_cam1_gt =
+ gt.Image(image_id2).CamFromWorld() *
+ colmap::Inverse(gt.Image(image_id1).CamFromWorld());
+
+ double rotation_error_deg = CalcAngle(cam2_from_cam1_gt, cam2_from_cam1);
+ EXPECT_LT(rotation_error_deg, max_rotation_error_deg);
+ }
+ }
+}
+
+void ExpectEqualGravity(
+ const colmap::Reconstruction& gt,
+ const std::unordered_map& images_computed,
+ const double max_gravity_error_deg) {
+ for (const auto& image_id : gt.RegImageIds()) {
+ const Eigen::Vector3d gravity_gt =
+ gt.Image(image_id).CamFromWorld().rotation * Eigen::Vector3d(0, 1, 0);
+ const Eigen::Vector3d gravity_computed =
+ images_computed.at(image_id).gravity_info.GetGravity();
+
+ double gravity_error_deg = CalcAngle(gravity_gt, gravity_computed);
+ EXPECT_LT(gravity_error_deg, max_gravity_error_deg);
+ }
+}
+
+TEST(RotationEstimator, WithoutNoise) {
+ const std::string database_path = colmap::CreateTestDir() + "/database.db";
+
+ colmap::Database database(database_path);
+ colmap::Reconstruction gt_reconstruction;
+ colmap::SyntheticDatasetOptions synthetic_dataset_options;
+ synthetic_dataset_options.num_cameras = 2;
+ synthetic_dataset_options.num_images = 9;
+ synthetic_dataset_options.num_points3D = 50;
+ synthetic_dataset_options.point2D_stddev = 0;
+ colmap::SynthesizeDataset(
+ synthetic_dataset_options, >_reconstruction, &database);
+
+ ViewGraph view_graph;
+ std::unordered_map cameras;
+ std::unordered_map images;
+ std::unordered_map tracks;
+
+ ConvertDatabaseToGlomap(database, view_graph, cameras, images);
+
+ // PrepareRelativeRotations(view_graph, images);
+ PrepareGravity(gt_reconstruction, images);
+
+ GlobalMapper global_mapper(CreateMapperTestOptions());
+ global_mapper.Solve(database, view_graph, cameras, images, tracks);
+
+ // Version with Gravity
+ for (bool use_gravity : {true, false}) {
+ SolveRotationAveraging(
+ view_graph, images, CreateRATestOptions(use_gravity));
+
+ colmap::Reconstruction reconstruction;
+ ConvertGlomapToColmap(cameras, images, tracks, reconstruction);
+ ExpectEqualRotations(
+ gt_reconstruction, reconstruction, /*max_rotation_error_deg=*/1e-2);
+ }
+}
+
+TEST(RotationEstimator, WithNoiseAndOutliers) {
+ const std::string database_path = colmap::CreateTestDir() + "/database.db";
+
+ // FLAGS_v = 1;
+ colmap::Database database(database_path);
+ colmap::Reconstruction gt_reconstruction;
+ colmap::SyntheticDatasetOptions synthetic_dataset_options;
+ synthetic_dataset_options.num_cameras = 2;
+ synthetic_dataset_options.num_images = 7;
+ synthetic_dataset_options.num_points3D = 100;
+ synthetic_dataset_options.point2D_stddev = 1;
+ synthetic_dataset_options.inlier_match_ratio = 0.6;
+ colmap::SynthesizeDataset(
+ synthetic_dataset_options, >_reconstruction, &database);
+
+ ViewGraph view_graph;
+ std::unordered_map cameras;
+ std::unordered_map images;
+ std::unordered_map tracks;
+
+ ConvertDatabaseToGlomap(database, view_graph, cameras, images);
+
+ PrepareGravity(gt_reconstruction, images, /*stddev_gravity=*/3e-1);
+
+ GlobalMapper global_mapper(CreateMapperTestOptions());
+ global_mapper.Solve(database, view_graph, cameras, images, tracks);
+
+ for (bool use_gravity : {true, false}) {
+ SolveRotationAveraging(
+ view_graph, images, CreateRATestOptions(use_gravity));
+
+ colmap::Reconstruction reconstruction;
+ ConvertGlomapToColmap(cameras, images, tracks, reconstruction);
+ if (use_gravity)
+ ExpectEqualRotations(
+ gt_reconstruction, reconstruction, /*max_rotation_error_deg=*/1.);
+ else
+ ExpectEqualRotations(
+ gt_reconstruction, reconstruction, /*max_rotation_error_deg=*/2.);
+ }
+}
+
+TEST(RotationEstimator, RefineGravity) {
+ const std::string database_path = colmap::CreateTestDir() + "/database.db";
+
+ // FLAGS_v = 2;
+ colmap::Database database(database_path);
+ colmap::Reconstruction gt_reconstruction;
+ colmap::SyntheticDatasetOptions synthetic_dataset_options;
+ synthetic_dataset_options.num_cameras = 2;
+ synthetic_dataset_options.num_images = 100;
+ synthetic_dataset_options.num_points3D = 200;
+ synthetic_dataset_options.point2D_stddev = 0;
+ colmap::SynthesizeDataset(
+ synthetic_dataset_options, >_reconstruction, &database);
+
+ ViewGraph view_graph;
+ std::unordered_map cameras;
+ std::unordered_map images;
+ std::unordered_map tracks;
+
+ ConvertDatabaseToGlomap(database, view_graph, cameras, images);
+
+ PrepareGravity(
+ gt_reconstruction, images, /*stddev_gravity=*/0., /*outlier_ratio=*/0.3);
+
+ GlobalMapper global_mapper(CreateMapperTestOptions());
+ global_mapper.Solve(database, view_graph, cameras, images, tracks);
+
+ GravityRefinerOptions opt_grav_refine;
+ GravityRefiner grav_refiner(opt_grav_refine);
+ grav_refiner.RefineGravity(view_graph, images);
+
+ // Check whether the gravity does not have error after refinement
+ ExpectEqualGravity(gt_reconstruction, images, /*max_gravity_error_deg=*/1e-2);
+}
+
+} // namespace
+} // namespace glomap
diff --git a/glomap/estimators/global_rotation_averaging.cc b/glomap/estimators/global_rotation_averaging.cc
index 3f7d0319..e7122feb 100644
--- a/glomap/estimators/global_rotation_averaging.cc
+++ b/glomap/estimators/global_rotation_averaging.cc
@@ -16,6 +16,15 @@ double RelAngleError(double angle_12, double angle_1, double angle_2) {
while (est < -EIGEN_PI) est += TWO_PI;
+ // Inject random noise if the angle is too close to the boundary to break the
+ // possible balance at the local minima
+ if (est > EIGEN_PI - 0.01 || est < -EIGEN_PI + 0.01) {
+ if (est < 0)
+ est += (rand() % 1000) / 1000.0 * 0.01;
+ else
+ est -= (rand() % 1000) / 1000.0 * 0.01;
+ }
+
return est;
}
} // namespace
@@ -56,6 +65,9 @@ bool RotationEstimator::EstimateRotations(
image.cam_from_world.rotation = Eigen::Quaterniond(AngleAxisToRotation(
rotation_estimated_.segment(image_id_to_idx_[image_id], 3)));
}
+ // Restore the prior position (t = -Rc = R * R_ori * t_ori = R * t_ori)
+ image.cam_from_world.translation =
+ (image.cam_from_world.rotation * image.cam_from_world.translation);
}
return true;
@@ -207,6 +219,8 @@ void RotationEstimator::SetupLinearSystem(
// Establish linear systems
size_t curr_pos = 0;
+ std::vector weights;
+ weights.reserve(3 * view_graph.image_pairs.size());
for (const auto& [pair_id, image_pair] : view_graph.image_pairs) {
if (!image_pair.is_valid) continue;
@@ -221,6 +235,10 @@ void RotationEstimator::SetupLinearSystem(
if (rel_temp_info_[pair_id].has_gravity) {
coeffs.emplace_back(Eigen::Triplet(curr_pos, vector_idx1, -1));
coeffs.emplace_back(Eigen::Triplet(curr_pos, vector_idx2, 1));
+ if (image_pair.weight >= 0)
+ weights.emplace_back(image_pair.weight);
+ else
+ weights.emplace_back(1);
curr_pos++;
} else {
// If it is not gravity aligned, then we need to consider 3 dof
@@ -245,6 +263,12 @@ void RotationEstimator::SetupLinearSystem(
} else
coeffs.emplace_back(
Eigen::Triplet(curr_pos + 1, vector_idx2, 1));
+ for (int i = 0; i < 3; i++) {
+ if (image_pair.weight >= 0)
+ weights.emplace_back(image_pair.weight);
+ else
+ weights.emplace_back(1);
+ }
curr_pos += 3;
}
@@ -257,11 +281,13 @@ void RotationEstimator::SetupLinearSystem(
images[fixed_camera_id_].gravity_info.has_gravity) {
coeffs.emplace_back(Eigen::Triplet(
curr_pos, image_id_to_idx_[fixed_camera_id_], 1));
+ weights.emplace_back(1);
curr_pos++;
} else {
for (int i = 0; i < 3; i++) {
coeffs.emplace_back(Eigen::Triplet(
curr_pos + i, image_id_to_idx_[fixed_camera_id_] + i, 1));
+ weights.emplace_back(1);
}
curr_pos += 3;
}
@@ -269,6 +295,14 @@ void RotationEstimator::SetupLinearSystem(
sparse_matrix_.resize(curr_pos, num_dof);
sparse_matrix_.setFromTriplets(coeffs.begin(), coeffs.end());
+ // Set up the weight matrix for the linear system
+ if (!options_.use_weight) {
+ weights_ = Eigen::ArrayXd::Ones(curr_pos);
+ } else {
+ weights_ = Eigen::ArrayXd(weights.size());
+ for (size_t i = 0; i < weights.size(); i++) weights_[i] = weights[i];
+ }
+
// Initialize x and b
tangent_space_step_.resize(num_dof);
tangent_space_residual_.resize(curr_pos);
@@ -279,8 +313,8 @@ bool RotationEstimator::SolveL1Regression(
L1SolverOptions opt_l1_solver;
opt_l1_solver.max_num_iterations = 10;
- L1Solver> l1_solver(opt_l1_solver,
- sparse_matrix_);
+ L1Solver> l1_solver(
+ opt_l1_solver, weights_.matrix().asDiagonal() * sparse_matrix_);
double last_norm = 0;
double curr_norm = 0;
@@ -295,7 +329,8 @@ bool RotationEstimator::SolveL1Regression(
// use the current residual as b (Ax - b)
tangent_space_step_.setZero();
- l1_solver.Solve(tangent_space_residual_, &tangent_space_step_);
+ l1_solver.Solve(weights_.matrix().asDiagonal() * tangent_space_residual_,
+ &tangent_space_step_);
if (tangent_space_step_.array().isNaN().any()) {
LOG(ERROR) << "nan error";
iteration++;
@@ -393,7 +428,9 @@ bool RotationEstimator::SolveIRLS(const ViewGraph& view_graph,
}
// Update the factorization for the weighted values.
- at_weight = sparse_matrix_.transpose() * weights_irls.matrix().asDiagonal();
+ at_weight = sparse_matrix_.transpose() *
+ weights_irls.matrix().asDiagonal() *
+ weights_.matrix().asDiagonal();
llt.factorize(at_weight * sparse_matrix_);
diff --git a/glomap/estimators/global_rotation_averaging.h b/glomap/estimators/global_rotation_averaging.h
index 8899a1cb..599c7c5f 100644
--- a/glomap/estimators/global_rotation_averaging.h
+++ b/glomap/estimators/global_rotation_averaging.h
@@ -63,7 +63,7 @@ struct RotationEstimatorOptions {
// Flg to use maximum spanning tree for initialization
bool skip_initialization = false;
- // TODO: Implement the weighted version for rotation averaging
+ // Flag to use weighting for rotation averaging
bool use_weight = false;
// Flag to use gravity for rotation averaging
@@ -145,6 +145,9 @@ class RotationEstimator {
// The fixed camera rotation (if with initialization, it would not be identity
// matrix)
Eigen::Vector3d fixed_camera_rotation_;
+
+ // The weights for the edges
+ Eigen::ArrayXd weights_;
};
} // namespace glomap
diff --git a/glomap/estimators/gravity_refinement.cc b/glomap/estimators/gravity_refinement.cc
index d1f96fa7..0f679bad 100644
--- a/glomap/estimators/gravity_refinement.cc
+++ b/glomap/estimators/gravity_refinement.cc
@@ -12,6 +12,10 @@ void GravityRefiner::RefineGravity(const ViewGraph& view_graph,
view_graph.image_pairs;
const std::unordered_map>&
adjacency_list = view_graph.GetAdjacencyList();
+ if (adjacency_list.empty()) {
+ LOG(INFO) << "Adjacency list not established" << std::endl;
+ return;
+ }
// Identify the images that are error prone
int counter_rect = 0;
@@ -75,6 +79,7 @@ void GravityRefiner::RefineGravity(const ViewGraph& view_graph,
if (gravities.size() < options_.min_num_neighbors) continue;
// Then, run refinment
+ gravity = AverageGravity(gravities);
colmap::SetSphereManifold<3>(&problem, gravity.data());
ceres::Solver::Summary summary_solver;
ceres::Solve(options_.solver_options, &problem, &summary_solver);
diff --git a/glomap/estimators/view_graph_calibration.cc b/glomap/estimators/view_graph_calibration.cc
index 30c7d343..a66b5a2c 100644
--- a/glomap/estimators/view_graph_calibration.cc
+++ b/glomap/estimators/view_graph_calibration.cc
@@ -179,7 +179,7 @@ size_t ViewGraphCalibrator::FilterImagePairs(ViewGraph& view_graph) const {
}
LOG(INFO) << "invalid / total number of two view geometry: "
- << invalid_counter << " / " << counter;
+ << invalid_counter << " / " << (counter / 2);
return invalid_counter;
}
diff --git a/glomap/exe/rotation_averager.cc b/glomap/exe/rotation_averager.cc
new file mode 100644
index 00000000..98460ab5
--- /dev/null
+++ b/glomap/exe/rotation_averager.cc
@@ -0,0 +1,105 @@
+
+#include "glomap/controllers/rotation_averager.h"
+
+#include "glomap/controllers/option_manager.h"
+#include "glomap/estimators/gravity_refinement.h"
+#include "glomap/io/colmap_io.h"
+#include "glomap/io/pose_io.h"
+#include "glomap/types.h"
+
+#include
+#include
+
+namespace glomap {
+// -------------------------------------
+// Running Global Rotation Averager
+// -------------------------------------
+int RunRotationAverager(int argc, char** argv) {
+ std::string relpose_path;
+ std::string output_path;
+ std::string gravity_path = "";
+ std::string weight_path = "";
+
+ bool use_stratified = true;
+ bool refine_gravity = false;
+ bool use_weight = false;
+
+ OptionManager options;
+ options.AddRequiredOption("relpose_path", &relpose_path);
+ options.AddRequiredOption("output_path", &output_path);
+ options.AddDefaultOption("gravity_path", &gravity_path);
+ options.AddDefaultOption("weight_path", &weight_path);
+ options.AddDefaultOption("use_stratified", &use_stratified);
+ options.AddDefaultOption("refine_gravity", &refine_gravity);
+ options.AddDefaultOption("use_weight", &use_weight);
+ options.AddGravityRefinerOptions();
+ options.Parse(argc, argv);
+
+ if (!colmap::ExistsFile(relpose_path)) {
+ LOG(ERROR) << "`relpose_path` is not a file";
+ return EXIT_FAILURE;
+ }
+
+ if (gravity_path != "" && !colmap::ExistsFile(gravity_path)) {
+ LOG(ERROR) << "`gravity_path` is not a file";
+ return EXIT_FAILURE;
+ }
+
+ if (weight_path != "" && !colmap::ExistsFile(weight_path)) {
+ LOG(ERROR) << "`weight_path` is not a file";
+ return EXIT_FAILURE;
+ }
+
+ if (use_weight && weight_path == "") {
+ LOG(ERROR) << "Weight path is required when use_weight is set to true";
+ return EXIT_FAILURE;
+ }
+
+ RotationAveragerOptions rotation_averager_options;
+ rotation_averager_options.skip_initialization = true;
+ rotation_averager_options.use_gravity = true;
+
+ rotation_averager_options.use_stratified = use_stratified;
+ rotation_averager_options.use_weight = use_weight;
+
+ // Load the database
+ ViewGraph view_graph;
+ std::unordered_map images;
+
+ ReadRelPose(relpose_path, images, view_graph);
+
+ if (gravity_path != "") {
+ ReadGravity(gravity_path, images);
+ }
+
+ if (use_weight) {
+ ReadRelWeight(weight_path, images, view_graph);
+ }
+
+ int num_img = view_graph.KeepLargestConnectedComponents(images);
+ LOG(INFO) << num_img << " / " << images.size()
+ << " are within the largest connected component";
+
+ if (refine_gravity && gravity_path != "") {
+ GravityRefiner grav_refiner(*options.gravity_refiner);
+ grav_refiner.RefineGravity(view_graph, images);
+ }
+
+ colmap::Timer run_timer;
+ run_timer.Start();
+ if (!SolveRotationAveraging(view_graph, images, rotation_averager_options)) {
+ LOG(ERROR) << "Failed to solve global rotation averaging";
+ return EXIT_FAILURE;
+ }
+ run_timer.Pause();
+ LOG(INFO) << "Global rotation averaging done in "
+ << run_timer.ElapsedSeconds() << " seconds";
+
+ // Write out the estimated rotation
+ WriteGlobalRotation(output_path, images);
+ LOG(INFO) << "Global rotation averaging done" << std::endl;
+
+ return EXIT_SUCCESS;
+}
+
+} // namespace glomap
\ No newline at end of file
diff --git a/glomap/exe/rotation_averager.h b/glomap/exe/rotation_averager.h
new file mode 100644
index 00000000..0786b817
--- /dev/null
+++ b/glomap/exe/rotation_averager.h
@@ -0,0 +1,10 @@
+#pragma once
+
+#include "glomap/estimators/global_rotation_averaging.h"
+
+namespace glomap {
+
+// Use default values for most of the settings from database
+int RunRotationAverager(int argc, char** argv);
+
+} // namespace glomap
\ No newline at end of file
diff --git a/glomap/glomap.cc b/glomap/glomap.cc
index aa300a19..49d9adcb 100644
--- a/glomap/glomap.cc
+++ b/glomap/glomap.cc
@@ -1,4 +1,5 @@
#include "glomap/exe/global_mapper.h"
+#include "glomap/exe/rotation_averager.h"
#include
@@ -38,6 +39,7 @@ int main(int argc, char** argv) {
std::vector> commands;
commands.emplace_back("mapper", &glomap::RunMapper);
commands.emplace_back("mapper_resume", &glomap::RunMapperResume);
+ commands.emplace_back("rotation_averager", &glomap::RunRotationAverager);
if (argc == 1) {
return ShowHelp(commands);
@@ -56,7 +58,7 @@ int main(int argc, char** argv) {
}
if (matched_command_func == nullptr) {
std::cout << "Command " << command << " not recognized. "
- << "To list the available commands, run `colmap help`."
+ << "To list the available commands, run `glomap help`."
<< std::endl;
return EXIT_FAILURE;
} else {
diff --git a/glomap/io/colmap_converter.cc b/glomap/io/colmap_converter.cc
index a52455cd..abfe1ffc 100644
--- a/glomap/io/colmap_converter.cc
+++ b/glomap/io/colmap_converter.cc
@@ -188,14 +188,15 @@ void ConvertDatabaseToGlomap(const colmap::Database& database,
<< images_colmap.size() << std::flush;
counter++;
- image_t image_id = image.ImageId();
+ const image_t image_id = image.ImageId();
if (image_id == colmap::kInvalidImageId) continue;
auto ite = images.insert(std::make_pair(
image_id, Image(image_id, image.CameraId(), image.Name())));
const colmap::PosePrior prior = database.ReadPosePrior(image_id);
if (prior.IsValid()) {
- ite.first->second.cam_from_world = Rigid3d(
- colmap::Rigid3d(Eigen::Quaterniond::Identity(), prior.position));
+ const colmap::Rigid3d world_from_cam_prior(Eigen::Quaterniond::Identity(),
+ prior.position);
+ ite.first->second.cam_from_world = Rigid3d(Inverse(world_from_cam_prior));
} else {
ite.first->second.cam_from_world = Rigid3d();
}
@@ -204,20 +205,18 @@ void ConvertDatabaseToGlomap(const colmap::Database& database,
// Read keypoints
for (auto& [image_id, image] : images) {
- colmap::FeatureKeypoints keypoints = database.ReadKeypoints(image_id);
-
- image.features.reserve(keypoints.size());
- for (int i = 0; i < keypoints.size(); i++) {
- image.features.emplace_back(
- Eigen::Vector2d(keypoints[i].x, keypoints[i].y));
+ const colmap::FeatureKeypoints keypoints = database.ReadKeypoints(image_id);
+ const int num_keypoints = keypoints.size();
+ image.features.resize(num_keypoints);
+ for (int i = 0; i < num_keypoints; i++) {
+ image.features[i] = Eigen::Vector2d(keypoints[i].x, keypoints[i].y);
}
}
// Add the cameras
std::vector cameras_colmap = database.ReadAllCameras();
for (auto& camera : cameras_colmap) {
- camera_t camera_id = camera.camera_id;
- cameras[camera_id] = camera;
+ cameras[camera.camera_id] = camera;
}
// Add the matches
diff --git a/glomap/io/gravity_io.cc b/glomap/io/gravity_io.cc
deleted file mode 100644
index 7e974db2..00000000
--- a/glomap/io/gravity_io.cc
+++ /dev/null
@@ -1,45 +0,0 @@
-#include "gravity_io.h"
-
-#include
-
-namespace glomap {
-void ReadGravity(const std::string& gravity_path,
- std::unordered_map& images) {
- std::unordered_map name_idx;
- for (const auto& [image_id, image] : images) {
- name_idx[image.file_name] = image_id;
- }
-
- std::ifstream file(gravity_path);
-
- // Read in the file list
- std::string line, item;
- Eigen::Vector3d gravity;
- int counter = 0;
- while (std::getline(file, line)) {
- std::stringstream line_stream(line);
-
- // file_name
- std::string name;
- std::getline(line_stream, name, ' ');
-
- // Gravity
- for (double i = 0; i < 3; i++) {
- std::getline(line_stream, item, ' ');
- gravity[i] = std::stod(item);
- }
-
- // Check whether the image present
- auto ite = name_idx.find(name);
- if (ite != name_idx.end()) {
- counter++;
- images[ite->second].gravity_info.SetGravity(gravity);
- // Make sure the initialization is aligned with the gravity
- images[ite->second].cam_from_world.rotation =
- images[ite->second].gravity_info.GetRAlign().transpose();
- }
- }
- LOG(INFO) << counter << " images are loaded with gravity" << std::endl;
-}
-
-} // namespace glomap
\ No newline at end of file
diff --git a/glomap/io/gravity_io.h b/glomap/io/gravity_io.h
deleted file mode 100644
index 4dabda23..00000000
--- a/glomap/io/gravity_io.h
+++ /dev/null
@@ -1,14 +0,0 @@
-#pragma once
-
-#include "glomap/scene/image.h"
-
-#include
-
-namespace glomap {
-// Require the gravity in the format: image_name, gravity (3 numbers)
-// Gravity should be the direction of [0,1,0] in the image frame
-// image.cam_from_world * [0,1,0]^T = g
-void ReadGravity(const std::string& gravity_path,
- std::unordered_map& images);
-
-} // namespace glomap
\ No newline at end of file
diff --git a/glomap/io/pose_io.cc b/glomap/io/pose_io.cc
new file mode 100644
index 00000000..eeda2e6b
--- /dev/null
+++ b/glomap/io/pose_io.cc
@@ -0,0 +1,212 @@
+#include "pose_io.h"
+
+#include
+#include