Add the rotation averager to GLOMAP (#113)

* Fix conversion of colmap pose prior

* d

* restore the pose prior after rotation averaging

* f

* f

* f

* gravity refinement tested

* add the stratified rotation averager and CLI

* f

* d

* d

* add timer

* add options for gravity refinement

* merge gravity_io to pose_io

* add readme for the rotation averager

* f

* Update glomap/math/gravity.cc

Co-authored-by: Johannes Schönberger <joschonb@microsoft.com>

* f

* Update glomap/controllers/rotation_averager.cc

Co-authored-by: Johannes Schönberger <joschonb@microsoft.com>

* Update glomap/controllers/rotation_averager.cc

Co-authored-by: Johannes Schönberger <joschonb@microsoft.com>

* change rotation averager from class to struct

* d

* d

* add weighting and corresponding IO. Tested

* change the writing of relative pose to be sorted

* unit test for rotation averager

* inject noise to avoid local minima for error-free case

* f

* add more points in the unit test

---------

Co-authored-by: Johannes Schönberger <joschonb@microsoft.com>

Author: lpanaf

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
@@ -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

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", "");
@@ -266,6 +268,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);
@@ -294,6 +308,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