Add the rotation averager to GLOMAP

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
 

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 <sub>2</sub>R<sub>1</sub> x<sub>1</sub> + <sub>2</sub>t<sub>1</sub> = x<sub>2</sub>.
+
+### 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.

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
@@ -28,6 +29,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
@@ -40,7 +42,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
@@ -99,7 +101,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)

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 <boost/filesystem/operations.hpp>
 #include <boost/property_tree/ini_parser.hpp>
@@ -14,6 +15,7 @@ OptionManager::OptionManager(bool add_project_options) {
   image_path = std::make_shared<std::string>();
 
   mapper = std::make_shared<GlobalMapperOptions>();
+  gravity_refiner = std::make_shared<GravityRefinerOptions>();
   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) {

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 <typename T>
   void AddRequiredOption(const std::string& name,
@@ -56,6 +58,7 @@ class OptionManager {
   std::shared_ptr<std::string> image_path;
 
   std::shared_ptr<GlobalMapperOptions> mapper;
+  std::shared_ptr<GravityRefinerOptions> gravity_refiner;
 
  private:
   template <typename T>
@@ -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 <typename T>

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<image_t, Image>& 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

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<image_t, Image>& images,
+                            const RotationAveragerOptions& options);
+
+}  // namespace glomap

glomap/estimators/global_rotation_averaging.cc

@@ -56,6 +56,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 +210,8 @@ void RotationEstimator::SetupLinearSystem(
 
   // Establish linear systems
   size_t curr_pos = 0;
+  std::vector<double> 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 +226,10 @@ void RotationEstimator::SetupLinearSystem(
     if (rel_temp_info_[pair_id].has_gravity) {
       coeffs.emplace_back(Eigen::Triplet<double>(curr_pos, vector_idx1, -1));
       coeffs.emplace_back(Eigen::Triplet<double>(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 +254,12 @@ void RotationEstimator::SetupLinearSystem(
       } else
         coeffs.emplace_back(
             Eigen::Triplet<double>(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,18 +272,28 @@ void RotationEstimator::SetupLinearSystem(
       images[fixed_camera_id_].gravity_info.has_gravity) {
     coeffs.emplace_back(Eigen::Triplet<double>(
         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<double>(
           curr_pos + i, image_id_to_idx_[fixed_camera_id_] + i, 1));
+      weights.emplace_back(1);
     }
     curr_pos += 3;
   }
 
   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 +304,8 @@ bool RotationEstimator::SolveL1Regression(
   L1SolverOptions opt_l1_solver;
   opt_l1_solver.max_num_iterations = 10;
 
-  L1Solver<Eigen::SparseMatrix<double>> l1_solver(opt_l1_solver,
-                                                  sparse_matrix_);
+  L1Solver<Eigen::SparseMatrix<double>> l1_solver(
+      opt_l1_solver, weights_.matrix().asDiagonal() * sparse_matrix_);
   double last_norm = 0;
   double curr_norm = 0;
 
@@ -295,7 +320,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 +419,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_);
 

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