diff --git a/glomap/CMakeLists.txt b/glomap/CMakeLists.txt
index c167135..c71b966 100644
--- a/glomap/CMakeLists.txt
+++ b/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
diff --git a/glomap/controllers/global_mapper.cc b/glomap/controllers/global_mapper.cc
index 7b54aa6..844363b 100644
--- a/glomap/controllers/global_mapper.cc
+++ b/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);
diff --git a/glomap/controllers/global_mapper.h b/glomap/controllers/global_mapper.h
index 8cc648b..79c1779 100644
--- a/glomap/controllers/global_mapper.h
+++ b/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 {
diff --git a/glomap/controllers/option_manager.cc b/glomap/controllers/option_manager.cc
index 0dc2cf7..823ec03 100644
--- a/glomap/controllers/option_manager.cc
+++ b/glomap/controllers/option_manager.cc
@@ -23,6 +23,7 @@ void OptionManager::AddAllOptions() {
   AddDatabaseOptions();
   AddImageOptions();
   AddGlobalMapperOptions();
+  AddInlierThresholdOptions();
   AddViewGraphCalibrationOptions();
   AddRelativePoseEstimationOptions();
   AddRotationEstimatorOptions();
@@ -30,7 +31,6 @@ void OptionManager::AddAllOptions() {
   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() {
diff --git a/glomap/controllers/track_establishment.cc b/glomap/controllers/track_establishment.cc
index 30ade78..7d12721 100644
--- a/glomap/controllers/track_establishment.cc
+++ b/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;
diff --git a/glomap/estimators/cost_function.h b/glomap/estimators/cost_function.h
index 13f1bd1..39296d0 100644
--- a/glomap/estimators/cost_function.h
+++ b/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
diff --git a/glomap/estimators/global_positioning.cc b/glomap/estimators/global_positioning.cc
index 8b80f11..e693674 100644
--- a/glomap/estimators/global_positioning.cc
+++ b/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);
   }
 }
 
diff --git a/glomap/estimators/global_rotation_averaging.cc b/glomap/estimators/global_rotation_averaging.cc
index 507b39e..b3edaad 100644
--- a/glomap/estimators/global_rotation_averaging.cc
+++ b/glomap/estimators/global_rotation_averaging.cc
@@ -2,6 +2,10 @@
 
 #include "glomap/math/l1_solver.h"
 #include "glomap/math/rigid3d.h"
+#include "glomap/math/tree.h"
+
+#include <iostream>
+#include <queue>
 
 namespace glomap {
 namespace {
@@ -18,6 +22,11 @@ double RelAngleError(double angle_12, double angle_1, double angle_2) {
 
 bool RotationEstimator::EstimateRotations(
     const ViewGraph& view_graph, std::unordered_map<image_t, Image>& images) {
+  // Initialize the rotation from maximum spanning tree
+  if (!options_.skip_initialization && !options_.use_gravity) {
+    InitializeFromMaximumSpanningTree(view_graph, images);
+  }
+
   // Set up the linear system
   SetupLinearSystem(view_graph, images);
 
@@ -41,6 +50,7 @@ bool RotationEstimator::EstimateRotations(
 
     if (options_.use_gravity && image.gravity_info.has_gravity) {
       image.cam_from_world.rotation = Eigen::Quaterniond(
+          image.gravity_info.GetRAlign() *
           AngleToRotUp(rotation_estimated_[image_id_to_idx_[image_id]]));
     } else {
       image.cam_from_world.rotation = Eigen::Quaterniond(AngleAxisToRotation(
@@ -51,6 +61,55 @@ bool RotationEstimator::EstimateRotations(
   return true;
 }
 
+void RotationEstimator::InitializeFromMaximumSpanningTree(
+    const ViewGraph& view_graph, std::unordered_map<image_t, Image>& images) {
+  // Here, we assume that largest connected component is already retrieved, so
+  // we do not need to do that again compute maximum spanning tree.
+  std::unordered_map<image_t, image_t> parents;
+  image_t root = MaximumSpanningTree(view_graph, images, parents, INLIER_NUM);
+
+  // Iterate through the tree to initialize the rotation
+  // Establish child info
+  std::unordered_map<image_t, std::vector<image_t>> children;
+  for (const auto& [image_id, image] : images) {
+    if (!image.is_registered) continue;
+    children.insert(std::make_pair(image_id, std::vector<image_t>()));
+  }
+  for (auto& [child, parent] : parents) {
+    if (root == child) continue;
+    children[parent].emplace_back(child);
+  }
+
+  std::queue<image_t> indexes;
+  indexes.push(root);
+
+  while (!indexes.empty()) {
+    image_t curr = indexes.front();
+    indexes.pop();
+
+    // Add all children into the tree
+    for (auto& child : children[curr]) indexes.push(child);
+    // If it is root, then fix it to be the original estimation
+    if (curr == root) continue;
+
+    // Directly use the relative pose for estimation rotation
+    const ImagePair& image_pair = view_graph.image_pairs.at(
+        ImagePair::ImagePairToPairId(curr, parents[curr]));
+    if (image_pair.image_id1 == curr) {
+      // 1_R_w = 2_R_1^T * 2_R_w
+      images[curr].cam_from_world.rotation =
+          (Inverse(image_pair.cam2_from_cam1) *
+           images[parents[curr]].cam_from_world)
+              .rotation;
+    } else {
+      // 2_R_w = 2_R_1 * 1_R_w
+      images[curr].cam_from_world.rotation =
+          (image_pair.cam2_from_cam1 * images[parents[curr]].cam_from_world)
+              .rotation;
+    }
+  }
+}
+
 void RotationEstimator::SetupLinearSystem(
     const ViewGraph& view_graph, std::unordered_map<image_t, Image>& images) {
   // Clear all the structures
@@ -71,16 +130,13 @@ void RotationEstimator::SetupLinearSystem(
     image_id_to_idx_[image_id] = num_dof;
     if (options_.use_gravity && image.gravity_info.has_gravity) {
       rotation_estimated_[num_dof] =
-          RotUpToAngle(image.gravity_info.R_align.transpose() *
+          RotUpToAngle(image.gravity_info.GetRAlign().transpose() *
                        image.cam_from_world.rotation.toRotationMatrix());
       num_dof++;
 
       if (fixed_camera_id_ == -1) {
-        fixed_camera_rotation_ = Eigen::Vector3d(
-            0,
-            RotUpToAngle(image.gravity_info.R_align.transpose() *
-                         image.cam_from_world.rotation.toRotationMatrix()),
-            0);
+        fixed_camera_rotation_ =
+            Eigen::Vector3d(0, rotation_estimated_[num_dof - 1], 0);
         fixed_camera_id_ = image_id;
       }
     } else {
@@ -107,11 +163,12 @@ void RotationEstimator::SetupLinearSystem(
   rotation_estimated_.conservativeResize(num_dof);
 
   // Prepare the relative information
+  int counter = 0;
   for (auto& [pair_id, image_pair] : view_graph.image_pairs) {
     if (!image_pair.is_valid) continue;
 
     int image_id1 = image_pair.image_id1;
-    int image_id2 = image_pair.image_id1;
+    int image_id2 = image_pair.image_id2;
 
     rel_temp_info_[pair_id].R_rel =
         image_pair.cam2_from_cam1.rotation.toRotationMatrix();
@@ -119,19 +176,21 @@ void RotationEstimator::SetupLinearSystem(
     // Align the relative rotation to the gravity
     if (options_.use_gravity) {
       if (images[image_id1].gravity_info.has_gravity) {
-        rel_temp_info_[pair_id].R_rel = rel_temp_info_[pair_id].R_rel *
-                                        images[image_id1].gravity_info.R_align;
+        rel_temp_info_[pair_id].R_rel =
+            rel_temp_info_[pair_id].R_rel *
+            images[image_id1].gravity_info.GetRAlign();
       }
 
       if (images[image_id2].gravity_info.has_gravity) {
         rel_temp_info_[pair_id].R_rel =
-            images[image_id2].gravity_info.R_align.transpose() *
+            images[image_id2].gravity_info.GetRAlign().transpose() *
             rel_temp_info_[pair_id].R_rel;
       }
     }
 
     if (options_.use_gravity && images[image_id1].gravity_info.has_gravity &&
         images[image_id2].gravity_info.has_gravity) {
+      counter++;
       Eigen::Vector3d aa = RotationToAngleAxis(rel_temp_info_[pair_id].R_rel);
       double error = aa[0] * aa[0] + aa[2] * aa[2];
 
@@ -145,6 +204,9 @@ void RotationEstimator::SetupLinearSystem(
     }
   }
 
+  if (options_.verbose)
+    LOG(INFO) << counter << " image pairs are gravity aligned" << std::endl;
+
   std::vector<Eigen::Triplet<double>> coeffs;
   coeffs.reserve(rel_temp_info_.size() * 6 + 3);
 
@@ -278,7 +340,6 @@ bool RotationEstimator::SolveIRLS(const ViewGraph& view_graph,
                                   std::unordered_map<image_t, Image>& images) {
   // TODO: Determine what is the best solver for this part
   Eigen::CholmodSupernodalLLT<Eigen::SparseMatrix<double>> llt;
-  // Eigen::SimplicialLLT<Eigen::SparseMatrix<double>> llt;
 
   // weight_matrix.setIdentity();
   // sparse_matrix_ = A_ori;
@@ -322,7 +383,7 @@ bool RotationEstimator::SolveIRLS(const ViewGraph& view_graph,
         double tmp = err_squared + sigma * sigma;
         w = sigma * sigma / (tmp * tmp);
       } else if (options_.weight_type == RotationEstimatorOptions::HALF_NORM) {
-        LOG(ERROR) << "NOT IMPLEMENTED!!!";
+        w = std::pow(err_squared, (0.5 - 2) / 2);
       }
 
       if (std::isnan(w)) {
diff --git a/glomap/estimators/global_rotation_averaging.h b/glomap/estimators/global_rotation_averaging.h
index b257cca..7b73d34 100644
--- a/glomap/estimators/global_rotation_averaging.h
+++ b/glomap/estimators/global_rotation_averaging.h
@@ -8,8 +8,8 @@
 #include <vector>
 
 // Code is adapted from Theia's RobustRotationEstimator
-// (http://www.theia-sfm.org/) For gravity aligned rotation averaging, refere to
-// the paper "Gravity Aligned Rotation Averaging"
+// (http://www.theia-sfm.org/). For gravity aligned rotation averaging, refere
+// to the paper "Gravity Aligned Rotation Averaging"
 namespace glomap {
 
 // The struct to store the temporary information for each image pair
@@ -51,12 +51,18 @@ struct RotationEstimatorOptions {
   // L2.
   double irls_loss_parameter_sigma = 5.0;  // in degree
 
-  // TODO: implement the HALF_NORM roation averaging
   enum WeightType {
+    // For Geman-McClure weight, refer to the paper "Efficient and robust
+    // large-scale rotation averaging" (Chatterjee et. al, 2013)
     GEMAN_MCCLURE,
+    // For Half Norm, refer to the paper "Robust Relative Rotation Averaging"
+    // (Chatterjee et. al, 2017)
     HALF_NORM,
   } weight_type = GEMAN_MCCLURE;
 
+  // Flg to use maximum spanning tree for initialization
+  bool skip_initialization = false;
+
   // TODO: Implement the weighted version for rotation averaging
   bool use_weight = false;
 
@@ -82,6 +88,11 @@ class RotationEstimator {
                          std::unordered_map<image_t, Image>& images);
 
  protected:
+  // Initialize the rotation from the maximum spanning tree
+  // Number of inliers serve as weights
+  void InitializeFromMaximumSpanningTree(
+      const ViewGraph& view_graph, std::unordered_map<image_t, Image>& images);
+
   // Sets up the sparse linear system such that dR_ij = dR_j - dR_i. This is the
   // first-order approximation of the angle-axis rotations. This should only be
   // called once.
diff --git a/glomap/estimators/gravity_refinement.cc b/glomap/estimators/gravity_refinement.cc
new file mode 100644
index 0000000..704d136
--- /dev/null
+++ b/glomap/estimators/gravity_refinement.cc
@@ -0,0 +1,155 @@
+#include "gravity_refinement.h"
+
+#include "glomap/estimators/cost_function.h"
+#include "glomap/math/gravity.h"
+
+#include <colmap/estimators/cost_functions.h>
+
+namespace glomap {
+void GravityRefiner::RefineGravity(const ViewGraph& view_graph,
+                                   std::unordered_map<image_t, Image>& images) {
+  const std::unordered_map<image_pair_t, ImagePair>& image_pairs =
+      view_graph.image_pairs;
+  const std::unordered_map<image_t, std::unordered_set<image_t>>&
+      adjacency_list = view_graph.GetAdjacencyList();
+
+  // Identify the images that are error prone
+  int counter_rect = 0;
+  std::unordered_set<image_t> error_prone_images;
+  IdentifyErrorProneGravity(view_graph, images, error_prone_images);
+
+  if (error_prone_images.empty()) {
+    LOG(INFO) << "No error prone images found" << std::endl;
+    return;
+  }
+
+  int counter_progress = 0;
+  // Iterate through the error prone images
+  for (auto image_id : error_prone_images) {
+    if ((counter_progress + 1) % 10 == 0 ||
+        counter_progress == error_prone_images.size() - 1) {
+      std::cout << "\r Refining image " << counter_progress + 1 << " / "
+                << error_prone_images.size() << std::flush;
+    }
+    counter_progress++;
+    const std::unordered_set<image_t>& neighbors = adjacency_list.at(image_id);
+    std::vector<Eigen::Vector3d> gravities;
+    gravities.reserve(neighbors.size());
+
+    ceres::Problem::Options problem_options;
+    problem_options.loss_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
+    ceres::Problem problem(problem_options);
+    int counter = 0;
+    Eigen::Vector3d gravity = images[image_id].gravity_info.GetGravity();
+    for (const auto& neighbor : neighbors) {
+      image_pair_t pair_id = ImagePair::ImagePairToPairId(image_id, neighbor);
+
+      image_t image_id1 = image_pairs.at(pair_id).image_id1;
+      image_t image_id2 = image_pairs.at(pair_id).image_id2;
+      if (images.at(image_id1).gravity_info.has_gravity == false ||
+          images.at(image_id2).gravity_info.has_gravity == false)
+        continue;
+
+      if (image_id1 == image_id) {
+        gravities.emplace_back((image_pairs.at(pair_id)
+                                    .cam2_from_cam1.rotation.toRotationMatrix()
+                                    .transpose() *
+                                images[image_id2].gravity_info.GetRAlign())
+                                   .col(1));
+      } else {
+        gravities.emplace_back(
+            (image_pairs.at(pair_id)
+                 .cam2_from_cam1.rotation.toRotationMatrix() *
+             images[image_id1].gravity_info.GetRAlign())
+                .col(1));
+      }
+
+      ceres::CostFunction* coor_cost =
+          GravError::CreateCost(gravities[counter]);
+      problem.AddResidualBlock(
+          coor_cost, options_.loss_function.get(), gravity.data());
+      counter++;
+    }
+
+    if (gravities.size() < options_.min_num_neighbors) continue;
+
+    // Then, run refinment
+    colmap::SetSphereManifold<3>(&problem, gravity.data());
+    ceres::Solver::Summary summary_solver;
+    ceres::Solve(options_.solver_options, &problem, &summary_solver);
+
+    // Check the error with respect to the neighbors
+    int counter_outlier = 0;
+    for (int i = 0; i < gravities.size(); i++) {
+      double error = RadToDeg(
+          std::acos(std::max(std::min(gravities[i].dot(gravity), 1.), -1.)));
+      if (error > options_.max_gravity_error * 2) counter_outlier++;
+    }
+    // If the refined gravity now consistent with more images, then accept it
+    if (double(counter_outlier) / double(gravities.size()) <
+        options_.max_outlier_ratio) {
+      counter_rect++;
+      images[image_id].gravity_info.SetGravity(gravity);
+    }
+  }
+  std::cout << std::endl;
+  LOG(INFO) << "Number of rectified images: " << counter_rect << " / "
+            << error_prone_images.size() << std::endl;
+}
+
+void GravityRefiner::IdentifyErrorProneGravity(
+    const ViewGraph& view_graph,
+    const std::unordered_map<image_t, Image>& images,
+    std::unordered_set<image_t>& error_prone_images) {
+  error_prone_images.clear();
+
+  // image_id: (mistake, total)
+  std::unordered_map<image_t, std::pair<int, int>> image_counter;
+  // Set the counter of all images to 0
+  for (const auto& [image_id, image] : images) {
+    image_counter[image_id] = std::make_pair(0, 0);
+  }
+
+  for (const auto& [pair_id, image_pair] : view_graph.image_pairs) {
+    if (!image_pair.is_valid) continue;
+    const auto& image1 = images.at(image_pair.image_id1);
+    const auto& image2 = images.at(image_pair.image_id2);
+    if (image1.gravity_info.has_gravity && image2.gravity_info.has_gravity) {
+      // Calculate the gravity aligned relative rotation
+      Eigen::Matrix3d R_rel =
+          image2.gravity_info.GetRAlign().transpose() *
+          image_pair.cam2_from_cam1.rotation.toRotationMatrix() *
+          image1.gravity_info.GetRAlign();
+      // Convert it to the closest upright rotation
+      Eigen::Matrix3d R_rel_up = AngleToRotUp(RotUpToAngle(R_rel));
+
+      double angle = CalcAngle(R_rel, R_rel_up);
+
+      // increment the total count
+      image_counter[image_pair.image_id1].second++;
+      image_counter[image_pair.image_id2].second++;
+
+      // increment the mistake count
+      if (angle > options_.max_gravity_error) {
+        image_counter[image_pair.image_id1].first++;
+        image_counter[image_pair.image_id2].first++;
+      }
+    }
+  }
+
+  const std::unordered_map<image_t, std::unordered_set<image_t>>&
+      adjacency_list = view_graph.GetAdjacencyList();
+
+  // Filter the images with too many mistakes
+  for (auto& [image_id, counter] : image_counter) {
+    if (images.at(image_id).gravity_info.has_gravity == false) continue;
+    if (counter.second < options_.min_num_neighbors) continue;
+    if (double(counter.first) / double(counter.second) >=
+        options_.max_outlier_ratio) {
+      error_prone_images.insert(image_id);
+    }
+  }
+  LOG(INFO) << "Number of error prone images: " << error_prone_images.size()
+            << std::endl;
+}
+}  // namespace glomap
diff --git a/glomap/estimators/gravity_refinement.h b/glomap/estimators/gravity_refinement.h
new file mode 100644
index 0000000..40a3b37
--- /dev/null
+++ b/glomap/estimators/gravity_refinement.h
@@ -0,0 +1,40 @@
+#pragma once
+
+#include "glomap/estimators/optimization_base.h"
+#include "glomap/math/rigid3d.h"
+#include "glomap/scene/types_sfm.h"
+#include "glomap/types.h"
+
+#include <ceres/ceres.h>
+
+namespace glomap {
+
+struct GravityRefinerOptions : public OptimizationBaseOptions {
+  // The minimal ratio that the gravity vector should be consistent with
+  double max_outlier_ratio = 0.5;
+  // The maximum allowed angle error in degree
+  bool max_gravity_error = 1.;
+  // Only refine the gravity of the images with more than min_neighbors
+  int min_num_neighbors = 7;
+
+  GravityRefinerOptions() : OptimizationBaseOptions() {
+    loss_function = std::make_shared<ceres::ArctanLoss>(
+        1 - std::cos(DegToRad(max_gravity_error)));
+  }
+};
+
+class GravityRefiner {
+ public:
+  GravityRefiner(const GravityRefinerOptions& options) : options_(options) {}
+  void RefineGravity(const ViewGraph& view_graph,
+                     std::unordered_map<image_t, Image>& images);
+
+ private:
+  void IdentifyErrorProneGravity(
+      const ViewGraph& view_graph,
+      const std::unordered_map<image_t, Image>& images,
+      std::unordered_set<image_t>& error_prone_images);
+  GravityRefinerOptions options_;
+};
+
+}  // namespace glomap
diff --git a/glomap/estimators/view_graph_calibration.cc b/glomap/estimators/view_graph_calibration.cc
index 343ba4b..29ebde9 100644
--- a/glomap/estimators/view_graph_calibration.cc
+++ b/glomap/estimators/view_graph_calibration.cc
@@ -147,6 +147,9 @@ void ViewGraphCalibrator::ConvertResults(
     // Update the focal length
     for (const size_t idx : camera.FocalLengthIdxs()) {
       camera.params[idx] = focals_[camera_id];
+      if (options_.verbose)
+        LOG(INFO) << "Camera " << idx << " focal: " << focals_[camera_id]
+                  << std::endl;
     }
   }
   LOG(INFO) << counter << " cameras are rejected in view graph calibration";
diff --git a/glomap/exe/global_mapper.cc b/glomap/exe/global_mapper.cc
index 2ff8a92..b81c34b 100644
--- a/glomap/exe/global_mapper.cc
+++ b/glomap/exe/global_mapper.cc
@@ -78,7 +78,7 @@ int RunMapper(int argc, char** argv) {
   global_mapper.Solve(database, view_graph, cameras, images, tracks);
   run_timer.Pause();
 
-  LOG(INFO) << "Recontruction done in " << run_timer.ElapsedSeconds()
+  LOG(INFO) << "Reconstruction done in " << run_timer.ElapsedSeconds()
             << " seconds";
 
   WriteGlomapReconstruction(
@@ -136,7 +136,7 @@ int RunMapperResume(int argc, char** argv) {
   global_mapper.Solve(database, view_graph, cameras, images, tracks);
   run_timer.Pause();
 
-  LOG(INFO) << "Recontruction done in " << run_timer.ElapsedSeconds()
+  LOG(INFO) << "Reconstruction done in " << run_timer.ElapsedSeconds()
             << " seconds";
 
   WriteGlomapReconstruction(
diff --git a/glomap/io/colmap_converter.cc b/glomap/io/colmap_converter.cc
index a2e2977..6a838c2 100644
--- a/glomap/io/colmap_converter.cc
+++ b/glomap/io/colmap_converter.cc
@@ -106,8 +106,6 @@ void ConvertGlomapToColmap(const std::unordered_map<camera_t, Camera>& cameras,
 
     reconstruction.AddImage(std::move(image_colmap));
   }
-
-  reconstruction.WriteText(".");
 }
 
 void ConvertColmapToGlomap(const colmap::Reconstruction& reconstruction,
diff --git a/glomap/io/colmap_io.cc b/glomap/io/colmap_io.cc
index 7fc9c7a..88dc4cc 100644
--- a/glomap/io/colmap_io.cc
+++ b/glomap/io/colmap_io.cc
@@ -43,13 +43,10 @@ void WriteGlomapReconstruction(
       ConvertGlomapToColmap(cameras, images, tracks, reconstruction, comp);
       // Read in colors
       if (image_path != "") {
-        LOG(INFO) << "Extracting colors ...";
         reconstruction.ExtractColorsForAllImages(image_path);
       }
       colmap::CreateDirIfNotExists(
           reconstruction_path + "/" + std::to_string(comp), true);
-      reconstruction.WriteText(reconstruction_path + "/" +
-                               std::to_string(comp));
       if (output_format == "txt") {
         reconstruction.WriteText(reconstruction_path + "/" +
                                  std::to_string(comp));
diff --git a/glomap/io/gravity_io.cc b/glomap/io/gravity_io.cc
new file mode 100644
index 0000000..7e974db
--- /dev/null
+++ b/glomap/io/gravity_io.cc
@@ -0,0 +1,45 @@
+#include "gravity_io.h"
+
+#include <fstream>
+
+namespace glomap {
+void ReadGravity(const std::string& gravity_path,
+                 std::unordered_map<image_t, Image>& images) {
+  std::unordered_map<std::string, image_t> 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
new file mode 100644
index 0000000..4dabda2
--- /dev/null
+++ b/glomap/io/gravity_io.h
@@ -0,0 +1,14 @@
+#pragma once
+
+#include "glomap/scene/image.h"
+
+#include <unordered_map>
+
+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<image_t, Image>& images);
+
+}  // namespace glomap
\ No newline at end of file
diff --git a/glomap/math/gravity.cc b/glomap/math/gravity.cc
index c1a19cf..485a4cd 100644
--- a/glomap/math/gravity.cc
+++ b/glomap/math/gravity.cc
@@ -1,5 +1,6 @@
 #include "gravity.h"
 
+#include "glomap/math/rigid3d.h"
 #include "glomap/scene/types_sfm.h"
 
 #include <Eigen/QR>
@@ -23,14 +24,11 @@ Eigen::Matrix3d GetAlignRot(const Eigen::Vector3d& gravity) {
 }
 
 double RotUpToAngle(const Eigen::Matrix3d& R_up) {
-  Eigen::Quaternion<double> q_pitch(R_up);
-  double theta_half = std::atan2(q_pitch.y(), q_pitch.w());
-  return theta_half * 2;
+  return RotationToAngleAxis(R_up)[1];
 }
 
 Eigen::Matrix3d AngleToRotUp(double angle) {
-  Eigen::Quaternion<double> q_pitch;
-  q_pitch.coeffs() << 0, std::sin(angle / 2), 0, std::cos(angle / 2);
-  return q_pitch.toRotationMatrix();
+  Eigen::Vector3d aa(0, angle, 0);
+  return AngleAxisToRotation(aa);
 }
 }  // namespace glomap
\ No newline at end of file
diff --git a/glomap/math/rigid3d.cc b/glomap/math/rigid3d.cc
index feb70ce..98869a5 100644
--- a/glomap/math/rigid3d.cc
+++ b/glomap/math/rigid3d.cc
@@ -26,6 +26,14 @@ double CalcTransAngle(const Rigid3d& pose1, const Rigid3d& pose2) {
   return std::acos(cos_r) * 180 / M_PI;
 }
 
+double CalcAngle(const Eigen::Matrix3d& rotation1,
+                 const Eigen::Matrix3d& rotation2) {
+  double cos_r = ((rotation1.transpose() * rotation2).trace() - 1) / 2;
+  cos_r = std::min(std::max(cos_r, -1.), 1.);
+
+  return std::acos(cos_r) * 180 / M_PI;
+}
+
 double DegToRad(double degree) { return degree * M_PI / 180; }
 
 double RadToDeg(double radian) { return radian * 180 / M_PI; }
diff --git a/glomap/math/rigid3d.h b/glomap/math/rigid3d.h
index 0debed3..00f3b41 100644
--- a/glomap/math/rigid3d.h
+++ b/glomap/math/rigid3d.h
@@ -1,6 +1,6 @@
 #pragma once
 
-#include "glomap/scene/rigid3d.h"
+#include "glomap/scene/types.h"
 #include "glomap/types.h"
 
 #include <Eigen/Geometry>
@@ -16,6 +16,10 @@ double CalcTrans(const Rigid3d& pose1, const Rigid3d& pose2);
 // Calculatet the translation direction difference between two poses
 double CalcTransAngle(const Rigid3d& pose1, const Rigid3d& pose2);
 
+// Calculate the rotation angle difference between two rotations
+double CalcAngle(const Eigen::Matrix3d& rotation1,
+                 const Eigen::Matrix3d& rotation2);
+
 // Convert degree to radian
 double DegToRad(double degree);
 
diff --git a/glomap/math/tree.cc b/glomap/math/tree.cc
new file mode 100644
index 0000000..1a0ad4e
--- /dev/null
+++ b/glomap/math/tree.cc
@@ -0,0 +1,145 @@
+#include "tree.h"
+
+// BGL includes
+#include <boost/graph/adjacency_list.hpp>
+#include <boost/graph/kruskal_min_spanning_tree.hpp>
+
+namespace glomap {
+
+namespace {
+
+typedef boost::adjacency_list<boost::vecS,
+                              boost::vecS,
+                              boost::undirectedS,
+                              boost::no_property,
+                              boost::property<boost::edge_weight_t, double>>
+    weighted_graph;
+typedef boost::property_map<weighted_graph, boost::edge_weight_t>::type
+    weight_map;
+typedef boost::graph_traits<weighted_graph>::edge_descriptor edge_desc;
+typedef boost::graph_traits<weighted_graph>::vertex_descriptor vertex_desc;
+
+}  // namespace
+
+// Function to perform breadth-first search (BFS) on a graph represented by an
+// adjacency list
+int BFS(const std::vector<std::vector<int>>& graph,
+        int root,
+        std::vector<int>& parents,
+        std::vector<std::pair<int, int>> banned_edges) {
+  int num_vertices = graph.size();
+
+  // Create a vector to store the visited status of each vertex
+  std::vector<bool> visited(num_vertices, false);
+
+  // Create a vector to store the parent vertex for each vertex
+  parents.clear();
+  parents.resize(num_vertices, -1);
+  parents[root] = root;
+
+  // Create a queue for BFS traversal
+  std::queue<int> q;
+
+  // Mark the start vertex as visited and enqueue it
+  visited[root] = true;
+  q.push(root);
+
+  int counter = 0;
+  while (!q.empty()) {
+    int current_vertex = q.front();
+    q.pop();
+
+    // Process the current vertex
+    // Traverse the adjacent vertices
+    for (int neighbor : graph[current_vertex]) {
+      if (std::find(banned_edges.begin(),
+                    banned_edges.end(),
+                    std::make_pair(current_vertex, neighbor)) !=
+          banned_edges.end())
+        continue;
+      if (std::find(banned_edges.begin(),
+                    banned_edges.end(),
+                    std::make_pair(neighbor, current_vertex)) !=
+          banned_edges.end())
+        continue;
+
+      if (!visited[neighbor]) {
+        visited[neighbor] = true;
+        parents[neighbor] = current_vertex;
+        q.push(neighbor);
+        counter++;
+      }
+    }
+  }
+
+  return counter;
+}
+
+image_t MaximumSpanningTree(const ViewGraph& view_graph,
+                            const std::unordered_map<image_t, Image>& images,
+                            std::unordered_map<image_t, image_t>& parents,
+                            WeightType type) {
+  std::unordered_map<image_t, int> image_id_to_idx;
+  image_id_to_idx.reserve(images.size());
+  std::unordered_map<int, image_t> idx_to_image_id;
+  idx_to_image_id.reserve(images.size());
+  for (auto& [image_id, image] : images) {
+    if (image.is_registered == false) continue;
+    idx_to_image_id[image_id_to_idx.size()] = image_id;
+    image_id_to_idx[image_id] = image_id_to_idx.size();
+  }
+
+  // establish graph
+  weighted_graph G(image_id_to_idx.size());
+  weight_map weights_boost = boost::get(boost::edge_weight, G);
+
+  edge_desc e;
+  for (auto& [pair_id, image_pair] : view_graph.image_pairs) {
+    if (image_pair.is_valid == false) continue;
+
+    const Image& image1 = images.at(image_pair.image_id1);
+    const Image& image2 = images.at(image_pair.image_id2);
+
+    if (image1.is_registered == false || image2.is_registered == false) {
+      continue;
+    }
+
+    int idx1 = image_id_to_idx[image_pair.image_id1];
+    int idx2 = image_id_to_idx[image_pair.image_id2];
+
+    // Set the weight to be negative, then the result would be a maximum
+    // spanning tree
+    e = boost::add_edge(idx1, idx2, G).first;
+    if (type == INLIER_NUM)
+      weights_boost[e] = -image_pair.inliers.size();
+    else if (type == INLIER_RATIO)
+      weights_boost[e] = -image_pair.weight;
+    else
+      weights_boost[e] = -image_pair.inliers.size();
+  }
+
+  std::vector<edge_desc>
+      mst;  // vector to store MST edges (not a property map!)
+  boost::kruskal_minimum_spanning_tree(G, std::back_inserter(mst));
+
+  std::vector<std::vector<int>> edges_list(image_id_to_idx.size());
+  for (const auto& edge : mst) {
+    auto source = boost::source(edge, G);
+    auto target = boost::target(edge, G);
+    edges_list[source].push_back(target);
+    edges_list[target].push_back(source);
+  }
+
+  std::vector<int> parents_idx;
+  BFS(edges_list, 0, parents_idx);
+
+  // change the index back to image id
+  parents.clear();
+  for (int i = 0; i < parents_idx.size(); i++) {
+    parents[idx_to_image_id[i]] = idx_to_image_id[parents_idx[i]];
+  }
+
+  return idx_to_image_id[0];
+}
+
+};  // namespace glomap
\ No newline at end of file
diff --git a/glomap/math/tree.h b/glomap/math/tree.h
new file mode 100644
index 0000000..15ebaac
--- /dev/null
+++ b/glomap/math/tree.h
@@ -0,0 +1,19 @@
+
+#include <unordered_set>
+
+#include <glomap/scene/types_sfm.h>
+
+namespace glomap {
+enum WeightType { INLIER_NUM, INLIER_RATIO };
+
+// Return the number of nodes in the tree
+int BFS(const std::vector<std::vector<int>>& graph,
+        int root,
+        std::vector<int>& parents,
+        std::vector<std::pair<int, int>> banned_edges = {});
+
+image_t MaximumSpanningTree(const ViewGraph& view_graph,
+                            const std::unordered_map<image_t, Image>& images,
+                            std::unordered_map<image_t, image_t>& parents,
+                            WeightType type);
+}  // namespace glomap
\ No newline at end of file
diff --git a/glomap/math/two_view_geometry.cc b/glomap/math/two_view_geometry.cc
index 6f0f04e..3d9a5d1 100644
--- a/glomap/math/two_view_geometry.cc
+++ b/glomap/math/two_view_geometry.cc
@@ -8,7 +8,7 @@ bool CheckCheirality(const Rigid3d& pose,
                      double min_depth,
                      double max_depth) {
   // This code assumes that x1 and x2 are unit vectors
-  const Eigen::Vector3d Rx1 = pose.Rotate(x1);
+  const Eigen::Vector3d Rx1 = pose.rotation * x1;
 
   // [1 a; a 1] * [lambda1; lambda2] = [b1; b2]
   // [lambda1; lambda2] = [1 s-a; -a 1] * [b1; b2] / (1 - a*a)
diff --git a/glomap/math/two_view_geometry.h b/glomap/math/two_view_geometry.h
index b8aee38..6c965d6 100644
--- a/glomap/math/two_view_geometry.h
+++ b/glomap/math/two_view_geometry.h
@@ -1,7 +1,7 @@
 #pragma once
 
 #include "glomap/scene/camera.h"
-#include "glomap/scene/rigid3d.h"
+#include "glomap/scene/types.h"
 #include "glomap/types.h"
 
 namespace glomap {
diff --git a/glomap/processors/image_pair_inliers.cc b/glomap/processors/image_pair_inliers.cc
index 614d8f6..c32878f 100644
--- a/glomap/processors/image_pair_inliers.cc
+++ b/glomap/processors/image_pair_inliers.cc
@@ -49,7 +49,7 @@ double ImagePairInliers::ScoreErrorEssential() {
   double score = 0.;
   Eigen::Vector3d pt1, pt2;
 
-  // TODO: determin the best threshold for triangulation angle
+  // TODO: determine the best threshold for triangulation angle
   // double thres_angle = std::cos(DegToRad(1.));
   double thres_epipole = std::cos(DegToRad(3.));
   double thres_angle = 1;
@@ -69,7 +69,7 @@ double ImagePairInliers::ScoreErrorEssential() {
       bool not_denegerate = true;
 
       // Check whether two image rays are too close
-      double diff_angle = pt1.dot(cam2_from_cam1.Derotate(pt2));
+      double diff_angle = pt1.dot(cam2_from_cam1.rotation.inverse() * pt2);
       not_denegerate = (diff_angle < thres_angle);
 
       // Check whether two points are too close to the epipoles
@@ -117,8 +117,9 @@ double ImagePairInliers::ScoreErrorFundamental() {
   image_t image_id1 = image_pair.image_id1;
   image_t image_id2 = image_pair.image_id2;
 
-  double sq_threshold =
-      options.max_epipolar_error_F * options.max_epipolar_error_F;
+  double thres = options.max_epipolar_error_F;
+  double sq_threshold = thres * thres;
+
   double score = 0.;
   Eigen::Vector2d pt1, pt2;
 
@@ -170,8 +171,8 @@ double ImagePairInliers::ScoreErrorHomography() {
   image_t image_id1 = image_pair.image_id1;
   image_t image_id2 = image_pair.image_id2;
 
-  double sq_threshold =
-      options.max_epipolar_error_H * options.max_epipolar_error_H;
+  double thres = options.max_epipolar_error_H;
+  double sq_threshold = thres * thres;
   double score = 0.;
   Eigen::Vector2d pt1, pt2;
   for (size_t k = 0; k < image_pair.matches.rows(); ++k) {
diff --git a/glomap/processors/view_graph_manipulation.cc b/glomap/processors/view_graph_manipulation.cc
index a4d8828..4571d45 100644
--- a/glomap/processors/view_graph_manipulation.cc
+++ b/glomap/processors/view_graph_manipulation.cc
@@ -12,12 +12,12 @@ image_pair_t ViewGraphManipulater::SparsifyGraph(
 
   // Keep track of chosen edges
   std::unordered_set<image_pair_t> chosen_edges;
-  std::unordered_map<image_t, std::unordered_set<image_t>>& adjacency_list =
-      view_graph.GetAdjacencyList();
+  const std::unordered_map<image_t, std::unordered_set<image_t>>&
+      adjacency_list = view_graph.GetAdjacencyList();
 
   // Here, the average is the mean of the degrees
   double average_degree = 0;
-  for (auto& [image_id, neighbors] : adjacency_list) {
+  for (const auto& [image_id, neighbors] : adjacency_list) {
     if (images[image_id].is_registered == false) continue;
     average_degree += neighbors.size();
   }
@@ -35,8 +35,8 @@ image_pair_t ViewGraphManipulater::SparsifyGraph(
         images[image_id2].is_registered == false)
       continue;
 
-    int degree1 = adjacency_list[image_id1].size();
-    int degree2 = adjacency_list[image_id2].size();
+    int degree1 = adjacency_list.at(image_id1).size();
+    int degree2 = adjacency_list.at(image_id2).size();
 
     if (degree1 <= expected_degree || degree2 <= expected_degree) {
       chosen_edges.insert(pair_id);
diff --git a/glomap/scene/image.h b/glomap/scene/image.h
index 23fb6c2..a191371 100644
--- a/glomap/scene/image.h
+++ b/glomap/scene/image.h
@@ -1,16 +1,22 @@
 #pragma once
 
 #include "glomap/math/gravity.h"
-#include "glomap/scene/rigid3d.h"
 #include "glomap/scene/types.h"
 #include "glomap/types.h"
 
 namespace glomap {
 
 struct GravityInfo {
+ public:
   // Whether the gravity information is available
   bool has_gravity = false;
 
+  const Eigen::Matrix3d& GetRAlign() const { return R_align; }
+
+  inline void SetGravity(const Eigen::Vector3d& g);
+  inline Eigen::Vector3d GetGravity();
+
+ private:
   // Direction of the gravity
   Eigen::Vector3d gravity;
 
@@ -49,16 +55,17 @@ struct Image {
 
   // Methods
   inline Eigen::Vector3d Center() const;
-  inline void SetGravityInfo(const Eigen::Vector3d& g);
 };
 
 Eigen::Vector3d Image::Center() const {
   return cam_from_world.rotation.inverse() * -cam_from_world.translation;
 }
 
-void Image::SetGravityInfo(const Eigen::Vector3d& g) {
-  gravity_info.gravity = g;
-  gravity_info.R_align = GetAlignRot(g);
-  gravity_info.has_gravity = true;
+void GravityInfo::SetGravity(const Eigen::Vector3d& g) {
+  gravity = g;
+  R_align = GetAlignRot(g);
+  has_gravity = true;
 }
+
+Eigen::Vector3d GravityInfo::GetGravity() { return gravity; }
 }  // namespace glomap
diff --git a/glomap/scene/image_pair.h b/glomap/scene/image_pair.h
index 43cc5f5..c913cab 100644
--- a/glomap/scene/image_pair.h
+++ b/glomap/scene/image_pair.h
@@ -1,6 +1,5 @@
 #pragma once
 
-#include "glomap/scene/rigid3d.h"
 #include "glomap/scene/types.h"
 #include "glomap/types.h"
 
diff --git a/glomap/scene/rigid3d.h b/glomap/scene/rigid3d.h
deleted file mode 100644
index 6a196d2..0000000
--- a/glomap/scene/rigid3d.h
+++ /dev/null
@@ -1,29 +0,0 @@
-#pragma once
-#include <colmap/geometry/rigid3.h>
-
-namespace glomap {
-struct Rigid3d : public colmap::Rigid3d {
-  Rigid3d() : colmap::Rigid3d() {}
-  Rigid3d(const colmap::Rigid3d& rigid3d) : colmap::Rigid3d(rigid3d) {}
-
-  Rigid3d& operator=(const colmap::Rigid3d& rigid3d) {
-    *this = Rigid3d(rigid3d);
-    return *this;
-  }
-
-  inline Eigen::Vector3d Rotate(const Eigen::Vector3d& point) const {
-    return rotation * point;
-  }
-
-  inline Eigen::Vector3d Derotate(const Eigen::Vector3d& point) const {
-    return rotation.inverse() * point;
-  }
-};
-
-inline std::ostream& operator<<(std::ostream& output, Rigid3d& pose) {
-  output << "q: " << pose.rotation.coeffs().transpose()
-         << ", t: " << pose.translation.transpose();
-  return output;
-}
-
-}  // namespace glomap
diff --git a/glomap/scene/types.h b/glomap/scene/types.h
index 1f48147..3499a10 100644
--- a/glomap/scene/types.h
+++ b/glomap/scene/types.h
@@ -1,5 +1,8 @@
 #pragma once
 
+#include <colmap/geometry/rigid3.h>
+#include <colmap/util/types.h>
+
 #include <cstdint>
 #include <limits>
 #include <unordered_map>
@@ -12,10 +15,10 @@ namespace glomap {
 ////////////////////////////////////////////////////////////////////////////////
 
 // Unique identifier for cameras.
-typedef uint32_t camera_t;
+using colmap::camera_t;
 
 // Unique identifier for images.
-typedef uint32_t image_t;
+using colmap::image_t;
 
 // Each image pair gets a unique ID, see `Database::ImagePairToPairId`.
 typedef uint64_t image_pair_t;
@@ -28,6 +31,8 @@ typedef uint32_t feature_t;
 // unique indices should be large.
 typedef uint64_t track_t;
 
+using colmap::Rigid3d;
+
 const image_t kMaxNumImages = std::numeric_limits<image_t>::max();
 const image_pair_t kInvalidImagePairId = -1;
 
diff --git a/glomap/scene/types_sfm.h b/glomap/scene/types_sfm.h
index 8810d56..4f03b5d 100644
--- a/glomap/scene/types_sfm.h
+++ b/glomap/scene/types_sfm.h
@@ -2,7 +2,6 @@
 // Types defined by GLOMAP
 #include "glomap/scene/camera.h"
 #include "glomap/scene/image.h"
-#include "glomap/scene/rigid3d.h"
 #include "glomap/scene/track.h"
 #include "glomap/scene/types.h"
 #include "glomap/scene/view_graph.h"
diff --git a/glomap/scene/view_graph.h b/glomap/scene/view_graph.h
index 5b4e957..7c28229 100644
--- a/glomap/scene/view_graph.h
+++ b/glomap/scene/view_graph.h
@@ -26,8 +26,8 @@ class ViewGraph {
   // Establish the adjacency list
   void EstablishAdjacencyList();
 
-  inline std::unordered_map<image_t, std::unordered_set<image_t>>&
-  GetAdjacencyList();
+  inline const std::unordered_map<image_t, std::unordered_set<image_t>>&
+  GetAdjacencyList() const;
 
   // Data
   std::unordered_map<image_pair_t, ImagePair> image_pairs;
@@ -47,8 +47,8 @@ class ViewGraph {
   std::vector<std::unordered_set<image_t>> connected_components;
 };
 
-std::unordered_map<image_t, std::unordered_set<image_t>>&
-ViewGraph::GetAdjacencyList() {
+const std::unordered_map<image_t, std::unordered_set<image_t>>&
+ViewGraph::GetAdjacencyList() const {
   return adjacency_list;
 }
 
diff --git a/glomap/types.h b/glomap/types.h
index c503ed0..ac7eda5 100644
--- a/glomap/types.h
+++ b/glomap/types.h
@@ -29,7 +29,7 @@ struct InlierThresholdOptions {
   // Thresholds for edges
   double min_inlier_num = 30;
   double min_inlier_ratio = 0.25;
-  double max_roation_error = 10.;  // in degree, for rotation averaging
+  double max_rotation_error = 10.;  // in degree, for rotation averaging
 };
 
 }  // namespace glomap