ceres-solver-v2

.gitattributes

@@ -43,3 +43,9 @@ src/exe/CMakeFiles/colmap_exe.dir/vocab_tree.cc.o filter=lfs diff=lfs merge=lfs
 src/exe/colmap filter=lfs diff=lfs merge=lfs -text
 src/libcolmap.a filter=lfs diff=lfs merge=lfs -text
 src/libcolmap_cuda.a filter=lfs diff=lfs merge=lfs -text
+colmap/bin/colmap filter=lfs diff=lfs merge=lfs -text
+colmap/lib/libcolmap.a filter=lfs diff=lfs merge=lfs -text
+colmap/lib/libcolmap_cuda.a filter=lfs diff=lfs merge=lfs -text
+colmap/lib/libpba.a filter=lfs diff=lfs merge=lfs -text
+colmap/lib/libpoisson_recon.a filter=lfs diff=lfs merge=lfs -text
+colmap/lib/libsift_gpu.a filter=lfs diff=lfs merge=lfs -text

colmap/bin/colmap

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:9efc313d53067e8a32ff66555e20318dc8382bd06ccc9ea769440f20da5d74be
+size 31933416

colmap/include/colmap/base/camera.h

@@ -0,0 +1,211 @@
+// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//
+//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
+//       its contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
+
+#ifndef COLMAP_SRC_BASE_CAMERA_H_
+#define COLMAP_SRC_BASE_CAMERA_H_
+
+#include <vector>
+
+#include "util/types.h"
+
+namespace colmap {
+
+// Camera class that holds the intrinsic parameters. Cameras may be shared
+// between multiple images, e.g., if the same "physical" camera took multiple
+// pictures with the exact same lens and intrinsics (focal length, etc.).
+// This class has a specific distortion model defined by a camera model class.
+class Camera {
+ public:
+  Camera();
+
+  // Access the unique identifier of the camera.
+  inline camera_t CameraId() const;
+  inline void SetCameraId(const camera_t camera_id);
+
+  // Access the camera model.
+  inline int ModelId() const;
+  std::string ModelName() const;
+  void SetModelId(const int model_id);
+  void SetModelIdFromName(const std::string& model_name);
+
+  // Access dimensions of the camera sensor.
+  inline size_t Width() const;
+  inline size_t Height() const;
+  inline void SetWidth(const size_t width);
+  inline void SetHeight(const size_t height);
+
+  // Access focal length parameters.
+  double MeanFocalLength() const;
+  double FocalLength() const;
+  double FocalLengthX() const;
+  double FocalLengthY() const;
+  void SetFocalLength(const double focal_length);
+  void SetFocalLengthX(const double focal_length_x);
+  void SetFocalLengthY(const double focal_length_y);
+
+  // Check if camera has prior focal length.
+  inline bool HasPriorFocalLength() const;
+  inline void SetPriorFocalLength(const bool prior);
+
+  // Access principal point parameters. Only works if there are two
+  // principal point parameters.
+  double PrincipalPointX() const;
+  double PrincipalPointY() const;
+  void SetPrincipalPointX(const double ppx);
+  void SetPrincipalPointY(const double ppy);
+
+  // Get the indices of the parameter groups in the parameter vector.
+  const std::vector<size_t>& FocalLengthIdxs() const;
+  const std::vector<size_t>& PrincipalPointIdxs() const;
+  const std::vector<size_t>& ExtraParamsIdxs() const;
+
+  // Get intrinsic calibration matrix composed from focal length and principal
+  // point parameters, excluding distortion parameters.
+  Eigen::Matrix3d CalibrationMatrix() const;
+
+  // Get human-readable information about the parameter vector ordering.
+  std::string ParamsInfo() const;
+
+  // Access the raw parameter vector.
+  inline size_t NumParams() const;
+  inline const std::vector<double>& Params() const;
+  inline std::vector<double>& Params();
+  inline double Params(const size_t idx) const;
+  inline double& Params(const size_t idx);
+  inline const double* ParamsData() const;
+  inline double* ParamsData();
+  inline void SetParams(const std::vector<double>& params);
+
+  // Concatenate parameters as comma-separated list.
+  std::string ParamsToString() const;
+
+  // Set camera parameters from comma-separated list.
+  bool SetParamsFromString(const std::string& string);
+
+  // Check whether parameters are valid, i.e. the parameter vector has
+  // the correct dimensions that match the specified camera model.
+  bool VerifyParams() const;
+
+  // Check whether camera is already undistorted
+  bool IsUndistorted() const;
+
+  // Check whether camera has bogus parameters.
+  bool HasBogusParams(const double min_focal_length_ratio,
+                      const double max_focal_length_ratio,
+                      const double max_extra_param) const;
+
+  // Initialize parameters for given camera model and focal length, and set
+  // the principal point to be the image center.
+  void InitializeWithId(const int model_id, const double focal_length,
+                        const size_t width, const size_t height);
+  void InitializeWithName(const std::string& model_name,
+                          const double focal_length, const size_t width,
+                          const size_t height);
+
+  // Project point in image plane to world / infinity.
+  Eigen::Vector2d ImageToWorld(const Eigen::Vector2d& image_point) const;
+
+  // Convert pixel threshold in image plane to world space.
+  double ImageToWorldThreshold(const double threshold) const;
+
+  // Project point from world / infinity to image plane.
+  Eigen::Vector2d WorldToImage(const Eigen::Vector2d& world_point) const;
+
+  // Rescale camera dimensions and accordingly the focal length and
+  // and the principal point.
+  void Rescale(const double scale);
+  void Rescale(const size_t width, const size_t height);
+
+ private:
+  // The unique identifier of the camera. If the identifier is not specified
+  // it is set to `kInvalidCameraId`.
+  camera_t camera_id_;
+
+  // The identifier of the camera model. If the camera model is not specified
+  // the identifier is `kInvalidCameraModelId`.
+  int model_id_;
+
+  // The dimensions of the image, 0 if not initialized.
+  size_t width_;
+  size_t height_;
+
+  // The focal length, principal point, and extra parameters. If the camera
+  // model is not specified, this vector is empty.
+  std::vector<double> params_;
+
+  // Whether there is a safe prior for the focal length,
+  // e.g. manually provided or extracted from EXIF
+  bool prior_focal_length_;
+};
+
+////////////////////////////////////////////////////////////////////////////////
+// Implementation
+////////////////////////////////////////////////////////////////////////////////
+
+camera_t Camera::CameraId() const { return camera_id_; }
+
+void Camera::SetCameraId(const camera_t camera_id) { camera_id_ = camera_id; }
+
+int Camera::ModelId() const { return model_id_; }
+
+size_t Camera::Width() const { return width_; }
+
+size_t Camera::Height() const { return height_; }
+
+void Camera::SetWidth(const size_t width) { width_ = width; }
+
+void Camera::SetHeight(const size_t height) { height_ = height; }
+
+bool Camera::HasPriorFocalLength() const { return prior_focal_length_; }
+
+void Camera::SetPriorFocalLength(const bool prior) {
+  prior_focal_length_ = prior;
+}
+
+size_t Camera::NumParams() const { return params_.size(); }
+
+const std::vector<double>& Camera::Params() const { return params_; }
+
+std::vector<double>& Camera::Params() { return params_; }
+
+double Camera::Params(const size_t idx) const { return params_[idx]; }
+
+double& Camera::Params(const size_t idx) { return params_[idx]; }
+
+const double* Camera::ParamsData() const { return params_.data(); }
+
+double* Camera::ParamsData() { return params_.data(); }
+
+void Camera::SetParams(const std::vector<double>& params) { params_ = params; }
+
+}  // namespace colmap
+
+#endif  // COLMAP_SRC_BASE_CAMERA_H_

colmap/include/colmap/base/camera_database.h

@@ -0,0 +1,58 @@
+// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//
+//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
+//       its contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
+
+#ifndef COLMAP_SRC_BASE_CAMERA_DATABASE_H_
+#define COLMAP_SRC_BASE_CAMERA_DATABASE_H_
+
+#include <string>
+
+#include "util/camera_specs.h"
+
+namespace colmap {
+
+// Database that contains sensor widths for many cameras, which is useful
+// to automatically extract the focal length if EXIF information is incomplete.
+class CameraDatabase {
+ public:
+  CameraDatabase();
+
+  size_t NumEntries() const { return specs_.size(); }
+
+  bool QuerySensorWidth(const std::string& make, const std::string& model,
+                        double* sensor_width);
+
+ private:
+  static const camera_specs_t specs_;
+};
+
+}  // namespace colmap
+
+#endif  // COLMAP_SRC_BASE_CAMERA_DATABASE_H_

colmap/include/colmap/base/camera_models.h

