Add reconstruction normalization after each step (#136)

* add reconstruction normalization after each step

* seperate out the helper function for normalization

* d

* f

glomap/CMakeLists.txt

@@ -18,6 +18,7 @@ set(SOURCES
     math/two_view_geometry.cc
     processors/image_pair_inliers.cc
     processors/image_undistorter.cc
+    processors/reconstruction_normalizer.cc
     processors/reconstruction_pruning.cc
     processors/relpose_filter.cc
     processors/track_filter.cc
@@ -49,6 +50,7 @@ set(HEADERS
     math/union_find.h
     processors/image_pair_inliers.h
     processors/image_undistorter.h
+    processors/reconstruction_normalizer.h
     processors/reconstruction_pruning.h
     processors/relpose_filter.h
     processors/track_filter.h

glomap/controllers/global_mapper.cc

@@ -1,7 +1,9 @@
 #include "global_mapper.h"
 
+#include "glomap/io/colmap_converter.h"
 #include "glomap/processors/image_pair_inliers.h"
 #include "glomap/processors/image_undistorter.h"
+#include "glomap/processors/reconstruction_normalizer.h"
 #include "glomap/processors/reconstruction_pruning.h"
 #include "glomap/processors/relpose_filter.h"
 #include "glomap/processors/track_filter.h"
@@ -167,6 +169,10 @@ bool GlobalMapper::Solve(const colmap::Database& database,
         images,
         tracks,
         options_.inlier_thresholds.max_angle_error);
+
+    // Normalize the structure
+    NormalizeReconstruction(cameras, images, tracks);
+
     run_timer.PrintSeconds();
   }
 
@@ -209,6 +215,9 @@ bool GlobalMapper::Solve(const colmap::Database& database,
       if (ite != options_.num_iteration_bundle_adjustment - 1)
         run_timer.PrintSeconds();
 
+      // Normalize the structure
+      NormalizeReconstruction(cameras, images, tracks);
+
       // 6.3. Filter tracks based on the estimation
       // For the filtering, in each round, the criteria for outlier is
       // tightened. If only few tracks are changed, no need to start bundle
@@ -292,6 +301,9 @@ bool GlobalMapper::Solve(const colmap::Database& database,
       run_timer.PrintSeconds();
     }
 
+    // Normalize the structure
+    NormalizeReconstruction(cameras, images, tracks);
+
     // Filter tracks based on the estimation
     UndistortImages(cameras, images, true);
     LOG(INFO) << "Filtering tracks by reprojection ...";

glomap/processors/reconstruction_normalizer.cc

@@ -0,0 +1,75 @@
+#include "reconstruction_normalizer.h"
+
+namespace glomap {
+
+colmap::Sim3d NormalizeReconstruction(
+    std::unordered_map<camera_t, Camera>& cameras,
+    std::unordered_map<image_t, Image>& images,
+    std::unordered_map<track_t, Track>& tracks,
+    bool fixed_scale,
+    double extent,
+    double p0,
+    double p1) {
+  // Coordinates of image centers or point locations.
+  std::vector<float> coords_x;
+  std::vector<float> coords_y;
+  std::vector<float> coords_z;
+
+  coords_x.reserve(images.size());
+  coords_y.reserve(images.size());
+  coords_z.reserve(images.size());
+  for (const auto& [image_id, image] : images) {
+    if (!image.is_registered) continue;
+    const Eigen::Vector3d proj_center = image.Center();
+    coords_x.push_back(static_cast<float>(proj_center(0)));
+    coords_y.push_back(static_cast<float>(proj_center(1)));
+    coords_z.push_back(static_cast<float>(proj_center(2)));
+  }
+
+  // Determine robust bounding box and mean.
+  std::sort(coords_x.begin(), coords_x.end());
+  std::sort(coords_y.begin(), coords_y.end());
+  std::sort(coords_z.begin(), coords_z.end());
+
+  const size_t P0 = static_cast<size_t>(
+      (coords_x.size() > 3) ? p0 * (coords_x.size() - 1) : 0);
+  const size_t P1 = static_cast<size_t>(
+      (coords_x.size() > 3) ? p1 * (coords_x.size() - 1) : coords_x.size() - 1);
+
+  const Eigen::Vector3d bbox_min(coords_x[P0], coords_y[P0], coords_z[P0]);
+  const Eigen::Vector3d bbox_max(coords_x[P1], coords_y[P1], coords_z[P1]);
+
+  Eigen::Vector3d mean_coord(0, 0, 0);
+  for (size_t i = P0; i <= P1; ++i) {
+    mean_coord(0) += coords_x[i];
+    mean_coord(1) += coords_y[i];
+    mean_coord(2) += coords_z[i];
+  }
+  mean_coord /= P1 - P0 + 1;
+
+  // Calculate scale and translation, such that
+  // translation is applied before scaling.
+  double scale = 1.;
+  if (!fixed_scale) {
+    const double old_extent = (bbox_max - bbox_min).norm();
+    if (old_extent >= std::numeric_limits<double>::epsilon()) {
+      scale = extent / old_extent;
+    }
+  }
+  colmap::Sim3d tform(
+      scale, Eigen::Quaterniond::Identity(), -scale * mean_coord);
+
+  for (auto& [_, image] : images) {
+    if (image.is_registered) {
+      image.cam_from_world = TransformCameraWorld(tform, image.cam_from_world);
+    }
+  }
+
+  for (auto& [_, track] : tracks) {
+    track.xyz = tform * track.xyz;
+  }
+
+  return tform;
+}
+
+}  // namespace glomap

glomap/processors/reconstruction_normalizer.h

@@ -0,0 +1,17 @@
+#pragma once
+
+#include "glomap/scene/types_sfm.h"
+
+#include "colmap/geometry/pose.h"
+
+namespace glomap {
+
+colmap::Sim3d NormalizeReconstruction(
+    std::unordered_map<camera_t, Camera>& cameras,
+    std::unordered_map<image_t, Image>& images,
+    std::unordered_map<track_t, Track>& tracks,
+    bool fixed_scale = false,
+    double extent = 10.,
+    double p0 = 0.1,
+    double p1 = 0.9);
+}  // namespace glomap