├── .pixi.sh ├── blueprint.rbl ├── .gitattributes ├── docker ├── build.sh ├── attach.sh ├── shutdown.sh ├── start.sh └── Dockerfile ├── .gitignore ├── glomap ├── processors │ ├── image_undistorter.h │ ├── reconstruction_pruning.h │ ├── relpose_filter.h │ ├── track_filter.h │ ├── view_graph_manipulation.h │ ├── image_pair_inliers.h │ ├── image_undistorter.cc │ ├── relpose_filter.cc │ ├── reconstruction_pruning.cc │ ├── track_filter.cc │ ├── image_pair_inliers.cc │ └── view_graph_manipulation.cc ├── exe │ ├── global_mapper.h │ └── global_mapper.cc ├── version.h.in ├── io │ ├── gravity_io.h │ ├── colmap_io.h │ ├── recording.h │ ├── gravity_io.cc │ ├── colmap_converter.h │ ├── colmap_io.cc │ └── recording.cc ├── scene │ ├── types_sfm.h │ ├── track.h │ ├── types.h │ ├── camera.h │ ├── view_graph.h │ ├── image.h │ ├── image_pair.h │ └── view_graph.cc ├── math │ ├── gravity.h │ ├── tree.h │ ├── gravity.cc │ ├── union_find.h │ ├── rigid3d.h │ ├── two_view_geometry.h │ ├── rigid3d.cc │ ├── two_view_geometry.cc │ ├── l1_solver.h │ └── tree.cc ├── estimators │ ├── relpose_estimation.h │ ├── optimization_base.h │ ├── gravity_refinement.h │ ├── view_graph_calibration.h │ ├── bundle_adjustment.h │ ├── relpose_estimation.cc │ ├── global_positioning.h │ ├── global_rotation_averaging.h │ ├── gravity_refinement.cc │ ├── view_graph_calibration.cc │ ├── bundle_adjustment.cc │ └── cost_function.h ├── controllers │ ├── track_retriangulation.h │ ├── track_establishment.h │ ├── global_mapper.h │ ├── global_mapper_test.cc │ ├── track_retriangulation.cc │ ├── option_manager.h │ └── track_establishment.cc ├── types.h ├── glomap.cc └── CMakeLists.txt ├── scripts ├── from_images.sh ├── from_video.sh ├── format │ └── clang_format.sh └── extract_frames.py ├── .clang-format ├── .clang-tidy ├── colmap-310-compat.patch ├── LICENSE ├── CMakeLists.txt ├── cmake └── FindDependencies.cmake ├── docs └── getting_started.md ├── pixi.toml ├── README.md └── .github └── workflows └── ci.yml /.pixi.sh: -------------------------------------------------------------------------------- 1 | export LD_LIBRARY_PATH=$CONDA_PREFIX/lib:$LD_LIBRARY_PATH -------------------------------------------------------------------------------- /blueprint.rbl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rerun-io/glomap/HEAD/blueprint.rbl -------------------------------------------------------------------------------- /.gitattributes: -------------------------------------------------------------------------------- 1 | # SCM syntax highlighting 2 | pixi.lock linguist-language=YAML linguist-generated=true 3 | -------------------------------------------------------------------------------- /docker/build.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | cd "$(dirname "$0")" 3 | 4 | docker build . -t glomap_container 5 | -------------------------------------------------------------------------------- /docker/attach.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | cd "$(dirname "$0")" 3 | 4 | # Attach terminal to the docker context 5 | docker exec -it glomap_container bash 6 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | build 2 | data 3 | .pixi 4 | venv 5 | *.mp4 6 | .vscode 7 | output 8 | shell.nix 9 | /run.sh 10 | 11 | # pixi environments 12 | .pixi 13 | *.egg-info 14 | -------------------------------------------------------------------------------- /docker/shutdown.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | cd "$(dirname "$0")" 3 | 4 | # Kill previous container if necessary 5 | docker kill glomap_container &> /dev/null 6 | docker remove glomap_container &> /dev/null 7 | 8 | echo 9 | echo 10 | echo "Docker container dissapeared :|" 11 | echo 12 | echo -------------------------------------------------------------------------------- /glomap/processors/image_undistorter.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "glomap/scene/types_sfm.h" 4 | 5 | namespace glomap { 6 | 7 | void UndistortImages(std::unordered_map& cameras, 8 | std::unordered_map& images, 9 | bool clean_points = true); 10 | 11 | } // namespace glomap 12 | -------------------------------------------------------------------------------- /scripts/from_images.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | colmap feature_extractor \ 4 | --image_path $1/images \ 5 | --database_path $1/database.db 6 | colmap exhaustive_matcher \ 7 | --database_path $1/database.db 8 | glomap mapper \ 9 | --database_path $1/database.db \ 10 | --image_path $1/images \ 11 | --output_path ./output/$1/sparse -------------------------------------------------------------------------------- /glomap/exe/global_mapper.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "glomap/controllers/global_mapper.h" 4 | 5 | namespace glomap { 6 | 7 | // Use default values for most of the settings from database 8 | int RunMapper(int argc, char** argv); 9 | 10 | // Use default values for most of the settings from colmap reconstruction 11 | int RunMapperResume(int argc, char** argv); 12 | 13 | } // namespace glomap -------------------------------------------------------------------------------- /.clang-format: -------------------------------------------------------------------------------- 1 | BasedOnStyle: Google 2 | BinPackArguments: false 3 | BinPackParameters: false 4 | DerivePointerAlignment: false 5 | IncludeBlocks: Regroup 6 | IncludeCategories: 7 | - Regex: '^"glomap' 8 | Priority: 1 9 | - Regex: '^' 12 | Priority: 4 13 | - Regex: '.*' 14 | Priority: 5 15 | SortIncludes: true 16 | -------------------------------------------------------------------------------- /glomap/version.h.in: -------------------------------------------------------------------------------- 1 | #ifndef @PROJECT_NAME_UPPERCASE@_VERSION_ 2 | #define @PROJECT_NAME_UPPERCASE@_VERSION_ 3 | 4 | #define @PROJECT_NAME_UPPERCASE@_MAJOR_VERSION (@MAJOR_VERSION@) 5 | #define @PROJECT_NAME_UPPERCASE@_MINOR_VERSION (@MINOR_VERSION@) 6 | #define @PROJECT_NAME_UPPERCASE@_PATCH_VERSION (@PATCH_VERSION@) 7 | #define @PROJECT_NAME_UPPERCASE@_VERSION "@PROJECT_VERSION@" 8 | 9 | #endif // @PROJECT_NAME_UPPERCASE@_VERSION_ -------------------------------------------------------------------------------- /scripts/from_video.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | 4 | colmap feature_extractor \ 5 | --image_path $1/images \ 6 | --database_path $1/database.db 7 | 8 | # Use sequential_matcher since images are from a video. 9 | colmap sequential_matcher \ 10 | --database_path $1/database.db 11 | 12 | # glomap mapper \ 13 | # --database_path $1/database.db \ 14 | # --image_path $1/images \ 15 | # --output_path ./output/$1/sparse 16 | 17 | -------------------------------------------------------------------------------- /glomap/processors/reconstruction_pruning.h: -------------------------------------------------------------------------------- 1 | 2 | #pragma once 3 | 4 | #include "glomap/scene/types_sfm.h" 5 | 6 | namespace glomap { 7 | 8 | image_t PruneWeaklyConnectedImages(std::unordered_map& images, 9 | std::unordered_map& tracks, 10 | int min_num_images = 2, 11 | int min_num_observations = 0); 12 | 13 | } // namespace glomap 14 | -------------------------------------------------------------------------------- /glomap/io/gravity_io.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "glomap/scene/image.h" 4 | 5 | #include 6 | 7 | namespace glomap { 8 | // Require the gravity in the format: image_name, gravity (3 numbers) 9 | // Gravity should be the direction of [0,1,0] in the image frame 10 | // image.cam_from_world * [0,1,0]^T = g 11 | void ReadGravity(const std::string& gravity_path, 12 | std::unordered_map& images); 13 | 14 | } // namespace glomap -------------------------------------------------------------------------------- /glomap/scene/types_sfm.h: -------------------------------------------------------------------------------- 1 | // This files contains all the necessary includes for sfm 2 | // Types defined by GLOMAP 3 | #include "glomap/scene/camera.h" 4 | #include "glomap/scene/image.h" 5 | #include "glomap/scene/track.h" 6 | #include "glomap/scene/types.h" 7 | #include "glomap/scene/view_graph.h" 8 | #include "glomap/types.h" 9 | 10 | // Standard libraries 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | -------------------------------------------------------------------------------- /glomap/math/gravity.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace glomap { 6 | 7 | // Get the aligment rotation matrix by QR decomposition 8 | // The second col of output is gravity direction 9 | Eigen::Matrix3d GetAlignRot(const Eigen::Vector3d& gravity); 10 | 11 | // Get the rotation angle for an upright rotation matrix 12 | double RotUpToAngle(const Eigen::Matrix3d& R_up); 13 | 14 | // Get the upright rotation matrix from a rotation angle 15 | Eigen::Matrix3d AngleToRotUp(double angle); 16 | 17 | } // namespace glomap 18 | -------------------------------------------------------------------------------- /.clang-tidy: -------------------------------------------------------------------------------- 1 | Checks: > 2 | performance-*, 3 | concurrency-*, 4 | bugprone-*, 5 | -bugprone-easily-swappable-parameters, 6 | -bugprone-exception-escape, 7 | -bugprone-implicit-widening-of-multiplication-result, 8 | -bugprone-narrowing-conversions, 9 | -bugprone-reserved-identifier, 10 | -bugprone-unchecked-optional-access, 11 | cppcoreguidelines-virtual-class-destructor, 12 | google-explicit-constructor, 13 | google-build-using-namespace, 14 | readability-avoid-const-params-in-decls, 15 | clang-analyzer-core*, 16 | clang-analyzer-cplusplus*, 17 | WarningsAsErrors: '*' 18 | FormatStyle: 'file' 19 | User: 'user' 20 | -------------------------------------------------------------------------------- /glomap/math/tree.h: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | 4 | #include 5 | 6 | namespace glomap { 7 | enum WeightType { INLIER_NUM, INLIER_RATIO }; 8 | 9 | // Return the number of nodes in the tree 10 | int BFS(const std::vector>& graph, 11 | int root, 12 | std::vector& parents, 13 | std::vector> banned_edges = {}); 14 | 15 | image_t MaximumSpanningTree(const ViewGraph& view_graph, 16 | const std::unordered_map& images, 17 | std::unordered_map& parents, 18 | WeightType type); 19 | } // namespace glomap -------------------------------------------------------------------------------- /glomap/scene/track.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "glomap/scene/types.h" 4 | 5 | #include 6 | 7 | #include 8 | 9 | namespace glomap { 10 | typedef std::pair Observation; 11 | 12 | struct Track { 13 | // The id of the track 14 | track_t track_id; 15 | 16 | // The 3D point of the track 17 | Eigen::Vector3d xyz = Eigen::Vector3d::Zero(); 18 | 19 | // The color of the track (now not used) 20 | Eigen::Vector3ub color = Eigen::Vector3ub::Zero(); 21 | 22 | // Whether the track has been estimated 23 | bool is_initialized = false; 24 | 25 | // The list where the track is observed (image_id, feature_id) 26 | std::vector observations; 27 | }; 28 | 29 | } // namespace glomap 30 | -------------------------------------------------------------------------------- /glomap/estimators/relpose_estimation.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "glomap/scene/types_sfm.h" 4 | 5 | #include 6 | 7 | namespace glomap { 8 | 9 | struct RelativePoseEstimationOptions { 10 | // Options for poselib solver 11 | poselib::RansacOptions ransac_options; 12 | poselib::BundleOptions bundle_options; 13 | 14 | RelativePoseEstimationOptions() { ransac_options.max_iterations = 50000; } 15 | }; 16 | 17 | void EstimateRelativePoses(ViewGraph& view_graph, 18 | std::unordered_map& cameras, 19 | std::unordered_map& images, 20 | const RelativePoseEstimationOptions& options); 21 | 22 | } // namespace glomap 23 | -------------------------------------------------------------------------------- /docker/start.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | cd "$(dirname "$0")" 3 | 4 | if [ "$(docker ps -a | grep glomap_container)" ]; then 5 | echo 6 | echo 7 | echo "Docker container is already running." 8 | echo "Use ./attach.sh to enter the container or use ./shutdown.sh to shutdown the container." 9 | echo "To restart container after shutdown, use ./start.sh" 10 | echo 11 | echo 12 | 13 | exit 1 14 | fi 15 | 16 | # Run the container 17 | docker run -it -d --rm --gpus=all --privileged \ 18 | --name glomap_container \ 19 | --volume "$PWD/..:/ws/" \ 20 | --volume="$HOME/.Xauthority:/root/.Xauthority:rw" \ 21 | --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \ 22 | --net=host \ 23 | -e NVIDIA_DRIVER_CAPABILITIES=all \ 24 | -e DISPLAY \ 25 | glomap_container 26 | -------------------------------------------------------------------------------- /glomap/io/colmap_io.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "glomap/io/colmap_converter.h" 4 | #include "glomap/scene/types_sfm.h" 5 | 6 | namespace glomap { 7 | 8 | void WriteGlomapReconstruction( 9 | const std::string& reconstruction_path, 10 | const std::unordered_map& cameras, 11 | const std::unordered_map& images, 12 | const std::unordered_map& tracks, 13 | const std::string output_format = "bin", 14 | const std::string image_path = ""); 15 | 16 | void WriteColmapReconstruction(const std::string& reconstruction_path, 17 | const colmap::Reconstruction& reconstruction, 18 | const std::string output_format = "bin"); 19 | 20 | } // namespace glomap 21 | -------------------------------------------------------------------------------- /glomap/controllers/track_retriangulation.h: -------------------------------------------------------------------------------- 1 | 2 | #pragma once 3 | 4 | #include "glomap/scene/types_sfm.h" 5 | 6 | #include 7 | 8 | namespace glomap { 9 | 10 | struct TriangulatorOptions { 11 | double tri_complete_max_reproj_error = 15.0; 12 | double tri_merge_max_reproj_error = 15.0; 13 | double tri_min_angle = 1.0; 14 | 15 | int min_num_matches = 15; 16 | }; 17 | 18 | bool RetriangulateTracks(const TriangulatorOptions& options, 19 | const colmap::Database& database, 20 | std::unordered_map& cameras, 21 | std::unordered_map& images, 22 | std::unordered_map& tracks); 23 | 24 | } // namespace glomap 25 | -------------------------------------------------------------------------------- /glomap/estimators/optimization_base.h: -------------------------------------------------------------------------------- 1 | 2 | #pragma once 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | namespace glomap { 10 | 11 | struct OptimizationBaseOptions { 12 | // Logging control 13 | bool verbose = false; 14 | 15 | // The threshold for the loss function 16 | double thres_loss_function = 1e-1; 17 | 18 | // The loss function for the calibration 19 | std::shared_ptr loss_function; 20 | 21 | // The options for the solver 22 | ceres::Solver::Options solver_options; 23 | 24 | OptimizationBaseOptions() { 25 | solver_options.num_threads = std::thread::hardware_concurrency(); 26 | solver_options.max_num_iterations = 100; 27 | solver_options.minimizer_progress_to_stdout = verbose; 28 | solver_options.function_tolerance = 1e-5; 29 | } 30 | }; 31 | 32 | } // namespace glomap 33 | -------------------------------------------------------------------------------- /glomap/processors/relpose_filter.h: -------------------------------------------------------------------------------- 1 | 2 | #pragma once 3 | 4 | #include "glomap/scene/types_sfm.h" 5 | 6 | namespace glomap { 7 | 8 | struct RelPoseFilter { 9 | // Filter relative pose based on rotation angle 10 | // max_angle: in degree 11 | static void FilterRotations(ViewGraph& view_graph, 12 | const std::unordered_map& images, 13 | double max_angle = 5.0); 14 | 15 | // Filter relative pose based on number of inliers 16 | // min_inlier_num: in degree 17 | static void FilterInlierNum(ViewGraph& view_graph, int min_inlier_num = 30); 18 | 19 | // Filter relative pose based on rate of inliers 20 | // min_weight: minimal ratio of inliers 21 | static void FilterInlierRatio(ViewGraph& view_graph, 22 | double min_inlier_ratio = 0.25); 23 | }; 24 | 25 | } // namespace glomap 26 | -------------------------------------------------------------------------------- /glomap/math/gravity.cc: -------------------------------------------------------------------------------- 1 | #include "gravity.h" 2 | 3 | #include "glomap/math/rigid3d.h" 4 | #include "glomap/scene/types_sfm.h" 5 | 6 | #include 7 | 8 | namespace glomap { 9 | 10 | // The second col of R_align is gravity direction 11 | Eigen::Matrix3d GetAlignRot(const Eigen::Vector3d& gravity) { 12 | Eigen::Matrix3d R; 13 | Eigen::Vector3d v = gravity.normalized(); 14 | R.col(1) = v; 15 | 16 | Eigen::Matrix3d Q = v.householderQr().householderQ(); 17 | Eigen::Matrix N = Q.rightCols(2); 18 | R.col(0) = N.col(0); 19 | R.col(2) = N.col(1); 20 | if (R.determinant() < 0) { 21 | R.col(2) = -R.col(2); 22 | } 23 | return R; 24 | } 25 | 26 | double RotUpToAngle(const Eigen::Matrix3d& R_up) { 27 | return RotationToAngleAxis(R_up)[1]; 28 | } 29 | 30 | Eigen::Matrix3d AngleToRotUp(double angle) { 31 | Eigen::Vector3d aa(0, angle, 0); 32 | return AngleAxisToRotation(aa); 33 | } 34 | } // namespace glomap -------------------------------------------------------------------------------- /colmap-310-compat.patch: -------------------------------------------------------------------------------- 1 | diff --git a/glomap/estimators/bundle_adjustment.cc b/glomap/estimators/bundle_adjustment.cc 2 | index a03bea5..a993590 100644 3 | --- a/glomap/estimators/bundle_adjustment.cc 4 | +++ b/glomap/estimators/bundle_adjustment.cc 5 | @@ -1,7 +1,6 @@ 6 | #include "bundle_adjustment.h" 7 | 8 | #include 9 | -#include 10 | #include 11 | 12 | namespace glomap { 13 | diff --git a/glomap/estimators/gravity_refinement.cc b/glomap/estimators/gravity_refinement.cc 14 | index 6015d66..2b7f6e1 100644 15 | --- a/glomap/estimators/gravity_refinement.cc 16 | +++ b/glomap/estimators/gravity_refinement.cc 17 | @@ -3,7 +3,7 @@ 18 | #include "glomap/estimators/cost_function.h" 19 | #include "glomap/math/gravity.h" 20 | 21 | -#include 22 | +#include 23 | 24 | namespace glomap { 25 | void GravityRefiner::RefineGravity(const ViewGraph& view_graph, 26 | -------------------------------------------------------------------------------- /glomap/io/recording.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include "colmap/sensor/bitmap.h" 4 | #include 5 | #include "glomap/estimators/global_positioning.h" 6 | 7 | namespace glomap { 8 | 9 | extern rerun::RecordingStream rr_rec; 10 | extern uint32_t algorithm_step; 11 | extern std::string image_path_global; // A hack to get it into the LogginCallback class without passing the option everywhere. 12 | 13 | void log_reconstruction( 14 | rerun::RecordingStream &rec, 15 | const std::unordered_map& cameras, 16 | const std::unordered_map& images, 17 | const std::unordered_map& tracks 18 | ); 19 | 20 | void log_colmap_reconstruction(rerun::RecordingStream& rec, colmap::Reconstruction& reconstruction); 21 | void log_bitmap(rerun::RecordingStream &rec, std::string_view entity_path, colmap::Bitmap& bitmap); 22 | void log_images(rerun::RecordingStream &rec, const std::unordered_map& images, const std::string image_path); 23 | void init_recording(); 24 | 25 | } 26 | -------------------------------------------------------------------------------- /glomap/processors/track_filter.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "glomap/scene/types_sfm.h" 4 | 5 | namespace glomap { 6 | 7 | struct TrackFilter { 8 | static int FilterTracksByReprojection( 9 | const ViewGraph& view_graph, 10 | const std::unordered_map& cameras, 11 | const std::unordered_map& images, 12 | std::unordered_map& tracks, 13 | double max_reprojection_error = 1e-2, 14 | bool in_normalized_image = true); 15 | 16 | static int FilterTracksByAngle( 17 | const ViewGraph& view_graph, 18 | const std::unordered_map& cameras, 19 | const std::unordered_map& images, 20 | std::unordered_map& tracks, 21 | double max_angle_error = 1.); 22 | 23 | static int FilterTrackTriangulationAngle( 24 | const ViewGraph& view_graph, 25 | const std::unordered_map& images, 26 | std::unordered_map& tracks, 27 | double min_angle = 1.); 28 | }; 29 | 30 | } // namespace glomap 31 | -------------------------------------------------------------------------------- /glomap/types.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | namespace glomap { 13 | 14 | constexpr double EPS = 1e-12; 15 | constexpr double HALF_PI = 3.141592653589793238462643383279502884L / 2; 16 | constexpr double TWO_PI = 2 * 3.141592653589793238462643383279502884L; 17 | 18 | struct InlierThresholdOptions { 19 | // Thresholds for 3D-2D matches 20 | double max_angle_error = 1.; // in degree, for global positioning 21 | double max_reprojection_error = 1e-2; // for bundle adjustment 22 | double min_triangulation_angle = 1.; // in degree, for triangulation 23 | 24 | // Thresholds for image_pair 25 | double max_epipolar_error_E = 1.; 26 | double max_epipolar_error_F = 4.; 27 | double max_epipolar_error_H = 4.; 28 | 29 | // Thresholds for edges 30 | double min_inlier_num = 30; 31 | double min_inlier_ratio = 0.25; 32 | double max_rotation_error = 10.; // in degree, for rotation averaging 33 | }; 34 | 35 | } // namespace glomap 36 | -------------------------------------------------------------------------------- /docker/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ubuntu:24.10 2 | 3 | RUN apt-get update && apt-get install -y --no-install-recommends --no-install-suggests \ 4 | curl \ 5 | fish \ 6 | git \ 7 | cmake \ 8 | ninja-build \ 9 | build-essential \ 10 | libboost-program-options-dev \ 11 | libboost-filesystem-dev \ 12 | libboost-graph-dev \ 13 | libboost-system-dev \ 14 | libeigen3-dev \ 15 | libflann-dev \ 16 | libfreeimage-dev \ 17 | libmetis-dev \ 18 | libgoogle-glog-dev \ 19 | libgtest-dev \ 20 | libsqlite3-dev \ 21 | libglew-dev \ 22 | qtbase5-dev \ 23 | libqt5opengl5-dev \ 24 | libcgal-dev \ 25 | libceres-dev \ 26 | python3-opencv \ 27 | python3-pip \ 28 | python3-venv \ 29 | pipx \ 30 | libgtk-3-dev \ 31 | libxkbcommon-x11-0 \ 32 | vulkan-tools \ 33 | colmap 34 | 35 | WORKDIR /ws/ 36 | 37 | RUN pipx install rerun-sdk==0.17.0 38 | ENV PATH=$PATH:/root/.local/share/pipx/venvs/rerun-sdk/bin/ 39 | 40 | # RUN ./install_colmap.sh 41 | # COPY ../. . 42 | # RUN cd /ws && ./build.sh 43 | -------------------------------------------------------------------------------- /glomap/math/union_find.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | namespace glomap { 6 | 7 | // UnionFind class to maintain disjoint sets for creating tracks 8 | template 9 | class UnionFind { 10 | public: 11 | // Find the root of the element x 12 | DataType Find(DataType x) { 13 | // If x is not in parent map, initialize it with x as its parent 14 | auto parentIt = parent_.find(x); 15 | if (parentIt == parent_.end()) { 16 | parent_.emplace_hint(parentIt, x, x); 17 | return x; 18 | } 19 | // Path compression: set the parent of x to the root of the set containing x 20 | if (parentIt->second != x) { 21 | parentIt->second = Find(parentIt->second); 22 | } 23 | return parentIt->second; 24 | } 25 | 26 | // Unite the sets containing x and y 27 | void Union(DataType x, DataType y) { 28 | DataType root_x = Find(x); 29 | DataType root_y = Find(y); 30 | if (root_x != root_y) parent_[root_x] = root_y; 31 | } 32 | 33 | void Clear() { parent_.clear(); } 34 | 35 | private: 36 | // Map to store the parent of each element 37 | std::unordered_map parent_; 38 | }; 39 | 40 | } // namespace glomap 41 | -------------------------------------------------------------------------------- /glomap/scene/types.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | namespace glomap { 12 | 13 | //////////////////////////////////////////////////////////////////////////////// 14 | // Index types, determines the maximum number of objects. 15 | //////////////////////////////////////////////////////////////////////////////// 16 | 17 | // Unique identifier for cameras. 18 | using colmap::camera_t; 19 | 20 | // Unique identifier for images. 21 | using colmap::image_t; 22 | 23 | // Each image pair gets a unique ID, see `Database::ImagePairToPairId`. 24 | typedef uint64_t image_pair_t; 25 | 26 | // Index per image, i.e. determines maximum number of 2D points per image. 27 | typedef uint32_t feature_t; 28 | 29 | // Unique identifier per added 3D point. Since we add many 3D points, 30 | // delete them, and possibly re-add them again, the maximum number of allowed 31 | // unique indices should be large. 32 | typedef uint64_t track_t; 33 | 34 | using colmap::Rigid3d; 35 | 36 | const image_t kMaxNumImages = std::numeric_limits::max(); 37 | const image_pair_t kInvalidImagePairId = -1; 38 | 39 | } // namespace glomap 40 | -------------------------------------------------------------------------------- /glomap/math/rigid3d.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "glomap/scene/types.h" 4 | #include "glomap/types.h" 5 | 6 | #include 7 | 8 | namespace glomap { 9 | 10 | // Calculate the rotation angle difference between two poses 11 | double CalcAngle(const Rigid3d& pose1, const Rigid3d& pose2); 12 | 13 | // Calculate the center difference between two poses 14 | double CalcTrans(const Rigid3d& pose1, const Rigid3d& pose2); 15 | 16 | // Calculatet the translation direction difference between two poses 17 | double CalcTransAngle(const Rigid3d& pose1, const Rigid3d& pose2); 18 | 19 | // Calculate the rotation angle difference between two rotations 20 | double CalcAngle(const Eigen::Matrix3d& rotation1, 21 | const Eigen::Matrix3d& rotation2); 22 | 23 | // Convert degree to radian 24 | double DegToRad(double degree); 25 | 26 | // Convert radian to degree 27 | double RadToDeg(double radian); 28 | 29 | // Convert pose to angle axis 30 | Eigen::Vector3d Rigid3dToAngleAxis(const Rigid3d& pose); 31 | 32 | // Convert rotation matrix to angle axis 33 | Eigen::Vector3d RotationToAngleAxis(const Eigen::Matrix3d& rot); 34 | 35 | // Convert angle axis to rotation matrix 36 | Eigen::Matrix3d AngleAxisToRotation(const Eigen::Vector3d& aa); 37 | 38 | } // namespace glomap 39 | -------------------------------------------------------------------------------- /glomap/processors/view_graph_manipulation.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "glomap/scene/types_sfm.h" 4 | 5 | namespace glomap { 6 | 7 | struct ViewGraphManipulater { 8 | enum StrongClusterCriteria { 9 | INLIER_NUM, 10 | WEIGHT, 11 | }; 12 | 13 | static image_pair_t SparsifyGraph(ViewGraph& view_graph, 14 | std::unordered_map& images, 15 | int expected_degree = 50); 16 | 17 | static image_t EstablishStrongClusters( 18 | ViewGraph& view_graph, 19 | std::unordered_map& images, 20 | StrongClusterCriteria criteria = INLIER_NUM, 21 | double min_thres = 100, // require strong edges 22 | int min_num_images = 2); 23 | 24 | static void UpdateImagePairsConfig( 25 | ViewGraph& view_graph, 26 | const std::unordered_map& cameras, 27 | const std::unordered_map& images); 28 | 29 | // Decompose the relative camera postion from the camera config 30 | static void DecomposeRelPose(ViewGraph& view_graph, 31 | std::unordered_map& cameras, 32 | std::unordered_map& images); 33 | }; 34 | 35 | } // namespace glomap 36 | -------------------------------------------------------------------------------- /glomap/scene/camera.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "glomap/types.h" 4 | 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | namespace glomap { 11 | 12 | struct Camera : public colmap::Camera { 13 | Camera() : colmap::Camera() {} 14 | Camera(const colmap::Camera& camera) : colmap::Camera(camera) {} 15 | 16 | Camera& operator=(const colmap::Camera& camera) { 17 | *this = Camera(camera); 18 | return *this; 19 | } 20 | 21 | bool has_refined_focal_length = false; 22 | 23 | inline double Focal() const; 24 | inline Eigen::Vector2d PrincipalPoint() const; 25 | inline Eigen::Matrix3d GetK() const; 26 | }; 27 | 28 | double Camera::Focal() const { return (FocalLengthX() + FocalLengthY()) / 2.0; } 29 | 30 | Eigen::Vector2d Camera::PrincipalPoint() const { 31 | return Eigen::Vector2d(PrincipalPointX(), PrincipalPointY()); 32 | } 33 | 34 | Eigen::Matrix3d Camera::GetK() const { 35 | Eigen::Matrix3d K; 36 | K << FocalLengthX(), 0, PrincipalPointX(), 0, FocalLengthY(), 37 | PrincipalPointY(), 0, 0, 1; 38 | return K; 39 | } 40 | 41 | inline poselib::Camera ColmapCameraToPoseLibCamera(const Camera& camera) { 42 | poselib::Camera pose_lib_camera( 43 | camera.ModelName(), camera.params, camera.width, camera.height); 44 | return pose_lib_camera; 45 | } 46 | 47 | } // namespace glomap 48 | -------------------------------------------------------------------------------- /scripts/format/clang_format.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | # This script applies clang-format to the whole repository. 4 | 5 | # Find clang-format 6 | tools=' 7 | clang-format 8 | ' 9 | 10 | clang_format='' 11 | for tool in ${tools}; do 12 | if type -p "${tool}" > /dev/null; then 13 | clang_format=$tool 14 | break 15 | fi 16 | done 17 | 18 | if [ -z "$clang_format" ]; then 19 | echo "Could not locate clang-format" 20 | exit 1 21 | fi 22 | echo "Found clang-format: $(which ${clang_format})" 23 | 24 | # Check version 25 | version_string=$($clang_format --version | sed -E 's/^.*(\d+\.\d+\.\d+-.*).*$/\1/') 26 | expected_version_string='14.0.0' 27 | if [[ "$version_string" =~ "$expected_version_string" ]]; then 28 | echo "clang-format version '$version_string' matches '$expected_version_string'" 29 | else 30 | echo "clang-format version '$version_string' doesn't match '$expected_version_string'" 31 | exit 1 32 | fi 33 | 34 | # Get all C++ files checked into the repo, excluding submodules 35 | root_folder=$(git rev-parse --show-toplevel) 36 | all_files=$( \ 37 | git ls-tree --full-tree -r --name-only HEAD . \ 38 | | grep "^glomap.*\(\.cc\|\.h\|\.hpp\|\.cpp\|\.cu\)$" \ 39 | | sed "s~^~$root_folder/~") 40 | num_files=$(echo $all_files | wc -w) 41 | echo "Formatting ${num_files} files" 42 | 43 | # Run clang-format 44 | ${clang_format} -i $all_files 45 | -------------------------------------------------------------------------------- /glomap/estimators/gravity_refinement.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "glomap/estimators/optimization_base.h" 4 | #include "glomap/math/rigid3d.h" 5 | #include "glomap/scene/types_sfm.h" 6 | #include "glomap/types.h" 7 | 8 | #include 9 | 10 | namespace glomap { 11 | 12 | struct GravityRefinerOptions : public OptimizationBaseOptions { 13 | // The minimal ratio that the gravity vector should be consistent with 14 | double max_outlier_ratio = 0.5; 15 | // The maximum allowed angle error in degree 16 | bool max_gravity_error = 1.; 17 | // Only refine the gravity of the images with more than min_neighbors 18 | int min_num_neighbors = 7; 19 | 20 | GravityRefinerOptions() : OptimizationBaseOptions() { 21 | loss_function = std::make_shared( 22 | 1 - std::cos(DegToRad(max_gravity_error))); 23 | } 24 | }; 25 | 26 | class GravityRefiner { 27 | public: 28 | GravityRefiner(const GravityRefinerOptions& options) : options_(options) {} 29 | void RefineGravity(const ViewGraph& view_graph, 30 | std::unordered_map& images); 31 | 32 | private: 33 | void IdentifyErrorProneGravity( 34 | const ViewGraph& view_graph, 35 | const std::unordered_map& images, 36 | std::unordered_set& error_prone_images); 37 | GravityRefinerOptions options_; 38 | }; 39 | 40 | } // namespace glomap 41 | -------------------------------------------------------------------------------- /glomap/io/gravity_io.cc: -------------------------------------------------------------------------------- 1 | #include "gravity_io.h" 2 | 3 | #include 4 | 5 | namespace glomap { 6 | void ReadGravity(const std::string& gravity_path, 7 | std::unordered_map& images) { 8 | std::unordered_map name_idx; 9 | for (const auto& [image_id, image] : images) { 10 | name_idx[image.file_name] = image_id; 11 | } 12 | 13 | std::ifstream file(gravity_path); 14 | 15 | // Read in the file list 16 | std::string line, item; 17 | Eigen::Vector3d gravity; 18 | int counter = 0; 19 | while (std::getline(file, line)) { 20 | std::stringstream line_stream(line); 21 | 22 | // file_name 23 | std::string name; 24 | std::getline(line_stream, name, ' '); 25 | 26 | // Gravity 27 | for (double i = 0; i < 3; i++) { 28 | std::getline(line_stream, item, ' '); 29 | gravity[i] = std::stod(item); 30 | } 31 | 32 | // Check whether the image present 33 | auto ite = name_idx.find(name); 34 | if (ite != name_idx.end()) { 35 | counter++; 36 | images[ite->second].gravity_info.SetGravity(gravity); 37 | // Make sure the initialization is aligned with the gravity 38 | images[ite->second].cam_from_world.rotation = 39 | images[ite->second].gravity_info.GetRAlign().transpose(); 40 | } 41 | } 42 | LOG(INFO) << counter << " images are loaded with gravity" << std::endl; 43 | } 44 | 45 | } // namespace glomap -------------------------------------------------------------------------------- /scripts/extract_frames.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | from __future__ import annotations 4 | 5 | import argparse 6 | import cv2 7 | import numpy as np 8 | from pathlib import Path 9 | 10 | def extract_frames(cap, desired_fps: float | None) -> list[np.ndarray]: 11 | fps = cap.get(cv2.CAP_PROP_FPS) 12 | if desired_fps: 13 | fps_ratio = desired_fps/fps 14 | else: 15 | fps_ratio = 1.0 16 | frames = [] 17 | portion = 0.0 18 | while True: 19 | ret, rgb_image = cap.read() 20 | if not ret: 21 | break 22 | portion += fps_ratio 23 | if portion >= 1.0: 24 | portion -= 1.0 25 | frames.append(rgb_image) 26 | return frames 27 | 28 | def save_frames(frames: list[np.ndarray], output_dir: Path): 29 | output_dir.mkdir(parents=True, exist_ok=True) 30 | for i, frame in enumerate(frames): 31 | cv2.imwrite(str(output_dir / f"{i:0>5}.jpg"), frame) 32 | 33 | 34 | def main(): 35 | parser = argparse.ArgumentParser() 36 | parser.add_argument("-o", "--output-dir", type=Path, required=True) 37 | parser.add_argument("-v", "--video-path", type=Path, required=True) 38 | parser.add_argument("--desired-fps", type=float, default=60.0) 39 | args = parser.parse_args() 40 | frames = extract_frames(cv2.VideoCapture(str(args.video_path)), args.desired_fps) 41 | save_frames(frames, args.output_dir) 42 | 43 | if __name__ == "__main__": 44 | main() 45 | -------------------------------------------------------------------------------- /glomap/processors/image_pair_inliers.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "glomap/math/rigid3d.h" 4 | #include "glomap/scene/types_sfm.h" 5 | #include "glomap/types.h" 6 | 7 | namespace glomap { 8 | 9 | class ImagePairInliers { 10 | public: 11 | ImagePairInliers( 12 | ImagePair& image_pair, 13 | const std::unordered_map& images, 14 | const InlierThresholdOptions& options, 15 | const std::unordered_map* cameras = nullptr) 16 | : image_pair(image_pair), 17 | images(images), 18 | cameras(cameras), 19 | options(options) {} 20 | 21 | // use the sampson error and put the inlier result into the image pair 22 | double ScoreError(); 23 | 24 | protected: 25 | // Error for the case of essential matrix 26 | double ScoreErrorEssential(); 27 | 28 | // Error for the case of fundamental matrix 29 | double ScoreErrorFundamental(); 30 | 31 | // Error for the case of homography matrix 32 | double ScoreErrorHomography(); 33 | 34 | ImagePair& image_pair; 35 | const std::unordered_map& images; 36 | const std::unordered_map* cameras; 37 | const InlierThresholdOptions& options; 38 | }; 39 | 40 | void ImagePairsInlierCount(ViewGraph& view_graph, 41 | const std::unordered_map& cameras, 42 | const std::unordered_map& images, 43 | const InlierThresholdOptions& options, 44 | bool clean_inliers); 45 | 46 | } // namespace glomap 47 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2024, ETH Zurich. 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | 14 | * Neither the name of ETH Zurich nor the names of its contributors may 15 | be used to endorse or promote products derived from this software 16 | without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE 22 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | POSSIBILITY OF SUCH DAMAGE. -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.28) 2 | 3 | project(glomap VERSION 0.0.1) 4 | 5 | set(CMAKE_CXX_STANDARD 17) 6 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 7 | 8 | set_property(GLOBAL PROPERTY GLOBAL_DEPENDS_NO_CYCLES ON) 9 | 10 | option(OPENMP_ENABLED "Whether to enable OpenMP parallelization" ON) 11 | option(TESTS_ENABLED "Whether to build test binaries" OFF) 12 | option(ASAN_ENABLED "Whether to enable AddressSanitizer flags" OFF) 13 | option(CCACHE_ENABLED "Whether to enable compiler caching, if available" ON) 14 | option(FETCH_COLMAP "Whether to use COLMAP with FetchContent or with self-installed software" ON) 15 | option(FETCH_POSELIB "Whether to use PoseLib with FetchContent or with self-installed software" ON) 16 | option(FETCH_RERUN "Whether to use Rerun with FetchContent or with self-installed software" ON) 17 | 18 | 19 | include(cmake/FindDependencies.cmake) 20 | 21 | if (TESTS_ENABLED) 22 | enable_testing() 23 | endif() 24 | 25 | if(ASAN_ENABLED) 26 | message(STATUS "Enabling ASan") 27 | add_compile_options(-fsanitize=address -fno-omit-frame-pointer -fsanitize-address-use-after-scope) 28 | add_link_options(-fsanitize=address) 29 | endif() 30 | 31 | if(CCACHE_ENABLED) 32 | find_program(CCACHE ccache) 33 | if(CCACHE) 34 | message(STATUS "Enabling ccache support") 35 | set(CMAKE_C_COMPILER_LAUNCHER ${CCACHE}) 36 | set(CMAKE_CXX_COMPILER_LAUNCHER ${CCACHE}) 37 | else() 38 | message(STATUS "Disabling ccache support") 39 | endif() 40 | else() 41 | message(STATUS "Disabling ccache support") 42 | endif() 43 | 44 | add_subdirectory(glomap) 45 | -------------------------------------------------------------------------------- /glomap/io/colmap_converter.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "glomap/scene/types_sfm.h" 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace glomap { 9 | 10 | void ConvertGlomapToColmapImage(const Image& image, 11 | colmap::Image& colmap_image, 12 | bool keep_points = false); 13 | 14 | void ConvertGlomapToColmap(const std::unordered_map& cameras, 15 | const std::unordered_map& images, 16 | const std::unordered_map& tracks, 17 | colmap::Reconstruction& reconstruction, 18 | int cluster_id = -1, 19 | bool include_image_points = false); 20 | 21 | void ConvertColmapToGlomap(const colmap::Reconstruction& reconstruction, 22 | std::unordered_map& cameras, 23 | std::unordered_map& images, 24 | std::unordered_map& tracks); 25 | 26 | void ConvertColmapPoints3DToGlomapTracks( 27 | const colmap::Reconstruction& reconstruction, 28 | std::unordered_map& tracks); 29 | 30 | void ConvertDatabaseToGlomap(const colmap::Database& database, 31 | ViewGraph& view_graph, 32 | std::unordered_map& cameras, 33 | std::unordered_map& images); 34 | 35 | } // namespace glomap 36 | -------------------------------------------------------------------------------- /glomap/processors/image_undistorter.cc: -------------------------------------------------------------------------------- 1 | #include "glomap/processors/image_undistorter.h" 2 | 3 | namespace glomap { 4 | 5 | void UndistortImages(std::unordered_map& cameras, 6 | std::unordered_map& images, 7 | bool clean_points) { 8 | std::vector image_ids; 9 | for (auto& [image_id, image] : images) { 10 | int num_points = image.features.size(); 11 | 12 | if (image.features_undist.size() == num_points && !clean_points) 13 | continue; // already undistorted 14 | image_ids.push_back(image_id); 15 | } 16 | 17 | LOG(INFO) << "Undistorting images.."; 18 | #pragma omp parallel for 19 | for (size_t i = 0; i < image_ids.size(); i++) { 20 | Eigen::Vector2d pt_undist; 21 | Eigen::Vector3d pt_undist_norm; 22 | 23 | image_t image_id = image_ids[i]; 24 | Image& image = images[image_id]; 25 | 26 | int camera_id = image.camera_id; 27 | Camera& camera = cameras[camera_id]; 28 | int num_points = image.features.size(); 29 | 30 | if (image.features_undist.size() == num_points && !clean_points) 31 | continue; // already undistorted 32 | 33 | image.features_undist.clear(); 34 | image.features_undist.reserve(num_points); 35 | for (int i = 0; i < num_points; i++) { 36 | // Undistort point in image 37 | pt_undist = camera.CamFromImg(image.features[i]); 38 | 39 | pt_undist_norm = pt_undist.homogeneous().normalized(); 40 | image.features_undist.emplace_back(pt_undist_norm); 41 | } 42 | } 43 | LOG(INFO) << "Image undistortion done"; 44 | } 45 | 46 | } // namespace glomap 47 | -------------------------------------------------------------------------------- /cmake/FindDependencies.cmake: -------------------------------------------------------------------------------- 1 | include(FetchContent) 2 | FetchContent_Declare(PoseLib 3 | GIT_REPOSITORY https://github.com/PoseLib/PoseLib.git 4 | GIT_TAG b3691b791bcedccd5451621b2275a1df0d9dcdeb 5 | EXCLUDE_FROM_ALL 6 | ) 7 | message(STATUS "Configuring PoseLib...") 8 | if (FETCH_POSELIB) 9 | FetchContent_MakeAvailable(PoseLib) 10 | else() 11 | find_package(PoseLib REQUIRED) 12 | endif() 13 | message(STATUS "Configuring PoseLib... done") 14 | 15 | FetchContent_Declare(COLMAP 16 | GIT_REPOSITORY https://github.com/colmap/colmap.git 17 | GIT_TAG 66fd8e56a0d160d68af2f29e9ac6941d442d2322 18 | EXCLUDE_FROM_ALL 19 | ) 20 | message(STATUS "Configuring COLMAP...") 21 | set(UNINSTALL_ENABLED OFF CACHE INTERNAL "") 22 | if (FETCH_COLMAP) 23 | FetchContent_MakeAvailable(COLMAP) 24 | else() 25 | find_package(COLMAP REQUIRED) 26 | endif() 27 | message(STATUS "Configuring COLMAP... done") 28 | 29 | message(STATUS "Configuring Rerun...") 30 | if (FETCH_RERUN) 31 | FetchContent_Declare(rerun_sdk URL 32 | https://github.com/rerun-io/rerun/releases/download/0.17.0/rerun_cpp_sdk.zip) 33 | FetchContent_MakeAvailable(rerun_sdk) 34 | else() 35 | find_package(rerun_sdk REQUIRED) 36 | endif() 37 | message(STATUS "Configuring Rerun... done") 38 | 39 | find_package(Eigen3 3.4 REQUIRED) 40 | find_package(Ceres REQUIRED COMPONENTS SuiteSparse) 41 | find_package(Boost REQUIRED) 42 | 43 | if(TESTS_ENABLED) 44 | message(STATUS "Enabling tests") 45 | find_package(GTest REQUIRED) 46 | endif() 47 | 48 | if (OPENMP_ENABLED) 49 | message(STATUS "Enabling OpenMP") 50 | find_package(OpenMP REQUIRED) 51 | endif() 52 | -------------------------------------------------------------------------------- /docs/getting_started.md: -------------------------------------------------------------------------------- 1 | # Getting started 2 | ### Installation and End-to-End Examples 3 | Please refer to the main `README.md` 4 | 5 | ### Recommended practice 6 | The default parameters do not always gaurantee satisfying reconstructions. 7 | Regarding this, there are several things which can generally help 8 | 9 | #### Share camera parameters as much as possible 10 | If images are known to be taken with the same camera, or images are well organized and known to be taken by several cameras, it is higly recommended to share the camera intrinsics 11 | To achieve this, one can set `--ImageReader.single_camera_per_folder` or `--ImageReader.single_camera_per_image` in `colmap feature_extractor` to be 1. 12 | 13 | #### Allow larger epipolar error 14 | If images are of high resolution, or are blurry, it is worth trying to increase the allowed epipolar error by modifying `--RelPoseEstimation.max_epipolar_error`. For example, make it 4, or 10. 15 | 16 | #### Cap the number of tracks 17 | If the number of images and points are large, the run-time of global bundle adjustment can be long. In this case, to further speed up the overall reconstruction process, the total number of points can be capped, by changing `--TrackEstablishment.max_num_tracks`. Typically, one image should not need more than 1000 tracks to achieve good performance, so this number can be adjusted to $1000 \times n$. 18 | Afterwards, if a full point cloud is desired (for example, to initialize a Gaussian Splatting), points can be triangulated directly by calling `colmap point_triangulator`. 19 | 20 | Note, if the `--skip_retriangulation` is not set true when calling `glomap mapper`, retriangulation should already been performed. 21 | -------------------------------------------------------------------------------- /glomap/scene/view_graph.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "glomap/scene/camera.h" 4 | #include "glomap/scene/image.h" 5 | #include "glomap/scene/image_pair.h" 6 | #include "glomap/scene/types.h" 7 | #include "glomap/types.h" 8 | 9 | namespace glomap { 10 | 11 | class ViewGraph { 12 | public: 13 | // Methods 14 | inline void RemoveInvalidPair(image_pair_t pair_id); 15 | 16 | // Mark the image which is not connected to any other images as not registered 17 | // Return: the number of images in the largest connected component 18 | int KeepLargestConnectedComponents( 19 | std::unordered_map& images); 20 | 21 | // Mark the cluster of the cameras (cluster_id sort by the the number of 22 | // images) 23 | int MarkConnectedComponents(std::unordered_map& images, 24 | int min_num_img = -1); 25 | 26 | // Establish the adjacency list 27 | void EstablishAdjacencyList(); 28 | 29 | inline const std::unordered_map>& 30 | GetAdjacencyList() const; 31 | 32 | // Data 33 | std::unordered_map image_pairs; 34 | 35 | image_t num_images = 0; 36 | image_pair_t num_pairs = 0; 37 | 38 | private: 39 | int FindConnectedComponent(); 40 | 41 | void BFS(image_t root, 42 | std::unordered_map& visited, 43 | std::unordered_set& component); 44 | 45 | // Data for processing 46 | std::unordered_map> adjacency_list; 47 | std::vector> connected_components; 48 | }; 49 | 50 | const std::unordered_map>& 51 | ViewGraph::GetAdjacencyList() const { 52 | return adjacency_list; 53 | } 54 | 55 | void ViewGraph::RemoveInvalidPair(image_pair_t pair_id) { 56 | ImagePair& pair = image_pairs.at(pair_id); 57 | pair.is_valid = false; 58 | } 59 | 60 | } // namespace glomap 61 | -------------------------------------------------------------------------------- /glomap/math/two_view_geometry.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "glomap/scene/camera.h" 4 | #include "glomap/scene/types.h" 5 | #include "glomap/types.h" 6 | 7 | namespace glomap { 8 | 9 | // Cheirality check for essential matrix 10 | bool CheckCheirality(const Rigid3d& pose, 11 | const Eigen::Vector3d& x1, 12 | const Eigen::Vector3d& x2, 13 | double min_depth = 0., 14 | double max_depth = 100.); 15 | 16 | // Get the orientation signum for fundamental matrix 17 | // For chierality check of fundamental matrix 18 | double GetOrientationSignum(const Eigen::Matrix3d& F, 19 | const Eigen::Vector3d& epipole, 20 | const Eigen::Vector2d& pt1, 21 | const Eigen::Vector2d& pt2); 22 | 23 | // Get the essential matrix from relative pose 24 | void EssentialFromMotion(const Rigid3d& pose, Eigen::Matrix3d* E); 25 | 26 | // Get the essential matrix from relative pose 27 | void FundamentalFromMotionAndCameras(const Camera& camera1, 28 | const Camera& camera2, 29 | const Rigid3d& pose, 30 | Eigen::Matrix3d* F); 31 | 32 | // Sampson error for the essential matrix 33 | // Input the normalized image coordinates (2d) 34 | double SampsonError(const Eigen::Matrix3d& E, 35 | const Eigen::Vector2d& x1, 36 | const Eigen::Vector2d& x2); 37 | 38 | // Sampson error for the essential matrix 39 | // Input the normalized image ray (3d) 40 | double SampsonError(const Eigen::Matrix3d& E, 41 | const Eigen::Vector3d& x1, 42 | const Eigen::Vector3d& x2); 43 | 44 | // Homography error for the homography matrix 45 | double HomographyError(const Eigen::Matrix3d& H, 46 | const Eigen::Vector2d& x1, 47 | const Eigen::Vector2d& x2); 48 | 49 | } // namespace glomap 50 | -------------------------------------------------------------------------------- /glomap/controllers/track_establishment.h: -------------------------------------------------------------------------------- 1 | 2 | #pragma once 3 | 4 | #include "glomap/math/union_find.h" 5 | #include "glomap/scene/types_sfm.h" 6 | 7 | namespace glomap { 8 | 9 | struct TrackEstablishmentOptions { 10 | // the max allowed distance for features in the same track in the same image 11 | double thres_inconsistency = 10.; 12 | 13 | // The minimal number of tracks for each view, 14 | int min_num_tracks_per_view = -1; 15 | 16 | // The minimal number of tracks for each view pair 17 | int min_num_view_per_track = 3; 18 | 19 | // The maximal number of tracks for each view pair 20 | int max_num_view_per_track = 100; 21 | 22 | // The maximal number of tracks 23 | int max_num_tracks = 10000000; 24 | }; 25 | 26 | class TrackEngine { 27 | public: 28 | TrackEngine(const ViewGraph& view_graph, 29 | const std::unordered_map& images, 30 | const TrackEstablishmentOptions& options) 31 | : options_(options), view_graph_(view_graph), images_(images) {} 32 | 33 | // Establish tracks from the view graph. Exclude the tracks that are not 34 | // consistent Return the number of tracks 35 | size_t EstablishFullTracks(std::unordered_map& tracks); 36 | 37 | // Subsample the tracks, and exclude too short / long tracks 38 | // Return the number of tracks 39 | size_t FindTracksForProblem( 40 | const std::unordered_map& tracks_full, 41 | std::unordered_map& tracks_selected); 42 | 43 | private: 44 | // Blindly concatenate tracks if any matches occur 45 | void BlindConcatenation(); 46 | 47 | // Iterate through the collected tracks and record the items for each track 48 | void TrackCollection(std::unordered_map& tracks); 49 | 50 | const TrackEstablishmentOptions& options_; 51 | 52 | const ViewGraph& view_graph_; 53 | const std::unordered_map& images_; 54 | 55 | // Internal structure used for concatenating tracks 56 | UnionFind uf_; 57 | }; 58 | 59 | } // namespace glomap 60 | -------------------------------------------------------------------------------- /pixi.toml: -------------------------------------------------------------------------------- 1 | [project] 2 | channels = ["conda-forge"] 3 | name = "glomap" 4 | platforms = ["linux-64"] 5 | version = "0.1.0" 6 | 7 | [activation] 8 | scripts = [".pixi.sh"] 9 | 10 | [tasks] 11 | clean = "rm -rf build" 12 | 13 | [tasks.download-example-data] 14 | cmd = """ 15 | test -e "data/south-building/" 16 | || ( 17 | mkdir -p checkpoints/ 18 | && wget https://demuc.de/colmap/datasets/south-building.zip 19 | && unzip south-building.zip -d data/ 20 | ) 21 | """ 22 | 23 | [tasks.example] 24 | cmd = """ 25 | glomap mapper \ 26 | --database_path data/south-building/database.db \ 27 | --image_path data/south-building/images \ 28 | --output_path data/south-building/sparse 29 | """ 30 | depends-on = ["download-example-data", "build"] 31 | 32 | [tasks._apply-patch] 33 | cmd = """ 34 | git apply --check colmap-310-compat.patch 2>/dev/null && \ 35 | git apply colmap-310-compat.patch || \ 36 | echo "Patch already applied or cannot be applied" 37 | """ 38 | [tasks._prepare] 39 | cmd = """ 40 | cmake -B build -S . -GNinja \ 41 | -DCMAKE_PREFIX_PATH=$CONDA_PREFIX \ 42 | -DCMAKE_INSTALL_PREFIX=$CONDA_PREFIX \ 43 | -DCMAKE_EXE_LINKER_FLAGS="-L$CONDA_PREFIX/lib" \ 44 | -DFETCH_COLMAP=OFF \ 45 | -DFETCH_POSELIB=OFF \ 46 | -DFETCH_RERUN=OFF 47 | """ 48 | depends-on = ["_apply-patch"] 49 | [tasks.build] 50 | cmd = "ninja -C build && ninja -C build install" 51 | depends-on = ["_prepare"] 52 | 53 | [dependencies] 54 | cmake = ">=3.30.5,<4" 55 | make = ">=4.4.1,<5" 56 | pkg-config = ">=0.29.2,<0.30" 57 | ninja = ">=1.12.1,<2" 58 | libgomp = ">=14.2.0,<15" 59 | eigen = ">=3.4.0,<4" 60 | libblas = ">=3.9.0,<4" 61 | libcblas = ">=3.9.0,<4" 62 | suitesparse = ">=7.8.3,<8" 63 | ceres-solver = ">=2.2.0,<3" 64 | libboost-devel = ">=1.86.0,<2" 65 | glog = ">=0.7.1,<0.8" 66 | poselib = ">=2.0.4,<3" 67 | colmap = ">=3.10,<4" 68 | cgal-cpp = ">=6.0.1,<7" 69 | freeimage = ">=3.18.0,<4" 70 | sqlite = ">=3.47.0,<4" 71 | gflags = ">=2.2.2,<3" 72 | rerun-sdk = "==0.17.0" 73 | librerun-sdk = ">=0.17.0,<0.18" 74 | unzip = ">=6.0,<7" 75 | wget = ">=1.21,<2" 76 | -------------------------------------------------------------------------------- /glomap/controllers/global_mapper.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "glomap/controllers/track_establishment.h" 4 | #include "glomap/controllers/track_retriangulation.h" 5 | #include "glomap/estimators/bundle_adjustment.h" 6 | #include "glomap/estimators/global_positioning.h" 7 | #include "glomap/estimators/global_rotation_averaging.h" 8 | #include "glomap/estimators/relpose_estimation.h" 9 | #include "glomap/estimators/view_graph_calibration.h" 10 | #include "glomap/types.h" 11 | 12 | #include 13 | 14 | namespace glomap { 15 | 16 | struct GlobalMapperOptions { 17 | // Options for each component 18 | ViewGraphCalibratorOptions opt_vgcalib; 19 | RelativePoseEstimationOptions opt_relpose; 20 | RotationEstimatorOptions opt_ra; 21 | TrackEstablishmentOptions opt_track; 22 | GlobalPositionerOptions opt_gp; 23 | BundleAdjusterOptions opt_ba; 24 | TriangulatorOptions opt_triangulator; 25 | 26 | // Inlier thresholds for each component 27 | InlierThresholdOptions inlier_thresholds; 28 | 29 | // Control the number of iterations for each component 30 | int num_iteration_bundle_adjustment = 3; 31 | int num_iteration_retriangulation = 1; 32 | 33 | // Control the flow of the global sfm 34 | bool skip_preprocessing = false; 35 | bool skip_view_graph_calibration = false; 36 | bool skip_relative_pose_estimation = false; 37 | bool skip_rotation_averaging = false; 38 | bool skip_track_establishment = false; 39 | bool skip_global_positioning = false; 40 | bool skip_bundle_adjustment = false; 41 | bool skip_retriangulation = false; 42 | bool skip_pruning = true; 43 | }; 44 | 45 | class GlobalMapper { 46 | public: 47 | GlobalMapper(const GlobalMapperOptions& options) : options_(options) {} 48 | 49 | bool Solve(const colmap::Database& database, 50 | ViewGraph& view_graph, 51 | std::unordered_map& cameras, 52 | std::unordered_map& images, 53 | std::unordered_map& tracks); 54 | 55 | private: 56 | const GlobalMapperOptions options_; 57 | }; 58 | 59 | } // namespace glomap 60 | -------------------------------------------------------------------------------- /glomap/scene/image.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "glomap/math/gravity.h" 4 | #include "glomap/scene/types.h" 5 | #include "glomap/types.h" 6 | 7 | namespace glomap { 8 | 9 | struct GravityInfo { 10 | public: 11 | // Whether the gravity information is available 12 | bool has_gravity = false; 13 | 14 | const Eigen::Matrix3d& GetRAlign() const { return R_align; } 15 | 16 | inline void SetGravity(const Eigen::Vector3d& g); 17 | inline Eigen::Vector3d GetGravity(); 18 | 19 | private: 20 | // Direction of the gravity 21 | Eigen::Vector3d gravity; 22 | 23 | // Alignment matrix, the second column is the gravity direction 24 | Eigen::Matrix3d R_align; 25 | }; 26 | 27 | struct Image { 28 | Image() : image_id(-1), file_name("") {} 29 | Image(image_t img_id, camera_t cam_id, std::string file_name) 30 | : image_id(img_id), file_name(file_name), camera_id(cam_id) {} 31 | 32 | // Basic information 33 | // image_id, file_name need to be specified at construction time 34 | const image_t image_id; 35 | const std::string file_name; 36 | 37 | // The id of the camera 38 | camera_t camera_id; 39 | 40 | // whether the image is within the largest connected component 41 | bool is_registered = false; 42 | int cluster_id = -1; 43 | 44 | // The pose of the image, defined as the transformation from world to camera. 45 | Rigid3d cam_from_world; 46 | 47 | // Gravity information 48 | GravityInfo gravity_info; 49 | 50 | // Features 51 | std::vector features; 52 | std::vector 53 | features_undist; // store the normalized features, can be obtained by 54 | // calling UndistortImages 55 | 56 | // Methods 57 | inline Eigen::Vector3d Center() const; 58 | }; 59 | 60 | Eigen::Vector3d Image::Center() const { 61 | return cam_from_world.rotation.inverse() * -cam_from_world.translation; 62 | } 63 | 64 | void GravityInfo::SetGravity(const Eigen::Vector3d& g) { 65 | gravity = g; 66 | R_align = GetAlignRot(g); 67 | has_gravity = true; 68 | } 69 | 70 | Eigen::Vector3d GravityInfo::GetGravity() { return gravity; } 71 | } // namespace glomap 72 | -------------------------------------------------------------------------------- /glomap/processors/relpose_filter.cc: -------------------------------------------------------------------------------- 1 | #include "glomap/processors/relpose_filter.h" 2 | 3 | #include "glomap/math/rigid3d.h" 4 | 5 | namespace glomap { 6 | 7 | void RelPoseFilter::FilterRotations( 8 | ViewGraph& view_graph, 9 | const std::unordered_map& images, 10 | double max_angle) { 11 | int num_invalid = 0; 12 | for (auto& [pair_id, image_pair] : view_graph.image_pairs) { 13 | if (image_pair.is_valid == false) continue; 14 | 15 | const Image& image1 = images.at(image_pair.image_id1); 16 | const Image& image2 = images.at(image_pair.image_id2); 17 | 18 | if (image1.is_registered == false || image2.is_registered == false) { 19 | continue; 20 | } 21 | 22 | Rigid3d pose_calc = image2.cam_from_world * Inverse(image1.cam_from_world); 23 | 24 | double angle = CalcAngle(pose_calc, image_pair.cam2_from_cam1); 25 | if (angle > max_angle) { 26 | image_pair.is_valid = false; 27 | num_invalid++; 28 | } 29 | } 30 | 31 | LOG(INFO) << "Filtered " << num_invalid << " relative rotation with angle > " 32 | << max_angle << " degrees"; 33 | } 34 | 35 | void RelPoseFilter::FilterInlierNum(ViewGraph& view_graph, int min_inlier_num) { 36 | int num_invalid = 0; 37 | for (auto& [pair_id, image_pair] : view_graph.image_pairs) { 38 | if (image_pair.is_valid == false) continue; 39 | 40 | if (image_pair.inliers.size() < min_inlier_num) { 41 | image_pair.is_valid = false; 42 | num_invalid++; 43 | } 44 | } 45 | 46 | LOG(INFO) << "Filtered " << num_invalid 47 | << " relative poses with inlier number < " << min_inlier_num; 48 | } 49 | 50 | void RelPoseFilter::FilterInlierRatio(ViewGraph& view_graph, 51 | double min_inlier_ratio) { 52 | int num_invalid = 0; 53 | for (auto& [pair_id, image_pair] : view_graph.image_pairs) { 54 | if (image_pair.is_valid == false) continue; 55 | 56 | if (image_pair.inliers.size() / double(image_pair.matches.rows()) < 57 | min_inlier_ratio) { 58 | image_pair.is_valid = false; 59 | num_invalid++; 60 | } 61 | } 62 | 63 | LOG(INFO) << "Filtered " << num_invalid 64 | << " relative poses with inlier ratio < " << min_inlier_ratio; 65 | } 66 | 67 | } // namespace glomap -------------------------------------------------------------------------------- /glomap/glomap.cc: -------------------------------------------------------------------------------- 1 | #include "glomap/exe/global_mapper.h" 2 | 3 | #include 4 | 5 | #include 6 | 7 | namespace { 8 | 9 | typedef std::function command_func_t; 10 | int ShowHelp( 11 | const std::vector>& commands) { 12 | std::cout << "GLOMAP -- Global Structure-from-Motion" << std::endl 13 | << std::endl; 14 | 15 | std::cout << "Usage:" << std::endl; 16 | std::cout << " glomap mapper --database_path DATABASE --output_path MODEL" 17 | << std::endl; 18 | std::cout << " glomap mapper_resume --input_path MODEL_INPUT --output_path " 19 | "MODEL_OUTPUT" 20 | << std::endl; 21 | 22 | std::cout << "Available commands:" << std::endl; 23 | std::cout << " help" << std::endl; 24 | for (const auto& command : commands) { 25 | std::cout << " " << command.first << std::endl; 26 | } 27 | std::cout << std::endl; 28 | 29 | return EXIT_SUCCESS; 30 | } 31 | 32 | } // namespace 33 | 34 | int main(int argc, char** argv) { 35 | colmap::InitializeGlog(argv); 36 | 37 | std::vector> commands; 38 | commands.emplace_back("mapper", &glomap::RunMapper); 39 | commands.emplace_back("mapper_resume", &glomap::RunMapperResume); 40 | 41 | if (argc == 1) { 42 | return ShowHelp(commands); 43 | } 44 | 45 | const std::string command = argv[1]; 46 | if (command == "help" || command == "-h" || command == "--help") { 47 | return ShowHelp(commands); 48 | } else { 49 | command_func_t matched_command_func = nullptr; 50 | for (const auto& command_func : commands) { 51 | if (command == command_func.first) { 52 | matched_command_func = command_func.second; 53 | break; 54 | } 55 | } 56 | if (matched_command_func == nullptr) { 57 | std::cout << "Command " << command << " not recognized. " 58 | << "To list the available commands, run `colmap help`." 59 | << std::endl; 60 | return EXIT_FAILURE; 61 | } else { 62 | int command_argc = argc - 1; 63 | char** command_argv = &argv[1]; 64 | command_argv[0] = argv[0]; 65 | return matched_command_func(command_argc, command_argv); 66 | } 67 | } 68 | 69 | return ShowHelp(commands); 70 | } 71 | -------------------------------------------------------------------------------- /glomap/math/rigid3d.cc: -------------------------------------------------------------------------------- 1 | #include "glomap/math/rigid3d.h" 2 | 3 | #include "glomap/scene/camera.h" 4 | 5 | namespace glomap { 6 | 7 | double CalcAngle(const Rigid3d& pose1, const Rigid3d& pose2) { 8 | double cos_r = 9 | ((pose1.rotation.inverse() * pose2.rotation).toRotationMatrix().trace() - 10 | 1) / 11 | 2; 12 | cos_r = std::min(std::max(cos_r, -1.), 1.); 13 | 14 | return std::acos(cos_r) * 180 / M_PI; 15 | } 16 | 17 | double CalcTrans(const Rigid3d& pose1, const Rigid3d& pose2) { 18 | return (Inverse(pose1).translation - Inverse(pose2).translation).norm(); 19 | } 20 | 21 | double CalcTransAngle(const Rigid3d& pose1, const Rigid3d& pose2) { 22 | double cos_r = (pose1.translation).dot(pose2.translation) / 23 | (pose1.translation.norm() * pose2.translation.norm()); 24 | cos_r = std::min(std::max(cos_r, -1.), 1.); 25 | 26 | return std::acos(cos_r) * 180 / M_PI; 27 | } 28 | 29 | double CalcAngle(const Eigen::Matrix3d& rotation1, 30 | const Eigen::Matrix3d& rotation2) { 31 | double cos_r = ((rotation1.transpose() * rotation2).trace() - 1) / 2; 32 | cos_r = std::min(std::max(cos_r, -1.), 1.); 33 | 34 | return std::acos(cos_r) * 180 / M_PI; 35 | } 36 | 37 | double DegToRad(double degree) { return degree * M_PI / 180; } 38 | 39 | double RadToDeg(double radian) { return radian * 180 / M_PI; } 40 | 41 | Eigen::Vector3d Rigid3dToAngleAxis(const Rigid3d& pose) { 42 | Eigen::AngleAxis aa(pose.rotation); 43 | Eigen::Vector3d aa_vec = aa.angle() * aa.axis(); 44 | return aa_vec; 45 | } 46 | 47 | Eigen::Vector3d RotationToAngleAxis(const Eigen::Matrix3d& rot) { 48 | Eigen::AngleAxis aa(rot); 49 | Eigen::Vector3d aa_vec = aa.angle() * aa.axis(); 50 | return aa_vec; 51 | } 52 | 53 | Eigen::Matrix3d AngleAxisToRotation(const Eigen::Vector3d& aa_vec) { 54 | double aa_norm = aa_vec.norm(); 55 | if (aa_norm > EPS) { 56 | return Eigen::AngleAxis(aa_norm, aa_vec.normalized()) 57 | .toRotationMatrix(); 58 | } else { 59 | Eigen::Matrix3d R; 60 | R(0, 0) = 1; 61 | R(1, 0) = aa_vec[2]; 62 | R(2, 0) = -aa_vec[1]; 63 | R(0, 1) = -aa_vec[2]; 64 | R(1, 1) = 1; 65 | R(2, 1) = aa_vec[0]; 66 | R(0, 2) = aa_vec[1]; 67 | R(1, 2) = -aa_vec[0]; 68 | R(2, 2) = 1; 69 | return R; 70 | } 71 | } 72 | } // namespace glomap -------------------------------------------------------------------------------- /glomap/estimators/view_graph_calibration.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "glomap/estimators/optimization_base.h" 4 | #include "glomap/scene/types_sfm.h" 5 | 6 | #include 7 | 8 | namespace glomap { 9 | 10 | struct ViewGraphCalibratorOptions : public OptimizationBaseOptions { 11 | // The minimal ratio of the estimated focal length against the prior focal 12 | // length 13 | double thres_lower_ratio = 0.1; 14 | // The maximal ratio of the estimated focal length against the prior focal 15 | // length 16 | double thres_higher_ratio = 10; 17 | 18 | // The threshold for the corresponding error in the problem for an image pair 19 | double thres_two_view_error = 2.; 20 | 21 | ViewGraphCalibratorOptions() : OptimizationBaseOptions() { 22 | thres_loss_function = 1e-2; 23 | } 24 | }; 25 | 26 | class ViewGraphCalibrator { 27 | public: 28 | ViewGraphCalibrator(const ViewGraphCalibratorOptions& options) 29 | : options_(options) {} 30 | 31 | // Entry point for the calibration 32 | bool Solve(ViewGraph& view_graph, 33 | std::unordered_map& cameras, 34 | std::unordered_map& images); 35 | 36 | private: 37 | // Reset the problem 38 | void Reset(const std::unordered_map& cameras); 39 | 40 | // Add the image pairs to the problem 41 | void AddImagePairsToProblem( 42 | const ViewGraph& view_graph, 43 | const std::unordered_map& cameras, 44 | const std::unordered_map& images); 45 | 46 | // Add a single image pair to the problem 47 | void AddImagePair(const ImagePair& image_pair, 48 | const std::unordered_map& cameras, 49 | const std::unordered_map& images); 50 | 51 | // Set the cameras to be constant if they have prior intrinsics 52 | size_t ParameterizeCameras( 53 | const std::unordered_map& cameras); 54 | 55 | // Convert the results back to the camera 56 | void CopyBackResults(std::unordered_map& cameras); 57 | 58 | // Filter the image pairs based on the calibration results 59 | size_t FilterImagePairs(ViewGraph& view_graph) const; 60 | 61 | ViewGraphCalibratorOptions options_; 62 | std::unique_ptr problem_; 63 | std::unordered_map focals_; 64 | }; 65 | 66 | } // namespace glomap 67 | -------------------------------------------------------------------------------- /glomap/estimators/bundle_adjustment.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "glomap/estimators/optimization_base.h" 4 | #include "glomap/scene/types_sfm.h" 5 | #include "glomap/types.h" 6 | 7 | #include 8 | 9 | namespace glomap { 10 | 11 | struct BundleAdjusterOptions : public OptimizationBaseOptions { 12 | public: 13 | // Flags for which parameters to optimize 14 | bool optimize_rotations = true; 15 | bool optimize_translation = true; 16 | bool optimize_intrinsics = true; 17 | bool optimize_points = true; 18 | 19 | // Constrain the minimum number of views per track 20 | int min_num_view_per_track = 3; 21 | 22 | BundleAdjusterOptions() : OptimizationBaseOptions() { 23 | thres_loss_function = 1.; 24 | loss_function = std::make_shared(thres_loss_function); 25 | solver_options.max_num_iterations = 200; 26 | } 27 | }; 28 | 29 | class BundleAdjuster { 30 | public: 31 | BundleAdjuster(const BundleAdjusterOptions& options) : options_(options) {} 32 | 33 | // Returns true if the optimization was a success, false if there was a 34 | // failure. 35 | // Assume tracks here are already filtered 36 | bool Solve(const ViewGraph& view_graph, 37 | std::unordered_map& cameras, 38 | std::unordered_map& images, 39 | std::unordered_map& tracks); 40 | 41 | BundleAdjusterOptions& GetOptions() { return options_; } 42 | 43 | private: 44 | // Reset the problem 45 | void Reset(); 46 | 47 | // Add tracks to the problem 48 | void AddPointToCameraConstraints( 49 | const ViewGraph& view_graph, 50 | std::unordered_map& cameras, 51 | std::unordered_map& images, 52 | std::unordered_map& tracks); 53 | 54 | // Set the parameter groups 55 | void AddCamerasAndPointsToParameterGroups( 56 | std::unordered_map& cameras, 57 | std::unordered_map& images, 58 | std::unordered_map& tracks); 59 | 60 | // Parameterize the variables, set some variables to be constant if desired 61 | void ParameterizeVariables(std::unordered_map& cameras, 62 | std::unordered_map& images, 63 | std::unordered_map& tracks); 64 | 65 | BundleAdjusterOptions options_; 66 | 67 | std::unique_ptr problem_; 68 | }; 69 | 70 | } // namespace glomap 71 | -------------------------------------------------------------------------------- /glomap/scene/image_pair.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "glomap/scene/types.h" 4 | #include "glomap/types.h" 5 | 6 | #include 7 | 8 | #include 9 | 10 | namespace glomap { 11 | 12 | // FUTURE: add covariance to the relative pose 13 | struct ImagePair { 14 | ImagePair() : pair_id(-1), image_id1(-1), image_id2(-1) {} 15 | ImagePair(image_t img_id1, image_t img_id2, Rigid3d pose_rel = Rigid3d()) 16 | : pair_id(ImagePairToPairId(img_id1, img_id2)), 17 | image_id1(img_id1), 18 | image_id2(img_id2), 19 | cam2_from_cam1(pose_rel) {} 20 | 21 | // Ids are kept constant 22 | const image_pair_t pair_id; 23 | const image_t image_id1; 24 | const image_t image_id2; 25 | 26 | // indicator whether the image pair is valid 27 | bool is_valid = true; 28 | 29 | // weight is the initial inlier rate 30 | double weight = 0; 31 | 32 | // one of `ConfigurationType`. 33 | int config = colmap::TwoViewGeometry::UNDEFINED; 34 | 35 | // Essential matrix. 36 | Eigen::Matrix3d E = Eigen::Matrix3d::Zero(); 37 | // Fundamental matrix. 38 | Eigen::Matrix3d F = Eigen::Matrix3d::Zero(); 39 | // Homography matrix. 40 | Eigen::Matrix3d H = Eigen::Matrix3d::Zero(); 41 | 42 | // Relative pose. 43 | Rigid3d cam2_from_cam1; 44 | 45 | // Matches between the two images. 46 | // First column is the index of the feature in the first image. 47 | // Second column is the index of the feature in the second image. 48 | Eigen::MatrixXi matches; 49 | 50 | // Row index of inliers in the matches matrix. 51 | std::vector inliers; 52 | 53 | static inline image_pair_t ImagePairToPairId(const image_t image_id1, 54 | const image_t image_id2); 55 | 56 | static inline void PairIdToImagePair(const image_pair_t pair_id, 57 | image_t& image_id1, 58 | image_t& image_id2); 59 | }; 60 | 61 | image_pair_t ImagePair::ImagePairToPairId(const image_t image_id1, 62 | const image_t image_id2) { 63 | if (image_id1 > image_id2) { 64 | return static_cast(kMaxNumImages) * image_id2 + image_id1; 65 | } else { 66 | return static_cast(kMaxNumImages) * image_id1 + image_id2; 67 | } 68 | } 69 | 70 | void ImagePair::PairIdToImagePair(const image_pair_t pair_id, 71 | image_t& image_id1, 72 | image_t& image_id2) { 73 | image_id1 = static_cast(pair_id % kMaxNumImages); 74 | image_id2 = static_cast((pair_id - image_id1) / kMaxNumImages); 75 | } 76 | 77 | } // namespace glomap 78 | -------------------------------------------------------------------------------- /glomap/estimators/relpose_estimation.cc: -------------------------------------------------------------------------------- 1 | #include "glomap/estimators/relpose_estimation.h" 2 | 3 | #include 4 | 5 | namespace glomap { 6 | 7 | void EstimateRelativePoses(ViewGraph& view_graph, 8 | std::unordered_map& cameras, 9 | std::unordered_map& images, 10 | const RelativePoseEstimationOptions& options) { 11 | std::vector valid_pair_ids; 12 | for (auto& [image_pair_id, image_pair] : view_graph.image_pairs) { 13 | if (!image_pair.is_valid) continue; 14 | valid_pair_ids.push_back(image_pair_id); 15 | } 16 | 17 | // Define outside loop to reuse memory and avoid reallocation. 18 | std::vector points2D_1, points2D_2; 19 | std::vector inliers; 20 | 21 | const size_t kNumChunks = 10; 22 | size_t inverval = std::ceil(valid_pair_ids.size() / kNumChunks); 23 | LOG(INFO) << "Estimating relative pose for " << valid_pair_ids.size() 24 | << " pairs"; 25 | for (size_t chunk_id = 0; chunk_id < kNumChunks; chunk_id++) { 26 | std::cout << "\r Estimating relative pose: " << chunk_id * kNumChunks << "%" 27 | << std::flush; 28 | const size_t start = chunk_id * inverval; 29 | const size_t end = 30 | std::min((chunk_id + 1) * inverval, valid_pair_ids.size()); 31 | 32 | #pragma omp parallel for schedule(dynamic) private( \ 33 | points2D_1, points2D_2, inliers) 34 | for (size_t pair_idx = start; pair_idx < end; pair_idx++) { 35 | ImagePair& image_pair = view_graph.image_pairs[valid_pair_ids[pair_idx]]; 36 | const Image& image1 = images[image_pair.image_id1]; 37 | const Image& image2 = images[image_pair.image_id2]; 38 | const Eigen::MatrixXi& matches = image_pair.matches; 39 | 40 | // Collect the original 2D points 41 | points2D_1.clear(); 42 | points2D_2.clear(); 43 | for (size_t idx = 0; idx < matches.rows(); idx++) { 44 | points2D_1.push_back(image1.features[matches(idx, 0)]); 45 | points2D_2.push_back(image2.features[matches(idx, 1)]); 46 | } 47 | 48 | inliers.clear(); 49 | poselib::CameraPose pose_rel_calc; 50 | poselib::estimate_relative_pose( 51 | points2D_1, 52 | points2D_2, 53 | ColmapCameraToPoseLibCamera(cameras[image1.camera_id]), 54 | ColmapCameraToPoseLibCamera(cameras[image2.camera_id]), 55 | options.ransac_options, 56 | options.bundle_options, 57 | &pose_rel_calc, 58 | &inliers); 59 | // Convert the relative pose to the glomap format 60 | for (int i = 0; i < 4; i++) { 61 | image_pair.cam2_from_cam1.rotation.coeffs()[i] = 62 | pose_rel_calc.q[(i + 1) % 4]; 63 | } 64 | image_pair.cam2_from_cam1.translation = pose_rel_calc.t; 65 | } 66 | } 67 | 68 | std::cout << "\r Estimating relative pose: 100%" << std::endl; 69 | LOG(INFO) << "Estimating relative pose done"; 70 | } 71 | 72 | } // namespace glomap 73 | -------------------------------------------------------------------------------- /glomap/processors/reconstruction_pruning.cc: -------------------------------------------------------------------------------- 1 | #include "glomap/processors/reconstruction_pruning.h" 2 | 3 | #include "glomap/processors/view_graph_manipulation.h" 4 | 5 | namespace glomap { 6 | image_t PruneWeaklyConnectedImages(std::unordered_map& images, 7 | std::unordered_map& tracks, 8 | int min_num_images, 9 | int min_num_observations) { 10 | // Prepare the 2d-3d correspondences 11 | std::unordered_map pair_covisibility_count; 12 | std::unordered_map image_observation_count; 13 | for (auto& [track_id, track] : tracks) { 14 | if (track.observations.size() <= 2) continue; 15 | 16 | for (size_t i = 0; i < track.observations.size(); i++) { 17 | image_observation_count[track.observations[i].first]++; 18 | for (size_t j = i + 1; j < track.observations.size(); j++) { 19 | image_t image_id1 = track.observations[i].first; 20 | image_t image_id2 = track.observations[j].first; 21 | if (image_id1 == image_id2) continue; 22 | image_pair_t pair_id = 23 | ImagePair::ImagePairToPairId(image_id1, image_id2); 24 | if (pair_covisibility_count.find(pair_id) == 25 | pair_covisibility_count.end()) { 26 | pair_covisibility_count[pair_id] = 1; 27 | } else { 28 | pair_covisibility_count[pair_id]++; 29 | } 30 | } 31 | } 32 | } 33 | 34 | // Establish the visibility graph 35 | size_t counter = 0; 36 | ViewGraph visibility_graph; 37 | std::vector pair_count; 38 | for (auto& [pair_id, count] : pair_covisibility_count) { 39 | // since the relative pose is only fixed if there are more than 5 points, 40 | // then require each pair to have at least 5 points 41 | if (count >= 5) { 42 | counter++; 43 | image_t image_id1, image_id2; 44 | ImagePair::PairIdToImagePair(pair_id, image_id1, image_id2); 45 | 46 | if (image_observation_count[image_id1] < min_num_observations || 47 | image_observation_count[image_id2] < min_num_observations) 48 | continue; 49 | 50 | visibility_graph.image_pairs.insert( 51 | std::make_pair(pair_id, ImagePair(image_id1, image_id2))); 52 | 53 | pair_count.push_back(count); 54 | visibility_graph.image_pairs[pair_id].is_valid = true; 55 | visibility_graph.image_pairs[pair_id].weight = count; 56 | } 57 | } 58 | LOG(INFO) << "Established visibility graph with " << counter << " pairs"; 59 | 60 | // sort the pair count 61 | std::sort(pair_count.begin(), pair_count.end()); 62 | double median_count = pair_count[pair_count.size() / 2]; 63 | 64 | // calculate the MAD (median absolute deviation) 65 | std::vector pair_count_diff(pair_count.size()); 66 | for (size_t i = 0; i < pair_count.size(); i++) { 67 | pair_count_diff[i] = std::abs(pair_count[i] - median_count); 68 | } 69 | std::sort(pair_count_diff.begin(), pair_count_diff.end()); 70 | double median_count_diff = pair_count_diff[pair_count_diff.size() / 2]; 71 | 72 | // The median 73 | LOG(INFO) << "Threshold for Strong Clustering: " 74 | << median_count - median_count_diff; 75 | 76 | return ViewGraphManipulater::EstablishStrongClusters( 77 | visibility_graph, 78 | images, 79 | ViewGraphManipulater::WEIGHT, 80 | std::max(median_count - median_count_diff, 20.), 81 | min_num_images); 82 | 83 | // return visibility_graph.MarkConnectedComponents(images, min_num_images); 84 | } 85 | 86 | } // namespace glomap -------------------------------------------------------------------------------- /glomap/math/two_view_geometry.cc: -------------------------------------------------------------------------------- 1 | #include "glomap/math/two_view_geometry.h" 2 | 3 | namespace glomap { 4 | // Code from PoseLib by Viktor Larsson 5 | bool CheckCheirality(const Rigid3d& pose, 6 | const Eigen::Vector3d& x1, 7 | const Eigen::Vector3d& x2, 8 | double min_depth, 9 | double max_depth) { 10 | // This code assumes that x1 and x2 are unit vectors 11 | const Eigen::Vector3d Rx1 = pose.rotation * x1; 12 | 13 | // [1 a; a 1] * [lambda1; lambda2] = [b1; b2] 14 | // [lambda1; lambda2] = [1 s-a; -a 1] * [b1; b2] / (1 - a*a) 15 | const double a = -Rx1.dot(x2); 16 | const double b1 = -Rx1.dot(pose.translation); 17 | const double b2 = x2.dot(pose.translation); 18 | 19 | // Note that we drop the factor 1.0/(1-a*a) since it is always positive. 20 | const double lambda1 = b1 - a * b2; 21 | const double lambda2 = -a * b1 + b2; 22 | 23 | min_depth = min_depth * (1 - a * a); 24 | max_depth = max_depth * (1 - a * a); 25 | 26 | bool status = lambda1 > min_depth && lambda2 > min_depth; 27 | status = status && (lambda1 < max_depth) && (lambda2 < max_depth); 28 | return status; 29 | } 30 | 31 | // This code is from GC-RANSAC by Daniel Barath 32 | double GetOrientationSignum(const Eigen::Matrix3d& F, 33 | const Eigen::Vector3d& epipole, 34 | const Eigen::Vector2d& pt1, 35 | const Eigen::Vector2d& pt2) { 36 | double signum1 = F(0, 0) * pt2[0] + F(1, 0) * pt2[1] + F(2, 0); 37 | double signum2 = epipole(1) - epipole(2) * pt1[1]; 38 | return signum1 * signum2; 39 | } 40 | 41 | void EssentialFromMotion(const Rigid3d& pose, Eigen::Matrix3d* E) { 42 | *E << 0.0, -pose.translation(2), pose.translation(1), pose.translation(2), 43 | 0.0, -pose.translation(0), -pose.translation(1), pose.translation(0), 0.0; 44 | *E = (*E) * pose.rotation.toRotationMatrix(); 45 | } 46 | 47 | // Get the essential matrix from relative pose 48 | void FundamentalFromMotionAndCameras(const Camera& camera1, 49 | const Camera& camera2, 50 | const Rigid3d& pose, 51 | Eigen::Matrix3d* F) { 52 | Eigen::Matrix3d E; 53 | EssentialFromMotion(pose, &E); 54 | *F = camera1.GetK().transpose().inverse() * E * camera2.GetK().inverse(); 55 | } 56 | 57 | double SampsonError(const Eigen::Matrix3d& E, 58 | const Eigen::Vector2d& x1, 59 | const Eigen::Vector2d& x2) { 60 | Eigen::Vector3d Ex1 = E * x1.homogeneous(); 61 | Eigen::Vector3d Etx2 = E.transpose() * x2.homogeneous(); 62 | 63 | double C = Ex1.dot(x2.homogeneous()); 64 | double Cx = Ex1.head(2).squaredNorm(); 65 | double Cy = Etx2.head(2).squaredNorm(); 66 | double r2 = C * C / (Cx + Cy); 67 | 68 | return r2; 69 | } 70 | 71 | double SampsonError(const Eigen::Matrix3d& E, 72 | const Eigen::Vector3d& x1, 73 | const Eigen::Vector3d& x2) { 74 | Eigen::Vector3d Ex1 = E * x1 / (EPS + x1[2]); 75 | Eigen::Vector3d Etx2 = E.transpose() * x2 / (EPS + x2[2]); 76 | 77 | double C = Ex1.dot(x2); 78 | double Cx = Ex1.head(2).squaredNorm(); 79 | double Cy = Etx2.head(2).squaredNorm(); 80 | double r2 = C * C / (Cx + Cy); 81 | 82 | return r2; 83 | } 84 | 85 | double HomographyError(const Eigen::Matrix3d& H, 86 | const Eigen::Vector2d& x1, 87 | const Eigen::Vector2d& x2) { 88 | Eigen::Vector3d Hx1 = H * x1.homogeneous(); 89 | Eigen::Vector2d Hx1_norm = Hx1.head(2) / (EPS + Hx1[2]); 90 | double r2 = (Hx1_norm - x2).squaredNorm(); 91 | 92 | return r2; 93 | } 94 | 95 | } // namespace glomap -------------------------------------------------------------------------------- /glomap/io/colmap_io.cc: -------------------------------------------------------------------------------- 1 | #include "glomap/io/colmap_io.h" 2 | #include 3 | #include 4 | 5 | namespace glomap { 6 | 7 | void WriteGlomapReconstruction( 8 | const std::string& reconstruction_path, 9 | const std::unordered_map& cameras, 10 | const std::unordered_map& images, 11 | const std::unordered_map& tracks, 12 | const std::string output_format, 13 | const std::string image_path) { 14 | // Check whether reconstruction pruning is applied. 15 | // If so, export seperate reconstruction 16 | int largest_component_num = -1; 17 | for (const auto& [image_id, image] : images) { 18 | if (image.cluster_id > largest_component_num) 19 | largest_component_num = image.cluster_id; 20 | } 21 | // If it is not seperated into several clusters, then output them as whole 22 | if (largest_component_num == -1) { 23 | colmap::Reconstruction reconstruction; 24 | 25 | ConvertGlomapToColmap(cameras, images, tracks, reconstruction); 26 | // Read in colors 27 | if (image_path != "") { 28 | LOG(INFO) << "Extracting colors ..."; 29 | reconstruction.ExtractColorsForAllImages(image_path); 30 | } 31 | 32 | // Convert back to GLOMAP so that we can log the reconstruction with color. 33 | std::unordered_map cameras_copy; 34 | std::unordered_map images_copy; 35 | std::unordered_map tracks_copy; 36 | ConvertColmapToGlomap(reconstruction, cameras_copy, images_copy, tracks_copy); 37 | rr_rec.set_time_sequence("step", algorithm_step++); 38 | rr_rec.log("status", rerun::TextLog("Converted to Colmap and extracted colors")); 39 | log_reconstruction(rr_rec, cameras_copy, images_copy, tracks_copy); 40 | 41 | colmap::CreateDirIfNotExists(reconstruction_path + "/0", true); 42 | if (output_format == "txt") { 43 | reconstruction.WriteText(reconstruction_path + "/0"); 44 | } else if (output_format == "bin") { 45 | reconstruction.WriteBinary(reconstruction_path + "/0"); 46 | } else { 47 | LOG(ERROR) << "Unsupported output type"; 48 | } 49 | } else { 50 | for (int comp = 0; comp <= largest_component_num; comp++) { 51 | std::cout << "\r Exporting reconstruction " << comp + 1 << " / " 52 | << largest_component_num + 1 << std::flush; 53 | colmap::Reconstruction reconstruction; 54 | ConvertGlomapToColmap(cameras, images, tracks, reconstruction, comp); 55 | // Read in colors 56 | if (image_path != "") { 57 | reconstruction.ExtractColorsForAllImages(image_path); 58 | } 59 | 60 | colmap::CreateDirIfNotExists( 61 | reconstruction_path + "/" + std::to_string(comp), true); 62 | if (output_format == "txt") { 63 | reconstruction.WriteText(reconstruction_path + "/" + 64 | std::to_string(comp)); 65 | } else if (output_format == "bin") { 66 | reconstruction.WriteBinary(reconstruction_path + "/" + 67 | std::to_string(comp)); 68 | } else { 69 | LOG(ERROR) << "Unsupported output type"; 70 | } 71 | } 72 | std::cout << std::endl; 73 | } 74 | } 75 | 76 | void WriteColmapReconstruction(const std::string& reconstruction_path, 77 | const colmap::Reconstruction& reconstruction, 78 | const std::string output_format) { 79 | colmap::CreateDirIfNotExists(reconstruction_path, true); 80 | if (output_format == "txt") { 81 | reconstruction.WriteText(reconstruction_path); 82 | } else if (output_format == "bin") { 83 | reconstruction.WriteBinary(reconstruction_path); 84 | } else { 85 | LOG(ERROR) << "Unsupported output type"; 86 | } 87 | } 88 | 89 | } // namespace glomap 90 | -------------------------------------------------------------------------------- /glomap/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(SOURCES 2 | controllers/global_mapper.cc 3 | controllers/option_manager.cc 4 | controllers/track_establishment.cc 5 | controllers/track_retriangulation.cc 6 | estimators/bundle_adjustment.cc 7 | estimators/global_positioning.cc 8 | estimators/global_rotation_averaging.cc 9 | estimators/gravity_refinement.cc 10 | estimators/relpose_estimation.cc 11 | estimators/view_graph_calibration.cc 12 | io/colmap_converter.cc 13 | io/colmap_io.cc 14 | io/gravity_io.cc 15 | io/recording.cc 16 | math/gravity.cc 17 | math/rigid3d.cc 18 | math/tree.cc 19 | math/two_view_geometry.cc 20 | processors/image_pair_inliers.cc 21 | processors/image_undistorter.cc 22 | processors/reconstruction_pruning.cc 23 | processors/relpose_filter.cc 24 | processors/track_filter.cc 25 | processors/view_graph_manipulation.cc 26 | scene/view_graph.cc 27 | ) 28 | 29 | set(HEADERS 30 | controllers/global_mapper.h 31 | controllers/option_manager.h 32 | controllers/track_establishment.h 33 | controllers/track_retriangulation.h 34 | estimators/bundle_adjustment.h 35 | estimators/cost_function.h 36 | estimators/global_positioning.h 37 | estimators/global_rotation_averaging.h 38 | estimators/gravity_refinement.h 39 | estimators/relpose_estimation.h 40 | estimators/optimization_base.h 41 | estimators/view_graph_calibration.h 42 | io/colmap_converter.h 43 | io/colmap_io.h 44 | io/gravity_io.h 45 | io/recording.h 46 | math/gravity.h 47 | math/l1_solver.h 48 | math/rigid3d.h 49 | math/tree.h 50 | math/two_view_geometry.h 51 | math/union_find.h 52 | processors/image_pair_inliers.h 53 | processors/image_undistorter.h 54 | processors/reconstruction_pruning.h 55 | processors/relpose_filter.h 56 | processors/track_filter.h 57 | processors/view_graph_manipulation.h 58 | scene/camera.h 59 | scene/image_pair.h 60 | scene/image.h 61 | scene/track.h 62 | scene/types_sfm.h 63 | scene/types.h 64 | ) 65 | 66 | add_library(glomap ${SOURCES} ${HEADERS}) 67 | if(NOT FETCH_COLMAP) 68 | target_link_libraries(glomap PUBLIC colmap::colmap) 69 | else() 70 | target_link_libraries(glomap PUBLIC colmap) 71 | endif() 72 | 73 | if(NOT FETCH_POSELIB) 74 | target_link_libraries(glomap PUBLIC PoseLib::PoseLib) 75 | else() 76 | target_link_libraries(glomap PUBLIC PoseLib) 77 | endif() 78 | 79 | target_link_libraries(glomap PUBLIC rerun_sdk) 80 | 81 | target_link_libraries( 82 | glomap 83 | PUBLIC 84 | Eigen3::Eigen 85 | Ceres::ceres 86 | ${BOOST_LIBRARIES} 87 | ${SuiteSparse_CHOLMOD_LIBRARY} 88 | ) 89 | target_include_directories( 90 | glomap 91 | PUBLIC 92 | .. 93 | ${SuiteSparse_CHOLMOD_INCLUDE_DIR} 94 | ) 95 | 96 | if(OPENMP_FOUND) 97 | target_link_libraries(glomap PUBLIC OpenMP::OpenMP_CXX) 98 | endif() 99 | 100 | if(MSVC) 101 | target_compile_options(glomap PRIVATE /bigobj) 102 | else() 103 | target_compile_options(glomap PRIVATE 104 | -Wall 105 | -Werror 106 | -Wno-sign-compare 107 | -Wno-unused-variable 108 | ) 109 | endif() 110 | 111 | 112 | add_executable(glomap_main 113 | glomap.cc 114 | exe/global_mapper.h 115 | exe/global_mapper.cc) 116 | target_link_libraries(glomap_main glomap) 117 | 118 | set_target_properties(glomap_main PROPERTIES OUTPUT_NAME glomap) 119 | install(TARGETS glomap_main DESTINATION bin) 120 | 121 | 122 | if(TESTS_ENABLED) 123 | add_executable(glomap_test 124 | controllers/global_mapper_test.cc 125 | ) 126 | target_link_libraries( 127 | glomap_test 128 | PRIVATE 129 | glomap 130 | GTest::gtest 131 | GTest::gtest_main) 132 | add_test(NAME glomap_test COMMAND glomap_test) 133 | endif() 134 | -------------------------------------------------------------------------------- /glomap/scene/view_graph.cc: -------------------------------------------------------------------------------- 1 | #include "glomap/scene/view_graph.h" 2 | 3 | #include "glomap/math/union_find.h" 4 | 5 | #include 6 | 7 | namespace glomap { 8 | 9 | int ViewGraph::KeepLargestConnectedComponents( 10 | std::unordered_map& images) { 11 | EstablishAdjacencyList(); 12 | 13 | int num_comp = FindConnectedComponent(); 14 | 15 | int max_idx = -1; 16 | int max_img = 0; 17 | for (int comp = 0; comp < num_comp; comp++) { 18 | if (connected_components[comp].size() > max_img) { 19 | max_img = connected_components[comp].size(); 20 | max_idx = comp; 21 | } 22 | } 23 | 24 | std::unordered_set largest_component = connected_components[max_idx]; 25 | 26 | // Set all images to not registered 27 | for (auto& [image_id, image] : images) image.is_registered = false; 28 | 29 | // Set the images in the largest component to registered 30 | for (auto image_id : largest_component) images[image_id].is_registered = true; 31 | 32 | // set all pairs not in the largest component to invalid 33 | num_pairs = 0; 34 | for (auto& [pair_id, image_pair] : image_pairs) { 35 | if (!images[image_pair.image_id1].is_registered || 36 | !images[image_pair.image_id2].is_registered) { 37 | image_pair.is_valid = false; 38 | } 39 | if (image_pair.is_valid) num_pairs++; 40 | } 41 | 42 | num_images = largest_component.size(); 43 | return max_img; 44 | } 45 | 46 | int ViewGraph::FindConnectedComponent() { 47 | connected_components.clear(); 48 | std::unordered_map visited; 49 | for (auto& [image_id, neighbors] : adjacency_list) { 50 | visited[image_id] = false; 51 | } 52 | 53 | for (auto& [image_id, neighbors] : adjacency_list) { 54 | if (!visited[image_id]) { 55 | std::unordered_set component; 56 | BFS(image_id, visited, component); 57 | connected_components.push_back(component); 58 | } 59 | } 60 | 61 | return connected_components.size(); 62 | } 63 | 64 | int ViewGraph::MarkConnectedComponents( 65 | std::unordered_map& images, int min_num_img) { 66 | EstablishAdjacencyList(); 67 | 68 | int num_comp = FindConnectedComponent(); 69 | 70 | std::vector> cluster_num_img(num_comp); 71 | for (int comp = 0; comp < num_comp; comp++) { 72 | cluster_num_img[comp] = 73 | std::make_pair(connected_components[comp].size(), comp); 74 | } 75 | std::sort(cluster_num_img.begin(), cluster_num_img.end(), std::greater<>()); 76 | 77 | // Set the cluster number of every image to be -1 78 | for (auto& [image_id, image] : images) image.cluster_id = -1; 79 | 80 | int comp = 0; 81 | for (; comp < num_comp; comp++) { 82 | if (cluster_num_img[comp].first < min_num_img) break; 83 | for (auto image_id : connected_components[cluster_num_img[comp].second]) 84 | images[image_id].cluster_id = comp; 85 | } 86 | 87 | return comp; 88 | } 89 | 90 | void ViewGraph::BFS(image_t root, 91 | std::unordered_map& visited, 92 | std::unordered_set& component) { 93 | std::queue q; 94 | q.push(root); 95 | visited[root] = true; 96 | component.insert(root); 97 | 98 | while (!q.empty()) { 99 | image_t curr = q.front(); 100 | q.pop(); 101 | 102 | for (image_t neighbor : adjacency_list[curr]) { 103 | if (!visited[neighbor]) { 104 | q.push(neighbor); 105 | visited[neighbor] = true; 106 | component.insert(neighbor); 107 | } 108 | } 109 | } 110 | } 111 | 112 | void ViewGraph::EstablishAdjacencyList() { 113 | adjacency_list.clear(); 114 | for (auto& [pair_id, image_pair] : image_pairs) { 115 | if (image_pair.is_valid) { 116 | adjacency_list[image_pair.image_id1].insert(image_pair.image_id2); 117 | adjacency_list[image_pair.image_id2].insert(image_pair.image_id1); 118 | } 119 | } 120 | } 121 | } // namespace glomap 122 | -------------------------------------------------------------------------------- /glomap/math/l1_solver.h: -------------------------------------------------------------------------------- 1 | // This code is adapted from Theia library (http://theia-sfm.org/), 2 | // with its original L1 solver adapted from 3 | // "https://web.stanford.edu/~boyd/papers/admm/least_abs_deviations/lad.html" 4 | 5 | #pragma once 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | // An L1 norm (|| A * x - b ||_1) approximation solver based on ADMM 14 | // (alternating direction method of multipliers, 15 | // https://web.stanford.edu/~boyd/papers/pdf/admm_distr_stats.pdf). 16 | namespace glomap { 17 | 18 | // TODO: L1 solver for dense matrix 19 | struct L1SolverOptions { 20 | int max_num_iterations = 1000; 21 | // Rho is the augmented Lagrangian parameter. 22 | double rho = 1.0; 23 | // Alpha is the over-relaxation parameter (typically between 1.0 and 1.8). 24 | double alpha = 1.0; 25 | 26 | double absolute_tolerance = 1e-4; 27 | double relative_tolerance = 1e-2; 28 | }; 29 | 30 | template 31 | class L1Solver { 32 | public: 33 | L1Solver(const L1SolverOptions& options, const MatrixType& mat) 34 | : options_(options), a_(mat) { 35 | // Pre-compute the sparsity pattern. 36 | const MatrixType spd_mat = a_.transpose() * a_; 37 | linear_solver_.compute(spd_mat); 38 | } 39 | 40 | void Solve(const Eigen::VectorXd& rhs, Eigen::VectorXd* solution) { 41 | Eigen::VectorXd& x = *solution; 42 | Eigen::VectorXd z(a_.rows()), u(a_.rows()); 43 | z.setZero(); 44 | u.setZero(); 45 | 46 | Eigen::VectorXd a_times_x(a_.rows()), z_old(z.size()), ax_hat(a_.rows()); 47 | // Precompute some convergence terms. 48 | const double rhs_norm = rhs.norm(); 49 | const double primal_abs_tolerance_eps = 50 | std::sqrt(a_.rows()) * options_.absolute_tolerance; 51 | const double dual_abs_tolerance_eps = 52 | std::sqrt(a_.cols()) * options_.absolute_tolerance; 53 | 54 | const std::string row_format = 55 | " % 4d % 4.4e % 4.4e % 4.4e % 4.4e"; 56 | for (int i = 0; i < options_.max_num_iterations; i++) { 57 | // Update x. 58 | x.noalias() = linear_solver_.solve(a_.transpose() * (rhs + z - u)); 59 | if (linear_solver_.info() != Eigen::Success) { 60 | LOG(ERROR) << "L1 Minimization failed. Could not solve the sparse " 61 | "linear system with Cholesky Decomposition"; 62 | return; 63 | } 64 | 65 | a_times_x.noalias() = a_ * x; 66 | ax_hat.noalias() = options_.alpha * a_times_x; 67 | ax_hat.noalias() += (1.0 - options_.alpha) * (z + rhs); 68 | 69 | // Update z and set z_old. 70 | std::swap(z, z_old); 71 | z.noalias() = Shrinkage(ax_hat - rhs + u, 1.0 / options_.rho); 72 | 73 | // Update u. 74 | u.noalias() += ax_hat - z - rhs; 75 | 76 | // Compute the convergence terms. 77 | const double r_norm = (a_times_x - z - rhs).norm(); 78 | const double s_norm = 79 | (-options_.rho * a_.transpose() * (z - z_old)).norm(); 80 | const double max_norm = std::max({a_times_x.norm(), z.norm(), rhs_norm}); 81 | const double primal_eps = 82 | primal_abs_tolerance_eps + options_.relative_tolerance * max_norm; 83 | const double dual_eps = dual_abs_tolerance_eps + 84 | options_.relative_tolerance * 85 | (options_.rho * a_.transpose() * u).norm(); 86 | 87 | // Determine if the minimizer has converged. 88 | if (r_norm < primal_eps && s_norm < dual_eps) { 89 | break; 90 | } 91 | } 92 | } 93 | 94 | private: 95 | const L1SolverOptions& options_; 96 | 97 | // Matrix A in || Ax - b ||_1 98 | const MatrixType a_; 99 | 100 | // Cholesky linear solver. Since our linear system will be a SPD matrix we can 101 | // utilize the Cholesky factorization. 102 | Eigen::CholmodSupernodalLLT> linear_solver_; 103 | 104 | static Eigen::VectorXd Shrinkage(const Eigen::VectorXd& vec, 105 | const double kappa) { 106 | Eigen::ArrayXd zero_vec(vec.size()); 107 | zero_vec.setZero(); 108 | return zero_vec.max(vec.array() - kappa) - 109 | zero_vec.max(-vec.array() - kappa); 110 | } 111 | }; 112 | 113 | } // namespace glomap 114 | -------------------------------------------------------------------------------- /glomap/estimators/global_positioning.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "glomap/estimators/optimization_base.h" 4 | #include "glomap/scene/types_sfm.h" 5 | #include "glomap/types.h" 6 | 7 | namespace glomap { 8 | 9 | struct GlobalPositionerOptions : public OptimizationBaseOptions { 10 | // ONLY_POINTS is recommended 11 | enum ConstraintType { 12 | // only include camera to point constraints 13 | ONLY_POINTS, 14 | // only include camera to camera constraints 15 | ONLY_CAMERAS, 16 | // the points and cameras are reweighted to have similar total contribution 17 | POINTS_AND_CAMERAS_BALANCED, 18 | // treat each contribution from camera to point and camera to camera equally 19 | POINTS_AND_CAMERAS, 20 | }; 21 | 22 | // Whether initialize the reconstruction randomly 23 | bool generate_random_positions = true; 24 | bool generate_random_points = true; 25 | bool generate_scales = true; // Now using fixed 1 as initializaiton 26 | 27 | // Flags for which parameters to optimize 28 | bool optimize_positions = true; 29 | bool optimize_points = true; 30 | bool optimize_scales = true; 31 | 32 | // Constrain the minimum number of views per track 33 | int min_num_view_per_track = 3; 34 | 35 | // Random seed 36 | unsigned seed = 1; 37 | 38 | // the type of global positioning 39 | ConstraintType constraint_type = ONLY_POINTS; 40 | double constraint_reweight_scale = 41 | 1.0; // only relevant for POINTS_AND_CAMERAS_BALANCED 42 | 43 | GlobalPositionerOptions() : OptimizationBaseOptions() { 44 | thres_loss_function = 1e-1; 45 | loss_function = std::make_shared(thres_loss_function); 46 | } 47 | }; 48 | 49 | class GlobalPositioner { 50 | public: 51 | GlobalPositioner(const GlobalPositionerOptions& options); 52 | 53 | // Returns true if the optimization was a success, false if there was a 54 | // failure. 55 | // Assume tracks here are already filtered 56 | bool Solve(const ViewGraph& view_graph, 57 | std::unordered_map& cameras, 58 | std::unordered_map& images, 59 | std::unordered_map& tracks); 60 | 61 | GlobalPositionerOptions& GetOptions() { return options_; } 62 | 63 | protected: 64 | // Reset the problem 65 | void Reset(); 66 | 67 | // Initialize all cameras to be random. 68 | void InitializeRandomPositions(const ViewGraph& view_graph, 69 | std::unordered_map& images, 70 | std::unordered_map& tracks); 71 | 72 | // Creates camera to camera constraints from relative translations. (3D) 73 | void AddCameraToCameraConstraints(const ViewGraph& view_graph, 74 | std::unordered_map& images); 75 | 76 | // Add tracks to the problem 77 | void AddPointToCameraConstraints( 78 | std::unordered_map& cameras, 79 | std::unordered_map& images, 80 | std::unordered_map& tracks); 81 | 82 | // Add a single track to the problem 83 | void AddTrackToProblem(const track_t& track_id, 84 | std::unordered_map& cameras, 85 | std::unordered_map& images, 86 | std::unordered_map& tracks); 87 | 88 | // Set the parameter groups 89 | void AddCamerasAndPointsToParameterGroups( 90 | std::unordered_map& images, 91 | std::unordered_map& tracks); 92 | 93 | // Parameterize the variables, set some variables to be constant if desired 94 | void ParameterizeVariables(std::unordered_map& images, 95 | std::unordered_map& tracks); 96 | 97 | // During the optimization, the camera translation is set to be the camera 98 | // center Convert the results back to camera poses 99 | void ConvertResults(std::unordered_map& images); 100 | 101 | // Data members 102 | GlobalPositionerOptions options_; 103 | 104 | std::mt19937 random_generator_; 105 | std::unique_ptr problem_; 106 | 107 | // loss functions for reweighted terms 108 | std::shared_ptr loss_function_ptcam_uncalibrated_; 109 | std::shared_ptr loss_function_ptcam_calibrated_; 110 | 111 | std::unordered_map scales_; 112 | }; 113 | 114 | } // namespace glomap 115 | -------------------------------------------------------------------------------- /glomap/io/recording.cc: -------------------------------------------------------------------------------- 1 | #include "recording.h" 2 | #include 3 | // #include shape = {height, width, nchannels}; 21 | auto buffer = bitmap.ConvertToRowMajorArray(); 22 | LOG(INFO) << buffer.size(); 23 | LOG(INFO) << entity_path; 24 | if (nchannels == 3) { 25 | for (size_t i = 0; i < buffer.size(); i+=3) { 26 | std::swap(buffer[i], buffer[i+2]); 27 | } 28 | } 29 | rec.log(entity_path, rerun::Image(shape, std::move(buffer))); 30 | } 31 | 32 | std::unordered_map> get_observation_to_point_map( 33 | const std::unordered_map& images, 34 | const std::unordered_map& tracks 35 | ) { 36 | // Prepare the 2d-3d correspondences 37 | std::unordered_map> image_to_point3D; 38 | if (tracks.size()) { 39 | // Initialize every point to corresponds to invalid point 40 | for (auto& [image_id, image] : images) { 41 | if (!image.is_registered) 42 | continue; 43 | image_to_point3D[image_id] = 44 | std::vector(image.features.size(), -1); 45 | } 46 | 47 | for (auto& [track_id, track] : tracks) { 48 | if (track.observations.size() < 3) { 49 | continue; 50 | } 51 | for (auto& observation : track.observations) { 52 | if (image_to_point3D.find(observation.first) != 53 | image_to_point3D.end()) { 54 | image_to_point3D[observation.first][observation.second] = track_id; 55 | } 56 | } 57 | } 58 | } 59 | return image_to_point3D; 60 | } 61 | 62 | void log_reconstruction( 63 | rerun::RecordingStream &rec, 64 | const std::unordered_map& cameras, 65 | const std::unordered_map& images, 66 | const std::unordered_map& tracks 67 | ) { 68 | std::vector points; 69 | std::vector colors; 70 | 71 | 72 | for (auto &[_, image] : images) { 73 | auto camera = cameras.at(image.camera_id); 74 | Eigen::Vector3f translation = image.cam_from_world.translation.cast(); 75 | Eigen::Matrix3f rotation = image.cam_from_world.rotation.toRotationMatrix().cast(); 76 | rec.log("images/" + image.file_name, rerun::Transform3D( 77 | rerun::datatypes::TranslationAndMat3x3( 78 | rerun::Vec3D(translation.data()), 79 | rerun::Mat3x3(rotation.data()), 80 | true 81 | ) 82 | )); 83 | rec.log_static("images/" + image.file_name, rerun::ViewCoordinates::RDF); 84 | Eigen::Matrix3Xf K = camera.GetK().cast(); 85 | 86 | rec.log( 87 | "images/" + image.file_name, 88 | rerun::Pinhole(rerun::components::PinholeProjection(rerun::datatypes::Mat3x3(K.data()))) 89 | .with_resolution(int(camera.width), int(camera.height)) 90 | ); 91 | } 92 | 93 | for (auto &[_, track] : tracks) { 94 | 95 | // Should actually be `track.observations.size() < options_.min_num_view_per_track`. 96 | if (track.observations.size() < 3) continue; 97 | 98 | auto xyz = track.xyz; 99 | points.emplace_back(xyz.x(), xyz.y(), xyz.z()); 100 | colors.emplace_back(track.color[0], track.color[1], track.color[2]); 101 | } 102 | rec.log("tracks", rerun::Points3D(points).with_colors(colors)); 103 | 104 | } 105 | 106 | void log_images(rerun::RecordingStream &rec, const std::unordered_map& images, const std::string image_path) { 107 | for (auto &[id, image] : images) { 108 | std::string path = colmap::JoinPaths(image_path, image.file_name); 109 | colmap::Bitmap bitmap; 110 | if (!bitmap.Read(path)) { 111 | LOG(ERROR) << "Failed to read image path"; 112 | } 113 | std::string entity_path = "images/"; 114 | entity_path += image.file_name; 115 | log_bitmap(rec, entity_path, bitmap); 116 | 117 | std::vector feature_positions; 118 | for (auto &point2d : image.features) { 119 | feature_positions.emplace_back(point2d[0], point2d[1]); 120 | } 121 | rec.log(entity_path + "/features", rerun::Points2D(feature_positions)); 122 | } 123 | } 124 | 125 | } -------------------------------------------------------------------------------- /glomap/processors/track_filter.cc: -------------------------------------------------------------------------------- 1 | #include "glomap/processors/track_filter.h" 2 | 3 | #include "glomap/math/rigid3d.h" 4 | 5 | namespace glomap { 6 | 7 | int TrackFilter::FilterTracksByReprojection( 8 | const ViewGraph& view_graph, 9 | const std::unordered_map& cameras, 10 | const std::unordered_map& images, 11 | std::unordered_map& tracks, 12 | double max_reprojection_error, 13 | bool in_normalized_image) { 14 | int counter = 0; 15 | for (auto& [track_id, track] : tracks) { 16 | std::vector observation_new; 17 | for (auto& [image_id, feature_id] : track.observations) { 18 | const Image& image = images.at(image_id); 19 | Eigen::Vector3d pt_calc = image.cam_from_world * track.xyz; 20 | if (pt_calc(2) < EPS) continue; 21 | 22 | double reprojection_error = max_reprojection_error; 23 | if (in_normalized_image) { 24 | const Eigen::Vector3d& feature_undist = 25 | image.features_undist.at(feature_id); 26 | 27 | Eigen::Vector2d pt_reproj = pt_calc.head(2) / pt_calc(2); 28 | reprojection_error = 29 | (pt_reproj - feature_undist.head(2) / (feature_undist(2) + EPS)) 30 | .norm(); 31 | } else { 32 | Eigen::Vector2d pt_reproj = pt_calc.head(2) / pt_calc(2); 33 | Eigen::Vector2d pt_dist; 34 | pt_dist = cameras.at(image.camera_id).ImgFromCam(pt_reproj); 35 | reprojection_error = (pt_dist - image.features.at(feature_id)).norm(); 36 | } 37 | 38 | // If the reprojection error is smaller than the threshold, then keep it 39 | if (reprojection_error < max_reprojection_error) { 40 | observation_new.emplace_back(std::make_pair(image_id, feature_id)); 41 | } 42 | } 43 | if (observation_new.size() != track.observations.size()) { 44 | counter++; 45 | track.observations = observation_new; 46 | } 47 | } 48 | LOG(INFO) << "Filtered " << counter << " / " << tracks.size() 49 | << " tracks by reprojection error"; 50 | return counter; 51 | } 52 | 53 | int TrackFilter::FilterTracksByAngle( 54 | const ViewGraph& view_graph, 55 | const std::unordered_map& cameras, 56 | const std::unordered_map& images, 57 | std::unordered_map& tracks, 58 | double max_angle_error) { 59 | int counter = 0; 60 | double thres = std::cos(DegToRad(max_angle_error)); 61 | double thres_uncalib = std::cos(DegToRad(max_angle_error * 2)); 62 | for (auto& [track_id, track] : tracks) { 63 | std::vector observation_new; 64 | for (auto& [image_id, feature_id] : track.observations) { 65 | const Image& image = images.at(image_id); 66 | // const Camera& camera = image.camera; 67 | const Eigen::Vector3d& feature_undist = 68 | image.features_undist.at(feature_id); 69 | Eigen::Vector3d pt_calc = image.cam_from_world * track.xyz; 70 | if (pt_calc(2) < EPS) continue; 71 | 72 | pt_calc = pt_calc.normalized(); 73 | double thres_cam = (cameras.at(image.camera_id).has_prior_focal_length) 74 | ? thres 75 | : thres_uncalib; 76 | 77 | if (pt_calc.dot(feature_undist) > thres_cam) { 78 | observation_new.emplace_back(std::make_pair(image_id, feature_id)); 79 | } 80 | } 81 | if (observation_new.size() != track.observations.size()) { 82 | counter++; 83 | track.observations = observation_new; 84 | } 85 | } 86 | LOG(INFO) << "Filtered " << counter << " / " << tracks.size() 87 | << " tracks by angle error"; 88 | return counter; 89 | } 90 | 91 | int TrackFilter::FilterTrackTriangulationAngle( 92 | const ViewGraph& view_graph, 93 | const std::unordered_map& images, 94 | std::unordered_map& tracks, 95 | double min_angle) { 96 | int counter = 0; 97 | double thres = std::cos(DegToRad(min_angle)); 98 | for (auto& [track_id, track] : tracks) { 99 | std::vector observation_new; 100 | std::vector pts_calc; 101 | pts_calc.reserve(track.observations.size()); 102 | for (auto& [image_id, feature_id] : track.observations) { 103 | const Image& image = images.at(image_id); 104 | Eigen::Vector3d pt_calc = (track.xyz - image.Center()).normalized(); 105 | pts_calc.emplace_back(pt_calc); 106 | } 107 | bool status = false; 108 | for (int i = 0; i < track.observations.size(); i++) { 109 | for (int j = i + 1; j < track.observations.size(); j++) { 110 | if (pts_calc[i].dot(pts_calc[j]) < thres) { 111 | status = true; 112 | break; 113 | } 114 | } 115 | } 116 | 117 | // If the triangulation angle is too small, just remove it 118 | if (!status) { 119 | counter++; 120 | track.observations.clear(); 121 | } 122 | } 123 | LOG(INFO) << "Filtered " << counter << " / " << tracks.size() 124 | << " tracks by too small triangulation angle"; 125 | return counter; 126 | } 127 | 128 | } // namespace glomap -------------------------------------------------------------------------------- /glomap/math/tree.cc: -------------------------------------------------------------------------------- 1 | #include "tree.h" 2 | 3 | // BGL includes 4 | #include 5 | #include 6 | 7 | namespace glomap { 8 | 9 | namespace { 10 | 11 | typedef boost::adjacency_list> 16 | weighted_graph; 17 | typedef boost::property_map::type 18 | weight_map; 19 | typedef boost::graph_traits::edge_descriptor edge_desc; 20 | typedef boost::graph_traits::vertex_descriptor vertex_desc; 21 | 22 | } // namespace 23 | 24 | // Function to perform breadth-first search (BFS) on a graph represented by an 25 | // adjacency list 26 | int BFS(const std::vector>& graph, 27 | int root, 28 | std::vector& parents, 29 | std::vector> banned_edges) { 30 | int num_vertices = graph.size(); 31 | 32 | // Create a vector to store the visited status of each vertex 33 | std::vector visited(num_vertices, false); 34 | 35 | // Create a vector to store the parent vertex for each vertex 36 | parents.clear(); 37 | parents.resize(num_vertices, -1); 38 | parents[root] = root; 39 | 40 | // Create a queue for BFS traversal 41 | std::queue q; 42 | 43 | // Mark the start vertex as visited and enqueue it 44 | visited[root] = true; 45 | q.push(root); 46 | 47 | int counter = 0; 48 | while (!q.empty()) { 49 | int current_vertex = q.front(); 50 | q.pop(); 51 | 52 | // Process the current vertex 53 | // Traverse the adjacent vertices 54 | for (int neighbor : graph[current_vertex]) { 55 | if (std::find(banned_edges.begin(), 56 | banned_edges.end(), 57 | std::make_pair(current_vertex, neighbor)) != 58 | banned_edges.end()) 59 | continue; 60 | if (std::find(banned_edges.begin(), 61 | banned_edges.end(), 62 | std::make_pair(neighbor, current_vertex)) != 63 | banned_edges.end()) 64 | continue; 65 | 66 | if (!visited[neighbor]) { 67 | visited[neighbor] = true; 68 | parents[neighbor] = current_vertex; 69 | q.push(neighbor); 70 | counter++; 71 | } 72 | } 73 | } 74 | 75 | return counter; 76 | } 77 | 78 | image_t MaximumSpanningTree(const ViewGraph& view_graph, 79 | const std::unordered_map& images, 80 | std::unordered_map& parents, 81 | WeightType type) { 82 | std::unordered_map image_id_to_idx; 83 | image_id_to_idx.reserve(images.size()); 84 | std::unordered_map idx_to_image_id; 85 | idx_to_image_id.reserve(images.size()); 86 | for (auto& [image_id, image] : images) { 87 | if (image.is_registered == false) continue; 88 | idx_to_image_id[image_id_to_idx.size()] = image_id; 89 | image_id_to_idx[image_id] = image_id_to_idx.size(); 90 | } 91 | 92 | // establish graph 93 | weighted_graph G(image_id_to_idx.size()); 94 | weight_map weights_boost = boost::get(boost::edge_weight, G); 95 | 96 | edge_desc e; 97 | for (auto& [pair_id, image_pair] : view_graph.image_pairs) { 98 | if (image_pair.is_valid == false) continue; 99 | 100 | const Image& image1 = images.at(image_pair.image_id1); 101 | const Image& image2 = images.at(image_pair.image_id2); 102 | 103 | if (image1.is_registered == false || image2.is_registered == false) { 104 | continue; 105 | } 106 | 107 | int idx1 = image_id_to_idx[image_pair.image_id1]; 108 | int idx2 = image_id_to_idx[image_pair.image_id2]; 109 | 110 | // Set the weight to be negative, then the result would be a maximum 111 | // spanning tree 112 | e = boost::add_edge(idx1, idx2, G).first; 113 | if (type == INLIER_NUM) 114 | weights_boost[e] = -image_pair.inliers.size(); 115 | else if (type == INLIER_RATIO) 116 | weights_boost[e] = -image_pair.weight; 117 | else 118 | weights_boost[e] = -image_pair.inliers.size(); 119 | } 120 | 121 | std::vector 122 | mst; // vector to store MST edges (not a property map!) 123 | boost::kruskal_minimum_spanning_tree(G, std::back_inserter(mst)); 124 | 125 | std::vector> edges_list(image_id_to_idx.size()); 126 | for (const auto& edge : mst) { 127 | auto source = boost::source(edge, G); 128 | auto target = boost::target(edge, G); 129 | edges_list[source].push_back(target); 130 | edges_list[target].push_back(source); 131 | } 132 | 133 | std::vector parents_idx; 134 | BFS(edges_list, 0, parents_idx); 135 | 136 | // change the index back to image id 137 | parents.clear(); 138 | for (int i = 0; i < parents_idx.size(); i++) { 139 | parents[idx_to_image_id[i]] = idx_to_image_id[parents_idx[i]]; 140 | } 141 | 142 | return idx_to_image_id[0]; 143 | } 144 | 145 | }; // namespace glomap -------------------------------------------------------------------------------- /glomap/controllers/global_mapper_test.cc: -------------------------------------------------------------------------------- 1 | #include "glomap/controllers/global_mapper.h" 2 | 3 | #include "glomap/io/colmap_io.h" 4 | #include "glomap/types.h" 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | namespace glomap { 13 | namespace { 14 | 15 | void ExpectEqualReconstructions(const colmap::Reconstruction& gt, 16 | const colmap::Reconstruction& computed, 17 | const double max_rotation_error_deg, 18 | const double max_proj_center_error, 19 | const double num_obs_tolerance) { 20 | EXPECT_EQ(computed.NumCameras(), gt.NumCameras()); 21 | EXPECT_EQ(computed.NumImages(), gt.NumImages()); 22 | EXPECT_EQ(computed.NumRegImages(), gt.NumRegImages()); 23 | EXPECT_GE(computed.ComputeNumObservations(), 24 | (1 - num_obs_tolerance) * gt.ComputeNumObservations()); 25 | 26 | colmap::Sim3d gtFromComputed; 27 | colmap::AlignReconstructionsViaProjCenters(computed, 28 | gt, 29 | /*max_proj_center_error=*/0.1, 30 | >FromComputed); 31 | 32 | const std::vector errors = 33 | colmap::ComputeImageAlignmentError(computed, gt, gtFromComputed); 34 | EXPECT_EQ(errors.size(), gt.NumImages()); 35 | for (const auto& error : errors) { 36 | EXPECT_LT(error.rotation_error_deg, max_rotation_error_deg); 37 | EXPECT_LT(error.proj_center_error, max_proj_center_error); 38 | } 39 | } 40 | 41 | GlobalMapperOptions CreateTestOptions() { 42 | GlobalMapperOptions options; 43 | // Control the verbosity of the global sfm 44 | options.opt_vgcalib.verbose = false; 45 | options.opt_ra.verbose = false; 46 | options.opt_gp.verbose = false; 47 | options.opt_ba.verbose = false; 48 | 49 | // Control the flow of the global sfm 50 | options.skip_view_graph_calibration = false; 51 | options.skip_relative_pose_estimation = false; 52 | options.skip_rotation_averaging = false; 53 | options.skip_track_establishment = false; 54 | options.skip_global_positioning = false; 55 | options.skip_bundle_adjustment = false; 56 | options.skip_retriangulation = false; 57 | return options; 58 | } 59 | 60 | TEST(GlobalMapper, WithoutNoise) { 61 | const std::string database_path = colmap::CreateTestDir() + "/database.db"; 62 | 63 | colmap::Database database(database_path); 64 | colmap::Reconstruction gt_reconstruction; 65 | colmap::SyntheticDatasetOptions synthetic_dataset_options; 66 | synthetic_dataset_options.num_cameras = 2; 67 | synthetic_dataset_options.num_images = 7; 68 | synthetic_dataset_options.num_points3D = 50; 69 | synthetic_dataset_options.point2D_stddev = 0; 70 | colmap::SynthesizeDataset( 71 | synthetic_dataset_options, >_reconstruction, &database); 72 | 73 | ViewGraph view_graph; 74 | std::unordered_map cameras; 75 | std::unordered_map images; 76 | std::unordered_map tracks; 77 | 78 | ConvertDatabaseToGlomap(database, view_graph, cameras, images); 79 | 80 | GlobalMapper global_mapper(CreateTestOptions()); 81 | global_mapper.Solve(database, view_graph, cameras, images, tracks); 82 | 83 | colmap::Reconstruction reconstruction; 84 | ConvertGlomapToColmap(cameras, images, tracks, reconstruction); 85 | 86 | ExpectEqualReconstructions(gt_reconstruction, 87 | reconstruction, 88 | /*max_rotation_error_deg=*/1e-2, 89 | /*max_proj_center_error=*/1e-4, 90 | /*num_obs_tolerance=*/0); 91 | } 92 | 93 | TEST(GlobalMapper, WithNoiseAndOutliers) { 94 | const std::string database_path = colmap::CreateTestDir() + "/database.db"; 95 | 96 | colmap::Database database(database_path); 97 | colmap::Reconstruction gt_reconstruction; 98 | colmap::SyntheticDatasetOptions synthetic_dataset_options; 99 | synthetic_dataset_options.num_cameras = 2; 100 | synthetic_dataset_options.num_images = 7; 101 | synthetic_dataset_options.num_points3D = 100; 102 | synthetic_dataset_options.point2D_stddev = 0.5; 103 | synthetic_dataset_options.inlier_match_ratio = 0.6; 104 | colmap::SynthesizeDataset( 105 | synthetic_dataset_options, >_reconstruction, &database); 106 | 107 | ViewGraph view_graph; 108 | std::unordered_map cameras; 109 | std::unordered_map images; 110 | std::unordered_map tracks; 111 | 112 | ConvertDatabaseToGlomap(database, view_graph, cameras, images); 113 | 114 | GlobalMapper global_mapper(CreateTestOptions()); 115 | global_mapper.Solve(database, view_graph, cameras, images, tracks); 116 | 117 | colmap::Reconstruction reconstruction; 118 | ConvertGlomapToColmap(cameras, images, tracks, reconstruction); 119 | 120 | ExpectEqualReconstructions(gt_reconstruction, 121 | reconstruction, 122 | /*max_rotation_error_deg=*/1e-1, 123 | /*max_proj_center_error=*/1e-1, 124 | /*num_obs_tolerance=*/0.02); 125 | } 126 | 127 | } // namespace 128 | } // namespace glomap 129 | -------------------------------------------------------------------------------- /glomap/controllers/track_retriangulation.cc: -------------------------------------------------------------------------------- 1 | #include "glomap/controllers/track_retriangulation.h" 2 | 3 | #include "glomap/io/colmap_converter.h" 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | namespace glomap { 10 | 11 | bool RetriangulateTracks(const TriangulatorOptions& options, 12 | const colmap::Database& database, 13 | std::unordered_map& cameras, 14 | std::unordered_map& images, 15 | std::unordered_map& tracks) { 16 | // Following code adapted from COLMAP 17 | auto database_cache = 18 | colmap::DatabaseCache::Create(database, 19 | options.min_num_matches, 20 | false, // ignore_watermarks 21 | {} // reconstruct all possible images 22 | ); 23 | 24 | // Check whether the image is in the database cache. If not, set the image 25 | // as not registered to avoid memory error. 26 | std::vector image_ids_notconnected; 27 | for (auto& image : images) { 28 | if (!database_cache->ExistsImage(image.first) && 29 | image.second.is_registered) { 30 | image.second.is_registered = false; 31 | image_ids_notconnected.push_back(image.first); 32 | } 33 | } 34 | 35 | // Convert the glomap data structures to colmap data structures 36 | std::shared_ptr reconstruction_ptr = 37 | std::make_shared(); 38 | ConvertGlomapToColmap(cameras, 39 | images, 40 | std::unordered_map(), 41 | *reconstruction_ptr); 42 | 43 | colmap::IncrementalMapperOptions options_colmap; 44 | options_colmap.triangulation.complete_max_reproj_error = 45 | options.tri_complete_max_reproj_error; 46 | options_colmap.triangulation.merge_max_reproj_error = 47 | options.tri_merge_max_reproj_error; 48 | options_colmap.triangulation.min_angle = options.tri_min_angle; 49 | 50 | reconstruction_ptr->DeleteAllPoints2DAndPoints3D(); 51 | reconstruction_ptr->TranscribeImageIdsToDatabase(database); 52 | 53 | colmap::IncrementalMapper mapper(database_cache); 54 | mapper.BeginReconstruction(reconstruction_ptr); 55 | 56 | // Triangulate all images. 57 | const auto tri_options = options_colmap.Triangulation(); 58 | const auto mapper_options = options_colmap.Mapper(); 59 | 60 | const std::vector& reg_image_ids = reconstruction_ptr->RegImageIds(); 61 | 62 | for (size_t i = 0; i < reg_image_ids.size(); ++i) { 63 | std::cout << "\r Triangulating image " << i + 1 << " / " 64 | << reg_image_ids.size() << std::flush; 65 | 66 | const image_t image_id = reg_image_ids[i]; 67 | const auto& image = reconstruction_ptr->Image(image_id); 68 | 69 | int num_tris = mapper.TriangulateImage(tri_options, image_id); 70 | } 71 | std::cout << std::endl; 72 | 73 | // Merge and complete tracks. 74 | mapper.CompleteAndMergeTracks(tri_options); 75 | 76 | auto ba_options = options_colmap.GlobalBundleAdjustment(); 77 | ba_options.refine_focal_length = false; 78 | ba_options.refine_principal_point = false; 79 | ba_options.refine_extra_params = false; 80 | ba_options.refine_extrinsics = false; 81 | 82 | // Configure bundle adjustment. 83 | colmap::BundleAdjustmentConfig ba_config; 84 | for (const image_t image_id : reg_image_ids) { 85 | ba_config.AddImage(image_id); 86 | } 87 | 88 | colmap::ObservationManager observation_manager(*reconstruction_ptr); 89 | 90 | for (int i = 0; i < options_colmap.ba_global_max_refinements; ++i) { 91 | std::cout << "\r Global bundle adjustment iteration " << i + 1 << " / " 92 | << options_colmap.ba_global_max_refinements << std::flush; 93 | // Avoid degeneracies in bundle adjustment. 94 | observation_manager.FilterObservationsWithNegativeDepth(); 95 | 96 | const size_t num_observations = 97 | reconstruction_ptr->ComputeNumObservations(); 98 | 99 | // PrintHeading1("Bundle adjustment"); 100 | colmap::BundleAdjuster bundle_adjuster(ba_options, ba_config); 101 | // THROW_CHECK(bundle_adjuster.Solve(reconstruction.get())); 102 | if (!bundle_adjuster.Solve(reconstruction_ptr.get())) { 103 | return false; 104 | } 105 | 106 | size_t num_changed_observations = 0; 107 | num_changed_observations += mapper.CompleteAndMergeTracks(tri_options); 108 | num_changed_observations += mapper.FilterPoints(mapper_options); 109 | const double changed = 110 | static_cast(num_changed_observations) / num_observations; 111 | if (changed < options_colmap.ba_global_max_refinement_change) { 112 | break; 113 | } 114 | } 115 | std::cout << std::endl; 116 | 117 | // Add the removed images to the reconstruction 118 | for (const auto& image_id : image_ids_notconnected) { 119 | images[image_id].is_registered = true; 120 | colmap::Image image_colmap; 121 | ConvertGlomapToColmapImage(images[image_id], image_colmap, true); 122 | reconstruction_ptr->AddImage(std::move(image_colmap)); 123 | } 124 | 125 | // Convert the colmap data structures back to glomap data structures 126 | ConvertColmapToGlomap(*reconstruction_ptr, cameras, images, tracks); 127 | 128 | return true; 129 | } 130 | 131 | } // namespace glomap -------------------------------------------------------------------------------- /glomap/exe/global_mapper.cc: -------------------------------------------------------------------------------- 1 | #include "glomap/controllers/global_mapper.h" 2 | 3 | #include "glomap/controllers/option_manager.h" 4 | #include "colmap/sensor/bitmap.h" 5 | #include "glomap/io/colmap_io.h" 6 | #include "glomap/io/recording.h" 7 | #include "glomap/types.h" 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | namespace glomap { 15 | 16 | // ------------------------------------- 17 | // Mappers starting from COLMAP database 18 | // ------------------------------------- 19 | int RunMapper(int argc, char** argv) { 20 | std::string database_path; 21 | std::string output_path; 22 | 23 | std::string image_path = ""; 24 | std::string constraint_type = "ONLY_POINTS"; 25 | std::string output_format = "bin"; 26 | 27 | OptionManager options; 28 | options.AddRequiredOption("database_path", &database_path); 29 | options.AddRequiredOption("output_path", &output_path); 30 | options.AddDefaultOption("image_path", &image_path); 31 | options.AddDefaultOption("constraint_type", 32 | &constraint_type, 33 | "{ONLY_POINTS, ONLY_CAMERAS, " 34 | "POINTS_AND_CAMERAS_BALANCED, POINTS_AND_CAMERAS}"); 35 | options.AddDefaultOption("output_format", &output_format, "{bin, txt}"); 36 | options.AddGlobalMapperFullOptions(); 37 | 38 | options.Parse(argc, argv); 39 | 40 | if (!colmap::ExistsFile(database_path)) { 41 | LOG(ERROR) << "`database_path` is not a file"; 42 | return EXIT_FAILURE; 43 | } 44 | 45 | if (constraint_type == "ONLY_POINTS") { 46 | options.mapper->opt_gp.constraint_type = 47 | GlobalPositionerOptions::ONLY_POINTS; 48 | } else if (constraint_type == "ONLY_CAMERAS") { 49 | options.mapper->opt_gp.constraint_type = 50 | GlobalPositionerOptions::ONLY_CAMERAS; 51 | } else if (constraint_type == "POINTS_AND_CAMERAS_BALANCED") { 52 | options.mapper->opt_gp.constraint_type = 53 | GlobalPositionerOptions::POINTS_AND_CAMERAS_BALANCED; 54 | } else if (constraint_type == "POINTS_AND_CAMERAS") { 55 | options.mapper->opt_gp.constraint_type = 56 | GlobalPositionerOptions::POINTS_AND_CAMERAS; 57 | } else { 58 | LOG(ERROR) << "Invalid constriant type"; 59 | return EXIT_FAILURE; 60 | } 61 | 62 | // Check whether output_format is valid 63 | if (output_format != "bin" && output_format != "txt") { 64 | LOG(ERROR) << "Invalid output format"; 65 | return EXIT_FAILURE; 66 | } 67 | 68 | init_recording(); 69 | image_path_global = image_path; 70 | 71 | // Load the database 72 | ViewGraph view_graph; 73 | std::unordered_map cameras; 74 | std::unordered_map images; 75 | std::unordered_map tracks; 76 | 77 | const colmap::Database database(database_path); 78 | ConvertDatabaseToGlomap(database, view_graph, cameras, images); 79 | 80 | log_images(rr_rec, images, image_path); 81 | 82 | GlobalMapper global_mapper(*options.mapper); 83 | 84 | // Main solver 85 | LOG(INFO) << "Loaded database"; 86 | colmap::Timer run_timer; 87 | run_timer.Start(); 88 | 89 | global_mapper.Solve(database, view_graph, cameras, images, tracks); 90 | run_timer.Pause(); 91 | 92 | LOG(INFO) << "Reconstruction done in " << run_timer.ElapsedSeconds() 93 | << " seconds"; 94 | 95 | WriteGlomapReconstruction( 96 | output_path, cameras, images, tracks, output_format, image_path); 97 | LOG(INFO) << "Export to COLMAP reconstruction done"; 98 | 99 | return EXIT_SUCCESS; 100 | } 101 | 102 | // ------------------------------------- 103 | // Mappers starting from COLMAP reconstruction 104 | // ------------------------------------- 105 | int RunMapperResume(int argc, char** argv) { 106 | std::string input_path; 107 | std::string output_path; 108 | std::string image_path = ""; 109 | std::string output_format = "bin"; 110 | 111 | OptionManager options; 112 | options.AddRequiredOption("input_path", &input_path); 113 | options.AddRequiredOption("output_path", &output_path); 114 | options.AddDefaultOption("image_path", &image_path); 115 | options.AddDefaultOption("output_format", &output_format, "{bin, txt}"); 116 | options.AddGlobalMapperResumeFullOptions(); 117 | 118 | options.Parse(argc, argv); 119 | 120 | if (!colmap::ExistsDir(input_path)) { 121 | LOG(ERROR) << "`input_path` is not a directory"; 122 | return EXIT_FAILURE; 123 | } 124 | 125 | // Check whether output_format is valid 126 | if (output_format != "bin" && output_format != "txt") { 127 | LOG(ERROR) << "Invalid output format"; 128 | return EXIT_FAILURE; 129 | } 130 | 131 | // Load the reconstruction 132 | ViewGraph view_graph; // dummy variable 133 | colmap::Database database; // dummy variable 134 | 135 | std::unordered_map cameras; 136 | std::unordered_map images; 137 | std::unordered_map tracks; 138 | colmap::Reconstruction reconstruction; 139 | 140 | init_recording(); 141 | image_path_global = image_path; 142 | 143 | reconstruction.Read(input_path); 144 | ConvertColmapToGlomap(reconstruction, cameras, images, tracks); 145 | 146 | log_images(rr_rec, images, image_path); 147 | 148 | GlobalMapper global_mapper(*options.mapper); 149 | 150 | // Main solver 151 | colmap::Timer run_timer; 152 | run_timer.Start(); 153 | global_mapper.Solve(database, view_graph, cameras, images, tracks); 154 | run_timer.Pause(); 155 | 156 | LOG(INFO) << "Reconstruction done in " << run_timer.ElapsedSeconds() 157 | << " seconds"; 158 | 159 | WriteGlomapReconstruction( 160 | output_path, cameras, images, tracks, output_format, image_path); 161 | LOG(INFO) << "Export to COLMAP reconstruction done"; 162 | 163 | return EXIT_SUCCESS; 164 | } 165 | 166 | } // namespace glomap -------------------------------------------------------------------------------- /glomap/controllers/option_manager.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | namespace glomap { 11 | 12 | struct GlobalMapperOptions; 13 | struct ViewGraphCalibratorOptions; 14 | struct RelativePoseEstimationOptions; 15 | struct RotationEstimatorOptions; 16 | struct TrackEstablishmentOptions; 17 | struct GlobalPositionerOptions; 18 | struct BundleAdjusterOptions; 19 | struct TriangulatorOptions; 20 | struct InlierThresholdOptions; 21 | 22 | class OptionManager { 23 | public: 24 | explicit OptionManager(bool add_project_options = true); 25 | void AddAllOptions(); 26 | void AddDatabaseOptions(); 27 | void AddImageOptions(); 28 | void AddGlobalMapperOptions(); 29 | void AddGlobalMapperFullOptions(); 30 | void AddGlobalMapperResumeOptions(); 31 | void AddGlobalMapperResumeFullOptions(); 32 | void AddViewGraphCalibrationOptions(); 33 | void AddRelativePoseEstimationOptions(); 34 | void AddRotationEstimatorOptions(); 35 | void AddTrackEstablishmentOptions(); 36 | void AddGlobalPositionerOptions(); 37 | void AddBundleAdjusterOptions(); 38 | void AddTriangulatorOptions(); 39 | void AddInlierThresholdOptions(); 40 | 41 | template 42 | void AddRequiredOption(const std::string& name, 43 | T* option, 44 | const std::string& help_text = ""); 45 | template 46 | void AddDefaultOption(const std::string& name, 47 | T* option, 48 | const std::string& help_text = ""); 49 | 50 | void Reset(); 51 | void ResetOptions(bool reset_paths); 52 | 53 | void Parse(int argc, char** argv); 54 | 55 | std::shared_ptr database_path; 56 | std::shared_ptr image_path; 57 | 58 | std::shared_ptr mapper; 59 | 60 | private: 61 | template 62 | void AddAndRegisterRequiredOption(const std::string& name, 63 | T* option, 64 | const std::string& help_text = ""); 65 | template 66 | void AddAndRegisterDefaultOption(const std::string& name, 67 | T* option, 68 | const std::string& help_text = ""); 69 | 70 | template 71 | void RegisterOption(const std::string& name, const T* option); 72 | 73 | std::shared_ptr desc_; 74 | 75 | std::vector> options_bool_; 76 | std::vector> options_int_; 77 | std::vector> options_double_; 78 | std::vector> options_string_; 79 | 80 | bool added_database_options_ = false; 81 | bool added_image_options_ = false; 82 | bool added_mapper_options_ = false; 83 | bool added_view_graph_calibration_options_ = false; 84 | bool added_relative_pose_options_ = false; 85 | bool added_rotation_averaging_options_ = false; 86 | bool added_track_establishment_options_ = false; 87 | bool added_global_positioning_options_ = false; 88 | bool added_bundle_adjustment_options_ = false; 89 | bool added_triangulation_options_ = false; 90 | bool added_inliers_options_ = false; 91 | }; 92 | 93 | template 94 | void OptionManager::AddRequiredOption(const std::string& name, 95 | T* option, 96 | const std::string& help_text) { 97 | desc_->add_options()(name.c_str(), 98 | boost::program_options::value(option)->required(), 99 | help_text.c_str()); 100 | } 101 | 102 | template 103 | void OptionManager::AddDefaultOption(const std::string& name, 104 | T* option, 105 | const std::string& help_text) { 106 | desc_->add_options()( 107 | name.c_str(), 108 | boost::program_options::value(option)->default_value(*option), 109 | help_text.c_str()); 110 | } 111 | 112 | template 113 | void OptionManager::AddAndRegisterRequiredOption(const std::string& name, 114 | T* option, 115 | const std::string& help_text) { 116 | desc_->add_options()(name.c_str(), 117 | boost::program_options::value(option)->required(), 118 | help_text.c_str()); 119 | RegisterOption(name, option); 120 | } 121 | 122 | template 123 | void OptionManager::AddAndRegisterDefaultOption(const std::string& name, 124 | T* option, 125 | const std::string& help_text) { 126 | desc_->add_options()( 127 | name.c_str(), 128 | boost::program_options::value(option)->default_value(*option), 129 | help_text.c_str()); 130 | RegisterOption(name, option); 131 | } 132 | 133 | template 134 | void OptionManager::RegisterOption(const std::string& name, const T* option) { 135 | if (std::is_same::value) { 136 | options_bool_.emplace_back(name, reinterpret_cast(option)); 137 | } else if (std::is_same::value) { 138 | options_int_.emplace_back(name, reinterpret_cast(option)); 139 | } else if (std::is_same::value) { 140 | options_double_.emplace_back(name, reinterpret_cast(option)); 141 | } else if (std::is_same::value) { 142 | options_string_.emplace_back(name, 143 | reinterpret_cast(option)); 144 | } else { 145 | LOG(ERROR) << "Unsupported option type: " << name; 146 | } 147 | } 148 | 149 | } // namespace glomap 150 | -------------------------------------------------------------------------------- /glomap/estimators/global_rotation_averaging.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "glomap/math/l1_solver.h" 4 | #include "glomap/scene/types_sfm.h" 5 | #include "glomap/types.h" 6 | 7 | #include 8 | #include 9 | 10 | // Code is adapted from Theia's RobustRotationEstimator 11 | // (http://www.theia-sfm.org/). For gravity aligned rotation averaging, refere 12 | // to the paper "Gravity Aligned Rotation Averaging" 13 | namespace glomap { 14 | 15 | // The struct to store the temporary information for each image pair 16 | struct ImagePairTempInfo { 17 | // The index of relative pose in the residual vector 18 | image_pair_t index = -1; 19 | 20 | // Whether the relative rotation is gravity aligned 21 | double has_gravity = false; 22 | 23 | // The relative rotation between the two images (x, z component) 24 | double xz_error = 0; 25 | 26 | // R_rel is gravity aligned if gravity prior is available, otherwise it is the 27 | // relative rotation between the two images 28 | Eigen::Matrix3d R_rel = Eigen::Matrix3d::Identity(); 29 | 30 | // angle_rel is the converted angle if gravity prior is available for both 31 | // images 32 | double angle_rel = 0; 33 | }; 34 | 35 | struct RotationEstimatorOptions { 36 | // Maximum number of times to run L1 minimization. 37 | int max_num_l1_iterations = 5; 38 | 39 | // Average step size threshold to terminate the L1 minimization 40 | double l1_step_convergence_threshold = 0.001; 41 | 42 | // The number of iterative reweighted least squares iterations to perform. 43 | int max_num_irls_iterations = 100; 44 | 45 | // Average step size threshold to termininate the IRLS minimization 46 | double irls_step_convergence_threshold = 0.001; 47 | 48 | Eigen::Vector3d axis = Eigen::Vector3d(0, 1, 0); 49 | 50 | // This is the point where the Huber-like cost function switches from L1 to 51 | // L2. 52 | double irls_loss_parameter_sigma = 5.0; // in degree 53 | 54 | enum WeightType { 55 | // For Geman-McClure weight, refer to the paper "Efficient and robust 56 | // large-scale rotation averaging" (Chatterjee et. al, 2013) 57 | GEMAN_MCCLURE, 58 | // For Half Norm, refer to the paper "Robust Relative Rotation Averaging" 59 | // (Chatterjee et. al, 2017) 60 | HALF_NORM, 61 | } weight_type = GEMAN_MCCLURE; 62 | 63 | // Flg to use maximum spanning tree for initialization 64 | bool skip_initialization = false; 65 | 66 | // TODO: Implement the weighted version for rotation averaging 67 | bool use_weight = false; 68 | 69 | // Flag to use gravity for rotation averaging 70 | bool use_gravity = false; 71 | 72 | // Flag whether report the verbose information 73 | bool verbose = false; 74 | }; 75 | 76 | // TODO: Implement the stratified camera rotation estimation 77 | // TODO: Implement the HALF_NORM loss for IRLS 78 | // TODO: Implement the weighted version for rotation averaging 79 | // TODO: Implement the gravity as prior for rotation averaging 80 | class RotationEstimator { 81 | public: 82 | explicit RotationEstimator(const RotationEstimatorOptions& options) 83 | : options_(options) {} 84 | 85 | // Estimates the global orientations of all views based on an initial 86 | // guess. Returns true on successful estimation and false otherwise. 87 | bool EstimateRotations(const ViewGraph& view_graph, 88 | std::unordered_map& images); 89 | 90 | protected: 91 | // Initialize the rotation from the maximum spanning tree 92 | // Number of inliers serve as weights 93 | void InitializeFromMaximumSpanningTree( 94 | const ViewGraph& view_graph, std::unordered_map& images); 95 | 96 | // Sets up the sparse linear system such that dR_ij = dR_j - dR_i. This is the 97 | // first-order approximation of the angle-axis rotations. This should only be 98 | // called once. 99 | void SetupLinearSystem(const ViewGraph& view_graph, 100 | std::unordered_map& images); 101 | 102 | // Performs the L1 robust loss minimization. 103 | bool SolveL1Regression(const ViewGraph& view_graph, 104 | std::unordered_map& images); 105 | 106 | // Performs the iteratively reweighted least squares. 107 | bool SolveIRLS(const ViewGraph& view_graph, 108 | std::unordered_map& images); 109 | 110 | // Updates the global rotations based on the current rotation change. 111 | void UpdateGlobalRotations(const ViewGraph& view_graph, 112 | std::unordered_map& images); 113 | 114 | // Computes the relative rotation (tangent space) residuals based on the 115 | // current global orientation estimates. 116 | void ComputeResiduals(const ViewGraph& view_graph, 117 | std::unordered_map& images); 118 | 119 | // Computes the average size of the most recent step of the algorithm. 120 | // The is the average over all non-fixed global_orientations_ of their 121 | // rotation magnitudes. 122 | double ComputeAverageStepSize( 123 | const std::unordered_map& images); 124 | 125 | // Data 126 | // Options for the solver. 127 | const RotationEstimatorOptions& options_; 128 | 129 | // The sparse matrix used to maintain the linear system. This is matrix A in 130 | // Ax = b. 131 | Eigen::SparseMatrix sparse_matrix_; 132 | 133 | // x in the linear system Ax = b. 134 | Eigen::VectorXd tangent_space_step_; 135 | 136 | // b in the linear system Ax = b. 137 | Eigen::VectorXd tangent_space_residual_; 138 | 139 | Eigen::VectorXd rotation_estimated_; 140 | 141 | // Varaibles for intermidiate results 142 | std::unordered_map image_id_to_idx_; 143 | std::unordered_map rel_temp_info_; 144 | 145 | // The fixed camera id. This is used to remove the ambiguity of the linear 146 | image_t fixed_camera_id_ = -1; 147 | 148 | // The fixed camera rotation (if with initialization, it would not be identity 149 | // matrix) 150 | Eigen::Vector3d fixed_camera_rotation_; 151 | }; 152 | 153 | } // namespace glomap 154 | -------------------------------------------------------------------------------- /glomap/estimators/gravity_refinement.cc: -------------------------------------------------------------------------------- 1 | #include "gravity_refinement.h" 2 | 3 | #include "glomap/estimators/cost_function.h" 4 | #include "glomap/math/gravity.h" 5 | 6 | #include 7 | 8 | namespace glomap { 9 | void GravityRefiner::RefineGravity(const ViewGraph& view_graph, 10 | std::unordered_map& images) { 11 | const std::unordered_map& image_pairs = 12 | view_graph.image_pairs; 13 | const std::unordered_map>& 14 | adjacency_list = view_graph.GetAdjacencyList(); 15 | 16 | // Identify the images that are error prone 17 | int counter_rect = 0; 18 | std::unordered_set error_prone_images; 19 | IdentifyErrorProneGravity(view_graph, images, error_prone_images); 20 | 21 | if (error_prone_images.empty()) { 22 | LOG(INFO) << "No error prone images found" << std::endl; 23 | return; 24 | } 25 | 26 | int counter_progress = 0; 27 | // Iterate through the error prone images 28 | for (auto image_id : error_prone_images) { 29 | if ((counter_progress + 1) % 10 == 0 || 30 | counter_progress == error_prone_images.size() - 1) { 31 | std::cout << "\r Refining image " << counter_progress + 1 << " / " 32 | << error_prone_images.size() << std::flush; 33 | } 34 | counter_progress++; 35 | const std::unordered_set& neighbors = adjacency_list.at(image_id); 36 | std::vector gravities; 37 | gravities.reserve(neighbors.size()); 38 | 39 | ceres::Problem::Options problem_options; 40 | problem_options.loss_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; 41 | ceres::Problem problem(problem_options); 42 | int counter = 0; 43 | Eigen::Vector3d gravity = images[image_id].gravity_info.GetGravity(); 44 | for (const auto& neighbor : neighbors) { 45 | image_pair_t pair_id = ImagePair::ImagePairToPairId(image_id, neighbor); 46 | 47 | image_t image_id1 = image_pairs.at(pair_id).image_id1; 48 | image_t image_id2 = image_pairs.at(pair_id).image_id2; 49 | if (images.at(image_id1).gravity_info.has_gravity == false || 50 | images.at(image_id2).gravity_info.has_gravity == false) 51 | continue; 52 | 53 | if (image_id1 == image_id) { 54 | gravities.emplace_back((image_pairs.at(pair_id) 55 | .cam2_from_cam1.rotation.toRotationMatrix() 56 | .transpose() * 57 | images[image_id2].gravity_info.GetRAlign()) 58 | .col(1)); 59 | } else { 60 | gravities.emplace_back( 61 | (image_pairs.at(pair_id) 62 | .cam2_from_cam1.rotation.toRotationMatrix() * 63 | images[image_id1].gravity_info.GetRAlign()) 64 | .col(1)); 65 | } 66 | 67 | ceres::CostFunction* coor_cost = 68 | GravError::CreateCost(gravities[counter]); 69 | problem.AddResidualBlock( 70 | coor_cost, options_.loss_function.get(), gravity.data()); 71 | counter++; 72 | } 73 | 74 | if (gravities.size() < options_.min_num_neighbors) continue; 75 | 76 | // Then, run refinment 77 | colmap::SetSphereManifold<3>(&problem, gravity.data()); 78 | ceres::Solver::Summary summary_solver; 79 | ceres::Solve(options_.solver_options, &problem, &summary_solver); 80 | 81 | // Check the error with respect to the neighbors 82 | int counter_outlier = 0; 83 | for (int i = 0; i < gravities.size(); i++) { 84 | double error = RadToDeg( 85 | std::acos(std::max(std::min(gravities[i].dot(gravity), 1.), -1.))); 86 | if (error > options_.max_gravity_error * 2) counter_outlier++; 87 | } 88 | // If the refined gravity now consistent with more images, then accept it 89 | if (double(counter_outlier) / double(gravities.size()) < 90 | options_.max_outlier_ratio) { 91 | counter_rect++; 92 | images[image_id].gravity_info.SetGravity(gravity); 93 | } 94 | } 95 | std::cout << std::endl; 96 | LOG(INFO) << "Number of rectified images: " << counter_rect << " / " 97 | << error_prone_images.size() << std::endl; 98 | } 99 | 100 | void GravityRefiner::IdentifyErrorProneGravity( 101 | const ViewGraph& view_graph, 102 | const std::unordered_map& images, 103 | std::unordered_set& error_prone_images) { 104 | error_prone_images.clear(); 105 | 106 | // image_id: (mistake, total) 107 | std::unordered_map> image_counter; 108 | // Set the counter of all images to 0 109 | for (const auto& [image_id, image] : images) { 110 | image_counter[image_id] = std::make_pair(0, 0); 111 | } 112 | 113 | for (const auto& [pair_id, image_pair] : view_graph.image_pairs) { 114 | if (!image_pair.is_valid) continue; 115 | const auto& image1 = images.at(image_pair.image_id1); 116 | const auto& image2 = images.at(image_pair.image_id2); 117 | if (image1.gravity_info.has_gravity && image2.gravity_info.has_gravity) { 118 | // Calculate the gravity aligned relative rotation 119 | Eigen::Matrix3d R_rel = 120 | image2.gravity_info.GetRAlign().transpose() * 121 | image_pair.cam2_from_cam1.rotation.toRotationMatrix() * 122 | image1.gravity_info.GetRAlign(); 123 | // Convert it to the closest upright rotation 124 | Eigen::Matrix3d R_rel_up = AngleToRotUp(RotUpToAngle(R_rel)); 125 | 126 | double angle = CalcAngle(R_rel, R_rel_up); 127 | 128 | // increment the total count 129 | image_counter[image_pair.image_id1].second++; 130 | image_counter[image_pair.image_id2].second++; 131 | 132 | // increment the mistake count 133 | if (angle > options_.max_gravity_error) { 134 | image_counter[image_pair.image_id1].first++; 135 | image_counter[image_pair.image_id2].first++; 136 | } 137 | } 138 | } 139 | 140 | const std::unordered_map>& 141 | adjacency_list = view_graph.GetAdjacencyList(); 142 | 143 | // Filter the images with too many mistakes 144 | for (auto& [image_id, counter] : image_counter) { 145 | if (images.at(image_id).gravity_info.has_gravity == false) continue; 146 | if (counter.second < options_.min_num_neighbors) continue; 147 | if (double(counter.first) / double(counter.second) >= 148 | options_.max_outlier_ratio) { 149 | error_prone_images.insert(image_id); 150 | } 151 | } 152 | LOG(INFO) << "Number of error prone images: " << error_prone_images.size() 153 | << std::endl; 154 | } 155 | } // namespace glomap 156 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # GLOMAP: Global Structure-from-Motion Revisited 2 | 3 | [Project page](https://lpanaf.github.io/eccv24_glomap/) | [Paper](https://arxiv.org/pdf/2407.20219) 4 | --- 5 | 6 | ## About 7 | 8 | GLOMAP is a general purpose global structure-from-motion pipeline for 9 | image-based reconstruction. GLOMAP requires a COLMAP database as input and 10 | outputs a COLMAP sparse reconstruction. As compared to COLMAP, this project 11 | provides a much more efficient and scalable reconstruction process, typically 12 | 1-2 orders of magnitude faster, with on-par or superior reconstruction quality. 13 | 14 | If you use this project for your research, please cite 15 | ``` 16 | @inproceedings{pan2024glomap, 17 | author={Pan, Linfei and Barath, Daniel and Pollefeys, Marc and Sch\"{o}nberger, Johannes Lutz}, 18 | title={{Global Structure-from-Motion Revisited}}, 19 | booktitle={European Conference on Computer Vision (ECCV)}, 20 | year={2024}, 21 | } 22 | ``` 23 | 24 | ## Using pixi 25 | Easiest way to run this example is using pixi, you can easily install it using curl explained here 26 | https://pixi.sh/latest/#installation 27 | 28 | then 29 | 30 | ``` 31 | pixi run example 32 | ``` 33 | 34 | 35 | ## End-to-End with Docker. 36 | 37 | Download one of the [official datasets](https://colmap.github.io/datasets.html) unzip it. 38 | For example if we download the `south-building.zip` 39 | ```bash 40 | mkdir -p data 41 | unzip south-building.zip -d data/ 42 | ``` 43 | 44 | Build and enter the docker container by running the following scripts: 45 | ```bash 46 | cd docker 47 | ./build.sh # Builds the docker image 48 | ./start.sh # Starts the docker container 49 | ./attach.sh # Enter the container 50 | 51 | ``` 52 | 53 | When we're inside the container we can build `glomap` and install it. 54 | ```bash 55 | mkdir -p build 56 | cd build 57 | cmake .. -GNinja 58 | ninja 59 | ninja install 60 | ``` 61 | 62 | To run it we must first extract the features and match them with `colmap`. 63 | ```bash 64 | cd /ws 65 | colmap feature_extractor \ 66 | --image_path data/south-building/images \ 67 | --database_path data/south-building/database.db 68 | 69 | colmap sequential_matcher \ 70 | --database_path data/south-building/database.db 71 | ``` 72 | 73 | Now we're ready to run and visualize `glomap`: 74 | ```bash 75 | glomap mapper \ 76 | --database_path data/south-building/database.db \ 77 | --image_path data/south-building/images \ 78 | --output_path data/south-building/sparse 79 | ``` 80 | To cleary see what's happening it's recommended you open the `blueprint.rbl` file in this directory. 81 | If you want a cooler visualization you can resume a mapping, you do this by first running the steps above and then running the following command: 82 | ```bash 83 | glomap mapper_resume \ 84 | --image_path data/south-building/images \ 85 | --input_path data/south-building/sparse/0 \ 86 | --output_path data/south-building/sparse 87 | ``` 88 | 89 | If you're experiencing performance issues with rerun you might have to run the viewer outside of the docker container. 90 | 91 | ### From video 92 | To run `glomap` on a video you will first have to split it into multiple frames and put them in a directory. To easily do this you can use the the script in `scripts/extract_frames.py` like this: 93 | ```bash 94 | # Split 'colosseum.mp4' into frames at 1 frame per second and puts the images in colosseum/images. 95 | scripts/extract_frames.py -v colosseum.mp4 -o colosseum/images --desired-fps 1 96 | 97 | # Then you can extract the features like normally. 98 | colmap feature_extractor \ 99 | --image_path colosseum/images \ 100 | --database_path colosseum/database.db 101 | colmap sequential_matcher \ 102 | --database_path colosseum/database.db 103 | 104 | glomap mapper \ 105 | --database_path colosseum/database.db \ 106 | --image_path colosseum/images \ 107 | --output_path colosseum/sparse 108 | ``` 109 | 110 | ### Notes 111 | 112 | - For larger scale datasets, it is recommended to use `sequential_matcher` or 113 | `vocab_tree_matcher` from `COLMAP`. 114 | ```shell 115 | colmap sequential_matcher --database_path DATABASE_PATH 116 | colmap vocab_tree_matcher --database_path DATABASE_PATH --VocabTreeMatching.vocab_tree_path VOCAB_TREE_PATH 117 | ``` 118 | - Alternatively, one can use 119 | [hloc](https://github.com/cvg/Hierarchical-Localization/) for image retrieval 120 | and matching with learning-based descriptors. 121 | 122 | 123 | 124 | ## Acknowledgement 125 | 126 | We are highly inspired by COLMAP, PoseLib, Theia. Please consider also citing 127 | them, if using GLOMAP in your work. 128 | 129 | ## Support 130 | 131 | Please, use GitHub Discussions at https://github.com/colmap/glomap/discussions 132 | for questions and the GitHub issue tracker at https://github.com/colmap/glomap 133 | for bug reports, feature requests/additions, etc. 134 | 135 | ## Contribution 136 | 137 | Contributions (bug reports, bug fixes, improvements, etc.) are very welcome and 138 | should be submitted in the form of new issues and/or pull requests on GitHub. 139 | 140 | ## License 141 | 142 | ``` 143 | Copyright (c) 2024, ETH Zurich. 144 | All rights reserved. 145 | 146 | Redistribution and use in source and binary forms, with or without 147 | modification, are permitted provided that the following conditions are met: 148 | 149 | * Redistributions of source code must retain the above copyright 150 | notice, this list of conditions and the following disclaimer. 151 | 152 | * Redistributions in binary form must reproduce the above copyright 153 | notice, this list of conditions and the following disclaimer in the 154 | documentation and/or other materials provided with the distribution. 155 | 156 | * Neither the name of ETH Zurich nor the names of its contributors may 157 | be used to endorse or promote products derived from this software 158 | without specific prior written permission. 159 | 160 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 161 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 162 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 163 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE 164 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 165 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 166 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 167 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 168 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 169 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 170 | POSSIBILITY OF SUCH DAMAGE. 171 | ``` 172 | -------------------------------------------------------------------------------- /glomap/estimators/view_graph_calibration.cc: -------------------------------------------------------------------------------- 1 | #include "glomap/estimators/view_graph_calibration.h" 2 | 3 | #include "glomap/estimators/cost_function.h" 4 | #include "glomap/math/two_view_geometry.h" 5 | 6 | #include 7 | 8 | #include 9 | 10 | namespace glomap { 11 | 12 | bool ViewGraphCalibrator::Solve(ViewGraph& view_graph, 13 | std::unordered_map& cameras, 14 | std::unordered_map& images) { 15 | // Reset the problem 16 | LOG(INFO) << "Start ViewGraphCalibrator"; 17 | 18 | Reset(cameras); 19 | 20 | // Set the solver options. 21 | if (cameras.size() < 50) 22 | options_.solver_options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY; 23 | else 24 | options_.solver_options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY; 25 | 26 | // Add the image pairs into the problem 27 | AddImagePairsToProblem(view_graph, cameras, images); 28 | 29 | // Set the cameras to be constant if they have prior intrinsics 30 | const size_t num_cameras = ParameterizeCameras(cameras); 31 | 32 | if (num_cameras == 0) { 33 | LOG(INFO) << "No cameras to optimize"; 34 | return true; 35 | } 36 | 37 | // Solve the problem 38 | ceres::Solver::Summary summary; 39 | options_.solver_options.minimizer_progress_to_stdout = options_.verbose; 40 | ceres::Solve(options_.solver_options, problem_.get(), &summary); 41 | 42 | // Print the summary only if verbose 43 | if (options_.verbose) { 44 | LOG(INFO) << summary.FullReport(); 45 | } 46 | 47 | // Convert the results back to the camera 48 | CopyBackResults(cameras); 49 | FilterImagePairs(view_graph); 50 | 51 | return summary.IsSolutionUsable(); 52 | } 53 | 54 | void ViewGraphCalibrator::Reset( 55 | const std::unordered_map& cameras) { 56 | // Initialize the problem 57 | focals_.clear(); 58 | focals_.reserve(cameras.size()); 59 | for (const auto& [camera_id, camera] : cameras) { 60 | focals_[camera_id] = camera.Focal(); 61 | } 62 | 63 | // Set up the problem 64 | ceres::Problem::Options problem_options; 65 | problem_options.loss_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; 66 | problem_ = std::make_unique(problem_options); 67 | options_.loss_function = 68 | std::make_shared(options_.thres_loss_function); 69 | } 70 | 71 | void ViewGraphCalibrator::AddImagePairsToProblem( 72 | const ViewGraph& view_graph, 73 | const std::unordered_map& cameras, 74 | const std::unordered_map& images) { 75 | for (auto& [image_pair_id, image_pair] : view_graph.image_pairs) { 76 | if (image_pair.config != colmap::TwoViewGeometry::CALIBRATED && 77 | image_pair.config != colmap::TwoViewGeometry::UNCALIBRATED) 78 | continue; 79 | if (image_pair.is_valid == false) continue; 80 | 81 | AddImagePair(image_pair, cameras, images); 82 | } 83 | } 84 | 85 | void ViewGraphCalibrator::AddImagePair( 86 | const ImagePair& image_pair, 87 | const std::unordered_map& cameras, 88 | const std::unordered_map& images) { 89 | const camera_t camera_id1 = images.at(image_pair.image_id1).camera_id; 90 | const camera_t camera_id2 = images.at(image_pair.image_id2).camera_id; 91 | 92 | if (camera_id1 == camera_id2) { 93 | problem_->AddResidualBlock( 94 | FetzerFocalLengthSameCameraCost::Create( 95 | image_pair.F, cameras.at(camera_id1).PrincipalPoint()), 96 | options_.loss_function.get(), 97 | &(focals_[camera_id1])); 98 | } else { 99 | problem_->AddResidualBlock( 100 | FetzerFocalLengthCost::Create(image_pair.F, 101 | cameras.at(camera_id1).PrincipalPoint(), 102 | cameras.at(camera_id2).PrincipalPoint()), 103 | options_.loss_function.get(), 104 | &(focals_[camera_id1]), 105 | &(focals_[camera_id2])); 106 | } 107 | } 108 | 109 | size_t ViewGraphCalibrator::ParameterizeCameras( 110 | const std::unordered_map& cameras) { 111 | size_t num_cameras = 0; 112 | for (auto& [camera_id, camera] : cameras) { 113 | if (!problem_->HasParameterBlock(&(focals_[camera_id]))) continue; 114 | 115 | num_cameras++; 116 | problem_->SetParameterLowerBound(&(focals_[camera_id]), 0, 1e-3); 117 | if (camera.has_prior_focal_length) { 118 | problem_->SetParameterBlockConstant(&(focals_[camera_id])); 119 | num_cameras--; 120 | } 121 | } 122 | 123 | return num_cameras; 124 | } 125 | 126 | void ViewGraphCalibrator::CopyBackResults( 127 | std::unordered_map& cameras) { 128 | size_t counter = 0; 129 | for (auto& [camera_id, camera] : cameras) { 130 | if (!problem_->HasParameterBlock(&(focals_[camera_id]))) continue; 131 | 132 | // if the estimated parameter is too crazy, reject it 133 | if (focals_[camera_id] / camera.Focal() > options_.thres_higher_ratio || 134 | focals_[camera_id] / camera.Focal() < options_.thres_lower_ratio) { 135 | if (options_.verbose) 136 | LOG(INFO) << "NOT ACCEPTED: Camera " << camera_id 137 | << " focal: " << focals_[camera_id] 138 | << " original focal: " << camera.Focal(); 139 | counter++; 140 | 141 | continue; 142 | } 143 | 144 | // Marke that the camera has refined intrinsics 145 | camera.has_refined_focal_length = true; 146 | 147 | // Update the focal length 148 | for (const size_t idx : camera.FocalLengthIdxs()) { 149 | camera.params[idx] = focals_[camera_id]; 150 | if (options_.verbose) 151 | LOG(INFO) << "Camera " << idx << " focal: " << focals_[camera_id] 152 | << std::endl; 153 | } 154 | } 155 | LOG(INFO) << counter << " cameras are rejected in view graph calibration"; 156 | } 157 | 158 | size_t ViewGraphCalibrator::FilterImagePairs(ViewGraph& view_graph) const { 159 | ceres::Problem::EvaluateOptions eval_options; 160 | eval_options.num_threads = options_.solver_options.num_threads; 161 | eval_options.apply_loss_function = false; 162 | std::vector residuals; 163 | problem_->Evaluate(eval_options, nullptr, &residuals, nullptr, nullptr); 164 | 165 | // Dump the residuals into the original data structure 166 | size_t counter = 0; 167 | size_t invalid_counter = 0; 168 | 169 | const double thres_two_view_error_sq = 170 | options_.thres_two_view_error * options_.thres_two_view_error; 171 | 172 | for (auto& [image_pair_id, image_pair] : view_graph.image_pairs) { 173 | if (image_pair.config != colmap::TwoViewGeometry::CALIBRATED && 174 | image_pair.config != colmap::TwoViewGeometry::UNCALIBRATED) 175 | continue; 176 | if (image_pair.is_valid == false) continue; 177 | 178 | const Eigen::Vector2d error(residuals[counter], residuals[counter + 1]); 179 | 180 | // Set the two view geometry to be invalid if the error is too high 181 | if (error.squaredNorm() > thres_two_view_error_sq) { 182 | invalid_counter++; 183 | image_pair.is_valid = false; 184 | } 185 | 186 | counter += 2; 187 | } 188 | 189 | LOG(INFO) << "invalid / total number of two view geometry: " 190 | << invalid_counter << " / " << counter; 191 | 192 | return invalid_counter; 193 | } 194 | 195 | } // namespace glomap 196 | -------------------------------------------------------------------------------- /glomap/estimators/bundle_adjustment.cc: -------------------------------------------------------------------------------- 1 | #include "bundle_adjustment.h" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace glomap { 8 | 9 | bool BundleAdjuster::Solve(const ViewGraph& view_graph, 10 | std::unordered_map& cameras, 11 | std::unordered_map& images, 12 | std::unordered_map& tracks) { 13 | // Check if the input data is valid 14 | if (images.empty()) { 15 | LOG(ERROR) << "Number of images = " << images.size(); 16 | return false; 17 | } 18 | if (tracks.empty()) { 19 | LOG(ERROR) << "Number of tracks = " << tracks.size(); 20 | return false; 21 | } 22 | 23 | // Reset the problem 24 | Reset(); 25 | 26 | // Add the constraints that the point tracks impose on the problem 27 | AddPointToCameraConstraints(view_graph, cameras, images, tracks); 28 | 29 | // Add the cameras and points to the parameter groups for schur-based 30 | // optimization 31 | AddCamerasAndPointsToParameterGroups(cameras, images, tracks); 32 | 33 | // Parameterize the variables 34 | ParameterizeVariables(cameras, images, tracks); 35 | 36 | // Set the solver options. 37 | ceres::Solver::Summary summary; 38 | 39 | // Do not use the iterative solver, as it does not seem to be helpful 40 | options_.solver_options.linear_solver_type = ceres::SPARSE_SCHUR; 41 | options_.solver_options.preconditioner_type = ceres::CLUSTER_TRIDIAGONAL; 42 | 43 | options_.solver_options.minimizer_progress_to_stdout = options_.verbose; 44 | ceres::Solve(options_.solver_options, problem_.get(), &summary); 45 | if (options_.verbose) 46 | LOG(INFO) << summary.FullReport(); 47 | else 48 | LOG(INFO) << summary.BriefReport(); 49 | 50 | return summary.IsSolutionUsable(); 51 | } 52 | 53 | void BundleAdjuster::Reset() { 54 | ceres::Problem::Options problem_options; 55 | problem_options.loss_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; 56 | problem_ = std::make_unique(problem_options); 57 | } 58 | 59 | void BundleAdjuster::AddPointToCameraConstraints( 60 | const ViewGraph& view_graph, 61 | std::unordered_map& cameras, 62 | std::unordered_map& images, 63 | std::unordered_map& tracks) { 64 | for (auto& [track_id, track] : tracks) { 65 | if (track.observations.size() < options_.min_num_view_per_track) continue; 66 | 67 | for (const auto& observation : tracks[track_id].observations) { 68 | if (images.find(observation.first) == images.end()) continue; 69 | 70 | Image& image = images[observation.first]; 71 | 72 | ceres::CostFunction* cost_function = 73 | colmap::CameraCostFunction( 74 | cameras[image.camera_id].model_id, 75 | image.features[observation.second]); 76 | 77 | if (cost_function != nullptr) { 78 | problem_->AddResidualBlock( 79 | cost_function, 80 | options_.loss_function.get(), 81 | image.cam_from_world.rotation.coeffs().data(), 82 | image.cam_from_world.translation.data(), 83 | tracks[track_id].xyz.data(), 84 | cameras[image.camera_id].params.data()); 85 | } else { 86 | LOG(ERROR) << "Camera model not supported: " 87 | << colmap::CameraModelIdToName( 88 | cameras[image.camera_id].model_id); 89 | } 90 | } 91 | } 92 | } 93 | 94 | void BundleAdjuster::AddCamerasAndPointsToParameterGroups( 95 | std::unordered_map& cameras, 96 | std::unordered_map& images, 97 | std::unordered_map& tracks) { 98 | if (tracks.size() == 0) return; 99 | 100 | // Create a custom ordering for Schur-based problems. 101 | options_.solver_options.linear_solver_ordering.reset( 102 | new ceres::ParameterBlockOrdering); 103 | ceres::ParameterBlockOrdering* parameter_ordering = 104 | options_.solver_options.linear_solver_ordering.get(); 105 | // Add point parameters to group 0. 106 | for (auto& [track_id, track] : tracks) { 107 | if (problem_->HasParameterBlock(track.xyz.data())) 108 | parameter_ordering->AddElementToGroup(track.xyz.data(), 0); 109 | } 110 | 111 | // Add camera parameters to group 1. 112 | for (auto& [image_id, image] : images) { 113 | if (problem_->HasParameterBlock(image.cam_from_world.translation.data())) { 114 | parameter_ordering->AddElementToGroup( 115 | image.cam_from_world.translation.data(), 1); 116 | parameter_ordering->AddElementToGroup( 117 | image.cam_from_world.rotation.coeffs().data(), 1); 118 | } 119 | } 120 | 121 | // Add camera parameters to group 1. 122 | for (auto& [camera_id, camera] : cameras) { 123 | if (problem_->HasParameterBlock(camera.params.data())) 124 | parameter_ordering->AddElementToGroup(camera.params.data(), 1); 125 | } 126 | } 127 | 128 | void BundleAdjuster::ParameterizeVariables( 129 | std::unordered_map& cameras, 130 | std::unordered_map& images, 131 | std::unordered_map& tracks) { 132 | image_t center; 133 | 134 | // Parameterize rotations, and set rotations and translations to be constant 135 | // if desired FUTURE: Consider fix the scale of the reconstruction 136 | int counter = 0; 137 | for (auto& [image_id, image] : images) { 138 | if (problem_->HasParameterBlock( 139 | image.cam_from_world.rotation.coeffs().data())) { 140 | colmap::SetQuaternionManifold( 141 | problem_.get(), image.cam_from_world.rotation.coeffs().data()); 142 | 143 | if (counter == 0) { 144 | center = image_id; 145 | counter++; 146 | } 147 | if (!options_.optimize_rotations) 148 | problem_->SetParameterBlockConstant( 149 | image.cam_from_world.rotation.coeffs().data()); 150 | if (!options_.optimize_translation) 151 | problem_->SetParameterBlockConstant( 152 | image.cam_from_world.translation.data()); 153 | } 154 | } 155 | 156 | // Set the first camera to be fixed to remove the gauge ambiguity. 157 | problem_->SetParameterBlockConstant( 158 | images[center].cam_from_world.rotation.coeffs().data()); 159 | problem_->SetParameterBlockConstant( 160 | images[center].cam_from_world.translation.data()); 161 | 162 | // Parameterize the camera parameters, or set them to be constant if desired 163 | if (options_.optimize_intrinsics) { 164 | for (auto& [camera_id, camera] : cameras) { 165 | if (problem_->HasParameterBlock(camera.params.data())) { 166 | std::vector principal_point_idxs; 167 | for (auto idx : camera.PrincipalPointIdxs()) { 168 | principal_point_idxs.push_back(idx); 169 | } 170 | colmap::SetSubsetManifold(camera.params.size(), 171 | principal_point_idxs, 172 | problem_.get(), 173 | camera.params.data()); 174 | } 175 | } 176 | 177 | } else { 178 | for (auto& [camera_id, camera] : cameras) { 179 | if (problem_->HasParameterBlock(camera.params.data())) { 180 | problem_->SetParameterBlockConstant(camera.params.data()); 181 | } 182 | } 183 | } 184 | 185 | if (!options_.optimize_points) { 186 | for (auto& [track_id, track] : tracks) { 187 | if (problem_->HasParameterBlock(track.xyz.data())) { 188 | problem_->SetParameterBlockConstant(track.xyz.data()); 189 | } 190 | } 191 | } 192 | } 193 | 194 | } // namespace glomap 195 | -------------------------------------------------------------------------------- /glomap/processors/image_pair_inliers.cc: -------------------------------------------------------------------------------- 1 | #include "glomap/processors/image_pair_inliers.h" 2 | 3 | #include "glomap/math/two_view_geometry.h" 4 | 5 | namespace glomap { 6 | 7 | double ImagePairInliers::ScoreError() { 8 | // Count inliers base on the type 9 | if (image_pair.config == colmap::TwoViewGeometry::PLANAR || 10 | image_pair.config == colmap::TwoViewGeometry::PANORAMIC || 11 | image_pair.config == colmap::TwoViewGeometry::PLANAR_OR_PANORAMIC) 12 | return ScoreErrorHomography(); 13 | else if (image_pair.config == colmap::TwoViewGeometry::UNCALIBRATED) 14 | return ScoreErrorFundamental(); 15 | else if (image_pair.config == colmap::TwoViewGeometry::CALIBRATED) 16 | return ScoreErrorEssential(); 17 | return 0; 18 | } 19 | 20 | double ImagePairInliers::ScoreErrorEssential() { 21 | const Rigid3d& cam2_from_cam1 = image_pair.cam2_from_cam1; 22 | Eigen::Matrix3d E; 23 | EssentialFromMotion(cam2_from_cam1, &E); 24 | 25 | // eij = camera i on image j 26 | Eigen::Vector3d epipole12, epipole21; 27 | epipole12 = cam2_from_cam1.translation; 28 | epipole21 = Inverse(cam2_from_cam1).translation; 29 | 30 | if (epipole12[2] < 0) epipole12 = -epipole12; 31 | if (epipole21[2] < 0) epipole21 = -epipole21; 32 | 33 | if (image_pair.inliers.size() > 0) { 34 | image_pair.inliers.clear(); 35 | } 36 | 37 | image_t image_id1 = image_pair.image_id1; 38 | image_t image_id2 = image_pair.image_id2; 39 | 40 | double thres = options.max_epipolar_error_E; 41 | 42 | // Conver the threshold from pixel space to normalized space 43 | thres = options.max_epipolar_error_E * 0.5 * 44 | (1. / cameras->at(images.at(image_id1).camera_id).Focal() + 45 | 1. / cameras->at(images.at(image_id2).camera_id).Focal()); 46 | 47 | // Square the threshold for faster computation 48 | double sq_threshold = thres * thres; 49 | double score = 0.; 50 | Eigen::Vector3d pt1, pt2; 51 | 52 | // TODO: determine the best threshold for triangulation angle 53 | // double thres_angle = std::cos(DegToRad(1.)); 54 | double thres_epipole = std::cos(DegToRad(3.)); 55 | double thres_angle = 1; 56 | thres_angle += 1e-6; 57 | thres_epipole += 1e-6; 58 | for (size_t k = 0; k < image_pair.matches.rows(); ++k) { 59 | // Use the undistorted features 60 | pt1 = images.at(image_id1).features_undist[image_pair.matches(k, 0)]; 61 | pt2 = images.at(image_id2).features_undist[image_pair.matches(k, 1)]; 62 | const double r2 = SampsonError(E, pt1, pt2); 63 | 64 | if (r2 < sq_threshold) { 65 | bool cheirality = CheckCheirality(cam2_from_cam1, pt1, pt2, 1e-2, 100.); 66 | 67 | // Check whether two image rays have small triangulation angle or are too 68 | // close to the epipoles 69 | bool not_denegerate = true; 70 | 71 | // Check whether two image rays are too close 72 | double diff_angle = pt1.dot(cam2_from_cam1.rotation.inverse() * pt2); 73 | not_denegerate = (diff_angle < thres_angle); 74 | 75 | // Check whether two points are too close to the epipoles 76 | double diff_epipole1 = pt1.dot(epipole21); 77 | double diff_epipole2 = pt2.dot(epipole12); 78 | not_denegerate = not_denegerate && (diff_epipole1 < thres_epipole && 79 | diff_epipole2 < thres_epipole); 80 | 81 | if (cheirality && not_denegerate) { 82 | score += r2; 83 | image_pair.inliers.push_back(k); 84 | } else { 85 | score += sq_threshold; 86 | } 87 | } else { 88 | score += sq_threshold; 89 | } 90 | } 91 | return score; 92 | } 93 | 94 | double ImagePairInliers::ScoreErrorFundamental() { 95 | if (image_pair.inliers.size() > 0) { 96 | image_pair.inliers.clear(); 97 | } 98 | 99 | Eigen::Vector3d epipole = image_pair.F.row(0).cross(image_pair.F.row(2)); 100 | 101 | bool status = false; 102 | for (auto i = 0; i < 3; i++) { 103 | if ((epipole(i) > EPS) || (epipole(i) < -EPS)) { 104 | status = true; 105 | break; 106 | } 107 | } 108 | if (!status) { 109 | epipole = image_pair.F.row(1).cross(image_pair.F.row(2)); 110 | } 111 | 112 | // First, get the orientation signum for every point 113 | std::vector signums; 114 | int positive_count = 0; 115 | int negative_count = 0; 116 | 117 | image_t image_id1 = image_pair.image_id1; 118 | image_t image_id2 = image_pair.image_id2; 119 | 120 | double thres = options.max_epipolar_error_F; 121 | double sq_threshold = thres * thres; 122 | 123 | double score = 0.; 124 | Eigen::Vector2d pt1, pt2; 125 | 126 | std::vector inliers_pre; 127 | std::vector errors; 128 | for (size_t k = 0; k < image_pair.matches.rows(); ++k) { 129 | pt1 = images.at(image_id1).features[image_pair.matches(k, 0)]; 130 | pt2 = images.at(image_id2).features[image_pair.matches(k, 1)]; 131 | const double r2 = SampsonError(image_pair.F, pt1, pt2); 132 | 133 | if (r2 < sq_threshold) { 134 | signums.push_back(GetOrientationSignum(image_pair.F, epipole, pt1, pt2)); 135 | if (signums.back() > 0) { 136 | positive_count++; 137 | } else { 138 | negative_count++; 139 | } 140 | 141 | inliers_pre.push_back(k); 142 | errors.push_back(r2); 143 | } else { 144 | score += sq_threshold; 145 | } 146 | } 147 | bool is_positive = (positive_count > negative_count); 148 | 149 | // If cannot distinguish the signum, the pair should be invalid 150 | if (positive_count == negative_count) return 0; 151 | 152 | // Then, if the signum is not consistent with the cheirality, discard the 153 | // point 154 | for (int k = 0; k < inliers_pre.size(); k++) { 155 | bool cheirality = (signums[k] > 0) == is_positive; 156 | if (!cheirality) { 157 | score += sq_threshold; 158 | } else { 159 | image_pair.inliers.push_back(inliers_pre[k]); 160 | score += errors[k]; 161 | } 162 | } 163 | return score; 164 | } 165 | 166 | double ImagePairInliers::ScoreErrorHomography() { 167 | if (image_pair.inliers.size() > 0) { 168 | image_pair.inliers.clear(); 169 | } 170 | 171 | image_t image_id1 = image_pair.image_id1; 172 | image_t image_id2 = image_pair.image_id2; 173 | 174 | double thres = options.max_epipolar_error_H; 175 | double sq_threshold = thres * thres; 176 | double score = 0.; 177 | Eigen::Vector2d pt1, pt2; 178 | for (size_t k = 0; k < image_pair.matches.rows(); ++k) { 179 | pt1 = images.at(image_id1).features[image_pair.matches(k, 0)]; 180 | pt2 = images.at(image_id2).features[image_pair.matches(k, 1)]; 181 | const double r2 = HomographyError(image_pair.H, pt1, pt2); 182 | 183 | if (r2 < sq_threshold) { 184 | // TODO: cheirality check for homography. Is that a thing? 185 | bool cheirality = true; 186 | 187 | if (cheirality) { 188 | score += r2; 189 | image_pair.inliers.push_back(k); 190 | } else { 191 | score += sq_threshold; 192 | } 193 | } else { 194 | score += sq_threshold; 195 | } 196 | } 197 | return score; 198 | } 199 | 200 | void ImagePairsInlierCount(ViewGraph& view_graph, 201 | const std::unordered_map& cameras, 202 | const std::unordered_map& images, 203 | const InlierThresholdOptions& options, 204 | bool clean_inliers) { 205 | for (auto& [pair_id, image_pair] : view_graph.image_pairs) { 206 | if (!clean_inliers && image_pair.inliers.size() > 0) continue; 207 | image_pair.inliers.clear(); 208 | 209 | if (image_pair.is_valid == false) continue; 210 | ImagePairInliers inlier_finder(image_pair, images, options, &cameras); 211 | inlier_finder.ScoreError(); 212 | } 213 | } 214 | 215 | } // namespace glomap -------------------------------------------------------------------------------- /.github/workflows/ci.yml: -------------------------------------------------------------------------------- 1 | name: Ubuntu 2 | 3 | on: 4 | push: 5 | branches: 6 | - main 7 | pull_request: 8 | types: [ assigned, opened, synchronize, reopened ] 9 | release: 10 | types: [ published, edited ] 11 | 12 | jobs: 13 | build: 14 | name: ${{ matrix.config.os }} ${{ matrix.config.cmakeBuildType }} ${{ matrix.config.cudaEnabled && 'CUDA' || '' }} ${{ matrix.config.asanEnabled && 'ASan' || '' }} 15 | runs-on: ${{ matrix.config.os }} 16 | strategy: 17 | matrix: 18 | config: [ 19 | 20 | { 21 | os: ubuntu-22.04, 22 | cmakeBuildType: Release, 23 | asanEnabled: false, 24 | cudaEnabled: false, 25 | checkCodeFormat: true, 26 | }, 27 | { 28 | os: ubuntu-22.04, 29 | cmakeBuildType: Release, 30 | asanEnabled: false, 31 | cudaEnabled: true, 32 | checkCodeFormat: false, 33 | }, 34 | { 35 | os: ubuntu-22.04, 36 | cmakeBuildType: Release, 37 | asanEnabled: true, 38 | cudaEnabled: false, 39 | checkCodeFormat: false, 40 | }, 41 | { 42 | os: ubuntu-22.04, 43 | cmakeBuildType: ClangTidy, 44 | asanEnabled: false, 45 | cudaEnabled: false, 46 | checkCodeFormat: false, 47 | }, 48 | ] 49 | 50 | env: 51 | COMPILER_CACHE_VERSION: 1 52 | COMPILER_CACHE_DIR: ${{ github.workspace }}/compiler-cache 53 | CCACHE_DIR: ${{ github.workspace }}/compiler-cache/ccache 54 | CCACHE_BASEDIR: ${{ github.workspace }} 55 | CTCACHE_DIR: ${{ github.workspace }}/compiler-cache/ctcache 56 | 57 | steps: 58 | - uses: actions/checkout@v4 59 | - uses: actions/cache@v4 60 | id: cache-builds 61 | with: 62 | key: v${{ env.COMPILER_CACHE_VERSION }}-${{ matrix.config.os }}-${{ matrix.config.cmakeBuildType }}-${{ matrix.config.asanEnabled }}--${{ matrix.config.cudaEnabled }}-${{ github.run_id }}-${{ github.run_number }} 63 | restore-keys: v${{ env.COMPILER_CACHE_VERSION }}-${{ matrix.config.os }}-${{ matrix.config.cmakeBuildType }}-${{ matrix.config.asanEnabled }}--${{ matrix.config.cudaEnabled }} 64 | path: ${{ env.COMPILER_CACHE_DIR }} 65 | 66 | - name: Install compiler cache 67 | run: | 68 | mkdir -p "$CCACHE_DIR" "$CTCACHE_DIR" 69 | echo "$COMPILER_CACHE_DIR/bin" >> $GITHUB_PATH 70 | if [ -f "$COMPILER_CACHE_DIR/bin/ccache" ]; then 71 | exit 0 72 | fi 73 | set -x 74 | wget https://github.com/ccache/ccache/releases/download/v4.8.2/ccache-4.8.2-linux-x86_64.tar.xz 75 | echo "0b33f39766fe9db67f40418aed6a5b3d7b2f4f7fab025a8213264b77a2d0e1b1 ccache-4.8.2-linux-x86_64.tar.xz" | sha256sum --check 76 | tar xfv ccache-4.8.2-linux-x86_64.tar.xz 77 | mkdir -p "$COMPILER_CACHE_DIR/bin" 78 | mv ./ccache-4.8.2-linux-x86_64/ccache "$COMPILER_CACHE_DIR/bin" 79 | ctcache_commit_id="66c3614175fc650591488519333c411b2eac15a3" 80 | wget https://github.com/matus-chochlik/ctcache/archive/${ctcache_commit_id}.zip 81 | echo "108b087f156a9fe7da0c796de1ef73f5855d2a33a27983769ea39061359a40fc ${ctcache_commit_id}.zip" | sha256sum --check 82 | unzip "${ctcache_commit_id}.zip" 83 | mv ctcache-${ctcache_commit_id}/clang-tidy* "$COMPILER_CACHE_DIR/bin" 84 | - name: Check code format 85 | if: matrix.config.checkCodeFormat 86 | run: | 87 | set +x -euo pipefail 88 | sudo apt-get update && sudo apt-get install -y clang-format-14 black 89 | ./scripts/format/clang_format.sh 90 | git diff --name-only 91 | git diff --exit-code || (echo "Code formatting failed" && exit 1) 92 | - name: Setup Ubuntu 93 | run: | 94 | sudo apt-get update && sudo apt-get install -y \ 95 | build-essential \ 96 | cmake \ 97 | ninja-build \ 98 | libboost-program-options-dev \ 99 | libboost-filesystem-dev \ 100 | libboost-graph-dev \ 101 | libboost-system-dev \ 102 | libeigen3-dev \ 103 | libsuitesparse-dev \ 104 | libceres-dev \ 105 | libflann-dev \ 106 | libfreeimage-dev \ 107 | libmetis-dev \ 108 | libgoogle-glog-dev \ 109 | libgtest-dev \ 110 | libsqlite3-dev \ 111 | libglew-dev \ 112 | qtbase5-dev \ 113 | libqt5opengl5-dev \ 114 | libcgal-dev \ 115 | libcgal-qt5-dev \ 116 | libgl1-mesa-dri \ 117 | libunwind-dev \ 118 | xvfb 119 | if [ "${{ matrix.config.cudaEnabled }}" == "true" ]; then 120 | if [ "${{ matrix.config.os }}" == "ubuntu-20.04" ]; then 121 | sudo apt-get install -y \ 122 | nvidia-cuda-toolkit \ 123 | nvidia-cuda-toolkit-gcc 124 | echo "CC=/usr/bin/cuda-gcc" >> $GITHUB_ENV 125 | echo "CXX=/usr/bin/cuda-g++" >> $GITHUB_ENV 126 | elif [ "${{ matrix.config.os }}" == "ubuntu-22.04" ]; then 127 | sudo apt-get install -y \ 128 | nvidia-cuda-toolkit \ 129 | nvidia-cuda-toolkit-gcc \ 130 | gcc-10 g++-10 131 | echo "CC=/usr/bin/gcc-10" >> $GITHUB_ENV 132 | echo "CXX=/usr/bin/g++-10" >> $GITHUB_ENV 133 | echo "CUDAHOSTCXX=/usr/bin/g++-10" >> $GITHUB_ENV 134 | fi 135 | fi 136 | if [ "${{ matrix.config.asanEnabled }}" == "true" ]; then 137 | sudo apt-get install -y clang-15 libomp-15-dev 138 | echo "CC=/usr/bin/clang-15" >> $GITHUB_ENV 139 | echo "CXX=/usr/bin/clang++-15" >> $GITHUB_ENV 140 | fi 141 | if [ "${{ matrix.config.cmakeBuildType }}" == "ClangTidy" ]; then 142 | sudo apt-get install -y clang-15 clang-tidy-15 libomp-15-dev 143 | echo "CC=/usr/bin/clang-15" >> $GITHUB_ENV 144 | echo "CXX=/usr/bin/clang++-15" >> $GITHUB_ENV 145 | fi 146 | - name: Upgrade CMake 147 | run: | 148 | CMAKE_VERSION=3.28.6 149 | CMAKE_DIR=cmake-${CMAKE_VERSION}-linux-x86_64 150 | wget https://github.com/Kitware/CMake/releases/download/v${CMAKE_VERSION}/${CMAKE_DIR}.tar.gz 151 | tar -xzf ${CMAKE_DIR}.tar.gz 152 | sudo cp -r ${CMAKE_DIR}/* /usr/local/ 153 | rm -rf ${CMAKE_DIR}* 154 | - name: Configure and build 155 | run: | 156 | set -x 157 | cmake --version 158 | mkdir build 159 | cd build 160 | cmake .. \ 161 | -GNinja \ 162 | -DCMAKE_BUILD_TYPE=${{ matrix.config.cmakeBuildType }} \ 163 | -DCMAKE_INSTALL_PREFIX=./install \ 164 | -DCMAKE_CUDA_ARCHITECTURES=50 \ 165 | -DSuiteSparse_CHOLMOD_LIBRARY="/usr/lib/x86_64-linux-gnu/libcholmod.so" \ 166 | -DSuiteSparse_CHOLMOD_INCLUDE_DIR="/usr/include/suitesparse" \ 167 | -DTESTS_ENABLED=ON \ 168 | -DASAN_ENABLED=${{ matrix.config.asanEnabled }} 169 | ninja -k 10000 170 | - name: Run tests 171 | if: ${{ matrix.config.cmakeBuildType != 'ClangTidy' }} 172 | run: | 173 | export DISPLAY=":99.0" 174 | export QT_QPA_PLATFORM="offscreen" 175 | Xvfb :99 & 176 | sleep 3 177 | cd build 178 | ctest --output-on-failure -E .+colmap_.* 179 | - name: Cleanup compiler cache 180 | run: | 181 | set -x 182 | ccache --show-stats --verbose 183 | ccache --evict-older-than 1d 184 | ccache --show-stats --verbose 185 | echo "Size of ctcache before: $(du -sh $CTCACHE_DIR)" 186 | echo "Number of ctcache files before: $(find $CTCACHE_DIR | wc -l)" 187 | # Delete cache older than 10 days. 188 | find "$CTCACHE_DIR"/*/ -mtime +10 -print0 | xargs -0 rm -rf 189 | echo "Size of ctcache after: $(du -sh $CTCACHE_DIR)" 190 | echo "Number of ctcache files after: $(find $CTCACHE_DIR | wc -l)" -------------------------------------------------------------------------------- /glomap/controllers/track_establishment.cc: -------------------------------------------------------------------------------- 1 | #include "track_establishment.h" 2 | 3 | namespace glomap { 4 | 5 | size_t TrackEngine::EstablishFullTracks( 6 | std::unordered_map& tracks) { 7 | tracks.clear(); 8 | uf_.Clear(); 9 | 10 | // Blindly concatenate tracks if any matches occur 11 | BlindConcatenation(); 12 | 13 | // Iterate through the collected tracks and record the items for each track 14 | TrackCollection(tracks); 15 | 16 | return tracks.size(); 17 | } 18 | 19 | void TrackEngine::BlindConcatenation() { 20 | // Initialize the union find data structure by connecting all the 21 | // correspondences 22 | image_pair_t counter = 0; 23 | for (auto pair : view_graph_.image_pairs) { 24 | if ((counter + 1) % 1000 == 0 || 25 | counter == view_graph_.image_pairs.size() - 1) { 26 | std::cout << "\r Initializing pairs " << counter + 1 << " / " 27 | << view_graph_.image_pairs.size() << std::flush; 28 | } 29 | counter++; 30 | 31 | const ImagePair& image_pair = pair.second; 32 | if (!image_pair.is_valid) continue; 33 | 34 | // Get the matches 35 | const Eigen::MatrixXi& matches = image_pair.matches; 36 | 37 | // Get the inlier mask 38 | const std::vector& inliers = image_pair.inliers; 39 | 40 | for (size_t i = 0; i < inliers.size(); i++) { 41 | size_t idx = inliers[i]; 42 | 43 | // Get point indices 44 | const uint32_t& point1_idx = matches(idx, 0); 45 | const uint32_t& point2_idx = matches(idx, 1); 46 | 47 | image_pair_t point_global_id1 = 48 | static_cast(image_pair.image_id1) << 32 | 49 | static_cast(point1_idx); 50 | image_pair_t point_global_id2 = 51 | static_cast(image_pair.image_id2) << 32 | 52 | static_cast(point2_idx); 53 | 54 | // Link the first point to the second point. Take the smallest one as the 55 | // root 56 | if (point_global_id2 < point_global_id1) { 57 | uf_.Union(point_global_id1, point_global_id2); 58 | } else 59 | uf_.Union(point_global_id2, point_global_id1); 60 | } 61 | } 62 | std::cout << std::endl; 63 | } 64 | 65 | void TrackEngine::TrackCollection(std::unordered_map& tracks) { 66 | std::unordered_map> track_map; 67 | std::unordered_map track_counter; 68 | 69 | // Create tracks from the connected components of the point correspondences 70 | size_t counter = 0; 71 | for (auto pair : view_graph_.image_pairs) { 72 | if ((counter + 1) % 1000 == 0 || 73 | counter == view_graph_.image_pairs.size() - 1) { 74 | std::cout << "\r Establishing pairs " << counter + 1 << " / " 75 | << view_graph_.image_pairs.size() << std::flush; 76 | } 77 | counter++; 78 | 79 | const ImagePair& image_pair = pair.second; 80 | if (!image_pair.is_valid) continue; 81 | 82 | // Get the matches 83 | const Eigen::MatrixXi& matches = image_pair.matches; 84 | 85 | // Get the inlier mask 86 | const std::vector& inliers = image_pair.inliers; 87 | 88 | for (size_t i = 0; i < inliers.size(); i++) { 89 | size_t idx = inliers[i]; 90 | 91 | // Get point indices 92 | const uint32_t& point1_idx = matches(idx, 0); 93 | const uint32_t& point2_idx = matches(idx, 1); 94 | 95 | image_pair_t point_global_id1 = 96 | static_cast(image_pair.image_id1) << 32 | 97 | static_cast(point1_idx); 98 | image_pair_t point_global_id2 = 99 | static_cast(image_pair.image_id2) << 32 | 100 | static_cast(point2_idx); 101 | 102 | image_pair_t track_id = uf_.Find(point_global_id1); 103 | 104 | track_map[track_id].insert(point_global_id1); 105 | track_map[track_id].insert(point_global_id2); 106 | track_counter[track_id]++; 107 | } 108 | } 109 | std::cout << std::endl; 110 | 111 | counter = 0; 112 | size_t discarded_counter = 0; 113 | for (auto& [track_id, correspondence_set] : track_map) { 114 | if ((counter + 1) % 1000 == 0 || counter == track_map.size() - 1) { 115 | std::cout << "\r Establishing tracks " << counter + 1 << " / " 116 | << track_map.size() << std::flush; 117 | } 118 | counter++; 119 | 120 | std::unordered_map> image_id_set; 121 | for (auto point_global_id : correspondence_set) { 122 | // image_id is the higher 32 bits and feature_id is the lower 32 bits 123 | image_t image_id = point_global_id >> 32; 124 | feature_t feature_id = point_global_id & 0xFFFFFFFF; 125 | if (image_id_set.find(image_id) != image_id_set.end()) { 126 | for (const auto& feature : image_id_set.at(image_id)) { 127 | if ((feature - images_.at(image_id).features[feature_id]).norm() > 128 | options_.thres_inconsistency) { 129 | tracks[track_id].observations.clear(); 130 | break; 131 | } 132 | } 133 | if (tracks[track_id].observations.size() == 0) { 134 | discarded_counter++; 135 | break; 136 | } 137 | } else 138 | image_id_set.insert( 139 | std::make_pair(image_id, std::vector())); 140 | 141 | image_id_set[image_id].push_back( 142 | images_.at(image_id).features[feature_id]); 143 | 144 | tracks[track_id].observations.emplace_back(image_id, feature_id); 145 | } 146 | } 147 | 148 | std::cout << std::endl; 149 | LOG(INFO) << "Discarded " << discarded_counter 150 | << " tracks due to inconsistency"; 151 | } 152 | 153 | size_t TrackEngine::FindTracksForProblem( 154 | const std::unordered_map& tracks_full, 155 | std::unordered_map& tracks_selected) { 156 | // Sort the tracks by length 157 | std::vector> track_lengths; 158 | 159 | // std::unordered_map> map_track; 160 | for (const auto& [track_id, track] : tracks_full) { 161 | if (track.observations.size() < options_.min_num_view_per_track) continue; 162 | // FUTURE: have a more elegant way of filtering tracks 163 | if (track.observations.size() > options_.max_num_view_per_track) continue; 164 | track_lengths.emplace_back( 165 | std::make_pair(track.observations.size(), track_id)); 166 | } 167 | std::sort(std::rbegin(track_lengths), std::rend(track_lengths)); 168 | 169 | // Initialize the track per camera number to zero 170 | std::unordered_map tracks_per_camera; 171 | 172 | // If we only want to select a subset of images, then only add the tracks 173 | // corresponding to those images 174 | std::unordered_map tracks; 175 | for (const auto& [image_id, image] : images_) { 176 | if (!image.is_registered) continue; 177 | 178 | tracks_per_camera[image_id] = 0; 179 | } 180 | 181 | int cameras_left = tracks_per_camera.size(); 182 | for (const auto& [track_length, track_id] : track_lengths) { 183 | const auto& track = tracks_full.at(track_id); 184 | 185 | // Collect the image ids. For each image, only increment the counter by 1 186 | std::unordered_set image_ids; 187 | Track track_temp; 188 | for (const auto& [image_id, feature_id] : track.observations) { 189 | if (tracks_per_camera.count(image_id) == 0) continue; 190 | 191 | track_temp.track_id = track_id; 192 | track_temp.observations.emplace_back( 193 | std::make_pair(image_id, feature_id)); 194 | image_ids.insert(image_id); 195 | } 196 | 197 | if (image_ids.size() < options_.min_num_view_per_track) continue; 198 | 199 | // A flag to see if the track has already been added or not to avoid 200 | // multiple insertion into the set to be efficient 201 | bool added = false; 202 | // for (auto &image_id : image_ids) { 203 | for (const auto& [image_id, feature_id] : track_temp.observations) { 204 | // Getting the current number of tracks 205 | auto& track_per_camera = tracks_per_camera[image_id]; 206 | if (track_per_camera > options_.min_num_tracks_per_view) continue; 207 | 208 | // Otherwise, increase the track number per camera 209 | ++track_per_camera; 210 | if (track_per_camera > options_.min_num_tracks_per_view) --cameras_left; 211 | 212 | if (!added) { 213 | tracks.insert(std::make_pair(track_id, track_temp)); 214 | added = true; 215 | } 216 | } 217 | // Stop iterating if all cameras have enough tracks assigned 218 | if (cameras_left == 0) break; 219 | if (tracks.size() > options_.max_num_tracks) break; 220 | } 221 | 222 | // To avoid flushing the track_full, we copy the selected tracks to the 223 | // selected tracks 224 | tracks_selected = tracks; 225 | 226 | return tracks.size(); 227 | } 228 | 229 | } // namespace glomap 230 | -------------------------------------------------------------------------------- /glomap/estimators/cost_function.h: -------------------------------------------------------------------------------- 1 | 2 | #pragma once 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace glomap { 9 | 10 | // ---------------------------------------- 11 | // BATAPairwiseDirectionError 12 | // ---------------------------------------- 13 | // Computes the error between a translation direction and the direction formed 14 | // from two positions such that t_ij - scale * (c_j - c_i) is minimized. 15 | struct BATAPairwiseDirectionError { 16 | BATAPairwiseDirectionError(const Eigen::Vector3d& translation_obs) 17 | : translation_obs_(translation_obs){}; 18 | 19 | // The error is given by the position error described above. 20 | template 21 | bool operator()(const T* position1, 22 | const T* position2, 23 | const T* scale, 24 | T* residuals) const { 25 | Eigen::Matrix translation; 26 | translation[0] = position2[0] - position1[0]; 27 | translation[1] = position2[1] - position1[1]; 28 | translation[2] = position2[2] - position1[2]; 29 | 30 | Eigen::Matrix residual_vec; 31 | 32 | residual_vec = translation_obs_.cast() - scale[0] * translation; 33 | 34 | residuals[0] = residual_vec(0); 35 | residuals[1] = residual_vec(1); 36 | residuals[2] = residual_vec(2); 37 | 38 | return true; 39 | } 40 | 41 | static ceres::CostFunction* Create(const Eigen::Vector3d& translation_obs) { 42 | return ( 43 | new ceres::AutoDiffCostFunction( 44 | new BATAPairwiseDirectionError(translation_obs))); 45 | } 46 | 47 | // TODO: add covariance 48 | const Eigen::Vector3d translation_obs_; 49 | }; 50 | 51 | // ---------------------------------------- 52 | // FetzerFocalLengthCost 53 | // ---------------------------------------- 54 | // Below are assets for DMAP by Philipp Lindenberger 55 | inline Eigen::Vector4d fetzer_d(const Eigen::Vector3d& ai, 56 | const Eigen::Vector3d& bi, 57 | const Eigen::Vector3d& aj, 58 | const Eigen::Vector3d& bj, 59 | const int u, 60 | const int v) { 61 | Eigen::Vector4d d; 62 | d.setZero(); 63 | d(0) = ai(u) * aj(v) - ai(v) * aj(u); 64 | d(1) = ai(u) * bj(v) - ai(v) * bj(u); 65 | d(2) = bi(u) * aj(v) - bi(v) * aj(u); 66 | d(3) = bi(u) * bj(v) - bi(v) * bj(u); 67 | return d; 68 | } 69 | 70 | inline std::array fetzer_ds( 71 | const Eigen::Matrix3d& i1_G_i0) { 72 | Eigen::JacobiSVD svd( 73 | i1_G_i0, Eigen::ComputeFullU | Eigen::ComputeFullV); 74 | Eigen::Vector3d s = svd.singularValues(); 75 | 76 | Eigen::Vector3d v_0 = svd.matrixV().col(0); 77 | Eigen::Vector3d v_1 = svd.matrixV().col(1); 78 | 79 | Eigen::Vector3d u_0 = svd.matrixU().col(0); 80 | Eigen::Vector3d u_1 = svd.matrixU().col(1); 81 | 82 | Eigen::Vector3d ai = 83 | Eigen::Vector3d(s(0) * s(0) * (v_0(0) * v_0(0) + v_0(1) * v_0(1)), 84 | s(0) * s(1) * (v_0(0) * v_1(0) + v_0(1) * v_1(1)), 85 | s(1) * s(1) * (v_1(0) * v_1(0) + v_1(1) * v_1(1))); 86 | 87 | Eigen::Vector3d aj = Eigen::Vector3d(u_1(0) * u_1(0) + u_1(1) * u_1(1), 88 | -(u_0(0) * u_1(0) + u_0(1) * u_1(1)), 89 | u_0(0) * u_0(0) + u_0(1) * u_0(1)); 90 | 91 | Eigen::Vector3d bi = Eigen::Vector3d(s(0) * s(0) * v_0(2) * v_0(2), 92 | s(0) * s(1) * v_0(2) * v_1(2), 93 | s(1) * s(1) * v_1(2) * v_1(2)); 94 | 95 | Eigen::Vector3d bj = 96 | Eigen::Vector3d(u_1(2) * u_1(2), -(u_0(2) * u_1(2)), u_0(2) * u_0(2)); 97 | 98 | Eigen::Vector4d d_01 = fetzer_d(ai, bi, aj, bj, 1, 0); 99 | Eigen::Vector4d d_02 = fetzer_d(ai, bi, aj, bj, 0, 2); 100 | Eigen::Vector4d d_12 = fetzer_d(ai, bi, aj, bj, 2, 1); 101 | 102 | std::array ds; 103 | ds[0] = d_01; 104 | ds[1] = d_02; 105 | ds[2] = d_12; 106 | 107 | return ds; 108 | } 109 | 110 | class FetzerFocalLengthCost { 111 | public: 112 | FetzerFocalLengthCost(const Eigen::Matrix3d& i1_F_i0, 113 | const Eigen::Vector2d& principal_point0, 114 | const Eigen::Vector2d& principal_point1) { 115 | Eigen::Matrix3d K0 = Eigen::Matrix3d::Identity(3, 3); 116 | K0(0, 2) = principal_point0(0); 117 | K0(1, 2) = principal_point0(1); 118 | 119 | Eigen::Matrix3d K1 = Eigen::Matrix3d::Identity(3, 3); 120 | K1(0, 2) = principal_point1(0); 121 | K1(1, 2) = principal_point1(1); 122 | 123 | const Eigen::Matrix3d i1_G_i0 = K1.transpose() * i1_F_i0 * K0; 124 | 125 | const std::array ds = fetzer_ds(i1_G_i0); 126 | 127 | d_01 = ds[0]; 128 | d_02 = ds[1]; 129 | d_12 = ds[2]; 130 | } 131 | 132 | static ceres::CostFunction* Create(const Eigen::Matrix3d i1_F_i0, 133 | const Eigen::Vector2d& principal_point0, 134 | const Eigen::Vector2d& principal_point1) { 135 | return (new ceres::AutoDiffCostFunction( 136 | new FetzerFocalLengthCost( 137 | i1_F_i0, principal_point0, principal_point1))); 138 | } 139 | 140 | template 141 | bool operator()(const T* const fi_, const T* const fj_, T* residuals) const { 142 | const Eigen::Vector d_01_ = d_01.cast(); 143 | const Eigen::Vector d_12_ = d_12.cast(); 144 | 145 | const T fi = fi_[0]; 146 | const T fj = fj_[0]; 147 | 148 | T di = (fj * fj * d_01_(0) + d_01_(1)); 149 | T dj = (fi * fi * d_12_(0) + d_12_(2)); 150 | di = di == T(0) ? T(1e-6) : di; 151 | dj = dj == T(0) ? T(1e-6) : dj; 152 | 153 | const T K0_01 = -(fj * fj * d_01_(2) + d_01_(3)) / di; 154 | const T K1_12 = -(fi * fi * d_12_(1) + d_12_(3)) / dj; 155 | 156 | residuals[0] = (fi * fi - K0_01) / (fi * fi); 157 | residuals[1] = (fj * fj - K1_12) / (fj * fj); 158 | 159 | return true; 160 | } 161 | 162 | private: 163 | Eigen::Vector4d d_01; 164 | Eigen::Vector4d d_02; 165 | Eigen::Vector4d d_12; 166 | }; 167 | 168 | // Calibration error for the image pairs sharing the camera 169 | class FetzerFocalLengthSameCameraCost { 170 | public: 171 | FetzerFocalLengthSameCameraCost(const Eigen::Matrix3d& i1_F_i0, 172 | const Eigen::Vector2d& principal_point) { 173 | Eigen::Matrix3d K0 = Eigen::Matrix3d::Identity(3, 3); 174 | K0(0, 2) = principal_point(0); 175 | K0(1, 2) = principal_point(1); 176 | 177 | Eigen::Matrix3d K1 = Eigen::Matrix3d::Identity(3, 3); 178 | K1(0, 2) = principal_point(0); 179 | K1(1, 2) = principal_point(1); 180 | 181 | const Eigen::Matrix3d i1_G_i0 = K1.transpose() * i1_F_i0 * K0; 182 | 183 | const std::array ds = fetzer_ds(i1_G_i0); 184 | 185 | d_01 = ds[0]; 186 | d_02 = ds[1]; 187 | d_12 = ds[2]; 188 | } 189 | 190 | static ceres::CostFunction* Create(const Eigen::Matrix3d i1_F_i0, 191 | const Eigen::Vector2d& principal_point) { 192 | return ( 193 | new ceres::AutoDiffCostFunction( 194 | new FetzerFocalLengthSameCameraCost(i1_F_i0, principal_point))); 195 | } 196 | 197 | template 198 | bool operator()(const T* const fi_, T* residuals) const { 199 | const Eigen::Vector d_01_ = d_01.cast(); 200 | const Eigen::Vector d_12_ = d_12.cast(); 201 | 202 | const T fi = fi_[0]; 203 | const T fj = fi_[0]; 204 | 205 | T di = (fj * fj * d_01_(0) + d_01_(1)); 206 | T dj = (fi * fi * d_12_(0) + d_12_(2)); 207 | di = di == T(0) ? T(1e-6) : di; 208 | dj = dj == T(0) ? T(1e-6) : dj; 209 | 210 | const T K0_01 = -(fj * fj * d_01_(2) + d_01_(3)) / di; 211 | const T K1_12 = -(fi * fi * d_12_(1) + d_12_(3)) / dj; 212 | 213 | residuals[0] = (fi * fi - K0_01) / (fi * fi); 214 | residuals[1] = (fj * fj - K1_12) / (fj * fj); 215 | 216 | return true; 217 | } 218 | 219 | private: 220 | Eigen::Vector4d d_01; 221 | Eigen::Vector4d d_02; 222 | Eigen::Vector4d d_12; 223 | }; 224 | 225 | // ---------------------------------------- 226 | // GravError 227 | // ---------------------------------------- 228 | struct GravError { 229 | GravError(const Eigen::Vector3d& grav_obs) : grav_obs_(grav_obs) {} 230 | 231 | template 232 | bool operator()(const T* const gvec, T* residuals) const { 233 | Eigen::Matrix grav_est; 234 | grav_est << gvec[0], gvec[1], gvec[2]; 235 | 236 | for (int i = 0; i < 3; i++) { 237 | residuals[i] = grav_est[i] - grav_obs_[i]; 238 | } 239 | 240 | return true; 241 | } 242 | 243 | // Factory function 244 | static ceres::CostFunction* CreateCost(const Eigen::Vector3d& grav_obs) { 245 | return (new ceres::AutoDiffCostFunction( 246 | new GravError(grav_obs))); 247 | } 248 | 249 | private: 250 | const Eigen::Vector3d& grav_obs_; 251 | }; 252 | 253 | } // namespace glomap 254 | -------------------------------------------------------------------------------- /glomap/processors/view_graph_manipulation.cc: -------------------------------------------------------------------------------- 1 | #include "view_graph_manipulation.h" 2 | 3 | #include "glomap/math/two_view_geometry.h" 4 | #include "glomap/math/union_find.h" 5 | 6 | namespace glomap { 7 | image_pair_t ViewGraphManipulater::SparsifyGraph( 8 | ViewGraph& view_graph, 9 | std::unordered_map& images, 10 | int expected_degree) { 11 | image_t num_img = view_graph.KeepLargestConnectedComponents(images); 12 | 13 | // Keep track of chosen edges 14 | std::unordered_set chosen_edges; 15 | const std::unordered_map>& 16 | adjacency_list = view_graph.GetAdjacencyList(); 17 | 18 | // Here, the average is the mean of the degrees 19 | double average_degree = 0; 20 | for (const auto& [image_id, neighbors] : adjacency_list) { 21 | if (images[image_id].is_registered == false) continue; 22 | average_degree += neighbors.size(); 23 | } 24 | average_degree = average_degree / num_img; 25 | 26 | // Go through the adjacency list and keep edge with probability 27 | // ((expected_degree * average_degree) / (degree1 * degree2)) 28 | for (auto& [pair_id, image_pair] : view_graph.image_pairs) { 29 | if (!image_pair.is_valid) continue; 30 | 31 | image_t image_id1 = image_pair.image_id1; 32 | image_t image_id2 = image_pair.image_id2; 33 | 34 | if (images[image_id1].is_registered == false || 35 | images[image_id2].is_registered == false) 36 | continue; 37 | 38 | int degree1 = adjacency_list.at(image_id1).size(); 39 | int degree2 = adjacency_list.at(image_id2).size(); 40 | 41 | if (degree1 <= expected_degree || degree2 <= expected_degree) { 42 | chosen_edges.insert(pair_id); 43 | continue; 44 | } 45 | 46 | if (rand() / double(RAND_MAX) < 47 | (expected_degree * average_degree) / (degree1 * degree2)) { 48 | chosen_edges.insert(pair_id); 49 | } 50 | } 51 | 52 | // Set all pairs not in the chosen edges to invalid 53 | for (auto& [pair_id, image_pair] : view_graph.image_pairs) { 54 | if (chosen_edges.find(pair_id) == chosen_edges.end()) { 55 | image_pair.is_valid = false; 56 | } 57 | } 58 | 59 | // Keep the largest connected component 60 | view_graph.KeepLargestConnectedComponents(images); 61 | 62 | return chosen_edges.size(); 63 | } 64 | 65 | image_t ViewGraphManipulater::EstablishStrongClusters( 66 | ViewGraph& view_graph, 67 | std::unordered_map& images, 68 | StrongClusterCriteria criteria, 69 | double min_thres, 70 | int min_num_images) { 71 | image_t num_img_before = view_graph.KeepLargestConnectedComponents(images); 72 | 73 | // Construct the initial cluster by keeping the pairs with weight > min_thres 74 | UnionFind uf; 75 | // Go through the edges, and add the edge with weight > min_thres 76 | for (auto& [pair_id, image_pair] : view_graph.image_pairs) { 77 | if (image_pair.is_valid == false) continue; 78 | 79 | bool status = false; 80 | status = status || 81 | (criteria == INLIER_NUM && image_pair.inliers.size() > min_thres); 82 | status = status || (criteria == WEIGHT && image_pair.weight > min_thres); 83 | if (status) { 84 | uf.Union(image_pair_t(image_pair.image_id1), 85 | image_pair_t(image_pair.image_id2)); 86 | } 87 | } 88 | 89 | // For every two connected components, we check the number of slightly weaker 90 | // pairs (> 0.75 min_thres) between them Two clusters are concatenated if the 91 | // number of such pairs is larger than a threshold (2) 92 | bool status = true; 93 | int iteration = 0; 94 | while (status) { 95 | status = false; 96 | iteration++; 97 | 98 | if (iteration > 10) { 99 | break; 100 | } 101 | 102 | std::unordered_map> 103 | num_pairs; 104 | for (auto& [pair_id, image_pair] : view_graph.image_pairs) { 105 | if (image_pair.is_valid == false) continue; 106 | 107 | // If the number of inliers < 0.75 of the threshold, skip 108 | bool status = false; 109 | status = status || (criteria == INLIER_NUM && 110 | image_pair.inliers.size() < 0.75 * min_thres); 111 | status = status || 112 | (criteria == WEIGHT && image_pair.weight < 0.75 * min_thres); 113 | if (status) continue; 114 | 115 | image_t image_id1 = image_pair.image_id1; 116 | image_t image_id2 = image_pair.image_id2; 117 | 118 | image_pair_t root1 = uf.Find(image_pair_t(image_id1)); 119 | image_pair_t root2 = uf.Find(image_pair_t(image_id2)); 120 | 121 | if (root1 == root2) { 122 | continue; 123 | } 124 | if (num_pairs.find(root1) == num_pairs.end()) 125 | num_pairs.insert( 126 | std::make_pair(root1, std::unordered_map())); 127 | if (num_pairs.find(root2) == num_pairs.end()) 128 | num_pairs.insert( 129 | std::make_pair(root2, std::unordered_map())); 130 | 131 | num_pairs[root1][root2]++; 132 | num_pairs[root2][root1]++; 133 | } 134 | // Connect the clusters progressively. If two clusters have more than 3 135 | // pairs, then connect them 136 | for (auto& [root1, counter] : num_pairs) { 137 | for (auto& [root2, count] : counter) { 138 | if (root1 <= root2) continue; 139 | 140 | if (count >= 2) { 141 | status = true; 142 | uf.Union(root1, root2); 143 | } 144 | } 145 | } 146 | } 147 | 148 | for (auto& [image_pair_id, image_pair] : view_graph.image_pairs) { 149 | if (image_pair.is_valid == false) continue; 150 | 151 | image_t image_id1 = image_pair.image_id1; 152 | image_t image_id2 = image_pair.image_id2; 153 | 154 | if (uf.Find(image_pair_t(image_id1)) != uf.Find(image_pair_t(image_id2))) { 155 | image_pair.is_valid = false; 156 | } 157 | } 158 | int num_comp = view_graph.MarkConnectedComponents(images); 159 | 160 | LOG(INFO) << "Clustering take " << iteration << " iterations. " 161 | << "Images are grouped into " << num_comp 162 | << " clusters after strong-clustering"; 163 | 164 | return num_comp; 165 | } 166 | 167 | void ViewGraphManipulater::UpdateImagePairsConfig( 168 | ViewGraph& view_graph, 169 | const std::unordered_map& cameras, 170 | const std::unordered_map& images) { 171 | // For each camera, check the number of times that the camera is involved in a 172 | // pair with configuration 2 First: the total occurence; second: the number of 173 | // pairs with configuration 2 174 | std::unordered_map> camera_counter; 175 | for (auto& [pair_id, image_pair] : view_graph.image_pairs) { 176 | if (image_pair.is_valid == false) continue; 177 | 178 | camera_t camera_id1 = images.at(image_pair.image_id1).camera_id; 179 | camera_t camera_id2 = images.at(image_pair.image_id2).camera_id; 180 | 181 | const Camera& camera1 = cameras.at(camera_id1); 182 | const Camera& camera2 = cameras.at(camera_id2); 183 | if (!camera1.has_prior_focal_length || !camera2.has_prior_focal_length) 184 | continue; 185 | 186 | if (image_pair.config == colmap::TwoViewGeometry::CALIBRATED) { 187 | camera_counter[camera_id1].first++; 188 | camera_counter[camera_id2].first++; 189 | camera_counter[camera_id1].second++; 190 | camera_counter[camera_id2].second++; 191 | } else if (image_pair.config == colmap::TwoViewGeometry::UNCALIBRATED) { 192 | camera_counter[camera_id1].first++; 193 | camera_counter[camera_id2].first++; 194 | } 195 | } 196 | 197 | // Check the ratio of valid and invalid relative pair, if the majority of the 198 | // pairs are valid, then set the camera to valid 199 | std::unordered_map camera_validity; 200 | for (auto& [camera_id, counter] : camera_counter) { 201 | if (counter.first == 0) { 202 | camera_validity[camera_id] = false; 203 | } else if (counter.second * 1. / counter.first > 0.5) { 204 | camera_validity[camera_id] = true; 205 | } else { 206 | camera_validity[camera_id] = false; 207 | } 208 | } 209 | 210 | for (auto& [pair_id, image_pair] : view_graph.image_pairs) { 211 | if (image_pair.is_valid == false) continue; 212 | if (image_pair.config != colmap::TwoViewGeometry::UNCALIBRATED) continue; 213 | 214 | camera_t camera_id1 = images.at(image_pair.image_id1).camera_id; 215 | camera_t camera_id2 = images.at(image_pair.image_id2).camera_id; 216 | 217 | const Camera& camera1 = 218 | cameras.at(images.at(image_pair.image_id1).camera_id); 219 | const Camera& camera2 = 220 | cameras.at(images.at(image_pair.image_id2).camera_id); 221 | 222 | if (camera_validity[camera_id1] && camera_validity[camera_id2]) { 223 | image_pair.config = colmap::TwoViewGeometry::CALIBRATED; 224 | FundamentalFromMotionAndCameras( 225 | camera1, camera2, image_pair.cam2_from_cam1, &image_pair.F); 226 | } 227 | } 228 | } 229 | 230 | // Decompose the relative camera postion from the camera config 231 | void ViewGraphManipulater::DecomposeRelPose( 232 | ViewGraph& view_graph, 233 | std::unordered_map& cameras, 234 | std::unordered_map& images) { 235 | std::vector image_pair_ids; 236 | for (auto& [pair_id, image_pair] : view_graph.image_pairs) { 237 | if (image_pair.is_valid == false) continue; 238 | if (!cameras[images[image_pair.image_id1].camera_id] 239 | .has_prior_focal_length || 240 | !cameras[images[image_pair.image_id2].camera_id].has_prior_focal_length) 241 | continue; 242 | image_pair_ids.push_back(pair_id); 243 | } 244 | LOG(INFO) << "Decompose relative pose for " << image_pair_ids.size() 245 | << " pairs"; 246 | 247 | #pragma omp parallel for 248 | for (size_t idx = 0; idx < image_pair_ids.size(); idx++) { 249 | ImagePair& image_pair = view_graph.image_pairs.at(image_pair_ids[idx]); 250 | image_t image_id1 = image_pair.image_id1; 251 | image_t image_id2 = image_pair.image_id2; 252 | 253 | camera_t camera_id1 = images.at(image_id1).camera_id; 254 | camera_t camera_id2 = images.at(image_id2).camera_id; 255 | 256 | // Use the two-view geometry to re-estimate the relative pose 257 | colmap::TwoViewGeometry two_view_geometry; 258 | two_view_geometry.E = image_pair.E; 259 | two_view_geometry.F = image_pair.F; 260 | two_view_geometry.H = image_pair.H; 261 | two_view_geometry.config = image_pair.config; 262 | 263 | colmap::EstimateTwoViewGeometryPose(cameras[camera_id1], 264 | images[image_id1].features, 265 | cameras[camera_id2], 266 | images[image_id2].features, 267 | &two_view_geometry); 268 | 269 | // if it planar, then use the estimated relative pose 270 | if (image_pair.config == colmap::TwoViewGeometry::PLANAR && 271 | cameras[camera_id1].has_prior_focal_length && 272 | cameras[camera_id2].has_prior_focal_length) { 273 | image_pair.config = colmap::TwoViewGeometry::CALIBRATED; 274 | continue; 275 | } else if (!(cameras[camera_id1].has_prior_focal_length && 276 | cameras[camera_id2].has_prior_focal_length)) 277 | continue; 278 | 279 | image_pair.config = two_view_geometry.config; 280 | image_pair.cam2_from_cam1 = two_view_geometry.cam2_from_cam1; 281 | 282 | if (image_pair.cam2_from_cam1.translation.norm() > EPS) { 283 | image_pair.cam2_from_cam1.translation = 284 | image_pair.cam2_from_cam1.translation.normalized(); 285 | } 286 | } 287 | size_t counter = 0; 288 | for (size_t idx = 0; idx < image_pair_ids.size(); idx++) { 289 | ImagePair& image_pair = view_graph.image_pairs.at(image_pair_ids[idx]); 290 | if (image_pair.config != colmap::TwoViewGeometry::CALIBRATED && 291 | image_pair.config != colmap::TwoViewGeometry::PLANAR_OR_PANORAMIC) 292 | counter++; 293 | } 294 | LOG(INFO) << "Decompose relative pose done. " << counter 295 | << " pairs are pure rotation"; 296 | } 297 | 298 | } // namespace glomap 299 | --------------------------------------------------------------------------------