@@ -0,0 +1,1533 @@
+// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//
+//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
+//       its contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
+
+#ifndef COLMAP_SRC_BASE_CAMERA_MODELS_H_
+#define COLMAP_SRC_BASE_CAMERA_MODELS_H_
+
+#include <cfloat>
+#include <string>
+#include <vector>
+
+#include <Eigen/Core>
+#include <Eigen/Dense>
+
+#include <ceres/ceres.h>
+
+namespace colmap {
+
+// This file defines several different camera models and arbitrary new camera
+// models can be added by the following steps:
+//
+//  1. Add a new struct in this file which implements all the necessary methods.
+//  2. Define an unique model_name and model_id for the camera model.
+//  3. Add camera model to `CAMERA_MODEL_CASES` macro in this file.
+//  4. Add new template specialization of test case for camera model to
+//     `camera_models_test.cc`.
+//
+// A camera model can have three different types of camera parameters: focal
+// length, principal point, extra parameters (distortion parameters). The
+// parameter array is split into different groups, so that we can enable or
+// disable the refinement of the individual groups during bundle adjustment. It
+// is up to the camera model to access the parameters correctly (it is free to
+// do so in an arbitrary manner) - the parameters are not accessed from outside.
+//
+// A camera model must have the following methods:
+//
+//  - `WorldToImage`: transform normalized camera coordinates to image
+//    coordinates (the inverse of `ImageToWorld`). Assumes that the world
+//    coordinates are given as (u, v, 1).
+//  - `ImageToWorld`: transform image coordinates to normalized camera
+//    coordinates (the inverse of `WorldToImage`). Produces world coordinates
+//    as (u, v, 1).
+//  - `ImageToWorldThreshold`: transform a threshold given in pixels to
+//    normalized units (e.g. useful for reprojection error thresholds).
+//
+// Whenever you specify the camera parameters in a list, they must appear
+// exactly in the order as they are accessed in the defined model struct.
+//
+// The camera models follow the convention that the upper left image corner has
+// the coordinate (0, 0), the lower right corner (width, height), i.e. that
+// the upper left pixel center has coordinate (0.5, 0.5) and the lower right
+// pixel center has the coordinate (width - 0.5, height - 0.5).
+
+static const int kInvalidCameraModelId = -1;
+
+#ifndef CAMERA_MODEL_DEFINITIONS
+#define CAMERA_MODEL_DEFINITIONS(model_id_value, model_name_value,             \
+                                 num_params_value)                             \
+  static const int kModelId = model_id_value;                                  \
+  static const size_t kNumParams = num_params_value;                           \
+  static const int model_id;                                                   \
+  static const std::string model_name;                                         \
+  static const size_t num_params;                                              \
+  static const std::string params_info;                                        \
+  static const std::vector<size_t> focal_length_idxs;                          \
+  static const std::vector<size_t> principal_point_idxs;                       \
+  static const std::vector<size_t> extra_params_idxs;                          \
+                                                                               \
+  static inline int InitializeModelId() { return model_id_value; };            \
+  static inline std::string InitializeModelName() {                            \
+    return model_name_value;                                                   \
+  };                                                                           \
+  static inline size_t InitializeNumParams() { return num_params_value; };     \
+  static inline std::string InitializeParamsInfo();                            \
+  static inline std::vector<size_t> InitializeFocalLengthIdxs();               \
+  static inline std::vector<size_t> InitializePrincipalPointIdxs();            \
+  static inline std::vector<size_t> InitializeExtraParamsIdxs();               \
+  static inline std::vector<double> InitializeParams(                          \
+      const double focal_length, const size_t width, const size_t height);     \
+                                                                               \
+  template <typename T>                                                        \
+  static void WorldToImage(const T* params, const T u, const T v, T* x, T* y); \
+  template <typename T>                                                        \
+  static void ImageToWorld(const T* params, const T x, const T y, T* u, T* v); \
+  template <typename T>                                                        \
+  static void Distortion(const T* extra_params, const T u, const T v, T* du,   \
+                         T* dv);
+#endif
+
+#ifndef CAMERA_MODEL_CASES
+#define CAMERA_MODEL_CASES                          \
+  CAMERA_MODEL_CASE(SimplePinholeCameraModel)       \
+  CAMERA_MODEL_CASE(PinholeCameraModel)             \
+  CAMERA_MODEL_CASE(SimpleRadialCameraModel)        \
+  CAMERA_MODEL_CASE(SimpleRadialFisheyeCameraModel) \
+  CAMERA_MODEL_CASE(RadialCameraModel)              \
+  CAMERA_MODEL_CASE(RadialFisheyeCameraModel)       \
+  CAMERA_MODEL_CASE(OpenCVCameraModel)              \
+  CAMERA_MODEL_CASE(OpenCVFisheyeCameraModel)       \
+  CAMERA_MODEL_CASE(FullOpenCVCameraModel)          \
+  CAMERA_MODEL_CASE(FOVCameraModel)                 \
+  CAMERA_MODEL_CASE(ThinPrismFisheyeCameraModel)
+#endif
+
+#ifndef CAMERA_MODEL_SWITCH_CASES
+#define CAMERA_MODEL_SWITCH_CASES         \
+  CAMERA_MODEL_CASES                      \
+  default:                                \
+    CAMERA_MODEL_DOES_NOT_EXIST_EXCEPTION \
+    break;
+#endif
+
+#define CAMERA_MODEL_DOES_NOT_EXIST_EXCEPTION \
+  throw std::domain_error("Camera model does not exist");
+
+// The "Curiously Recurring Template Pattern" (CRTP) is used here, so that we
+// can reuse some shared functionality between all camera models -
+// defined in the BaseCameraModel.
+template <typename CameraModel>
+struct BaseCameraModel {
+  template <typename T>
+  static inline bool HasBogusParams(const std::vector<T>& params,
+                                    const size_t width, const size_t height,
+                                    const T min_focal_length_ratio,
+                                    const T max_focal_length_ratio,
+                                    const T max_extra_param);
+
+  template <typename T>
+  static inline bool HasBogusFocalLength(const std::vector<T>& params,
+                                         const size_t width,
+                                         const size_t height,
+                                         const T min_focal_length_ratio,
+                                         const T max_focal_length_ratio);
+
+  template <typename T>
+  static inline bool HasBogusPrincipalPoint(const std::vector<T>& params,
+                                            const size_t width,
+                                            const size_t height);
+
+  template <typename T>
+  static inline bool HasBogusExtraParams(const std::vector<T>& params,
+                                         const T max_extra_param);
+
+  template <typename T>
+  static inline T ImageToWorldThreshold(const T* params, const T threshold);
+
+  template <typename T>
+  static inline void IterativeUndistortion(const T* params, T* u, T* v);
+};
+
+// Simple Pinhole camera model.
+//
+// No Distortion is assumed. Only focal length and principal point is modeled.
+//
+// Parameter list is expected in the following order:
+//
+//   f, cx, cy
+//
+// See https://en.wikipedia.org/wiki/Pinhole_camera_model
+struct SimplePinholeCameraModel
+    : public BaseCameraModel<SimplePinholeCameraModel> {
+  CAMERA_MODEL_DEFINITIONS(0, "SIMPLE_PINHOLE", 3)
+};
+
+// Pinhole camera model.
+//
+// No Distortion is assumed. Only focal length and principal point is modeled.
+//
+// Parameter list is expected in the following order:
+//
+//    fx, fy, cx, cy
+//
+// See https://en.wikipedia.org/wiki/Pinhole_camera_model
+struct PinholeCameraModel : public BaseCameraModel<PinholeCameraModel> {
+  CAMERA_MODEL_DEFINITIONS(1, "PINHOLE", 4)
+};
+
+// Simple camera model with one focal length and one radial distortion
+// parameter.
+//
+// This model is similar to the camera model that VisualSfM uses with the
+// difference that the distortion here is applied to the projections and
+// not to the measurements.
+//
+// Parameter list is expected in the following order:
+//
+//    f, cx, cy, k
+//
+struct SimpleRadialCameraModel
+    : public BaseCameraModel<SimpleRadialCameraModel> {
+  CAMERA_MODEL_DEFINITIONS(2, "SIMPLE_RADIAL", 4)
+};
+
+// Simple camera model with one focal length and two radial distortion
+// parameters.
+//
+// This model is equivalent to the camera model that Bundler uses
+// (except for an inverse z-axis in the camera coordinate system).
+//
+// Parameter list is expected in the following order:
+//
+//    f, cx, cy, k1, k2
+//
+struct RadialCameraModel : public BaseCameraModel<RadialCameraModel> {
+  CAMERA_MODEL_DEFINITIONS(3, "RADIAL", 5)
+};
+
+// OpenCV camera model.
+//
+// Based on the pinhole camera model. Additionally models radial and
+// tangential distortion (up to 2nd degree of coefficients). Not suitable for
+// large radial distortions of fish-eye cameras.
+//
+// Parameter list is expected in the following order:
+//
+//    fx, fy, cx, cy, k1, k2, p1, p2
+//
+// See
+// http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html
+struct OpenCVCameraModel : public BaseCameraModel<OpenCVCameraModel> {
+  CAMERA_MODEL_DEFINITIONS(4, "OPENCV", 8)
+};
+
+// OpenCV fish-eye camera model.
+//
+// Based on the pinhole camera model. Additionally models radial and
+// tangential Distortion (up to 2nd degree of coefficients). Suitable for
+// large radial distortions of fish-eye cameras.
+//
+// Parameter list is expected in the following order:
+//
+//    fx, fy, cx, cy, k1, k2, k3, k4
+//
+// See
+// http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html
+struct OpenCVFisheyeCameraModel
+    : public BaseCameraModel<OpenCVFisheyeCameraModel> {
+  CAMERA_MODEL_DEFINITIONS(5, "OPENCV_FISHEYE", 8)
+};
+
+// Full OpenCV camera model.
+//
+// Based on the pinhole camera model. Additionally models radial and
+// tangential Distortion.
+//
+// Parameter list is expected in the following order:
+//
+//    fx, fy, cx, cy, k1, k2, p1, p2, k3, k4, k5, k6
+//
+// See
+// http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html
+struct FullOpenCVCameraModel : public BaseCameraModel<FullOpenCVCameraModel> {
+  CAMERA_MODEL_DEFINITIONS(6, "FULL_OPENCV", 12)
+};
+
+// FOV camera model.
+//
+// Based on the pinhole camera model. Additionally models radial distortion.
+// This model is for example used by Project Tango for its equidistant
+// calibration type.
+//
+// Parameter list is expected in the following order:
+//
+//    fx, fy, cx, cy, omega
+//
+// See:
+// Frederic Devernay, Olivier Faugeras. Straight lines have to be straight:
+// Automatic calibration and removal of distortion from scenes of structured
+// environments. Machine vision and applications, 2001.
+struct FOVCameraModel : public BaseCameraModel<FOVCameraModel> {
+  CAMERA_MODEL_DEFINITIONS(7, "FOV", 5)
+
+  template <typename T>
+  static void Undistortion(const T* extra_params, const T u, const T v, T* du,
+                           T* dv);
+};
+
+// Simple camera model with one focal length and one radial distortion
+// parameter, suitable for fish-eye cameras.
+//
+// This model is equivalent to the OpenCVFisheyeCameraModel but has only one
+// radial distortion coefficient.
+//
+// Parameter list is expected in the following order:
+//
+//    f, cx, cy, k
+//
+struct SimpleRadialFisheyeCameraModel
+    : public BaseCameraModel<SimpleRadialFisheyeCameraModel> {
+  CAMERA_MODEL_DEFINITIONS(8, "SIMPLE_RADIAL_FISHEYE", 4)
+};
+
+// Simple camera model with one focal length and two radial distortion
+// parameters, suitable for fish-eye cameras.
+//
+// This model is equivalent to the OpenCVFisheyeCameraModel but has only two
+// radial distortion coefficients.
+//
+// Parameter list is expected in the following order:
+//
+//    f, cx, cy, k1, k2
+//
+struct RadialFisheyeCameraModel
+    : public BaseCameraModel<RadialFisheyeCameraModel> {
+  CAMERA_MODEL_DEFINITIONS(9, "RADIAL_FISHEYE", 5)
+};
+
+// Camera model with radial and tangential distortion coefficients and
+// additional coefficients accounting for thin-prism distortion.
+//
+// This camera model is described in
+//
+//    "Camera Calibration with Distortion Models and Accuracy Evaluation",
+//    J Weng et al., TPAMI, 1992.
+//
+// Parameter list is expected in the following order:
+//
+//    fx, fy, cx, cy, k1, k2, p1, p2, k3, k4, sx1, sy1
+//
+struct ThinPrismFisheyeCameraModel
+    : public BaseCameraModel<ThinPrismFisheyeCameraModel> {
+  CAMERA_MODEL_DEFINITIONS(10, "THIN_PRISM_FISHEYE", 12)
+};
+
+// Check whether camera model with given name or identifier exists.
+bool ExistsCameraModelWithName(const std::string& model_name);
+bool ExistsCameraModelWithId(const int model_id);
+
+// Convert camera name to unique camera model identifier.
+//
+// @param name         Unique name of camera model.
+//
+// @return             Unique identifier of camera model.
+int CameraModelNameToId(const std::string& model_name);
+
+// Convert camera model identifier to unique camera model name.
+//
+// @param model_id     Unique identifier of camera model.
+//
+// @return             Unique name of camera model.
+std::string CameraModelIdToName(const int model_id);
+
+// Initialize camera parameters using given image properties.
+//
+// Initializes all focal length parameters to the same given focal length and
+// sets the principal point to the image center.
+//
+// @param model_id      Unique identifier of camera model.
+// @param focal_length  Focal length, equal for all focal length parameters.
+// @param width         Sensor width of the camera.
+// @param height        Sensor height of the camera.
+std::vector<double> CameraModelInitializeParams(const int model_id,
+                                                const double focal_length,
+                                                const size_t width,
+                                                const size_t height);
+
+// Get human-readable information about the parameter vector order.
+//
+// @param model_id     Unique identifier of camera model.
+std::string CameraModelParamsInfo(const int model_id);
+
+// Get the indices of the parameter groups in the parameter vector.
+//
+// @param model_id     Unique identifier of camera model.
+const std::vector<size_t>& CameraModelFocalLengthIdxs(const int model_id);
+const std::vector<size_t>& CameraModelPrincipalPointIdxs(const int model_id);
+const std::vector<size_t>& CameraModelExtraParamsIdxs(const int model_id);
+
+// Get the total number of parameters of a camera model.
+size_t CameraModelNumParams(const int model_id);
+
+// Check whether parameters are valid, i.e. the parameter vector has
+// the correct dimensions that match the specified camera model.
+//
+// @param model_id      Unique identifier of camera model.
+// @param params        Array of camera parameters.
+bool CameraModelVerifyParams(const int model_id,
+                             const std::vector<double>& params);
+
+// Check whether camera has bogus parameters.
+//
+// @param model_id                Unique identifier of camera model.
+// @param params                  Array of camera parameters.
+// @param width                   Sensor width of the camera.
+// @param height                  Sensor height of the camera.
+// @param min_focal_length_ratio  Minimum ratio of focal length over
+//                                maximum sensor dimension.
+// @param min_focal_length_ratio  Maximum ratio of focal length over
+//                                maximum sensor dimension.
+// @param max_extra_param         Maximum magnitude of each extra parameter.
+bool CameraModelHasBogusParams(const int model_id,
+                               const std::vector<double>& params,
+                               const size_t width, const size_t height,
+                               const double min_focal_length_ratio,
+                               const double max_focal_length_ratio,
+                               const double max_extra_param);
+
+// Transform world coordinates in camera coordinate system to image coordinates.
+//
+// This is the inverse of `CameraModelImageToWorld`.
+//
+// @param model_id     Unique model_id of camera model as defined in
+//                     `CAMERA_MODEL_NAME_TO_CODE`.
+// @param params       Array of camera parameters.
+// @param u, v         Coordinates in camera system as (u, v, 1).
+// @param x, y         Output image coordinates in pixels.
+inline void CameraModelWorldToImage(const int model_id,
+                                    const std::vector<double>& params,
+                                    const double u, const double v, double* x,
+                                    double* y);
+
+// Transform image coordinates to world coordinates in camera coordinate system.
+//
+// This is the inverse of `CameraModelWorldToImage`.
+//
+// @param model_id      Unique identifier of camera model.
+// @param params        Array of camera parameters.
+// @param x, y          Image coordinates in pixels.
+// @param v, u          Output Coordinates in camera system as (u, v, 1).
+inline void CameraModelImageToWorld(const int model_id,
+                                    const std::vector<double>& params,
+                                    const double x, const double y, double* u,
+                                    double* v);
+
+// Convert pixel threshold in image plane to world space by dividing
+// the threshold through the mean focal length.
+//
+// @param model_id      Unique identifier of camera model.
+// @param params        Array of camera parameters.
+// @param threshold     Image space threshold in pixels.
+//
+// @ return             World space threshold.
+inline double CameraModelImageToWorldThreshold(
+    const int model_id, const std::vector<double>& params,
+    const double threshold);
+
+////////////////////////////////////////////////////////////////////////////////
+// Implementation
+////////////////////////////////////////////////////////////////////////////////
+
+////////////////////////////////////////////////////////////////////////////////
+// BaseCameraModel
+
+template <typename CameraModel>
+template <typename T>
+bool BaseCameraModel<CameraModel>::HasBogusParams(
+    const std::vector<T>& params, const size_t width, const size_t height,
+    const T min_focal_length_ratio, const T max_focal_length_ratio,
+    const T max_extra_param) {
+  if (HasBogusPrincipalPoint(params, width, height)) {
+    return true;
+  }
+
+  if (HasBogusFocalLength(params, width, height, min_focal_length_ratio,
+                          max_focal_length_ratio)) {
+    return true;
+  }
+
+  if (HasBogusExtraParams(params, max_extra_param)) {
+    return true;
+  }
+
+  return false;
+}
+
+template <typename CameraModel>
+template <typename T>
+bool BaseCameraModel<CameraModel>::HasBogusFocalLength(
+    const std::vector<T>& params, const size_t width, const size_t height,
+    const T min_focal_length_ratio, const T max_focal_length_ratio) {
+  const size_t max_size = std::max(width, height);
+
+  for (const auto& idx : CameraModel::focal_length_idxs) {
+    const T focal_length_ratio = params[idx] / max_size;
+    if (focal_length_ratio < min_focal_length_ratio ||
+        focal_length_ratio > max_focal_length_ratio) {
+      return true;
+    }
+  }
+
+  return false;
+}
+
+template <typename CameraModel>
+template <typename T>
+bool BaseCameraModel<CameraModel>::HasBogusPrincipalPoint(
+    const std::vector<T>& params, const size_t width, const size_t height) {
+  const T cx = params[CameraModel::principal_point_idxs[0]];
+  const T cy = params[CameraModel::principal_point_idxs[1]];
+  return cx < 0 || cx > width || cy < 0 || cy > height;
+}
+
+template <typename CameraModel>
+template <typename T>
+bool BaseCameraModel<CameraModel>::HasBogusExtraParams(
+    const std::vector<T>& params, const T max_extra_param) {
+  for (const auto& idx : CameraModel::extra_params_idxs) {
+    if (std::abs(params[idx]) > max_extra_param) {
+      return true;
+    }
+  }
+
+  return false;
+}
+
+template <typename CameraModel>
+template <typename T>
+T BaseCameraModel<CameraModel>::ImageToWorldThreshold(const T* params,
+                                                      const T threshold) {
+  T mean_focal_length = 0;
+  for (const auto& idx : CameraModel::focal_length_idxs) {
+    mean_focal_length += params[idx];
+  }
+  mean_focal_length /= CameraModel::focal_length_idxs.size();
+  return threshold / mean_focal_length;
+}
+
+template <typename CameraModel>
+template <typename T>
+void BaseCameraModel<CameraModel>::IterativeUndistortion(const T* params, T* u,
+                                                         T* v) {
+  // Parameters for Newton iteration using numerical differentiation with
+  // central differences, 100 iterations should be enough even for complex
+  // camera models with higher order terms.
+  const size_t kNumIterations = 100;
+  const double kMaxStepNorm = 1e-10;
+  const double kRelStepSize = 1e-6;
+
+  Eigen::Matrix2d J;
+  const Eigen::Vector2d x0(*u, *v);
+  Eigen::Vector2d x(*u, *v);
+  Eigen::Vector2d dx;
+  Eigen::Vector2d dx_0b;
+  Eigen::Vector2d dx_0f;
+  Eigen::Vector2d dx_1b;
+  Eigen::Vector2d dx_1f;
+
+  for (size_t i = 0; i < kNumIterations; ++i) {
+    const double step0 = std::max(std::numeric_limits<double>::epsilon(),
+                                  std::abs(kRelStepSize * x(0)));
+    const double step1 = std::max(std::numeric_limits<double>::epsilon(),
+                                  std::abs(kRelStepSize * x(1)));
+    CameraModel::Distortion(params, x(0), x(1), &dx(0), &dx(1));
+    CameraModel::Distortion(params, x(0) - step0, x(1), &dx_0b(0), &dx_0b(1));
+    CameraModel::Distortion(params, x(0) + step0, x(1), &dx_0f(0), &dx_0f(1));
+    CameraModel::Distortion(params, x(0), x(1) - step1, &dx_1b(0), &dx_1b(1));
+    CameraModel::Distortion(params, x(0), x(1) + step1, &dx_1f(0), &dx_1f(1));
+    J(0, 0) = 1 + (dx_0f(0) - dx_0b(0)) / (2 * step0);
+    J(0, 1) = (dx_1f(0) - dx_1b(0)) / (2 * step1);
+    J(1, 0) = (dx_0f(1) - dx_0b(1)) / (2 * step0);
+    J(1, 1) = 1 + (dx_1f(1) - dx_1b(1)) / (2 * step1);
+    const Eigen::Vector2d step_x = J.inverse() * (x + dx - x0);
+    x -= step_x;
+    if (step_x.squaredNorm() < kMaxStepNorm) {
+      break;
+    }
+  }
+
+  *u = x(0);
+  *v = x(1);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// SimplePinholeCameraModel
+
+std::string SimplePinholeCameraModel::InitializeParamsInfo() {
+  return "f, cx, cy";
+}
+
+std::vector<size_t> SimplePinholeCameraModel::InitializeFocalLengthIdxs() {
+  return {0};
+}
+
+std::vector<size_t> SimplePinholeCameraModel::InitializePrincipalPointIdxs() {
+  return {1, 2};
+}
+
+std::vector<size_t> SimplePinholeCameraModel::InitializeExtraParamsIdxs() {
+  return {};
+}
+
+std::vector<double> SimplePinholeCameraModel::InitializeParams(
+    const double focal_length, const size_t width, const size_t height) {
+  return {focal_length, width / 2.0, height / 2.0};
+}
+
+template <typename T>
+void SimplePinholeCameraModel::WorldToImage(const T* params, const T u,
+                                            const T v, T* x, T* y) {
+  const T f = params[0];
+  const T c1 = params[1];
+  const T c2 = params[2];
+
+  // No Distortion
+
+  // Transform to image coordinates
+  *x = f * u + c1;
+  *y = f * v + c2;
+}
+
+template <typename T>
+void SimplePinholeCameraModel::ImageToWorld(const T* params, const T x,
+                                            const T y, T* u, T* v) {
+  const T f = params[0];
+  const T c1 = params[1];
+  const T c2 = params[2];
+
+  *u = (x - c1) / f;
+  *v = (y - c2) / f;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// PinholeCameraModel
+
+std::string PinholeCameraModel::InitializeParamsInfo() {
+  return "fx, fy, cx, cy";
+}
+
+std::vector<size_t> PinholeCameraModel::InitializeFocalLengthIdxs() {
+  return {0, 1};
+}
+
+std::vector<size_t> PinholeCameraModel::InitializePrincipalPointIdxs() {
+  return {2, 3};
+}
+
+std::vector<size_t> PinholeCameraModel::InitializeExtraParamsIdxs() {
+  return {};
+}
+
+std::vector<double> PinholeCameraModel::InitializeParams(
+    const double focal_length, const size_t width, const size_t height) {
+  return {focal_length, focal_length, width / 2.0, height / 2.0};
+}
+
+template <typename T>
+void PinholeCameraModel::WorldToImage(const T* params, const T u, const T v,
+                                      T* x, T* y) {
+  const T f1 = params[0];
+  const T f2 = params[1];
+  const T c1 = params[2];
+  const T c2 = params[3];
+
+  // No Distortion
+
+  // Transform to image coordinates
+  *x = f1 * u + c1;
+  *y = f2 * v + c2;
+}
+
+template <typename T>
+void PinholeCameraModel::ImageToWorld(const T* params, const T x, const T y,
+                                      T* u, T* v) {
+  const T f1 = params[0];
+  const T f2 = params[1];
+  const T c1 = params[2];
+  const T c2 = params[3];
+
+  *u = (x - c1) / f1;
+  *v = (y - c2) / f2;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// SimpleRadialCameraModel
+
+std::string SimpleRadialCameraModel::InitializeParamsInfo() {
+  return "f, cx, cy, k";
+}
+
+std::vector<size_t> SimpleRadialCameraModel::InitializeFocalLengthIdxs() {
+  return {0};
+}
+
+std::vector<size_t> SimpleRadialCameraModel::InitializePrincipalPointIdxs() {
+  return {1, 2};
+}
+
+std::vector<size_t> SimpleRadialCameraModel::InitializeExtraParamsIdxs() {
+  return {3};
+}
+
+std::vector<double> SimpleRadialCameraModel::InitializeParams(
+    const double focal_length, const size_t width, const size_t height) {
+  return {focal_length, width / 2.0, height / 2.0, 0};
+}
+
+template <typename T>
+void SimpleRadialCameraModel::WorldToImage(const T* params, const T u,
+                                           const T v, T* x, T* y) {
+  const T f = params[0];
+  const T c1 = params[1];
+  const T c2 = params[2];
+
+  // Distortion
+  T du, dv;
+  Distortion(&params[3], u, v, &du, &dv);
+  *x = u + du;
+  *y = v + dv;
+
+  // Transform to image coordinates
+  *x = f * *x + c1;
+  *y = f * *y + c2;
+}
+
+template <typename T>
+void SimpleRadialCameraModel::ImageToWorld(const T* params, const T x,
+                                           const T y, T* u, T* v) {
+  const T f = params[0];
+  const T c1 = params[1];
+  const T c2 = params[2];
+
+  // Lift points to normalized plane
+  *u = (x - c1) / f;
+  *v = (y - c2) / f;
+
+  IterativeUndistortion(&params[3], u, v);
+}
+
+template <typename T>
+void SimpleRadialCameraModel::Distortion(const T* extra_params, const T u,
+                                         const T v, T* du, T* dv) {
+  const T k = extra_params[0];
+
+  const T u2 = u * u;
+  const T v2 = v * v;
+  const T r2 = u2 + v2;
+  const T radial = k * r2;
+  *du = u * radial;
+  *dv = v * radial;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// RadialCameraModel
+
+std::string RadialCameraModel::InitializeParamsInfo() {
+  return "f, cx, cy, k1, k2";
+}
+
+std::vector<size_t> RadialCameraModel::InitializeFocalLengthIdxs() {
+  return {0};
+}
+
+std::vector<size_t> RadialCameraModel::InitializePrincipalPointIdxs() {
+  return {1, 2};
+}
+
+std::vector<size_t> RadialCameraModel::InitializeExtraParamsIdxs() {
+  return {3, 4};
+}
+
+std::vector<double> RadialCameraModel::InitializeParams(
+    const double focal_length, const size_t width, const size_t height) {
+  return {focal_length, width / 2.0, height / 2.0, 0, 0};
+}
+
+template <typename T>
+void RadialCameraModel::WorldToImage(const T* params, const T u, const T v,
+                                     T* x, T* y) {
+  const T f = params[0];
+  const T c1 = params[1];
+  const T c2 = params[2];
+
+  // Distortion
+  T du, dv;
+  Distortion(&params[3], u, v, &du, &dv);
+  *x = u + du;
+  *y = v + dv;
+
+  // Transform to image coordinates
+  *x = f * *x + c1;
+  *y = f * *y + c2;
+}
+
+template <typename T>
+void RadialCameraModel::ImageToWorld(const T* params, const T x, const T y,
+                                     T* u, T* v) {
+  const T f = params[0];
+  const T c1 = params[1];
+  const T c2 = params[2];
+
+  // Lift points to normalized plane
+  *u = (x - c1) / f;
+  *v = (y - c2) / f;
+
+  IterativeUndistortion(&params[3], u, v);
+}
+
+template <typename T>
+void RadialCameraModel::Distortion(const T* extra_params, const T u, const T v,
+                                   T* du, T* dv) {
+  const T k1 = extra_params[0];
+  const T k2 = extra_params[1];
+
+  const T u2 = u * u;
+  const T v2 = v * v;
+  const T r2 = u2 + v2;
+  const T radial = k1 * r2 + k2 * r2 * r2;
+  *du = u * radial;
+  *dv = v * radial;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// OpenCVCameraModel
+
+std::string OpenCVCameraModel::InitializeParamsInfo() {
+  return "fx, fy, cx, cy, k1, k2, p1, p2";
+}
+
+std::vector<size_t> OpenCVCameraModel::InitializeFocalLengthIdxs() {
+  return {0, 1};
+}
+
+std::vector<size_t> OpenCVCameraModel::InitializePrincipalPointIdxs() {
+  return {2, 3};
+}
+
+std::vector<size_t> OpenCVCameraModel::InitializeExtraParamsIdxs() {
+  return {4, 5, 6, 7};
+}
+
+std::vector<double> OpenCVCameraModel::InitializeParams(
+    const double focal_length, const size_t width, const size_t height) {
+  return {focal_length, focal_length, width / 2.0, height / 2.0, 0, 0, 0, 0};
+}
+
+template <typename T>
+void OpenCVCameraModel::WorldToImage(const T* params, const T u, const T v,
+                                     T* x, T* y) {
+  const T f1 = params[0];
+  const T f2 = params[1];
+  const T c1 = params[2];
+  const T c2 = params[3];
+
+  // Distortion
+  T du, dv;
+  Distortion(&params[4], u, v, &du, &dv);
+  *x = u + du;
+  *y = v + dv;
+
+  // Transform to image coordinates
+  *x = f1 * *x + c1;
+  *y = f2 * *y + c2;
+}
+
+template <typename T>
+void OpenCVCameraModel::ImageToWorld(const T* params, const T x, const T y,
+                                     T* u, T* v) {
+  const T f1 = params[0];
+  const T f2 = params[1];
+  const T c1 = params[2];
+  const T c2 = params[3];
+
+  // Lift points to normalized plane
+  *u = (x - c1) / f1;
+  *v = (y - c2) / f2;
+
+  IterativeUndistortion(&params[4], u, v);
+}
+
+template <typename T>
+void OpenCVCameraModel::Distortion(const T* extra_params, const T u, const T v,
+                                   T* du, T* dv) {
+  const T k1 = extra_params[0];
+  const T k2 = extra_params[1];
+  const T p1 = extra_params[2];
+  const T p2 = extra_params[3];
+
+  const T u2 = u * u;
+  const T uv = u * v;
+  const T v2 = v * v;
+  const T r2 = u2 + v2;
+  const T radial = k1 * r2 + k2 * r2 * r2;
+  *du = u * radial + T(2) * p1 * uv + p2 * (r2 + T(2) * u2);
+  *dv = v * radial + T(2) * p2 * uv + p1 * (r2 + T(2) * v2);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// OpenCVFisheyeCameraModel
+
+std::string OpenCVFisheyeCameraModel::InitializeParamsInfo() {
+  return "fx, fy, cx, cy, k1, k2, k3, k4";
+}
+
+std::vector<size_t> OpenCVFisheyeCameraModel::InitializeFocalLengthIdxs() {
+  return {0, 1};
+}
+
+std::vector<size_t> OpenCVFisheyeCameraModel::InitializePrincipalPointIdxs() {
+  return {2, 3};
+}
+
+std::vector<size_t> OpenCVFisheyeCameraModel::InitializeExtraParamsIdxs() {
+  return {4, 5, 6, 7};
+}
+
+std::vector<double> OpenCVFisheyeCameraModel::InitializeParams(
+    const double focal_length, const size_t width, const size_t height) {
+  return {focal_length, focal_length, width / 2.0, height / 2.0, 0, 0, 0, 0};
+}
+
+template <typename T>
+void OpenCVFisheyeCameraModel::WorldToImage(const T* params, const T u,
+                                            const T v, T* x, T* y) {
+  const T f1 = params[0];
+  const T f2 = params[1];
+  const T c1 = params[2];
+  const T c2 = params[3];
+
+  // Distortion
+  T du, dv;
+  Distortion(&params[4], u, v, &du, &dv);
+  *x = u + du;
+  *y = v + dv;
+
+  // Transform to image coordinates
+  *x = f1 * *x + c1;
+  *y = f2 * *y + c2;
+}
+
+template <typename T>
+void OpenCVFisheyeCameraModel::ImageToWorld(const T* params, const T x,
+                                            const T y, T* u, T* v) {
+  const T f1 = params[0];
+  const T f2 = params[1];
+  const T c1 = params[2];
+  const T c2 = params[3];
+
+  // Lift points to normalized plane
+  *u = (x - c1) / f1;
+  *v = (y - c2) / f2;
+
+  IterativeUndistortion(&params[4], u, v);
+}
+
+template <typename T>
+void OpenCVFisheyeCameraModel::Distortion(const T* extra_params, const T u,
+                                          const T v, T* du, T* dv) {
+  const T k1 = extra_params[0];
+  const T k2 = extra_params[1];
+  const T k3 = extra_params[2];
+  const T k4 = extra_params[3];
+
+  const T r = ceres::sqrt(u * u + v * v);
+
+  if (r > T(std::numeric_limits<double>::epsilon())) {
+    const T theta = ceres::atan(r);
+    const T theta2 = theta * theta;
+    const T theta4 = theta2 * theta2;
+    const T theta6 = theta4 * theta2;
+    const T theta8 = theta4 * theta4;
+    const T thetad =
+        theta * (T(1) + k1 * theta2 + k2 * theta4 + k3 * theta6 + k4 * theta8);
+    *du = u * thetad / r - u;
+    *dv = v * thetad / r - v;
+  } else {
+    *du = T(0);
+    *dv = T(0);
+  }
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// FullOpenCVCameraModel
+
+std::string FullOpenCVCameraModel::InitializeParamsInfo() {
+  return "fx, fy, cx, cy, k1, k2, p1, p2, k3, k4, k5, k6";
+}
+
+std::vector<size_t> FullOpenCVCameraModel::InitializeFocalLengthIdxs() {
+  return {0, 1};
+}
+
+std::vector<size_t> FullOpenCVCameraModel::InitializePrincipalPointIdxs() {
+  return {2, 3};
+}
+
+std::vector<size_t> FullOpenCVCameraModel::InitializeExtraParamsIdxs() {
+  return {4, 5, 6, 7, 8, 9, 10, 11};
+}
+
+std::vector<double> FullOpenCVCameraModel::InitializeParams(
+    const double focal_length, const size_t width, const size_t height) {
+  return {focal_length,
+          focal_length,
+          width / 2.0,
+          height / 2.0,
+          0,
+          0,
+          0,
+          0,
+          0,
+          0,
+          0,
+          0};
+}
+
+template <typename T>
+void FullOpenCVCameraModel::WorldToImage(const T* params, const T u, const T v,
+                                         T* x, T* y) {
+  const T f1 = params[0];
+  const T f2 = params[1];
+  const T c1 = params[2];
+  const T c2 = params[3];
+
+  // Distortion
+  T du, dv;
+  Distortion(&params[4], u, v, &du, &dv);
+  *x = u + du;
+  *y = v + dv;
+
+  // Transform to image coordinates
+  *x = f1 * *x + c1;
+  *y = f2 * *y + c2;
+}
+
+template <typename T>
+void FullOpenCVCameraModel::ImageToWorld(const T* params, const T x, const T y,
+                                         T* u, T* v) {
+  const T f1 = params[0];
+  const T f2 = params[1];
+  const T c1 = params[2];
+  const T c2 = params[3];
+
+  // Lift points to normalized plane
+  *u = (x - c1) / f1;
+  *v = (y - c2) / f2;
+
+  IterativeUndistortion(&params[4], u, v);
+}
+
+template <typename T>
+void FullOpenCVCameraModel::Distortion(const T* extra_params, const T u,
+                                       const T v, T* du, T* dv) {
+  const T k1 = extra_params[0];
+  const T k2 = extra_params[1];
+  const T p1 = extra_params[2];
+  const T p2 = extra_params[3];
+  const T k3 = extra_params[4];
+  const T k4 = extra_params[5];
+  const T k5 = extra_params[6];
+  const T k6 = extra_params[7];
+
+  const T u2 = u * u;
+  const T uv = u * v;
+  const T v2 = v * v;
+  const T r2 = u2 + v2;
+  const T r4 = r2 * r2;
+  const T r6 = r4 * r2;
+  const T radial = (T(1) + k1 * r2 + k2 * r4 + k3 * r6) /
+                   (T(1) + k4 * r2 + k5 * r4 + k6 * r6);
+  *du = u * radial + T(2) * p1 * uv + p2 * (r2 + T(2) * u2) - u;
+  *dv = v * radial + T(2) * p2 * uv + p1 * (r2 + T(2) * v2) - v;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// FOVCameraModel
+
+std::string FOVCameraModel::InitializeParamsInfo() {
+  return "fx, fy, cx, cy, omega";
+}
+
+std::vector<size_t> FOVCameraModel::InitializeFocalLengthIdxs() {
+  return {0, 1};
+}
+
+std::vector<size_t> FOVCameraModel::InitializePrincipalPointIdxs() {
+  return {2, 3};
+}
+
+std::vector<size_t> FOVCameraModel::InitializeExtraParamsIdxs() { return {4}; }
+
+std::vector<double> FOVCameraModel::InitializeParams(const double focal_length,
+                                                     const size_t width,
+                                                     const size_t height) {
+  return {focal_length, focal_length, width / 2.0, height / 2.0, 1e-2};
+}
+
+template <typename T>
+void FOVCameraModel::WorldToImage(const T* params, const T u, const T v, T* x,
+                                  T* y) {
+  const T f1 = params[0];
+  const T f2 = params[1];
+  const T c1 = params[2];
+  const T c2 = params[3];
+
+  // Distortion
+  Distortion(&params[4], u, v, x, y);
+
+  // Transform to image coordinates
+  *x = f1 * *x + c1;
+  *y = f2 * *y + c2;
+}
+
+template <typename T>
+void FOVCameraModel::ImageToWorld(const T* params, const T x, const T y, T* u,
+                                  T* v) {
+  const T f1 = params[0];
+  const T f2 = params[1];
+  const T c1 = params[2];
+  const T c2 = params[3];
+
+  // Lift points to normalized plane
+  const T uu = (x - c1) / f1;
+  const T vv = (y - c2) / f2;
+
+  // Undistortion
+  Undistortion(&params[4], uu, vv, u, v);
+}
+
+template <typename T>
+void FOVCameraModel::Distortion(const T* extra_params, const T u, const T v,
+                                T* du, T* dv) {
+  const T omega = extra_params[0];
+
+  // Chosen arbitrarily.
+  const T kEpsilon = T(1e-4);
+
+  const T radius2 = u * u + v * v;
+  const T omega2 = omega * omega;
+
+  T factor;
+  if (omega2 < kEpsilon) {
+    // Derivation of this case with Matlab:
+    // syms radius omega;
+    // factor(radius) = atan(radius * 2 * tan(omega / 2)) / ...
+    //                  (radius * omega);
+    // simplify(taylor(factor, omega, 'order', 3))
+    factor = (omega2 * radius2) / T(3) - omega2 / T(12) + T(1);
+  } else if (radius2 < kEpsilon) {
+    // Derivation of this case with Matlab:
+    // syms radius omega;
+    // factor(radius) = atan(radius * 2 * tan(omega / 2)) / ...
+    //                  (radius * omega);
+    // simplify(taylor(factor, radius, 'order', 3))
+    const T tan_half_omega = ceres::tan(omega / T(2));
+    factor = (T(-2) * tan_half_omega *
+              (T(4) * radius2 * tan_half_omega * tan_half_omega - T(3))) /
+             (T(3) * omega);
+  } else {
+    const T radius = ceres::sqrt(radius2);
+    const T numerator = ceres::atan(radius * T(2) * ceres::tan(omega / T(2)));
+    factor = numerator / (radius * omega);
+  }
+
+  *du = u * factor;
+  *dv = v * factor;
+}
+
+template <typename T>
+void FOVCameraModel::Undistortion(const T* extra_params, const T u, const T v,
+                                  T* du, T* dv) {
+  T omega = extra_params[0];
+
+  // Chosen arbitrarily.
+  const T kEpsilon = T(1e-4);
+
+  const T radius2 = u * u + v * v;
+  const T omega2 = omega * omega;
+
+  T factor;
+  if (omega2 < kEpsilon) {
+    // Derivation of this case with Matlab:
+    // syms radius omega;
+    // factor(radius) = tan(radius * omega) / ...
+    //                  (radius * 2*tan(omega/2));
+    // simplify(taylor(factor, omega, 'order', 3))
+    factor = (omega2 * radius2) / T(3) - omega2 / T(12) + T(1);
+  } else if (radius2 < kEpsilon) {
+    // Derivation of this case with Matlab:
+    // syms radius omega;
+    // factor(radius) = tan(radius * omega) / ...
+    //                  (radius * 2*tan(omega/2));
+    // simplify(taylor(factor, radius, 'order', 3))
+    factor = (omega * (omega * omega * radius2 + T(3))) /
+             (T(6) * ceres::tan(omega / T(2)));
+  } else {
+    const T radius = ceres::sqrt(radius2);
+    const T numerator = ceres::tan(radius * omega);
+    factor = numerator / (radius * T(2) * ceres::tan(omega / T(2)));
+  }
+
+  *du = u * factor;
+  *dv = v * factor;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// SimpleRadialFisheyeCameraModel
+
+std::string SimpleRadialFisheyeCameraModel::InitializeParamsInfo() {
+  return "f, cx, cy, k";
+}
+
+std::vector<size_t>
+SimpleRadialFisheyeCameraModel::InitializeFocalLengthIdxs() {
+  return {0};
+}
+
+std::vector<size_t>
+SimpleRadialFisheyeCameraModel::InitializePrincipalPointIdxs() {
+  return {1, 2};
+}
+
+std::vector<size_t>
+SimpleRadialFisheyeCameraModel::InitializeExtraParamsIdxs() {
+  return {3};
+}
+
+std::vector<double> SimpleRadialFisheyeCameraModel::InitializeParams(
+    const double focal_length, const size_t width, const size_t height) {
+  return {focal_length, width / 2.0, height / 2.0, 0};
+}
+
+template <typename T>
+void SimpleRadialFisheyeCameraModel::WorldToImage(const T* params, const T u,
+                                                  const T v, T* x, T* y) {
+  const T f = params[0];
+  const T c1 = params[1];
+  const T c2 = params[2];
+
+  // Distortion
+  T du, dv;
+  Distortion(&params[3], u, v, &du, &dv);
+  *x = u + du;
+  *y = v + dv;
+
+  // Transform to image coordinates
+  *x = f * *x + c1;
+  *y = f * *y + c2;
+}
+
+template <typename T>
+void SimpleRadialFisheyeCameraModel::ImageToWorld(const T* params, const T x,
+                                                  const T y, T* u, T* v) {
+  const T f = params[0];
+  const T c1 = params[1];
+  const T c2 = params[2];
+
+  // Lift points to normalized plane
+  *u = (x - c1) / f;
+  *v = (y - c2) / f;
+
+  IterativeUndistortion(&params[3], u, v);
+}
+
+template <typename T>
+void SimpleRadialFisheyeCameraModel::Distortion(const T* extra_params,
+                                                const T u, const T v, T* du,
+                                                T* dv) {
+  const T k = extra_params[0];
+
+  const T r = ceres::sqrt(u * u + v * v);
+
+  if (r > T(std::numeric_limits<double>::epsilon())) {
+    const T theta = ceres::atan(r);
+    const T theta2 = theta * theta;
+    const T thetad = theta * (T(1) + k * theta2);
+    *du = u * thetad / r - u;
+    *dv = v * thetad / r - v;
+  } else {
+    *du = T(0);
+    *dv = T(0);
+  }
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// RadialFisheyeCameraModel
+
+std::string RadialFisheyeCameraModel::InitializeParamsInfo() {
+  return "f, cx, cy, k1, k2";
+}
+
+std::vector<size_t> RadialFisheyeCameraModel::InitializeFocalLengthIdxs() {
+  return {0};
+}
+
+std::vector<size_t> RadialFisheyeCameraModel::InitializePrincipalPointIdxs() {
+  return {1, 2};
+}
+
+std::vector<size_t> RadialFisheyeCameraModel::InitializeExtraParamsIdxs() {
+  return {3, 4};
+}
+
+std::vector<double> RadialFisheyeCameraModel::InitializeParams(
+    const double focal_length, const size_t width, const size_t height) {
+  return {focal_length, width / 2.0, height / 2.0, 0, 0};
+}
+
+template <typename T>
+void RadialFisheyeCameraModel::WorldToImage(const T* params, const T u,
+                                            const T v, T* x, T* y) {
+  const T f = params[0];
+  const T c1 = params[1];
+  const T c2 = params[2];
+
+  // Distortion
+  T du, dv;
+  Distortion(&params[3], u, v, &du, &dv);
+  *x = u + du;
+  *y = v + dv;
+
+  // Transform to image coordinates
+  *x = f * *x + c1;
+  *y = f * *y + c2;
+}
+
+template <typename T>
+void RadialFisheyeCameraModel::ImageToWorld(const T* params, const T x,
+                                            const T y, T* u, T* v) {
+  const T f = params[0];
+  const T c1 = params[1];
+  const T c2 = params[2];
+
+  // Lift points to normalized plane
+  *u = (x - c1) / f;
+  *v = (y - c2) / f;
+
+  IterativeUndistortion(&params[3], u, v);
+}
+
+template <typename T>
+void RadialFisheyeCameraModel::Distortion(const T* extra_params, const T u,
+                                          const T v, T* du, T* dv) {
+  const T k1 = extra_params[0];
+  const T k2 = extra_params[1];
+
+  const T r = ceres::sqrt(u * u + v * v);
+
+  if (r > T(std::numeric_limits<double>::epsilon())) {
+    const T theta = ceres::atan(r);
+    const T theta2 = theta * theta;
+    const T theta4 = theta2 * theta2;
+    const T thetad = theta * (T(1) + k1 * theta2 + k2 * theta4);
+    *du = u * thetad / r - u;
+    *dv = v * thetad / r - v;
+  } else {
+    *du = T(0);
+    *dv = T(0);
+  }
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// ThinPrismFisheyeCameraModel
+
+std::string ThinPrismFisheyeCameraModel::InitializeParamsInfo() {
+  return "fx, fy, cx, cy, k1, k2, p1, p2, k3, k4, sx1, sy1";
+}
+
+std::vector<size_t> ThinPrismFisheyeCameraModel::InitializeFocalLengthIdxs() {
+  return {0, 1};
+}
+
+std::vector<size_t>
+ThinPrismFisheyeCameraModel::InitializePrincipalPointIdxs() {
+  return {2, 3};
+}
+
+std::vector<size_t> ThinPrismFisheyeCameraModel::InitializeExtraParamsIdxs() {
+  return {4, 5, 6, 7, 8, 9, 10, 11};
+}
+
+std::vector<double> ThinPrismFisheyeCameraModel::InitializeParams(
+    const double focal_length, const size_t width, const size_t height) {
+  return {focal_length,
+          focal_length,
+          width / 2.0,
+          height / 2.0,
+          0,
+          0,
+          0,
+          0,
+          0,
+          0,
+          0,
+          0};
+}
+
+template <typename T>
+void ThinPrismFisheyeCameraModel::WorldToImage(const T* params, const T u,
+                                               const T v, T* x, T* y) {
+  const T f1 = params[0];
+  const T f2 = params[1];
+  const T c1 = params[2];
+  const T c2 = params[3];
+
+  const T r = ceres::sqrt(u * u + v * v);
+
+  T uu, vv;
+  if (r > T(std::numeric_limits<double>::epsilon())) {
+    const T theta = ceres::atan(r);
+    uu = theta * u / r;
+    vv = theta * v / r;
+  } else {
+    uu = u;
+    vv = v;
+  }
+
+  // Distortion
+  T du, dv;
+  Distortion(&params[4], uu, vv, &du, &dv);
+  *x = uu + du;
+  *y = vv + dv;
+
+  // Transform to image coordinates
+  *x = f1 * *x + c1;
+  *y = f2 * *y + c2;
+}
+
+template <typename T>
+void ThinPrismFisheyeCameraModel::ImageToWorld(const T* params, const T x,
+                                               const T y, T* u, T* v) {
+  const T f1 = params[0];
+  const T f2 = params[1];
+  const T c1 = params[2];
+  const T c2 = params[3];
+
+  // Lift points to normalized plane
+  *u = (x - c1) / f1;
+  *v = (y - c2) / f2;
+
+  IterativeUndistortion(&params[4], u, v);
+
+  const T theta = ceres::sqrt(*u * *u + *v * *v);
+  const T theta_cos_theta = theta * ceres::cos(theta);
+  if (theta_cos_theta > T(std::numeric_limits<double>::epsilon())) {
+    const T scale = ceres::sin(theta) / theta_cos_theta;
+    *u *= scale;
+    *v *= scale;
+  }
+}
+
+template <typename T>
+void ThinPrismFisheyeCameraModel::Distortion(const T* extra_params, const T u,
+                                             const T v, T* du, T* dv) {
+  const T k1 = extra_params[0];
+  const T k2 = extra_params[1];
+  const T p1 = extra_params[2];
+  const T p2 = extra_params[3];
+  const T k3 = extra_params[4];
+  const T k4 = extra_params[5];
+  const T sx1 = extra_params[6];
+  const T sy1 = extra_params[7];
+
+  const T u2 = u * u;
+  const T uv = u * v;
+  const T v2 = v * v;
+  const T r2 = u2 + v2;
+  const T r4 = r2 * r2;
+  const T r6 = r4 * r2;
+  const T r8 = r6 * r2;
+  const T radial = k1 * r2 + k2 * r4 + k3 * r6 + k4 * r8;
+  *du = u * radial + T(2) * p1 * uv + p2 * (r2 + T(2) * u2) + sx1 * r2;
+  *dv = v * radial + T(2) * p2 * uv + p1 * (r2 + T(2) * v2) + sy1 * r2;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+
+void CameraModelWorldToImage(const int model_id,
+                             const std::vector<double>& params, const double u,
+                             const double v, double* x, double* y) {
+  switch (model_id) {
+#define CAMERA_MODEL_CASE(CameraModel)                    \
+  case CameraModel::kModelId:                             \
+    CameraModel::WorldToImage(params.data(), u, v, x, y); \
+    break;
+
+    CAMERA_MODEL_SWITCH_CASES
+
+#undef CAMERA_MODEL_CASE
+  }
+}
+
+void CameraModelImageToWorld(const int model_id,
+                             const std::vector<double>& params, const double x,
+                             const double y, double* u, double* v) {
+  switch (model_id) {
+#define CAMERA_MODEL_CASE(CameraModel)                    \
+  case CameraModel::kModelId:                             \
+    CameraModel::ImageToWorld(params.data(), x, y, u, v); \
+    break;
+
+    CAMERA_MODEL_SWITCH_CASES
+
+#undef CAMERA_MODEL_CASE
+  }
+}
+
+double CameraModelImageToWorldThreshold(const int model_id,
+                                        const std::vector<double>& params,
+                                        const double threshold) {
+  switch (model_id) {
+#define CAMERA_MODEL_CASE(CameraModel)                                   \
+  case CameraModel::kModelId:                                            \
+    return CameraModel::ImageToWorldThreshold(params.data(), threshold); \
+    break;
+
+    CAMERA_MODEL_SWITCH_CASES
+
+#undef CAMERA_MODEL_CASE
+  }
+
+  return -1;
+}
+
+}  // namespace colmap
+
+#endif  // COLMAP_SRC_BASE_CAMERA_MODELS_H_

colmap/include/colmap/base/camera_rig.h

@@ -0,0 +1,128 @@
+// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//
+//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
+//       its contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
+
+#ifndef COLMAP_SRC_BASE_CAMERA_RIG_H_
+#define COLMAP_SRC_BASE_CAMERA_RIG_H_
+
+#include <unordered_map>
+#include <vector>
+
+#include "base/camera.h"
+#include "base/pose.h"
+#include "base/reconstruction.h"
+#include "util/alignment.h"
+#include "util/types.h"
+
+namespace colmap {
+
+// This class holds information about the relative configuration of camera rigs.
+// Camera rigs are composed of multiple cameras with a rigid relative extrinsic
+// configuration over multiple snapshots. Snapshots are defined as the
+// collection of images captured simultaneously by all cameras in the rig.
+class CameraRig {
+ public:
+  CameraRig();
+
+  // The number of cameras in the rig.
+  size_t NumCameras() const;
+
+  // The number of snapshots captured by this rig.
+  size_t NumSnapshots() const;
+
+  // Check whether the given camera is part of the rig.
+  bool HasCamera(const camera_t camera_id) const;
+
+  // Access the reference camera.
+  camera_t RefCameraId() const;
+  void SetRefCameraId(const camera_t camera_id);
+
+  // Get the identifiers of the cameras in the rig.
+  std::vector<camera_t> GetCameraIds() const;
+
+  // Get the snapshots of the camera rig.
+  const std::vector<std::vector<image_t>>& Snapshots() const;
+
+  // Add a new camera to the rig. The relative pose may contain dummy values and
+  // can then be computed automatically from a given reconstruction using the
+  // method `ComputeRelativePoses`.
+  void AddCamera(const camera_t camera_id, const Eigen::Vector4d& rel_qvec,
+                 const Eigen::Vector3d& rel_tvec);
+
+  // Add the images of a single snapshot to rig. A snapshot consists of the
+  // captured images of all cameras of the rig. All images of a snapshot share
+  // the same global camera rig pose, i.e. all images in the camera rig are
+  // captured simultaneously.
+  void AddSnapshot(const std::vector<image_t>& image_ids);
+
+  // Check whether the camera rig setup is valid.
+  void Check(const Reconstruction& reconstruction) const;
+
+  // Get the relative poses of the cameras in the rig.
+  Eigen::Vector4d& RelativeQvec(const camera_t camera_id);
+  const Eigen::Vector4d& RelativeQvec(const camera_t camera_id) const;
+  Eigen::Vector3d& RelativeTvec(const camera_t camera_id);
+  const Eigen::Vector3d& RelativeTvec(const camera_t camera_id) const;
+
+  // Compute the scaling factor from the reconstruction to the camera rig
+  // dimensions by averaging over the distances between the projection centers.
+  // Note that this assumes that there is at least one camera pair in the rig
+  // with non-zero baseline, otherwise the function returns NaN.
+  double ComputeScale(const Reconstruction& reconstruction) const;
+
+  // Compute the relative poses in the rig from the reconstruction by averaging
+  // the relative poses over all snapshots. The pose of the reference camera
+  // will be the identity transformation. This assumes that the camera rig has
+  // snapshots that are registered in the reconstruction.
+  bool ComputeRelativePoses(const Reconstruction& reconstruction);
+
+  // Compute the absolute camera pose of the rig. The absolute camera pose of
+  // the rig is computed as the average of all relative camera poses in the rig
+  // and their corresponding image poses in the reconstruction.
+  void ComputeAbsolutePose(const size_t snapshot_idx,
+                           const Reconstruction& reconstruction,
+                           Eigen::Vector4d* abs_qvec,
+                           Eigen::Vector3d* abs_tvec) const;
+
+ private:
+  struct RigCamera {
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    Eigen::Vector4d rel_qvec = ComposeIdentityQuaternion();
+    Eigen::Vector3d rel_tvec = Eigen::Vector3d(0, 0, 0);
+  };
+
+  camera_t ref_camera_id_ = kInvalidCameraId;
+  EIGEN_STL_UMAP(camera_t, RigCamera) rig_cameras_;
+  std::vector<std::vector<image_t>> snapshots_;
+};
+
+}  // namespace colmap
+
+#endif  // COLMAP_SRC_BASE_CAMERA_RIG_H_

colmap/include/colmap/base/correspondence_graph.h

@@ -0,0 +1,206 @@
+// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//
+//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
+//       its contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
+
+#ifndef COLMAP_SRC_BASE_CORRESPONDENCE_GRAPH_H_
+#define COLMAP_SRC_BASE_CORRESPONDENCE_GRAPH_H_
+
+#include <unordered_map>
+#include <vector>
+
+#include "base/database.h"
+#include "util/types.h"
+
+namespace colmap {
+
+// Scene graph represents the graph of image to image and feature to feature
+// correspondences of a dataset. It should be accessed from the DatabaseCache.
+class CorrespondenceGraph {
+ public:
+  struct Correspondence {
+    Correspondence()
+        : image_id(kInvalidImageId), point2D_idx(kInvalidPoint2DIdx) {}
+    Correspondence(const image_t image_id, const point2D_t point2D_idx)
+        : image_id(image_id), point2D_idx(point2D_idx) {}
+
+    // The identifier of the corresponding image.
+    image_t image_id;
+
+    // The index of the corresponding point in the corresponding image.
+    point2D_t point2D_idx;
+  };
+
+  CorrespondenceGraph();
+
+  // Number of added images.
+  inline size_t NumImages() const;
+
+  // Number of added images.
+  inline size_t NumImagePairs() const;
+
+  // Check whether image exists.
+  inline bool ExistsImage(const image_t image_id) const;
+
+  // Get the number of observations in an image. An observation is an image
+  // point that has at least one correspondence.
+  inline point2D_t NumObservationsForImage(const image_t image_id) const;
+
+  // Get the number of correspondences per image.
+  inline point2D_t NumCorrespondencesForImage(const image_t image_id) const;
+
+  // Get the number of correspondences between a pair of images.
+  inline point2D_t NumCorrespondencesBetweenImages(
+      const image_t image_id1, const image_t image_id2) const;
+
+  // Get the number of correspondences between all images.
+  std::unordered_map<image_pair_t, point2D_t> NumCorrespondencesBetweenImages()
+      const;
+
+  // Finalize the database manager.
+  //
+  // - Calculates the number of observations per image by counting the number
+  //   of image points that have at least one correspondence.
+  // - Deletes images without observations, as they are useless for SfM.
+  // - Shrinks the correspondence vectors to their size to save memory.
+  void Finalize();
+
+  // Add new image to the correspondence graph.
+  void AddImage(const image_t image_id, const size_t num_points2D);
+
+  // Add correspondences between images. This function ignores invalid
+  // correspondences where the point indices are out of bounds or duplicate
+  // correspondences between the same image points. Whenever either of the two
+  // cases occur this function prints a warning to the standard output.
+  void AddCorrespondences(const image_t image_id1, const image_t image_id2,
+                          const FeatureMatches& matches);
+
+  // Find the correspondence of an image observation to all other images.
+  inline const std::vector<Correspondence>& FindCorrespondences(
+      const image_t image_id, const point2D_t point2D_idx) const;
+
+  // Find correspondences to the given observation.
+  //
+  // Transitively collects correspondences to the given observation by first
+  // finding correspondences to the given observation, then looking for
+  // correspondences to the collected correspondences in the first step, and so
+  // forth until the transitivity is exhausted or no more correspondences are
+  // found. The returned list does not contain duplicates and contains
+  // the given observation.
+  void FindTransitiveCorrespondences(
+      const image_t image_id, const point2D_t point2D_idx,
+      const size_t transitivity,
+      std::vector<Correspondence>* found_corrs) const;
+
+  // Find all correspondences between two images.
+  FeatureMatches FindCorrespondencesBetweenImages(
+      const image_t image_id1, const image_t image_id2) const;
+
+  // Check whether the image point has correspondences.
+  inline bool HasCorrespondences(const image_t image_id,
+                                 const point2D_t point2D_idx) const;
+
+  // Check whether the given observation is part of a two-view track, i.e.
+  // it only has one correspondence and that correspondence has the given
+  // observation as its only correspondence.
+  bool IsTwoViewObservation(const image_t image_id,
+                            const point2D_t point2D_idx) const;
+
+ private:
+  struct Image {
+    // Number of 2D points with at least one correspondence to another image.
+    point2D_t num_observations = 0;
+
+    // Total number of correspondences to other images. This measure is useful
+    // to find a good initial pair, that is connected to many images.
+    point2D_t num_correspondences = 0;
+
+    // Correspondences to other images per image point.
+    std::vector<std::vector<Correspondence>> corrs;
+  };
+
+  struct ImagePair {
+    // The number of correspondences between pairs of images.
+    point2D_t num_correspondences = 0;
+  };
+
+  EIGEN_STL_UMAP(image_t, Image) images_;
+  std::unordered_map<image_pair_t, ImagePair> image_pairs_;
+};
+
+////////////////////////////////////////////////////////////////////////////////
+// Implementation
+////////////////////////////////////////////////////////////////////////////////
+
+size_t CorrespondenceGraph::NumImages() const { return images_.size(); }
+
+size_t CorrespondenceGraph::NumImagePairs() const {
+  return image_pairs_.size();
+}
+
+bool CorrespondenceGraph::ExistsImage(const image_t image_id) const {
+  return images_.find(image_id) != images_.end();
+}
+
+point2D_t CorrespondenceGraph::NumObservationsForImage(
+    const image_t image_id) const {
+  return images_.at(image_id).num_observations;
+}
+
+point2D_t CorrespondenceGraph::NumCorrespondencesForImage(
+    const image_t image_id) const {
+  return images_.at(image_id).num_correspondences;
+}
+
+point2D_t CorrespondenceGraph::NumCorrespondencesBetweenImages(
+    const image_t image_id1, const image_t image_id2) const {
+  const image_pair_t pair_id =
+      Database::ImagePairToPairId(image_id1, image_id2);
+  const auto it = image_pairs_.find(pair_id);
+  if (it == image_pairs_.end()) {
+    return 0;
+  } else {
+    return static_cast<point2D_t>(it->second.num_correspondences);
+  }
+}
+
+const std::vector<CorrespondenceGraph::Correspondence>&
+CorrespondenceGraph::FindCorrespondences(const image_t image_id,
+                                         const point2D_t point2D_idx) const {
+  return images_.at(image_id).corrs.at(point2D_idx);
+}
+
+bool CorrespondenceGraph::HasCorrespondences(
+    const image_t image_id, const point2D_t point2D_idx) const {
+  return !images_.at(image_id).corrs.at(point2D_idx).empty();
+}
+
+}  // namespace colmap
+
+#endif  // COLMAP_SRC_BASE_CORRESPONDENCE_GRAPH_H_

colmap/include/colmap/base/cost_functions.h

@@ -0,0 +1,301 @@
+// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//
+//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
+//       its contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
+
+#ifndef COLMAP_SRC_BASE_COST_FUNCTIONS_H_
+#define COLMAP_SRC_BASE_COST_FUNCTIONS_H_
+
+#include <Eigen/Core>
+
+#include <ceres/ceres.h>
+#include <ceres/rotation.h>
+
+namespace colmap {
+
+// Standard bundle adjustment cost function for variable
+// camera pose and calibration and point parameters.
+template <typename CameraModel>
+class BundleAdjustmentCostFunction {
+ public:
+  explicit BundleAdjustmentCostFunction(const Eigen::Vector2d& point2D)
+      : observed_x_(point2D(0)), observed_y_(point2D(1)) {}
+
+  static ceres::CostFunction* Create(const Eigen::Vector2d& point2D) {
+    return (new ceres::AutoDiffCostFunction<
+            BundleAdjustmentCostFunction<CameraModel>, 2, 4, 3, 3,
+            CameraModel::kNumParams>(
+        new BundleAdjustmentCostFunction(point2D)));
+  }
+
+  template <typename T>
+  bool operator()(const T* const qvec, const T* const tvec,
+                  const T* const point3D, const T* const camera_params,
+                  T* residuals) const {
+    // Rotate and translate.
+    T projection[3];
+    ceres::UnitQuaternionRotatePoint(qvec, point3D, projection);
+    projection[0] += tvec[0];
+    projection[1] += tvec[1];
+    projection[2] += tvec[2];
+
+    // Project to image plane.
+    projection[0] /= projection[2];
+    projection[1] /= projection[2];
+
+    // Distort and transform to pixel space.
+    CameraModel::WorldToImage(camera_params, projection[0], projection[1],
+                              &residuals[0], &residuals[1]);
+
+    // Re-projection error.
+    residuals[0] -= T(observed_x_);
+    residuals[1] -= T(observed_y_);
+
+    return true;
+  }
+
+ private:
+  const double observed_x_;
+  const double observed_y_;
+};
+
+// Bundle adjustment cost function for variable
+// camera calibration and point parameters, and fixed camera pose.
+template <typename CameraModel>
+class BundleAdjustmentConstantPoseCostFunction {
+ public:
+  BundleAdjustmentConstantPoseCostFunction(const Eigen::Vector4d& qvec,
+                                           const Eigen::Vector3d& tvec,
+                                           const Eigen::Vector2d& point2D)
+      : qw_(qvec(0)),
+        qx_(qvec(1)),
+        qy_(qvec(2)),
+        qz_(qvec(3)),
+        tx_(tvec(0)),
+        ty_(tvec(1)),
+        tz_(tvec(2)),
+        observed_x_(point2D(0)),
+        observed_y_(point2D(1)) {}
+
+  static ceres::CostFunction* Create(const Eigen::Vector4d& qvec,
+                                     const Eigen::Vector3d& tvec,
+                                     const Eigen::Vector2d& point2D) {
+    return (new ceres::AutoDiffCostFunction<
+            BundleAdjustmentConstantPoseCostFunction<CameraModel>, 2, 3,
+            CameraModel::kNumParams>(
+        new BundleAdjustmentConstantPoseCostFunction(qvec, tvec, point2D)));
+  }
+
+  template <typename T>
+  bool operator()(const T* const point3D, const T* const camera_params,
+                  T* residuals) const {
+    const T qvec[4] = {T(qw_), T(qx_), T(qy_), T(qz_)};
+
+    // Rotate and translate.
+    T projection[3];
+    ceres::UnitQuaternionRotatePoint(qvec, point3D, projection);
+    projection[0] += T(tx_);
+    projection[1] += T(ty_);
+    projection[2] += T(tz_);
+
+    // Project to image plane.
+    projection[0] /= projection[2];
+    projection[1] /= projection[2];
+
+    // Distort and transform to pixel space.
+    CameraModel::WorldToImage(camera_params, projection[0], projection[1],
+                              &residuals[0], &residuals[1]);
+
+    // Re-projection error.
+    residuals[0] -= T(observed_x_);
+    residuals[1] -= T(observed_y_);
+
+    return true;
+  }
+
+ private:
+  const double qw_;
+  const double qx_;
+  const double qy_;
+  const double qz_;
+  const double tx_;
+  const double ty_;
+  const double tz_;
+  const double observed_x_;
+  const double observed_y_;
+};
+
+// Rig bundle adjustment cost function for variable camera pose and calibration
+// and point parameters. Different from the standard bundle adjustment function,
+// this cost function is suitable for camera rigs with consistent relative poses
+// of the cameras within the rig. The cost function first projects points into
+// the local system of the camera rig and then into the local system of the
+// camera within the rig.
+template <typename CameraModel>
+class RigBundleAdjustmentCostFunction {
+ public:
+  explicit RigBundleAdjustmentCostFunction(const Eigen::Vector2d& point2D)
+      : observed_x_(point2D(0)), observed_y_(point2D(1)) {}
+
+  static ceres::CostFunction* Create(const Eigen::Vector2d& point2D) {
+    return (new ceres::AutoDiffCostFunction<
+            RigBundleAdjustmentCostFunction<CameraModel>, 2, 4, 3, 4, 3, 3,
+            CameraModel::kNumParams>(
+        new RigBundleAdjustmentCostFunction(point2D)));
+  }
+
+  template <typename T>
+  bool operator()(const T* const rig_qvec, const T* const rig_tvec,
+                  const T* const rel_qvec, const T* const rel_tvec,
+                  const T* const point3D, const T* const camera_params,
+                  T* residuals) const {
+    // Concatenate rotations.
+    T qvec[4];
+    ceres::QuaternionProduct(rel_qvec, rig_qvec, qvec);
+
+    // Concatenate translations.
+    T tvec[3];
+    ceres::UnitQuaternionRotatePoint(rel_qvec, rig_tvec, tvec);
+    tvec[0] += rel_tvec[0];
+    tvec[1] += rel_tvec[1];
+    tvec[2] += rel_tvec[2];
+
+    // Rotate and translate.
+    T projection[3];
+    ceres::UnitQuaternionRotatePoint(qvec, point3D, projection);
+    projection[0] += tvec[0];
+    projection[1] += tvec[1];
+    projection[2] += tvec[2];
+
+    // Project to image plane.
+    projection[0] /= projection[2];
+    projection[1] /= projection[2];
+
+    // Distort and transform to pixel space.
+    CameraModel::WorldToImage(camera_params, projection[0], projection[1],
+                              &residuals[0], &residuals[1]);
+
+    // Re-projection error.
+    residuals[0] -= T(observed_x_);
+    residuals[1] -= T(observed_y_);
+
+    return true;
+  }
+
+ private:
+  const double observed_x_;
+  const double observed_y_;
+};
+
+// Cost function for refining two-view geometry based on the Sampson-Error.
+//
+// First pose is assumed to be located at the origin with 0 rotation. Second
+// pose is assumed to be on the unit sphere around the first pose, i.e. the
+// pose of the second camera is parameterized by a 3D rotation and a
+// 3D translation with unit norm. `tvec` is therefore over-parameterized as is
+// and should be down-projected using `SphereManifold`.
+class RelativePoseCostFunction {
+ public:
+  RelativePoseCostFunction(const Eigen::Vector2d& x1, const Eigen::Vector2d& x2)
+      : x1_(x1(0)), y1_(x1(1)), x2_(x2(0)), y2_(x2(1)) {}
+
+  static ceres::CostFunction* Create(const Eigen::Vector2d& x1,
+                                     const Eigen::Vector2d& x2) {
+    return (new ceres::AutoDiffCostFunction<RelativePoseCostFunction, 1, 4, 3>(
+        new RelativePoseCostFunction(x1, x2)));
+  }
+
+  template <typename T>
+  bool operator()(const T* const qvec, const T* const tvec,
+                  T* residuals) const {
+    Eigen::Matrix<T, 3, 3, Eigen::RowMajor> R;
+    ceres::QuaternionToRotation(qvec, R.data());
+
+    // Matrix representation of the cross product t x R.
+    Eigen::Matrix<T, 3, 3> t_x;
+    t_x << T(0), -tvec[2], tvec[1], tvec[2], T(0), -tvec[0], -tvec[1], tvec[0],
+        T(0);
+
+    // Essential matrix.
+    const Eigen::Matrix<T, 3, 3> E = t_x * R;
+
+    // Homogeneous image coordinates.
+    const Eigen::Matrix<T, 3, 1> x1_h(T(x1_), T(y1_), T(1));
+    const Eigen::Matrix<T, 3, 1> x2_h(T(x2_), T(y2_), T(1));
+
+    // Squared sampson error.
+    const Eigen::Matrix<T, 3, 1> Ex1 = E * x1_h;
+    const Eigen::Matrix<T, 3, 1> Etx2 = E.transpose() * x2_h;
+    const T x2tEx1 = x2_h.transpose() * Ex1;
+    residuals[0] = x2tEx1 * x2tEx1 /
+                   (Ex1(0) * Ex1(0) + Ex1(1) * Ex1(1) + Etx2(0) * Etx2(0) +
+                    Etx2(1) * Etx2(1));
+
+    return true;
+  }
+
+ private:
+  const double x1_;
+  const double y1_;
+  const double x2_;
+  const double y2_;
+};
+
+inline void SetQuaternionManifold(ceres::Problem* problem, double* qvec) {
+#if CERES_VERSION_MAJOR >= 2 && CERES_VERSION_MINOR >= 1
+  problem->SetManifold(qvec, new ceres::QuaternionManifold);
+#else
+  problem->SetParameterization(qvec, new ceres::QuaternionParameterization);
+#endif
+}
+
+inline void SetSubsetManifold(int size, const std::vector<int>& constant_params,
+                              ceres::Problem* problem, double* params) {
+#if CERES_VERSION_MAJOR >= 2 && CERES_VERSION_MINOR >= 1
+  problem->SetManifold(params,
+                       new ceres::SubsetManifold(size, constant_params));
+#else
+  problem->SetParameterization(
+      params, new ceres::SubsetParameterization(size, constant_params));
+#endif
+}
+
+template <int size>
+inline void SetSphereManifold(ceres::Problem* problem, double* params) {
+#if CERES_VERSION_MAJOR >= 2 && CERES_VERSION_MINOR >= 1
+  problem->SetManifold(params, new ceres::SphereManifold<size>);
+#else
+  problem->SetParameterization(
+      params, new ceres::HomogeneousVectorParameterization(size));
+#endif
+}
+
+}  // namespace colmap
+
+#endif  // COLMAP_SRC_BASE_COST_FUNCTIONS_H_

colmap/include/colmap/base/database.h

@@ -0,0 +1,394 @@
+// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//
+//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
+//       its contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
+
+#ifndef COLMAP_SRC_BASE_DATABASE_H_
+#define COLMAP_SRC_BASE_DATABASE_H_
+
+#include <mutex>
+#include <unordered_map>
+#include <vector>
+
+#include <Eigen/Core>
+#include <sqlite3.h>
+
+#include "base/camera.h"
+#include "base/image.h"
+#include "estimators/two_view_geometry.h"
+#include "feature/types.h"
+#include "util/types.h"
+
+namespace colmap {
+
+// Database class to read and write images, features, cameras, matches, etc.
+// from a SQLite database. The class is not thread-safe and must not be accessed
+// concurrently. The class is optimized for single-thread speed and for optimal
+// performance, wrap multiple method calls inside a leading `BeginTransaction`
+// and trailing `EndTransaction`.
+class Database {
+ public:
+  const static int kSchemaVersion = 1;
+
+  // The maximum number of images, that can be stored in the database.
+  // This limitation arises due to the fact, that we generate unique IDs for
+  // image pairs manually. Note: do not change this to
+  // another type than `size_t`.
+  const static size_t kMaxNumImages;
+
+  Database();
+  explicit Database(const std::string& path);
+  ~Database();
+
+  // Open and close database. The same database should not be opened
+  // concurrently in multiple threads or processes.
+  void Open(const std::string& path);
+  void Close();
+
+  // Check if entry already exists in database. For image pairs, the order of
+  // `image_id1` and `image_id2` does not matter.
+  bool ExistsCamera(const camera_t camera_id) const;
+  bool ExistsImage(const image_t image_id) const;
+  bool ExistsImageWithName(std::string name) const;
+  bool ExistsKeypoints(const image_t image_id) const;
+  bool ExistsDescriptors(const image_t image_id) const;
+  bool ExistsMatches(const image_t image_id1, const image_t image_id2) const;
+  bool ExistsInlierMatches(const image_t image_id1,
+                           const image_t image_id2) const;
+
+  // Number of rows in `cameras` table.
+  size_t NumCameras() const;
+
+  //  Number of rows in `images` table.
+  size_t NumImages() const;
+
+  // Sum of `rows` column in `keypoints` table, i.e. number of total keypoints.
+  size_t NumKeypoints() const;
+
+  // The number of keypoints for the image with most features.
+  size_t MaxNumKeypoints() const;
+
+  // Number of descriptors for specific image.
+  size_t NumKeypointsForImage(const image_t image_id) const;
+
+  // Sum of `rows` column in `descriptors` table,
+  // i.e. number of total descriptors.
+  size_t NumDescriptors() const;
+
+  // The number of descriptors for the image with most features.
+  size_t MaxNumDescriptors() const;
+
+  // Number of descriptors for specific image.
+  size_t NumDescriptorsForImage(const image_t image_id) const;
+
+  // Sum of `rows` column in `matches` table, i.e. number of total matches.
+  size_t NumMatches() const;
+
+  // Sum of `rows` column in `two_view_geometries` table,
+  // i.e. number of total inlier matches.
+  size_t NumInlierMatches() const;
+
+  // Number of rows in `matches` table.
+  size_t NumMatchedImagePairs() const;
+
+  // Number of rows in `two_view_geometries` table.
+  size_t NumVerifiedImagePairs() const;
+
+  // Each image pair is assigned an unique ID in the `matches` and
+  // `two_view_geometries` table. We intentionally avoid to store the pairs in a
+  // separate table by using e.g. AUTOINCREMENT, since the overhead of querying
+  // the unique pair ID is significant.
+  inline static image_pair_t ImagePairToPairId(const image_t image_id1,
+                                               const image_t image_id2);
+
+  inline static void PairIdToImagePair(const image_pair_t pair_id,
+                                       image_t* image_id1, image_t* image_id2);
+
+  // Return true if image pairs should be swapped. Used to enforce a specific
+  // image order to generate unique image pair identifiers independent of the
+  // order in which the image identifiers are used.
+  inline static bool SwapImagePair(const image_t image_id1,
+                                   const image_t image_id2);
+
+  // Read an existing entry in the database. The user is responsible for making
+  // sure that the entry actually exists. For image pairs, the order of
+  // `image_id1` and `image_id2` does not matter.
+  Camera ReadCamera(const camera_t camera_id) const;
+  std::vector<Camera> ReadAllCameras() const;
+
+  Image ReadImage(const image_t image_id) const;
+  Image ReadImageWithName(const std::string& name) const;
+  std::vector<Image> ReadAllImages() const;
+
+  FeatureKeypoints ReadKeypoints(const image_t image_id) const;
+  FeatureDescriptors ReadDescriptors(const image_t image_id) const;
+
+  FeatureMatches ReadMatches(const image_t image_id1,
+                             const image_t image_id2) const;
+  std::vector<std::pair<image_pair_t, FeatureMatches>> ReadAllMatches() const;
+
+  TwoViewGeometry ReadTwoViewGeometry(const image_t image_id1,
+                                      const image_t image_id2) const;
+  void ReadTwoViewGeometries(
+      std::vector<image_pair_t>* image_pair_ids,
+      std::vector<TwoViewGeometry>* two_view_geometries) const;
+
+  // Read all image pairs that have an entry in the `NumVerifiedImagePairs`
+  // table with at least one inlier match and their number of inlier matches.
+  void ReadTwoViewGeometryNumInliers(
+      std::vector<std::pair<image_t, image_t>>* image_pairs,
+      std::vector<int>* num_inliers) const;
+
+  // Add new camera and return its database identifier. If `use_camera_id`
+  // is false a new identifier is automatically generated.
+  camera_t WriteCamera(const Camera& camera,
+                       const bool use_camera_id = false) const;
+
+  // Add new image and return its database identifier. If `use_image_id`
+  // is false a new identifier is automatically generated.
+  image_t WriteImage(const Image& image, const bool use_image_id = false) const;
+
+  // Write a new entry in the database. The user is responsible for making sure
+  // that the entry does not yet exist. For image pairs, the order of
+  // `image_id1` and `image_id2` does not matter.
+  void WriteKeypoints(const image_t image_id,
+                      const FeatureKeypoints& keypoints) const;
+  void WriteDescriptors(const image_t image_id,
+                        const FeatureDescriptors& descriptors) const;
+  void WriteMatches(const image_t image_id1, const image_t image_id2,
+                    const FeatureMatches& matches) const;
+  void WriteTwoViewGeometry(const image_t image_id1, const image_t image_id2,
+                            const TwoViewGeometry& two_view_geometry) const;
+
+  // Update an existing camera in the database. The user is responsible for
+  // making sure that the entry already exists.
+  void UpdateCamera(const Camera& camera) const;
+
+  // Update an existing image in the database. The user is responsible for
+  // making sure that the entry already exists.
+  void UpdateImage(const Image& image) const;
+
+  // Delete matches of an image pair.
+  void DeleteMatches(const image_t image_id1, const image_t image_id2) const;
+
+  // Delete inlier matches of an image pair.
+  void DeleteInlierMatches(const image_t image_id1,
+                           const image_t image_id2) const;
+
+  // Clear all database tables
+  void ClearAllTables() const;
+
+  // Clear the entire cameras table
+  void ClearCameras() const;
+
+  // Clear the entire images, keypoints, and descriptors tables
+  void ClearImages() const;
+
+  // Clear the entire descriptors table
+  void ClearDescriptors() const;
+
+  // Clear the entire keypoints table
+  void ClearKeypoints() const;
+
+  // Clear the entire matches table.
+  void ClearMatches() const;
+
+  // Clear the entire inlier matches table.
+  void ClearTwoViewGeometries() const;
+
+  // Merge two databases into a single, new database.
+  static void Merge(const Database& database1, const Database& database2,
+                    Database* merged_database);
+
+ private:
+  friend class DatabaseTransaction;
+
+  // Combine multiple queries into one transaction by wrapping a code section
+  // into a `BeginTransaction` and `EndTransaction`. You can create a scoped
+  // transaction with `DatabaseTransaction` that ends when the transaction
+  // object is destructed. Combining queries results in faster transaction time
+  // due to reduced locking of the database etc.
+  void BeginTransaction() const;
+  void EndTransaction() const;
+
+  // Prepare SQL statements once at construction of the database, and reuse
+  // the statements for multiple queries by resetting their states.
+  void PrepareSQLStatements();
+  void FinalizeSQLStatements();
+
+  // Create database tables, if not existing, called when opening a database.
+  void CreateTables() const;
+  void CreateCameraTable() const;
+  void CreateImageTable() const;
+  void CreateKeypointsTable() const;
+  void CreateDescriptorsTable() const;
+  void CreateMatchesTable() const;
+  void CreateTwoViewGeometriesTable() const;
+
+  void UpdateSchema() const;
+
+  bool ExistsTable(const std::string& table_name) const;
+  bool ExistsColumn(const std::string& table_name,
+                    const std::string& column_name) const;
+
+  bool ExistsRowId(sqlite3_stmt* sql_stmt, const sqlite3_int64 row_id) const;
+  bool ExistsRowString(sqlite3_stmt* sql_stmt,
+                       const std::string& row_entry) const;
+
+  size_t CountRows(const std::string& table) const;
+  size_t CountRowsForEntry(sqlite3_stmt* sql_stmt,
+                           const sqlite3_int64 row_id) const;
+  size_t SumColumn(const std::string& column, const std::string& table) const;
+  size_t MaxColumn(const std::string& column, const std::string& table) const;
+
+  sqlite3* database_ = nullptr;
+
+  // Check if elements got removed from the database to only apply
+  // the VACUUM command in such case
+  mutable bool database_cleared_ = false;
+
+  // Ensure that only one database object at a time updates the schema of a
+  // database. Since the schema is updated every time a database is opened, this
+  // is to ensure that there are no race conditions ("database locked" error
+  // messages) when the user actually only intends to read from the database,
+  // which requires to open it.
+  static std::mutex update_schema_mutex_;
+
+  // Used to ensure that only one transaction is active at the same time.
+  std::mutex transaction_mutex_;
+
+  // A collection of all `sqlite3_stmt` objects for deletion in the destructor.
+  std::vector<sqlite3_stmt*> sql_stmts_;
+
+  // num_*
+  sqlite3_stmt* sql_stmt_num_keypoints_ = nullptr;
+  sqlite3_stmt* sql_stmt_num_descriptors_ = nullptr;
+
+  // exists_*
+  sqlite3_stmt* sql_stmt_exists_camera_ = nullptr;
+  sqlite3_stmt* sql_stmt_exists_image_id_ = nullptr;
+  sqlite3_stmt* sql_stmt_exists_image_name_ = nullptr;
+  sqlite3_stmt* sql_stmt_exists_keypoints_ = nullptr;
+  sqlite3_stmt* sql_stmt_exists_descriptors_ = nullptr;
+  sqlite3_stmt* sql_stmt_exists_matches_ = nullptr;
+  sqlite3_stmt* sql_stmt_exists_two_view_geometry_ = nullptr;
+
+  // add_*
+  sqlite3_stmt* sql_stmt_add_camera_ = nullptr;
+  sqlite3_stmt* sql_stmt_add_image_ = nullptr;
+
+  // update_*
+  sqlite3_stmt* sql_stmt_update_camera_ = nullptr;
+  sqlite3_stmt* sql_stmt_update_image_ = nullptr;
+
+  // read_*
+  sqlite3_stmt* sql_stmt_read_camera_ = nullptr;
+  sqlite3_stmt* sql_stmt_read_cameras_ = nullptr;
+  sqlite3_stmt* sql_stmt_read_image_id_ = nullptr;
+  sqlite3_stmt* sql_stmt_read_image_name_ = nullptr;
+  sqlite3_stmt* sql_stmt_read_images_ = nullptr;
+  sqlite3_stmt* sql_stmt_read_keypoints_ = nullptr;
+  sqlite3_stmt* sql_stmt_read_descriptors_ = nullptr;
+  sqlite3_stmt* sql_stmt_read_matches_ = nullptr;
+  sqlite3_stmt* sql_stmt_read_matches_all_ = nullptr;
+  sqlite3_stmt* sql_stmt_read_two_view_geometry_ = nullptr;
+  sqlite3_stmt* sql_stmt_read_two_view_geometries_ = nullptr;
+  sqlite3_stmt* sql_stmt_read_two_view_geometry_num_inliers_ = nullptr;
+
+  // write_*
+  sqlite3_stmt* sql_stmt_write_keypoints_ = nullptr;
+  sqlite3_stmt* sql_stmt_write_descriptors_ = nullptr;
+  sqlite3_stmt* sql_stmt_write_matches_ = nullptr;
+  sqlite3_stmt* sql_stmt_write_two_view_geometry_ = nullptr;
+
+  // delete_*
+  sqlite3_stmt* sql_stmt_delete_matches_ = nullptr;
+  sqlite3_stmt* sql_stmt_delete_two_view_geometry_ = nullptr;
+
+  // clear_*
+  sqlite3_stmt* sql_stmt_clear_cameras_ = nullptr;
+  sqlite3_stmt* sql_stmt_clear_images_ = nullptr;
+  sqlite3_stmt* sql_stmt_clear_descriptors_ = nullptr;
+  sqlite3_stmt* sql_stmt_clear_keypoints_ = nullptr;
+  sqlite3_stmt* sql_stmt_clear_matches_ = nullptr;
+  sqlite3_stmt* sql_stmt_clear_two_view_geometries_ = nullptr;
+};
+
+// This class automatically manages the scope of a database transaction by
+// calling `BeginTransaction` and `EndTransaction` during construction and
+// destruction, respectively.
+class DatabaseTransaction {
+ public:
+  explicit DatabaseTransaction(Database* database);
+  ~DatabaseTransaction();
+
+ private:
+  NON_COPYABLE(DatabaseTransaction)
+  NON_MOVABLE(DatabaseTransaction)
+  Database* database_;
+  std::unique_lock<std::mutex> database_lock_;
+};
+
+////////////////////////////////////////////////////////////////////////////////
+// Implementation
+////////////////////////////////////////////////////////////////////////////////
+
+image_pair_t Database::ImagePairToPairId(const image_t image_id1,
+                                         const image_t image_id2) {
+  CHECK_GE(image_id1, 0);
+  CHECK_GE(image_id2, 0);
+  CHECK_LT(image_id1, kMaxNumImages);
+  CHECK_LT(image_id2, kMaxNumImages);
+  if (SwapImagePair(image_id1, image_id2)) {
+    return static_cast<image_pair_t>(kMaxNumImages) * image_id2 + image_id1;
+  } else {
+    return static_cast<image_pair_t>(kMaxNumImages) * image_id1 + image_id2;
+  }
+}
+
+void Database::PairIdToImagePair(const image_pair_t pair_id, image_t* image_id1,
+                                 image_t* image_id2) {
+  *image_id2 = static_cast<image_t>(pair_id % kMaxNumImages);
+  *image_id1 = static_cast<image_t>((pair_id - *image_id2) / kMaxNumImages);
+  CHECK_GE(*image_id1, 0);
+  CHECK_GE(*image_id2, 0);
+  CHECK_LT(*image_id1, kMaxNumImages);
+  CHECK_LT(*image_id2, kMaxNumImages);
+}
+
+// Return true if image pairs should be swapped. Used to enforce a specific
+// image order to generate unique image pair identifiers independent of the
+// order in which the image identifiers are used.
+bool Database::SwapImagePair(const image_t image_id1, const image_t image_id2) {
+  return image_id1 > image_id2;
+}
+
+}  // namespace colmap
+
+#endif  // COLMAP_SRC_BASE_DATABASE_H_

colmap/include/colmap/base/database_cache.h

@@ -0,0 +1,151 @@
+// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//
+//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
+//       its contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
+
+#ifndef COLMAP_SRC_BASE_DATABASE_CACHE_H_
+#define COLMAP_SRC_BASE_DATABASE_CACHE_H_
+
+#include <string>
+#include <unordered_map>
+#include <unordered_set>
+#include <vector>
+
+#include <Eigen/Core>
+
+#include "base/camera.h"
+#include "base/camera_models.h"
+#include "base/correspondence_graph.h"
+#include "base/database.h"
+#include "base/image.h"
+#include "util/alignment.h"
+#include "util/types.h"
+
+namespace colmap {
+
+// A class that caches the contents of the database in memory, used to quickly
+// create new reconstruction instances when multiple models are reconstructed.
+class DatabaseCache {
+ public:
+  DatabaseCache();
+
+  // Get number of objects.
+  inline size_t NumCameras() const;
+  inline size_t NumImages() const;
+
+  // Get specific objects.
+  inline class Camera& Camera(const camera_t camera_id);
+  inline const class Camera& Camera(const camera_t camera_id) const;
+  inline class Image& Image(const image_t image_id);
+  inline const class Image& Image(const image_t image_id) const;
+
+  // Get all objects.
+  inline const EIGEN_STL_UMAP(camera_t, class Camera) & Cameras() const;
+  inline const EIGEN_STL_UMAP(image_t, class Image) & Images() const;
+
+  // Check whether specific object exists.
+  inline bool ExistsCamera(const camera_t camera_id) const;
+  inline bool ExistsImage(const image_t image_id) const;
+
+  // Get reference to correspondence graph.
+  inline const class CorrespondenceGraph& CorrespondenceGraph() const;
+
+  // Manually add data to cache.
+  void AddCamera(class Camera camera);
+  void AddImage(class Image image);
+
+  // Load cameras, images, features, and matches from database.
+  //
+  // @param database              Source database from which to load data.
+  // @param min_num_matches       Only load image pairs with a minimum number
+  //                              of matches.
+  // @param ignore_watermarks     Whether to ignore watermark image pairs.
+  // @param image_names           Whether to use only load the data for a subset
+  //                              of the images. All images are used if empty.
+  void Load(const Database& database, const size_t min_num_matches,
+            const bool ignore_watermarks,
+            const std::unordered_set<std::string>& image_names);
+
+  // Find specific image by name. Note that this uses linear search.
+  const class Image* FindImageWithName(const std::string& name) const;
+
+ private:
+  class CorrespondenceGraph correspondence_graph_;
+
+  EIGEN_STL_UMAP(camera_t, class Camera) cameras_;
+  EIGEN_STL_UMAP(image_t, class Image) images_;
+};
+
+////////////////////////////////////////////////////////////////////////////////
+// Implementation
+////////////////////////////////////////////////////////////////////////////////
+
+size_t DatabaseCache::NumCameras() const { return cameras_.size(); }
+size_t DatabaseCache::NumImages() const { return images_.size(); }
+
+class Camera& DatabaseCache::Camera(const camera_t camera_id) {
+  return cameras_.at(camera_id);
+}
+
+const class Camera& DatabaseCache::Camera(const camera_t camera_id) const {
+  return cameras_.at(camera_id);
+}
+
+class Image& DatabaseCache::Image(const image_t image_id) {
+  return images_.at(image_id);
+}
+
+const class Image& DatabaseCache::Image(const image_t image_id) const {
+  return images_.at(image_id);
+}
+
+const EIGEN_STL_UMAP(camera_t, class Camera) & DatabaseCache::Cameras() const {
+  return cameras_;
+}
+
+const EIGEN_STL_UMAP(image_t, class Image) & DatabaseCache::Images() const {
+  return images_;
+}
+
+bool DatabaseCache::ExistsCamera(const camera_t camera_id) const {
+  return cameras_.find(camera_id) != cameras_.end();
+}
+
+bool DatabaseCache::ExistsImage(const image_t image_id) const {
+  return images_.find(image_id) != images_.end();
+}
+
+inline const class CorrespondenceGraph& DatabaseCache::CorrespondenceGraph()
+    const {
+  return correspondence_graph_;
+}
+
+}  // namespace colmap
+
+#endif  // COLMAP_SRC_BASE_DATABASE_CACHE_H_

colmap/include/colmap/base/essential_matrix.h

@@ -0,0 +1,160 @@
+// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//
+//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
+//       its contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
+
+#ifndef COLMAP_SRC_BASE_ESSENTIAL_MATRIX_H_
+#define COLMAP_SRC_BASE_ESSENTIAL_MATRIX_H_
+
+#include <vector>
+
+#include <Eigen/Core>
+
+#include <ceres/ceres.h>
+
+#include "util/alignment.h"
+#include "util/types.h"
+
+namespace colmap {
+
+// Decompose an essential matrix into the possible rotations and translations.
+//
+// The first pose is assumed to be P = [I | 0] and the set of four other
+// possible second poses are defined as: {[R1 | t], [R2 | t],
+//                                        [R1 | -t], [R2 | -t]}
+//
+// @param E          3x3 essential matrix.
+// @param R1         First possible 3x3 rotation matrix.
+// @param R2         Second possible 3x3 rotation matrix.
+// @param t          3x1 possible translation vector (also -t possible).
+void DecomposeEssentialMatrix(const Eigen::Matrix3d& E, Eigen::Matrix3d* R1,
+                              Eigen::Matrix3d* R2, Eigen::Vector3d* t);
+
+// Recover the most probable pose from the given essential matrix.
+//
+// The pose of the first image is assumed to be P = [I | 0].
+//
+// @param E            3x3 essential matrix.
+// @param points1      First set of corresponding points.
+// @param points2      Second set of corresponding points.
+// @param inlier_mask  Only points with `true` in the inlier mask are
+//                     considered in the cheirality test. Size of the
+//                     inlier mask must match the number of points N.
+// @param R            Most probable 3x3 rotation matrix.
+// @param t            Most probable 3x1 translation vector.
+// @param points3D     Triangulated 3D points infront of camera.
+void PoseFromEssentialMatrix(const Eigen::Matrix3d& E,
+                             const std::vector<Eigen::Vector2d>& points1,
+                             const std::vector<Eigen::Vector2d>& points2,
+                             Eigen::Matrix3d* R, Eigen::Vector3d* t,
+                             std::vector<Eigen::Vector3d>* points3D);
+
+// Compose essential matrix from relative camera poses.
+//
+// Assumes that first camera pose has projection matrix P = [I | 0], and
+// pose of second camera is given as transformation from world to camera system.
+//
+// @param R             3x3 rotation matrix.
+// @param t             3x1 translation vector.
+//
+// @return              3x3 essential matrix.
+Eigen::Matrix3d EssentialMatrixFromPose(const Eigen::Matrix3d& R,
+                                        const Eigen::Vector3d& t);
+
+// Compose essential matrix from two absolute camera poses.
+//
+// @param proj_matrix1     3x4 projection matrix.
+// @param proj_matrix2     3x4 projection matrix.
+//
+// @return                 3x3 essential matrix.
+Eigen::Matrix3d EssentialMatrixFromAbsolutePoses(
+    const Eigen::Matrix3x4d& proj_matrix1,
+    const Eigen::Matrix3x4d& proj_matrix2);
+
+// Find optimal image points, such that:
+//
+//     optimal_point1^t * E * optimal_point2 = 0
+//
+// as described in:
+//
+//   Lindstrom, P., "Triangulation made easy",
+//   Computer Vision and Pattern Recognition (CVPR),
+//   2010 IEEE Conference on , vol., no., pp.1554,1561, 13-18 June 2010
+//
+// @param E                Essential or fundamental matrix.
+// @param point1           Corresponding 2D point in first image.
+// @param point2           Corresponding 2D point in second image.
+// @param optimal_point1   Estimated optimal image point in the first image.
+// @param optimal_point2   Estimated optimal image point in the second image.
+void FindOptimalImageObservations(const Eigen::Matrix3d& E,
+                                  const Eigen::Vector2d& point1,
+                                  const Eigen::Vector2d& point2,
+                                  Eigen::Vector2d* optimal_point1,
+                                  Eigen::Vector2d* optimal_point2);
+
+// Compute the location of the epipole in homogeneous coordinates.
+//
+// @param E           3x3 essential matrix.
+// @param left_image  If true, epipole in left image is computed,
+//                    else in right image.
+//
+// @return            Epipole in homogeneous coordinates.
+Eigen::Vector3d EpipoleFromEssentialMatrix(const Eigen::Matrix3d& E,
+                                           const bool left_image);
+
+// Invert the essential matrix, i.e. if the essential matrix E describes the
+// transformation from camera A to B, the inverted essential matrix E' describes
+// the transformation from camera B to A.
+//
+// @param E      3x3 essential matrix.
+//
+// @return       Inverted essential matrix.
+Eigen::Matrix3d InvertEssentialMatrix(const Eigen::Matrix3d& matrix);
+
+// Refine essential matrix.
+//
+// Decomposes the essential matrix into rotation and translation components
+// and refines the relative pose using the function `RefineRelativePose`.
+//
+// @param E                3x3 essential matrix.
+// @param points1          First set of corresponding points.
+// @param points2          Second set of corresponding points.
+// @param inlier_mask      Inlier mask for corresponding points.
+// @param options          Solver options.
+//
+// @return                 Flag indicating if solution is usable.
+bool RefineEssentialMatrix(const ceres::Solver::Options& options,
+                           const std::vector<Eigen::Vector2d>& points1,
+                           const std::vector<Eigen::Vector2d>& points2,
+                           const std::vector<char>& inlier_mask,
+                           Eigen::Matrix3d* E);
+
+}  // namespace colmap
+
+#endif  // COLMAP_SRC_BASE_ESSENTIAL_MATRIX_H_

colmap/include/colmap/base/gps.h

@@ -0,0 +1,89 @@
+// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//
+//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
+//       its contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
+
+#ifndef COLMAP_SRC_BASE_GPS_H_
+#define COLMAP_SRC_BASE_GPS_H_
+
+#include <vector>
+
+#include <Eigen/Core>
+
+#include "util/alignment.h"
+#include "util/types.h"
+
+namespace colmap {
+
+// Transform ellipsoidal GPS coordinates to Cartesian GPS coordinate
+// representation and vice versa.
+class GPSTransform {
+ public:
+  enum ELLIPSOID { GRS80, WGS84 };
+
+  explicit GPSTransform(const int ellipsoid = GRS80);
+
+  std::vector<Eigen::Vector3d> EllToXYZ(
+      const std::vector<Eigen::Vector3d>& ell) const;
+
+  std::vector<Eigen::Vector3d> XYZToEll(
+      const std::vector<Eigen::Vector3d>& xyz) const;
+
+  // Convert GPS (lat / lon / alt) to ENU coords. with lat0 and lon0
+  // defining the origin of the ENU frame
+  std::vector<Eigen::Vector3d> EllToENU(const std::vector<Eigen::Vector3d>& ell,
+                                        const double lat0,
+                                        const double lon0) const;
+
+  std::vector<Eigen::Vector3d> XYZToENU(const std::vector<Eigen::Vector3d>& xyz,
+                                        const double lat0,
+                                        const double lon0) const;
+
+  std::vector<Eigen::Vector3d> ENUToEll(const std::vector<Eigen::Vector3d>& enu,
+                                        const double lat0, const double lon0,
+                                        const double alt0) const;
+
+  std::vector<Eigen::Vector3d> ENUToXYZ(const std::vector<Eigen::Vector3d>& enu,
+                                        const double lat0, const double lon0,
+                                        const double alt0) const;
+
+ private:
+  // Semimajor axis.
+  double a_;
+  // Semiminor axis.
+  double b_;
+  // Flattening.
+  double f_;
+  // Numerical eccentricity.
+  double e2_;
+};
+
+}  // namespace colmap
+
+#endif  // COLMAP_SRC_BASE_GPS_H_

colmap/include/colmap/base/graph_cut.h

@@ -0,0 +1,209 @@
+// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//
+//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
+//       its contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
+
+#ifndef COLMAP_SRC_BASE_GRAPH_CUT_H_
+#define COLMAP_SRC_BASE_GRAPH_CUT_H_
+
+#include <unordered_map>
+#include <vector>
+
+#include <boost/graph/adjacency_list.hpp>
+#include <boost/graph/boykov_kolmogorov_max_flow.hpp>
+#include <boost/graph/graph_traits.hpp>
+#include <boost/graph/one_bit_color_map.hpp>
+
+#include "util/logging.h"
+
+namespace colmap {
+
+// Compute the min-cut of a undirected graph using the Stoer Wagner algorithm.
+void ComputeMinGraphCutStoerWagner(
+    const std::vector<std::pair<int, int>>& edges,
+    const std::vector<int>& weights, int* cut_weight,
+    std::vector<char>* cut_labels);
+
+// Compute the normalized min-cut of an undirected graph using Metis.
+// Partitions the graph into clusters and returns the cluster labels per vertex.
+std::unordered_map<int, int> ComputeNormalizedMinGraphCut(
+    const std::vector<std::pair<int, int>>& edges,
+    const std::vector<int>& weights, const int num_parts);
+
+// Compute the minimum graph cut of a directed S-T graph using the
+// Boykov-Kolmogorov max-flow min-cut algorithm, as descibed in:
+//   "An Experimental Comparison of Min-Cut/Max-Flow Algorithms for Energy
+//    Minimization in Vision". Yuri Boykov and Vladimir Kolmogorov. PAMI, 2004.
+template <typename node_t, typename value_t>
+class MinSTGraphCut {
+ public:
+  typedef boost::adjacency_list_traits<boost::vecS, boost::vecS,
+                                       boost::directedS>
+      graph_traits_t;
+  typedef graph_traits_t::edge_descriptor edge_descriptor_t;
+  typedef graph_traits_t::vertices_size_type vertices_size_t;
+
+  struct Edge {
+    value_t capacity;
+    value_t residual;
+    edge_descriptor_t reverse;
+  };
+
+  typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS,
+                                size_t, Edge>
+      graph_t;
+
+  MinSTGraphCut(const size_t num_nodes);
+
+  // Count the number of nodes and edges in the graph.
+  size_t NumNodes() const;
+  size_t NumEdges() const;
+
+  // Add node to the graph.
+  void AddNode(const node_t node_idx, const value_t source_capacity,
+               const value_t sink_capacity);
+
+  // Add edge to the graph.
+  void AddEdge(const node_t node_idx1, const node_t node_idx2,
+               const value_t capacity, const value_t reverse_capacity);
+
+  // Compute the min-cut using the max-flow algorithm. Returns the flow.
+  value_t Compute();
+
+  // Check whether node is connected to source or sink after computing the cut.
+  bool IsConnectedToSource(const node_t node_idx) const;
+  bool IsConnectedToSink(const node_t node_idx) const;
+
+ private:
+  const node_t S_node_;
+  const node_t T_node_;
+  graph_t graph_;
+  std::vector<boost::default_color_type> colors_;
+};
+
+////////////////////////////////////////////////////////////////////////////////
+// Implementation
+////////////////////////////////////////////////////////////////////////////////
+
+template <typename node_t, typename value_t>
+MinSTGraphCut<node_t, value_t>::MinSTGraphCut(const size_t num_nodes)
+    : S_node_(num_nodes), T_node_(num_nodes + 1), graph_(num_nodes + 2) {}
+
+template <typename node_t, typename value_t>
+size_t MinSTGraphCut<node_t, value_t>::NumNodes() const {
+  return boost::num_vertices(graph_) - 2;
+}
+
+template <typename node_t, typename value_t>
+size_t MinSTGraphCut<node_t, value_t>::NumEdges() const {
+  return boost::num_edges(graph_);
+}
+
+template <typename node_t, typename value_t>
+void MinSTGraphCut<node_t, value_t>::AddNode(const node_t node_idx,
+                                             const value_t source_capacity,
+                                             const value_t sink_capacity) {
+  CHECK_GE(node_idx, 0);
+  CHECK_LE(node_idx, boost::num_vertices(graph_));
+  CHECK_GE(source_capacity, 0);
+  CHECK_GE(sink_capacity, 0);
+
+  if (source_capacity > 0) {
+    const edge_descriptor_t edge =
+        boost::add_edge(S_node_, node_idx, graph_).first;
+    const edge_descriptor_t edge_reverse =
+        boost::add_edge(node_idx, S_node_, graph_).first;
+    graph_[edge].capacity = source_capacity;
+    graph_[edge].reverse = edge_reverse;
+    graph_[edge_reverse].reverse = edge;
+  }
+
+  if (sink_capacity > 0) {
+    const edge_descriptor_t edge =
+        boost::add_edge(node_idx, T_node_, graph_).first;
+    const edge_descriptor_t edge_reverse =
+        boost::add_edge(T_node_, node_idx, graph_).first;
+    graph_[edge].capacity = sink_capacity;
+    graph_[edge].reverse = edge_reverse;
+    graph_[edge_reverse].reverse = edge;
+  }
+}
+
+template <typename node_t, typename value_t>
+void MinSTGraphCut<node_t, value_t>::AddEdge(const node_t node_idx1,
+                                             const node_t node_idx2,
+                                             const value_t capacity,
+                                             const value_t reverse_capacity) {
+  CHECK_GE(node_idx1, 0);
+  CHECK_LE(node_idx1, boost::num_vertices(graph_));
+  CHECK_GE(node_idx2, 0);
+  CHECK_LE(node_idx2, boost::num_vertices(graph_));
+  CHECK_GE(capacity, 0);
+  CHECK_GE(reverse_capacity, 0);
+
+  const edge_descriptor_t edge =
+      boost::add_edge(node_idx1, node_idx2, graph_).first;
+  const edge_descriptor_t edge_reverse =
+      boost::add_edge(node_idx2, node_idx1, graph_).first;
+  graph_[edge].capacity = capacity;
+  graph_[edge_reverse].capacity = reverse_capacity;
+  graph_[edge].reverse = edge_reverse;
+  graph_[edge_reverse].reverse = edge;
+}
+
+template <typename node_t, typename value_t>
+value_t MinSTGraphCut<node_t, value_t>::Compute() {
+  const vertices_size_t num_vertices = boost::num_vertices(graph_);
+
+  colors_.resize(num_vertices);
+  std::vector<edge_descriptor_t> predecessors(num_vertices);
+  std::vector<vertices_size_t> distances(num_vertices);
+
+  return boost::boykov_kolmogorov_max_flow(
+      graph_, boost::get(&Edge::capacity, graph_),
+      boost::get(&Edge::residual, graph_), boost::get(&Edge::reverse, graph_),
+      predecessors.data(), colors_.data(), distances.data(),
+      boost::get(boost::vertex_index, graph_), S_node_, T_node_);
+}
+
+template <typename node_t, typename value_t>
+bool MinSTGraphCut<node_t, value_t>::IsConnectedToSource(
+    const node_t node_idx) const {
+  return colors_.at(node_idx) != boost::white_color;
+}
+
+template <typename node_t, typename value_t>
+bool MinSTGraphCut<node_t, value_t>::IsConnectedToSink(
+    const node_t node_idx) const {
+  return colors_.at(node_idx) == boost::white_color;
+}
+
+}  // namespace colmap
+
+#endif  // COLMAP_SRC_BASE_GRAPH_CUT_H_

colmap/include/colmap/base/homography_matrix.h

@@ -0,0 +1,111 @@
+// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//
+//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
+//       its contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
+
+#ifndef COLMAP_SRC_BASE_HOMOGRAPHY_MATRIX_UTILS_H_
+#define COLMAP_SRC_BASE_HOMOGRAPHY_MATRIX_UTILS_H_
+
+#include <vector>
+
+#include <Eigen/Core>
+
+#include "util/alignment.h"
+#include "util/types.h"
+
+namespace colmap {
+
+// Decompose an homography matrix into the possible rotations, translations,
+// and plane normal vectors, according to:
+//
+//    Malis, Ezio, and Manuel Vargas. "Deeper understanding of the homography
+//    decomposition for vision-based control." (2007): 90.
+//
+// The first pose is assumed to be P = [I | 0]. Note that the homography is
+// plane-induced if `R.size() == t.size() == n.size() == 4`. If `R.size() ==
+// t.size() == n.size() == 1` the homography is pure-rotational.
+//
+// @param H          3x3 homography matrix.
+// @param K          3x3 calibration matrix.
+// @param R          Possible 3x3 rotation matrices.
+// @param t          Possible translation vectors.
+// @param n          Possible normal vectors.
+void DecomposeHomographyMatrix(const Eigen::Matrix3d& H,
+                               const Eigen::Matrix3d& K1,
+                               const Eigen::Matrix3d& K2,
+                               std::vector<Eigen::Matrix3d>* R,
+                               std::vector<Eigen::Vector3d>* t,
+                               std::vector<Eigen::Vector3d>* n);
+
+// Recover the most probable pose from the given homography matrix.
+//
+// The pose of the first image is assumed to be P = [I | 0].
+//
+// @param H            3x3 homography matrix.
+// @param K1           3x3 calibration matrix of first camera.
+// @param K2           3x3 calibration matrix of second camera.
+// @param points1      First set of corresponding points.
+// @param points2      Second set of corresponding points.
+// @param inlier_mask  Only points with `true` in the inlier mask are
+//                     considered in the cheirality test. Size of the
+//                     inlier mask must match the number of points N.
+// @param R            Most probable 3x3 rotation matrix.
+// @param t            Most probable 3x1 translation vector.
+// @param n            Most probable 3x1 normal vector.
+// @param points3D     Triangulated 3D points infront of camera
+//                     (only if homography is not pure-rotational).
+void PoseFromHomographyMatrix(const Eigen::Matrix3d& H,
+                              const Eigen::Matrix3d& K1,
+                              const Eigen::Matrix3d& K2,
+                              const std::vector<Eigen::Vector2d>& points1,
+                              const std::vector<Eigen::Vector2d>& points2,
+                              Eigen::Matrix3d* R, Eigen::Vector3d* t,
+                              Eigen::Vector3d* n,
+                              std::vector<Eigen::Vector3d>* points3D);
+
+// Compose homography matrix from relative pose.
+//
+// @param K1      3x3 calibration matrix of first camera.
+// @param K2      3x3 calibration matrix of second camera.
+// @param R       Most probable 3x3 rotation matrix.
+// @param t       Most probable 3x1 translation vector.
+// @param n       Most probable 3x1 normal vector.
+// @param d       Orthogonal distance from plane.
+//
+// @return        3x3 homography matrix.
+Eigen::Matrix3d HomographyMatrixFromPose(const Eigen::Matrix3d& K1,
+                                         const Eigen::Matrix3d& K2,
+                                         const Eigen::Matrix3d& R,
+                                         const Eigen::Vector3d& t,
+                                         const Eigen::Vector3d& n,
+                                         const double d);
+
+}  // namespace colmap
+
+#endif  // COLMAP_SRC_BASE_HOMOGRAPHY_MATRIX_UTILS_H_

colmap/include/colmap/base/image.h

@@ -0,0 +1,364 @@
+// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//
+//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
+//       its contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
+
+#ifndef COLMAP_SRC_BASE_IMAGE_H_
+#define COLMAP_SRC_BASE_IMAGE_H_
+
+#include <string>
+#include <vector>
+
+#include <Eigen/Core>
+
+#include "base/camera.h"
+#include "base/point2d.h"
+#include "base/visibility_pyramid.h"
+#include "util/alignment.h"
+#include "util/logging.h"
+#include "util/math.h"
+#include "util/types.h"
+
+namespace colmap {
+
+// Class that holds information about an image. An image is the product of one
+// camera shot at a certain location (parameterized as the pose). An image may
+// share a camera with multiple other images, if its intrinsics are the same.
+class Image {
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  Image();
+
+  // Setup / tear down the image and necessary internal data structures before
+  // and after being used in reconstruction.
+  void SetUp(const Camera& camera);
+  void TearDown();
+
+  // Access the unique identifier of the image.
+  inline image_t ImageId() const;
+  inline void SetImageId(const image_t image_id);
+
+  // Access the name of the image.
+  inline const std::string& Name() const;
+  inline std::string& Name();
+  inline void SetName(const std::string& name);
+
+  // Access the unique identifier of the camera. Note that multiple images
+  // might share the same camera.
+  inline camera_t CameraId() const;
+  inline void SetCameraId(const camera_t camera_id);
+  // Check whether identifier of camera has been set.
+  inline bool HasCamera() const;
+
+  // Check if image is registered.
+  inline bool IsRegistered() const;
+  inline void SetRegistered(const bool registered);
+
+  // Get the number of image points.
+  inline point2D_t NumPoints2D() const;
+
+  // Get the number of triangulations, i.e. the number of points that
+  // are part of a 3D point track.
+  inline point2D_t NumPoints3D() const;
+
+  // Get the number of observations, i.e. the number of image points that
+  // have at least one correspondence to another image.
+  inline point2D_t NumObservations() const;
+  inline void SetNumObservations(const point2D_t num_observations);
+
+  // Get the number of correspondences for all image points.
+  inline point2D_t NumCorrespondences() const;
+  inline void SetNumCorrespondences(const point2D_t num_observations);
+
+  // Get the number of observations that see a triangulated point, i.e. the
+  // number of image points that have at least one correspondence to a
+  // triangulated point in another image.
+  inline point2D_t NumVisiblePoints3D() const;
+
+  // Get the score of triangulated observations. In contrast to
+  // `NumVisiblePoints3D`, this score also captures the distribution
+  // of triangulated observations in the image. This is useful to select
+  // the next best image in incremental reconstruction, because a more
+  // uniform distribution of observations results in more robust registration.
+  inline size_t Point3DVisibilityScore() const;
+
+  // Access quaternion vector as (qw, qx, qy, qz) specifying the rotation of the
+  // pose which is defined as the transformation from world to image space.
+  inline const Eigen::Vector4d& Qvec() const;
+  inline Eigen::Vector4d& Qvec();
+  inline double Qvec(const size_t idx) const;
+  inline double& Qvec(const size_t idx);
+  inline void SetQvec(const Eigen::Vector4d& qvec);
+
+  // Quaternion prior, e.g. given by EXIF gyroscope tag.
+  inline const Eigen::Vector4d& QvecPrior() const;
+  inline Eigen::Vector4d& QvecPrior();
+  inline double QvecPrior(const size_t idx) const;
+  inline double& QvecPrior(const size_t idx);
+  inline bool HasQvecPrior() const;
+  inline void SetQvecPrior(const Eigen::Vector4d& qvec);
+
+  // Access translation vector as (tx, ty, tz) specifying the translation of the
+  // pose which is defined as the transformation from world to image space.
+  inline const Eigen::Vector3d& Tvec() const;
+  inline Eigen::Vector3d& Tvec();
+  inline double Tvec(const size_t idx) const;
+  inline double& Tvec(const size_t idx);
+  inline void SetTvec(const Eigen::Vector3d& tvec);
+
+  // Translation prior, e.g. given by EXIF GPS tag.
+  inline const Eigen::Vector3d& TvecPrior() const;
+  inline Eigen::Vector3d& TvecPrior();
+  inline double TvecPrior(const size_t idx) const;
+  inline double& TvecPrior(const size_t idx);
+  inline bool HasTvecPrior() const;
+  inline void SetTvecPrior(const Eigen::Vector3d& tvec);
+
+  // Access the coordinates of image points.
+  inline const class Point2D& Point2D(const point2D_t point2D_idx) const;
+  inline class Point2D& Point2D(const point2D_t point2D_idx);
+  inline const std::vector<class Point2D>& Points2D() const;
+  void SetPoints2D(const std::vector<Eigen::Vector2d>& points);
+  void SetPoints2D(const std::vector<class Point2D>& points);
+
+  // Set the point as triangulated, i.e. it is part of a 3D point track.
+  void SetPoint3DForPoint2D(const point2D_t point2D_idx,
+                            const point3D_t point3D_id);
+
+  // Set the point as not triangulated, i.e. it is not part of a 3D point track.
+  void ResetPoint3DForPoint2D(const point2D_t point2D_idx);
+
+  // Check whether an image point has a correspondence to an image point in
+  // another image that has a 3D point.
+  inline bool IsPoint3DVisible(const point2D_t point2D_idx) const;
+
+  // Check whether one of the image points is part of the 3D point track.
+  bool HasPoint3D(const point3D_t point3D_id) const;
+
+  // Indicate that another image has a point that is triangulated and has
+  // a correspondence to this image point. Note that this must only be called
+  // after calling `SetUp`.
+  void IncrementCorrespondenceHasPoint3D(const point2D_t point2D_idx);
+
+  // Indicate that another image has a point that is not triangulated any more
+  // and has a correspondence to this image point. This assumes that
+  // `IncrementCorrespondenceHasPoint3D` was called for the same image point
+  // and correspondence before. Note that this must only be called
+  // after calling `SetUp`.
+  void DecrementCorrespondenceHasPoint3D(const point2D_t point2D_idx);
+
+  // Normalize the quaternion vector.
+  void NormalizeQvec();
+
+  // Compose the projection matrix from world to image space.
+  Eigen::Matrix3x4d ProjectionMatrix() const;
+
+  // Compose the inverse projection matrix from image to world space
+  Eigen::Matrix3x4d InverseProjectionMatrix() const;
+
+  // Compose rotation matrix from quaternion vector.
+  Eigen::Matrix3d RotationMatrix() const;
+
+  // Extract the projection center in world space.
+  Eigen::Vector3d ProjectionCenter() const;
+
+  // Extract the viewing direction of the image.
+  Eigen::Vector3d ViewingDirection() const;
+
+  // The number of levels in the 3D point multi-resolution visibility pyramid.
+  static const int kNumPoint3DVisibilityPyramidLevels;
+
+ private:
+  // Identifier of the image, if not specified `kInvalidImageId`.
+  image_t image_id_;
+
+  // The name of the image, i.e. the relative path.
+  std::string name_;
+
+  // The identifier of the associated camera. Note that multiple images might
+  // share the same camera. If not specified `kInvalidCameraId`.
+  camera_t camera_id_;
+
+  // Whether the image is successfully registered in the reconstruction.
+  bool registered_;
+
+  // The number of 3D points the image observes, i.e. the sum of its `points2D`
+  // where `point3D_id != kInvalidPoint3DId`.
+  point2D_t num_points3D_;
+
+  // The number of image points that have at least one correspondence to
+  // another image.
+  point2D_t num_observations_;
+
+  // The sum of correspondences per image point.
+  point2D_t num_correspondences_;
+
+  // The number of 2D points, which have at least one corresponding 2D point in
+  // another image that is part of a 3D point track, i.e. the sum of `points2D`
+  // where `num_tris > 0`.
+  point2D_t num_visible_points3D_;
+
+  // The pose of the image, defined as the transformation from world to image.
+  Eigen::Vector4d qvec_;
+  Eigen::Vector3d tvec_;
+
+  // The pose prior of the image, e.g. extracted from EXIF tags.
+  Eigen::Vector4d qvec_prior_;
+  Eigen::Vector3d tvec_prior_;
+
+  // All image points, including points that are not part of a 3D point track.
+  std::vector<class Point2D> points2D_;
+
+  // Per image point, the number of correspondences that have a 3D point.
+  std::vector<point2D_t> num_correspondences_have_point3D_;
+
+  // Data structure to compute the distribution of triangulated correspondences
+  // in the image. Note that this structure is only usable after `SetUp`.
+  VisibilityPyramid point3D_visibility_pyramid_;
+};
+
+////////////////////////////////////////////////////////////////////////////////
+// Implementation
+////////////////////////////////////////////////////////////////////////////////
+
+image_t Image::ImageId() const { return image_id_; }
+
+void Image::SetImageId(const image_t image_id) { image_id_ = image_id; }
+
+const std::string& Image::Name() const { return name_; }
+
+std::string& Image::Name() { return name_; }
+
+void Image::SetName(const std::string& name) { name_ = name; }
+
+inline camera_t Image::CameraId() const { return camera_id_; }
+
+inline void Image::SetCameraId(const camera_t camera_id) {
+  CHECK_NE(camera_id, kInvalidCameraId);
+  camera_id_ = camera_id;
+}
+
+inline bool Image::HasCamera() const { return camera_id_ != kInvalidCameraId; }
+
+bool Image::IsRegistered() const { return registered_; }
+
+void Image::SetRegistered(const bool registered) { registered_ = registered; }
+
+point2D_t Image::NumPoints2D() const {
+  return static_cast<point2D_t>(points2D_.size());
+}
+
+point2D_t Image::NumPoints3D() const { return num_points3D_; }
+
+point2D_t Image::NumObservations() const { return num_observations_; }
+
+void Image::SetNumObservations(const point2D_t num_observations) {
+  num_observations_ = num_observations;
+}
+
+point2D_t Image::NumCorrespondences() const { return num_correspondences_; }
+
+void Image::SetNumCorrespondences(const point2D_t num_correspondences) {
+  num_correspondences_ = num_correspondences;
+}
+
+point2D_t Image::NumVisiblePoints3D() const { return num_visible_points3D_; }
+
+size_t Image::Point3DVisibilityScore() const {
+  return point3D_visibility_pyramid_.Score();
+}
+
+const Eigen::Vector4d& Image::Qvec() const { return qvec_; }
+
+Eigen::Vector4d& Image::Qvec() { return qvec_; }
+
+inline double Image::Qvec(const size_t idx) const { return qvec_(idx); }
+
+inline double& Image::Qvec(const size_t idx) { return qvec_(idx); }
+
+void Image::SetQvec(const Eigen::Vector4d& qvec) { qvec_ = qvec; }
+
+const Eigen::Vector4d& Image::QvecPrior() const { return qvec_prior_; }
+
+Eigen::Vector4d& Image::QvecPrior() { return qvec_prior_; }
+
+inline double Image::QvecPrior(const size_t idx) const {
+  return qvec_prior_(idx);
+}
+
+inline double& Image::QvecPrior(const size_t idx) { return qvec_prior_(idx); }
+
+inline bool Image::HasQvecPrior() const { return !IsNaN(qvec_prior_.sum()); }
+
+void Image::SetQvecPrior(const Eigen::Vector4d& qvec) { qvec_prior_ = qvec; }
+
+const Eigen::Vector3d& Image::Tvec() const { return tvec_; }
+
+Eigen::Vector3d& Image::Tvec() { return tvec_; }
+
+inline double Image::Tvec(const size_t idx) const { return tvec_(idx); }
+
+inline double& Image::Tvec(const size_t idx) { return tvec_(idx); }
+
+void Image::SetTvec(const Eigen::Vector3d& tvec) { tvec_ = tvec; }
+
+const Eigen::Vector3d& Image::TvecPrior() const { return tvec_prior_; }
+
+Eigen::Vector3d& Image::TvecPrior() { return tvec_prior_; }
+
+inline double Image::TvecPrior(const size_t idx) const {
+  return tvec_prior_(idx);
+}
+
+inline double& Image::TvecPrior(const size_t idx) { return tvec_prior_(idx); }
+
+inline bool Image::HasTvecPrior() const { return !IsNaN(tvec_prior_.sum()); }
+
+void Image::SetTvecPrior(const Eigen::Vector3d& tvec) { tvec_prior_ = tvec; }
+
+const class Point2D& Image::Point2D(const point2D_t point2D_idx) const {
+  return points2D_.at(point2D_idx);
+}
+
+class Point2D& Image::Point2D(const point2D_t point2D_idx) {
+  return points2D_.at(point2D_idx);
+}
+
+const std::vector<class Point2D>& Image::Points2D() const { return points2D_; }
+
+bool Image::IsPoint3DVisible(const point2D_t point2D_idx) const {
+  return num_correspondences_have_point3D_.at(point2D_idx) > 0;
+}
+
+}  // namespace colmap
+
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(colmap::Image)
+
+#endif  // COLMAP_SRC_BASE_IMAGE_H_

colmap/include/colmap/base/image_reader.h

@@ -0,0 +1,133 @@
+// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//
+//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
+//       its contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
+
+#ifndef COLMAP_SRC_BASE_IMAGE_READER_H_
+#define COLMAP_SRC_BASE_IMAGE_READER_H_
+
+#include <unordered_set>
+
+#include "base/database.h"
+#include "util/bitmap.h"
+#include "util/threading.h"
+
+namespace colmap {
+
+struct ImageReaderOptions {
+  // Path to database in which to store the extracted data.
+  std::string database_path = "";
+
+  // Root path to folder which contains the images.
+  std::string image_path = "";
+
+  // Optional root path to folder which contains image masks. For a given image,
+  // the corresponding mask must have the same sub-path below this root as the
+  // image has below image_path. The filename must be equal, aside from the
+  // added extension .png. For example, for an image image_path/abc/012.jpg, the
+  // mask would be mask_path/abc/012.jpg.png. No features will be extracted in
+  // regions where the mask image is black (pixel intensity value 0 in
+  // grayscale).
+  std::string mask_path = "";
+
+  // Optional list of images to read. The list must contain the relative path
+  // of the images with respect to the image_path.
+  std::vector<std::string> image_list;
+
+  // Name of the camera model.
+  std::string camera_model = "SIMPLE_RADIAL";
+
+  // Whether to use the same camera for all images.
+  bool single_camera = false;
+
+  // Whether to use the same camera for all images in the same sub-folder.
+  bool single_camera_per_folder = false;
+
+  // Whether to use a different camera for each image.
+  bool single_camera_per_image = false;
+
+  // Whether to explicitly use an existing camera for all images. Note that in
+  // this case the specified camera model and parameters are ignored.
+  int existing_camera_id = kInvalidCameraId;
+
+  // Manual specification of camera parameters. If empty, camera parameters
+  // will be extracted from EXIF, i.e. principal point and focal length.
+  std::string camera_params = "";
+
+  // If camera parameters are not specified manually and the image does not
+  // have focal length EXIF information, the focal length is set to the
+  // value `default_focal_length_factor * max(width, height)`.
+  double default_focal_length_factor = 1.2;
+
+  // Optional path to an image file specifying a mask for all images. No
+  // features will be extracted in regions where the mask is black (pixel
+  // intensity value 0 in grayscale).
+  std::string camera_mask_path = "";
+
+  bool Check() const;
+};
+
+// Recursively iterate over the images in a directory. Skips an image if it
+// already exists in the database. Extracts the camera intrinsics from EXIF and
+// writes the camera information to the database.
+class ImageReader {
+ public:
+  enum class Status {
+    FAILURE,
+    SUCCESS,
+    IMAGE_EXISTS,
+    BITMAP_ERROR,
+    CAMERA_SINGLE_DIM_ERROR,
+    CAMERA_EXIST_DIM_ERROR,
+    CAMERA_PARAM_ERROR
+  };
+
+  ImageReader(const ImageReaderOptions& options, Database* database);
+
+  Status Next(Camera* camera, Image* image, Bitmap* bitmap, Bitmap* mask);
+  size_t NextIndex() const;
+  size_t NumImages() const;
+
+ private:
+  // Image reader options.
+  ImageReaderOptions options_;
+  Database* database_;
+  // Index of previously processed image.
+  size_t image_index_;
+  // Previously processed camera.
+  Camera prev_camera_;
+  std::unordered_map<std::string, camera_t> camera_model_to_id_;
+  // Names of image sub-folders.
+  std::string prev_image_folder_;
+  std::unordered_set<std::string> image_folders_;
+};
+
+}  // namespace colmap
+
+#endif  // COLMAP_SRC_BASE_IMAGE_READER_H_

colmap/include/colmap/base/line.h

@@ -0,0 +1,66 @@
+// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//
+//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
+//       its contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
+
+#ifndef COLMAP_SRC_BASE_LINE_H_
+#define COLMAP_SRC_BASE_LINE_H_
+
+#include <Eigen/Core>
+
+#include "util/alignment.h"
+#include "util/bitmap.h"
+
+namespace colmap {
+
+struct LineSegment {
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  Eigen::Vector2d start;
+  Eigen::Vector2d end;
+};
+
+enum class LineSegmentOrientation {
+  HORIZONTAL = 1,
+  VERTICAL = -1,
+  UNDEFINED = 0,
+};
+
+// Detect line segments in the given bitmap image.
+std::vector<LineSegment> DetectLineSegments(const Bitmap& bitmap,
+                                            const double min_length = 3);
+
+// Classify line segments into horizontal/vertical.
+std::vector<LineSegmentOrientation> ClassifyLineSegmentOrientations(
+    const std::vector<LineSegment>& segments, const double tolerance = 0.25);
+
+}  // namespace colmap
+
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(colmap::LineSegment)
+
+#endif  // COLMAP_SRC_BASE_LINE_H_

colmap/include/colmap/base/point2d.h

@@ -0,0 +1,98 @@
+// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//
+//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
+//       its contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
+
+#ifndef COLMAP_SRC_BASE_POINT2D_H_
+#define COLMAP_SRC_BASE_POINT2D_H_
+
+#include <Eigen/Core>
+
+#include "util/alignment.h"
+#include "util/types.h"
+
+namespace colmap {
+
+// 2D point class corresponds to a feature in an image. It may or may not have a
+// corresponding 3D point if it is part of a triangulated track.
+class Point2D {
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  Point2D();
+
+  // The coordinate in image space in pixels.
+  inline const Eigen::Vector2d& XY() const;
+  inline Eigen::Vector2d& XY();
+  inline double X() const;
+  inline double Y() const;
+  inline void SetXY(const Eigen::Vector2d& xy);
+
+  // The identifier of the observed 3D point. If the image point does not
+  // observe a 3D point, the identifier is `kInvalidPoint3Did`.
+  inline point3D_t Point3DId() const;
+  inline bool HasPoint3D() const;
+  inline void SetPoint3DId(const point3D_t point3D_id);
+
+ private:
+  // The image coordinates in pixels, starting at upper left corner with 0.
+  Eigen::Vector2d xy_;
+
+  // The identifier of the 3D point. If the 2D point is not part of a 3D point
+  // track the identifier is `kInvalidPoint3DId` and `HasPoint3D() = false`.
+  point3D_t point3D_id_;
+};
+
+////////////////////////////////////////////////////////////////////////////////
+// Implementation
+////////////////////////////////////////////////////////////////////////////////
+
+const Eigen::Vector2d& Point2D::XY() const { return xy_; }
+
+Eigen::Vector2d& Point2D::XY() { return xy_; }
+
+double Point2D::X() const { return xy_.x(); }
+
+double Point2D::Y() const { return xy_.y(); }
+
+void Point2D::SetXY(const Eigen::Vector2d& xy) { xy_ = xy; }
+
+point3D_t Point2D::Point3DId() const { return point3D_id_; }
+
+bool Point2D::HasPoint3D() const { return point3D_id_ != kInvalidPoint3DId; }
+
+void Point2D::SetPoint3DId(const point3D_t point3D_id) {
+  point3D_id_ = point3D_id;
+}
+
+}  // namespace colmap
+
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(colmap::Point2D)
+
+#endif  // COLMAP_SRC_BASE_POINT2D_H_

colmap/include/colmap/base/point3d.h

@@ -0,0 +1,140 @@
+// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//
+//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
+//       its contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
+
+#ifndef COLMAP_SRC_BASE_POINT3D_H_
+#define COLMAP_SRC_BASE_POINT3D_H_
+
+#include <vector>
+
+#include <Eigen/Core>
+
+#include "base/track.h"
+#include "util/logging.h"
+#include "util/types.h"
+
+namespace colmap {
+
+// 3D point class that holds information about triangulated 2D points.
+class Point3D {
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  Point3D();
+
+  // The point coordinate in world space.
+  inline const Eigen::Vector3d& XYZ() const;
+  inline Eigen::Vector3d& XYZ();
+  inline double XYZ(const size_t idx) const;
+  inline double& XYZ(const size_t idx);
+  inline double X() const;
+  inline double Y() const;
+  inline double Z() const;
+  inline void SetXYZ(const Eigen::Vector3d& xyz);
+
+  // The RGB color of the point.
+  inline const Eigen::Vector3ub& Color() const;
+  inline Eigen::Vector3ub& Color();
+  inline uint8_t Color(const size_t idx) const;
+  inline uint8_t& Color(const size_t idx);
+  inline void SetColor(const Eigen::Vector3ub& color);
+
+  // The mean reprojection error in image space.
+  inline double Error() const;
+  inline bool HasError() const;
+  inline void SetError(const double error);
+
+  inline const class Track& Track() const;
+  inline class Track& Track();
+  inline void SetTrack(class Track track);
+
+ private:
+  // The 3D position of the point.
+  Eigen::Vector3d xyz_;
+
+  // The color of the point in the range [0, 255].
+  Eigen::Vector3ub color_;
+
+  // The mean reprojection error in pixels.
+  double error_;
+
+  // The track of the point as a list of image observations.
+  class Track track_;
+};
+
+////////////////////////////////////////////////////////////////////////////////
+// Implementation
+////////////////////////////////////////////////////////////////////////////////
+
+const Eigen::Vector3d& Point3D::XYZ() const { return xyz_; }
+
+Eigen::Vector3d& Point3D::XYZ() { return xyz_; }
+
+double Point3D::XYZ(const size_t idx) const { return xyz_(idx); }
+
+double& Point3D::XYZ(const size_t idx) { return xyz_(idx); }
+
+double Point3D::X() const { return xyz_.x(); }
+
+double Point3D::Y() const { return xyz_.y(); }
+
+double Point3D::Z() const { return xyz_.z(); }
+
+void Point3D::SetXYZ(const Eigen::Vector3d& xyz) { xyz_ = xyz; }
+
+const Eigen::Vector3ub& Point3D::Color() const { return color_; }
+
+Eigen::Vector3ub& Point3D::Color() { return color_; }
+
+uint8_t Point3D::Color(const size_t idx) const { return color_(idx); }
+
+uint8_t& Point3D::Color(const size_t idx) { return color_(idx); }
+
+void Point3D::SetColor(const Eigen::Vector3ub& color) { color_ = color; }
+
+double Point3D::Error() const { return error_; }
+
+bool Point3D::HasError() const { return error_ != -1.0; }
+
+void Point3D::SetError(const double error) { error_ = error; }
+
+const class Track& Point3D::Track() const { return track_; }
+
+class Track& Point3D::Track() {
+  return track_;
+}
+
+void Point3D::SetTrack(class Track track) { track_ = std::move(track); }
+
+}  // namespace colmap
+
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(colmap::Point3D)
+
+#endif  // COLMAP_SRC_BASE_POINT3D_H_

colmap/include/colmap/base/polynomial.h

@@ -0,0 +1,101 @@
+// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//
+//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
+//       its contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
+
+#ifndef COLMAP_SRC_BASE_POLYNOMIAL_H_
+#define COLMAP_SRC_BASE_POLYNOMIAL_H_
+
+#include <Eigen/Core>
+
+namespace colmap {
+
+// All polynomials are assumed to be the form:
+//
+//   sum_{i=0}^N polynomial(i) x^{N-i}.
+//
+// and are given by a vector of coefficients of size N + 1.
+//
+// The implementation is based on COLMAP's old polynomial functionality and is
+// inspired by Ceres-Solver's/Theia's implementation to support complex
+// polynomials. The companion matrix implementation is based on NumPy.
+
+// Evaluate the polynomial for the given coefficients at x using the Horner
+// scheme. This function is templated such that the polynomial may be evaluated
+// at real and/or imaginary points.
+template <typename T>
+T EvaluatePolynomial(const Eigen::VectorXd& coeffs, const T& x);
+
+// Find the root of polynomials of the form: a * x + b = 0.
+// The real and/or imaginary variable may be NULL if the output is not needed.
+bool FindLinearPolynomialRoots(const Eigen::VectorXd& coeffs,
+                               Eigen::VectorXd* real, Eigen::VectorXd* imag);
+
+// Find the roots of polynomials of the form: a * x^2 + b * x + c = 0.
+// The real and/or imaginary variable may be NULL if the output is not needed.
+bool FindQuadraticPolynomialRoots(const Eigen::VectorXd& coeffs,
+                                  Eigen::VectorXd* real, Eigen::VectorXd* imag);
+
+// Find the roots of a polynomial using the Durand-Kerner method, based on:
+//
+//    https://en.wikipedia.org/wiki/Durand%E2%80%93Kerner_method
+//
+// The Durand-Kerner is comparatively fast but often unstable/inaccurate.
+// The real and/or imaginary variable may be NULL if the output is not needed.
+bool FindPolynomialRootsDurandKerner(const Eigen::VectorXd& coeffs,
+                                     Eigen::VectorXd* real,
+                                     Eigen::VectorXd* imag);
+
+// Find the roots of a polynomial using the companion matrix method, based on:
+//
+//    R. A. Horn & C. R. Johnson, Matrix Analysis. Cambridge,
+//    UK: Cambridge University Press, 1999, pp. 146-7.
+//
+// Compared to Durand-Kerner, this method is slower but more stable/accurate.
+// The real and/or imaginary variable may be NULL if the output is not needed.
+bool FindPolynomialRootsCompanionMatrix(const Eigen::VectorXd& coeffs,
+                                        Eigen::VectorXd* real,
+                                        Eigen::VectorXd* imag);
+
+////////////////////////////////////////////////////////////////////////////////
+// Implementation
+////////////////////////////////////////////////////////////////////////////////
+
+template <typename T>
+T EvaluatePolynomial(const Eigen::VectorXd& coeffs, const T& x) {
+  T value = 0.0;
+  for (Eigen::VectorXd::Index i = 0; i < coeffs.size(); ++i) {
+    value = value * x + coeffs(i);
+  }
+  return value;
+}
+
+}  // namespace colmap
+
+#endif  // COLMAP_SRC_BASE_POLYNOMIAL_H_

colmap/include/colmap/base/pose.h

@@ -0,0 +1,230 @@
+// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//
+//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
+//       its contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
+
+#ifndef COLMAP_SRC_BASE_POSE_H_
+#define COLMAP_SRC_BASE_POSE_H_
+
+#include <vector>
+
+#include <Eigen/Core>
+
+#include "util/alignment.h"
+#include "util/types.h"
+
+namespace colmap {
+
+// Compose the skew symmetric cross product matrix from a vector.
+Eigen::Matrix3d CrossProductMatrix(const Eigen::Vector3d& vector);
+
+// Convert 3D rotation matrix to Euler angles.
+//
+// The convention `R = Rx * Ry * Rz` is used,
+// using a right-handed coordinate system.
+//
+// @param R              3x3 rotation matrix.
+// @param rx, ry, rz     Euler angles in radians.
+void RotationMatrixToEulerAngles(const Eigen::Matrix3d& R, double* rx,
+                                 double* ry, double* rz);
+
+// Convert Euler angles to 3D rotation matrix.
+//
+// The convention `R = Rz * Ry * Rx` is used,
+// using a right-handed coordinate system.
+//
+// @param rx, ry, rz     Euler angles in radians.
+//
+// @return               3x3 rotation matrix.
+Eigen::Matrix3d EulerAnglesToRotationMatrix(const double rx, const double ry,
+                                            const double rz);
+
+// Convert 3D rotation matrix to Quaternion representation.
+//
+// @param rot_mat        3x3 rotation matrix.
+//
+// @return               Unit Quaternion rotation coefficients (w, x, y, z).
+Eigen::Vector4d RotationMatrixToQuaternion(const Eigen::Matrix3d& rot_mat);
+
+// Convert Quaternion representation to 3D rotation matrix.
+//
+// @param qvec           Unit Quaternion rotation coefficients (w, x, y, z).
+//
+// @return               3x3 rotation matrix.
+Eigen::Matrix3d QuaternionToRotationMatrix(const Eigen::Vector4d& qvec);
+
+// Compose the Quaternion vector corresponding to a  identity transformation.
+inline Eigen::Vector4d ComposeIdentityQuaternion();
+
+// Normalize Quaternion vector.
+//
+// @param qvec          Quaternion rotation coefficients (w, x, y, z).
+//
+// @return              Unit Quaternion rotation coefficients (w, x, y, z).
+Eigen::Vector4d NormalizeQuaternion(const Eigen::Vector4d& qvec);
+
+// Invert Quaternion vector to return Quaternion of inverse rotation.
+//
+// @param qvec          Quaternion rotation coefficients (w, x, y, z).
+//
+// @return              Inverse Quaternion rotation coefficients (w, x, y, z).
+Eigen::Vector4d InvertQuaternion(const Eigen::Vector4d& qvec);
+
+// Concatenate Quaternion rotations such that the rotation of `qvec1` is applied
+// before the rotation of `qvec2`.
+//
+// @param qvec1         Quaternion rotation coefficients (w, x, y, z).
+// @param qvec2         Quaternion rotation coefficients (w, x, y, z).
+//
+// @return              Concatenated Quaternion coefficients (w, x, y, z).
+Eigen::Vector4d ConcatenateQuaternions(const Eigen::Vector4d& qvec1,
+                                       const Eigen::Vector4d& qvec2);
+
+// Transform point by quaternion rotation.
+//
+// @param qvec          Quaternion rotation coefficients (w, x, y, z).
+// @param point         Point to rotate.
+//
+// @return              Rotated point.
+Eigen::Vector3d QuaternionRotatePoint(const Eigen::Vector4d& qvec,
+                                      const Eigen::Vector3d& point);
+
+// Compute the weighted average of multiple Quaternions according to:
+//
+//    Markley, F. Landis, et al. "Averaging quaternions."
+//    Journal of Guidance, Control, and Dynamics 30.4 (2007): 1193-1197.
+//
+// @param qvecs         The Quaternions to be averaged.
+// @param weights       Non-negative weights.
+//
+// @return              The average Quaternion.
+Eigen::Vector4d AverageQuaternions(const std::vector<Eigen::Vector4d>& qvecs,
+                                   const std::vector<double>& weights);
+
+// Compose rotation matrix that rotates unit vector 1 to unit vector 2.
+// Note that when vector 1 points into the opposite direction of vector 2,
+// the function returns an identity rotation.
+Eigen::Matrix3d RotationFromUnitVectors(const Eigen::Vector3d& vec1,
+                                        const Eigen::Vector3d& vec2);
+
+// Extract camera projection center from projection matrix, i.e. the projection
+// center in world coordinates `-R^T t`.
+Eigen::Vector3d ProjectionCenterFromMatrix(
+    const Eigen::Matrix3x4d& proj_matrix);
+
+// Extract camera projection center from projection parameters.
+//
+// @param qvec           Unit Quaternion rotation coefficients (w, x, y, z).
+// @param tvec           3x1 translation vector.
+//
+// @return               3x1 camera projection center.
+Eigen::Vector3d ProjectionCenterFromPose(const Eigen::Vector4d& qvec,
+                                         const Eigen::Vector3d& tvec);
+
+// Compute the relative transformation from pose 1 to 2.
+//
+// @param qvec1, tvec1      First camera pose.
+// @param qvec2, tvec2      Second camera pose.
+// @param qvec12, tvec12    Relative pose.
+void ComputeRelativePose(const Eigen::Vector4d& qvec1,
+                         const Eigen::Vector3d& tvec1,
+                         const Eigen::Vector4d& qvec2,
+                         const Eigen::Vector3d& tvec2, Eigen::Vector4d* qvec12,
+                         Eigen::Vector3d* tvec12);
+
+// Concatenate the transformations of the two poses.
+//
+// @param qvec1, tvec1      First camera pose.
+// @param qvec2, tvec2      Second camera pose.
+// @param qvec12, tvec12    Concatenated pose.
+void ConcatenatePoses(const Eigen::Vector4d& qvec1,
+                      const Eigen::Vector3d& tvec1,
+                      const Eigen::Vector4d& qvec2,
+                      const Eigen::Vector3d& tvec2, Eigen::Vector4d* qvec12,
+                      Eigen::Vector3d* tvec12);
+
+// Invert transformation of the pose.
+// @param qvec, tvec          Input camera pose.
+// @param inv_qvec, inv_tvec  Inverse camera pose.
+void InvertPose(const Eigen::Vector4d& qvec, const Eigen::Vector3d& tvec,
+                Eigen::Vector4d* inv_qvec, Eigen::Vector3d* inv_tvec);
+
+// Linearly interpolate camera pose.
+//
+// @param qvec1, tvec1      Camera pose at t0 = 0.
+// @param qvec2, tvec2      Camera pose at t1 = 1.
+// @param t                 Interpolation time.
+// @param qveci, tveci      Camera pose at time t.
+void InterpolatePose(const Eigen::Vector4d& qvec1, const Eigen::Vector3d& tvec1,
+                     const Eigen::Vector4d& qvec2, const Eigen::Vector3d& tvec2,
+                     const double t, Eigen::Vector4d* qveci,
+                     Eigen::Vector3d* tveci);
+
+// Calculate baseline vector from first to second pose.
+//
+// The given rotation and orientation is expected as the
+// world to camera transformation.
+//
+// @param qvec1           Unit Quaternion rotation coefficients (w, x, y, z).
+// @param tvec1           3x1 translation vector.
+// @param qvec2           Unit Quaternion rotation coefficients (w, x, y, z).
+// @param tvec2           3x1 translation vector.
+//
+// @return                Baseline vector from 1 to 2.
+Eigen::Vector3d CalculateBaseline(const Eigen::Vector4d& qvec1,
+                                  const Eigen::Vector3d& tvec1,
+                                  const Eigen::Vector4d& qvec2,
+                                  const Eigen::Vector3d& tvec2);
+
+// Perform cheirality constraint test, i.e., determine which of the triangulated
+// correspondences lie in front of of both cameras. The first camera has the
+// projection matrix P1 = [I | 0] and the second camera has the projection
+// matrix P2 = [R | t].
+//
+// @param R            3x3 rotation matrix of second projection matrix.
+// @param t            3x1 translation vector of second projection matrix.
+// @param points1      First set of corresponding points.
+// @param points2      Second set of corresponding points.
+// @param points3D     Points that lie in front of both cameras.
+bool CheckCheirality(const Eigen::Matrix3d& R, const Eigen::Vector3d& t,
+                     const std::vector<Eigen::Vector2d>& points1,
+                     const std::vector<Eigen::Vector2d>& points2,
+                     std::vector<Eigen::Vector3d>* points3D);
+
+////////////////////////////////////////////////////////////////////////////////
+// Implementation
+////////////////////////////////////////////////////////////////////////////////
+
+Eigen::Vector4d ComposeIdentityQuaternion() {
+  return Eigen::Vector4d(1, 0, 0, 0);
+}
+
+}  // namespace colmap
+
+#endif  // COLMAP_SRC_BASE_POSE_H_

colmap/include/colmap/base/projection.h

@@ -0,0 +1,167 @@
+// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//
+//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
+//       its contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
+
+#ifndef COLMAP_SRC_BASE_PROJECTION_H_
+#define COLMAP_SRC_BASE_PROJECTION_H_
+
+#include <limits>
+#include <vector>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include "base/camera.h"
+
+namespace colmap {
+
+// Compose projection matrix from rotation and translation components.
+//
+// The projection matrix transforms 3D world to image points.
+//
+// @param qvec           Unit Quaternion rotation coefficients (w, x, y, z).
+// @param tvec           3x1 translation vector.
+//
+// @return               3x4 projection matrix.
+Eigen::Matrix3x4d ComposeProjectionMatrix(const Eigen::Vector4d& qvec,
+                                          const Eigen::Vector3d& tvec);
+
+// Compose projection matrix from rotation matrix and translation components).
+//
+// The projection matrix transforms 3D world to image points.
+//
+// @param R              3x3 rotation matrix.
+// @param t              3x1 translation vector.
+//
+// @return               3x4 projection matrix.
+Eigen::Matrix3x4d ComposeProjectionMatrix(const Eigen::Matrix3d& R,
+                                          const Eigen::Vector3d& T);
+
+// Invert projection matrix, defined as:
+//
+//    P = [R | t] with R \in SO(3) and t \in R^3
+//
+// and the inverse projection matrix as:
+//
+//    P' = [R^T | -R^T t]
+//
+// @param proj_matrix    3x4 projection matrix.
+//
+// @return               3x4 inverse projection matrix.
+Eigen::Matrix3x4d InvertProjectionMatrix(const Eigen::Matrix3x4d& proj_matrix);
+
+// Compute the closes rotation matrix with the closest Frobenius norm by setting
+// the singular values of the given matrix to 1.
+Eigen::Matrix3d ComputeClosestRotationMatrix(const Eigen::Matrix3d& matrix);
+
+// Decompose projection matrix into intrinsic camera matrix, rotation matrix and
+// translation vector. Returns false if decomposition fails. This implementation
+// is inspired by the OpenCV implementation with some additional checks.
+bool DecomposeProjectionMatrix(const Eigen::Matrix3x4d& proj_matrix,
+                               Eigen::Matrix3d* K, Eigen::Matrix3d* R,
+                               Eigen::Vector3d* T);
+
+// Project 3D point to image.
+//
+// @param points3D         3D world point as 3x1 vector.
+// @param proj_matrix      3x4 projection matrix.
+// @param camera           Camera used to project to image plane.
+//
+// @return                 Projected image point.
+Eigen::Vector2d ProjectPointToImage(const Eigen::Vector3d& point3D,
+                                    const Eigen::Matrix3x4d& proj_matrix,
+                                    const Camera& camera);
+
+// Calculate the reprojection error.
+//
+// The reprojection error is the Euclidean distance between the observation
+// in the image and the projection of the 3D point into the image. If the
+// 3D point is behind the camera, then this function returns DBL_MAX.
+double CalculateSquaredReprojectionError(const Eigen::Vector2d& point2D,
+                                         const Eigen::Vector3d& point3D,
+                                         const Eigen::Vector4d& qvec,
+                                         const Eigen::Vector3d& tvec,
+                                         const Camera& camera);
+double CalculateSquaredReprojectionError(const Eigen::Vector2d& point2D,
+                                         const Eigen::Vector3d& point3D,
+                                         const Eigen::Matrix3x4d& proj_matrix,
+                                         const Camera& camera);
+
+// Calculate the angular error.
+//
+// The angular error is the angle between the observed viewing ray and the
+// actual viewing ray from the camera center to the 3D point.
+double CalculateAngularError(const Eigen::Vector2d& point2D,
+                             const Eigen::Vector3d& point3D,
+                             const Eigen::Vector4d& qvec,
+                             const Eigen::Vector3d& tvec, const Camera& camera);
+double CalculateAngularError(const Eigen::Vector2d& point2D,
+                             const Eigen::Vector3d& point3D,
+                             const Eigen::Matrix3x4d& proj_matrix,
+                             const Camera& camera);
+
+// Calculate angulate error using normalized image points.
+//
+// The angular error is the angle between the observed viewing ray and the
+// actual viewing ray from the camera center to the 3D point.
+double CalculateNormalizedAngularError(const Eigen::Vector2d& point2D,
+                                       const Eigen::Vector3d& point3D,
+                                       const Eigen::Vector4d& qvec,
+                                       const Eigen::Vector3d& tvec);
+double CalculateNormalizedAngularError(const Eigen::Vector2d& point2D,
+                                       const Eigen::Vector3d& point3D,
+                                       const Eigen::Matrix3x4d& proj_matrix);
+
+// Calculate depth of 3D point with respect to camera.
+//
+// The depth is defined as the Euclidean distance of a 3D point from the
+// camera and is positive if the 3D point is in front and negative if
+// behind of the camera.
+//
+// @param proj_matrix     3x4 projection matrix.
+// @param point3D         3D point as 3x1 vector.
+//
+// @return                Depth of 3D point.
+double CalculateDepth(const Eigen::Matrix3x4d& proj_matrix,
+                      const Eigen::Vector3d& point3D);
+
+// Check if 3D point passes cheirality constraint,
+// i.e. it lies in front of the camera and not in the image plane.
+//
+// @param proj_matrix     3x4 projection matrix.
+// @param point3D         3D point as 3x1 vector.
+//
+// @return                True if point lies in front of camera.
+bool HasPointPositiveDepth(const Eigen::Matrix3x4d& proj_matrix,
+                           const Eigen::Vector3d& point3D);
+
+}  // namespace colmap
+
+#endif  // COLMAP_SRC_BASE_PROJECTION_H_

colmap/include/colmap/base/reconstruction.h

@@ -0,0 +1,651 @@
+// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//
+//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
+//       its contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
+
+#ifndef COLMAP_SRC_BASE_RECONSTRUCTION_H_
+#define COLMAP_SRC_BASE_RECONSTRUCTION_H_
+
+#include <tuple>
+#include <unordered_map>
+#include <unordered_set>
+#include <vector>
+
+#include <Eigen/Core>
+
+#include "base/camera.h"
+#include "base/database.h"
+#include "base/image.h"
+#include "base/point2d.h"
+#include "base/point3d.h"
+#include "base/similarity_transform.h"
+#include "base/track.h"
+#include "estimators/similarity_transform.h"
+#include "optim/loransac.h"
+#include "util/alignment.h"
+#include "util/types.h"
+
+namespace colmap {
+
+struct PlyPoint;
+struct RANSACOptions;
+class DatabaseCache;
+class CorrespondenceGraph;
+
+// Reconstruction class holds all information about a single reconstructed
+// model. It is used by the mapping and bundle adjustment classes and can be
+// written to and read from disk.
+class Reconstruction {
+ public:
+  struct ImagePairStat {
+    // The number of triangulated correspondences between two images.
+    size_t num_tri_corrs = 0;
+    // The number of total correspondences/matches between two images.
+    size_t num_total_corrs = 0;
+  };
+
+  Reconstruction();
+
+  // Get number of objects.
+  inline size_t NumCameras() const;
+  inline size_t NumImages() const;
+  inline size_t NumRegImages() const;
+  inline size_t NumPoints3D() const;
+  inline size_t NumImagePairs() const;
+
+  // Get const objects.
+  inline const class Camera& Camera(const camera_t camera_id) const;
+  inline const class Image& Image(const image_t image_id) const;
+  inline const class Point3D& Point3D(const point3D_t point3D_id) const;
+  inline const ImagePairStat& ImagePair(const image_pair_t pair_id) const;
+  inline ImagePairStat& ImagePair(const image_t image_id1,
+                                  const image_t image_id2);
+
+  // Get mutable objects.
+  inline class Camera& Camera(const camera_t camera_id);
+  inline class Image& Image(const image_t image_id);
+  inline class Point3D& Point3D(const point3D_t point3D_id);
+  inline ImagePairStat& ImagePair(const image_pair_t pair_id);
+  inline const ImagePairStat& ImagePair(const image_t image_id1,
+                                        const image_t image_id2) const;
+
+  // Get reference to all objects.
+  inline const EIGEN_STL_UMAP(camera_t, class Camera) & Cameras() const;
+  inline const EIGEN_STL_UMAP(image_t, class Image) & Images() const;
+  inline const std::vector<image_t>& RegImageIds() const;
+  inline const EIGEN_STL_UMAP(point3D_t, class Point3D) & Points3D() const;
+  inline const std::unordered_map<image_pair_t, ImagePairStat>& ImagePairs()
+      const;
+
+  // Identifiers of all 3D points.
+  std::unordered_set<point3D_t> Point3DIds() const;
+
+  // Check whether specific object exists.
+  inline bool ExistsCamera(const camera_t camera_id) const;
+  inline bool ExistsImage(const image_t image_id) const;
+  inline bool ExistsPoint3D(const point3D_t point3D_id) const;
+  inline bool ExistsImagePair(const image_pair_t pair_id) const;
+
+  // Load data from given `DatabaseCache`.
+  void Load(const DatabaseCache& database_cache);
+
+  // Setup all relevant data structures before reconstruction. Note the
+  // correspondence graph object must live until `TearDown` is called.
+  void SetUp(const CorrespondenceGraph* correspondence_graph);
+
+  // Finalize the Reconstruction after the reconstruction has finished.
+  //
+  // Once a scene has been finalized, it cannot be used for reconstruction.
+  //
+  // This removes all not yet registered images and unused cameras, in order to
+  // save memory.
+  void TearDown();
+
+  // Add new camera. There is only one camera per image, while multiple images
+  // might be taken by the same camera.
+  void AddCamera(class Camera camera);
+
+  // Add new image.
+  void AddImage(class Image image);
+
+  // Add new 3D object, and return its unique ID.
+  point3D_t AddPoint3D(
+      const Eigen::Vector3d& xyz, Track track,
+      const Eigen::Vector3ub& color = Eigen::Vector3ub::Zero());
+
+  // Add observation to existing 3D point.
+  void AddObservation(const point3D_t point3D_id, const TrackElement& track_el);
+
+  // Merge two 3D points and return new identifier of new 3D point.
+  // The location of the merged 3D point is a weighted average of the two
+  // original 3D point's locations according to their track lengths.
+  point3D_t MergePoints3D(const point3D_t point3D_id1,
+                          const point3D_t point3D_id2);
+
+  // Delete a 3D point, and all its references in the observed images.
+  void DeletePoint3D(const point3D_t point3D_id);
+
+  // Delete one observation from an image and the corresponding 3D point.
+  // Note that this deletes the entire 3D point, if the track has two elements
+  // prior to calling this method.
+  void DeleteObservation(const image_t image_id, const point2D_t point2D_idx);
+
+  // Delete all 2D points of all images and all 3D points.
+  void DeleteAllPoints2DAndPoints3D();
+
+  // Register an existing image.
+  void RegisterImage(const image_t image_id);
+
+  // De-register an existing image, and all its references.
+  void DeRegisterImage(const image_t image_id);
+
+  // Check if image is registered.
+  inline bool IsImageRegistered(const image_t image_id) const;
+
+  // Normalize scene by scaling and translation to avoid degenerate
+  // visualization after bundle adjustment and to improve numerical
+  // stability of algorithms.
+  //
+  // Translates scene such that the mean of the camera centers or point
+  // locations are at the origin of the coordinate system.
+  //
+  // Scales scene such that the minimum and maximum camera centers are at the
+  // given `extent`, whereas `p0` and `p1` determine the minimum and
+  // maximum percentiles of the camera centers considered.
+  void Normalize(const double extent = 10.0, const double p0 = 0.1,
+                 const double p1 = 0.9, const bool use_images = true);
+
+  // Compute the centroid of the 3D points
+  Eigen::Vector3d ComputeCentroid(const double p0 = 0.1,
+                                  const double p1 = 0.9) const;
+
+  // Compute the bounding box corners of the 3D points
+  std::pair<Eigen::Vector3d, Eigen::Vector3d> ComputeBoundingBox(
+      const double p0 = 0.0, const double p1 = 1.0) const;
+
+  // Apply the 3D similarity transformation to all images and points.
+  void Transform(const SimilarityTransform3& tform);
+
+  // Creates a cropped reconstruction using the input bounds as corner points
+  // of the bounding box containing the included 3D points of the new
+  // reconstruction. Only the cameras and images of the included points are
+  // registered.
+  Reconstruction Crop(
+      const std::pair<Eigen::Vector3d, Eigen::Vector3d>& bbox) const;
+
+  // Merge the given reconstruction into this reconstruction by registering the
+  // images registered in the given but not in this reconstruction and by
+  // merging the two clouds and their tracks. The coordinate frames of the two
+  // reconstructions are aligned using the projection centers of common
+  // registered images. Return true if the two reconstructions could be merged.
+  bool Merge(const Reconstruction& reconstruction,
+             const double max_reproj_error);
+
+  // Align the given reconstruction with a set of pre-defined camera positions.
+  // Assuming that locations[i] gives the 3D coordinates of the center
+  // of projection of the image with name image_names[i].
+  template <bool kEstimateScale = true>
+  bool Align(const std::vector<std::string>& image_names,
+             const std::vector<Eigen::Vector3d>& locations,
+             const int min_common_images,
+             SimilarityTransform3* tform = nullptr);
+
+  // Robust alignment using RANSAC.
+  template <bool kEstimateScale = true>
+  bool AlignRobust(const std::vector<std::string>& image_names,
+                   const std::vector<Eigen::Vector3d>& locations,
+                   const int min_common_images,
+                   const RANSACOptions& ransac_options,
+                   SimilarityTransform3* tform = nullptr);
+
+  // Find specific image by name. Note that this uses linear search.
+  const class Image* FindImageWithName(const std::string& name) const;
+
+  // Find images that are both present in this and the given reconstruction.
+  std::vector<image_t> FindCommonRegImageIds(
+      const Reconstruction& reconstruction) const;
+
+  // Update the image identifiers to match the ones in the database by matching
+  // the names of the images.
+  void TranscribeImageIdsToDatabase(const Database& database);
+
+  // Filter 3D points with large reprojection error, negative depth, or
+  // insufficient triangulation angle.
+  //
+  // @param max_reproj_error    The maximum reprojection error.
+  // @param min_tri_angle       The minimum triangulation angle.
+  // @param point3D_ids         The points to be filtered.
+  //
+  // @return                    The number of filtered observations.
+  size_t FilterPoints3D(const double max_reproj_error,
+                        const double min_tri_angle,
+                        const std::unordered_set<point3D_t>& point3D_ids);
+  size_t FilterPoints3DInImages(const double max_reproj_error,
+                                const double min_tri_angle,
+                                const std::unordered_set<image_t>& image_ids);
+  size_t FilterAllPoints3D(const double max_reproj_error,
+                           const double min_tri_angle);
+
+  // Filter observations that have negative depth.
+  //
+  // @return    The number of filtered observations.
+  size_t FilterObservationsWithNegativeDepth();
+
+  // Filter images without observations or bogus camera parameters.
+  //
+  // @return    The identifiers of the filtered images.
+  std::vector<image_t> FilterImages(const double min_focal_length_ratio,
+                                    const double max_focal_length_ratio,
+                                    const double max_extra_param);
+
+  // Compute statistics for scene.
+  size_t ComputeNumObservations() const;
+  double ComputeMeanTrackLength() const;
+  double ComputeMeanObservationsPerRegImage() const;
+  double ComputeMeanReprojectionError() const;
+
+  // Read data from text or binary file. Prefer binary data if it exists.
+  void Read(const std::string& path);
+  void Write(const std::string& path) const;
+
+  // Read data from binary/text file.
+  void ReadText(const std::string& path);
+  void ReadBinary(const std::string& path);
+
+  // Write data from binary/text file.
+  void WriteText(const std::string& path) const;
+  void WriteBinary(const std::string& path) const;
+
+  // Convert 3D points in reconstruction to PLY point cloud.
+  std::vector<PlyPoint> ConvertToPLY() const;
+
+  // Import from other data formats. Note that these import functions are
+  // only intended for visualization of data and usable for reconstruction.
+  void ImportPLY(const std::string& path);
+  void ImportPLY(const std::vector<PlyPoint>& ply_points);
+
+  // Export to other data formats.
+
+  // Exports in NVM format http://ccwu.me/vsfm/doc.html#nvm. Only supports
+  // SIMPLE_RADIAL camera model when exporting distortion parameters. When
+  // skip_distortion == true it supports all camera models with the caveat that
+  // it's using the mean focal length which will be inaccurate for camera models
+  // with two focal lengths and distortion.
+  bool ExportNVM(const std::string& path, bool skip_distortion = false) const;
+
+  // Exports in CAM format which is a simple text file that contains pose
+  // information and camera intrinsics for each image and exports one file per
+  // image; it does not include information on the 3D points. The format is as
+  // follows (2 lines of text with space separated numbers):
+  // <Tvec; 3 values> <Rotation matrix in row-major format; 9 values>
+  // <focal_length> <k1> <k2> 1.0 <principal point X> <principal point Y>
+  // Note that focal length is relative to the image max(width, height),
+  // and principal points x and y are relative to width and height respectively.
+  //
+  // Only supports SIMPLE_RADIAL and RADIAL camera models when exporting
+  // distortion parameters. When skip_distortion == true it supports all camera
+  // models with the caveat that it's using the mean focal length which will be
+  // inaccurate for camera models with two focal lengths and distortion.
+  bool ExportCam(const std::string& path, bool skip_distortion = false) const;
+
+  // Exports in Recon3D format which consists of three text files with the
+  // following format and content:
+  // 1) imagemap_0.txt: a list of image numeric IDs with one entry per line.
+  // 2) urd-images.txt: A list of images with one entry per line as:
+  //    <image file name> <width> <height>
+  // 3) synth_0.out: Contains information for image poses, camera intrinsics,
+  //    and 3D points as:
+  //    <N; num images> <M; num points>
+  //    <N lines of image entries>
+  //    <M lines of point entries>
+  //
+  //    Each image entry consists of 5 lines as:
+  //    <focal length> <k1> <k2>
+  //    <Rotation matrix; 3x3 array>
+  //    <Tvec; 3 values>
+  //    Note that the focal length is scaled by 1 / max(width, height)
+  //
+  //    Each point entry consists of 3 lines as:
+  //    <point x, y, z coordinates>
+  //    <point RGB color>
+  //    <K; num track elements> <Track Element 1> ... <Track Element K>
+  //
+  //    Each track elemenet is a sequence of 5 values as:
+  //    <image ID> <2D point ID> -1.0 <X> <Y>
+  //    Note that the 2D point coordinates are centered around the principal
+  //    point and scaled by 1 / max(width, height).
+  //
+  // When skip_distortion == true it supports all camera models with the
+  // caveat that it's using the mean focal length which will be inaccurate
+  // for camera models with two focal lengths and distortion.
+  bool ExportRecon3D(const std::string& path,
+                     bool skip_distortion = false) const;
+
+  // Exports in Bundler format https://www.cs.cornell.edu/~snavely/bundler/.
+  // Supports SIMPLE_PINHOLE, PINHOLE, SIMPLE_RADIAL and RADIAL camera models
+  // when exporting distortion parameters. When skip_distortion == true it
+  // supports all camera models with the caveat that it's using the mean focal
+  // length which will be inaccurate for camera models with two focal lengths
+  // and distortion.
+  bool ExportBundler(const std::string& path, const std::string& list_path,
+                     bool skip_distortion = false) const;
+
+  // Exports 3D points only in PLY format.
+  void ExportPLY(const std::string& path) const;
+
+  // Exports in VRML format https://en.wikipedia.org/wiki/VRML.
+  void ExportVRML(const std::string& images_path,
+                  const std::string& points3D_path, const double image_scale,
+                  const Eigen::Vector3d& image_rgb) const;
+
+  // Extract colors for 3D points of given image. Colors will be extracted
+  // only for 3D points which are completely black.
+  //
+  // @param image_id      Identifier of the image for which to extract colors.
+  // @param path          Absolute or relative path to root folder of image.
+  //                      The image path is determined by concatenating the
+  //                      root path and the name of the image.
+  //
+  // @return              True if image could be read at given path.
+  bool ExtractColorsForImage(const image_t image_id, const std::string& path);
+
+  // Extract colors for all 3D points by computing the mean color of all images.
+  //
+  // @param path          Absolute or relative path to root folder of image.
+  //                      The image path is determined by concatenating the
+  //                      root path and the name of the image.
+  void ExtractColorsForAllImages(const std::string& path);
+
+  // Create all image sub-directories in the given path.
+  void CreateImageDirs(const std::string& path) const;
+
+ private:
+  size_t FilterPoints3DWithSmallTriangulationAngle(
+      const double min_tri_angle,
+      const std::unordered_set<point3D_t>& point3D_ids);
+  size_t FilterPoints3DWithLargeReprojectionError(
+      const double max_reproj_error,
+      const std::unordered_set<point3D_t>& point3D_ids);
+
+  std::tuple<Eigen::Vector3d, Eigen::Vector3d, Eigen::Vector3d>
+  ComputeBoundsAndCentroid(const double p0, const double p1,
+                           const bool use_images) const;
+
+  void ReadCamerasText(const std::string& path);
+  void ReadImagesText(const std::string& path);
+  void ReadPoints3DText(const std::string& path);
+  void ReadCamerasBinary(const std::string& path);
+  void ReadImagesBinary(const std::string& path);
+  void ReadPoints3DBinary(const std::string& path);
+
+  void WriteCamerasText(const std::string& path) const;
+  void WriteImagesText(const std::string& path) const;
+  void WritePoints3DText(const std::string& path) const;
+  void WriteCamerasBinary(const std::string& path) const;
+  void WriteImagesBinary(const std::string& path) const;
+  void WritePoints3DBinary(const std::string& path) const;
+
+  void SetObservationAsTriangulated(const image_t image_id,
+                                    const point2D_t point2D_idx,
+                                    const bool is_continued_point3D);
+  void ResetTriObservations(const image_t image_id, const point2D_t point2D_idx,
+                            const bool is_deleted_point3D);
+
+  const CorrespondenceGraph* correspondence_graph_;
+
+  EIGEN_STL_UMAP(camera_t, class Camera) cameras_;
+  EIGEN_STL_UMAP(image_t, class Image) images_;
+  EIGEN_STL_UMAP(point3D_t, class Point3D) points3D_;
+
+  std::unordered_map<image_pair_t, ImagePairStat> image_pair_stats_;
+
+  // { image_id, ... } where `images_.at(image_id).registered == true`.
+  std::vector<image_t> reg_image_ids_;
+
+  // Total number of added 3D points, used to generate unique identifiers.
+  point3D_t num_added_points3D_;
+};
+
+////////////////////////////////////////////////////////////////////////////////
+// Implementation
+////////////////////////////////////////////////////////////////////////////////
+
+size_t Reconstruction::NumCameras() const { return cameras_.size(); }
+
+size_t Reconstruction::NumImages() const { return images_.size(); }
+
+size_t Reconstruction::NumRegImages() const { return reg_image_ids_.size(); }
+
+size_t Reconstruction::NumPoints3D() const { return points3D_.size(); }
+
+size_t Reconstruction::NumImagePairs() const {
+  return image_pair_stats_.size();
+}
+
+const class Camera& Reconstruction::Camera(const camera_t camera_id) const {
+  return cameras_.at(camera_id);
+}
+
+const class Image& Reconstruction::Image(const image_t image_id) const {
+  return images_.at(image_id);
+}
+
+const class Point3D& Reconstruction::Point3D(const point3D_t point3D_id) const {
+  return points3D_.at(point3D_id);
+}
+
+const Reconstruction::ImagePairStat& Reconstruction::ImagePair(
+    const image_pair_t pair_id) const {
+  return image_pair_stats_.at(pair_id);
+}
+
+const Reconstruction::ImagePairStat& Reconstruction::ImagePair(
+    const image_t image_id1, const image_t image_id2) const {
+  const auto pair_id = Database::ImagePairToPairId(image_id1, image_id2);
+  return image_pair_stats_.at(pair_id);
+}
+
+class Camera& Reconstruction::Camera(const camera_t camera_id) {
+  return cameras_.at(camera_id);
+}
+
+class Image& Reconstruction::Image(const image_t image_id) {
+  return images_.at(image_id);
+}
+
+class Point3D& Reconstruction::Point3D(const point3D_t point3D_id) {
+  return points3D_.at(point3D_id);
+}
+
+Reconstruction::ImagePairStat& Reconstruction::ImagePair(
+    const image_pair_t pair_id) {
+  return image_pair_stats_.at(pair_id);
+}
+
+Reconstruction::ImagePairStat& Reconstruction::ImagePair(
+    const image_t image_id1, const image_t image_id2) {
+  const auto pair_id = Database::ImagePairToPairId(image_id1, image_id2);
+  return image_pair_stats_.at(pair_id);
+}
+
+const EIGEN_STL_UMAP(camera_t, Camera) & Reconstruction::Cameras() const {
+  return cameras_;
+}
+
+const EIGEN_STL_UMAP(image_t, class Image) & Reconstruction::Images() const {
+  return images_;
+}
+
+const std::vector<image_t>& Reconstruction::RegImageIds() const {
+  return reg_image_ids_;
+}
+
+const EIGEN_STL_UMAP(point3D_t, Point3D) & Reconstruction::Points3D() const {
+  return points3D_;
+}
+
+const std::unordered_map<image_pair_t, Reconstruction::ImagePairStat>&
+Reconstruction::ImagePairs() const {
+  return image_pair_stats_;
+}
+
+bool Reconstruction::ExistsCamera(const camera_t camera_id) const {
+  return cameras_.find(camera_id) != cameras_.end();
+}
+
+bool Reconstruction::ExistsImage(const image_t image_id) const {
+  return images_.find(image_id) != images_.end();
+}
+
+bool Reconstruction::ExistsPoint3D(const point3D_t point3D_id) const {
+  return points3D_.find(point3D_id) != points3D_.end();
+}
+
+bool Reconstruction::ExistsImagePair(const image_pair_t pair_id) const {
+  return image_pair_stats_.find(pair_id) != image_pair_stats_.end();
+}
+
+bool Reconstruction::IsImageRegistered(const image_t image_id) const {
+  return Image(image_id).IsRegistered();
+}
+
+template <bool kEstimateScale>
+bool Reconstruction::Align(const std::vector<std::string>& image_names,
+                           const std::vector<Eigen::Vector3d>& locations,
+                           const int min_common_images,
+                           SimilarityTransform3* tform) {
+  CHECK_GE(min_common_images, 3);
+  CHECK_EQ(image_names.size(), locations.size());
+
+  // Find out which images are contained in the reconstruction and get the
+  // positions of their camera centers.
+  std::unordered_set<image_t> common_image_ids;
+  std::vector<Eigen::Vector3d> src;
+  std::vector<Eigen::Vector3d> dst;
+  for (size_t i = 0; i < image_names.size(); ++i) {
+    const class Image* image = FindImageWithName(image_names[i]);
+    if (image == nullptr) {
+      continue;
+    }
+
+    if (!IsImageRegistered(image->ImageId())) {
+      continue;
+    }
+
+    // Ignore duplicate images.
+    if (common_image_ids.count(image->ImageId()) > 0) {
+      continue;
+    }
+
+    common_image_ids.insert(image->ImageId());
+    src.push_back(image->ProjectionCenter());
+    dst.push_back(locations[i]);
+  }
+
+  // Only compute the alignment if there are enough correspondences.
+  if (common_image_ids.size() < static_cast<size_t>(min_common_images)) {
+    return false;
+  }
+
+  SimilarityTransform3 transform;
+  if (!transform.Estimate<kEstimateScale>(src, dst)) {
+    return false;
+  }
+
+  Transform(transform);
+
+  if (tform != nullptr) {
+    *tform = transform;
+  }
+
+  return true;
+}
+
+template <bool kEstimateScale>
+bool Reconstruction::AlignRobust(const std::vector<std::string>& image_names,
+                                 const std::vector<Eigen::Vector3d>& locations,
+                                 const int min_common_images,
+                                 const RANSACOptions& ransac_options,
+                                 SimilarityTransform3* tform) {
+  CHECK_GE(min_common_images, 3);
+  CHECK_EQ(image_names.size(), locations.size());
+
+  // Find out which images are contained in the reconstruction and get the
+  // positions of their camera centers.
+  std::unordered_set<image_t> common_image_ids;
+  std::vector<Eigen::Vector3d> src;
+  std::vector<Eigen::Vector3d> dst;
+  for (size_t i = 0; i < image_names.size(); ++i) {
+    const class Image* image = FindImageWithName(image_names[i]);
+    if (image == nullptr) {
+      continue;
+    }
+
+    if (!IsImageRegistered(image->ImageId())) {
+      continue;
+    }
+
+    // Ignore duplicate images.
+    if (common_image_ids.count(image->ImageId()) > 0) {
+      continue;
+    }
+
+    common_image_ids.insert(image->ImageId());
+    src.push_back(image->ProjectionCenter());
+    dst.push_back(locations[i]);
+  }
+
+  // Only compute the alignment if there are enough correspondences.
+  if (common_image_ids.size() < static_cast<size_t>(min_common_images)) {
+    return false;
+  }
+
+  LORANSAC<SimilarityTransformEstimator<3, kEstimateScale>,
+           SimilarityTransformEstimator<3, kEstimateScale>>
+      ransac(ransac_options);
+
+  const auto report = ransac.Estimate(src, dst);
+
+  if (report.support.num_inliers < static_cast<size_t>(min_common_images)) {
+    return false;
+  }
+
+  SimilarityTransform3 transform = SimilarityTransform3(report.model);
+  Transform(transform);
+
+  if (tform != nullptr) {
+    *tform = transform;
+  }
+
+  return true;
+}
+
+}  // namespace colmap
+
+#endif  // COLMAP_SRC_BASE_RECONSTRUCTION_H_

colmap/include/colmap/base/reconstruction_manager.h

@@ -0,0 +1,81 @@
+// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//
+//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
+//       its contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
+
+#ifndef COLMAP_SRC_BASE_RECONSTRUCTION_MANAGER_H_
+#define COLMAP_SRC_BASE_RECONSTRUCTION_MANAGER_H_
+
+#include "base/reconstruction.h"
+
+namespace colmap {
+
+class OptionManager;
+
+class ReconstructionManager {
+ public:
+  ReconstructionManager();
+
+  // Move constructor and assignment.
+  ReconstructionManager(ReconstructionManager&& other);
+  ReconstructionManager& operator=(ReconstructionManager&& other);
+
+  // The number of reconstructions managed.
+  size_t Size() const;
+
+  // Get a reference to a specific reconstruction.
+  const Reconstruction& Get(const size_t idx) const;
+  Reconstruction& Get(const size_t idx);
+
+  // Add a new empty reconstruction and return its index.
+  size_t Add();
+
+  // Delete a specific reconstruction.
+  void Delete(const size_t idx);
+
+  // Delete all reconstructions.
+  void Clear();
+
+  // Read and add a new reconstruction and return its index.
+  size_t Read(const std::string& path);
+
+  // Write all managed reconstructions into sub-folders "0", "1", "2", ...
+  // If the option manager object is not null, the options are written
+  // to each respective reconstruction folder as well.
+  void Write(const std::string& path, const OptionManager* options) const;
+
+ private:
+  NON_COPYABLE(ReconstructionManager)
+
+  std::vector<std::unique_ptr<Reconstruction>> reconstructions_;
+};
+
+}  // namespace colmap
+
+#endif  // COLMAP_SRC_BASE_RECONSTRUCTION_MANAGER_H_

colmap/include/colmap/base/scene_clustering.h

@@ -0,0 +1,100 @@
+// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//
+//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
+//       its contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
+
+#ifndef COLMAP_SRC_BASE_SCENE_CLUSTERING_H_
+#define COLMAP_SRC_BASE_SCENE_CLUSTERING_H_
+
+#include <list>
+#include <vector>
+
+#include "base/database.h"
+#include "util/types.h"
+
+namespace colmap {
+
+// Scene clustering approach using normalized cuts on the scene graph. The scene
+// is hierarchically partitioned into overlapping clusters until a maximum
+// number of images is in a leaf node.
+class SceneClustering {
+ public:
+  struct Options {
+    // Flag for hierarchical vs flat clustering
+    bool is_hierarchical = true;
+
+    // The branching factor of the hierarchical clustering.
+    int branching = 2;
+
+    // The number of overlapping images between child clusters.
+    int image_overlap = 50;
+
+    // The max related images matches to look for in a flat cluster
+    int num_image_matches = 20;
+
+    // The maximum number of images in a leaf node cluster, otherwise the
+    // cluster is further partitioned using the given branching factor. Note
+    // that a cluster leaf node will have at most `leaf_max_num_images +
+    // overlap` images to satisfy the overlap constraint.
+    int leaf_max_num_images = 500;
+
+    bool Check() const;
+  };
+
+  struct Cluster {
+    std::vector<image_t> image_ids;
+    std::vector<Cluster> child_clusters;
+  };
+
+  SceneClustering(const Options& options);
+
+  void Partition(const std::vector<std::pair<image_t, image_t>>& image_pairs,
+                 const std::vector<int>& num_inliers);
+
+  const Cluster* GetRootCluster() const;
+  std::vector<const Cluster*> GetLeafClusters() const;
+
+  static SceneClustering Create(const Options& options,
+                                const Database& database);
+
+ private:
+  void PartitionHierarchicalCluster(
+      const std::vector<std::pair<int, int>>& edges,
+      const std::vector<int>& weights, Cluster* cluster);
+
+  void PartitionFlatCluster(const std::vector<std::pair<int, int>>& edges,
+                            const std::vector<int>& weights);
+
+  const Options options_;
+  std::unique_ptr<Cluster> root_cluster_;
+};
+
+}  // namespace colmap
+
+#endif  // COLMAP_SRC_BASE_SCENE_CLUSTERING_H_

colmap/include/colmap/base/similarity_transform.h

@@ -0,0 +1,121 @@
+// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//
+//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
+//       its contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
+
+#ifndef COLMAP_SRC_BASE_SIMILARITY_TRANSFORM_H_
+#define COLMAP_SRC_BASE_SIMILARITY_TRANSFORM_H_
+
+#include <vector>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include "estimators/similarity_transform.h"
+#include "util/alignment.h"
+#include "util/types.h"
+
+namespace colmap {
+
+struct RANSACOptions;
+class Reconstruction;
+
+// 3D similarity transformation with 7 degrees of freedom.
+class SimilarityTransform3 {
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  SimilarityTransform3();
+
+  explicit SimilarityTransform3(const Eigen::Matrix3x4d& matrix);
+
+  explicit SimilarityTransform3(
+      const Eigen::Transform<double, 3, Eigen::Affine>& transform);
+
+  SimilarityTransform3(const double scale, const Eigen::Vector4d& qvec,
+                       const Eigen::Vector3d& tvec);
+
+  void Write(const std::string& path);
+
+  template <bool kEstimateScale = true>
+  bool Estimate(const std::vector<Eigen::Vector3d>& src,
+                const std::vector<Eigen::Vector3d>& dst);
+
+  SimilarityTransform3 Inverse() const;
+
+  void TransformPoint(Eigen::Vector3d* xyz) const;
+  void TransformPose(Eigen::Vector4d* qvec, Eigen::Vector3d* tvec) const;
+
+  Eigen::Matrix4d Matrix() const;
+  double Scale() const;
+  Eigen::Vector4d Rotation() const;
+  Eigen::Vector3d Translation() const;
+
+  static SimilarityTransform3 FromFile(const std::string& path);
+
+ private:
+  Eigen::Transform<double, 3, Eigen::Affine> transform_;
+};
+
+// Robustly compute alignment between reconstructions by finding images that
+// are registered in both reconstructions. The alignment is then estimated
+// robustly inside RANSAC from corresponding projection centers. An alignment
+// is verified by reprojecting common 3D point observations.
+// The min_inlier_observations threshold determines how many observations
+// in a common image must reproject within the given threshold.
+bool ComputeAlignmentBetweenReconstructions(
+    const Reconstruction& src_reconstruction,
+    const Reconstruction& ref_reconstruction,
+    const double min_inlier_observations, const double max_reproj_error,
+    Eigen::Matrix3x4d* alignment);
+
+////////////////////////////////////////////////////////////////////////////////
+// Implementation
+////////////////////////////////////////////////////////////////////////////////
+
+template <bool kEstimateScale>
+bool SimilarityTransform3::Estimate(const std::vector<Eigen::Vector3d>& src,
+                                    const std::vector<Eigen::Vector3d>& dst) {
+  const auto results =
+      SimilarityTransformEstimator<3, kEstimateScale>().Estimate(src, dst);
+  if (results.empty()) {
+    return false;
+  }
+
+  CHECK_EQ(results.size(), 1);
+  transform_.matrix().topLeftCorner<3, 4>() = results[0];
+
+  return true;
+}
+
+}  // namespace colmap
+
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(colmap::SimilarityTransform3)
+
+#endif  // COLMAP_SRC_BASE_SIMILARITY_TRANSFORM_H_

colmap/include/colmap/base/track.h

@@ -0,0 +1,139 @@
+// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//
+//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
+//       its contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
+
+#ifndef COLMAP_SRC_BASE_TRACK_H_
+#define COLMAP_SRC_BASE_TRACK_H_
+
+#include <vector>
+
+#include "util/logging.h"
+#include "util/types.h"
+
+namespace colmap {
+
+// Track class stores all observations of a 3D point.
+struct TrackElement {
+  TrackElement();
+  TrackElement(const image_t image_id, const point2D_t point2D_idx);
+  // The image in which the track element is observed.
+  image_t image_id;
+  // The point in the image that the track element is observed.
+  point2D_t point2D_idx;
+};
+
+class Track {
+ public:
+  Track();
+
+  // The number of track elements.
+  inline size_t Length() const;
+
+  // Access all elements.
+  inline const std::vector<TrackElement>& Elements() const;
+  inline std::vector<TrackElement>& Elements();
+  inline void SetElements(std::vector<TrackElement> elements);
+
+  // Access specific elements.
+  inline const TrackElement& Element(const size_t idx) const;
+  inline TrackElement& Element(const size_t idx);
+  inline void SetElement(const size_t idx, const TrackElement& element);
+
+  // Append new elements.
+  inline void AddElement(const TrackElement& element);
+  inline void AddElement(const image_t image_id, const point2D_t point2D_idx);
+  inline void AddElements(const std::vector<TrackElement>& elements);
+
+  // Delete existing element.
+  inline void DeleteElement(const size_t idx);
+  void DeleteElement(const image_t image_id, const point2D_t point2D_idx);
+
+  // Requests that the track capacity be at least enough to contain the
+  // specified number of elements.
+  inline void Reserve(const size_t num_elements);
+
+  // Shrink the capacity of track vector to fit its size to save memory.
+  inline void Compress();
+
+ private:
+  std::vector<TrackElement> elements_;
+};
+
+////////////////////////////////////////////////////////////////////////////////
+// Implementation
+////////////////////////////////////////////////////////////////////////////////
+
+size_t Track::Length() const { return elements_.size(); }
+
+const std::vector<TrackElement>& Track::Elements() const { return elements_; }
+
+std::vector<TrackElement>& Track::Elements() { return elements_; }
+
+void Track::SetElements(std::vector<TrackElement> elements) {
+  elements_ = std::move(elements);
+}
+
+// Access specific elements.
+const TrackElement& Track::Element(const size_t idx) const {
+  return elements_.at(idx);
+}
+
+TrackElement& Track::Element(const size_t idx) { return elements_.at(idx); }
+
+void Track::SetElement(const size_t idx, const TrackElement& element) {
+  elements_.at(idx) = element;
+}
+
+void Track::AddElement(const TrackElement& element) {
+  elements_.push_back(element);
+}
+
+void Track::AddElement(const image_t image_id, const point2D_t point2D_idx) {
+  elements_.emplace_back(image_id, point2D_idx);
+}
+
+void Track::AddElements(const std::vector<TrackElement>& elements) {
+  elements_.insert(elements_.end(), elements.begin(), elements.end());
+}
+
+void Track::DeleteElement(const size_t idx) {
+  CHECK_LT(idx, elements_.size());
+  elements_.erase(elements_.begin() + idx);
+}
+
+void Track::Reserve(const size_t num_elements) {
+  elements_.reserve(num_elements);
+}
+
+void Track::Compress() { elements_.shrink_to_fit(); }
+
+}  // namespace colmap
+
+#endif  // COLMAP_SRC_BASE_TRACK_H_

colmap/include/colmap/base/triangulation.h

@@ -0,0 +1,118 @@
+// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//
+//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
+//       its contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
+
+#ifndef COLMAP_SRC_BASE_TRIANGULATION_H_
+#define COLMAP_SRC_BASE_TRIANGULATION_H_
+
+#include <vector>
+
+#include <Eigen/Core>
+
+#include "base/camera.h"
+#include "util/alignment.h"
+#include "util/math.h"
+#include "util/types.h"
+
+namespace colmap {
+
+// Triangulate 3D point from corresponding image point observations.
+//
+// Implementation of the direct linear transform triangulation method in
+//   R. Hartley and A. Zisserman, Multiple View Geometry in Computer Vision,
+//   Cambridge Univ. Press, 2003.
+//
+// @param proj_matrix1   Projection matrix of the first image as 3x4 matrix.
+// @param proj_matrix2   Projection matrix of the second image as 3x4 matrix.
+// @param point1         Corresponding 2D point in first image.
+// @param point2         Corresponding 2D point in second image.
+//
+// @return               Triangulated 3D point.
+Eigen::Vector3d TriangulatePoint(const Eigen::Matrix3x4d& proj_matrix1,
+                                 const Eigen::Matrix3x4d& proj_matrix2,
+                                 const Eigen::Vector2d& point1,
+                                 const Eigen::Vector2d& point2);
+
+// Triangulate multiple 3D points from multiple image correspondences.
+std::vector<Eigen::Vector3d> TriangulatePoints(
+    const Eigen::Matrix3x4d& proj_matrix1,
+    const Eigen::Matrix3x4d& proj_matrix2,
+    const std::vector<Eigen::Vector2d>& points1,
+    const std::vector<Eigen::Vector2d>& points2);
+
+// Triangulate point from multiple views minimizing the L2 error.
+//
+// @param proj_matrices       Projection matrices of multi-view observations.
+// @param points              Image observations of multi-view observations.
+//
+// @return                    Estimated 3D point.
+Eigen::Vector3d TriangulateMultiViewPoint(
+    const std::vector<Eigen::Matrix3x4d>& proj_matrices,
+    const std::vector<Eigen::Vector2d>& points);
+
+// Triangulate optimal 3D point from corresponding image point observations by
+// finding the optimal image observations.
+//
+// Note that camera poses should be very good in order for this method to yield
+// good results. Otherwise just use `TriangulatePoint`.
+//
+// Implementation of the method described in
+//   P. Lindstrom, "Triangulation Made Easy," IEEE Computer Vision and Pattern
+//   Recognition 2010, pp. 1554-1561, June 2010.
+//
+// @param proj_matrix1   Projection matrix of the first image as 3x4 matrix.
+// @param proj_matrix2   Projection matrix of the second image as 3x4 matrix.
+// @param point1         Corresponding 2D point in first image.
+// @param point2         Corresponding 2D point in second image.
+//
+// @return               Triangulated optimal 3D point.
+Eigen::Vector3d TriangulateOptimalPoint(const Eigen::Matrix3x4d& proj_matrix1,
+                                        const Eigen::Matrix3x4d& proj_matrix2,
+                                        const Eigen::Vector2d& point1,
+                                        const Eigen::Vector2d& point2);
+
+// Triangulate multiple optimal 3D points from multiple image correspondences.
+std::vector<Eigen::Vector3d> TriangulateOptimalPoints(
+    const Eigen::Matrix3x4d& proj_matrix1,
+    const Eigen::Matrix3x4d& proj_matrix2,
+    const std::vector<Eigen::Vector2d>& points1,
+    const std::vector<Eigen::Vector2d>& points2);
+
+// Calculate angle in radians between the two rays of a triangulated point.
+double CalculateTriangulationAngle(const Eigen::Vector3d& proj_center1,
+                                   const Eigen::Vector3d& proj_center2,
+                                   const Eigen::Vector3d& point3D);
+std::vector<double> CalculateTriangulationAngles(
+    const Eigen::Vector3d& proj_center1, const Eigen::Vector3d& proj_center2,
+    const std::vector<Eigen::Vector3d>& points3D);
+
+}  // namespace colmap
+
+#endif  // COLMAP_SRC_BASE_TRIANGULATION_H_

colmap/include/colmap/base/undistortion.h

@@ -0,0 +1,234 @@
+// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//
+//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
+//       its contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
+
+#ifndef COLMAP_SRC_BASE_UNDISTORTION_H_
+#define COLMAP_SRC_BASE_UNDISTORTION_H_
+
+#include "base/reconstruction.h"
+#include "util/alignment.h"
+#include "util/bitmap.h"
+#include "util/misc.h"
+#include "util/threading.h"
+
+namespace colmap {
+
+struct UndistortCameraOptions {
+  // The amount of blank pixels in the undistorted image in the range [0, 1].
+  double blank_pixels = 0.0;
+
+  // Minimum and maximum scale change of camera used to satisfy the blank
+  // pixel constraint.
+  double min_scale = 0.2;
+  double max_scale = 2.0;
+
+  // Maximum image size in terms of width or height of the undistorted camera.
+  int max_image_size = -1;
+
+  // The 4 factors in the range [0, 1] that define the ROI (region of interest)
+  // in original image. The bounding box pixel coordinates are calculated as
+  //    (roi_min_x * Width, roi_min_y * Height) and
+  //    (roi_max_x * Width, roi_max_y * Height).
+  double roi_min_x = 0.0;
+  double roi_min_y = 0.0;
+  double roi_max_x = 1.0;
+  double roi_max_y = 1.0;
+};
+
+// Undistort images and export undistorted cameras, as required by the
+// mvs::PatchMatchController class.
+class COLMAPUndistorter : public Thread {
+ public:
+  COLMAPUndistorter(
+      const UndistortCameraOptions& options,
+      const Reconstruction& reconstruction, const std::string& image_path,
+      const std::string& output_path, const int num_related_images = 20,
+      const CopyType copy_type = CopyType::COPY,
+      const std::vector<image_t>& image_ids = std::vector<image_t>());
+
+ private:
+  void Run();
+
+  bool Undistort(const image_t image_id) const;
+  void WritePatchMatchConfig() const;
+  void WriteFusionConfig() const;
+  void WriteScript(const bool geometric) const;
+
+  UndistortCameraOptions options_;
+  const std::string image_path_;
+  const std::string output_path_;
+  const CopyType copy_type_;
+  const int num_patch_match_src_images_;
+  const Reconstruction& reconstruction_;
+  const std::vector<image_t> image_ids_;
+  std::vector<std::string> image_names_;
+};
+
+// Undistort images and prepare data for CMVS/PMVS.
+class PMVSUndistorter : public Thread {
+ public:
+  PMVSUndistorter(const UndistortCameraOptions& options,
+                  const Reconstruction& reconstruction,
+                  const std::string& image_path,
+                  const std::string& output_path);
+
+ private:
+  void Run();
+
+  bool Undistort(const size_t reg_image_idx) const;
+  void WriteVisibilityData() const;
+  void WriteOptionFile() const;
+  void WritePMVSScript() const;
+  void WriteCMVSPMVSScript() const;
+  void WriteCOLMAPScript(const bool geometric) const;
+  void WriteCMVSCOLMAPScript(const bool geometric) const;
+
+  UndistortCameraOptions options_;
+  std::string image_path_;
+  std::string output_path_;
+  const Reconstruction& reconstruction_;
+};
+
+// Undistort images and prepare data for CMP-MVS.
+class CMPMVSUndistorter : public Thread {
+ public:
+  CMPMVSUndistorter(const UndistortCameraOptions& options,
+                    const Reconstruction& reconstruction,
+                    const std::string& image_path,
+                    const std::string& output_path);
+
+ private:
+  void Run();
+
+  bool Undistort(const size_t reg_image_idx) const;
+
+  UndistortCameraOptions options_;
+  std::string image_path_;
+  std::string output_path_;
+  const Reconstruction& reconstruction_;
+};
+
+// Undistort images and export undistorted cameras without the need for a
+// reconstruction. Instead, the image names and camera model information are
+// read from a text file.
+class PureImageUndistorter : public Thread {
+ public:
+  PureImageUndistorter(const UndistortCameraOptions& options,
+                       const std::string& image_path,
+                       const std::string& output_path,
+                       const std::vector<std::pair<std::string, Camera>>&
+                           image_names_and_cameras);
+
+ private:
+  void Run();
+
+  bool Undistort(const size_t reg_image_idx) const;
+
+  UndistortCameraOptions options_;
+  std::string image_path_;
+  std::string output_path_;
+  const std::vector<std::pair<std::string, Camera>>& image_names_and_cameras_;
+};
+
+// Rectify stereo image pairs.
+class StereoImageRectifier : public Thread {
+ public:
+  StereoImageRectifier(
+      const UndistortCameraOptions& options,
+      const Reconstruction& reconstruction, const std::string& image_path,
+      const std::string& output_path,
+      const std::vector<std::pair<image_t, image_t>>& stereo_pairs);
+
+ private:
+  void Run();
+
+  void Rectify(const image_t image_id1, const image_t image_id2) const;
+
+  UndistortCameraOptions options_;
+  std::string image_path_;
+  std::string output_path_;
+  const std::vector<std::pair<image_t, image_t>>& stereo_pairs_;
+  const Reconstruction& reconstruction_;
+};
+
+// Undistort camera by resizing the image and shifting the principal point.
+//
+// The scaling factor is computed such that no blank pixels are in the
+// undistorted image (blank_pixels=0) or all pixels in distorted image are
+// contained in output image (blank_pixels=1).
+//
+// The focal length of the image is preserved and the dimensions of the
+// undistorted pinhole camera are adjusted such that either all pixels in
+// the undistorted image have a corresponding pixel in the distorted image
+// (i.e. no blank pixels at the borders, for `blank_pixels=0`), or all pixels
+// in the distorted image project have a corresponding pixel in the undistorted
+// image (i.e. blank pixels at the borders, for `blank_pixels=1`). Intermediate
+// states can be achieved by setting `blank_pixels` between 0 and 1.
+//
+// The relative location of the principal point of the distorted camera is
+// preserved. The scaling of the image dimensions is subject to the `min_scale`,
+// `max_scale`, and `max_image_size` constraints.
+Camera UndistortCamera(const UndistortCameraOptions& options,
+                       const Camera& camera);
+
+// Undistort image such that the viewing geometry of the undistorted image
+// follows a pinhole camera model. See `UndistortCamera` for more details
+// on the undistortion conventions.
+void UndistortImage(const UndistortCameraOptions& options,
+                    const Bitmap& distorted_image,
+                    const Camera& distorted_camera, Bitmap* undistorted_image,
+                    Camera* undistorted_camera);
+
+// Undistort all cameras in the reconstruction and accordingly all
+// observations in their corresponding images.
+void UndistortReconstruction(const UndistortCameraOptions& options,
+                             Reconstruction* reconstruction);
+
+// Compute stereo rectification homographies that transform two images,
+// such that corresponding pixels in one image lie on the same scanline in the
+// other image. The matrix Q transforms disparity values to world coordinates
+// as [x, y, disparity, 1] * Q = [X, Y, Z, 1] * w. Note that this function
+// assumes that the two cameras are already undistorted.
+void RectifyStereoCameras(const Camera& camera1, const Camera& camera2,
+                          const Eigen::Vector4d& qvec,
+                          const Eigen::Vector3d& tvec, Eigen::Matrix3d* H1,
+                          Eigen::Matrix3d* H2, Eigen::Matrix4d* Q);
+
+// Rectify and undistort the stereo image pair using the given geometry.
+void RectifyAndUndistortStereoImages(
+    const UndistortCameraOptions& options, const Bitmap& distorted_image1,
+    const Bitmap& distorted_image2, const Camera& distorted_camera1,
+    const Camera& distorted_camera2, const Eigen::Vector4d& qvec,
+    const Eigen::Vector3d& tvec, Bitmap* undistorted_image1,
+    Bitmap* undistorted_image2, Camera* undistorted_camera, Eigen::Matrix4d* Q);
+
+}  // namespace colmap
+
+#endif  // COLMAP_SRC_BASE_UNDISTORTION_H_

colmap/include/colmap/base/visibility_pyramid.h

@@ -0,0 +1,104 @@
+// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//
+//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
+//       its contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
+
+#ifndef COLMAP_SRC_BASE_VISIBILITY_PYRAMID_H_
+#define COLMAP_SRC_BASE_VISIBILITY_PYRAMID_H_
+
+#include <vector>
+
+#include <Eigen/Core>
+
+#include "util/alignment.h"
+
+namespace colmap {
+
+// A class that captures the distribution of points in a 2D grid.
+// For example, to capture the distribution of visible 3D points in an image.
+//
+// The class captures the distribution of points by a score. A higher score
+// corresponds to a more uniform distribution of the points in the grid.
+//
+// The score is computed by the number of populated cells in a multi-resolution
+// pyramid. A populated cell contributes to the overall score if it is
+// populated by at least one point and the contributed score is according
+// to its resolution in the pyramid. A cell in a higher resolution level
+// contributes a higher score to the overall score.
+class VisibilityPyramid {
+ public:
+  VisibilityPyramid();
+  VisibilityPyramid(const size_t num_levels, const size_t width,
+                    const size_t height);
+
+  void SetPoint(const double x, const double y);
+  void ResetPoint(const double x, const double y);
+
+  inline size_t NumLevels() const;
+  inline size_t Width() const;
+  inline size_t Height() const;
+
+  inline size_t Score() const;
+  inline size_t MaxScore() const;
+
+ private:
+  void CellForPoint(const double x, const double y, size_t* cx,
+                    size_t* cy) const;
+
+  // Range of the input points.
+  size_t width_;
+  size_t height_;
+
+  // The overall visibility score.
+  size_t score_;
+
+  // The maximum score when all cells are populated.
+  size_t max_score_;
+
+  // The visibilty pyramid with multiple levels.
+  std::vector<Eigen::MatrixXi> pyramid_;
+};
+
+////////////////////////////////////////////////////////////////////////////////
+// Implementation
+////////////////////////////////////////////////////////////////////////////////
+
+size_t VisibilityPyramid::NumLevels() const { return pyramid_.size(); }
+
+size_t VisibilityPyramid::Width() const { return width_; }
+
+size_t VisibilityPyramid::Height() const { return height_; }
+
+size_t VisibilityPyramid::Score() const { return score_; }
+
+size_t VisibilityPyramid::MaxScore() const { return max_score_; }
+
+}  // namespace colmap
+
+#endif  // COLMAP_SRC_BASE_VISIBILITY_PYRAMID_H_

colmap/include/colmap/base/warp.h

@@ -0,0 +1,80 @@
+// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//
+//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
+//       its contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
+
+#ifndef COLMAP_SRC_BASE_WARP_H_
+#define COLMAP_SRC_BASE_WARP_H_
+
+#include "base/camera.h"
+#include "util/alignment.h"
+#include "util/bitmap.h"
+
+namespace colmap {
+
+// Warp source image to target image by projecting the pixels of the target
+// image up to infinity and projecting it down into the source image
+// (i.e. an inverse mapping). The function allocates the target image.
+void WarpImageBetweenCameras(const Camera& source_camera,
+                             const Camera& target_camera,
+                             const Bitmap& source_image, Bitmap* target_image);
+
+// Warp an image with the given homography, where H defines the pixel mapping
+// from the target to source image. Note that the pixel centers are assumed to
+// have coordinates (0.5, 0.5).
+void WarpImageWithHomography(const Eigen::Matrix3d& H,
+                             const Bitmap& source_image, Bitmap* target_image);
+
+// First, warp source image to target image by projecting the pixels of the
+// target image up to infinity and projecting it down into the source image
+// (i.e. an inverse mapping). Second, warp the coordinates from the first
+// warping with the given homography. The function allocates the target image.
+void WarpImageWithHomographyBetweenCameras(const Eigen::Matrix3d& H,
+                                           const Camera& source_camera,
+                                           const Camera& target_camera,
+                                           const Bitmap& source_image,
+                                           Bitmap* target_image);
+
+// Resample row-major image using bilinear interpolation.
+void ResampleImageBilinear(const float* data, const int rows, const int cols,
+                           const int new_rows, const int new_cols,
+                           float* resampled);
+
+// Smooth row-major image using a Gaussian filter kernel.
+void SmoothImage(const float* data, const int rows, const int cols,
+                 const float sigma_r, const float sigma_c, float* smoothed);
+
+// Downsample row-major image by first smoothing and then resampling.
+void DownsampleImage(const float* data, const int rows, const int cols,
+                     const int new_rows, const int new_cols,
+                     float* downsampled);
+
+}  // namespace colmap
+
+#endif  // COLMAP_SRC_BASE_WARP_H_

colmap/include/colmap/controllers/automatic_reconstruction.h

@@ -0,0 +1,123 @@
+// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//
+//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
+//       its contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
+
+#ifndef COLMAP_SRC_CONTROLLERS_AUTOMATIC_RECONSTRUCTION_H_
+#define COLMAP_SRC_CONTROLLERS_AUTOMATIC_RECONSTRUCTION_H_
+
+#include <string>
+
+#include "base/reconstruction_manager.h"
+#include "util/option_manager.h"
+#include "util/threading.h"
+
+namespace colmap {
+
+class AutomaticReconstructionController : public Thread {
+ public:
+  enum class DataType { INDIVIDUAL, VIDEO, INTERNET };
+  enum class Quality { LOW, MEDIUM, HIGH, EXTREME };
+  enum class Mesher { POISSON, DELAUNAY };
+
+  struct Options {
+    // The path to the workspace folder in which all results are stored.
+    std::string workspace_path;
+
+    // The path to the image folder which are used as input.
+    std::string image_path;
+
+    // The path to the mask folder which are used as input.
+    std::string mask_path;
+
+    // The path to the vocabulary tree for feature matching.
+    std::string vocab_tree_path;
+
+    // The type of input data used to choose optimal mapper settings.
+    DataType data_type = DataType::INDIVIDUAL;
+
+    // Whether to perform low- or high-quality reconstruction.
+    Quality quality = Quality::HIGH;
+
+    // Whether to use shared intrinsics or not.
+    bool single_camera = false;
+
+    // Which camera model to use for images.
+    std::string camera_model = "SIMPLE_RADIAL";
+
+    // Whether to perform sparse mapping.
+    bool sparse = true;
+
+// Whether to perform dense mapping.
+#ifdef CUDA_ENABLED
+    bool dense = true;
+#else
+    bool dense = false;
+#endif
+
+    // The meshing algorithm to be used.
+    Mesher mesher = Mesher::POISSON;
+
+    // The number of threads to use in all stages.
+    int num_threads = -1;
+
+    // Whether to use the GPU in feature extraction and matching.
+    bool use_gpu = true;
+
+    // Index of the GPU used for GPU stages. For multi-GPU computation,
+    // you should separate multiple GPU indices by comma, e.g., "0,1,2,3".
+    // By default, all GPUs will be used in all stages.
+    std::string gpu_index = "-1";
+  };
+
+  AutomaticReconstructionController(
+      const Options& options, ReconstructionManager* reconstruction_manager);
+
+  void Stop() override;
+
+ private:
+  void Run() override;
+  void RunFeatureExtraction();
+  void RunFeatureMatching();
+  void RunSparseMapper();
+  void RunDenseMapper();
+
+  const Options options_;
+  OptionManager option_manager_;
+  ReconstructionManager* reconstruction_manager_;
+  Thread* active_thread_;
+  std::unique_ptr<Thread> feature_extractor_;
+  std::unique_ptr<Thread> exhaustive_matcher_;
+  std::unique_ptr<Thread> sequential_matcher_;
+  std::unique_ptr<Thread> vocab_tree_matcher_;
+};
+
+}  // namespace colmap
+
+#endif  // COLMAP_SRC_CONTROLLERS_AUTOMATIC_RECONSTRUCTION_H_

colmap/include/colmap/controllers/bundle_adjustment.h

@@ -0,0 +1,56 @@
+// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//
+//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
+//       its contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
+
+#ifndef COLMAP_SRC_CONTROLLERS_BUNDLE_ADJUSTMENT_H_
+#define COLMAP_SRC_CONTROLLERS_BUNDLE_ADJUSTMENT_H_
+
+#include "base/reconstruction.h"
+#include "util/option_manager.h"
+#include "util/threading.h"
+
+namespace colmap {
+
+// Class that controls the global bundle adjustment procedure.
+class BundleAdjustmentController : public Thread {
+ public:
+  BundleAdjustmentController(const OptionManager& options,
+                             Reconstruction* reconstruction);
+
+ private:
+  void Run();
+
+  const OptionManager options_;
+  Reconstruction* reconstruction_;
+};
+
+}  // namespace colmap
+
+#endif  // COLMAP_SRC_CONTROLLERS_BUNDLE_ADJUSTMENT_H_

colmap/include/colmap/controllers/hierarchical_mapper.h

@@ -0,0 +1,82 @@
+// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//
+//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
+//       its contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
+
+#ifndef COLMAP_SRC_CONTROLLERS_HIERARCHICAL_MAPPER_H_
+#define COLMAP_SRC_CONTROLLERS_HIERARCHICAL_MAPPER_H_
+
+#include "base/reconstruction_manager.h"
+#include "base/scene_clustering.h"
+#include "controllers/incremental_mapper.h"
+#include "util/threading.h"
+
+namespace colmap {
+
+// Hierarchical mapping first hierarchically partitions the scene into multiple
+// overlapping clusters, then reconstructs them separately using incremental
+// mapping, and finally merges them all into a globally consistent
+// reconstruction. This is especially useful for larger-scale scenes, since
+// incremental mapping becomes slow with an increasing number of images.
+class HierarchicalMapperController : public Thread {
+ public:
+  struct Options {
+    // The path to the image folder which are used as input.
+    std::string image_path;
+
+    // The path to the database file which is used as input.
+    std::string database_path;
+
+    // The maximum number of trials to initialize a cluster.
+    int init_num_trials = 10;
+
+    // The number of workers used to reconstruct clusters in parallel.
+    int num_workers = -1;
+
+    bool Check() const;
+  };
+
+  HierarchicalMapperController(
+      const Options& options,
+      const SceneClustering::Options& clustering_options,
+      const IncrementalMapperOptions& mapper_options,
+      ReconstructionManager* reconstruction_manager);
+
+ private:
+  void Run() override;
+
+  const Options options_;
+  const SceneClustering::Options clustering_options_;
+  const IncrementalMapperOptions mapper_options_;
+  ReconstructionManager* reconstruction_manager_;
+};
+
+}  // namespace colmap
+
+#endif  // COLMAP_SRC_CONTROLLERS_HIERARCHICAL_MAPPER_H_

colmap/include/colmap/controllers/incremental_mapper.h

@@ -0,0 +1,199 @@
+// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//
+//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
+//       its contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
+
+#ifndef COLMAP_SRC_CONTROLLERS_INCREMENTAL_MAPPER_H_
+#define COLMAP_SRC_CONTROLLERS_INCREMENTAL_MAPPER_H_
+
+#include "base/reconstruction_manager.h"
+#include "sfm/incremental_mapper.h"
+#include "util/threading.h"
+
+namespace colmap {
+
+struct IncrementalMapperOptions {
+ public:
+  // The minimum number of matches for inlier matches to be considered.
+  int min_num_matches = 15;
+
+  // Whether to ignore the inlier matches of watermark image pairs.
+  bool ignore_watermarks = false;
+
+  // Whether to reconstruct multiple sub-models.
+  bool multiple_models = true;
+
+  // The number of sub-models to reconstruct.
+  int max_num_models = 50;
+
+  // The maximum number of overlapping images between sub-models. If the
+  // current sub-models shares more than this number of images with another
+  // model, then the reconstruction is stopped.
+  int max_model_overlap = 20;
+
+  // The minimum number of registered images of a sub-model, otherwise the
+  // sub-model is discarded.
+  int min_model_size = 10;
+
+  // The image identifiers used to initialize the reconstruction. Note that
+  // only one or both image identifiers can be specified. In the former case,
+  // the second image is automatically determined.
+  int init_image_id1 = -1;
+  int init_image_id2 = -1;
+
+  // The number of trials to initialize the reconstruction.
+  int init_num_trials = 200;
+
+  // Whether to extract colors for reconstructed points.
+  bool extract_colors = true;
+
+  // The number of threads to use during reconstruction.
+  int num_threads = -1;
+
+  // Thresholds for filtering images with degenerate intrinsics.
+  double min_focal_length_ratio = 0.1;
+  double max_focal_length_ratio = 10.0;
+  double max_extra_param = 1.0;
+
+  // Which intrinsic parameters to optimize during the reconstruction.
+  bool ba_refine_focal_length = true;
+  bool ba_refine_principal_point = false;
+  bool ba_refine_extra_params = true;
+
+  // The minimum number of residuals per bundle adjustment problem to
+  // enable multi-threading solving of the problems.
+  int ba_min_num_residuals_for_multi_threading = 50000;
+
+  // The number of images to optimize in local bundle adjustment.
+  int ba_local_num_images = 6;
+
+  // Ceres solver function tolerance for local bundle adjustment
+  double ba_local_function_tolerance = 0.0;
+
+  // The maximum number of local bundle adjustment iterations.
+  int ba_local_max_num_iterations = 25;
+
+  // Whether to use PBA in global bundle adjustment.
+  bool ba_global_use_pba = false;
+
+  // The GPU index for PBA bundle adjustment.
+  int ba_global_pba_gpu_index = -1;
+
+  // The growth rates after which to perform global bundle adjustment.
+  double ba_global_images_ratio = 1.1;
+  double ba_global_points_ratio = 1.1;
+  int ba_global_images_freq = 500;
+  int ba_global_points_freq = 250000;
+
+  // Ceres solver function tolerance for global bundle adjustment
+  double ba_global_function_tolerance = 0.0;
+
+  // The maximum number of global bundle adjustment iterations.
+  int ba_global_max_num_iterations = 50;
+
+  // The thresholds for iterative bundle adjustment refinements.
+  int ba_local_max_refinements = 2;
+  double ba_local_max_refinement_change = 0.001;
+  int ba_global_max_refinements = 5;
+  double ba_global_max_refinement_change = 0.0005;
+
+  // Path to a folder with reconstruction snapshots during incremental
+  // reconstruction. Snapshots will be saved according to the specified
+  // frequency of registered images.
+  std::string snapshot_path = "";
+  int snapshot_images_freq = 0;
+
+  // Which images to reconstruct. If no images are specified, all images will
+  // be reconstructed by default.
+  std::unordered_set<std::string> image_names;
+
+  // If reconstruction is provided as input, fix the existing image poses.
+  bool fix_existing_images = false;
+
+  IncrementalMapper::Options Mapper() const;
+  IncrementalTriangulator::Options Triangulation() const;
+  BundleAdjustmentOptions LocalBundleAdjustment() const;
+  BundleAdjustmentOptions GlobalBundleAdjustment() const;
+  ParallelBundleAdjuster::Options ParallelGlobalBundleAdjustment() const;
+
+  bool Check() const;
+
+ private:
+  friend class OptionManager;
+  friend class MapperGeneralOptionsWidget;
+  friend class MapperTriangulationOptionsWidget;
+  friend class MapperRegistrationOptionsWidget;
+  friend class MapperInitializationOptionsWidget;
+  friend class MapperBundleAdjustmentOptionsWidget;
+  friend class MapperFilteringOptionsWidget;
+  friend class ReconstructionOptionsWidget;
+  IncrementalMapper::Options mapper;
+  IncrementalTriangulator::Options triangulation;
+};
+
+// Class that controls the incremental mapping procedure by iteratively
+// initializing reconstructions from the same scene graph.
+class IncrementalMapperController : public Thread {
+ public:
+  enum {
+    INITIAL_IMAGE_PAIR_REG_CALLBACK,
+    NEXT_IMAGE_REG_CALLBACK,
+    LAST_IMAGE_REG_CALLBACK,
+  };
+
+  IncrementalMapperController(const IncrementalMapperOptions* options,
+                              const std::string& image_path,
+                              const std::string& database_path,
+                              ReconstructionManager* reconstruction_manager);
+
+ private:
+  void Run();
+  bool LoadDatabase();
+  void Reconstruct(const IncrementalMapper::Options& init_mapper_options);
+
+  const IncrementalMapperOptions* options_;
+  const std::string image_path_;
+  const std::string database_path_;
+  ReconstructionManager* reconstruction_manager_;
+  DatabaseCache database_cache_;
+};
+
+// Globally filter points and images in mapper.
+size_t FilterPoints(const IncrementalMapperOptions& options,
+                    IncrementalMapper* mapper);
+size_t FilterImages(const IncrementalMapperOptions& options,
+                    IncrementalMapper* mapper);
+
+// Globally complete and merge tracks in mapper.
+size_t CompleteAndMergeTracks(const IncrementalMapperOptions& options,
+                              IncrementalMapper* mapper);
+
+}  // namespace colmap
+
+#endif  // COLMAP_SRC_CONTROLLERS_INCREMENTAL_MAPPER_H_

colmap/include/colmap/estimators/absolute_pose.h

@@ -0,0 +1,182 @@
+// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//
+//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
+//       its contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
+
+#ifndef COLMAP_SRC_ESTIMATORS_ABSOLUTE_POSE_H_
+#define COLMAP_SRC_ESTIMATORS_ABSOLUTE_POSE_H_
+
+#include <array>
+#include <vector>
+
+#include <Eigen/Core>
+
+#include "util/alignment.h"
+#include "util/types.h"
+
+namespace colmap {
+
+// Analytic solver for the P3P (Perspective-Three-Point) problem.
+//
+// The algorithm is based on the following paper:
+//
+//    X.S. Gao, X.-R. Hou, J. Tang, H.-F. Chang. Complete Solution
+//    Classification for the Perspective-Three-Point Problem.
+//    http://www.mmrc.iss.ac.cn/~xgao/paper/ieee.pdf
+class P3PEstimator {
+ public:
+  // The 2D image feature observations.
+  typedef Eigen::Vector2d X_t;
+  // The observed 3D features in the world frame.
+  typedef Eigen::Vector3d Y_t;
+  // The transformation from the world to the camera frame.
+  typedef Eigen::Matrix3x4d M_t;
+
+  // The minimum number of samples needed to estimate a model.
+  static const int kMinNumSamples = 3;
+
+  // Estimate the most probable solution of the P3P problem from a set of
+  // three 2D-3D point correspondences.
+  //
+  // @param points2D   Normalized 2D image points as 3x2 matrix.
+  // @param points3D   3D world points as 3x3 matrix.
+  //
+  // @return           Most probable pose as length-1 vector of a 3x4 matrix.
+  static std::vector<M_t> Estimate(const std::vector<X_t>& points2D,
+                                   const std::vector<Y_t>& points3D);
+
+  // Calculate the squared reprojection error given a set of 2D-3D point
+  // correspondences and a projection matrix.
+  //
+  // @param points2D     Normalized 2D image points as Nx2 matrix.
+  // @param points3D     3D world points as Nx3 matrix.
+  // @param proj_matrix  3x4 projection matrix.
+  // @param residuals    Output vector of residuals.
+  static void Residuals(const std::vector<X_t>& points2D,
+                        const std::vector<Y_t>& points3D,
+                        const M_t& proj_matrix, std::vector<double>* residuals);
+};
+
+// EPNP solver for the PNP (Perspective-N-Point) problem. The solver needs a
+// minimum of 4 2D-3D correspondences.
+//
+// The algorithm is based on the following paper:
+//
+//    Lepetit, Vincent, Francesc Moreno-Noguer, and Pascal Fua.
+//    "Epnp: An accurate o (n) solution to the pnp problem."
+//    International journal of computer vision 81.2 (2009): 155-166.
+//
+// The implementation is based on their original open-source release, but is
+// ported to Eigen and contains several improvements over the original code.
+class EPNPEstimator {
+ public:
+  // The 2D image feature observations.
+  typedef Eigen::Vector2d X_t;
+  // The observed 3D features in the world frame.
+  typedef Eigen::Vector3d Y_t;
+  // The transformation from the world to the camera frame.
+  typedef Eigen::Matrix3x4d M_t;
+
+  // The minimum number of samples needed to estimate a model.
+  static const int kMinNumSamples = 4;
+
+  // Estimate the most probable solution of the P3P problem from a set of
+  // three 2D-3D point correspondences.
+  //
+  // @param points2D   Normalized 2D image points as 3x2 matrix.
+  // @param points3D   3D world points as 3x3 matrix.
+  //
+  // @return           Most probable pose as length-1 vector of a 3x4 matrix.
+  static std::vector<M_t> Estimate(const std::vector<X_t>& points2D,
+                                   const std::vector<Y_t>& points3D);
+
+  // Calculate the squared reprojection error given a set of 2D-3D point
+  // correspondences and a projection matrix.
+  //
+  // @param points2D     Normalized 2D image points as Nx2 matrix.
+  // @param points3D     3D world points as Nx3 matrix.
+  // @param proj_matrix  3x4 projection matrix.
+  // @param residuals    Output vector of residuals.
+  static void Residuals(const std::vector<X_t>& points2D,
+                        const std::vector<Y_t>& points3D,
+                        const M_t& proj_matrix, std::vector<double>* residuals);
+
+ private:
+  bool ComputePose(const std::vector<Eigen::Vector2d>& points2D,
+                   const std::vector<Eigen::Vector3d>& points3D,
+                   Eigen::Matrix3x4d* proj_matrix);
+
+  void ChooseControlPoints();
+  bool ComputeBarycentricCoordinates();
+
+  Eigen::Matrix<double, Eigen::Dynamic, 12> ComputeM();
+  Eigen::Matrix<double, 6, 10> ComputeL6x10(
+      const Eigen::Matrix<double, 12, 12>& Ut);
+  Eigen::Matrix<double, 6, 1> ComputeRho();
+
+  void FindBetasApprox1(const Eigen::Matrix<double, 6, 10>& L_6x10,
+                        const Eigen::Matrix<double, 6, 1>& rho,
+                        Eigen::Vector4d* betas);
+  void FindBetasApprox2(const Eigen::Matrix<double, 6, 10>& L_6x10,
+                        const Eigen::Matrix<double, 6, 1>& rho,
+                        Eigen::Vector4d* betas);
+  void FindBetasApprox3(const Eigen::Matrix<double, 6, 10>& L_6x10,
+                        const Eigen::Matrix<double, 6, 1>& rho,
+                        Eigen::Vector4d* betas);
+
+  void RunGaussNewton(const Eigen::Matrix<double, 6, 10>& L_6x10,
+                      const Eigen::Matrix<double, 6, 1>& rho,
+                      Eigen::Vector4d* betas);
+
+  double ComputeRT(const Eigen::Matrix<double, 12, 12>& Ut,
+                   const Eigen::Vector4d& betas, Eigen::Matrix3d* R,
+                   Eigen::Vector3d* t);
+
+  void ComputeCcs(const Eigen::Vector4d& betas,
+                  const Eigen::Matrix<double, 12, 12>& Ut);
+  void ComputePcs();
+
+  void SolveForSign();
+
+  void EstimateRT(Eigen::Matrix3d* R, Eigen::Vector3d* t);
+
+  double ComputeTotalReprojectionError(const Eigen::Matrix3d& R,
+                                       const Eigen::Vector3d& t);
+
+  const std::vector<Eigen::Vector2d>* points2D_ = nullptr;
+  const std::vector<Eigen::Vector3d>* points3D_ = nullptr;
+  std::vector<Eigen::Vector3d> pcs_;
+  std::vector<Eigen::Vector4d> alphas_;
+  std::array<Eigen::Vector3d, 4> cws_;
+  std::array<Eigen::Vector3d, 4> ccs_;
+};
+
+}  // namespace colmap
+
+#endif  // COLMAP_SRC_ESTIMATORS_ABSOLUTE_POSE_H_

colmap/include/colmap/estimators/affine_transform.h

@@ -0,0 +1,65 @@
+// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//
+//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
+//       its contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
+
+#ifndef COLMAP_SRC_ESTIMATORS_AFFINE_TRANSFORM_H_
+#define COLMAP_SRC_ESTIMATORS_AFFINE_TRANSFORM_H_
+
+#include <vector>
+
+#include <Eigen/Core>
+
+#include "util/alignment.h"
+#include "util/types.h"
+
+namespace colmap {
+
+class AffineTransformEstimator {
+ public:
+  typedef Eigen::Vector2d X_t;
+  typedef Eigen::Vector2d Y_t;
+  typedef Eigen::Matrix<double, 2, 3> M_t;
+
+  // The minimum number of samples needed to estimate a model.
+  static const int kMinNumSamples = 3;
+
+  // Estimate the affine transformation from at least 3 correspondences.
+  static std::vector<M_t> Estimate(const std::vector<X_t>& points1,
+                                   const std::vector<Y_t>& points2);
+
+  // Compute the squared transformation error.
+  static void Residuals(const std::vector<X_t>& points1,
+                        const std::vector<Y_t>& points2, const M_t& E,
+                        std::vector<double>* residuals);
+};
+
+}  // namespace colmap
+
+#endif  // COLMAP_SRC_ESTIMATORS_AFFINE_TRANSFORM_H_

colmap/include/colmap/estimators/coordinate_frame.h

@@ -0,0 +1,88 @@
+// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//
+//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
+//       its contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
+
+#ifndef COLMAP_SRC_ESTIMATORS_COORDINATE_AXES_H_
+#define COLMAP_SRC_ESTIMATORS_COORDINATE_AXES_H_
+
+#include <Eigen/Core>
+
+#include "base/reconstruction.h"
+
+namespace colmap {
+
+struct ManhattanWorldFrameEstimationOptions {
+  // The maximum image size for line detection.
+  int max_image_size = 1024;
+  // The minimum length of line segments in pixels.
+  double min_line_length = 3;
+  // The tolerance for classifying lines into horizontal/vertical.
+  double line_orientation_tolerance = 0.2;
+  // The maximum distance in pixels between lines and the vanishing points.
+  double max_line_vp_distance = 0.5;
+  // The maximum cosine distance between estimated axes to be inliers.
+  double max_axis_distance = 0.05;
+};
+
+// Estimate gravity vector by assuming gravity-aligned image orientation, i.e.
+// the majority of images is assumed to have the gravity vector aligned with an
+// upright image plane.
+Eigen::Vector3d EstimateGravityVectorFromImageOrientation(
+    const Reconstruction& reconstruction,
+    const double max_axis_distance = 0.05);
+
+// Estimate the coordinate frame of the reconstruction assuming a Manhattan
+// world by finding the major vanishing points in each image. This function
+// assumes that the majority of images is taken in upright direction, i.e.
+// people are standing upright in the image. The orthonormal axes of the
+// estimated coordinate frame will be given in the columns of the returned
+// matrix. If one axis could not be determined, the respective column will be
+// zero. The axes are specified in the world coordinate system in the order
+// rightward, downward, forward.
+Eigen::Matrix3d EstimateManhattanWorldFrame(
+    const ManhattanWorldFrameEstimationOptions& options,
+    const Reconstruction& reconstruction, const std::string& image_path);
+
+// Aligns the reconstruction to the plane defined by running PCA on the 3D
+// points. The model centroid is at the origin of the new coordinate system
+// and the X axis is the first principal component with the Y axis being the
+// second principal component
+void AlignToPrincipalPlane(Reconstruction* recon, SimilarityTransform3* tform);
+
+// Aligns the reconstruction to the local ENU plane orientation. Rotates the
+// reconstruction such that the x-y plane aligns with the ENU tangent plane at
+// the point cloud centroid and translates the origin to the centroid.
+// If unscaled == true, then the original scale of the model remains unchanged.
+void AlignToENUPlane(Reconstruction* recon, SimilarityTransform3* tform,
+                     bool unscaled);
+
+}  // namespace colmap
+
+#endif  // COLMAP_SRC_ESTIMATORS_COORDINATE_AXES_H_

colmap/include/colmap/estimators/essential_matrix.h

@@ -0,0 +1,127 @@
+// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//
+//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
+//       its contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
+
+#ifndef COLMAP_SRC_ESTIMATORS_ESSENTIAL_MATRIX_H_
+#define COLMAP_SRC_ESTIMATORS_ESSENTIAL_MATRIX_H_
+
+#include <vector>
+
+#include <Eigen/Core>
+
+#include <ceres/ceres.h>
+
+#include "util/alignment.h"
+#include "util/types.h"
+
+namespace colmap {
+
+// Essential matrix estimator from corresponding normalized point pairs.
+//
+// This algorithm solves the 5-Point problem based on the following paper:
+//
+//    D. Nister, An efficient solution to the five-point relative pose problem,
+//    IEEE-T-PAMI, 26(6), 2004.
+//    http://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.86.8769
+class EssentialMatrixFivePointEstimator {
+ public:
+  typedef Eigen::Vector2d X_t;
+  typedef Eigen::Vector2d Y_t;
+  typedef Eigen::Matrix3d M_t;
+
+  // The minimum number of samples needed to estimate a model.
+  static const int kMinNumSamples = 5;
+
+  // Estimate up to 10 possible essential matrix solutions from a set of
+  // corresponding points.
+  //
+  //  The number of corresponding points must be at least 5.
+  //
+  // @param points1  First set of corresponding points.
+  // @param points2  Second set of corresponding points.
+  //
+  // @return         Up to 10 solutions as a vector of 3x3 essential matrices.
+  static std::vector<M_t> Estimate(const std::vector<X_t>& points1,
+                                   const std::vector<Y_t>& points2);
+
+  // Calculate the residuals of a set of corresponding points and a given
+  // essential matrix.
+  //
+  // Residuals are defined as the squared Sampson error.
+  //
+  // @param points1    First set of corresponding points.
+  // @param points2    Second set of corresponding points.
+  // @param E          3x3 essential matrix.
+  // @param residuals  Output vector of residuals.
+  static void Residuals(const std::vector<X_t>& points1,
+                        const std::vector<Y_t>& points2, const M_t& E,
+                        std::vector<double>* residuals);
+};
+
+// Essential matrix estimator from corresponding normalized point pairs.
+//
+// This algorithm solves the 8-Point problem based on the following paper:
+//
+//    Hartley and Zisserman, Multiple View Geometry, algorithm 11.1, page 282.
+class EssentialMatrixEightPointEstimator {
+ public:
+  typedef Eigen::Vector2d X_t;
+  typedef Eigen::Vector2d Y_t;
+  typedef Eigen::Matrix3d M_t;
+
+  // The minimum number of samples needed to estimate a model.
+  static const int kMinNumSamples = 8;
+
+  // Estimate essential matrix solutions from  set of corresponding points.
+  //
+  // The number of corresponding points must be at least 8.
+  //
+  // @param points1  First set of corresponding points.
+  // @param points2  Second set of corresponding points.
+  static std::vector<M_t> Estimate(const std::vector<X_t>& points1,
+                                   const std::vector<Y_t>& points2);
+
+  // Calculate the residuals of a set of corresponding points and a given
+  // essential matrix.
+  //
+  // Residuals are defined as the squared Sampson error.
+  //
+  // @param points1    First set of corresponding points.
+  // @param points2    Second set of corresponding points.
+  // @param E          3x3 essential matrix.
+  // @param residuals  Output vector of residuals.
+  static void Residuals(const std::vector<X_t>& points1,
+                        const std::vector<Y_t>& points2, const M_t& E,
+                        std::vector<double>* residuals);
+};
+
+}  // namespace colmap
+
+#endif  // COLMAP_SRC_ESTIMATORS_ESSENTIAL_MATRIX_H_

colmap/include/colmap/estimators/essential_matrix_coeffs.h

@@ -0,0 +1,206 @@
+// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//
+//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
+//       its contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
+
+{
+  const double* b = B.data();
+
+  coeffs(0) = b[0] * b[17] * b[34] + b[26] * b[4] * b[21] -
+              b[26] * b[17] * b[8] - b[13] * b[4] * b[34] -
+              b[0] * b[21] * b[30] + b[13] * b[30] * b[8];
+  coeffs(1) =
+      b[26] * b[4] * b[22] + b[14] * b[30] * b[8] + b[13] * b[31] * b[8] +
+      b[1] * b[17] * b[34] - b[13] * b[5] * b[34] + b[26] * b[5] * b[21] -
+      b[0] * b[21] * b[31] - b[26] * b[17] * b[9] - b[1] * b[21] * b[30] +
+      b[27] * b[4] * b[21] + b[0] * b[17] * b[35] - b[0] * b[22] * b[30] +
+      b[13] * b[30] * b[9] + b[0] * b[18] * b[34] - b[27] * b[17] * b[8] -
+      b[14] * b[4] * b[34] - b[13] * b[4] * b[35] - b[26] * b[18] * b[8];
+  coeffs(2) =
+      b[14] * b[30] * b[9] + b[14] * b[31] * b[8] + b[13] * b[31] * b[9] -
+      b[13] * b[4] * b[36] - b[13] * b[5] * b[35] + b[15] * b[30] * b[8] -
+      b[13] * b[6] * b[34] + b[13] * b[30] * b[10] + b[13] * b[32] * b[8] -
+      b[14] * b[4] * b[35] - b[14] * b[5] * b[34] + b[26] * b[4] * b[23] +
+      b[26] * b[5] * b[22] + b[26] * b[6] * b[21] - b[26] * b[17] * b[10] -
+      b[15] * b[4] * b[34] - b[26] * b[18] * b[9] - b[26] * b[19] * b[8] +
+      b[27] * b[4] * b[22] + b[27] * b[5] * b[21] - b[27] * b[17] * b[9] -
+      b[27] * b[18] * b[8] - b[1] * b[21] * b[31] - b[0] * b[23] * b[30] -
+      b[0] * b[21] * b[32] + b[28] * b[4] * b[21] - b[28] * b[17] * b[8] +
+      b[2] * b[17] * b[34] + b[0] * b[18] * b[35] - b[0] * b[22] * b[31] +
+      b[0] * b[17] * b[36] + b[0] * b[19] * b[34] - b[1] * b[22] * b[30] +
+      b[1] * b[18] * b[34] + b[1] * b[17] * b[35] - b[2] * b[21] * b[30];
+  coeffs(3) =
+      b[14] * b[30] * b[10] + b[14] * b[32] * b[8] - b[3] * b[21] * b[30] +
+      b[3] * b[17] * b[34] + b[13] * b[32] * b[9] + b[13] * b[33] * b[8] -
+      b[13] * b[4] * b[37] - b[13] * b[5] * b[36] + b[15] * b[30] * b[9] +
+      b[15] * b[31] * b[8] - b[16] * b[4] * b[34] - b[13] * b[6] * b[35] -
+      b[13] * b[7] * b[34] + b[13] * b[30] * b[11] + b[13] * b[31] * b[10] +
+      b[14] * b[31] * b[9] - b[14] * b[4] * b[36] - b[14] * b[5] * b[35] -
+      b[14] * b[6] * b[34] + b[16] * b[30] * b[8] - b[26] * b[20] * b[8] +
+      b[26] * b[4] * b[24] + b[26] * b[5] * b[23] + b[26] * b[6] * b[22] +
+      b[26] * b[7] * b[21] - b[26] * b[17] * b[11] - b[15] * b[4] * b[35] -
+      b[15] * b[5] * b[34] - b[26] * b[18] * b[10] - b[26] * b[19] * b[9] +
+      b[27] * b[4] * b[23] + b[27] * b[5] * b[22] + b[27] * b[6] * b[21] -
+      b[27] * b[17] * b[10] - b[27] * b[18] * b[9] - b[27] * b[19] * b[8] +
+      b[0] * b[17] * b[37] - b[0] * b[23] * b[31] - b[0] * b[24] * b[30] -
+      b[0] * b[21] * b[33] - b[29] * b[17] * b[8] + b[28] * b[4] * b[22] +
+      b[28] * b[5] * b[21] - b[28] * b[17] * b[9] - b[28] * b[18] * b[8] +
+      b[29] * b[4] * b[21] + b[1] * b[19] * b[34] - b[2] * b[21] * b[31] +
+      b[0] * b[20] * b[34] + b[0] * b[19] * b[35] + b[0] * b[18] * b[36] -
+      b[0] * b[22] * b[32] - b[1] * b[23] * b[30] - b[1] * b[21] * b[32] +
+      b[1] * b[18] * b[35] - b[1] * b[22] * b[31] - b[2] * b[22] * b[30] +
+      b[2] * b[17] * b[35] + b[1] * b[17] * b[36] + b[2] * b[18] * b[34];
+  coeffs(4) =
+      -b[14] * b[6] * b[35] - b[14] * b[7] * b[34] - b[3] * b[22] * b[30] -
+      b[3] * b[21] * b[31] + b[3] * b[17] * b[35] + b[3] * b[18] * b[34] +
+      b[13] * b[32] * b[10] + b[13] * b[33] * b[9] - b[13] * b[4] * b[38] -
+      b[13] * b[5] * b[37] - b[15] * b[6] * b[34] + b[15] * b[30] * b[10] +
+      b[15] * b[32] * b[8] - b[16] * b[4] * b[35] - b[13] * b[6] * b[36] -
+      b[13] * b[7] * b[35] + b[13] * b[31] * b[11] + b[13] * b[30] * b[12] +
+      b[14] * b[32] * b[9] + b[14] * b[33] * b[8] - b[14] * b[4] * b[37] -
+      b[14] * b[5] * b[36] + b[16] * b[30] * b[9] + b[16] * b[31] * b[8] -
+      b[26] * b[20] * b[9] + b[26] * b[4] * b[25] + b[26] * b[5] * b[24] +
+      b[26] * b[6] * b[23] + b[26] * b[7] * b[22] - b[26] * b[17] * b[12] +
+      b[14] * b[30] * b[11] + b[14] * b[31] * b[10] + b[15] * b[31] * b[9] -
+      b[15] * b[4] * b[36] - b[15] * b[5] * b[35] - b[26] * b[18] * b[11] -
+      b[26] * b[19] * b[10] - b[27] * b[20] * b[8] + b[27] * b[4] * b[24] +
+      b[27] * b[5] * b[23] + b[27] * b[6] * b[22] + b[27] * b[7] * b[21] -
+      b[27] * b[17] * b[11] - b[27] * b[18] * b[10] - b[27] * b[19] * b[9] -
+      b[16] * b[5] * b[34] - b[29] * b[17] * b[9] - b[29] * b[18] * b[8] +
+      b[28] * b[4] * b[23] + b[28] * b[5] * b[22] + b[28] * b[6] * b[21] -
+      b[28] * b[17] * b[10] - b[28] * b[18] * b[9] - b[28] * b[19] * b[8] +
+      b[29] * b[4] * b[22] + b[29] * b[5] * b[21] - b[2] * b[23] * b[30] +
+      b[2] * b[18] * b[35] - b[1] * b[22] * b[32] - b[2] * b[21] * b[32] +
+      b[2] * b[19] * b[34] + b[0] * b[19] * b[36] - b[0] * b[22] * b[33] +
+      b[0] * b[20] * b[35] - b[0] * b[23] * b[32] - b[0] * b[25] * b[30] +
+      b[0] * b[17] * b[38] + b[0] * b[18] * b[37] - b[0] * b[24] * b[31] +
+      b[1] * b[17] * b[37] - b[1] * b[23] * b[31] - b[1] * b[24] * b[30] -
+      b[1] * b[21] * b[33] + b[1] * b[20] * b[34] + b[1] * b[19] * b[35] +
+      b[1] * b[18] * b[36] + b[2] * b[17] * b[36] - b[2] * b[22] * b[31];
+  coeffs(5) =
+      -b[14] * b[6] * b[36] - b[14] * b[7] * b[35] + b[14] * b[31] * b[11] -
+      b[3] * b[23] * b[30] - b[3] * b[21] * b[32] + b[3] * b[18] * b[35] -
+      b[3] * b[22] * b[31] + b[3] * b[17] * b[36] + b[3] * b[19] * b[34] +
+      b[13] * b[32] * b[11] + b[13] * b[33] * b[10] - b[13] * b[5] * b[38] -
+      b[15] * b[6] * b[35] - b[15] * b[7] * b[34] + b[15] * b[30] * b[11] +
+      b[15] * b[31] * b[10] + b[16] * b[31] * b[9] - b[13] * b[6] * b[37] -
+      b[13] * b[7] * b[36] + b[13] * b[31] * b[12] + b[14] * b[32] * b[10] +
+      b[14] * b[33] * b[9] - b[14] * b[4] * b[38] - b[14] * b[5] * b[37] -
+      b[16] * b[6] * b[34] + b[16] * b[30] * b[10] + b[16] * b[32] * b[8] -
+      b[26] * b[20] * b[10] + b[26] * b[5] * b[25] + b[26] * b[6] * b[24] +
+      b[26] * b[7] * b[23] + b[14] * b[30] * b[12] + b[15] * b[32] * b[9] +
+      b[15] * b[33] * b[8] - b[15] * b[4] * b[37] - b[15] * b[5] * b[36] +
+      b[29] * b[5] * b[22] + b[29] * b[6] * b[21] - b[26] * b[18] * b[12] -
+      b[26] * b[19] * b[11] - b[27] * b[20] * b[9] + b[27] * b[4] * b[25] +
+      b[27] * b[5] * b[24] + b[27] * b[6] * b[23] + b[27] * b[7] * b[22] -
+      b[27] * b[17] * b[12] - b[27] * b[18] * b[11] - b[27] * b[19] * b[10] -
+      b[28] * b[20] * b[8] - b[16] * b[4] * b[36] - b[16] * b[5] * b[35] -
+      b[29] * b[17] * b[10] - b[29] * b[18] * b[9] - b[29] * b[19] * b[8] +
+      b[28] * b[4] * b[24] + b[28] * b[5] * b[23] + b[28] * b[6] * b[22] +
+      b[28] * b[7] * b[21] - b[28] * b[17] * b[11] - b[28] * b[18] * b[10] -
+      b[28] * b[19] * b[9] + b[29] * b[4] * b[23] - b[2] * b[22] * b[32] -
+      b[2] * b[21] * b[33] - b[1] * b[24] * b[31] + b[0] * b[18] * b[38] -
+      b[0] * b[24] * b[32] + b[0] * b[19] * b[37] + b[0] * b[20] * b[36] -
+      b[0] * b[25] * b[31] - b[0] * b[23] * b[33] + b[1] * b[19] * b[36] -
+      b[1] * b[22] * b[33] + b[1] * b[20] * b[35] + b[2] * b[19] * b[35] -
+      b[2] * b[24] * b[30] - b[2] * b[23] * b[31] + b[2] * b[20] * b[34] +
+      b[2] * b[17] * b[37] - b[1] * b[25] * b[30] + b[1] * b[18] * b[37] +
+      b[1] * b[17] * b[38] - b[1] * b[23] * b[32] + b[2] * b[18] * b[36];
+  coeffs(6) =
+      -b[14] * b[6] * b[37] - b[14] * b[7] * b[36] + b[14] * b[31] * b[12] +
+      b[3] * b[17] * b[37] - b[3] * b[23] * b[31] - b[3] * b[24] * b[30] -
+      b[3] * b[21] * b[33] + b[3] * b[20] * b[34] + b[3] * b[19] * b[35] +
+      b[3] * b[18] * b[36] - b[3] * b[22] * b[32] + b[13] * b[32] * b[12] +
+      b[13] * b[33] * b[11] - b[15] * b[6] * b[36] - b[15] * b[7] * b[35] +
+      b[15] * b[31] * b[11] + b[15] * b[30] * b[12] + b[16] * b[32] * b[9] +
+      b[16] * b[33] * b[8] - b[13] * b[6] * b[38] - b[13] * b[7] * b[37] +
+      b[14] * b[32] * b[11] + b[14] * b[33] * b[10] - b[14] * b[5] * b[38] -
+      b[16] * b[6] * b[35] - b[16] * b[7] * b[34] + b[16] * b[30] * b[11] +
+      b[16] * b[31] * b[10] - b[26] * b[19] * b[12] - b[26] * b[20] * b[11] +
+      b[26] * b[6] * b[25] + b[26] * b[7] * b[24] + b[15] * b[32] * b[10] +
+      b[15] * b[33] * b[9] - b[15] * b[4] * b[38] - b[15] * b[5] * b[37] +
+      b[29] * b[5] * b[23] + b[29] * b[6] * b[22] + b[29] * b[7] * b[21] -
+      b[27] * b[20] * b[10] + b[27] * b[5] * b[25] + b[27] * b[6] * b[24] +
+      b[27] * b[7] * b[23] - b[27] * b[18] * b[12] - b[27] * b[19] * b[11] -
+      b[28] * b[20] * b[9] - b[16] * b[4] * b[37] - b[16] * b[5] * b[36] +
+      b[0] * b[19] * b[38] - b[0] * b[24] * b[33] + b[0] * b[20] * b[37] -
+      b[29] * b[17] * b[11] - b[29] * b[18] * b[10] - b[29] * b[19] * b[9] +
+      b[28] * b[4] * b[25] + b[28] * b[5] * b[24] + b[28] * b[6] * b[23] +
+      b[28] * b[7] * b[22] - b[28] * b[17] * b[12] - b[28] * b[18] * b[11] -
+      b[28] * b[19] * b[10] - b[29] * b[20] * b[8] + b[29] * b[4] * b[24] +
+      b[2] * b[18] * b[37] - b[0] * b[25] * b[32] + b[1] * b[18] * b[38] -
+      b[1] * b[24] * b[32] + b[1] * b[19] * b[37] + b[1] * b[20] * b[36] -
+      b[1] * b[25] * b[31] + b[2] * b[17] * b[38] + b[2] * b[19] * b[36] -
+      b[2] * b[24] * b[31] - b[2] * b[22] * b[33] - b[2] * b[23] * b[32] +
+      b[2] * b[20] * b[35] - b[1] * b[23] * b[33] - b[2] * b[25] * b[30];
+  coeffs(7) =
+      -b[14] * b[6] * b[38] - b[14] * b[7] * b[37] + b[3] * b[19] * b[36] -
+      b[3] * b[22] * b[33] + b[3] * b[20] * b[35] - b[3] * b[23] * b[32] -
+      b[3] * b[25] * b[30] + b[3] * b[17] * b[38] + b[3] * b[18] * b[37] -
+      b[3] * b[24] * b[31] - b[15] * b[6] * b[37] - b[15] * b[7] * b[36] +
+      b[15] * b[31] * b[12] + b[16] * b[32] * b[10] + b[16] * b[33] * b[9] +
+      b[13] * b[33] * b[12] - b[13] * b[7] * b[38] + b[14] * b[32] * b[12] +
+      b[14] * b[33] * b[11] - b[16] * b[6] * b[36] - b[16] * b[7] * b[35] +
+      b[16] * b[31] * b[11] + b[16] * b[30] * b[12] + b[15] * b[32] * b[11] +
+      b[15] * b[33] * b[10] - b[15] * b[5] * b[38] + b[29] * b[5] * b[24] +
+      b[29] * b[6] * b[23] - b[26] * b[20] * b[12] + b[26] * b[7] * b[25] -
+      b[27] * b[19] * b[12] - b[27] * b[20] * b[11] + b[27] * b[6] * b[25] +
+      b[27] * b[7] * b[24] - b[28] * b[20] * b[10] - b[16] * b[4] * b[38] -
+      b[16] * b[5] * b[37] + b[29] * b[7] * b[22] - b[29] * b[17] * b[12] -
+      b[29] * b[18] * b[11] - b[29] * b[19] * b[10] + b[28] * b[5] * b[25] +
+      b[28] * b[6] * b[24] + b[28] * b[7] * b[23] - b[28] * b[18] * b[12] -
+      b[28] * b[19] * b[11] - b[29] * b[20] * b[9] + b[29] * b[4] * b[25] -
+      b[2] * b[24] * b[32] + b[0] * b[20] * b[38] - b[0] * b[25] * b[33] +
+      b[1] * b[19] * b[38] - b[1] * b[24] * b[33] + b[1] * b[20] * b[37] -
+      b[2] * b[25] * b[31] + b[2] * b[20] * b[36] - b[1] * b[25] * b[32] +
+      b[2] * b[19] * b[37] + b[2] * b[18] * b[38] - b[2] * b[23] * b[33];
+  coeffs(8) =
+      b[3] * b[18] * b[38] - b[3] * b[24] * b[32] + b[3] * b[19] * b[37] +
+      b[3] * b[20] * b[36] - b[3] * b[25] * b[31] - b[3] * b[23] * b[33] -
+      b[15] * b[6] * b[38] - b[15] * b[7] * b[37] + b[16] * b[32] * b[11] +
+      b[16] * b[33] * b[10] - b[16] * b[5] * b[38] - b[16] * b[6] * b[37] -
+      b[16] * b[7] * b[36] + b[16] * b[31] * b[12] + b[14] * b[33] * b[12] -
+      b[14] * b[7] * b[38] + b[15] * b[32] * b[12] + b[15] * b[33] * b[11] +
+      b[29] * b[5] * b[25] + b[29] * b[6] * b[24] - b[27] * b[20] * b[12] +
+      b[27] * b[7] * b[25] - b[28] * b[19] * b[12] - b[28] * b[20] * b[11] +
+      b[29] * b[7] * b[23] - b[29] * b[18] * b[12] - b[29] * b[19] * b[11] +
+      b[28] * b[6] * b[25] + b[28] * b[7] * b[24] - b[29] * b[20] * b[10] +
+      b[2] * b[19] * b[38] - b[1] * b[25] * b[33] + b[2] * b[20] * b[37] -
+      b[2] * b[24] * b[33] - b[2] * b[25] * b[32] + b[1] * b[20] * b[38];
+  coeffs(9) =
+      b[29] * b[7] * b[24] - b[29] * b[20] * b[11] + b[2] * b[20] * b[38] -
+      b[2] * b[25] * b[33] - b[28] * b[20] * b[12] + b[28] * b[7] * b[25] -
+      b[29] * b[19] * b[12] - b[3] * b[24] * b[33] + b[15] * b[33] * b[12] +
+      b[3] * b[19] * b[38] - b[16] * b[6] * b[38] + b[3] * b[20] * b[37] +
+      b[16] * b[32] * b[12] + b[29] * b[6] * b[25] - b[16] * b[7] * b[37] -
+      b[3] * b[25] * b[32] - b[15] * b[7] * b[38] + b[16] * b[33] * b[11];
+  coeffs(10) = -b[29] * b[20] * b[12] + b[29] * b[7] * b[25] +
+               b[16] * b[33] * b[12] - b[16] * b[7] * b[38] +
+               b[3] * b[20] * b[38] - b[3] * b[25] * b[33];
+}

colmap/include/colmap/estimators/euclidean_transform.h

@@ -0,0 +1,56 @@
+// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//
+//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
+//       its contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
+
+#ifndef COLMAP_SRC_ESTIMATORS_EUCLIDEAN_TRANSFORM_H_
+#define COLMAP_SRC_ESTIMATORS_EUCLIDEAN_TRANSFORM_H_
+
+#include "base/similarity_transform.h"
+#include "util/alignment.h"
+
+namespace colmap {
+
+// N-D Euclidean transform estimator from corresponding point pairs in the
+// source and destination coordinate systems.
+//
+// This algorithm is based on the following paper:
+//
+//      S. Umeyama. Least-Squares Estimation of Transformation Parameters
+//      Between Two Point Patterns. IEEE Transactions on Pattern Analysis and
+//      Machine Intelligence, Volume 13 Issue 4, Page 376-380, 1991.
+//      http://www.stanford.edu/class/cs273/refs/umeyama.pdf
+//
+// and uses the Eigen implementation.
+template <int kDim>
+using EuclideanTransformEstimator = SimilarityTransformEstimator<kDim, false>;
+
+}  // namespace colmap
+
+#endif  // COLMAP_SRC_ESTIMATORS_EUCLIDEAN_TRANSFORM_H_

colmap/include/colmap/estimators/fundamental_matrix.h

@@ -0,0 +1,129 @@
+// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//
+//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
+//       its contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
+
+#ifndef COLMAP_SRC_ESTIMATORS_FUNDAMENTAL_MATRIX_H_
+#define COLMAP_SRC_ESTIMATORS_FUNDAMENTAL_MATRIX_H_
+
+#include <vector>
+
+#include <Eigen/Core>
+
+#include "estimators/homography_matrix.h"
+#include "util/alignment.h"
+#include "util/types.h"
+
+namespace colmap {
+
+// Fundamental matrix estimator from corresponding point pairs.
+//
+// This algorithm solves the 7-Point problem and is based on the following
+// paper:
+//
+//    Zhengyou Zhang and T. Kanade, Determining the Epipolar Geometry and its
+//    Uncertainty: A Review, International Journal of Computer Vision, 1998.
+//    http://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.33.4540
+class FundamentalMatrixSevenPointEstimator {
+ public:
+  typedef Eigen::Vector2d X_t;
+  typedef Eigen::Vector2d Y_t;
+  typedef Eigen::Matrix3d M_t;
+
+  // The minimum number of samples needed to estimate a model.
+  static const int kMinNumSamples = 7;
+
+  // Estimate either 1 or 3 possible fundamental matrix solutions from a set of
+  // corresponding points.
+  //
+  // The number of corresponding points must be exactly 7.
+  //
+  // @param points1  First set of corresponding points.
+  // @param points2  Second set of corresponding points
+  //
+  // @return         Up to 4 solutions as a vector of 3x3 fundamental matrices.
+  static std::vector<M_t> Estimate(const std::vector<X_t>& points1,
+                                   const std::vector<Y_t>& points2);
+
+  // Calculate the residuals of a set of corresponding points and a given
+  // fundamental matrix.
+  //
+  // Residuals are defined as the squared Sampson error.
+  //
+  // @param points1    First set of corresponding points as Nx2 matrix.
+  // @param points2    Second set of corresponding points as Nx2 matrix.
+  // @param F          3x3 fundamental matrix.
+  // @param residuals  Output vector of residuals.
+  static void Residuals(const std::vector<X_t>& points1,
+                        const std::vector<Y_t>& points2, const M_t& F,
+                        std::vector<double>* residuals);
+};
+
+// Fundamental matrix estimator from corresponding point pairs.
+//
+// This algorithm solves the 8-Point problem based on the following paper:
+//
+//    Hartley and Zisserman, Multiple View Geometry, algorithm 11.1, page 282.
+class FundamentalMatrixEightPointEstimator {
+ public:
+  typedef Eigen::Vector2d X_t;
+  typedef Eigen::Vector2d Y_t;
+  typedef Eigen::Matrix3d M_t;
+
+  // The minimum number of samples needed to estimate a model.
+  static const int kMinNumSamples = 8;
+
+  // Estimate fundamental matrix solutions from a set of corresponding points.
+  //
+  // The number of corresponding points must be at least 8.
+  //
+  // @param points1  First set of corresponding points.
+  // @param points2  Second set of corresponding points
+  //
+  // @return         Single solution as a vector of 3x3 fundamental matrices.
+  static std::vector<M_t> Estimate(const std::vector<X_t>& points1,
+                                   const std::vector<Y_t>& points2);
+
+  // Calculate the residuals of a set of corresponding points and a given
+  // fundamental matrix.
+  //
+  // Residuals are defined as the squared Sampson error.
+  //
+  // @param points1    First set of corresponding points as Nx2 matrix.
+  // @param points2    Second set of corresponding points as Nx2 matrix.
+  // @param F          3x3 fundamental matrix.
+  // @param residuals  Output vector of residuals.
+  static void Residuals(const std::vector<X_t>& points1,
+                        const std::vector<Y_t>& points2, const M_t& F,
+                        std::vector<double>* residuals);
+};
+
+}  // namespace colmap
+
+#endif  // COLMAP_SRC_ESTIMATORS_FUNDAMENTAL_MATRIX_H_

colmap/include/colmap/estimators/generalized_absolute_pose.h

@@ -0,0 +1,100 @@
+// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//
+//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
+//       its contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
+
+#ifndef COLMAP_SRC_ESTIMATORS_GENERALIZED_ABSOLUTE_POSE_H_
+#define COLMAP_SRC_ESTIMATORS_GENERALIZED_ABSOLUTE_POSE_H_
+
+#include <vector>
+
+#include <Eigen/Core>
+
+#include "util/alignment.h"
+#include "util/types.h"
+
+namespace colmap {
+
+// Solver for the Generalized P3P problem (NP3P or GP3P), based on:
+//
+//      Lee, Gim Hee, et al. "Minimal solutions for pose estimation of a
+//      multi-camera system." Robotics Research. Springer International
+//      Publishing, 2016. 521-538.
+//
+// This class is based on an original implementation by Federico Camposeco.
+class GP3PEstimator {
+ public:
+  // The generalized image observations, which is composed of the relative pose
+  // of the specific camera in the generalized camera and its image observation.
+  struct X_t {
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    // The relative transformation from the generalized camera to the camera
+    // frame of the observation.
+    Eigen::Matrix3x4d rel_tform;
+    // The 2D image feature observation.
+    Eigen::Vector2d xy;
+  };
+
+  // The observed 3D feature points in the world frame.
+  typedef Eigen::Vector3d Y_t;
+  // The transformation from the world to the generalized camera frame.
+  typedef Eigen::Matrix3x4d M_t;
+
+  // The minimum number of samples needed to estimate a model.
+  static const int kMinNumSamples = 3;
+
+  enum class ResidualType {
+    CosineDistance,
+    ReprojectionError,
+  };
+
+  // Whether to compute the cosine similarity or the reprojection error.
+  // [WARNING] The reprojection error being in normalized coordinates,
+  // the unique error threshold of RANSAC corresponds to different pixel values
+  // in the different cameras of the rig if they have different intrinsics.
+  ResidualType residual_type = ResidualType::CosineDistance;
+
+  // Estimate the most probable solution of the GP3P problem from a set of
+  // three 2D-3D point correspondences.
+  static std::vector<M_t> Estimate(const std::vector<X_t>& points2D,
+                                   const std::vector<Y_t>& points3D);
+
+  // Calculate the squared cosine distance error between the rays given a set of
+  // 2D-3D point correspondences and a projection matrix of the generalized
+  // camera.
+  void Residuals(const std::vector<X_t>& points2D,
+                 const std::vector<Y_t>& points3D,
+                 const M_t& proj_matrix, std::vector<double>* residuals);
+};
+
+}  // namespace colmap
+
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(colmap::GP3PEstimator::X_t)
+
+#endif  // COLMAP_SRC_ESTIMATORS_GENERALIZED_ABSOLUTE_POSE_H_

colmap/include/colmap/estimators/generalized_absolute_pose_coeffs.h

@@ -0,0 +1,44 @@
+// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//
+//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
+//       its contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
+
+#ifndef COLMAP_SRC_ESTIMATORS_GENERALIZED_ABSOLUTE_POSE_COEFFS_H_
+#define COLMAP_SRC_ESTIMATORS_GENERALIZED_ABSOLUTE_POSE_COEFFS_H_
+
+#include <Eigen/Core>
+
+namespace colmap {
+
+Eigen::Matrix<double, 9, 1> ComputeDepthsSylvesterCoeffs(
+    const Eigen::Matrix<double, 3, 6>& K);
+
+}  // namespace colmap
+
+#endif  // COLMAP_SRC_ESTIMATORS_GENERALIZED_ABSOLUTE_POSE_COEFFS_H_

colmap/include/colmap/estimators/generalized_relative_pose.h

@@ -0,0 +1,94 @@
+// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//
+//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
+//       its contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
+
+#ifndef COLMAP_SRC_ESTIMATORS_GENERALIZED_RELATIVE_POSE_H_
+#define COLMAP_SRC_ESTIMATORS_GENERALIZED_RELATIVE_POSE_H_
+
+#include <vector>
+
+#include <Eigen/Core>
+
+#include "util/alignment.h"
+#include "util/types.h"
+
+namespace colmap {
+
+// Solver for the Generalized Relative Pose problem using a minimal of 8 2D-2D
+// correspondences. This implementation is based on:
+//
+//    "Efficient Computation of Relative Pose for Multi-Camera Systems",
+//    Kneip and Li. CVPR 2014.
+//
+// Note that the solution to this problem is degenerate in the case of pure
+// translation and when all correspondences are observed from the same cameras.
+//
+// The implementation is a modified and improved version of Kneip's original
+// implementation in OpenGV licensed under the BSD license.
+class GR6PEstimator {
+ public:
+  // The generalized image observations of the left camera, which is composed of
+  // the relative pose of the specific camera in the generalized camera and its
+  // image observation.
+  struct X_t {
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    // The relative transformation from the generalized camera to the camera
+    // frame of the observation.
+    Eigen::Matrix3x4d rel_tform;
+    // The 2D image feature observation.
+    Eigen::Vector2d xy;
+  };
+
+  // The normalized image feature points in the left camera.
+  typedef X_t Y_t;
+  // The relative transformation between the two generalized cameras.
+  typedef Eigen::Matrix3x4d M_t;
+
+  // The minimum number of samples needed to estimate a model. Note that in
+  // theory the minimum required number of samples is 6 but Laurent Kneip showed
+  // in his paper that using 8 samples is more stable.
+  static const int kMinNumSamples = 8;
+
+  // Estimate the most probable solution of the GR6P problem from a set of
+  // six 2D-2D point correspondences.
+  static std::vector<M_t> Estimate(const std::vector<X_t>& points1,
+                                   const std::vector<Y_t>& points2);
+
+  // Calculate the squared Sampson error between corresponding points.
+  static void Residuals(const std::vector<X_t>& points1,
+                        const std::vector<Y_t>& points2, const M_t& proj_matrix,
+                        std::vector<double>* residuals);
+};
+
+}  // namespace colmap
+
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(colmap::GR6PEstimator::X_t)
+
+#endif  // COLMAP_SRC_ESTIMATORS_GENERALIZED_RELATIVE_POSE_H_

colmap/include/colmap/estimators/homography_matrix.h

@@ -0,0 +1,83 @@
+// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//
+//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
+//       its contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
+
+#ifndef COLMAP_SRC_ESTIMATORS_HOMOGRAPHY_MATRIX_H_
+#define COLMAP_SRC_ESTIMATORS_HOMOGRAPHY_MATRIX_H_
+
+#include <vector>
+
+#include <Eigen/Core>
+
+#include "util/alignment.h"
+#include "util/types.h"
+
+namespace colmap {
+
+// Direct linear transformation algorithm to compute the homography between
+// point pairs. This algorithm computes the least squares estimate for
+// the homography from at least 4 correspondences.
+class HomographyMatrixEstimator {
+ public:
+  typedef Eigen::Vector2d X_t;
+  typedef Eigen::Vector2d Y_t;
+  typedef Eigen::Matrix3d M_t;
+
+  // The minimum number of samples needed to estimate a model.
+  static const int kMinNumSamples = 4;
+
+  // Estimate the projective transformation (homography).
+  //
+  // The number of corresponding points must be at least 4.
+  //
+  // @param points1    First set of corresponding points.
+  // @param points2    Second set of corresponding points.
+  //
+  // @return         3x3 homogeneous transformation matrix.
+  static std::vector<M_t> Estimate(const std::vector<X_t>& points1,
+                                   const std::vector<Y_t>& points2);
+
+  // Calculate the transformation error for each corresponding point pair.
+  //
+  // Residuals are defined as the squared transformation error when
+  // transforming the source to the destination coordinates.
+  //
+  // @param points1    First set of corresponding points.
+  // @param points2    Second set of corresponding points.
+  // @param H          3x3 projective matrix.
+  // @param residuals  Output vector of residuals.
+  static void Residuals(const std::vector<X_t>& points1,
+                        const std::vector<Y_t>& points2, const M_t& H,
+                        std::vector<double>* residuals);
+};
+
+}  // namespace colmap
+
+#endif  // COLMAP_SRC_ESTIMATORS_HOMOGRAPHY_MATRIX_H_

colmap/include/colmap/estimators/pose.h

@@ -0,0 +1,234 @@
+// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//
+//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
+//       its contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
+
+#ifndef COLMAP_SRC_ESTIMATORS_POSE_H_
+#define COLMAP_SRC_ESTIMATORS_POSE_H_
+
+#include <vector>
+
+#include <Eigen/Core>
+
+#include <ceres/ceres.h>
+
+#include "base/camera.h"
+#include "base/camera_models.h"
+#include "optim/loransac.h"
+#include "util/alignment.h"
+#include "util/logging.h"
+#include "util/threading.h"
+#include "util/types.h"
+
+namespace colmap {
+
+struct AbsolutePoseEstimationOptions {
+  // Whether to estimate the focal length.
+  bool estimate_focal_length = false;
+
+  // Number of discrete samples for focal length estimation.
+  size_t num_focal_length_samples = 30;
+
+  // Minimum focal length ratio for discrete focal length sampling
+  // around focal length of given camera.
+  double min_focal_length_ratio = 0.2;
+
+  // Maximum focal length ratio for discrete focal length sampling
+  // around focal length of given camera.
+  double max_focal_length_ratio = 5;
+
+  // Number of threads for parallel estimation of focal length.
+  int num_threads = ThreadPool::kMaxNumThreads;
+
+  // Options used for P3P RANSAC.
+  RANSACOptions ransac_options;
+
+  void Check() const {
+    CHECK_GT(num_focal_length_samples, 0);
+    CHECK_GT(min_focal_length_ratio, 0);
+    CHECK_GT(max_focal_length_ratio, 0);
+    CHECK_LT(min_focal_length_ratio, max_focal_length_ratio);
+    ransac_options.Check();
+  }
+};
+
+struct AbsolutePoseRefinementOptions {
+  // Convergence criterion.
+  double gradient_tolerance = 1.0;
+
+  // Maximum number of solver iterations.
+  int max_num_iterations = 100;
+
+  // Scaling factor determines at which residual robustification takes place.
+  double loss_function_scale = 1.0;
+
+  // Whether to refine the focal length parameter group.
+  bool refine_focal_length = true;
+
+  // Whether to refine the extra parameter group.
+  bool refine_extra_params = true;
+
+  // Whether to print final summary.
+  bool print_summary = true;
+
+  void Check() const {
+    CHECK_GE(gradient_tolerance, 0.0);
+    CHECK_GE(max_num_iterations, 0);
+    CHECK_GE(loss_function_scale, 0.0);
+  }
+};
+
+// Estimate absolute pose (optionally focal length) from 2D-3D correspondences.
+//
+// Focal length estimation is performed using discrete sampling around the
+// focal length of the given camera. The focal length that results in the
+// maximal number of inliers is assigned to the given camera.
+//
+// @param options              Absolute pose estimation options.
+// @param points2D             Corresponding 2D points.
+// @param points3D             Corresponding 3D points.
+// @param qvec                 Estimated rotation component as
+//                             unit Quaternion coefficients (w, x, y, z).
+// @param tvec                 Estimated translation component.
+// @param camera               Camera for which to estimate pose. Modified
+//                             in-place to store the estimated focal length.
+// @param num_inliers          Number of inliers in RANSAC.
+// @param inlier_mask          Inlier mask for 2D-3D correspondences.
+//
+// @return                     Whether pose is estimated successfully.
+bool EstimateAbsolutePose(const AbsolutePoseEstimationOptions& options,
+                          const std::vector<Eigen::Vector2d>& points2D,
+                          const std::vector<Eigen::Vector3d>& points3D,
+                          Eigen::Vector4d* qvec, Eigen::Vector3d* tvec,
+                          Camera* camera, size_t* num_inliers,
+                          std::vector<char>* inlier_mask);
+
+// Estimate relative from 2D-2D correspondences.
+//
+// Pose of first camera is assumed to be at the origin without rotation. Pose
+// of second camera is given as world-to-image transformation,
+// i.e. `x2 = [R | t] * X2`.
+//
+// @param ransac_options       RANSAC options.
+// @param points1              Corresponding 2D points.
+// @param points2              Corresponding 2D points.
+// @param qvec                 Estimated rotation component as
+//                             unit Quaternion coefficients (w, x, y, z).
+// @param tvec                 Estimated translation component.
+//
+// @return                     Number of RANSAC inliers.
+size_t EstimateRelativePose(const RANSACOptions& ransac_options,
+                            const std::vector<Eigen::Vector2d>& points1,
+                            const std::vector<Eigen::Vector2d>& points2,
+                            Eigen::Vector4d* qvec, Eigen::Vector3d* tvec);
+
+// Refine absolute pose (optionally focal length) from 2D-3D correspondences.
+//
+// @param options              Refinement options.
+// @param inlier_mask          Inlier mask for 2D-3D correspondences.
+// @param points2D             Corresponding 2D points.
+// @param points3D             Corresponding 3D points.
+// @param qvec                 Estimated rotation component as
+//                             unit Quaternion coefficients (w, x, y, z).
+// @param tvec                 Estimated translation component.
+// @param camera               Camera for which to estimate pose. Modified
+//                             in-place to store the estimated focal length.
+// @param rot_tvec_covariance  Estimated 6x6 covariance matrix of
+//                             the rotation (as axis-angle, in tangent space)
+//                             and translation terms (optional).
+//
+// @return                     Whether the solution is usable.
+bool RefineAbsolutePose(const AbsolutePoseRefinementOptions& options,
+                        const std::vector<char>& inlier_mask,
+                        const std::vector<Eigen::Vector2d>& points2D,
+                        const std::vector<Eigen::Vector3d>& points3D,
+                        Eigen::Vector4d* qvec, Eigen::Vector3d* tvec,
+                        Camera* camera,
+                        Eigen::Matrix6d* rot_tvec_covariance = nullptr);
+
+// Refine relative pose of two cameras.
+//
+// Minimizes the Sampson error between corresponding normalized points using
+// a robust cost function, i.e. the corresponding points need not necessarily
+// be inliers given a sufficient initial guess for the relative pose.
+//
+// Assumes that first camera pose has projection matrix P = [I | 0], and
+// pose of second camera is given as transformation from world to camera system.
+//
+// Assumes that the given translation vector is normalized, and refines
+// the translation up to an unknown scale (i.e. refined translation vector
+// is a unit vector again).
+//
+// @param options          Solver options.
+// @param points1          First set of corresponding points.
+// @param points2          Second set of corresponding points.
+// @param qvec             Unit Quaternion rotation coefficients (w, x, y, z).
+// @param tvec             3x1 translation vector.
+//
+// @return                 Flag indicating if solution is usable.
+bool RefineRelativePose(const ceres::Solver::Options& options,
+                        const std::vector<Eigen::Vector2d>& points1,
+                        const std::vector<Eigen::Vector2d>& points2,
+                        Eigen::Vector4d* qvec, Eigen::Vector3d* tvec);
+
+// Refine generalized absolute pose (optionally focal lengths)
+// from 2D-3D correspondences.
+//
+// @param options              Refinement options.
+// @param inlier_mask          Inlier mask for 2D-3D correspondences.
+// @param points2D             Corresponding 2D points.
+// @param points3D             Corresponding 3D points.
+// @param camera_idxs          Index of the rig camera for each correspondence.
+// @param rig_qvecs            Relative rotations from rig to each camera frame
+// @param rig_tvecs            Relative translations from rig
+//                             to each camera frame.
+// @param qvec                 Estimated rotation component of the rig as
+//                             unit Quaternion coefficients (w, x, y, z).
+// @param tvec                 Estimated translation component of the rig.
+// @param cameras              Cameras for which to estimate pose. Modified
+//                             in-place to store the estimated focal lengths.
+// @param rot_tvec_covariance  Estimated 6x6 covariance matrix of
+//                             the rotation (as axis-angle, in tangent space)
+//                             and translation terms (optional).
+//
+// @return                     Whether the solution is usable.
+bool RefineGeneralizedAbsolutePose(
+    const AbsolutePoseRefinementOptions& options,
+    const std::vector<char>& inlier_mask,
+    const std::vector<Eigen::Vector2d>& points2D,
+    const std::vector<Eigen::Vector3d>& points3D,
+    const std::vector<size_t>& camera_idxs,
+    const std::vector<Eigen::Vector4d>& rig_qvecs,
+    const std::vector<Eigen::Vector3d>& rig_tvecs, Eigen::Vector4d* qvec,
+    Eigen::Vector3d* tvec, std::vector<Camera>* cameras,
+    Eigen::Matrix6d* rot_tvec_covariance = nullptr);
+
+}  // namespace colmap
+
+#endif  // COLMAP_SRC_ESTIMATORS_POSE_H_

colmap/include/colmap/estimators/similarity_transform.h

@@ -0,0 +1,138 @@
+// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//
+//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
+//       its contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
+
+#ifndef COLMAP_SRC_ESTIMATORS_SIMILARITY_TRANSFORM_H_
+#define COLMAP_SRC_ESTIMATORS_SIMILARITY_TRANSFORM_H_
+
+#include <vector>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include "base/projection.h"
+#include "util/alignment.h"
+#include "util/logging.h"
+#include "util/types.h"
+
+namespace colmap {
+
+// N-D similarity transform estimator from corresponding point pairs in the
+// source and destination coordinate systems.
+//
+// This algorithm is based on the following paper:
+//
+//      S. Umeyama. Least-Squares Estimation of Transformation Parameters
+//      Between Two Point Patterns. IEEE Transactions on Pattern Analysis and
+//      Machine Intelligence, Volume 13 Issue 4, Page 376-380, 1991.
+//      http://www.stanford.edu/class/cs273/refs/umeyama.pdf
+//
+// and uses the Eigen implementation.
+template <int kDim, bool kEstimateScale = true>
+class SimilarityTransformEstimator {
+ public:
+  typedef Eigen::Matrix<double, kDim, 1> X_t;
+  typedef Eigen::Matrix<double, kDim, 1> Y_t;
+  typedef Eigen::Matrix<double, kDim, kDim + 1> M_t;
+
+  // The minimum number of samples needed to estimate a model. Note that
+  // this only returns the true minimal sample in the two-dimensional case.
+  // For higher dimensions, the system will alway be over-determined.
+  static const int kMinNumSamples = kDim;
+
+  // Estimate the similarity transform.
+  //
+  // @param src      Set of corresponding source points.
+  // @param dst      Set of corresponding destination points.
+  //
+  // @return         4x4 homogeneous transformation matrix.
+  static std::vector<M_t> Estimate(const std::vector<X_t>& src,
+                                   const std::vector<Y_t>& dst);
+
+  // Calculate the transformation error for each corresponding point pair.
+  //
+  // Residuals are defined as the squared transformation error when
+  // transforming the source to the destination coordinates.
+  //
+  // @param src        Set of corresponding points in the source coordinate
+  //                   system as a Nx3 matrix.
+  // @param dst        Set of corresponding points in the destination
+  //                   coordinate system as a Nx3 matrix.
+  // @param matrix     4x4 homogeneous transformation matrix.
+  // @param residuals  Output vector of residuals for each point pair.
+  static void Residuals(const std::vector<X_t>& src,
+                        const std::vector<Y_t>& dst, const M_t& matrix,
+                        std::vector<double>* residuals);
+};
+
+////////////////////////////////////////////////////////////////////////////////
+// Implementation
+////////////////////////////////////////////////////////////////////////////////
+
+template <int kDim, bool kEstimateScale>
+std::vector<typename SimilarityTransformEstimator<kDim, kEstimateScale>::M_t>
+SimilarityTransformEstimator<kDim, kEstimateScale>::Estimate(
+    const std::vector<X_t>& src, const std::vector<Y_t>& dst) {
+  CHECK_EQ(src.size(), dst.size());
+
+  Eigen::Matrix<double, kDim, Eigen::Dynamic> src_mat(kDim, src.size());
+  Eigen::Matrix<double, kDim, Eigen::Dynamic> dst_mat(kDim, dst.size());
+  for (size_t i = 0; i < src.size(); ++i) {
+    src_mat.col(i) = src[i];
+    dst_mat.col(i) = dst[i];
+  }
+
+  const M_t model = Eigen::umeyama(src_mat, dst_mat, kEstimateScale)
+                        .topLeftCorner(kDim, kDim + 1);
+
+  if (model.array().isNaN().any()) {
+    return std::vector<M_t>{};
+  }
+
+  return {model};
+}
+
+template <int kDim, bool kEstimateScale>
+void SimilarityTransformEstimator<kDim, kEstimateScale>::Residuals(
+    const std::vector<X_t>& src, const std::vector<Y_t>& dst, const M_t& matrix,
+    std::vector<double>* residuals) {
+  CHECK_EQ(src.size(), dst.size());
+
+  residuals->resize(src.size());
+
+  for (size_t i = 0; i < src.size(); ++i) {
+    const Y_t dst_transformed = matrix * src[i].homogeneous();
+    (*residuals)[i] = (dst[i] - dst_transformed).squaredNorm();
+  }
+}
+
+}  // namespace colmap
+
+#endif  // COLMAP_SRC_ESTIMATORS_SIMILARITY_TRANSFORM_H_

colmap/include/colmap/estimators/translation_transform.h

@@ -0,0 +1,120 @@
+// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//
+//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
+//       its contributors may be used to endorse or promote products derived
+//       from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
+
+#ifndef COLMAP_SRC_BASE_SIMILARITY_TRANSFORM_H_
+#define COLMAP_SRC_BASE_SIMILARITY_TRANSFORM_H_
+
+#include <vector>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include "util/alignment.h"
+#include "util/logging.h"
+#include "util/types.h"
+
+namespace colmap {
+
+// Estimate a N-D translation transformation between point pairs.
+template <int kDim>
+class TranslationTransformEstimator {
+ public:
+  typedef Eigen::Matrix<double, kDim, 1> X_t;
+  typedef Eigen::Matrix<double, kDim, 1> Y_t;
+  typedef Eigen::Matrix<double, kDim, 1> M_t;
+
+  // The minimum number of samples needed to estimate a model.
+  static const int kMinNumSamples = 1;
+
+  // Estimate the 2D translation transform.
+  //
+  // @param points1      Set of corresponding source 2D points.
+  // @param points2      Set of corresponding destination 2D points.
+  //
+  // @return             Translation vector.
+  static std::vector<M_t> Estimate(const std::vector<X_t>& points1,
+                                   const std::vector<Y_t>& points2);
+
+  // Calculate the squared translation error.
+  //
+  // @param points1      Set of corresponding source 2D points.
+  // @param points2      Set of corresponding destination 2D points.
+  // @param translation  Translation vector.
+  // @param residuals    Output vector of residuals for each point pair.
+  static void Residuals(const std::vector<X_t>& points1,
+                        const std::vector<Y_t>& points2, const M_t& translation,
+                        std::vector<double>* residuals);
+};
+
+////////////////////////////////////////////////////////////////////////////////
+// Implementation
+////////////////////////////////////////////////////////////////////////////////
+
+template <int kDim>
+std::vector<typename TranslationTransformEstimator<kDim>::M_t>
+TranslationTransformEstimator<kDim>::Estimate(const std::vector<X_t>& points1,
+                                              const std::vector<Y_t>& points2) {
+  CHECK_EQ(points1.size(), points2.size());
+
+  X_t mean_src = X_t::Zero();
+  Y_t mean_dst = Y_t::Zero();
+
+  for (size_t i = 0; i < points1.size(); ++i) {
+    mean_src += points1[i];
+    mean_dst += points2[i];
+  }
+
+  mean_src /= points1.size();
+  mean_dst /= points2.size();
+
+  std::vector<M_t> models(1);
+  models[0] = mean_dst - mean_src;
+
+  return models;
+}
+
+template <int kDim>
+void TranslationTransformEstimator<kDim>::Residuals(
+    const std::vector<X_t>& points1, const std::vector<Y_t>& points2,
+    const M_t& translation, std::vector<double>* residuals) {
+  CHECK_EQ(points1.size(), points2.size());
+
+  residuals->resize(points1.size());
+
+  for (size_t i = 0; i < points1.size(); ++i) {
+    const M_t diff = points2[i] - points1[i] - translation;
+    (*residuals)[i] = diff.squaredNorm();
+  }
+}
+
+}  // namespace colmap
+
+#endif  // COLMAP_SRC_BASE_SIMILARITY_TRANSFORM_H_