Refined rotation averaging

glomap/CMakeLists.txt

@@ -6,12 +6,15 @@ set(SOURCES
     estimators/bundle_adjustment.cc
     estimators/global_positioning.cc
     estimators/global_rotation_averaging.cc
+    estimators/gravity_refinement.cc
     estimators/relpose_estimation.cc
     estimators/view_graph_calibration.cc
     io/colmap_converter.cc
     io/colmap_io.cc
+    io/gravity_io.cc
     math/gravity.cc
     math/rigid3d.cc
+    math/tree.cc
     math/two_view_geometry.cc
     processors/image_pair_inliers.cc
     processors/image_undistorter.cc
@@ -31,14 +34,17 @@ set(HEADERS
     estimators/cost_function.h
     estimators/global_positioning.h
     estimators/global_rotation_averaging.h
+    estimators/gravity_refinement.h
     estimators/relpose_estimation.h
     estimators/optimization_base.h
     estimators/view_graph_calibration.h
     io/colmap_converter.h
     io/colmap_io.h
+    io/gravity_io.h
     math/gravity.h
     math/l1_solver.h
     math/rigid3d.h
+    math/tree.h
     math/two_view_geometry.h
     math/union_find.h
     processors/image_pair_inliers.h
@@ -50,7 +56,6 @@ set(HEADERS
     scene/camera.h
     scene/image_pair.h
     scene/image.h
-    scene/rigid3d.h
     scene/track.h
     scene/types_sfm.h
     scene/types.h

glomap/controllers/global_mapper.cc

@@ -78,22 +78,19 @@ bool GlobalMapper::Solve(const colmap::Database& database,
     run_timer.Start();
 
     RotationEstimator ra_engine(options_.opt_ra);
-    // The first run is for initialization
-    ra_engine.EstimateRotations(view_graph, images);
-
-    // The second run is for filtering
+    // The first run is for filtering
     ra_engine.EstimateRotations(view_graph, images);
 
     RelPoseFilter::FilterRotations(
-        view_graph, images, options_.inlier_thresholds.max_roation_error);
+        view_graph, images, options_.inlier_thresholds.max_rotation_error);
     view_graph.KeepLargestConnectedComponents(images);
 
-    // The third run is for final estimation
+    // The second run is for final estimation
     if (!ra_engine.EstimateRotations(view_graph, images)) {
       return false;
     }
     RelPoseFilter::FilterRotations(
-        view_graph, images, options_.inlier_thresholds.max_roation_error);
+        view_graph, images, options_.inlier_thresholds.max_rotation_error);
     image_t num_img = view_graph.KeepLargestConnectedComponents(images);
     LOG(INFO) << num_img << " / " << images.size()
               << " images are within the connected component." << std::endl;
@@ -167,6 +164,7 @@ bool GlobalMapper::Solve(const colmap::Database& database,
     std::cout << "-------------------------------------" << std::endl;
     std::cout << "Running bundle adjustment ..." << std::endl;
     std::cout << "-------------------------------------" << std::endl;
+    LOG(INFO) << "Bundle adjustment start" << std::endl;
 
     colmap::Timer run_timer;
     run_timer.Start();
@@ -201,6 +199,9 @@ bool GlobalMapper::Solve(const colmap::Database& database,
         run_timer.PrintSeconds();
 
       // 6.3. Filter tracks based on the estatimation
+      // For the filtering, in each round, the criteria for outlier is
+      // tightened. If only few tracks are changed, no need to start bundle
+      // adjustment right away. Instead, use a more strict criteria to filter
       UndistortImages(cameras, images, true);
       LOG(INFO) << "Filtering tracks by reprojection ...";
 
@@ -225,6 +226,22 @@ bool GlobalMapper::Solve(const colmap::Database& database,
         break;
       }
     }
+
+    // Filter tracks based on the estatimation
+    UndistortImages(cameras, images, true);
+    LOG(INFO) << "Filtering tracks by reprojection ...";
+    TrackFilter::FilterTracksByReprojection(
+        view_graph,
+        cameras,
+        images,
+        tracks,
+        options_.inlier_thresholds.max_reprojection_error);
+    TrackFilter::FilterTrackTriangulationAngle(
+        view_graph,
+        images,
+        tracks,
+        options_.inlier_thresholds.min_triangulation_angle);
+
     run_timer.PrintSeconds();
   }
 
@@ -243,6 +260,7 @@ bool GlobalMapper::Solve(const colmap::Database& database,
       std::cout << "-------------------------------------" << std::endl;
       std::cout << "Running bundle adjustment ..." << std::endl;
       std::cout << "-------------------------------------" << std::endl;
+      LOG(INFO) << "Bundle adjustment start" << std::endl;
       BundleAdjuster ba_engine(options_.opt_ba);
       if (!ba_engine.Solve(view_graph, cameras, images, tracks)) {
         return false;
@@ -262,16 +280,6 @@ bool GlobalMapper::Solve(const colmap::Database& database,
       }
       run_timer.PrintSeconds();
     }
-  }
-
-  // 8. Postprocessing
-  if (!options_.skip_postprocessing) {
-    std::cout << "-------------------------------------" << std::endl;
-    std::cout << "Running postprocessing ..." << std::endl;
-    std::cout << "-------------------------------------" << std::endl;
-
-    colmap::Timer run_timer;
-    run_timer.Start();
 
     // Filter tracks based on the estatimation
     UndistortImages(cameras, images, true);
@@ -287,6 +295,16 @@ bool GlobalMapper::Solve(const colmap::Database& database,
         images,
         tracks,
         options_.inlier_thresholds.min_triangulation_angle);
+  }
+
+  // 8. Reconstruction pruning
+  if (!options_.skip_pruning) {
+    std::cout << "-------------------------------------" << std::endl;
+    std::cout << "Running postprocessing ..." << std::endl;
+    std::cout << "-------------------------------------" << std::endl;
+
+    colmap::Timer run_timer;
+    run_timer.Start();
 
     // Prune weakly connected images
     PruneWeaklyConnectedImages(images, tracks);

glomap/controllers/global_mapper.h

@@ -39,7 +39,7 @@ struct GlobalMapperOptions {
   bool skip_global_positioning = false;
   bool skip_bundle_adjustment = false;
   bool skip_retriangulation = false;
-  bool skip_postprocessing = true;
+  bool skip_pruning = true;
 };
 
 class GlobalMapper {

glomap/controllers/option_manager.cc

@@ -23,14 +23,14 @@ void OptionManager::AddAllOptions() {
   AddDatabaseOptions();
   AddImageOptions();
   AddGlobalMapperOptions();
+  AddInlierThresholdOptions();
   AddViewGraphCalibrationOptions();
   AddRelativePoseEstimationOptions();
   AddRotationEstimatorOptions();
   AddTrackEstablishmentOptions();
   AddGlobalPositionerOptions();
   AddBundleAdjusterOptions();
   AddTriangulatorOptions();
-  AddInlierThresholdOptions();
 }
 
 void OptionManager::AddDatabaseOptions() {
@@ -75,8 +75,7 @@ void OptionManager::AddGlobalMapperOptions() {
                               &mapper->skip_bundle_adjustment);
   AddAndRegisterDefaultOption("skip_retriangulation",
                               &mapper->skip_retriangulation);
-  AddAndRegisterDefaultOption("skip_postprocessing",
-                              &mapper->skip_postprocessing);
+  AddAndRegisterDefaultOption("skip_pruning", &mapper->skip_pruning);
 }
 
 void OptionManager::AddGlobalMapperFullOptions() {
@@ -115,8 +114,7 @@ void OptionManager::AddGlobalMapperResumeOptions() {
                               &mapper->skip_global_positioning);
   AddAndRegisterDefaultOption("skip_bundle_adjustment",
                               &mapper->skip_bundle_adjustment);
-  AddAndRegisterDefaultOption("skip_postprocessing",
-                              &mapper->skip_postprocessing);
+  AddAndRegisterDefaultOption("skip_pruning", &mapper->skip_pruning);
 }
 
 void OptionManager::AddGlobalMapperResumeFullOptions() {
@@ -137,7 +135,7 @@ void OptionManager::AddViewGraphCalibrationOptions() {
                               &mapper->opt_vgcalib.thres_lower_ratio);
   AddAndRegisterDefaultOption("ViewGraphCalib.thres_higher_ratio",
                               &mapper->opt_vgcalib.thres_higher_ratio);
-  AddAndRegisterDefaultOption("ViewGraphCalib.robust_loss_thres",
+  AddAndRegisterDefaultOption("ViewGraphCalib.thres_two_view_error",
                               &mapper->opt_vgcalib.thres_two_view_error);
 }
 
@@ -146,7 +144,11 @@ void OptionManager::AddRelativePoseEstimationOptions() {
     return;
   }
   added_relative_pose_options_ = true;
+  AddAndRegisterDefaultOption(
+      "RelPoseEstimation.max_epipolar_error",
+      &mapper->opt_relpose.ransac_options.max_epipolar_error);
 }
+
 void OptionManager::AddRotationEstimatorOptions() {
   if (added_rotation_averaging_options_) {
     return;
@@ -221,7 +223,18 @@ void OptionManager::AddInlierThresholdOptions() {
     return;
   }
   added_inliers_options_ = true;
-  // TODO: maybe add options for inlier threshold
+  AddAndRegisterDefaultOption("Thresholds.max_epipolar_error_E",
+                              &mapper->inlier_thresholds.max_epipolar_error_E);
+  AddAndRegisterDefaultOption("Thresholds.max_epipolar_error_F",
+                              &mapper->inlier_thresholds.max_epipolar_error_F);
+  AddAndRegisterDefaultOption("Thresholds.max_epipolar_error_H",
+                              &mapper->inlier_thresholds.max_epipolar_error_H);
+  AddAndRegisterDefaultOption("Thresholds.min_inlier_num",
+                              &mapper->inlier_thresholds.min_inlier_num);
+  AddAndRegisterDefaultOption("Thresholds.min_inlier_ratio",
+                              &mapper->inlier_thresholds.min_inlier_ratio);
+  AddAndRegisterDefaultOption("Thresholds.max_rotation_error",
+                              &mapper->inlier_thresholds.max_rotation_error);
 }
 
 void OptionManager::Reset() {

glomap/controllers/track_establishment.cc

@@ -21,7 +21,7 @@ void TrackEngine::BlindConcatenation() {
   // correspondences
   image_pair_t counter = 0;
   for (auto pair : view_graph_.image_pairs) {
-    if ((++counter + 1) % 1000 == 0 ||
+    if ((counter + 1) % 1000 == 0 ||
         counter == view_graph_.image_pairs.size() - 1) {
       std::cout << "\r Initializing pairs " << counter + 1 << " / "
                 << view_graph_.image_pairs.size() << std::flush;

glomap/estimators/cost_function.h

@@ -215,4 +215,32 @@ class FetzerFocalLengthSameCameraCost {
   Eigen::Vector4d d_12;
 };
 
+// ----------------------------------------
+// GravError
+// ----------------------------------------
+struct GravError {
+  GravError(const Eigen::Vector3d& grav_obs) : grav_obs_(grav_obs) {}
+
+  template <typename T>
+  bool operator()(const T* const gvec, T* residuals) const {
+    Eigen::Matrix<T, 3, 1> grav_est;
+    grav_est << gvec[0], gvec[1], gvec[2];
+
+    for (int i = 0; i < 3; i++) {
+      residuals[i] = grav_est[i] - grav_obs_[i];
+    }
+
+    return true;
+  }
+
+  // Factory function
+  static ceres::CostFunction* CreateCost(const Eigen::Vector3d& grav_obs) {
+    return (new ceres::AutoDiffCostFunction<GravError, 3, 3>(
+        new GravError(grav_obs)));
+  }
+
+ private:
+  const Eigen::Vector3d& grav_obs_;
+};
+
 }  // namespace glomap

glomap/estimators/global_positioning.cc

@@ -148,8 +148,9 @@ void GlobalPositioner::AddCameraToCameraConstraints(
     track_t counter = scales_.size();
     scales_.insert(std::make_pair(counter, 1));
 
-    Eigen::Vector3d translation = -images[image_id2].cam_from_world.Derotate(
-        image_pair.cam2_from_cam1.translation);
+    Eigen::Vector3d translation =
+        -(images[image_id2].cam_from_world.rotation.inverse() *
+          image_pair.cam2_from_cam1.translation);
     ceres::CostFunction* cost_function =
         BATAPairwiseDirectionError::Create(translation);
     problem_->AddResidualBlock(
@@ -240,8 +241,8 @@ void GlobalPositioner::AddTrackToProblem(
     Image& image = images[observation.first];
     if (!image.is_registered) continue;
 
-    Eigen::Vector3d translation = image.cam_from_world.Derotate(
-        image.features_undist[observation.second]);
+    Eigen::Vector3d translation = image.cam_from_world.rotation.inverse() *
+                                  image.features_undist[observation.second];
     ceres::CostFunction* cost_function =
         BATAPairwiseDirectionError::Create(translation);
 
@@ -353,7 +354,7 @@ void GlobalPositioner::ConvertResults(
   // translation
   for (auto& [image_id, image] : images) {
     image.cam_from_world.translation =
-        -image.cam_from_world.Rotate(image.cam_from_world.translation);
+        -(image.cam_from_world.rotation * image.cam_from_world.translation);
   }
 }