From f7ea57d25fec469fba6fe8533b0adbe41d41190a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Johannes=20Sch=C3=B6nberger?= Date: Wed, 7 Aug 2024 14:19:42 +0200 Subject: [PATCH 01/45] Fixes for Windows build (#45) * Fixes for Windows build * f --- CMakeLists.txt | 16 ++++++++++++++++ cmake/FindDependencies.cmake | 8 ++++++++ glomap/estimators/global_rotation_averaging.cc | 6 +++--- glomap/estimators/gravity_refinement.cc | 6 +++--- glomap/estimators/gravity_refinement.h | 2 +- glomap/estimators/relpose_estimation.cc | 18 +++++++++--------- glomap/math/rigid3d.cc | 10 +++++----- glomap/processors/image_undistorter.cc | 16 +++++----------- glomap/processors/view_graph_manipulation.cc | 7 ++++--- 9 files changed, 54 insertions(+), 35 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 76a31e34..564b027f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -39,4 +39,20 @@ else() message(STATUS "Disabling ccache support") endif() +if(CMAKE_CXX_COMPILER_ID STREQUAL "MSVC") + # Some fixes for the Glog library. + add_definitions("-DGLOG_USE_GLOG_EXPORT") + add_definitions("-DGLOG_NO_ABBREVIATED_SEVERITIES") + add_definitions("-DGL_GLEXT_PROTOTYPES") + add_definitions("-DNOMINMAX") + add_compile_options(/EHsc) + # Disable warning: 'initializing': conversion from 'X' to 'Y', possible loss of data + add_compile_options(/wd4244 /wd4267 /wd4305) + # Enable object level parallel builds in Visual Studio. + add_compile_options(/MP) + if("${CMAKE_BUILD_TYPE}" STREQUAL "Debug" OR "${CMAKE_BUILD_TYPE}" STREQUAL "RelWithDebInfo") + add_compile_options(/bigobj) + endif() +endif() + add_subdirectory(glomap) diff --git a/cmake/FindDependencies.cmake b/cmake/FindDependencies.cmake index 1a74717b..fbd65171 100644 --- a/cmake/FindDependencies.cmake +++ b/cmake/FindDependencies.cmake @@ -39,3 +39,11 @@ if (OPENMP_ENABLED) message(STATUS "Enabling OpenMP") find_package(OpenMP REQUIRED) endif() + +find_package(SuiteSparse QUIET) +if(SuiteSparse_FOUND) + set(SuiteSparse_CHOLMOD_INCLUDE_DIR "${SUITESPARSE_INCLUDE_DIRS}/suitesparse") + set(SuiteSparse_CHOLMOD_LIBRARY SuiteSparse::cholmod) +else() + message(STATUS "SuiteSparse not found, assuming Ceres provides SuiteSparse") +endif() diff --git a/glomap/estimators/global_rotation_averaging.cc b/glomap/estimators/global_rotation_averaging.cc index b3edaad8..429a6bdc 100644 --- a/glomap/estimators/global_rotation_averaging.cc +++ b/glomap/estimators/global_rotation_averaging.cc @@ -12,9 +12,9 @@ namespace { double RelAngleError(double angle_12, double angle_1, double angle_2) { double est = (angle_2 - angle_1) - angle_12; - while (est >= M_PI) est -= TWO_PI; + while (est >= EIGEN_PI) est -= TWO_PI; - while (est < -M_PI) est += TWO_PI; + while (est < -EIGEN_PI) est += TWO_PI; return est; } @@ -301,7 +301,7 @@ bool RotationEstimator::SolveL1Regression( tangent_space_step_.setZero(); l1_solver.Solve(tangent_space_residual_, &tangent_space_step_); - if (tangent_space_step_.array().isNaN().sum() > 0) { + if (tangent_space_step_.array().isNaN().any()) { LOG(ERROR) << "nan error"; iteration++; return false; diff --git a/glomap/estimators/gravity_refinement.cc b/glomap/estimators/gravity_refinement.cc index c4b2e2d0..6015d66b 100644 --- a/glomap/estimators/gravity_refinement.cc +++ b/glomap/estimators/gravity_refinement.cc @@ -116,14 +116,14 @@ void GravityRefiner::IdentifyErrorProneGravity( const auto& image2 = images.at(image_pair.image_id2); if (image1.gravity_info.has_gravity && image2.gravity_info.has_gravity) { // Calculate the gravity aligned relative rotation - Eigen::Matrix3d R_rel = + const Eigen::Matrix3d R_rel = image2.gravity_info.GetRAlign().transpose() * image_pair.cam2_from_cam1.rotation.toRotationMatrix() * image1.gravity_info.GetRAlign(); // Convert it to the closest upright rotation - Eigen::Matrix3d R_rel_up = AngleToRotUp(RotUpToAngle(R_rel)); + const Eigen::Matrix3d R_rel_up = AngleToRotUp(RotUpToAngle(R_rel)); - double angle = CalcAngle(R_rel, R_rel_up); + const double angle = CalcAngle(R_rel, R_rel_up); // increment the total count image_counter[image_pair.image_id1].second++; diff --git a/glomap/estimators/gravity_refinement.h b/glomap/estimators/gravity_refinement.h index 40a3b37f..581b434e 100644 --- a/glomap/estimators/gravity_refinement.h +++ b/glomap/estimators/gravity_refinement.h @@ -13,7 +13,7 @@ struct GravityRefinerOptions : public OptimizationBaseOptions { // The minimal ratio that the gravity vector should be consistent with double max_outlier_ratio = 0.5; // The maximum allowed angle error in degree - bool max_gravity_error = 1.; + double max_gravity_error = 1.; // Only refine the gravity of the images with more than min_neighbors int min_num_neighbors = 7; diff --git a/glomap/estimators/relpose_estimation.cc b/glomap/estimators/relpose_estimation.cc index 1b9b9626..89bdab3a 100644 --- a/glomap/estimators/relpose_estimation.cc +++ b/glomap/estimators/relpose_estimation.cc @@ -18,20 +18,20 @@ void EstimateRelativePoses(ViewGraph& view_graph, std::vector points2D_1, points2D_2; std::vector inliers; - const size_t kNumChunks = 10; - size_t inverval = std::ceil(valid_pair_ids.size() / kNumChunks); - LOG(INFO) << "Estimating relative pose for " << valid_pair_ids.size() - << " pairs"; - for (size_t chunk_id = 0; chunk_id < kNumChunks; chunk_id++) { + const int64_t num_image_pairs = valid_pair_ids.size(); + const int64_t kNumChunks = 10; + const int64_t inverval = std::ceil(num_image_pairs / kNumChunks); + LOG(INFO) << "Estimating relative pose for " << num_image_pairs << " pairs"; + for (int64_t chunk_id = 0; chunk_id < kNumChunks; chunk_id++) { std::cout << "\r Estimating relative pose: " << chunk_id * kNumChunks << "%" << std::flush; - const size_t start = chunk_id * inverval; - const size_t end = - std::min((chunk_id + 1) * inverval, valid_pair_ids.size()); + const int64_t start = chunk_id * inverval; + const int64_t end = + std::min((chunk_id + 1) * inverval, num_image_pairs); #pragma omp parallel for schedule(dynamic) private( \ points2D_1, points2D_2, inliers) - for (size_t pair_idx = start; pair_idx < end; pair_idx++) { + for (int64_t pair_idx = start; pair_idx < end; pair_idx++) { ImagePair& image_pair = view_graph.image_pairs[valid_pair_ids[pair_idx]]; const Image& image1 = images[image_pair.image_id1]; const Image& image2 = images[image_pair.image_id2]; diff --git a/glomap/math/rigid3d.cc b/glomap/math/rigid3d.cc index 98869a5a..cc9f5c7f 100644 --- a/glomap/math/rigid3d.cc +++ b/glomap/math/rigid3d.cc @@ -11,7 +11,7 @@ double CalcAngle(const Rigid3d& pose1, const Rigid3d& pose2) { 2; cos_r = std::min(std::max(cos_r, -1.), 1.); - return std::acos(cos_r) * 180 / M_PI; + return std::acos(cos_r) * 180 / EIGEN_PI; } double CalcTrans(const Rigid3d& pose1, const Rigid3d& pose2) { @@ -23,7 +23,7 @@ double CalcTransAngle(const Rigid3d& pose1, const Rigid3d& pose2) { (pose1.translation.norm() * pose2.translation.norm()); cos_r = std::min(std::max(cos_r, -1.), 1.); - return std::acos(cos_r) * 180 / M_PI; + return std::acos(cos_r) * 180 / EIGEN_PI; } double CalcAngle(const Eigen::Matrix3d& rotation1, @@ -31,12 +31,12 @@ double CalcAngle(const Eigen::Matrix3d& rotation1, double cos_r = ((rotation1.transpose() * rotation2).trace() - 1) / 2; cos_r = std::min(std::max(cos_r, -1.), 1.); - return std::acos(cos_r) * 180 / M_PI; + return std::acos(cos_r) * 180 / EIGEN_PI; } -double DegToRad(double degree) { return degree * M_PI / 180; } +double DegToRad(double degree) { return degree * EIGEN_PI / 180; } -double RadToDeg(double radian) { return radian * 180 / M_PI; } +double RadToDeg(double radian) { return radian * 180 / EIGEN_PI; } Eigen::Vector3d Rigid3dToAngleAxis(const Rigid3d& pose) { Eigen::AngleAxis aa(pose.rotation); diff --git a/glomap/processors/image_undistorter.cc b/glomap/processors/image_undistorter.cc index ae397484..c83402ae 100644 --- a/glomap/processors/image_undistorter.cc +++ b/glomap/processors/image_undistorter.cc @@ -15,13 +15,10 @@ void UndistortImages(std::unordered_map& cameras, } LOG(INFO) << "Undistorting images.."; + const int num_images = image_ids.size(); #pragma omp parallel for - for (size_t i = 0; i < image_ids.size(); i++) { - Eigen::Vector2d pt_undist; - Eigen::Vector3d pt_undist_norm; - - image_t image_id = image_ids[i]; - Image& image = images[image_id]; + for (int image_idx = 0; image_idx < num_images; image_idx++) { + Image& image = images[image_ids[image_idx]]; int camera_id = image.camera_id; Camera& camera = cameras[camera_id]; @@ -33,11 +30,8 @@ void UndistortImages(std::unordered_map& cameras, image.features_undist.clear(); image.features_undist.reserve(num_points); for (int i = 0; i < num_points; i++) { - // Undistort point in image - pt_undist = camera.CamFromImg(image.features[i]); - - pt_undist_norm = pt_undist.homogeneous().normalized(); - image.features_undist.emplace_back(pt_undist_norm); + image.features_undist.emplace_back( + camera.CamFromImg(image.features[i]).homogeneous().normalized()); } } LOG(INFO) << "Image undistortion done"; diff --git a/glomap/processors/view_graph_manipulation.cc b/glomap/processors/view_graph_manipulation.cc index 4571d452..9373fbb2 100644 --- a/glomap/processors/view_graph_manipulation.cc +++ b/glomap/processors/view_graph_manipulation.cc @@ -241,11 +241,12 @@ void ViewGraphManipulater::DecomposeRelPose( continue; image_pair_ids.push_back(pair_id); } - LOG(INFO) << "Decompose relative pose for " << image_pair_ids.size() - << " pairs"; + + const int64_t num_image_pairs = image_pair_ids.size(); + LOG(INFO) << "Decompose relative pose for " << num_image_pairs << " pairs"; #pragma omp parallel for - for (size_t idx = 0; idx < image_pair_ids.size(); idx++) { + for (int64_t idx = 0; idx < num_image_pairs; idx++) { ImagePair& image_pair = view_graph.image_pairs.at(image_pair_ids[idx]); image_t image_id1 = image_pair.image_id1; image_t image_id2 = image_pair.image_id2; From f8ea0563deb146bf98cd1acdfe08d10ba5aa6628 Mon Sep 17 00:00:00 2001 From: Mark Shachkov Date: Wed, 7 Aug 2024 14:20:41 +0200 Subject: [PATCH 02/45] Abort early in case of database with no matches (#41) --- glomap/exe/global_mapper.cc | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/glomap/exe/global_mapper.cc b/glomap/exe/global_mapper.cc index b81c34b7..7e4deac3 100644 --- a/glomap/exe/global_mapper.cc +++ b/glomap/exe/global_mapper.cc @@ -69,6 +69,11 @@ int RunMapper(int argc, char** argv) { const colmap::Database database(database_path); ConvertDatabaseToGlomap(database, view_graph, cameras, images); + if (view_graph.image_pairs.empty()) { + LOG(ERROR) << "Can't continue without image pairs"; + return EXIT_FAILURE; + } + GlobalMapper global_mapper(*options.mapper); // Main solver From 5fe42fc2735d7fd8973628ffa68bb18a921c3f05 Mon Sep 17 00:00:00 2001 From: Linfei Pan <36349740+lpanaf@users.noreply.github.com> Date: Thu, 8 Aug 2024 15:40:25 +0200 Subject: [PATCH 03/45] f (#47) --- glomap/glomap.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/glomap/glomap.cc b/glomap/glomap.cc index d2a5230a..aa300a19 100644 --- a/glomap/glomap.cc +++ b/glomap/glomap.cc @@ -33,6 +33,7 @@ int ShowHelp( int main(int argc, char** argv) { colmap::InitializeGlog(argv); + FLAGS_alsologtostderr = true; std::vector> commands; commands.emplace_back("mapper", &glomap::RunMapper); From 5a42cd10a08a7cffa3cd17b6e354c8dd183d6f09 Mon Sep 17 00:00:00 2001 From: Mark Shachkov Date: Thu, 8 Aug 2024 15:42:48 +0200 Subject: [PATCH 04/45] Compute reprojection errors during conversion to colmap (#40) --- glomap/io/colmap_converter.cc | 2 ++ 1 file changed, 2 insertions(+) diff --git a/glomap/io/colmap_converter.cc b/glomap/io/colmap_converter.cc index 6a838c22..eceaead4 100644 --- a/glomap/io/colmap_converter.cc +++ b/glomap/io/colmap_converter.cc @@ -106,6 +106,8 @@ void ConvertGlomapToColmap(const std::unordered_map& cameras, reconstruction.AddImage(std::move(image_colmap)); } + + reconstruction.UpdatePoint3DErrors(); } void ConvertColmapToGlomap(const colmap::Reconstruction& reconstruction, From 55e3abd41d65da9eadea1cf94f37500f8551fabb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Johannes=20Sch=C3=B6nberger?= Date: Thu, 8 Aug 2024 23:03:32 +0200 Subject: [PATCH 05/45] Add CI build for Windows (#46) * Update README.md * update CI pipelines * add install-ccache.ps1 * d * d * directly find suitesparse (before assume Ceres achieve this) * try removing the direct linking of CHOLMOD library * add vcpkg.json * remove unnecessary features * d * remove requirement for cuda * d * d * d * d * D * d * d * d * d * d * d * d * d * d * d * d * d * d * d * d * d * d * d --------- Co-authored-by: Linfei Pan <36349740+lpanaf@users.noreply.github.com> Co-authored-by: lpanaf --- .github/workflows/install-ccache.ps1 | 38 ++ .github/workflows/{ci.yml => ubuntu.yml} | 2 - .github/workflows/windows.yml | 142 ++++++ CMakeLists.txt | 1 + cmake/FindDependencies.cmake | 48 +- cmake/FindGlog.cmake | 118 +++++ cmake/FindMETIS.cmake | 110 +++++ cmake/FindSuiteSparse.cmake | 537 +++++++++++++++++++++++ glomap/CMakeLists.txt | 9 +- scripts/shell/enter_vs_dev_shell.ps1 | 25 ++ vcpkg.json | 46 ++ 11 files changed, 1045 insertions(+), 31 deletions(-) create mode 100644 .github/workflows/install-ccache.ps1 rename .github/workflows/{ci.yml => ubuntu.yml} (97%) create mode 100644 .github/workflows/windows.yml create mode 100644 cmake/FindGlog.cmake create mode 100644 cmake/FindMETIS.cmake create mode 100644 cmake/FindSuiteSparse.cmake create mode 100644 scripts/shell/enter_vs_dev_shell.ps1 create mode 100644 vcpkg.json diff --git a/.github/workflows/install-ccache.ps1 b/.github/workflows/install-ccache.ps1 new file mode 100644 index 00000000..f56a9533 --- /dev/null +++ b/.github/workflows/install-ccache.ps1 @@ -0,0 +1,38 @@ +[CmdletBinding()] +param ( + [Parameter(Mandatory = $true)] + [string] $Destination +) + +$version = "4.8" +$folder = "ccache-$version-windows-x86_64" +$url = "https://github.com/ccache/ccache/releases/download/v$version/$folder.zip" +$expectedSha256 = "A2B3BAB4BB8318FFC5B3E4074DC25636258BC7E4B51261F7D9BEF8127FDA8309" + +$ErrorActionPreference = "Stop" + +try { + New-Item -Path "$Destination" -ItemType Container -ErrorAction SilentlyContinue + + Write-Host "Download CCache" + $zipFilePath = Join-Path "$env:TEMP" "$folder.zip" + Invoke-WebRequest -Uri $url -UseBasicParsing -OutFile "$zipFilePath" -MaximumRetryCount 3 + + $hash = Get-FileHash $zipFilePath -Algorithm "sha256" + if ($hash.Hash -ne $expectedSha256) { + throw "File $Path hash $hash.Hash did not match expected hash $expectedHash" + } + + Write-Host "Unzip CCache" + Expand-Archive -Path "$zipFilePath" -DestinationPath "$env:TEMP" + + Write-Host "Move CCache" + Move-Item -Force "$env:TEMP/$folder/ccache.exe" "$Destination" + Remove-Item "$zipFilePath" + Remove-Item -Recurse "$env:TEMP/$folder" +} +catch { + Write-Host "Installation failed with an error" + $_.Exception | Format-List + exit -1 +} diff --git a/.github/workflows/ci.yml b/.github/workflows/ubuntu.yml similarity index 97% rename from .github/workflows/ci.yml rename to .github/workflows/ubuntu.yml index 888a383a..455e8d83 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ubuntu.yml @@ -162,8 +162,6 @@ jobs: -DCMAKE_BUILD_TYPE=${{ matrix.config.cmakeBuildType }} \ -DCMAKE_INSTALL_PREFIX=./install \ -DCMAKE_CUDA_ARCHITECTURES=50 \ - -DSuiteSparse_CHOLMOD_LIBRARY="/usr/lib/x86_64-linux-gnu/libcholmod.so" \ - -DSuiteSparse_CHOLMOD_INCLUDE_DIR="/usr/include/suitesparse" \ -DTESTS_ENABLED=ON \ -DASAN_ENABLED=${{ matrix.config.asanEnabled }} ninja -k 10000 diff --git a/.github/workflows/windows.yml b/.github/workflows/windows.yml new file mode 100644 index 00000000..74e1fe5b --- /dev/null +++ b/.github/workflows/windows.yml @@ -0,0 +1,142 @@ +name: Windows + +on: + push: + branches: + - main + pull_request: + types: [ assigned, opened, synchronize, reopened ] + release: + types: [ published, edited ] + +jobs: + build: + name: ${{ matrix.config.os }} ${{ matrix.config.cmakeBuildType }} ${{ matrix.config.cudaEnabled && 'CUDA' || '' }} + runs-on: ${{ matrix.config.os }} + strategy: + matrix: + config: [ + { + os: windows-2019, + cmakeBuildType: Release, + cudaEnabled: false, + testsEnabled: true, + exportPackage: false, + }, + { + os: windows-2022, + cmakeBuildType: Release, + cudaEnabled: false, + testsEnabled: true, + exportPackage: true, + }, + ] + + env: + COMPILER_CACHE_VERSION: 1 + COMPILER_CACHE_DIR: ${{ github.workspace }}/compiler-cache + CCACHE_DIR: ${{ github.workspace }}/compiler-cache/ccache + CCACHE_BASEDIR: ${{ github.workspace }} + VCPKG_COMMIT_ID: e01906b2ba7e645a76ee021a19de616edc98d29f + VCPKG_BINARY_SOURCES: "clear;x-gha,readwrite" + + steps: + - uses: actions/checkout@v4 + + - name: Export GitHub Actions cache env + uses: actions/github-script@v7 + with: + script: | + core.exportVariable('ACTIONS_CACHE_URL', process.env.ACTIONS_CACHE_URL || ''); + core.exportVariable('ACTIONS_RUNTIME_TOKEN', process.env.ACTIONS_RUNTIME_TOKEN || ''); + + - name: Compiler cache + uses: actions/cache@v4 + id: cache-builds + with: + key: v${{ env.COMPILER_CACHE_VERSION }}-${{ matrix.config.os }}-${{ matrix.config.cmakeBuildType }}-${{ matrix.config.asanEnabled }}--${{ matrix.config.cudaEnabled }}-${{ github.run_id }}-${{ github.run_number }} + restore-keys: v${{ env.COMPILER_CACHE_VERSION }}-${{ matrix.config.os }}-${{ matrix.config.cmakeBuildType }}-${{ matrix.config.asanEnabled }}--${{ matrix.config.cudaEnabled }} + path: ${{ env.COMPILER_CACHE_DIR }} + + - name: Install ccache + shell: pwsh + run: | + New-Item -ItemType Directory -Force -Path "${{ env.CCACHE_DIR }}" + echo "${{ env.COMPILER_CACHE_DIR }}/bin" | Out-File -Encoding utf8 -Append -FilePath $env:GITHUB_PATH + + if (Test-Path -PathType Leaf "${{ env.COMPILER_CACHE_DIR }}/bin/ccache.exe") { + exit + } + + .github/workflows/install-ccache.ps1 -Destination "${{ env.COMPILER_CACHE_DIR }}/bin" + + - name: Install CMake and Ninja + uses: lukka/get-cmake@latest + + - name: Setup vcpkg + shell: pwsh + run: | + ./scripts/shell/enter_vs_dev_shell.ps1 + cd ${{ github.workspace }} + git clone https://github.com/microsoft/vcpkg + cd vcpkg + git reset --hard ${{ env.VCPKG_COMMIT_ID }} + ./bootstrap-vcpkg.bat + + - name: Configure and build + shell: pwsh + run: | + ./scripts/shell/enter_vs_dev_shell.ps1 + cd ${{ github.workspace }} + ./vcpkg/vcpkg.exe integrate install + mkdir build + cd build + cmake .. ` + -GNinja ` + -DCMAKE_MAKE_PROGRAM=ninja ` + -DCMAKE_BUILD_TYPE=Release ` + -DTESTS_ENABLED=ON ` + -DCUDA_ENABLED=OFF ` + -DGUI_ENABLED=OFF ` + -DCGAL_ENABLED=OFF ` + -DCMAKE_CUDA_ARCHITECTURES=all-major ` + -DCMAKE_TOOLCHAIN_FILE="${{ github.workspace }}/vcpkg/scripts/buildsystems/vcpkg.cmake" ` + -DVCPKG_TARGET_TRIPLET=x64-windows-release ` + -DCMAKE_INSTALL_PREFIX=install + ninja + + - name: Run tests + shell: pwsh + run: | + ./vcpkg/vcpkg.exe integrate install + cd build + ctest -E .+colmap_.* --output-on-failure + + - name: Export package + if: matrix.config.exportPackage + shell: pwsh + run: | + ./vcpkg/vcpkg.exe integrate install + + cd build + ninja install + + ../vcpkg/vcpkg.exe install ` + --triplet=x64-windows-release + ../vcpkg/vcpkg.exe export --raw --output-dir vcpkg_export --output glomap + cp vcpkg_export/glomap/installed/x64-windows/bin/*.dll install/bin + cp vcpkg_export/glomap/installed/x64-windows-release/bin/*.dll install/bin + + - name: Upload package + uses: actions/upload-artifact@v4 + if: ${{ matrix.config.exportPackage }} + with: + name: glomap-x64-windows + path: build/install + + - name: Cleanup compiler cache + shell: pwsh + run: | + ccache --show-stats --verbose + ccache --evict-older-than 1d + ccache --show-stats --verbose diff --git a/CMakeLists.txt b/CMakeLists.txt index 564b027f..5a252abe 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,6 +16,7 @@ option(FETCH_POSELIB "Whether to use PoseLib with FetchContent or with self-inst include(cmake/FindDependencies.cmake) +# Propagate options to vcpkg manifest. if (TESTS_ENABLED) enable_testing() endif() diff --git a/cmake/FindDependencies.cmake b/cmake/FindDependencies.cmake index fbd65171..824f8b07 100644 --- a/cmake/FindDependencies.cmake +++ b/cmake/FindDependencies.cmake @@ -1,3 +1,29 @@ +set(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake") + +find_package(Eigen3 3.4 REQUIRED) +find_package(SuiteSparse COMPONENTS CHOLMOD REQUIRED) +find_package(Ceres REQUIRED COMPONENTS SuiteSparse) +find_package(Boost REQUIRED) + +if(CMAKE_CXX_COMPILER_ID STREQUAL "MSVC") + find_package(Glog REQUIRED) + if(DEFINED glog_VERSION_MAJOR) + # Older versions of glog don't export version variables. + add_definitions("-DGLOG_VERSION_MAJOR=${glog_VERSION_MAJOR}") + add_definitions("-DGLOG_VERSION_MINOR=${glog_VERSION_MINOR}") + endif() +endif() + +if(TESTS_ENABLED) + message(STATUS "Enabling tests") + find_package(GTest REQUIRED) +endif() + +if (OPENMP_ENABLED) + message(STATUS "Enabling OpenMP") + find_package(OpenMP REQUIRED) +endif() + include(FetchContent) FetchContent_Declare(PoseLib GIT_REPOSITORY https://github.com/PoseLib/PoseLib.git @@ -25,25 +51,3 @@ else() find_package(COLMAP REQUIRED) endif() message(STATUS "Configuring COLMAP... done") - -find_package(Eigen3 3.4 REQUIRED) -find_package(Ceres REQUIRED COMPONENTS SuiteSparse) -find_package(Boost REQUIRED) - -if(TESTS_ENABLED) - message(STATUS "Enabling tests") - find_package(GTest REQUIRED) -endif() - -if (OPENMP_ENABLED) - message(STATUS "Enabling OpenMP") - find_package(OpenMP REQUIRED) -endif() - -find_package(SuiteSparse QUIET) -if(SuiteSparse_FOUND) - set(SuiteSparse_CHOLMOD_INCLUDE_DIR "${SUITESPARSE_INCLUDE_DIRS}/suitesparse") - set(SuiteSparse_CHOLMOD_LIBRARY SuiteSparse::cholmod) -else() - message(STATUS "SuiteSparse not found, assuming Ceres provides SuiteSparse") -endif() diff --git a/cmake/FindGlog.cmake b/cmake/FindGlog.cmake new file mode 100644 index 00000000..8bf2ea49 --- /dev/null +++ b/cmake/FindGlog.cmake @@ -0,0 +1,118 @@ +# Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of +# its contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + + +# Find package module for Glog library. +# +# The following variables are set by this module: +# +# GLOG_FOUND: TRUE if Glog is found. +# glog::glog: Imported target to link against. +# +# The following variables control the behavior of this module: +# +# GLOG_INCLUDE_DIR_HINTS: List of additional directories in which to +# search for Glog includes. +# GLOG_LIBRARY_DIR_HINTS: List of additional directories in which to +# search for Glog libraries. + +set(GLOG_INCLUDE_DIR_HINTS "" CACHE PATH "Glog include directory") +set(GLOG_LIBRARY_DIR_HINTS "" CACHE PATH "Glog library directory") + +unset(GLOG_FOUND) + +find_package(glog CONFIG QUIET) +if(TARGET glog::glog) + set(GLOG_FOUND TRUE) + message(STATUS "Found Glog") + message(STATUS " Target : glog::glog") +else() + # Older versions of glog don't come with a find_package config. + # Fall back to custom logic to find the library and remap to imported target. + + include(FindPackageHandleStandardArgs) + + list(APPEND GLOG_CHECK_INCLUDE_DIRS + /usr/local/include + /usr/local/homebrew/include + /opt/local/var/macports/software + /opt/local/include + /usr/include) + list(APPEND GLOG_CHECK_PATH_SUFFIXES + glog/include + glog/Include + Glog/include + Glog/Include + src/windows) + + list(APPEND GLOG_CHECK_LIBRARY_DIRS + /usr/local/lib + /usr/local/homebrew/lib + /opt/local/lib + /usr/lib) + list(APPEND GLOG_CHECK_LIBRARY_SUFFIXES + glog/lib + glog/Lib + Glog/lib + Glog/Lib + x64/Release) + + find_path(GLOG_INCLUDE_DIRS + NAMES + glog/logging.h + PATHS + ${GLOG_INCLUDE_DIR_HINTS} + ${GLOG_CHECK_INCLUDE_DIRS} + PATH_SUFFIXES + ${GLOG_CHECK_PATH_SUFFIXES}) + find_library(GLOG_LIBRARIES + NAMES + glog + libglog + PATHS + ${GLOG_LIBRARY_DIR_HINTS} + ${GLOG_CHECK_LIBRARY_DIRS} + PATH_SUFFIXES + ${GLOG_CHECK_LIBRARY_SUFFIXES}) + + if(GLOG_INCLUDE_DIRS AND GLOG_LIBRARIES) + set(GLOG_FOUND TRUE) + message(STATUS "Found Glog") + message(STATUS " Includes : ${GLOG_INCLUDE_DIRS}") + message(STATUS " Libraries : ${GLOG_LIBRARIES}") + endif() + + add_library(glog::glog INTERFACE IMPORTED) + target_include_directories(glog::glog INTERFACE ${GLOG_INCLUDE_DIRS}) + target_link_libraries(glog::glog INTERFACE ${GLOG_LIBRARIES}) +endif() + +if(NOT GLOG_FOUND AND GLOG_FIND_REQUIRED) + message(FATAL_ERROR "Could not find Glog") +endif() diff --git a/cmake/FindMETIS.cmake b/cmake/FindMETIS.cmake new file mode 100644 index 00000000..5f41792d --- /dev/null +++ b/cmake/FindMETIS.cmake @@ -0,0 +1,110 @@ +# +# Copyright (c) 2022 Sergiu Deitsch +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTMETISLAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. +# +#[=======================================================================[.rst: +Module for locating METIS +========================= + +Read-only variables: + +``METIS_FOUND`` + Indicates whether the library has been found. + +``METIS_VERSION`` + Indicates library version. + +Targets +------- + +``METIS::METIS`` + Specifies targets that should be passed to target_link_libararies. +]=======================================================================] + +include (FindPackageHandleStandardArgs) + +find_path (METIS_INCLUDE_DIR NAMES metis.h + PATH_SUFFIXES include + DOC "METIS include directory") +find_library (METIS_LIBRARY_DEBUG NAMES metis + PATH_SUFFIXES Debug + DOC "METIS debug library") +find_library (METIS_LIBRARY_RELEASE NAMES metis + PATH_SUFFIXES Release + DOC "METIS release library") + +if (METIS_LIBRARY_RELEASE) + if (METIS_LIBRARY_DEBUG) + set (METIS_LIBRARY debug ${METIS_LIBRARY_DEBUG} optimized + ${METIS_LIBRARY_RELEASE} CACHE STRING "METIS library") + else (METIS_LIBRARY_DEBUG) + set (METIS_LIBRARY ${METIS_LIBRARY_RELEASE} CACHE FILEPATH "METIS library") + endif (METIS_LIBRARY_DEBUG) +elseif (METIS_LIBRARY_DEBUG) + set (METIS_LIBRARY ${METIS_LIBRARY_DEBUG} CACHE FILEPATH "METIS library") +endif (METIS_LIBRARY_RELEASE) + +set (_METIS_VERSION_HEADER ${METIS_INCLUDE_DIR}/metis.h) + +if (EXISTS ${_METIS_VERSION_HEADER}) + file (READ ${_METIS_VERSION_HEADER} _METIS_VERSION_CONTENTS) + + string (REGEX REPLACE ".*#define METIS_VER_MAJOR[ \t]+([0-9]+).*" "\\1" + METIS_VERSION_MAJOR "${_METIS_VERSION_CONTENTS}") + string (REGEX REPLACE ".*#define METIS_VER_MINOR[ \t]+([0-9]+).*" "\\1" + METIS_VERSION_MINOR "${_METIS_VERSION_CONTENTS}") + string (REGEX REPLACE ".*#define METIS_VER_SUBMINOR[ \t]+([0-9]+).*" "\\1" + METIS_VERSION_PATCH "${_METIS_VERSION_CONTENTS}") + + set (METIS_VERSION + ${METIS_VERSION_MAJOR}.${METIS_VERSION_MINOR}.${METIS_VERSION_PATCH}) + set (METIS_VERSION_COMPONENTS 3) +endif (EXISTS ${_METIS_VERSION_HEADER}) + +mark_as_advanced (METIS_INCLUDE_DIR METIS_LIBRARY_DEBUG METIS_LIBRARY_RELEASE + METIS_LIBRARY) + +if (NOT TARGET METIS::METIS) + if (METIS_INCLUDE_DIR OR METIS_LIBRARY) + add_library (METIS::METIS IMPORTED UNKNOWN) + endif (METIS_INCLUDE_DIR OR METIS_LIBRARY) +endif (NOT TARGET METIS::METIS) + +if (METIS_INCLUDE_DIR) + set_property (TARGET METIS::METIS PROPERTY INTERFACE_INCLUDE_DIRECTORIES + ${METIS_INCLUDE_DIR}) +endif (METIS_INCLUDE_DIR) + +if (METIS_LIBRARY_RELEASE) + set_property (TARGET METIS::METIS PROPERTY IMPORTED_LOCATION_RELEASE + ${METIS_LIBRARY_RELEASE}) + set_property (TARGET METIS::METIS APPEND PROPERTY IMPORTED_CONFIGURATIONS + RELEASE) +endif (METIS_LIBRARY_RELEASE) + +if (METIS_LIBRARY_DEBUG) + set_property (TARGET METIS::METIS PROPERTY IMPORTED_LOCATION_DEBUG + ${METIS_LIBRARY_DEBUG}) + set_property (TARGET METIS::METIS APPEND PROPERTY IMPORTED_CONFIGURATIONS + DEBUG) +endif (METIS_LIBRARY_DEBUG) + +find_package_handle_standard_args (METIS REQUIRED_VARS + METIS_INCLUDE_DIR METIS_LIBRARY VERSION_VAR METIS_VERSION) diff --git a/cmake/FindSuiteSparse.cmake b/cmake/FindSuiteSparse.cmake new file mode 100644 index 00000000..bccd89fe --- /dev/null +++ b/cmake/FindSuiteSparse.cmake @@ -0,0 +1,537 @@ +# Ceres Solver - A fast non-linear least squares minimizer +# Copyright 2023 Google Inc. All rights reserved. +# http://ceres-solver.org/ +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# * Neither the name of Google Inc. nor the names of its contributors may be +# used to endorse or promote products derived from this software without +# specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# Author: alexs.mac@gmail.com (Alex Stewart) +# + +#[=======================================================================[.rst: +FindSuiteSparse +=============== + +Module for locating SuiteSparse libraries and its dependencies. + +This module defines the following variables: + +``SuiteSparse_FOUND`` + ``TRUE`` iff SuiteSparse and all dependencies have been found. + +``SuiteSparse_VERSION`` + Extracted from ``SuiteSparse_config.h`` (>= v4). + +``SuiteSparse_VERSION_MAJOR`` + Equal to 4 if ``SuiteSparse_VERSION`` = 4.2.1 + +``SuiteSparse_VERSION_MINOR`` + Equal to 2 if ``SuiteSparse_VERSION`` = 4.2.1 + +``SuiteSparse_VERSION_PATCH`` + Equal to 1 if ``SuiteSparse_VERSION`` = 4.2.1 + +The following variables control the behaviour of this module: + +``SuiteSparse_NO_CMAKE`` + Do not attempt to use the native SuiteSparse CMake package configuration. + + +Targets +------- + +The following targets define the SuiteSparse components searched for. + +``SuiteSparse::AMD`` + Symmetric Approximate Minimum Degree (AMD) + +``SuiteSparse::CAMD`` + Constrained Approximate Minimum Degree (CAMD) + +``SuiteSparse::COLAMD`` + Column Approximate Minimum Degree (COLAMD) + +``SuiteSparse::CCOLAMD`` + Constrained Column Approximate Minimum Degree (CCOLAMD) + +``SuiteSparse::CHOLMOD`` + Sparse Supernodal Cholesky Factorization and Update/Downdate (CHOLMOD) + +``SuiteSparse::Partition`` + CHOLMOD with METIS support + +``SuiteSparse::SPQR`` + Multifrontal Sparse QR (SuiteSparseQR) + +``SuiteSparse::Config`` + Common configuration for all but CSparse (SuiteSparse version >= 4). + +Optional SuiteSparse dependencies: + +``METIS::METIS`` + Serial Graph Partitioning and Fill-reducing Matrix Ordering (METIS) +]=======================================================================] + +if (NOT SuiteSparse_NO_CMAKE) + find_package (SuiteSparse NO_MODULE QUIET) +endif (NOT SuiteSparse_NO_CMAKE) + +if (SuiteSparse_FOUND) + return () +endif (SuiteSparse_FOUND) + +# Push CMP0057 to enable support for IN_LIST, when cmake_minimum_required is +# set to <3.3. +cmake_policy (PUSH) +cmake_policy (SET CMP0057 NEW) + +if (NOT SuiteSparse_FIND_COMPONENTS) + set (SuiteSparse_FIND_COMPONENTS + AMD + CAMD + CCOLAMD + CHOLMOD + COLAMD + SPQR + ) + + foreach (component IN LISTS SuiteSparse_FIND_COMPONENTS) + set (SuiteSparse_FIND_REQUIRED_${component} TRUE) + endforeach (component IN LISTS SuiteSparse_FIND_COMPONENTS) +endif (NOT SuiteSparse_FIND_COMPONENTS) + +# Assume SuiteSparse was found and set it to false only if third-party +# dependencies could not be located. SuiteSparse components are handled by +# FindPackageHandleStandardArgs HANDLE_COMPONENTS option. +set (SuiteSparse_FOUND TRUE) + +include (CheckLibraryExists) +include (CheckSymbolExists) +include (CMakePushCheckState) + +# Config is a base component and thus always required +set (SuiteSparse_IMPLICIT_COMPONENTS Config) + +# CHOLMOD depends on AMD, CAMD, CCOLAMD, and COLAMD. +if (CHOLMOD IN_LIST SuiteSparse_FIND_COMPONENTS) + list (APPEND SuiteSparse_IMPLICIT_COMPONENTS AMD CAMD CCOLAMD COLAMD) +endif (CHOLMOD IN_LIST SuiteSparse_FIND_COMPONENTS) + +# SPQR depends on CHOLMOD. +if (SPQR IN_LIST SuiteSparse_FIND_COMPONENTS) + list (APPEND SuiteSparse_IMPLICIT_COMPONENTS CHOLMOD) +endif (SPQR IN_LIST SuiteSparse_FIND_COMPONENTS) + +# Implicit components are always required +foreach (component IN LISTS SuiteSparse_IMPLICIT_COMPONENTS) + set (SuiteSparse_FIND_REQUIRED_${component} TRUE) +endforeach (component IN LISTS SuiteSparse_IMPLICIT_COMPONENTS) + +list (APPEND SuiteSparse_FIND_COMPONENTS ${SuiteSparse_IMPLICIT_COMPONENTS}) + +# Do not list components multiple times. +list (REMOVE_DUPLICATES SuiteSparse_FIND_COMPONENTS) + +# Reset CALLERS_CMAKE_FIND_LIBRARY_PREFIXES to its value when +# FindSuiteSparse was invoked. +macro(SuiteSparse_RESET_FIND_LIBRARY_PREFIX) + if (MSVC) + set(CMAKE_FIND_LIBRARY_PREFIXES "${CALLERS_CMAKE_FIND_LIBRARY_PREFIXES}") + endif (MSVC) +endmacro(SuiteSparse_RESET_FIND_LIBRARY_PREFIX) + +# Called if we failed to find SuiteSparse or any of it's required dependencies, +# unsets all public (designed to be used externally) variables and reports +# error message at priority depending upon [REQUIRED/QUIET/] argument. +macro(SuiteSparse_REPORT_NOT_FOUND REASON_MSG) + # Will be set to FALSE by find_package_handle_standard_args + unset (SuiteSparse_FOUND) + + # Do NOT unset SuiteSparse_REQUIRED_VARS here, as it is used by + # FindPackageHandleStandardArgs() to generate the automatic error message on + # failure which highlights which components are missing. + + suitesparse_reset_find_library_prefix() + + # Note _FIND_[REQUIRED/QUIETLY] variables defined by FindPackage() + # use the camelcase library name, not uppercase. + if (SuiteSparse_FIND_QUIETLY) + message(STATUS "Failed to find SuiteSparse - " ${REASON_MSG} ${ARGN}) + elseif (SuiteSparse_FIND_REQUIRED) + message(FATAL_ERROR "Failed to find SuiteSparse - " ${REASON_MSG} ${ARGN}) + else() + # Neither QUIETLY nor REQUIRED, use no priority which emits a message + # but continues configuration and allows generation. + message("-- Failed to find SuiteSparse - " ${REASON_MSG} ${ARGN}) + endif (SuiteSparse_FIND_QUIETLY) + + # Do not call return(), s/t we keep processing if not called with REQUIRED + # and report all missing components, rather than bailing after failing to find + # the first. +endmacro(SuiteSparse_REPORT_NOT_FOUND) + +# Handle possible presence of lib prefix for libraries on MSVC, see +# also SuiteSparse_RESET_FIND_LIBRARY_PREFIX(). +if (MSVC) + # Preserve the caller's original values for CMAKE_FIND_LIBRARY_PREFIXES + # s/t we can set it back before returning. + set(CALLERS_CMAKE_FIND_LIBRARY_PREFIXES "${CMAKE_FIND_LIBRARY_PREFIXES}") + # The empty string in this list is important, it represents the case when + # the libraries have no prefix (shared libraries / DLLs). + set(CMAKE_FIND_LIBRARY_PREFIXES "lib" "" "${CMAKE_FIND_LIBRARY_PREFIXES}") +endif (MSVC) + +# Additional suffixes to try appending to each search path. +list(APPEND SuiteSparse_CHECK_PATH_SUFFIXES + suitesparse) # Windows/Ubuntu + +# Wrappers to find_path/library that pass the SuiteSparse search hints/paths. +# +# suitesparse_find_component( [FILES name1 [name2 ...]] +# [LIBRARIES name1 [name2 ...]]) +macro(suitesparse_find_component COMPONENT) + include(CMakeParseArguments) + set(MULTI_VALUE_ARGS FILES LIBRARIES) + cmake_parse_arguments(SuiteSparse_FIND_COMPONENT_${COMPONENT} + "" "" "${MULTI_VALUE_ARGS}" ${ARGN}) + + set(SuiteSparse_${COMPONENT}_FOUND TRUE) + if (SuiteSparse_FIND_COMPONENT_${COMPONENT}_FILES) + find_path(SuiteSparse_${COMPONENT}_INCLUDE_DIR + NAMES ${SuiteSparse_FIND_COMPONENT_${COMPONENT}_FILES} + PATH_SUFFIXES ${SuiteSparse_CHECK_PATH_SUFFIXES}) + if (SuiteSparse_${COMPONENT}_INCLUDE_DIR) + message(STATUS "Found ${COMPONENT} headers in: " + "${SuiteSparse_${COMPONENT}_INCLUDE_DIR}") + mark_as_advanced(SuiteSparse_${COMPONENT}_INCLUDE_DIR) + else() + # Specified headers not found. + set(SuiteSparse_${COMPONENT}_FOUND FALSE) + if (SuiteSparse_FIND_REQUIRED_${COMPONENT}) + suitesparse_report_not_found( + "Did not find ${COMPONENT} header (required SuiteSparse component).") + else() + message(STATUS "Did not find ${COMPONENT} header (optional " + "SuiteSparse component).") + # Hide optional vars from CMake GUI even if not found. + mark_as_advanced(SuiteSparse_${COMPONENT}_INCLUDE_DIR) + endif() + endif() + endif() + + if (SuiteSparse_FIND_COMPONENT_${COMPONENT}_LIBRARIES) + find_library(SuiteSparse_${COMPONENT}_LIBRARY + NAMES ${SuiteSparse_FIND_COMPONENT_${COMPONENT}_LIBRARIES} + PATH_SUFFIXES ${SuiteSparse_CHECK_PATH_SUFFIXES}) + if (SuiteSparse_${COMPONENT}_LIBRARY) + message(STATUS "Found ${COMPONENT} library: ${SuiteSparse_${COMPONENT}_LIBRARY}") + mark_as_advanced(SuiteSparse_${COMPONENT}_LIBRARY) + else () + # Specified libraries not found. + set(SuiteSparse_${COMPONENT}_FOUND FALSE) + if (SuiteSparse_FIND_REQUIRED_${COMPONENT}) + suitesparse_report_not_found( + "Did not find ${COMPONENT} library (required SuiteSparse component).") + else() + message(STATUS "Did not find ${COMPONENT} library (optional SuiteSparse " + "dependency)") + # Hide optional vars from CMake GUI even if not found. + mark_as_advanced(SuiteSparse_${COMPONENT}_LIBRARY) + endif() + endif() + endif() + + # A component can be optional (given to OPTIONAL_COMPONENTS). However, if the + # component is implicit (must be always present, such as the Config component) + # assume it be required as well. + if (SuiteSparse_FIND_REQUIRED_${COMPONENT}) + list (APPEND SuiteSparse_REQUIRED_VARS SuiteSparse_${COMPONENT}_INCLUDE_DIR) + list (APPEND SuiteSparse_REQUIRED_VARS SuiteSparse_${COMPONENT}_LIBRARY) + endif (SuiteSparse_FIND_REQUIRED_${COMPONENT}) + + # Define the target only if the include directory and the library were found + if (SuiteSparse_${COMPONENT}_INCLUDE_DIR AND SuiteSparse_${COMPONENT}_LIBRARY) + if (NOT TARGET SuiteSparse::${COMPONENT}) + add_library(SuiteSparse::${COMPONENT} IMPORTED UNKNOWN) + endif (NOT TARGET SuiteSparse::${COMPONENT}) + + set_property(TARGET SuiteSparse::${COMPONENT} PROPERTY + INTERFACE_INCLUDE_DIRECTORIES ${SuiteSparse_${COMPONENT}_INCLUDE_DIR}) + set_property(TARGET SuiteSparse::${COMPONENT} PROPERTY + IMPORTED_LOCATION ${SuiteSparse_${COMPONENT}_LIBRARY}) + endif (SuiteSparse_${COMPONENT}_INCLUDE_DIR AND SuiteSparse_${COMPONENT}_LIBRARY) +endmacro() + +# Given the number of components of SuiteSparse, and to ensure that the +# automatic failure message generated by FindPackageHandleStandardArgs() +# when not all required components are found is helpful, we maintain a list +# of all variables that must be defined for SuiteSparse to be considered found. +unset(SuiteSparse_REQUIRED_VARS) + +# BLAS. +find_package(BLAS QUIET) +if (NOT BLAS_FOUND) + suitesparse_report_not_found( + "Did not find BLAS library (required for SuiteSparse).") +endif (NOT BLAS_FOUND) + +# LAPACK. +find_package(LAPACK QUIET) +if (NOT LAPACK_FOUND) + suitesparse_report_not_found( + "Did not find LAPACK library (required for SuiteSparse).") +endif (NOT LAPACK_FOUND) + +foreach (component IN LISTS SuiteSparse_FIND_COMPONENTS) + if (component STREQUAL Partition) + # Partition is a meta component that neither provides additional headers nor + # a separate library. It is strictly part of CHOLMOD. + continue () + endif (component STREQUAL Partition) + string (TOLOWER ${component} component_library) + + if (component STREQUAL "Config") + set (component_header SuiteSparse_config.h) + set (component_library suitesparseconfig) + elseif (component STREQUAL "SPQR") + set (component_header SuiteSparseQR.hpp) + else (component STREQUAL "SPQR") + set (component_header ${component_library}.h) + endif (component STREQUAL "Config") + + suitesparse_find_component(${component} + FILES ${component_header} + LIBRARIES ${component_library}) +endforeach (component IN LISTS SuiteSparse_FIND_COMPONENTS) + +if (TARGET SuiteSparse::SPQR) + # SuiteSparseQR may be compiled with Intel Threading Building Blocks, + # we assume that if TBB is installed, SuiteSparseQR was compiled with + # support for it, this will do no harm if it wasn't. + find_package(TBB QUIET) + if (TBB_FOUND) + message(STATUS "Found Intel Thread Building Blocks (TBB) library " + "(${TBB_VERSION_MAJOR}.${TBB_VERSION_MINOR} / ${TBB_INTERFACE_VERSION}) " + "include location: ${TBB_INCLUDE_DIRS}. Assuming SuiteSparseQR was " + "compiled with TBB.") + # Add the TBB libraries to the SuiteSparseQR libraries (the only + # libraries to optionally depend on TBB). + if (TARGET TBB::tbb) + # Native TBB package configuration provides an imported target. Use it if + # available. + set_property (TARGET SuiteSparse::SPQR APPEND PROPERTY + INTERFACE_LINK_LIBRARIES TBB::tbb) + else (TARGET TBB::tbb) + set_property (TARGET SuiteSparse::SPQR APPEND PROPERTY + INTERFACE_INCLUDE_DIRECTORIES ${TBB_INCLUDE_DIRS}) + set_property (TARGET SuiteSparse::SPQR APPEND PROPERTY + INTERFACE_LINK_LIBRARIES ${TBB_LIBRARIES}) + endif (TARGET TBB::tbb) + else (TBB_FOUND) + message(STATUS "Did not find Intel TBB library, assuming SuiteSparseQR was " + "not compiled with TBB.") + endif (TBB_FOUND) +endif (TARGET SuiteSparse::SPQR) + +check_library_exists(rt shm_open "" HAVE_LIBRT) + +if (TARGET SuiteSparse::Config) + # SuiteSparse_config (SuiteSparse version >= 4) requires librt library for + # timing by default when compiled on Linux or Unix, but not on OSX (which + # does not have librt). + if (HAVE_LIBRT) + message(STATUS "Adding librt to " + "SuiteSparse_config libraries (required on Linux & Unix [not OSX] if " + "SuiteSparse is compiled with timing).") + set_property (TARGET SuiteSparse::Config APPEND PROPERTY + INTERFACE_LINK_LIBRARIES $) + else (HAVE_LIBRT) + message(STATUS "Could not find librt, but found SuiteSparse_config, " + "assuming that SuiteSparse was compiled without timing.") + endif (HAVE_LIBRT) + + # Add BLAS and LAPACK as dependencies of SuiteSparse::Config for convenience + # given that all components depend on it. + if (BLAS_FOUND) + if (TARGET BLAS::BLAS) + set_property (TARGET SuiteSparse::Config APPEND PROPERTY + INTERFACE_LINK_LIBRARIES $) + else (TARGET BLAS::BLAS) + set_property (TARGET SuiteSparse::Config APPEND PROPERTY + INTERFACE_LINK_LIBRARIES ${BLAS_LIBRARIES}) + endif (TARGET BLAS::BLAS) + endif (BLAS_FOUND) + + if (LAPACK_FOUND) + if (TARGET LAPACK::LAPACK) + set_property (TARGET SuiteSparse::Config APPEND PROPERTY + INTERFACE_LINK_LIBRARIES $) + else (TARGET LAPACK::LAPACK) + set_property (TARGET SuiteSparse::Config APPEND PROPERTY + INTERFACE_LINK_LIBRARIES ${LAPACK_LIBRARIES}) + endif (TARGET LAPACK::LAPACK) + endif (LAPACK_FOUND) + + # SuiteSparse version >= 4. + set(SuiteSparse_VERSION_FILE + ${SuiteSparse_Config_INCLUDE_DIR}/SuiteSparse_config.h) + if (NOT EXISTS ${SuiteSparse_VERSION_FILE}) + suitesparse_report_not_found( + "Could not find file: ${SuiteSparse_VERSION_FILE} containing version " + "information for >= v4 SuiteSparse installs, but SuiteSparse_config was " + "found (only present in >= v4 installs).") + else (NOT EXISTS ${SuiteSparse_VERSION_FILE}) + file(READ ${SuiteSparse_VERSION_FILE} Config_CONTENTS) + + string(REGEX MATCH "#define SUITESPARSE_MAIN_VERSION[ \t]+([0-9]+)" + SuiteSparse_VERSION_LINE "${Config_CONTENTS}") + set (SuiteSparse_VERSION_MAJOR ${CMAKE_MATCH_1}) + + string(REGEX MATCH "#define SUITESPARSE_SUB_VERSION[ \t]+([0-9]+)" + SuiteSparse_VERSION_LINE "${Config_CONTENTS}") + set (SuiteSparse_VERSION_MINOR ${CMAKE_MATCH_1}) + + string(REGEX MATCH "#define SUITESPARSE_SUBSUB_VERSION[ \t]+([0-9]+)" + SuiteSparse_VERSION_LINE "${Config_CONTENTS}") + set (SuiteSparse_VERSION_PATCH ${CMAKE_MATCH_1}) + + unset (SuiteSparse_VERSION_LINE) + + # This is on a single line s/t CMake does not interpret it as a list of + # elements and insert ';' separators which would result in 4.;2.;1 nonsense. + set(SuiteSparse_VERSION + "${SuiteSparse_VERSION_MAJOR}.${SuiteSparse_VERSION_MINOR}.${SuiteSparse_VERSION_PATCH}") + + if (SuiteSparse_VERSION MATCHES "[0-9]+\\.[0-9]+\\.[0-9]+") + set(SuiteSparse_VERSION_COMPONENTS 3) + else (SuiteSparse_VERSION MATCHES "[0-9]+\\.[0-9]+\\.[0-9]+") + message (WARNING "Could not parse SuiteSparse_config.h: SuiteSparse " + "version will not be available") + + unset (SuiteSparse_VERSION) + unset (SuiteSparse_VERSION_MAJOR) + unset (SuiteSparse_VERSION_MINOR) + unset (SuiteSparse_VERSION_PATCH) + endif (SuiteSparse_VERSION MATCHES "[0-9]+\\.[0-9]+\\.[0-9]+") + endif (NOT EXISTS ${SuiteSparse_VERSION_FILE}) +endif (TARGET SuiteSparse::Config) + +# CHOLMOD requires AMD CAMD CCOLAMD COLAMD +if (TARGET SuiteSparse::CHOLMOD) + foreach (component IN ITEMS AMD CAMD CCOLAMD COLAMD) + if (TARGET SuiteSparse::${component}) + set_property (TARGET SuiteSparse::CHOLMOD APPEND PROPERTY + INTERFACE_LINK_LIBRARIES SuiteSparse::${component}) + else (TARGET SuiteSparse::${component}) + # Consider CHOLMOD not found if COLAMD cannot be found + set (SuiteSparse_CHOLMOD_FOUND FALSE) + endif (TARGET SuiteSparse::${component}) + endforeach (component IN ITEMS AMD CAMD CCOLAMD COLAMD) +endif (TARGET SuiteSparse::CHOLMOD) + +# SPQR requires CHOLMOD +if (TARGET SuiteSparse::SPQR) + if (TARGET SuiteSparse::CHOLMOD) + set_property (TARGET SuiteSparse::SPQR APPEND PROPERTY + INTERFACE_LINK_LIBRARIES SuiteSparse::CHOLMOD) + else (TARGET SuiteSparse::CHOLMOD) + # Consider SPQR not found if CHOLMOD cannot be found + set (SuiteSparse_SQPR_FOUND FALSE) + endif (TARGET SuiteSparse::CHOLMOD) +endif (TARGET SuiteSparse::SPQR) + +# Add SuiteSparse::Config as dependency to all components +if (TARGET SuiteSparse::Config) + foreach (component IN LISTS SuiteSparse_FIND_COMPONENTS) + if (component STREQUAL Config) + continue () + endif (component STREQUAL Config) + + if (TARGET SuiteSparse::${component}) + set_property (TARGET SuiteSparse::${component} APPEND PROPERTY + INTERFACE_LINK_LIBRARIES SuiteSparse::Config) + endif (TARGET SuiteSparse::${component}) + endforeach (component IN LISTS SuiteSparse_FIND_COMPONENTS) +endif (TARGET SuiteSparse::Config) + +# Check whether CHOLMOD was compiled with METIS support. The check can be +# performed only after the main components have been set up. +if (TARGET SuiteSparse::CHOLMOD) + # NOTE If SuiteSparse was compiled as a static library we'll need to link + # against METIS already during the check. Otherwise, the check can fail due to + # undefined references even though SuiteSparse was compiled with METIS. + find_package (METIS) + + if (TARGET METIS::METIS) + cmake_push_check_state (RESET) + set (CMAKE_REQUIRED_LIBRARIES SuiteSparse::CHOLMOD METIS::METIS) + check_symbol_exists (cholmod_metis cholmod.h SuiteSparse_CHOLMOD_USES_METIS) + cmake_pop_check_state () + + if (SuiteSparse_CHOLMOD_USES_METIS) + set_property (TARGET SuiteSparse::CHOLMOD APPEND PROPERTY + INTERFACE_LINK_LIBRARIES $) + + # Provide the SuiteSparse::Partition component whose availability indicates + # that CHOLMOD was compiled with the Partition module. + if (NOT TARGET SuiteSparse::Partition) + add_library (SuiteSparse::Partition IMPORTED INTERFACE) + endif (NOT TARGET SuiteSparse::Partition) + + set_property (TARGET SuiteSparse::Partition APPEND PROPERTY + INTERFACE_LINK_LIBRARIES SuiteSparse::CHOLMOD) + endif (SuiteSparse_CHOLMOD_USES_METIS) + endif (TARGET METIS::METIS) +endif (TARGET SuiteSparse::CHOLMOD) + +# We do not use suitesparse_find_component to find Partition and therefore must +# handle the availability in an extra step. +if (TARGET SuiteSparse::Partition) + set (SuiteSparse_Partition_FOUND TRUE) +else (TARGET SuiteSparse::Partition) + set (SuiteSparse_Partition_FOUND FALSE) +endif (TARGET SuiteSparse::Partition) + +suitesparse_reset_find_library_prefix() + +# Handle REQUIRED and QUIET arguments to FIND_PACKAGE +include(FindPackageHandleStandardArgs) +if (SuiteSparse_FOUND) + find_package_handle_standard_args(SuiteSparse + REQUIRED_VARS ${SuiteSparse_REQUIRED_VARS} + VERSION_VAR SuiteSparse_VERSION + FAIL_MESSAGE "Failed to find some/all required components of SuiteSparse." + HANDLE_COMPONENTS) +else (SuiteSparse_FOUND) + # Do not pass VERSION_VAR to FindPackageHandleStandardArgs() if we failed to + # find SuiteSparse to avoid a confusing autogenerated failure message + # that states 'not found (missing: FOO) (found version: x.y.z)'. + find_package_handle_standard_args(SuiteSparse + REQUIRED_VARS ${SuiteSparse_REQUIRED_VARS} + FAIL_MESSAGE "Failed to find some/all required components of SuiteSparse." + HANDLE_COMPONENTS) +endif (SuiteSparse_FOUND) + +# Pop CMP0057. +cmake_policy (POP) diff --git a/glomap/CMakeLists.txt b/glomap/CMakeLists.txt index c71b966a..a5d82447 100644 --- a/glomap/CMakeLists.txt +++ b/glomap/CMakeLists.txt @@ -79,15 +79,10 @@ target_link_libraries( PUBLIC Eigen3::Eigen Ceres::ceres + SuiteSparse::CHOLMOD ${BOOST_LIBRARIES} - ${SuiteSparse_CHOLMOD_LIBRARY} -) -target_include_directories( - glomap - PUBLIC - .. - ${SuiteSparse_CHOLMOD_INCLUDE_DIR} ) +target_include_directories(glomap PUBLIC ..) if(OPENMP_FOUND) target_link_libraries(glomap PUBLIC OpenMP::OpenMP_CXX) diff --git a/scripts/shell/enter_vs_dev_shell.ps1 b/scripts/shell/enter_vs_dev_shell.ps1 new file mode 100644 index 00000000..9396c5c6 --- /dev/null +++ b/scripts/shell/enter_vs_dev_shell.ps1 @@ -0,0 +1,25 @@ +if (!$env:VisualStudioDevShell) { + $vswhere = "${Env:ProgramFiles(x86)}/Microsoft Visual Studio/Installer/vswhere.exe" + if (!(Test-Path $vswhere)) { + throw "Failed to find vswhere.exe" + } + + & $vswhere -latest -format json + $vsInstance = & $vswhere -latest -format json | ConvertFrom-Json + if ($LASTEXITCODE) { + throw "vswhere.exe returned exit code $LASTEXITCODE" + } + + Import-Module "$($vsInstance.installationPath)/Common7/Tools/Microsoft.VisualStudio.DevShell.dll" + $prevCwd = Get-Location + try { + Enter-VsDevShell $vsInstance.instanceId -DevCmdArguments "-no_logo -host_arch=amd64 -arch=amd64" + } catch { + Write-Host $_ + Write-Error "Failed to enter Visual Studio Dev Shell" + exit 1 + } + Set-Location $prevCwd + + $env:VisualStudioDevShell = $true +} diff --git a/vcpkg.json b/vcpkg.json new file mode 100644 index 00000000..4ec57780 --- /dev/null +++ b/vcpkg.json @@ -0,0 +1,46 @@ +{ + "name": "glomap", + "description": "GLOMAP is a general purpose global structure-from-motion pipeline for image-based reconstruction. GLOMAP requires a COLMAP database as input and outputs a COLMAP sparse reconstruction. As compared to COLMAP, this project provides a much more efficient and scalable reconstruction process, typically 1-2 orders of magnitude faster, with on-par or superior reconstruction quality.", + "homepage": "https://github.com/colmap/glomap", + "license": "BSD-3-Clause", + "supports": "(linux | (windows & !static) | osx) & (x86 | x64 | arm64)", + "dependencies": [ + "boost-algorithm", + "boost-filesystem", + "boost-graph", + "boost-heap", + "boost-program-options", + "boost-property-map", + "boost-property-tree", + { + "name": "ceres", + "features": [ + "lapack", + "suitesparse" + ] + }, + "eigen3", + "flann", + "freeimage", + "gflags", + "glog", + { + "name": "jasper", + "default-features": false + }, + "metis", + "sqlite3", + { + "name": "vcpkg-cmake", + "host": true + }, + { + "name": "vcpkg-cmake-config", + "host": true + }, + "gtest", + "suitesparse" + ], + "features": { + } +} From 0b9b595c3e6fd618eca6a6a28630639c62b94f63 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Johannes=20Sch=C3=B6nberger?= Date: Fri, 9 Aug 2024 13:21:49 +0200 Subject: [PATCH 06/45] Update version to 1.0.0 (#50) --- CMakeLists.txt | 2 +- glomap/version.h.in | 9 --------- 2 files changed, 1 insertion(+), 10 deletions(-) delete mode 100644 glomap/version.h.in diff --git a/CMakeLists.txt b/CMakeLists.txt index 5a252abe..56919a11 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 3.28) -project(glomap VERSION 0.0.1) +project(glomap VERSION 1.0.0) set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) diff --git a/glomap/version.h.in b/glomap/version.h.in deleted file mode 100644 index aa41e55b..00000000 --- a/glomap/version.h.in +++ /dev/null @@ -1,9 +0,0 @@ -#ifndef @PROJECT_NAME_UPPERCASE@_VERSION_ -#define @PROJECT_NAME_UPPERCASE@_VERSION_ - -#define @PROJECT_NAME_UPPERCASE@_MAJOR_VERSION (@MAJOR_VERSION@) -#define @PROJECT_NAME_UPPERCASE@_MINOR_VERSION (@MINOR_VERSION@) -#define @PROJECT_NAME_UPPERCASE@_PATCH_VERSION (@PATCH_VERSION@) -#define @PROJECT_NAME_UPPERCASE@_VERSION "@PROJECT_VERSION@" - -#endif // @PROJECT_NAME_UPPERCASE@_VERSION_ \ No newline at end of file From fa8662cc04c67f0caaacc6464e5da622739663fe Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Johannes=20Sch=C3=B6nberger?= Date: Fri, 9 Aug 2024 13:52:03 +0200 Subject: [PATCH 07/45] More content for documentation about visualization and settings (#51) * More content for documentation about visualization and recommended settings * Update README.md --------- Co-authored-by: Linfei Pan <36349740+lpanaf@users.noreply.github.com> --- README.md | 18 +++++++++++++++ docs/getting_started.md | 50 +++++++++++++++++++++++++++++++---------- 2 files changed, 56 insertions(+), 12 deletions(-) diff --git a/README.md b/README.md index 74046957..11ac1323 100644 --- a/README.md +++ b/README.md @@ -37,6 +37,8 @@ glomap mapper --database_path DATABASE_PATH --output_path OUTPUT_PATH --image_pa ``` For more details on the command line interface, one can type `glomap -h` or `glomap mapper -h` for help. +We also provide a guide on improving the obtained reconstruction, which can be found [here](docs/getting_started.md) + Note: - GLOMAP depends on two external libraries - [COLMAP](https://github.com/colmap/colmap) and [PoseLib](https://github.com/PoseLib/PoseLib). With the default setting, the library is built automatically by GLOMAP via `FetchContent`. @@ -80,6 +82,22 @@ glomap mapper \ --output_path ./output/south-building/sparse ``` +### Visualize and use the results + +The results are written out in the COLMAP sparse reconstruction format. Please +refer to [COLMAP](https://colmap.github.io/format.html#sparse-reconstruction) +for more details. + +The reconstruction can be visualized using the COLMAP GUI, for example: +```shell +colmap gui --import_path ./output/south-building/sparse/0 +``` +Alternatives like [rerun.io](https://rerun.io/examples/3d-reconstruction/glomap) +also enable visualization of COLMAP and GLOMAP outputs. + +If you want to inspect the reconstruction programmatically, you can use +`pycolmap` in Python or link against COLMAP's C++ library interface. + ### Notes - For larger scale datasets, it is recommended to use `sequential_matcher` or diff --git a/docs/getting_started.md b/docs/getting_started.md index 653b4364..6ae06514 100644 --- a/docs/getting_started.md +++ b/docs/getting_started.md @@ -1,20 +1,46 @@ # Getting started -### Installation and End-to-End Examples -Please refer to the main `README.md` -### Recommended practice +## Installation and end-to-end examples + +Please refer to the main `README.md`. + +## Recommended settings + The default parameters do not always gaurantee satisfying reconstructions. -Regarding this, there are several things which can generally help +Regarding this, there are several things which can generally help. + +### Share camera parameters + +If images are known to be taken with the same physical camera under identical +camera settings, or images are well organized and known to be taken by several +cameras, it is higly recommended to share the camera intrinsics as appropriate. +To achieve this, one can set `--ImageReader.single_camera_per_folder` or +`--ImageReader.single_camera_per_image` in `colmap feature_extractor`. -#### Share camera parameters as much as possible -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 -To achieve this, one can set `--ImageReader.single_camera_per_folder` or `--ImageReader.single_camera_per_image` in `colmap feature_extractor` to be 1. +### Handle high-resolution or blurry images -#### Allow larger epipolar error -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. +If images have high resolution or are blurry, it is worth trying to increase the +allowed epipolar error by modifying `--RelPoseEstimation.max_epipolar_error`. +For example, increase it to 4 or 10. + +### Speedup reconstruction process #### Cap the number of tracks -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$. -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`. -Note, if the `--skip_retriangulation` is not set true when calling `glomap mapper`, retriangulation should already been performed. +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$. 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`. + +Note, if the `--skip_retriangulation` is not set when calling `glomap mapper`, +retriangulation should already been performed. + +#### Limit optimization iterations + +The number of global positioning and bundle adjustment iterations can be limited +using the `--GlobalPositioning.max_num_iterations` and +`--BundleAdjustment.max_num_iterations` options. From d1fcdb8318b5c5d2c0fbe109cee2cbb059b90e60 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Johannes=20Sch=C3=B6nberger?= Date: Fri, 9 Aug 2024 19:49:43 +0200 Subject: [PATCH 08/45] Document pre-compiled Windows binary source (#52) --- README.md | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 11ac1323..e5f4455b 100644 --- a/README.md +++ b/README.md @@ -23,7 +23,7 @@ If you use this project for your research, please cite ## Getting Started -To install GLOMAP, first install [COLMAP](https://colmap.github.io/install.html#build-from-source) +To build GLOMAP, first install [COLMAP](https://colmap.github.io/install.html#build-from-source) dependencies and then build GLOMAP using the following commands: ```shell mkdir build @@ -31,6 +31,9 @@ cd build cmake .. -GNinja ninja && ninja install ``` +Pre-compiled Windows binaries can be downloaded from the official +[release page](https://github.com/colmap/glomap/releases). + After installation, one can run GLOMAP by (starting from a database) ```shell glomap mapper --database_path DATABASE_PATH --output_path OUTPUT_PATH --image_path IMAGE_PATH From dae16bfba551117e41b0b41d26b5b3d0cee76f0e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Johannes=20Sch=C3=B6nberger?= Date: Mon, 12 Aug 2024 09:43:34 +0200 Subject: [PATCH 09/45] Update PoseLib to support OPENCV_FISHEYE camera model (#54) --- cmake/FindDependencies.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/FindDependencies.cmake b/cmake/FindDependencies.cmake index 824f8b07..2425a523 100644 --- a/cmake/FindDependencies.cmake +++ b/cmake/FindDependencies.cmake @@ -27,7 +27,7 @@ endif() include(FetchContent) FetchContent_Declare(PoseLib GIT_REPOSITORY https://github.com/PoseLib/PoseLib.git - GIT_TAG b3691b791bcedccd5451621b2275a1df0d9dcdeb + GIT_TAG 0439b2d361125915b8821043fca9376e6cc575b9 EXCLUDE_FROM_ALL ) message(STATUS "Configuring PoseLib...") From d2d463f7eadea6dc13ff42566ab412e5be28ccd5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Johannes=20Sch=C3=B6nberger?= Date: Mon, 12 Aug 2024 13:59:59 +0200 Subject: [PATCH 10/45] Fix invalid reference to scales auxiliary variable in global positioning (#59) * Fix invalid reference to scales auxiliary variable in global positioning * d --- glomap/estimators/global_positioning.cc | 58 +++++++++++++++---------- glomap/estimators/global_positioning.h | 10 ++--- 2 files changed, 41 insertions(+), 27 deletions(-) diff --git a/glomap/estimators/global_positioning.cc b/glomap/estimators/global_positioning.cc index e6936746..0b94dd0d 100644 --- a/glomap/estimators/global_positioning.cc +++ b/glomap/estimators/global_positioning.cc @@ -42,8 +42,8 @@ bool GlobalPositioner::Solve(const ViewGraph& view_graph, LOG(INFO) << "Setting up the global positioner problem"; - // Initialize the problem - Reset(); + // Setup the problem. + SetupProblem(view_graph, tracks); // Initialize camera translations to be random. // Also, convert the camera pose translation to be the camera center. @@ -81,11 +81,24 @@ bool GlobalPositioner::Solve(const ViewGraph& view_graph, return summary.IsSolutionUsable(); } -void GlobalPositioner::Reset() { +void GlobalPositioner::SetupProblem( + const ViewGraph& view_graph, + const std::unordered_map& tracks) { ceres::Problem::Options problem_options; problem_options.loss_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; problem_ = std::make_unique(problem_options); + // Allocate enough memory for the scales. One for each residual. + // Due to possibly invalid image pairs or tracks, the actual number of + // residuals may be smaller. scales_.clear(); + scales_.reserve( + view_graph.image_pairs.size() + + std::accumulate(tracks.begin(), + tracks.end(), + 0, + [](int sum, const std::pair& track) { + return sum + track.second.observations.size(); + })); } void GlobalPositioner::InitializeRandomPositions( @@ -145,8 +158,9 @@ void GlobalPositioner::AddCameraToCameraConstraints( images.find(image_id2) == images.end()) continue; - track_t counter = scales_.size(); - scales_.insert(std::make_pair(counter, 1)); + CHECK_GT(scales_.capacity(), scales_.size()) + << "Not enough capacity was reserved for the scales."; + double& scale = scales_.emplace_back(1); Eigen::Vector3d translation = -(images[image_id2].cam_from_world.rotation.inverse() * @@ -158,9 +172,9 @@ void GlobalPositioner::AddCameraToCameraConstraints( options_.loss_function.get(), images[image_id1].cam_from_world.translation.data(), images[image_id2].cam_from_world.translation.data(), - &(scales_[counter])); + &scale); - problem_->SetParameterLowerBound(&(scales_[counter]), 0, 1e-5); + problem_->SetParameterLowerBound(&scale, 0, 1e-5); } if (options_.verbose) @@ -246,14 +260,14 @@ void GlobalPositioner::AddTrackToProblem( ceres::CostFunction* cost_function = BATAPairwiseDirectionError::Create(translation); - track_t counter = scales_.size(); - if (options_.generate_scales || !tracks[track_id].is_initialized) { - scales_.insert(std::make_pair(counter, 1)); - } else { - Eigen::Vector3d trans_calc = + CHECK_GT(scales_.capacity(), scales_.size()) + << "Not enough capacity was reserved for the scales."; + double& scale = scales_.emplace_back(1); + if (!options_.generate_scales && tracks[track_id].is_initialized) { + const Eigen::Vector3d trans_calc = tracks[track_id].xyz - image.cam_from_world.translation; - double scale = translation.dot(trans_calc) / trans_calc.squaredNorm(); - scales_.insert(std::make_pair(counter, std::max(scale, 1e-5))); + scale = std::max(1e-5, + translation.dot(trans_calc) / trans_calc.squaredNorm()); } // For calibrated and uncalibrated cameras, use different loss functions @@ -263,14 +277,14 @@ void GlobalPositioner::AddTrackToProblem( loss_function_ptcam_calibrated_.get(), image.cam_from_world.translation.data(), tracks[track_id].xyz.data(), - &(scales_[counter])) + &scale) : problem_->AddResidualBlock(cost_function, loss_function_ptcam_uncalibrated_.get(), image.cam_from_world.translation.data(), tracks[track_id].xyz.data(), - &(scales_[counter])); + &scale); - problem_->SetParameterLowerBound(&(scales_[counter]), 0, 1e-5); + problem_->SetParameterLowerBound(&scale, 0, 1e-5); } } @@ -284,8 +298,8 @@ void GlobalPositioner::AddCamerasAndPointsToParameterGroups( options_.solver_options.linear_solver_ordering.get(); // Add scale parameters to group 0 (large and independent) - for (auto& [i, scale] : scales_) { - parameter_ordering->AddElementToGroup(&(scales_[i]), 0); + for (double& scale : scales_) { + parameter_ordering->AddElementToGroup(&scale, 0); } // Add point parameters to group 1. @@ -332,8 +346,8 @@ void GlobalPositioner::ParameterizeVariables( // If do not optimize the scales, set the scales to be constant if (!options_.optimize_scales) { - for (auto& [i, scale] : scales_) { - problem_->SetParameterBlockConstant(&(scales_[i])); + for (double& scale : scales_) { + problem_->SetParameterBlockConstant(&scale); } } @@ -358,4 +372,4 @@ void GlobalPositioner::ConvertResults( } } -} // namespace glomap \ No newline at end of file +} // namespace glomap diff --git a/glomap/estimators/global_positioning.h b/glomap/estimators/global_positioning.h index 24260dc2..6a9b3236 100644 --- a/glomap/estimators/global_positioning.h +++ b/glomap/estimators/global_positioning.h @@ -61,8 +61,8 @@ class GlobalPositioner { GlobalPositionerOptions& GetOptions() { return options_; } protected: - // Reset the problem - void Reset(); + void SetupProblem(const ViewGraph& view_graph, + const std::unordered_map& tracks); // Initialize all cameras to be random. void InitializeRandomPositions(const ViewGraph& view_graph, @@ -98,17 +98,17 @@ class GlobalPositioner { // center Convert the results back to camera poses void ConvertResults(std::unordered_map& images); - // Data members GlobalPositionerOptions options_; std::mt19937 random_generator_; std::unique_ptr problem_; - // loss functions for reweighted terms + // Loss functions for reweighted terms. std::shared_ptr loss_function_ptcam_uncalibrated_; std::shared_ptr loss_function_ptcam_calibrated_; - std::unordered_map scales_; + // Auxiliary scale variables. + std::vector scales_; }; } // namespace glomap From 6d488365c7b2558ca55d012987bc99ebc9a40d66 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Johannes=20Sch=C3=B6nberger?= Date: Mon, 12 Aug 2024 15:10:05 +0200 Subject: [PATCH 11/45] Robustly handle undistortion failures (#58) * Robustly handle undistortion failures * f * d * f --------- Co-authored-by: Linfei Pan --- glomap/estimators/cost_function.h | 20 ++++------- glomap/estimators/global_positioning.cc | 46 ++++++++++++++++--------- glomap/estimators/global_positioning.h | 2 +- glomap/estimators/relpose_estimation.cc | 25 +++++++++----- glomap/processors/track_filter.cc | 2 +- glomap/scene/image.h | 7 ++-- 6 files changed, 57 insertions(+), 45 deletions(-) diff --git a/glomap/estimators/cost_function.h b/glomap/estimators/cost_function.h index d0a8ce48..64c3e466 100644 --- a/glomap/estimators/cost_function.h +++ b/glomap/estimators/cost_function.h @@ -14,7 +14,7 @@ namespace glomap { // from two positions such that t_ij - scale * (c_j - c_i) is minimized. struct BATAPairwiseDirectionError { BATAPairwiseDirectionError(const Eigen::Vector3d& translation_obs) - : translation_obs_(translation_obs){}; + : translation_obs_(translation_obs) {} // The error is given by the position error described above. template @@ -22,19 +22,11 @@ struct BATAPairwiseDirectionError { const T* position2, const T* scale, T* residuals) const { - Eigen::Matrix translation; - translation[0] = position2[0] - position1[0]; - translation[1] = position2[1] - position1[1]; - translation[2] = position2[2] - position1[2]; - - Eigen::Matrix residual_vec; - - residual_vec = translation_obs_.cast() - scale[0] * translation; - - residuals[0] = residual_vec(0); - residuals[1] = residual_vec(1); - residuals[2] = residual_vec(2); - + Eigen::Map> residuals_vec(residuals); + residuals_vec = + translation_obs_.cast() - + scale[0] * (Eigen::Map>(position2) - + Eigen::Map>(position1)); return true; } diff --git a/glomap/estimators/global_positioning.cc b/glomap/estimators/global_positioning.cc index 0b94dd0d..5590903e 100644 --- a/glomap/estimators/global_positioning.cc +++ b/glomap/estimators/global_positioning.cc @@ -155,14 +155,15 @@ void GlobalPositioner::AddCameraToCameraConstraints( const image_t image_id1 = image_pair.image_id1; const image_t image_id2 = image_pair.image_id2; if (images.find(image_id1) == images.end() || - images.find(image_id2) == images.end()) + images.find(image_id2) == images.end()) { continue; + } CHECK_GT(scales_.capacity(), scales_.size()) << "Not enough capacity was reserved for the scales."; double& scale = scales_.emplace_back(1); - Eigen::Vector3d translation = + const Eigen::Vector3d translation = -(images[image_id2].cam_from_world.rotation.inverse() * image_pair.cam2_from_cam1.translation); ceres::CostFunction* cost_function = @@ -244,7 +245,7 @@ void GlobalPositioner::AddPointToCameraConstraints( } void GlobalPositioner::AddTrackToProblem( - const track_t& track_id, + track_t track_id, std::unordered_map& cameras, std::unordered_map& images, std::unordered_map& tracks) { @@ -255,8 +256,19 @@ void GlobalPositioner::AddTrackToProblem( Image& image = images[observation.first]; if (!image.is_registered) continue; - Eigen::Vector3d translation = image.cam_from_world.rotation.inverse() * - image.features_undist[observation.second]; + const Eigen::Vector3d& feature_undist = + image.features_undist[observation.second]; + if (feature_undist.array().isNaN().any()) { + LOG(WARNING) + << "Ignoring feature because it failed to undistort: track_id=" + << track_id << ", image_id=" << observation.first + << ", feature_id=" << observation.second; + continue; + } + + const Eigen::Vector3d translation = + image.cam_from_world.rotation.inverse() * + image.features_undist[observation.second]; ceres::CostFunction* cost_function = BATAPairwiseDirectionError::Create(translation); @@ -272,17 +284,19 @@ void GlobalPositioner::AddTrackToProblem( // For calibrated and uncalibrated cameras, use different loss functions // Down weight the uncalibrated cameras - (cameras[image.camera_id].has_prior_focal_length) - ? problem_->AddResidualBlock(cost_function, - loss_function_ptcam_calibrated_.get(), - image.cam_from_world.translation.data(), - tracks[track_id].xyz.data(), - &scale) - : problem_->AddResidualBlock(cost_function, - loss_function_ptcam_uncalibrated_.get(), - image.cam_from_world.translation.data(), - tracks[track_id].xyz.data(), - &scale); + if (cameras[image.camera_id].has_prior_focal_length) { + problem_->AddResidualBlock(cost_function, + loss_function_ptcam_calibrated_.get(), + image.cam_from_world.translation.data(), + tracks[track_id].xyz.data(), + &scale); + } else { + problem_->AddResidualBlock(cost_function, + loss_function_ptcam_uncalibrated_.get(), + image.cam_from_world.translation.data(), + tracks[track_id].xyz.data(), + &scale); + } problem_->SetParameterLowerBound(&scale, 0, 1e-5); } diff --git a/glomap/estimators/global_positioning.h b/glomap/estimators/global_positioning.h index 6a9b3236..0eb1185c 100644 --- a/glomap/estimators/global_positioning.h +++ b/glomap/estimators/global_positioning.h @@ -80,7 +80,7 @@ class GlobalPositioner { std::unordered_map& tracks); // Add a single track to the problem - void AddTrackToProblem(const track_t& track_id, + void AddTrackToProblem(track_t track_id, std::unordered_map& cameras, std::unordered_map& images, std::unordered_map& tracks); diff --git a/glomap/estimators/relpose_estimation.cc b/glomap/estimators/relpose_estimation.cc index 89bdab3a..c53f572a 100644 --- a/glomap/estimators/relpose_estimation.cc +++ b/glomap/estimators/relpose_estimation.cc @@ -47,15 +47,22 @@ void EstimateRelativePoses(ViewGraph& view_graph, inliers.clear(); poselib::CameraPose pose_rel_calc; - poselib::estimate_relative_pose( - points2D_1, - points2D_2, - ColmapCameraToPoseLibCamera(cameras[image1.camera_id]), - ColmapCameraToPoseLibCamera(cameras[image2.camera_id]), - options.ransac_options, - options.bundle_options, - &pose_rel_calc, - &inliers); + try { + poselib::estimate_relative_pose( + points2D_1, + points2D_2, + ColmapCameraToPoseLibCamera(cameras[image1.camera_id]), + ColmapCameraToPoseLibCamera(cameras[image2.camera_id]), + options.ransac_options, + options.bundle_options, + &pose_rel_calc, + &inliers); + } catch (const std::exception& e) { + LOG(ERROR) << "Error in relative pose estimation: " << e.what(); + image_pair.is_valid = false; + continue; + } + // Convert the relative pose to the glomap format for (int i = 0; i < 4; i++) { image_pair.cam2_from_cam1.rotation.coeffs()[i] = diff --git a/glomap/processors/track_filter.cc b/glomap/processors/track_filter.cc index 226af020..c3a78f7d 100644 --- a/glomap/processors/track_filter.cc +++ b/glomap/processors/track_filter.cc @@ -37,7 +37,7 @@ int TrackFilter::FilterTracksByReprojection( // If the reprojection error is smaller than the threshold, then keep it if (reprojection_error < max_reprojection_error) { - observation_new.emplace_back(std::make_pair(image_id, feature_id)); + observation_new.emplace_back(image_id, feature_id); } } if (observation_new.size() != track.observations.size()) { diff --git a/glomap/scene/image.h b/glomap/scene/image.h index a191371a..1497efae 100644 --- a/glomap/scene/image.h +++ b/glomap/scene/image.h @@ -47,11 +47,10 @@ struct Image { // Gravity information GravityInfo gravity_info; - // Features + // Distorted feature points in pixels. std::vector features; - std::vector - features_undist; // store the normalized features, can be obtained by - // calling UndistortImages + // Normalized feature rays, can be obtained by calling UndistortImages. + std::vector features_undist; // Methods inline Eigen::Vector3d Center() const; From 27f9a0874b4f645e902c8af846cf3b272c3be1c5 Mon Sep 17 00:00:00 2001 From: Asad Mehboob Ali <86931093+Asadali242@users.noreply.github.com> Date: Wed, 14 Aug 2024 05:34:32 -0400 Subject: [PATCH 12/45] Add complete installation guide for COLMAP and GLOMAP on macOS (#63) * Add complete installation guide for COLMAP and GLOMAP on macOS * Update INSTALL_MAC.md --------- Co-authored-by: Linfei Pan <36349740+lpanaf@users.noreply.github.com> --- docs/INSTALL_MAC.md | 193 ++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 193 insertions(+) create mode 100644 docs/INSTALL_MAC.md diff --git a/docs/INSTALL_MAC.md b/docs/INSTALL_MAC.md new file mode 100644 index 00000000..638f5743 --- /dev/null +++ b/docs/INSTALL_MAC.md @@ -0,0 +1,193 @@ +We would like to thank [Asadali242](https://github.com/Asadali242) for providing the installation guide for MAC. +Leave your comments at [Issue #62](https://github.com/colmap/glomap/issues/62) if you encounter any problems. + +## Installing the COLMAP: + +*1. Open the Terminal and install the brew dependencies:* +``` +brew install \ +cmake \ +ninja \ +boost \ +eigen \ +flann \ +libomp \ #(Install Libomp as well) +freeimage \ +metis \ +glog \ +googletest \ +ceres-solver \ +qt@5 \ +glew \ +cgal \ +sqlite3 +``` + +*2. Clone the COLMAP repository:* +``` +git clone https://github.com/colmap/colmap.git +cd colmap +``` + +*3. Ensure Qt5 is in your PATH:* +``` +export PATH="/opt/homebrew/opt/qt@5/bin:$PATH" +``` + +*4. Create a build directory:* +``` +mkdir build +cd build +``` + +*5. After installing, link Qt5 to make sure it’s accessible:* +``` +brew link qt@5 --force +``` + +*6. Run CMake with the specific paths for ARM Mac(M1 and above):* +``` +cmake .. -GNinja \ + -DCMAKE_PREFIX_PATH="/opt/homebrew/opt/flann;/opt/homebrew/opt/metis;/opt/homebrew/opt/suite-sparse;/opt/homebrew/opt/qt@5;/opt/homebrew/opt/freeimage" +``` + +*7. Build and install COLMAP:* +``` +ninja +sudo ninja install +``` + +*8. Confirm COLMAP installation by running:* +``` +colmap -h +colmap gui +``` + +**This is the first part and will install Colmap on your device.** + + + +## Installing the GLOMAP: + +*1. Clone the GitHub repository:* +``` +git clone https://github.com/colmap/glomap +cd glomap +``` + +*2. Create a build directory:* +``` +mkdir build +cd build +``` + +*3. Export Environment Variables Again:* +``` +export PATH="/opt/homebrew/opt/qt@5/bin:$PATH" +export LDFLAGS="-L/opt/homebrew/opt/libomp/lib" +export CPPFLAGS="-I/opt/homebrew/opt/libomp/include" +export CMAKE_PREFIX_PATH="/opt/homebrew/opt/qt@5;/opt/homebrew/opt/libomp" +export PKG_CONFIG_PATH="/opt/homebrew/opt/qt@5/lib/pkgconfig" +``` + +*4. Run the CMake Command:* +``` +cmake -DCMAKE_PREFIX_PATH="/opt/homebrew/Cellar/qt@5/5.15.13_1;/opt/homebrew/opt/libomp" \ +-DOpenMP_C_FLAGS="-Xclang -fopenmp" \ +-DOpenMP_C_LIB_NAMES="libomp" \ +-DOpenMP_CXX_FLAGS="-Xclang -fopenmp" \ +-DOpenMP_CXX_LIB_NAMES="libomp" \ +-DOpenMP_C_INCLUDE_DIRS="/opt/homebrew/opt/libomp/include" \ +-DOpenMP_CXX_INCLUDE_DIRS="/opt/homebrew/opt/libomp/include" \ +-DOpenMP_libomp_LIBRARY=/opt/homebrew/opt/libomp/lib/libomp.dylib \ +-DOpenMP_INCLUDE_DIR=/opt/homebrew/opt/libomp/include \ +.. -GNinja +``` + +*5. Build the Project:* +``` +ninja +``` + +**NOTE: If at this point, there are build errors related to ‘cholmod.h’ or ‘omp.h’, clean the build and then re-run the make with the following commands:** +``` +cmake -DCMAKE_PREFIX_PATH="/opt/homebrew/Cellar/qt@5/5.15.13_1;/opt/homebrew/opt/libomp" \ +-DOpenMP_C_FLAGS="-Xclang -fopenmp -I/opt/homebrew/opt/libomp/include" \ +-DOpenMP_C_LIB_NAMES="libomp" \ +-DOpenMP_CXX_FLAGS="-Xclang -fopenmp -I/opt/homebrew/opt/libomp/include" \ +-DOpenMP_CXX_LIB_NAMES="libomp" \ +-DOpenMP_libomp_LIBRARY=/opt/homebrew/opt/libomp/lib/libomp.dylib \ +.. -GNinja +``` + +**After the build is successful:** + +*6. Install the Built Project:* +``` +sudo ninja install +``` + +*7. Test the Installation:* +``` +glomap -h +``` + +**It should display something like:** +``` +GLOMAP -- Global Structure-from-Motion +Usage: +glomap mapper --database_path DATABASE --output_path +MODEL +glomap mapper_resume --input_path MODEL_INPUT --output_path MODEL_OUTPUT +Available commands: +help +mapper +mapper_resume +``` + + +## Testing with the end-to-end examples provided: + +*1. Open the end-to-end example database link provided:* +https://lpanaf.github.io/eccv24_glomap/ + +*2. Download one of the datasets provided and extract the zip file.* + +*3. Create a new directory named ‘data’ in the root directory ‘glomap’.* + +*4. Place the extracted dataset in the directory ‘data’.* + +*5. Now navigate back to the project directory ‘glomap’.* + +**NOTE: Following commands are assuming the dataset to be south-building:** + +*6. Extract Features with COLMAP:* +``` +colmap feature_extractor \ +--image_path ./data/south-building/images \ +--database_path ./data/south-building/database.db +``` + +*7. Match Features with COLMAP:* +``` +colmap exhaustive_matcher \ +--database_path ./data/south-building/database.db +``` + +*8. Run GLOMAP Mapper:* +``` +glomap mapper \ +--database_path ./data/south-building/database.db \ +--image_path ./data/south-building/images \ +--output_path ./output/south-building/sparse +``` + +**This should generate a new directory named ‘output’ in the project directory.** + +*9. Visualize the Results:* +``` +colmap gui \ +--database_path ./data/south-building/database.db \ +--image_path ./data/south-building/images \ +--import_path ./output/south-building/sparse/0 +``` From a0e8082714330812afaa0a1434ca36e576463845 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Johannes=20Sch=C3=B6nberger?= Date: Sun, 25 Aug 2024 20:20:44 +0200 Subject: [PATCH 13/45] Enable verbose logging options through command-line flags (#77) --- glomap/controllers/global_mapper_test.cc | 7 ------ glomap/controllers/option_manager.cc | 3 +++ glomap/estimators/bundle_adjustment.cc | 4 ++-- glomap/estimators/global_positioning.cc | 24 ++++++++----------- .../estimators/global_rotation_averaging.cc | 22 +++++++---------- glomap/estimators/global_rotation_averaging.h | 3 --- glomap/estimators/optimization_base.h | 5 +--- glomap/estimators/view_graph_calibration.cc | 17 ++++--------- 8 files changed, 29 insertions(+), 56 deletions(-) diff --git a/glomap/controllers/global_mapper_test.cc b/glomap/controllers/global_mapper_test.cc index 044c917e..b1a40628 100644 --- a/glomap/controllers/global_mapper_test.cc +++ b/glomap/controllers/global_mapper_test.cc @@ -40,13 +40,6 @@ void ExpectEqualReconstructions(const colmap::Reconstruction& gt, GlobalMapperOptions CreateTestOptions() { GlobalMapperOptions options; - // Control the verbosity of the global sfm - options.opt_vgcalib.verbose = false; - options.opt_ra.verbose = false; - options.opt_gp.verbose = false; - options.opt_ba.verbose = false; - - // Control the flow of the global sfm options.skip_view_graph_calibration = false; options.skip_relative_pose_estimation = false; options.skip_rotation_averaging = false; diff --git a/glomap/controllers/option_manager.cc b/glomap/controllers/option_manager.cc index 39c17f8f..d1e03042 100644 --- a/glomap/controllers/option_manager.cc +++ b/glomap/controllers/option_manager.cc @@ -17,6 +17,9 @@ OptionManager::OptionManager(bool add_project_options) { Reset(); desc_->add_options()("help,h", ""); + + AddAndRegisterDefaultOption("log_to_stderr", &FLAGS_logtostderr); + AddAndRegisterDefaultOption("log_level", &FLAGS_v); } void OptionManager::AddAllOptions() { diff --git a/glomap/estimators/bundle_adjustment.cc b/glomap/estimators/bundle_adjustment.cc index a03bea5c..32204baf 100644 --- a/glomap/estimators/bundle_adjustment.cc +++ b/glomap/estimators/bundle_adjustment.cc @@ -40,9 +40,9 @@ bool BundleAdjuster::Solve(const ViewGraph& view_graph, options_.solver_options.linear_solver_type = ceres::SPARSE_SCHUR; options_.solver_options.preconditioner_type = ceres::CLUSTER_TRIDIAGONAL; - options_.solver_options.minimizer_progress_to_stdout = options_.verbose; + options_.solver_options.minimizer_progress_to_stdout = VLOG_IS_ON(2); ceres::Solve(options_.solver_options, problem_.get(), &summary); - if (options_.verbose) + if (VLOG_IS_ON(2)) LOG(INFO) << summary.FullReport(); else LOG(INFO) << summary.BriefReport(); diff --git a/glomap/estimators/global_positioning.cc b/glomap/estimators/global_positioning.cc index 5590903e..b18fa80b 100644 --- a/glomap/estimators/global_positioning.cc +++ b/glomap/estimators/global_positioning.cc @@ -68,10 +68,10 @@ bool GlobalPositioner::Solve(const ViewGraph& view_graph, LOG(INFO) << "Solving the global positioner problem"; ceres::Solver::Summary summary; - options_.solver_options.minimizer_progress_to_stdout = options_.verbose; + options_.solver_options.minimizer_progress_to_stdout = VLOG_IS_ON(2); ceres::Solve(options_.solver_options, problem_.get(), &summary); - if (options_.verbose) { + if (VLOG_IS_ON(2)) { LOG(INFO) << summary.FullReport(); } else { LOG(INFO) << summary.BriefReport(); @@ -143,8 +143,7 @@ void GlobalPositioner::InitializeRandomPositions( image.cam_from_world.translation = image.Center(); } - if (options_.verbose) - LOG(INFO) << "Constrained positions: " << constrained_positions.size(); + VLOG(2) << "Constrained positions: " << constrained_positions.size(); } void GlobalPositioner::AddCameraToCameraConstraints( @@ -178,10 +177,9 @@ void GlobalPositioner::AddCameraToCameraConstraints( problem_->SetParameterLowerBound(&scale, 0, 1e-5); } - if (options_.verbose) - LOG(INFO) << problem_->NumResidualBlocks() - << " camera to camera constraints were added to the position " - "estimation problem."; + VLOG(2) << problem_->NumResidualBlocks() + << " camera to camera constraints were added to the position " + "estimation problem."; } void GlobalPositioner::AddPointToCameraConstraints( @@ -194,10 +192,9 @@ void GlobalPositioner::AddPointToCameraConstraints( // Find the tracks that are relevant to the current set of cameras const size_t num_pt_to_cam = tracks.size(); - if (options_.verbose) - LOG(INFO) << num_pt_to_cam - << " point to camera constriants were added to the position " - "estimation problem."; + VLOG(2) << num_pt_to_cam + << " point to camera constriants were added to the position " + "estimation problem."; if (num_pt_to_cam == 0) return; @@ -211,8 +208,7 @@ void GlobalPositioner::AddPointToCameraConstraints( static_cast(num_cam_to_cam) / static_cast(num_pt_to_cam); } - if (options_.verbose) - LOG(INFO) << "Point to camera weight scaled: " << weight_scale_pt; + VLOG(2) << "Point to camera weight scaled: " << weight_scale_pt; if (loss_function_ptcam_uncalibrated_ == nullptr) { loss_function_ptcam_uncalibrated_ = diff --git a/glomap/estimators/global_rotation_averaging.cc b/glomap/estimators/global_rotation_averaging.cc index 429a6bdc..3f7d0319 100644 --- a/glomap/estimators/global_rotation_averaging.cc +++ b/glomap/estimators/global_rotation_averaging.cc @@ -156,10 +156,6 @@ void RotationEstimator::SetupLinearSystem( } } - if (options_.verbose) - LOG(INFO) << "num_img: " << image_id_to_idx_.size() - << ", num_dof: " << num_dof; - rotation_estimated_.conservativeResize(num_dof); // Prepare the relative information @@ -204,8 +200,7 @@ void RotationEstimator::SetupLinearSystem( } } - if (options_.verbose) - LOG(INFO) << counter << " image pairs are gravity aligned" << std::endl; + VLOG(2) << counter << " image pairs are gravity aligned" << std::endl; std::vector> coeffs; coeffs.reserve(rel_temp_info_.size() * 6 + 3); @@ -290,11 +285,11 @@ bool RotationEstimator::SolveL1Regression( double curr_norm = 0; ComputeResiduals(view_graph, images); - if (options_.verbose) LOG(INFO) << "ComputeResiduals done "; + VLOG(2) << "ComputeResiduals done"; int iteration = 0; for (iteration = 0; iteration < options_.max_num_l1_iterations; iteration++) { - if (options_.verbose) LOG(INFO) << "L1 ADMM iteration: " << iteration; + VLOG(2) << "L1 ADMM iteration: " << iteration; last_norm = curr_norm; // use the current residual as b (Ax - b) @@ -307,7 +302,7 @@ bool RotationEstimator::SolveL1Regression( return false; } - if (options_.verbose) + if (VLOG_IS_ON(2)) LOG(INFO) << "residual:" << (sparse_matrix_ * tangent_space_step_ - tangent_space_residual_) @@ -332,7 +327,7 @@ bool RotationEstimator::SolveL1Regression( opt_l1_solver.max_num_iterations = std::min(opt_l1_solver.max_num_iterations * 2, 100); } - if (options_.verbose) LOG(INFO) << "L1 ADMM total iteration: " << iteration; + VLOG(2) << "L1 ADMM total iteration: " << iteration; return true; } @@ -347,8 +342,7 @@ bool RotationEstimator::SolveIRLS(const ViewGraph& view_graph, llt.analyzePattern(sparse_matrix_.transpose() * sparse_matrix_); const double sigma = DegToRad(options_.irls_loss_parameter_sigma); - if (options_.verbose) - LOG(INFO) << "sigma: " << options_.irls_loss_parameter_sigma; + VLOG(2) << "sigma: " << options_.irls_loss_parameter_sigma; Eigen::ArrayXd weights_irls(sparse_matrix_.rows()); Eigen::SparseMatrix at_weight; @@ -362,7 +356,7 @@ bool RotationEstimator::SolveIRLS(const ViewGraph& view_graph, int iteration = 0; for (iteration = 0; iteration < options_.max_num_irls_iterations; iteration++) { - if (options_.verbose) LOG(INFO) << "IRLS iteration: " << iteration; + VLOG(2) << "IRLS iteration: " << iteration; // Compute the weights for IRLS for (auto& [pair_id, pair_info] : rel_temp_info_) { @@ -416,7 +410,7 @@ bool RotationEstimator::SolveIRLS(const ViewGraph& view_graph, break; } } - if (options_.verbose) LOG(INFO) << "IRLS total iteration: " << iteration; + VLOG(2) << "IRLS total iteration: " << iteration; return true; } diff --git a/glomap/estimators/global_rotation_averaging.h b/glomap/estimators/global_rotation_averaging.h index 7b73d344..8899a1cb 100644 --- a/glomap/estimators/global_rotation_averaging.h +++ b/glomap/estimators/global_rotation_averaging.h @@ -68,9 +68,6 @@ struct RotationEstimatorOptions { // Flag to use gravity for rotation averaging bool use_gravity = false; - - // Flag whether report the verbose information - bool verbose = false; }; // TODO: Implement the stratified camera rotation estimation diff --git a/glomap/estimators/optimization_base.h b/glomap/estimators/optimization_base.h index 6234ca26..2b43e066 100644 --- a/glomap/estimators/optimization_base.h +++ b/glomap/estimators/optimization_base.h @@ -9,9 +9,6 @@ namespace glomap { struct OptimizationBaseOptions { - // Logging control - bool verbose = false; - // The threshold for the loss function double thres_loss_function = 1e-1; @@ -24,7 +21,7 @@ struct OptimizationBaseOptions { OptimizationBaseOptions() { solver_options.num_threads = std::thread::hardware_concurrency(); solver_options.max_num_iterations = 100; - solver_options.minimizer_progress_to_stdout = verbose; + solver_options.minimizer_progress_to_stdout = false; solver_options.function_tolerance = 1e-5; } }; diff --git a/glomap/estimators/view_graph_calibration.cc b/glomap/estimators/view_graph_calibration.cc index 79910554..7d5b4572 100644 --- a/glomap/estimators/view_graph_calibration.cc +++ b/glomap/estimators/view_graph_calibration.cc @@ -36,13 +36,10 @@ bool ViewGraphCalibrator::Solve(ViewGraph& view_graph, // Solve the problem ceres::Solver::Summary summary; - options_.solver_options.minimizer_progress_to_stdout = options_.verbose; + options_.solver_options.minimizer_progress_to_stdout = VLOG_IS_ON(2); ceres::Solve(options_.solver_options, problem_.get(), &summary); - // Print the summary only if verbose - if (options_.verbose) { - LOG(INFO) << summary.FullReport(); - } + VLOG(2) << summary.FullReport(); // Convert the results back to the camera CopyBackResults(cameras); @@ -132,10 +129,9 @@ void ViewGraphCalibrator::CopyBackResults( // if the estimated parameter is too crazy, reject it if (focals_[camera_id] / camera.Focal() > options_.thres_higher_ratio || focals_[camera_id] / camera.Focal() < options_.thres_lower_ratio) { - if (options_.verbose) - LOG(INFO) << "NOT ACCEPTED: Camera " << camera_id - << " focal: " << focals_[camera_id] - << " original focal: " << camera.Focal(); + VLOG(2) << "Ignoring degenerate camera camera " << camera_id + << " focal: " << focals_[camera_id] + << " original focal: " << camera.Focal(); counter++; continue; @@ -147,9 +143,6 @@ void ViewGraphCalibrator::CopyBackResults( // Update the focal length for (const size_t idx : camera.FocalLengthIdxs()) { camera.params[idx] = focals_[camera_id]; - if (options_.verbose) - LOG(INFO) << "Camera " << idx << " focal: " << focals_[camera_id] - << std::endl; } } LOG(INFO) << counter << " cameras are rejected in view graph calibration"; From 3d053c2fb96efa046555aef664fe6e82c0316a1c Mon Sep 17 00:00:00 2001 From: creeper <104205641+cre185@users.noreply.github.com> Date: Tue, 3 Sep 2024 18:09:26 +0800 Subject: [PATCH 14/45] fix bug caused by negative weight when calculating mst (#90) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * fix bug caused by negative weight when calculating mst * Apply suggestion Co-authored-by: Johannes Schönberger --------- Co-authored-by: Johannes Schönberger --- glomap/math/tree.cc | 18 ++++++++++++++---- 1 file changed, 14 insertions(+), 4 deletions(-) diff --git a/glomap/math/tree.cc b/glomap/math/tree.cc index 1a0ad4e5..5f329e64 100644 --- a/glomap/math/tree.cc +++ b/glomap/math/tree.cc @@ -89,6 +89,16 @@ image_t MaximumSpanningTree(const ViewGraph& view_graph, image_id_to_idx[image_id] = image_id_to_idx.size(); } + double max_weight = 0; + for (const auto& [pair_id, image_pair] : view_graph.image_pairs) { + if (image_pair.is_valid == false) continue; + if (type == INLIER_RATIO) + max_weight = std::max(max_weight, image_pair.weight); + else + max_weight = + std::max(max_weight, static_cast(image_pair.inliers.size())); + } + // establish graph weighted_graph G(image_id_to_idx.size()); weight_map weights_boost = boost::get(boost::edge_weight, G); @@ -111,11 +121,11 @@ image_t MaximumSpanningTree(const ViewGraph& view_graph, // spanning tree e = boost::add_edge(idx1, idx2, G).first; if (type == INLIER_NUM) - weights_boost[e] = -image_pair.inliers.size(); + weights_boost[e] = max_weight - image_pair.inliers.size(); else if (type == INLIER_RATIO) - weights_boost[e] = -image_pair.weight; + weights_boost[e] = max_weight - image_pair.weight; else - weights_boost[e] = -image_pair.inliers.size(); + weights_boost[e] = max_weight - image_pair.inliers.size(); } std::vector @@ -142,4 +152,4 @@ image_t MaximumSpanningTree(const ViewGraph& view_graph, return idx_to_image_id[0]; } -}; // namespace glomap \ No newline at end of file +}; // namespace glomap From 733795fbc2181ce800188d82fe2d8998d19f8e14 Mon Sep 17 00:00:00 2001 From: lnex Date: Tue, 10 Sep 2024 00:19:17 +0800 Subject: [PATCH 15/45] fix: arg type for std::ceil and typo in glomap::EstimateRelativePoses (#94) --- glomap/estimators/relpose_estimation.cc | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/glomap/estimators/relpose_estimation.cc b/glomap/estimators/relpose_estimation.cc index c53f572a..45f206bd 100644 --- a/glomap/estimators/relpose_estimation.cc +++ b/glomap/estimators/relpose_estimation.cc @@ -20,14 +20,15 @@ void EstimateRelativePoses(ViewGraph& view_graph, const int64_t num_image_pairs = valid_pair_ids.size(); const int64_t kNumChunks = 10; - const int64_t inverval = std::ceil(num_image_pairs / kNumChunks); + const int64_t interval = + std::ceil(static_cast(num_image_pairs) / kNumChunks); LOG(INFO) << "Estimating relative pose for " << num_image_pairs << " pairs"; for (int64_t chunk_id = 0; chunk_id < kNumChunks; chunk_id++) { std::cout << "\r Estimating relative pose: " << chunk_id * kNumChunks << "%" << std::flush; - const int64_t start = chunk_id * inverval; + const int64_t start = chunk_id * interval; const int64_t end = - std::min((chunk_id + 1) * inverval, num_image_pairs); + std::min((chunk_id + 1) * interval, num_image_pairs); #pragma omp parallel for schedule(dynamic) private( \ points2D_1, points2D_2, inliers) From 65a4d40bfb6a9f70e28ca1b15e74f440c3c6b760 Mon Sep 17 00:00:00 2001 From: lnex Date: Sat, 14 Sep 2024 23:12:41 +0800 Subject: [PATCH 16/45] fix: vector out of bounds in ViewGraph::KeepLargestConnectedComponents (#99) * fix: vector out of bounds in ViewGraph::KeepLargestConnectedComponents * fix: return GlobalMapper::Solve when no connected components --- glomap/controllers/global_mapper.cc | 14 ++++++++++++-- glomap/scene/view_graph.cc | 2 ++ 2 files changed, 14 insertions(+), 2 deletions(-) diff --git a/glomap/controllers/global_mapper.cc b/glomap/controllers/global_mapper.cc index af436128..22f14c8f 100644 --- a/glomap/controllers/global_mapper.cc +++ b/glomap/controllers/global_mapper.cc @@ -63,7 +63,10 @@ bool GlobalMapper::Solve(const colmap::Database& database, RelPoseFilter::FilterInlierRatio( view_graph, options_.inlier_thresholds.min_inlier_ratio); - view_graph.KeepLargestConnectedComponents(images); + if (view_graph.KeepLargestConnectedComponents(images) == 0) { + LOG(ERROR) << "no connected components are found"; + return false; + } run_timer.PrintSeconds(); } @@ -83,7 +86,10 @@ bool GlobalMapper::Solve(const colmap::Database& database, RelPoseFilter::FilterRotations( view_graph, images, options_.inlier_thresholds.max_rotation_error); - view_graph.KeepLargestConnectedComponents(images); + if (view_graph.KeepLargestConnectedComponents(images) == 0) { + LOG(ERROR) << "no connected components are found"; + return false; + } // The second run is for final estimation if (!ra_engine.EstimateRotations(view_graph, images)) { @@ -92,6 +98,10 @@ bool GlobalMapper::Solve(const colmap::Database& database, RelPoseFilter::FilterRotations( view_graph, images, options_.inlier_thresholds.max_rotation_error); image_t num_img = view_graph.KeepLargestConnectedComponents(images); + if (num_img == 0) { + LOG(ERROR) << "no connected components are found"; + return false; + } LOG(INFO) << num_img << " / " << images.size() << " images are within the connected component." << std::endl; diff --git a/glomap/scene/view_graph.cc b/glomap/scene/view_graph.cc index d3a540f6..0443d90d 100644 --- a/glomap/scene/view_graph.cc +++ b/glomap/scene/view_graph.cc @@ -21,6 +21,8 @@ int ViewGraph::KeepLargestConnectedComponents( } } + if (max_img == 0) return 0; + std::unordered_set largest_component = connected_components[max_idx]; // Set all images to not registered From d46b1d899c2f4b3ab235cca001cc4d3d8c49962f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Johannes=20Sch=C3=B6nberger?= Date: Thu, 19 Sep 2024 13:41:12 +0200 Subject: [PATCH 17/45] Use colmap thread pool to simplify mac installation (#107) * Use colmap thread pool to simplify mac installation * d * d * d * d --- .github/workflows/mac.yml | 83 ++++++++ .github/workflows/ubuntu.yml | 2 +- .gitignore | 5 +- CMakeLists.txt | 1 - cmake/FindDependencies.cmake | 5 - docs/INSTALL_MAC.md | 193 ------------------- glomap/CMakeLists.txt | 4 - glomap/estimators/relpose_estimation.cc | 89 +++++---- glomap/processors/image_undistorter.cc | 32 +-- glomap/processors/view_graph_manipulation.cc | 80 ++++---- 10 files changed, 198 insertions(+), 296 deletions(-) create mode 100644 .github/workflows/mac.yml delete mode 100644 docs/INSTALL_MAC.md diff --git a/.github/workflows/mac.yml b/.github/workflows/mac.yml new file mode 100644 index 00000000..6cfdbeb0 --- /dev/null +++ b/.github/workflows/mac.yml @@ -0,0 +1,83 @@ +name: Mac + +on: + push: + branches: + - main + pull_request: + types: [ assigned, opened, synchronize, reopened ] + release: + types: [ published, edited ] + +jobs: + build: + name: ${{ matrix.config.os }} ${{ matrix.config.arch }} ${{ matrix.config.cmakeBuildType }} + runs-on: ${{ matrix.config.os }} + strategy: + matrix: + config: [ + { + os: macos-14, + arch: arm64, + cmakeBuildType: Release, + }, + ] + + env: + COMPILER_CACHE_VERSION: 1 + COMPILER_CACHE_DIR: ${{ github.workspace }}/compiler-cache + CCACHE_DIR: ${{ github.workspace }}/compiler-cache/ccache + CCACHE_BASEDIR: ${{ github.workspace }} + + steps: + - uses: actions/checkout@v4 + - uses: actions/cache@v4 + id: cache-builds + with: + key: v${{ env.COMPILER_CACHE_VERSION }}-${{ matrix.config.os }}-${{ matrix.config.arch }}-${{ matrix.config.cmakeBuildType }}-${{ github.run_id }}-${{ github.run_number }} + restore-keys: v${{ env.COMPILER_CACHE_VERSION }}-${{ matrix.config.os }}-${{ matrix.config.arch }}-${{ matrix.config.cmakeBuildType }} + path: ${{ env.COMPILER_CACHE_DIR }} + + - name: Setup Mac + run: | + brew install \ + cmake \ + ninja \ + boost \ + eigen \ + flann \ + freeimage \ + metis \ + glog \ + googletest \ + ceres-solver \ + qt5 \ + glew \ + cgal \ + sqlite3 \ + ccache + + - name: Configure and build + run: | + cmake --version + mkdir build + cd build + cmake .. \ + -GNinja \ + -DCMAKE_BUILD_TYPE=${{ matrix.config.cmakeBuildType }} \ + -DTESTS_ENABLED=ON \ + -DCMAKE_PREFIX_PATH="$(brew --prefix qt@5)" + ninja + + - name: Run tests + run: | + cd build + set +e + ctest --output-on-failure -E .+colmap_.* + + - name: Cleanup compiler cache + run: | + set -x + ccache --show-stats --verbose + ccache --evict-older-than 1d + ccache --show-stats --verbose diff --git a/.github/workflows/ubuntu.yml b/.github/workflows/ubuntu.yml index 455e8d83..a8246f96 100644 --- a/.github/workflows/ubuntu.yml +++ b/.github/workflows/ubuntu.yml @@ -185,4 +185,4 @@ jobs: # Delete cache older than 10 days. find "$CTCACHE_DIR"/*/ -mtime +10 -print0 | xargs -0 rm -rf echo "Size of ctcache after: $(du -sh $CTCACHE_DIR)" - echo "Number of ctcache files after: $(find $CTCACHE_DIR | wc -l)" \ No newline at end of file + echo "Number of ctcache files after: $(find $CTCACHE_DIR | wc -l)"'' diff --git a/.gitignore b/.gitignore index b4caee1c..52abcd91 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,3 @@ -build -data +/build +/data +/.vscode diff --git a/CMakeLists.txt b/CMakeLists.txt index 56919a11..1d1c2729 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -7,7 +7,6 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON) set_property(GLOBAL PROPERTY GLOBAL_DEPENDS_NO_CYCLES ON) -option(OPENMP_ENABLED "Whether to enable OpenMP parallelization" ON) option(TESTS_ENABLED "Whether to build test binaries" OFF) option(ASAN_ENABLED "Whether to enable AddressSanitizer flags" OFF) option(CCACHE_ENABLED "Whether to enable compiler caching, if available" ON) diff --git a/cmake/FindDependencies.cmake b/cmake/FindDependencies.cmake index 2425a523..7bcf2020 100644 --- a/cmake/FindDependencies.cmake +++ b/cmake/FindDependencies.cmake @@ -19,11 +19,6 @@ if(TESTS_ENABLED) find_package(GTest REQUIRED) endif() -if (OPENMP_ENABLED) - message(STATUS "Enabling OpenMP") - find_package(OpenMP REQUIRED) -endif() - include(FetchContent) FetchContent_Declare(PoseLib GIT_REPOSITORY https://github.com/PoseLib/PoseLib.git diff --git a/docs/INSTALL_MAC.md b/docs/INSTALL_MAC.md deleted file mode 100644 index 638f5743..00000000 --- a/docs/INSTALL_MAC.md +++ /dev/null @@ -1,193 +0,0 @@ -We would like to thank [Asadali242](https://github.com/Asadali242) for providing the installation guide for MAC. -Leave your comments at [Issue #62](https://github.com/colmap/glomap/issues/62) if you encounter any problems. - -## Installing the COLMAP: - -*1. Open the Terminal and install the brew dependencies:* -``` -brew install \ -cmake \ -ninja \ -boost \ -eigen \ -flann \ -libomp \ #(Install Libomp as well) -freeimage \ -metis \ -glog \ -googletest \ -ceres-solver \ -qt@5 \ -glew \ -cgal \ -sqlite3 -``` - -*2. Clone the COLMAP repository:* -``` -git clone https://github.com/colmap/colmap.git -cd colmap -``` - -*3. Ensure Qt5 is in your PATH:* -``` -export PATH="/opt/homebrew/opt/qt@5/bin:$PATH" -``` - -*4. Create a build directory:* -``` -mkdir build -cd build -``` - -*5. After installing, link Qt5 to make sure it’s accessible:* -``` -brew link qt@5 --force -``` - -*6. Run CMake with the specific paths for ARM Mac(M1 and above):* -``` -cmake .. -GNinja \ - -DCMAKE_PREFIX_PATH="/opt/homebrew/opt/flann;/opt/homebrew/opt/metis;/opt/homebrew/opt/suite-sparse;/opt/homebrew/opt/qt@5;/opt/homebrew/opt/freeimage" -``` - -*7. Build and install COLMAP:* -``` -ninja -sudo ninja install -``` - -*8. Confirm COLMAP installation by running:* -``` -colmap -h -colmap gui -``` - -**This is the first part and will install Colmap on your device.** - - - -## Installing the GLOMAP: - -*1. Clone the GitHub repository:* -``` -git clone https://github.com/colmap/glomap -cd glomap -``` - -*2. Create a build directory:* -``` -mkdir build -cd build -``` - -*3. Export Environment Variables Again:* -``` -export PATH="/opt/homebrew/opt/qt@5/bin:$PATH" -export LDFLAGS="-L/opt/homebrew/opt/libomp/lib" -export CPPFLAGS="-I/opt/homebrew/opt/libomp/include" -export CMAKE_PREFIX_PATH="/opt/homebrew/opt/qt@5;/opt/homebrew/opt/libomp" -export PKG_CONFIG_PATH="/opt/homebrew/opt/qt@5/lib/pkgconfig" -``` - -*4. Run the CMake Command:* -``` -cmake -DCMAKE_PREFIX_PATH="/opt/homebrew/Cellar/qt@5/5.15.13_1;/opt/homebrew/opt/libomp" \ --DOpenMP_C_FLAGS="-Xclang -fopenmp" \ --DOpenMP_C_LIB_NAMES="libomp" \ --DOpenMP_CXX_FLAGS="-Xclang -fopenmp" \ --DOpenMP_CXX_LIB_NAMES="libomp" \ --DOpenMP_C_INCLUDE_DIRS="/opt/homebrew/opt/libomp/include" \ --DOpenMP_CXX_INCLUDE_DIRS="/opt/homebrew/opt/libomp/include" \ --DOpenMP_libomp_LIBRARY=/opt/homebrew/opt/libomp/lib/libomp.dylib \ --DOpenMP_INCLUDE_DIR=/opt/homebrew/opt/libomp/include \ -.. -GNinja -``` - -*5. Build the Project:* -``` -ninja -``` - -**NOTE: If at this point, there are build errors related to ‘cholmod.h’ or ‘omp.h’, clean the build and then re-run the make with the following commands:** -``` -cmake -DCMAKE_PREFIX_PATH="/opt/homebrew/Cellar/qt@5/5.15.13_1;/opt/homebrew/opt/libomp" \ --DOpenMP_C_FLAGS="-Xclang -fopenmp -I/opt/homebrew/opt/libomp/include" \ --DOpenMP_C_LIB_NAMES="libomp" \ --DOpenMP_CXX_FLAGS="-Xclang -fopenmp -I/opt/homebrew/opt/libomp/include" \ --DOpenMP_CXX_LIB_NAMES="libomp" \ --DOpenMP_libomp_LIBRARY=/opt/homebrew/opt/libomp/lib/libomp.dylib \ -.. -GNinja -``` - -**After the build is successful:** - -*6. Install the Built Project:* -``` -sudo ninja install -``` - -*7. Test the Installation:* -``` -glomap -h -``` - -**It should display something like:** -``` -GLOMAP -- Global Structure-from-Motion -Usage: -glomap mapper --database_path DATABASE --output_path -MODEL -glomap mapper_resume --input_path MODEL_INPUT --output_path MODEL_OUTPUT -Available commands: -help -mapper -mapper_resume -``` - - -## Testing with the end-to-end examples provided: - -*1. Open the end-to-end example database link provided:* -https://lpanaf.github.io/eccv24_glomap/ - -*2. Download one of the datasets provided and extract the zip file.* - -*3. Create a new directory named ‘data’ in the root directory ‘glomap’.* - -*4. Place the extracted dataset in the directory ‘data’.* - -*5. Now navigate back to the project directory ‘glomap’.* - -**NOTE: Following commands are assuming the dataset to be south-building:** - -*6. Extract Features with COLMAP:* -``` -colmap feature_extractor \ ---image_path ./data/south-building/images \ ---database_path ./data/south-building/database.db -``` - -*7. Match Features with COLMAP:* -``` -colmap exhaustive_matcher \ ---database_path ./data/south-building/database.db -``` - -*8. Run GLOMAP Mapper:* -``` -glomap mapper \ ---database_path ./data/south-building/database.db \ ---image_path ./data/south-building/images \ ---output_path ./output/south-building/sparse -``` - -**This should generate a new directory named ‘output’ in the project directory.** - -*9. Visualize the Results:* -``` -colmap gui \ ---database_path ./data/south-building/database.db \ ---image_path ./data/south-building/images \ ---import_path ./output/south-building/sparse/0 -``` diff --git a/glomap/CMakeLists.txt b/glomap/CMakeLists.txt index a5d82447..1325715b 100644 --- a/glomap/CMakeLists.txt +++ b/glomap/CMakeLists.txt @@ -84,10 +84,6 @@ target_link_libraries( ) target_include_directories(glomap PUBLIC ..) -if(OPENMP_FOUND) - target_link_libraries(glomap PUBLIC OpenMP::OpenMP_CXX) -endif() - if(MSVC) target_compile_options(glomap PRIVATE /bigobj) else() diff --git a/glomap/estimators/relpose_estimation.cc b/glomap/estimators/relpose_estimation.cc index 45f206bd..4676eb22 100644 --- a/glomap/estimators/relpose_estimation.cc +++ b/glomap/estimators/relpose_estimation.cc @@ -1,5 +1,7 @@ #include "glomap/estimators/relpose_estimation.h" +#include + #include namespace glomap { @@ -14,14 +16,13 @@ void EstimateRelativePoses(ViewGraph& view_graph, valid_pair_ids.push_back(image_pair_id); } - // Define outside loop to reuse memory and avoid reallocation. - std::vector points2D_1, points2D_2; - std::vector inliers; - const int64_t num_image_pairs = valid_pair_ids.size(); const int64_t kNumChunks = 10; const int64_t interval = std::ceil(static_cast(num_image_pairs) / kNumChunks); + + colmap::ThreadPool thread_pool(colmap::ThreadPool::kMaxNumThreads); + LOG(INFO) << "Estimating relative pose for " << num_image_pairs << " pairs"; for (int64_t chunk_id = 0; chunk_id < kNumChunks; chunk_id++) { std::cout << "\r Estimating relative pose: " << chunk_id * kNumChunks << "%" @@ -30,47 +31,55 @@ void EstimateRelativePoses(ViewGraph& view_graph, const int64_t end = std::min((chunk_id + 1) * interval, num_image_pairs); -#pragma omp parallel for schedule(dynamic) private( \ - points2D_1, points2D_2, inliers) for (int64_t pair_idx = start; pair_idx < end; pair_idx++) { - ImagePair& image_pair = view_graph.image_pairs[valid_pair_ids[pair_idx]]; - const Image& image1 = images[image_pair.image_id1]; - const Image& image2 = images[image_pair.image_id2]; - const Eigen::MatrixXi& matches = image_pair.matches; + thread_pool.AddTask([&, pair_idx]() { + // Define as thread-local to reuse memory allocation in different tasks. + thread_local std::vector points2D_1; + thread_local std::vector points2D_2; + thread_local std::vector inliers; + + ImagePair& image_pair = + view_graph.image_pairs[valid_pair_ids[pair_idx]]; + const Image& image1 = images[image_pair.image_id1]; + const Image& image2 = images[image_pair.image_id2]; + const Eigen::MatrixXi& matches = image_pair.matches; - // Collect the original 2D points - points2D_1.clear(); - points2D_2.clear(); - for (size_t idx = 0; idx < matches.rows(); idx++) { - points2D_1.push_back(image1.features[matches(idx, 0)]); - points2D_2.push_back(image2.features[matches(idx, 1)]); - } + // Collect the original 2D points + points2D_1.clear(); + points2D_2.clear(); + for (size_t idx = 0; idx < matches.rows(); idx++) { + points2D_1.push_back(image1.features[matches(idx, 0)]); + points2D_2.push_back(image2.features[matches(idx, 1)]); + } - inliers.clear(); - poselib::CameraPose pose_rel_calc; - try { - poselib::estimate_relative_pose( - points2D_1, - points2D_2, - ColmapCameraToPoseLibCamera(cameras[image1.camera_id]), - ColmapCameraToPoseLibCamera(cameras[image2.camera_id]), - options.ransac_options, - options.bundle_options, - &pose_rel_calc, - &inliers); - } catch (const std::exception& e) { - LOG(ERROR) << "Error in relative pose estimation: " << e.what(); - image_pair.is_valid = false; - continue; - } + inliers.clear(); + poselib::CameraPose pose_rel_calc; + try { + poselib::estimate_relative_pose( + points2D_1, + points2D_2, + ColmapCameraToPoseLibCamera(cameras[image1.camera_id]), + ColmapCameraToPoseLibCamera(cameras[image2.camera_id]), + options.ransac_options, + options.bundle_options, + &pose_rel_calc, + &inliers); + } catch (const std::exception& e) { + LOG(ERROR) << "Error in relative pose estimation: " << e.what(); + image_pair.is_valid = false; + return; + } - // Convert the relative pose to the glomap format - for (int i = 0; i < 4; i++) { - image_pair.cam2_from_cam1.rotation.coeffs()[i] = - pose_rel_calc.q[(i + 1) % 4]; - } - image_pair.cam2_from_cam1.translation = pose_rel_calc.t; + // Convert the relative pose to the glomap format + for (int i = 0; i < 4; i++) { + image_pair.cam2_from_cam1.rotation.coeffs()[i] = + pose_rel_calc.q[(i + 1) % 4]; + } + image_pair.cam2_from_cam1.translation = pose_rel_calc.t; + }); } + + thread_pool.Wait(); } std::cout << "\r Estimating relative pose: 100%" << std::endl; diff --git a/glomap/processors/image_undistorter.cc b/glomap/processors/image_undistorter.cc index c83402ae..012465d1 100644 --- a/glomap/processors/image_undistorter.cc +++ b/glomap/processors/image_undistorter.cc @@ -1,5 +1,7 @@ #include "glomap/processors/image_undistorter.h" +#include + namespace glomap { void UndistortImages(std::unordered_map& cameras, @@ -7,33 +9,35 @@ void UndistortImages(std::unordered_map& cameras, bool clean_points) { std::vector image_ids; for (auto& [image_id, image] : images) { - int num_points = image.features.size(); - + const int num_points = image.features.size(); if (image.features_undist.size() == num_points && !clean_points) continue; // already undistorted image_ids.push_back(image_id); } + colmap::ThreadPool thread_pool(colmap::ThreadPool::kMaxNumThreads); + LOG(INFO) << "Undistorting images.."; const int num_images = image_ids.size(); -#pragma omp parallel for for (int image_idx = 0; image_idx < num_images; image_idx++) { Image& image = images[image_ids[image_idx]]; - - int camera_id = image.camera_id; - Camera& camera = cameras[camera_id]; - int num_points = image.features.size(); - + const int num_points = image.features.size(); if (image.features_undist.size() == num_points && !clean_points) continue; // already undistorted - image.features_undist.clear(); - image.features_undist.reserve(num_points); - for (int i = 0; i < num_points; i++) { - image.features_undist.emplace_back( - camera.CamFromImg(image.features[i]).homogeneous().normalized()); - } + const Camera& camera = cameras[image.camera_id]; + + thread_pool.AddTask([&image, &camera, num_points]() { + image.features_undist.clear(); + image.features_undist.reserve(num_points); + for (int i = 0; i < num_points; i++) { + image.features_undist.emplace_back( + camera.CamFromImg(image.features[i]).homogeneous().normalized()); + } + }); } + + thread_pool.Wait(); LOG(INFO) << "Image undistortion done"; } diff --git a/glomap/processors/view_graph_manipulation.cc b/glomap/processors/view_graph_manipulation.cc index 9373fbb2..38ec4dc6 100644 --- a/glomap/processors/view_graph_manipulation.cc +++ b/glomap/processors/view_graph_manipulation.cc @@ -3,7 +3,10 @@ #include "glomap/math/two_view_geometry.h" #include "glomap/math/union_find.h" +#include + namespace glomap { + image_pair_t ViewGraphManipulater::SparsifyGraph( ViewGraph& view_graph, std::unordered_map& images, @@ -245,46 +248,51 @@ void ViewGraphManipulater::DecomposeRelPose( const int64_t num_image_pairs = image_pair_ids.size(); LOG(INFO) << "Decompose relative pose for " << num_image_pairs << " pairs"; -#pragma omp parallel for + colmap::ThreadPool thread_pool(colmap::ThreadPool::kMaxNumThreads); for (int64_t idx = 0; idx < num_image_pairs; idx++) { - ImagePair& image_pair = view_graph.image_pairs.at(image_pair_ids[idx]); - image_t image_id1 = image_pair.image_id1; - image_t image_id2 = image_pair.image_id2; + thread_pool.AddTask([&, idx]() { + ImagePair& image_pair = view_graph.image_pairs.at(image_pair_ids[idx]); + image_t image_id1 = image_pair.image_id1; + image_t image_id2 = image_pair.image_id2; - camera_t camera_id1 = images.at(image_id1).camera_id; - camera_t camera_id2 = images.at(image_id2).camera_id; - - // Use the two-view geometry to re-estimate the relative pose - colmap::TwoViewGeometry two_view_geometry; - two_view_geometry.E = image_pair.E; - two_view_geometry.F = image_pair.F; - two_view_geometry.H = image_pair.H; - two_view_geometry.config = image_pair.config; - - colmap::EstimateTwoViewGeometryPose(cameras[camera_id1], - images[image_id1].features, - cameras[camera_id2], - images[image_id2].features, - &two_view_geometry); - - // if it planar, then use the estimated relative pose - if (image_pair.config == colmap::TwoViewGeometry::PLANAR && - cameras[camera_id1].has_prior_focal_length && - cameras[camera_id2].has_prior_focal_length) { - image_pair.config = colmap::TwoViewGeometry::CALIBRATED; - continue; - } else if (!(cameras[camera_id1].has_prior_focal_length && - cameras[camera_id2].has_prior_focal_length)) - continue; + camera_t camera_id1 = images.at(image_id1).camera_id; + camera_t camera_id2 = images.at(image_id2).camera_id; + + // Use the two-view geometry to re-estimate the relative pose + colmap::TwoViewGeometry two_view_geometry; + two_view_geometry.E = image_pair.E; + two_view_geometry.F = image_pair.F; + two_view_geometry.H = image_pair.H; + two_view_geometry.config = image_pair.config; + + colmap::EstimateTwoViewGeometryPose(cameras[camera_id1], + images[image_id1].features, + cameras[camera_id2], + images[image_id2].features, + &two_view_geometry); + + // if it planar, then use the estimated relative pose + if (image_pair.config == colmap::TwoViewGeometry::PLANAR && + cameras[camera_id1].has_prior_focal_length && + cameras[camera_id2].has_prior_focal_length) { + image_pair.config = colmap::TwoViewGeometry::CALIBRATED; + return; + } else if (!(cameras[camera_id1].has_prior_focal_length && + cameras[camera_id2].has_prior_focal_length)) + return; + + image_pair.config = two_view_geometry.config; + image_pair.cam2_from_cam1 = two_view_geometry.cam2_from_cam1; + + if (image_pair.cam2_from_cam1.translation.norm() > EPS) { + image_pair.cam2_from_cam1.translation = + image_pair.cam2_from_cam1.translation.normalized(); + } + }); + } - image_pair.config = two_view_geometry.config; - image_pair.cam2_from_cam1 = two_view_geometry.cam2_from_cam1; + thread_pool.Wait(); - if (image_pair.cam2_from_cam1.translation.norm() > EPS) { - image_pair.cam2_from_cam1.translation = - image_pair.cam2_from_cam1.translation.normalized(); - } - } size_t counter = 0; for (size_t idx = 0; idx < image_pair_ids.size(); idx++) { ImagePair& image_pair = view_graph.image_pairs.at(image_pair_ids[idx]); From 0c0431daac91fecb44ced77bac107ba57dd0b1fa Mon Sep 17 00:00:00 2001 From: Zhihao Zhan <92145772+zhan994@users.noreply.github.com> Date: Fri, 18 Oct 2024 19:21:01 +0800 Subject: [PATCH 18/45] fix calculation for fundmental_matrix (#125) --- glomap/math/two_view_geometry.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/glomap/math/two_view_geometry.cc b/glomap/math/two_view_geometry.cc index 3d9a5d1d..b7d5f5d1 100644 --- a/glomap/math/two_view_geometry.cc +++ b/glomap/math/two_view_geometry.cc @@ -51,7 +51,7 @@ void FundamentalFromMotionAndCameras(const Camera& camera1, Eigen::Matrix3d* F) { Eigen::Matrix3d E; EssentialFromMotion(pose, &E); - *F = camera1.GetK().transpose().inverse() * E * camera2.GetK().inverse(); + *F = camera2.GetK().transpose().inverse() * E * camera1.GetK().inverse(); } double SampsonError(const Eigen::Matrix3d& E, From 323d2cfd59e67c62d836c45acf337cae361ba9ea Mon Sep 17 00:00:00 2001 From: Paul-Edouard Sarlin <15985472+sarlinpe@users.noreply.github.com> Date: Fri, 15 Nov 2024 17:41:35 +0100 Subject: [PATCH 19/45] Update to COLMAP HEAD (#132) * Update to COLMAP HEAD * Bump COLMAP tag * Install gmock in CI * Apply clang-tidy only to GLOMAP headers * Fix clang-tidy regexp header filter * Make PoseLib a system include --- .github/workflows/ubuntu.yml | 1 + cmake/FindDependencies.cmake | 3 ++- glomap/controllers/global_mapper.cc | 3 ++- glomap/controllers/track_retriangulation.cc | 16 +++++++++------- glomap/estimators/bundle_adjustment.cc | 2 +- glomap/exe/global_mapper.cc | 3 ++- glomap/io/colmap_converter.cc | 11 +++++++---- glomap/io/colmap_io.cc | 1 + 8 files changed, 25 insertions(+), 15 deletions(-) diff --git a/.github/workflows/ubuntu.yml b/.github/workflows/ubuntu.yml index a8246f96..58000156 100644 --- a/.github/workflows/ubuntu.yml +++ b/.github/workflows/ubuntu.yml @@ -107,6 +107,7 @@ jobs: libmetis-dev \ libgoogle-glog-dev \ libgtest-dev \ + libgmock-dev \ libsqlite3-dev \ libglew-dev \ qtbase5-dev \ diff --git a/cmake/FindDependencies.cmake b/cmake/FindDependencies.cmake index 7bcf2020..5a89c7c3 100644 --- a/cmake/FindDependencies.cmake +++ b/cmake/FindDependencies.cmake @@ -24,6 +24,7 @@ FetchContent_Declare(PoseLib GIT_REPOSITORY https://github.com/PoseLib/PoseLib.git GIT_TAG 0439b2d361125915b8821043fca9376e6cc575b9 EXCLUDE_FROM_ALL + SYSTEM ) message(STATUS "Configuring PoseLib...") if (FETCH_POSELIB) @@ -35,7 +36,7 @@ message(STATUS "Configuring PoseLib... done") FetchContent_Declare(COLMAP GIT_REPOSITORY https://github.com/colmap/colmap.git - GIT_TAG 66fd8e56a0d160d68af2f29e9ac6941d442d2322 + GIT_TAG 3254263266949413d7c669e44abeb4a7c2670a8b EXCLUDE_FROM_ALL ) message(STATUS "Configuring COLMAP...") diff --git a/glomap/controllers/global_mapper.cc b/glomap/controllers/global_mapper.cc index 22f14c8f..c3594199 100644 --- a/glomap/controllers/global_mapper.cc +++ b/glomap/controllers/global_mapper.cc @@ -7,6 +7,7 @@ #include "glomap/processors/track_filter.h" #include "glomap/processors/view_graph_manipulation.h" +#include #include namespace glomap { @@ -325,4 +326,4 @@ bool GlobalMapper::Solve(const colmap::Database& database, return true; } -} // namespace glomap \ No newline at end of file +} // namespace glomap diff --git a/glomap/controllers/track_retriangulation.cc b/glomap/controllers/track_retriangulation.cc index 18559c20..678f99a1 100644 --- a/glomap/controllers/track_retriangulation.cc +++ b/glomap/controllers/track_retriangulation.cc @@ -2,10 +2,12 @@ #include "glomap/io/colmap_converter.h" -#include +#include #include #include +#include + namespace glomap { bool RetriangulateTracks(const TriangulatorOptions& options, @@ -40,7 +42,7 @@ bool RetriangulateTracks(const TriangulatorOptions& options, std::unordered_map(), *reconstruction_ptr); - colmap::IncrementalMapperOptions options_colmap; + colmap::IncrementalPipelineOptions options_colmap; options_colmap.triangulation.complete_max_reproj_error = options.tri_complete_max_reproj_error; options_colmap.triangulation.merge_max_reproj_error = @@ -57,13 +59,13 @@ bool RetriangulateTracks(const TriangulatorOptions& options, const auto tri_options = options_colmap.Triangulation(); const auto mapper_options = options_colmap.Mapper(); - const std::vector& reg_image_ids = reconstruction_ptr->RegImageIds(); + const std::set& reg_image_ids = reconstruction_ptr->RegImageIds(); - for (size_t i = 0; i < reg_image_ids.size(); ++i) { - std::cout << "\r Triangulating image " << i + 1 << " / " + size_t image_idx = 0; + for (const image_t image_id : reg_image_ids) { + std::cout << "\r Triangulating image " << image_idx++ + 1 << " / " << reg_image_ids.size() << std::flush; - const image_t image_id = reg_image_ids[i]; const auto& image = reconstruction_ptr->Image(image_id); int num_tris = mapper.TriangulateImage(tri_options, image_id); @@ -128,4 +130,4 @@ bool RetriangulateTracks(const TriangulatorOptions& options, return true; } -} // namespace glomap \ No newline at end of file +} // namespace glomap diff --git a/glomap/estimators/bundle_adjustment.cc b/glomap/estimators/bundle_adjustment.cc index 32204baf..f9aa51c4 100644 --- a/glomap/estimators/bundle_adjustment.cc +++ b/glomap/estimators/bundle_adjustment.cc @@ -70,7 +70,7 @@ void BundleAdjuster::AddPointToCameraConstraints( Image& image = images[observation.first]; ceres::CostFunction* cost_function = - colmap::CameraCostFunction( + colmap::CreateCameraCostFunction( cameras[image.camera_id].model_id, image.features[observation.second]); diff --git a/glomap/exe/global_mapper.cc b/glomap/exe/global_mapper.cc index 7e4deac3..fb1e512c 100644 --- a/glomap/exe/global_mapper.cc +++ b/glomap/exe/global_mapper.cc @@ -4,6 +4,7 @@ #include "glomap/io/colmap_io.h" #include "glomap/types.h" +#include #include #include @@ -151,4 +152,4 @@ int RunMapperResume(int argc, char** argv) { return EXIT_SUCCESS; } -} // namespace glomap \ No newline at end of file +} // namespace glomap diff --git a/glomap/io/colmap_converter.cc b/glomap/io/colmap_converter.cc index eceaead4..75ab3ba4 100644 --- a/glomap/io/colmap_converter.cc +++ b/glomap/io/colmap_converter.cc @@ -9,9 +9,10 @@ void ConvertGlomapToColmapImage(const Image& image, bool keep_points) { image_colmap.SetImageId(image.image_id); image_colmap.SetCameraId(image.camera_id); - image_colmap.SetRegistered(image.is_registered); image_colmap.SetName(image.file_name); - image_colmap.CamFromWorld() = image.cam_from_world; + if (image.is_registered) { + image_colmap.SetCamFromWorld(image.cam_from_world); + } if (keep_points) { image_colmap.SetPoints2D(image.features); @@ -130,8 +131,10 @@ void ConvertColmapToGlomap(const colmap::Reconstruction& reconstruction, image_colmap.Name()))); Image& image = ite.first->second; - image.is_registered = image_colmap.IsRegistered(); - image.cam_from_world = static_cast(image_colmap.CamFromWorld()); + image.is_registered = image_colmap.HasPose(); + if (image_colmap.HasPose()) { + image.cam_from_world = static_cast(image_colmap.CamFromWorld()); + } image.features.clear(); image.features.reserve(image_colmap.NumPoints2D()); diff --git a/glomap/io/colmap_io.cc b/glomap/io/colmap_io.cc index 88dc4ccf..5189e6d2 100644 --- a/glomap/io/colmap_io.cc +++ b/glomap/io/colmap_io.cc @@ -1,5 +1,6 @@ #include "glomap/io/colmap_io.h" +#include #include namespace glomap { From 03d4818a6b0e9fda9e9f03817e69498bb754aa22 Mon Sep 17 00:00:00 2001 From: Linfei Pan <36349740+lpanaf@users.noreply.github.com> Date: Tue, 19 Nov 2024 13:45:51 +0100 Subject: [PATCH 20/45] Add reconstruction normalization after each step (#136) * add reconstruction normalization after each step * seperate out the helper function for normalization * d * f --- glomap/CMakeLists.txt | 2 + glomap/controllers/global_mapper.cc | 12 +++ .../processors/reconstruction_normalizer.cc | 75 +++++++++++++++++++ glomap/processors/reconstruction_normalizer.h | 17 +++++ 4 files changed, 106 insertions(+) create mode 100644 glomap/processors/reconstruction_normalizer.cc create mode 100644 glomap/processors/reconstruction_normalizer.h diff --git a/glomap/CMakeLists.txt b/glomap/CMakeLists.txt index 1325715b..7d60ee17 100644 --- a/glomap/CMakeLists.txt +++ b/glomap/CMakeLists.txt @@ -18,6 +18,7 @@ set(SOURCES math/two_view_geometry.cc processors/image_pair_inliers.cc processors/image_undistorter.cc + processors/reconstruction_normalizer.cc processors/reconstruction_pruning.cc processors/relpose_filter.cc processors/track_filter.cc @@ -49,6 +50,7 @@ set(HEADERS math/union_find.h processors/image_pair_inliers.h processors/image_undistorter.h + processors/reconstruction_normalizer.h processors/reconstruction_pruning.h processors/relpose_filter.h processors/track_filter.h diff --git a/glomap/controllers/global_mapper.cc b/glomap/controllers/global_mapper.cc index c3594199..6de88d92 100644 --- a/glomap/controllers/global_mapper.cc +++ b/glomap/controllers/global_mapper.cc @@ -1,7 +1,9 @@ #include "global_mapper.h" +#include "glomap/io/colmap_converter.h" #include "glomap/processors/image_pair_inliers.h" #include "glomap/processors/image_undistorter.h" +#include "glomap/processors/reconstruction_normalizer.h" #include "glomap/processors/reconstruction_pruning.h" #include "glomap/processors/relpose_filter.h" #include "glomap/processors/track_filter.h" @@ -167,6 +169,10 @@ bool GlobalMapper::Solve(const colmap::Database& database, images, tracks, options_.inlier_thresholds.max_angle_error); + + // Normalize the structure + NormalizeReconstruction(cameras, images, tracks); + run_timer.PrintSeconds(); } @@ -209,6 +215,9 @@ bool GlobalMapper::Solve(const colmap::Database& database, if (ite != options_.num_iteration_bundle_adjustment - 1) run_timer.PrintSeconds(); + // Normalize the structure + NormalizeReconstruction(cameras, images, tracks); + // 6.3. Filter tracks based on the estimation // For the filtering, in each round, the criteria for outlier is // tightened. If only few tracks are changed, no need to start bundle @@ -292,6 +301,9 @@ bool GlobalMapper::Solve(const colmap::Database& database, run_timer.PrintSeconds(); } + // Normalize the structure + NormalizeReconstruction(cameras, images, tracks); + // Filter tracks based on the estimation UndistortImages(cameras, images, true); LOG(INFO) << "Filtering tracks by reprojection ..."; diff --git a/glomap/processors/reconstruction_normalizer.cc b/glomap/processors/reconstruction_normalizer.cc new file mode 100644 index 00000000..b2af04a7 --- /dev/null +++ b/glomap/processors/reconstruction_normalizer.cc @@ -0,0 +1,75 @@ +#include "reconstruction_normalizer.h" + +namespace glomap { + +colmap::Sim3d NormalizeReconstruction( + std::unordered_map& cameras, + std::unordered_map& images, + std::unordered_map& tracks, + bool fixed_scale, + double extent, + double p0, + double p1) { + // Coordinates of image centers or point locations. + std::vector coords_x; + std::vector coords_y; + std::vector coords_z; + + coords_x.reserve(images.size()); + coords_y.reserve(images.size()); + coords_z.reserve(images.size()); + for (const auto& [image_id, image] : images) { + if (!image.is_registered) continue; + const Eigen::Vector3d proj_center = image.Center(); + coords_x.push_back(static_cast(proj_center(0))); + coords_y.push_back(static_cast(proj_center(1))); + coords_z.push_back(static_cast(proj_center(2))); + } + + // Determine robust bounding box and mean. + std::sort(coords_x.begin(), coords_x.end()); + std::sort(coords_y.begin(), coords_y.end()); + std::sort(coords_z.begin(), coords_z.end()); + + const size_t P0 = static_cast( + (coords_x.size() > 3) ? p0 * (coords_x.size() - 1) : 0); + const size_t P1 = static_cast( + (coords_x.size() > 3) ? p1 * (coords_x.size() - 1) : coords_x.size() - 1); + + const Eigen::Vector3d bbox_min(coords_x[P0], coords_y[P0], coords_z[P0]); + const Eigen::Vector3d bbox_max(coords_x[P1], coords_y[P1], coords_z[P1]); + + Eigen::Vector3d mean_coord(0, 0, 0); + for (size_t i = P0; i <= P1; ++i) { + mean_coord(0) += coords_x[i]; + mean_coord(1) += coords_y[i]; + mean_coord(2) += coords_z[i]; + } + mean_coord /= P1 - P0 + 1; + + // Calculate scale and translation, such that + // translation is applied before scaling. + double scale = 1.; + if (!fixed_scale) { + const double old_extent = (bbox_max - bbox_min).norm(); + if (old_extent >= std::numeric_limits::epsilon()) { + scale = extent / old_extent; + } + } + colmap::Sim3d tform( + scale, Eigen::Quaterniond::Identity(), -scale * mean_coord); + + for (auto& [_, image] : images) { + if (image.is_registered) { + image.cam_from_world = TransformCameraWorld(tform, image.cam_from_world); + } + } + + for (auto& [_, track] : tracks) { + track.xyz = tform * track.xyz; + } + + return tform; +} + +} // namespace glomap \ No newline at end of file diff --git a/glomap/processors/reconstruction_normalizer.h b/glomap/processors/reconstruction_normalizer.h new file mode 100644 index 00000000..51d51fd6 --- /dev/null +++ b/glomap/processors/reconstruction_normalizer.h @@ -0,0 +1,17 @@ +#pragma once + +#include "glomap/scene/types_sfm.h" + +#include "colmap/geometry/pose.h" + +namespace glomap { + +colmap::Sim3d NormalizeReconstruction( + std::unordered_map& cameras, + std::unordered_map& images, + std::unordered_map& tracks, + bool fixed_scale = false, + double extent = 10., + double p0 = 0.1, + double p1 = 0.9); +} // namespace glomap From c2260dd5afdcacd28b6866c9931957c6f303ff5c Mon Sep 17 00:00:00 2001 From: Jonas Konrad <79854538+JonasKonrad@users.noreply.github.com> Date: Wed, 27 Nov 2024 13:34:15 +0100 Subject: [PATCH 21/45] Moved glog version export out of MSVC scope. (#140) --- cmake/FindDependencies.cmake | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/cmake/FindDependencies.cmake b/cmake/FindDependencies.cmake index 5a89c7c3..f4eba332 100644 --- a/cmake/FindDependencies.cmake +++ b/cmake/FindDependencies.cmake @@ -7,11 +7,12 @@ find_package(Boost REQUIRED) if(CMAKE_CXX_COMPILER_ID STREQUAL "MSVC") find_package(Glog REQUIRED) - if(DEFINED glog_VERSION_MAJOR) - # Older versions of glog don't export version variables. - add_definitions("-DGLOG_VERSION_MAJOR=${glog_VERSION_MAJOR}") - add_definitions("-DGLOG_VERSION_MINOR=${glog_VERSION_MINOR}") - endif() +endif() + +if(DEFINED glog_VERSION_MAJOR) + # Older versions of glog don't export version variables. + add_definitions("-DGLOG_VERSION_MAJOR=${glog_VERSION_MAJOR}") + add_definitions("-DGLOG_VERSION_MINOR=${glog_VERSION_MINOR}") endif() if(TESTS_ENABLED) From 2efb13ad6278547f258af23b272dee72976bc6fd Mon Sep 17 00:00:00 2001 From: Linfei Pan <36349740+lpanaf@users.noreply.github.com> Date: Thu, 28 Nov 2024 15:00:54 +0100 Subject: [PATCH 22/45] update colmap version (#143) * update colmap version * f * d --- cmake/FindDependencies.cmake | 2 +- glomap/controllers/track_retriangulation.cc | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/cmake/FindDependencies.cmake b/cmake/FindDependencies.cmake index f4eba332..d7e5e586 100644 --- a/cmake/FindDependencies.cmake +++ b/cmake/FindDependencies.cmake @@ -37,7 +37,7 @@ message(STATUS "Configuring PoseLib... done") FetchContent_Declare(COLMAP GIT_REPOSITORY https://github.com/colmap/colmap.git - GIT_TAG 3254263266949413d7c669e44abeb4a7c2670a8b + GIT_TAG 78f1eefacae542d753c2e4f6a26771a0d976227d EXCLUDE_FROM_ALL ) message(STATUS "Configuring COLMAP...") diff --git a/glomap/controllers/track_retriangulation.cc b/glomap/controllers/track_retriangulation.cc index 678f99a1..92d73e0f 100644 --- a/glomap/controllers/track_retriangulation.cc +++ b/glomap/controllers/track_retriangulation.cc @@ -98,10 +98,10 @@ bool RetriangulateTracks(const TriangulatorOptions& options, const size_t num_observations = reconstruction_ptr->ComputeNumObservations(); - // PrintHeading1("Bundle adjustment"); - colmap::BundleAdjuster bundle_adjuster(ba_options, ba_config); - // THROW_CHECK(bundle_adjuster.Solve(reconstruction.get())); - if (!bundle_adjuster.Solve(reconstruction_ptr.get())) { + std::unique_ptr bundle_adjuster; + bundle_adjuster = + CreateDefaultBundleAdjuster(ba_options, ba_config, *reconstruction_ptr); + if (bundle_adjuster->Solve().termination_type == ceres::FAILURE) { return false; } From e7f37ac21c25e1d8c75aa880e4c4f7158bfd641d Mon Sep 17 00:00:00 2001 From: Linfei Pan <36349740+lpanaf@users.noreply.github.com> Date: Fri, 13 Dec 2024 15:10:26 +0100 Subject: [PATCH 23/45] fix the bug for threshold update (#148) * fix the bug that the update of threshold from command line is not reflected in the structure * f * update * refactor the loss_function * d --- glomap/estimators/bundle_adjustment.cc | 3 ++- glomap/estimators/bundle_adjustment.h | 6 +++++- glomap/estimators/global_positioning.cc | 14 +++++++------- glomap/estimators/global_positioning.h | 6 +++++- glomap/estimators/gravity_refinement.cc | 5 +++-- glomap/estimators/gravity_refinement.h | 7 +++++-- glomap/estimators/optimization_base.h | 3 --- glomap/estimators/view_graph_calibration.cc | 7 +++---- glomap/estimators/view_graph_calibration.h | 5 +++++ pybind11 | 1 + pyglomap/pybind11 | 1 + 11 files changed, 37 insertions(+), 21 deletions(-) create mode 160000 pybind11 create mode 160000 pyglomap/pybind11 diff --git a/glomap/estimators/bundle_adjustment.cc b/glomap/estimators/bundle_adjustment.cc index f9aa51c4..ee2be7a7 100644 --- a/glomap/estimators/bundle_adjustment.cc +++ b/glomap/estimators/bundle_adjustment.cc @@ -54,6 +54,7 @@ void BundleAdjuster::Reset() { ceres::Problem::Options problem_options; problem_options.loss_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; problem_ = std::make_unique(problem_options); + loss_function_ = options_.CreateLossFunction(); } void BundleAdjuster::AddPointToCameraConstraints( @@ -77,7 +78,7 @@ void BundleAdjuster::AddPointToCameraConstraints( if (cost_function != nullptr) { problem_->AddResidualBlock( cost_function, - options_.loss_function.get(), + loss_function_.get(), image.cam_from_world.rotation.coeffs().data(), image.cam_from_world.translation.data(), tracks[track_id].xyz.data(), diff --git a/glomap/estimators/bundle_adjustment.h b/glomap/estimators/bundle_adjustment.h index 97419a5d..76a81b9e 100644 --- a/glomap/estimators/bundle_adjustment.h +++ b/glomap/estimators/bundle_adjustment.h @@ -21,9 +21,12 @@ struct BundleAdjusterOptions : public OptimizationBaseOptions { BundleAdjusterOptions() : OptimizationBaseOptions() { thres_loss_function = 1.; - loss_function = std::make_shared(thres_loss_function); solver_options.max_num_iterations = 200; } + + std::shared_ptr CreateLossFunction() { + return std::make_shared(thres_loss_function); + } }; class BundleAdjuster { @@ -65,6 +68,7 @@ class BundleAdjuster { BundleAdjusterOptions options_; std::unique_ptr problem_; + std::shared_ptr loss_function_; }; } // namespace glomap diff --git a/glomap/estimators/global_positioning.cc b/glomap/estimators/global_positioning.cc index b18fa80b..8a9b064a 100644 --- a/glomap/estimators/global_positioning.cc +++ b/glomap/estimators/global_positioning.cc @@ -87,6 +87,8 @@ void GlobalPositioner::SetupProblem( ceres::Problem::Options problem_options; problem_options.loss_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; problem_ = std::make_unique(problem_options); + loss_function_ = options_.CreateLossFunction(); + // Allocate enough memory for the scales. One for each residual. // Due to possibly invalid image pairs or tracks, the actual number of // residuals may be smaller. @@ -169,7 +171,7 @@ void GlobalPositioner::AddCameraToCameraConstraints( BATAPairwiseDirectionError::Create(translation); problem_->AddResidualBlock( cost_function, - options_.loss_function.get(), + loss_function_.get(), images[image_id1].cam_from_world.translation.data(), images[image_id2].cam_from_world.translation.data(), &scale); @@ -212,19 +214,17 @@ void GlobalPositioner::AddPointToCameraConstraints( if (loss_function_ptcam_uncalibrated_ == nullptr) { loss_function_ptcam_uncalibrated_ = - std::make_shared(options_.loss_function.get(), + std::make_shared(loss_function_.get(), 0.5 * weight_scale_pt, ceres::DO_NOT_TAKE_OWNERSHIP); } if (options_.constraint_type == GlobalPositionerOptions::POINTS_AND_CAMERAS_BALANCED) { - loss_function_ptcam_calibrated_ = - std::make_shared(options_.loss_function.get(), - weight_scale_pt, - ceres::DO_NOT_TAKE_OWNERSHIP); + loss_function_ptcam_calibrated_ = std::make_shared( + loss_function_.get(), weight_scale_pt, ceres::DO_NOT_TAKE_OWNERSHIP); } else { - loss_function_ptcam_calibrated_ = options_.loss_function; + loss_function_ptcam_calibrated_ = loss_function_; } for (auto& [track_id, track] : tracks) { diff --git a/glomap/estimators/global_positioning.h b/glomap/estimators/global_positioning.h index 0eb1185c..3926ff7b 100644 --- a/glomap/estimators/global_positioning.h +++ b/glomap/estimators/global_positioning.h @@ -42,7 +42,10 @@ struct GlobalPositionerOptions : public OptimizationBaseOptions { GlobalPositionerOptions() : OptimizationBaseOptions() { thres_loss_function = 1e-1; - loss_function = std::make_shared(thres_loss_function); + } + + std::shared_ptr CreateLossFunction() { + return std::make_shared(thres_loss_function); } }; @@ -104,6 +107,7 @@ class GlobalPositioner { std::unique_ptr problem_; // Loss functions for reweighted terms. + std::shared_ptr loss_function_; std::shared_ptr loss_function_ptcam_uncalibrated_; std::shared_ptr loss_function_ptcam_calibrated_; diff --git a/glomap/estimators/gravity_refinement.cc b/glomap/estimators/gravity_refinement.cc index 6015d66b..d1f96fa7 100644 --- a/glomap/estimators/gravity_refinement.cc +++ b/glomap/estimators/gravity_refinement.cc @@ -23,6 +23,8 @@ void GravityRefiner::RefineGravity(const ViewGraph& view_graph, return; } + loss_function_ = options_.CreateLossFunction(); + int counter_progress = 0; // Iterate through the error prone images for (auto image_id : error_prone_images) { @@ -66,8 +68,7 @@ void GravityRefiner::RefineGravity(const ViewGraph& view_graph, ceres::CostFunction* coor_cost = GravError::CreateCost(gravities[counter]); - problem.AddResidualBlock( - coor_cost, options_.loss_function.get(), gravity.data()); + problem.AddResidualBlock(coor_cost, loss_function_.get(), gravity.data()); counter++; } diff --git a/glomap/estimators/gravity_refinement.h b/glomap/estimators/gravity_refinement.h index 581b434e..b9667155 100644 --- a/glomap/estimators/gravity_refinement.h +++ b/glomap/estimators/gravity_refinement.h @@ -17,8 +17,10 @@ struct GravityRefinerOptions : public OptimizationBaseOptions { // Only refine the gravity of the images with more than min_neighbors int min_num_neighbors = 7; - GravityRefinerOptions() : OptimizationBaseOptions() { - loss_function = std::make_shared( + GravityRefinerOptions() : OptimizationBaseOptions() {} + + std::shared_ptr CreateLossFunction() { + return std::make_shared( 1 - std::cos(DegToRad(max_gravity_error))); } }; @@ -35,6 +37,7 @@ class GravityRefiner { const std::unordered_map& images, std::unordered_set& error_prone_images); GravityRefinerOptions options_; + std::shared_ptr loss_function_; }; } // namespace glomap diff --git a/glomap/estimators/optimization_base.h b/glomap/estimators/optimization_base.h index 2b43e066..c6a67abb 100644 --- a/glomap/estimators/optimization_base.h +++ b/glomap/estimators/optimization_base.h @@ -12,9 +12,6 @@ struct OptimizationBaseOptions { // The threshold for the loss function double thres_loss_function = 1e-1; - // The loss function for the calibration - std::shared_ptr loss_function; - // The options for the solver ceres::Solver::Options solver_options; diff --git a/glomap/estimators/view_graph_calibration.cc b/glomap/estimators/view_graph_calibration.cc index 7d5b4572..30c7d343 100644 --- a/glomap/estimators/view_graph_calibration.cc +++ b/glomap/estimators/view_graph_calibration.cc @@ -61,8 +61,7 @@ void ViewGraphCalibrator::Reset( ceres::Problem::Options problem_options; problem_options.loss_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; problem_ = std::make_unique(problem_options); - options_.loss_function = - std::make_shared(options_.thres_loss_function); + loss_function_ = options_.CreateLossFunction(); } void ViewGraphCalibrator::AddImagePairsToProblem( @@ -90,14 +89,14 @@ void ViewGraphCalibrator::AddImagePair( problem_->AddResidualBlock( FetzerFocalLengthSameCameraCost::Create( image_pair.F, cameras.at(camera_id1).PrincipalPoint()), - options_.loss_function.get(), + loss_function_.get(), &(focals_[camera_id1])); } else { problem_->AddResidualBlock( FetzerFocalLengthCost::Create(image_pair.F, cameras.at(camera_id1).PrincipalPoint(), cameras.at(camera_id2).PrincipalPoint()), - options_.loss_function.get(), + loss_function_.get(), &(focals_[camera_id1]), &(focals_[camera_id2])); } diff --git a/glomap/estimators/view_graph_calibration.h b/glomap/estimators/view_graph_calibration.h index f7f5104e..c1cebd65 100644 --- a/glomap/estimators/view_graph_calibration.h +++ b/glomap/estimators/view_graph_calibration.h @@ -21,6 +21,10 @@ struct ViewGraphCalibratorOptions : public OptimizationBaseOptions { ViewGraphCalibratorOptions() : OptimizationBaseOptions() { thres_loss_function = 1e-2; } + + std::shared_ptr CreateLossFunction() { + return std::make_shared(thres_loss_function); + } }; class ViewGraphCalibrator { @@ -61,6 +65,7 @@ class ViewGraphCalibrator { ViewGraphCalibratorOptions options_; std::unique_ptr problem_; std::unordered_map focals_; + std::shared_ptr loss_function_; }; } // namespace glomap diff --git a/pybind11 b/pybind11 new file mode 160000 index 00000000..3ebdc503 --- /dev/null +++ b/pybind11 @@ -0,0 +1 @@ +Subproject commit 3ebdc503d29c7f089b9a0bc1823add0dda76f40d diff --git a/pyglomap/pybind11 b/pyglomap/pybind11 new file mode 160000 index 00000000..3ebdc503 --- /dev/null +++ b/pyglomap/pybind11 @@ -0,0 +1 @@ +Subproject commit 3ebdc503d29c7f089b9a0bc1823add0dda76f40d From 9a8361b99fdd49ce06d9ed7b3272e27d99eee7d9 Mon Sep 17 00:00:00 2001 From: duan-she-li <1063135843@qq.com> Date: Fri, 13 Dec 2024 22:23:23 +0800 Subject: [PATCH 24/45] fix (#149) --- glomap/io/colmap_converter.cc | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/glomap/io/colmap_converter.cc b/glomap/io/colmap_converter.cc index 75ab3ba4..eb4bf515 100644 --- a/glomap/io/colmap_converter.cc +++ b/glomap/io/colmap_converter.cc @@ -34,6 +34,7 @@ void ConvertGlomapToColmap(const std::unordered_map& cameras, } // Prepare the 2d-3d correspondences + size_t min_supports = 2; std::unordered_map> image_to_point3D; if (tracks.size() > 0 || include_image_points) { // Initialize every point to corresponds to invalid point @@ -47,7 +48,7 @@ void ConvertGlomapToColmap(const std::unordered_map& cameras, if (tracks.size() > 0) { for (auto& [track_id, track] : tracks) { - if (track.observations.size() < 3) { + if (track.observations.size() < min_supports) { continue; } for (auto& observation : track.observations) { @@ -80,7 +81,7 @@ void ConvertGlomapToColmap(const std::unordered_map& cameras, colmap_point.track.AddElement(colmap_track_el); } - if (colmap_point.track.Length() < 2) continue; + if (colmap_point.track.Length() < min_supports) continue; colmap_point.track.Compress(); reconstruction.AddPoint3D(track_id, std::move(colmap_point)); From af514ac03d16f679e0acfaddb3be6ecf05ca7055 Mon Sep 17 00:00:00 2001 From: Linfei Pan <36349740+lpanaf@users.noreply.github.com> Date: Fri, 13 Dec 2024 19:19:24 +0100 Subject: [PATCH 25/45] fix colmap convention for the pair id calculation (#150) --- glomap/scene/types.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/glomap/scene/types.h b/glomap/scene/types.h index 3499a10f..dc3aa038 100644 --- a/glomap/scene/types.h +++ b/glomap/scene/types.h @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include @@ -33,7 +34,7 @@ typedef uint64_t track_t; using colmap::Rigid3d; -const image_t kMaxNumImages = std::numeric_limits::max(); +const image_t kMaxNumImages = colmap::Database::kMaxNumImages; const image_pair_t kInvalidImagePairId = -1; } // namespace glomap From 606db2dcefaebc457009c63961b43458c79cdaba Mon Sep 17 00:00:00 2001 From: Linfei Pan <36349740+lpanaf@users.noreply.github.com> Date: Tue, 17 Dec 2024 15:03:41 +0100 Subject: [PATCH 26/45] avoid the inconsistency between the exported 3d point and 3d point in images (#151) --- glomap/io/colmap_converter.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/glomap/io/colmap_converter.cc b/glomap/io/colmap_converter.cc index eb4bf515..a52455cd 100644 --- a/glomap/io/colmap_converter.cc +++ b/glomap/io/colmap_converter.cc @@ -81,7 +81,7 @@ void ConvertGlomapToColmap(const std::unordered_map& cameras, colmap_point.track.AddElement(colmap_track_el); } - if (colmap_point.track.Length() < min_supports) continue; + if (track.observations.size() < min_supports) continue; colmap_point.track.Compress(); reconstruction.AddPoint3D(track_id, std::move(colmap_point)); From b464ef5f5dc5cdf2640efd91bd88cba6f017dc4f Mon Sep 17 00:00:00 2001 From: Linfei Pan <36349740+lpanaf@users.noreply.github.com> Date: Thu, 13 Mar 2025 13:56:12 +0100 Subject: [PATCH 27/45] add more options in CLI and add support to optimize principal point (#170) * add more options in CLI and add support to optimize principal point * d * by default, do not optimize pp --- glomap/controllers/option_manager.cc | 10 ++++++++++ glomap/estimators/bundle_adjustment.cc | 6 +++--- glomap/estimators/bundle_adjustment.h | 1 + 3 files changed, 14 insertions(+), 3 deletions(-) diff --git a/glomap/controllers/option_manager.cc b/glomap/controllers/option_manager.cc index d1e03042..9b8fb6a1 100644 --- a/glomap/controllers/option_manager.cc +++ b/glomap/controllers/option_manager.cc @@ -205,6 +205,8 @@ void OptionManager::AddBundleAdjusterOptions() { &mapper->opt_ba.optimize_translation); AddAndRegisterDefaultOption("BundleAdjustment.optimize_intrinsics", &mapper->opt_ba.optimize_intrinsics); + AddAndRegisterDefaultOption("BundleAdjustment.optimize_principal_point", + &mapper->opt_ba.optimize_principal_point); AddAndRegisterDefaultOption("BundleAdjustment.optimize_points", &mapper->opt_ba.optimize_points); AddAndRegisterDefaultOption("BundleAdjustment.thres_loss_function", @@ -234,6 +236,14 @@ void OptionManager::AddInlierThresholdOptions() { return; } added_inliers_options_ = true; + AddAndRegisterDefaultOption("Thresholds.max_angle_error", + &mapper->inlier_thresholds.max_angle_error); + AddAndRegisterDefaultOption( + "Thresholds.max_reprojection_error", + &mapper->inlier_thresholds.max_reprojection_error); + AddAndRegisterDefaultOption( + "Thresholds.min_triangulation_angle", + &mapper->inlier_thresholds.min_triangulation_angle); AddAndRegisterDefaultOption("Thresholds.max_epipolar_error_E", &mapper->inlier_thresholds.max_epipolar_error_E); AddAndRegisterDefaultOption("Thresholds.max_epipolar_error_F", diff --git a/glomap/estimators/bundle_adjustment.cc b/glomap/estimators/bundle_adjustment.cc index ee2be7a7..3bc83d0f 100644 --- a/glomap/estimators/bundle_adjustment.cc +++ b/glomap/estimators/bundle_adjustment.cc @@ -161,7 +161,7 @@ void BundleAdjuster::ParameterizeVariables( images[center].cam_from_world.translation.data()); // Parameterize the camera parameters, or set them to be constant if desired - if (options_.optimize_intrinsics) { + if (options_.optimize_intrinsics && !options_.optimize_principal_point) { for (auto& [camera_id, camera] : cameras) { if (problem_->HasParameterBlock(camera.params.data())) { std::vector principal_point_idxs; @@ -174,8 +174,8 @@ void BundleAdjuster::ParameterizeVariables( camera.params.data()); } } - - } else { + } else if (!options_.optimize_intrinsics && + !options_.optimize_principal_point) { for (auto& [camera_id, camera] : cameras) { if (problem_->HasParameterBlock(camera.params.data())) { problem_->SetParameterBlockConstant(camera.params.data()); diff --git a/glomap/estimators/bundle_adjustment.h b/glomap/estimators/bundle_adjustment.h index 76a81b9e..36a142f3 100644 --- a/glomap/estimators/bundle_adjustment.h +++ b/glomap/estimators/bundle_adjustment.h @@ -14,6 +14,7 @@ struct BundleAdjusterOptions : public OptimizationBaseOptions { bool optimize_rotations = true; bool optimize_translation = true; bool optimize_intrinsics = true; + bool optimize_principal_point = false; bool optimize_points = true; // Constrain the minimum number of views per track From 6486b175b2b0d67ccffb93718cadbe459540020d Mon Sep 17 00:00:00 2001 From: Linfei Pan <36349740+lpanaf@users.noreply.github.com> Date: Thu, 20 Mar 2025 16:01:22 +0100 Subject: [PATCH 28/45] Relpose estimation bug fix (#171) * fix the bug that relative pose estimation fails when camera model is not supported by poselib * allow the focal length to be different for unrecognized camera type --- glomap/estimators/relpose_estimation.cc | 52 ++++++++++++++++++++----- 1 file changed, 43 insertions(+), 9 deletions(-) diff --git a/glomap/estimators/relpose_estimation.cc b/glomap/estimators/relpose_estimation.cc index 4676eb22..8cd3b380 100644 --- a/glomap/estimators/relpose_estimation.cc +++ b/glomap/estimators/relpose_estimation.cc @@ -44,6 +44,13 @@ void EstimateRelativePoses(ViewGraph& view_graph, const Image& image2 = images[image_pair.image_id2]; const Eigen::MatrixXi& matches = image_pair.matches; + const Camera& camera1 = cameras[image1.camera_id]; + const Camera& camera2 = cameras[image2.camera_id]; + poselib::Camera camera_poselib1 = ColmapCameraToPoseLibCamera(camera1); + poselib::Camera camera_poselib2 = ColmapCameraToPoseLibCamera(camera2); + bool valid_camera_model = + (camera_poselib1.model_id >= 0 && camera_poselib2.model_id >= 0); + // Collect the original 2D points points2D_1.clear(); points2D_2.clear(); @@ -51,19 +58,46 @@ void EstimateRelativePoses(ViewGraph& view_graph, points2D_1.push_back(image1.features[matches(idx, 0)]); points2D_2.push_back(image2.features[matches(idx, 1)]); } + // If the camera model is not supported by poselib + if (!valid_camera_model) { + // Undistort points + // Note that here, we still rescale by the focal length (to avoid + // change the RANSAC threshold) + Eigen::Matrix2d K1_new = Eigen::Matrix2d::Zero(); + Eigen::Matrix2d K2_new = Eigen::Matrix2d::Zero(); + K1_new(0, 0) = camera1.FocalLengthX(); + K1_new(1, 1) = camera1.FocalLengthY(); + K2_new(0, 0) = camera2.FocalLengthX(); + K2_new(1, 1) = camera2.FocalLengthY(); + for (size_t idx = 0; idx < matches.rows(); idx++) { + points2D_1[idx] = K1_new * camera1.CamFromImg(points2D_1[idx]); + points2D_2[idx] = K2_new * camera2.CamFromImg(points2D_2[idx]); + } + // Reset the camera to be the pinhole camera with original focal + // length and zero principal point + camera_poselib1 = poselib::Camera( + "PINHOLE", + {camera1.FocalLengthX(), camera1.FocalLengthY(), 0., 0.}, + camera1.width, + camera1.height); + camera_poselib2 = poselib::Camera( + "PINHOLE", + {camera2.FocalLengthX(), camera2.FocalLengthY(), 0., 0.}, + camera2.width, + camera2.height); + } inliers.clear(); poselib::CameraPose pose_rel_calc; try { - poselib::estimate_relative_pose( - points2D_1, - points2D_2, - ColmapCameraToPoseLibCamera(cameras[image1.camera_id]), - ColmapCameraToPoseLibCamera(cameras[image2.camera_id]), - options.ransac_options, - options.bundle_options, - &pose_rel_calc, - &inliers); + poselib::estimate_relative_pose(points2D_1, + points2D_2, + camera_poselib1, + camera_poselib2, + options.ransac_options, + options.bundle_options, + &pose_rel_calc, + &inliers); } catch (const std::exception& e) { LOG(ERROR) << "Error in relative pose estimation: " << e.what(); image_pair.is_valid = false; From f3b3db92d152fde3a87971d42294010619ccc82b Mon Sep 17 00:00:00 2001 From: Gareth Date: Thu, 20 Mar 2025 08:01:43 -0700 Subject: [PATCH 29/45] Use CMAKE_CURRENT_SOURCE_DIR in FindDependencies.cmake (#175) --- cmake/FindDependencies.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/FindDependencies.cmake b/cmake/FindDependencies.cmake index d7e5e586..769ee7e6 100644 --- a/cmake/FindDependencies.cmake +++ b/cmake/FindDependencies.cmake @@ -1,4 +1,4 @@ -set(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake") +set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake") find_package(Eigen3 3.4 REQUIRED) find_package(SuiteSparse COMPONENTS CHOLMOD REQUIRED) From 8246cf54537892dae7e7030f3a4d7e24a439fac8 Mon Sep 17 00:00:00 2001 From: Charlie Date: Fri, 21 Mar 2025 00:14:24 +0900 Subject: [PATCH 30/45] added gpu support (#174) * added gpu support * add gpu to bundle adjustment * f * add gpu to global positioning and bundle adjustment * format * change back to original glomap naming * keep consistent between gp and ba * change to the workflow to test cuda as well * add option in cmake whether to use cuda * d * d * d * d * d * change version * keep the minimum_num to be consistent with COLMAP --------- Co-authored-by: Linfei Pan --- .github/workflows/ubuntu.yml | 3 +- .github/workflows/windows.yml | 35 +++++++++++-- CMakeLists.txt | 23 ++++++--- cmake/FindDependencies.cmake | 68 ++++++++++++++++++++++++- glomap/controllers/option_manager.cc | 8 +++ glomap/estimators/bundle_adjustment.cc | 54 ++++++++++++++++++++ glomap/estimators/bundle_adjustment.h | 4 ++ glomap/estimators/global_positioning.cc | 55 ++++++++++++++++++++ glomap/estimators/global_positioning.h | 4 ++ glomap/glomap.cc | 6 +++ vcpkg.json | 8 +++ 11 files changed, 254 insertions(+), 14 deletions(-) diff --git a/.github/workflows/ubuntu.yml b/.github/workflows/ubuntu.yml index 58000156..ebd56549 100644 --- a/.github/workflows/ubuntu.yml +++ b/.github/workflows/ubuntu.yml @@ -35,7 +35,7 @@ jobs: os: ubuntu-22.04, cmakeBuildType: Release, asanEnabled: true, - cudaEnabled: false, + cudaEnabled: true, checkCodeFormat: false, }, { @@ -163,6 +163,7 @@ jobs: -DCMAKE_BUILD_TYPE=${{ matrix.config.cmakeBuildType }} \ -DCMAKE_INSTALL_PREFIX=./install \ -DCMAKE_CUDA_ARCHITECTURES=50 \ + -DCUDA_ENABLED=${{ matrix.config.cudaEnabled }} \ -DTESTS_ENABLED=ON \ -DASAN_ENABLED=${{ matrix.config.asanEnabled }} ninja -k 10000 diff --git a/.github/workflows/windows.yml b/.github/workflows/windows.yml index 74e1fe5b..1adc3635 100644 --- a/.github/workflows/windows.yml +++ b/.github/workflows/windows.yml @@ -23,6 +23,13 @@ jobs: testsEnabled: true, exportPackage: false, }, + { + os: windows-2022, + cmakeBuildType: Release, + cudaEnabled: true, + testsEnabled: true, + exportPackage: true, + }, { os: windows-2022, cmakeBuildType: Release, @@ -70,6 +77,15 @@ jobs: .github/workflows/install-ccache.ps1 -Destination "${{ env.COMPILER_CACHE_DIR }}/bin" + - name: Install CUDA + uses: Jimver/cuda-toolkit@v0.2.18 + if: matrix.config.cudaEnabled + id: cuda-toolkit + with: + cuda: '12.6.2' + sub-packages: '["nvcc", "nvtx", "cudart", "curand", "curand_dev", "nvrtc_dev"]' + method: 'network' + - name: Install CMake and Ninja uses: lukka/get-cmake@latest @@ -96,10 +112,11 @@ jobs: -DCMAKE_MAKE_PROGRAM=ninja ` -DCMAKE_BUILD_TYPE=Release ` -DTESTS_ENABLED=ON ` - -DCUDA_ENABLED=OFF ` + -DCUDA_ENABLED=${{ matrix.config.cudaEnabled }} ` -DGUI_ENABLED=OFF ` -DCGAL_ENABLED=OFF ` -DCMAKE_CUDA_ARCHITECTURES=all-major ` + -DCUDAToolkit_ROOT="${{ steps.cuda-toolkit.outputs.CUDA_PATH }}" ` -DCMAKE_TOOLCHAIN_FILE="${{ github.workspace }}/vcpkg/scripts/buildsystems/vcpkg.cmake" ` -DVCPKG_TARGET_TRIPLET=x64-windows-release ` -DCMAKE_INSTALL_PREFIX=install @@ -123,15 +140,27 @@ jobs: ../vcpkg/vcpkg.exe install ` --triplet=x64-windows-release + $(if ($${{ matrix.config.cudaEnabled }}) { echo "--x-feature=cuda" }) ../vcpkg/vcpkg.exe export --raw --output-dir vcpkg_export --output glomap cp vcpkg_export/glomap/installed/x64-windows/bin/*.dll install/bin cp vcpkg_export/glomap/installed/x64-windows-release/bin/*.dll install/bin + if ($${{ matrix.config.cudaEnabled }}) { + cp "${{ steps.cuda-toolkit.outputs.CUDA_PATH }}/bin/cudart64_*.dll" install/bin + cp "${{ steps.cuda-toolkit.outputs.CUDA_PATH }}/bin/curand64_*.dll" install/bin + } + + - name: Upload package + uses: actions/upload-artifact@v4 + if: ${{ matrix.config.exportPackage && matrix.config.cudaEnabled }} + with: + name: glomap-x64-windows-cuda + path: build/install - name: Upload package uses: actions/upload-artifact@v4 - if: ${{ matrix.config.exportPackage }} + if: ${{ matrix.config.exportPackage && !matrix.config.cudaEnabled }} with: - name: glomap-x64-windows + name: glomap-x64-windows-nocuda path: build/install - name: Cleanup compiler cache diff --git a/CMakeLists.txt b/CMakeLists.txt index 1d1c2729..1c0f9ab3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,21 +1,27 @@ cmake_minimum_required(VERSION 3.28) -project(glomap VERSION 1.0.0) - -set(CMAKE_CXX_STANDARD 17) -set(CMAKE_CXX_STANDARD_REQUIRED ON) - -set_property(GLOBAL PROPERTY GLOBAL_DEPENDS_NO_CYCLES ON) - +option(CUDA_ENABLED "Whether to enable CUDA, if available" ON) option(TESTS_ENABLED "Whether to build test binaries" OFF) option(ASAN_ENABLED "Whether to enable AddressSanitizer flags" OFF) option(CCACHE_ENABLED "Whether to enable compiler caching, if available" ON) option(FETCH_COLMAP "Whether to use COLMAP with FetchContent or with self-installed software" ON) option(FETCH_POSELIB "Whether to use PoseLib with FetchContent or with self-installed software" ON) +# Propagate options to vcpkg manifest. +if(CUDA_ENABLED) + list(APPEND VCPKG_MANIFEST_FEATURES "cuda") +endif() + +# Initialize the project. +project(glomap VERSION 1.1.0) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +set_property(GLOBAL PROPERTY GLOBAL_DEPENDS_NO_CYCLES ON) + include(cmake/FindDependencies.cmake) -# Propagate options to vcpkg manifest. if (TESTS_ENABLED) enable_testing() endif() @@ -39,6 +45,7 @@ else() message(STATUS "Disabling ccache support") endif() + if(CMAKE_CXX_COMPILER_ID STREQUAL "MSVC") # Some fixes for the Glog library. add_definitions("-DGLOG_USE_GLOG_EXPORT") diff --git a/cmake/FindDependencies.cmake b/cmake/FindDependencies.cmake index 769ee7e6..a59323a1 100644 --- a/cmake/FindDependencies.cmake +++ b/cmake/FindDependencies.cmake @@ -28,7 +28,7 @@ FetchContent_Declare(PoseLib SYSTEM ) message(STATUS "Configuring PoseLib...") -if (FETCH_POSELIB) +if (FETCH_POSELIB) FetchContent_MakeAvailable(PoseLib) else() find_package(PoseLib REQUIRED) @@ -42,9 +42,73 @@ FetchContent_Declare(COLMAP ) message(STATUS "Configuring COLMAP...") set(UNINSTALL_ENABLED OFF CACHE INTERNAL "") -if (FETCH_COLMAP) +if (FETCH_COLMAP) FetchContent_MakeAvailable(COLMAP) else() find_package(COLMAP REQUIRED) endif() message(STATUS "Configuring COLMAP... done") + +set(CUDA_MIN_VERSION "7.0") +if(CUDA_ENABLED) + if(CMAKE_VERSION VERSION_LESS 3.17) + find_package(CUDA QUIET) + if(CUDA_FOUND) + message(STATUS "Found CUDA version ${CUDA_VERSION} installed in " + "${CUDA_TOOLKIT_ROOT_DIR} via legacy CMake (<3.17) module. " + "Using the legacy CMake module means that any installation of " + "COLMAP will require that the CUDA libraries are " + "available under LD_LIBRARY_PATH.") + message(STATUS "Found CUDA ") + message(STATUS " Includes : ${CUDA_INCLUDE_DIRS}") + message(STATUS " Libraries : ${CUDA_LIBRARIES}") + + enable_language(CUDA) + + macro(declare_imported_cuda_target module) + add_library(CUDA::${module} INTERFACE IMPORTED) + target_include_directories( + CUDA::${module} INTERFACE ${CUDA_INCLUDE_DIRS}) + target_link_libraries( + CUDA::${module} INTERFACE ${CUDA_${module}_LIBRARY} ${ARGN}) + endmacro() + + declare_imported_cuda_target(cudart ${CUDA_LIBRARIES}) + declare_imported_cuda_target(curand ${CUDA_LIBRARIES}) + + set(CUDAToolkit_VERSION "${CUDA_VERSION_STRING}") + set(CUDAToolkit_BIN_DIR "${CUDA_TOOLKIT_ROOT_DIR}/bin") + else() + message(STATUS "CUDA not found") + endif() + else() + find_package(CUDAToolkit QUIET) + if(CUDAToolkit_FOUND) + set(CUDA_FOUND ON) + enable_language(CUDA) + else() + message(STATUS "CUDA not found") + endif() + endif() +endif() + +if(CUDA_ENABLED AND CUDA_FOUND) + if(NOT DEFINED CMAKE_CUDA_ARCHITECTURES) + set(CMAKE_CUDA_ARCHITECTURES "native") + endif() + + add_definitions("-DGLOMAP_CUDA_ENABLED") + + # Do not show warnings if the architectures are deprecated. + set(CMAKE_CUDA_FLAGS "${CMAKE_CUDA_FLAGS} -Wno-deprecated-gpu-targets") + # Explicitly set PIC flags for CUDA targets. + if(NOT IS_MSVC) + set(CMAKE_CUDA_FLAGS "${CMAKE_CUDA_FLAGS} --compiler-options -fPIC") + endif() + + message(STATUS "Enabling CUDA support (version: ${CUDAToolkit_VERSION}, " + "archs: ${CMAKE_CUDA_ARCHITECTURES})") +else() + set(CUDA_ENABLED OFF) + message(STATUS "Disabling CUDA support") +endif() diff --git a/glomap/controllers/option_manager.cc b/glomap/controllers/option_manager.cc index 9b8fb6a1..d8041a62 100644 --- a/glomap/controllers/option_manager.cc +++ b/glomap/controllers/option_manager.cc @@ -180,6 +180,10 @@ void OptionManager::AddGlobalPositionerOptions() { return; } added_global_positioning_options_ = true; + AddAndRegisterDefaultOption("GlobalPositioning.use_gpu", + &mapper->opt_gp.use_gpu); + AddAndRegisterDefaultOption("GlobalPositioning.gpu_index", + &mapper->opt_gp.gpu_index); AddAndRegisterDefaultOption("GlobalPositioning.optimize_positions", &mapper->opt_gp.optimize_positions); AddAndRegisterDefaultOption("GlobalPositioning.optimize_points", @@ -199,6 +203,10 @@ void OptionManager::AddBundleAdjusterOptions() { return; } added_bundle_adjustment_options_ = true; + AddAndRegisterDefaultOption("BundleAdjustment.use_gpu", + &mapper->opt_ba.use_gpu); + AddAndRegisterDefaultOption("BundleAdjustment.gpu_index", + &mapper->opt_ba.gpu_index); AddAndRegisterDefaultOption("BundleAdjustment.optimize_rotations", &mapper->opt_ba.optimize_rotations); AddAndRegisterDefaultOption("BundleAdjustment.optimize_translation", diff --git a/glomap/estimators/bundle_adjustment.cc b/glomap/estimators/bundle_adjustment.cc index 3bc83d0f..57f85b13 100644 --- a/glomap/estimators/bundle_adjustment.cc +++ b/glomap/estimators/bundle_adjustment.cc @@ -3,6 +3,8 @@ #include #include #include +#include +#include namespace glomap { @@ -36,6 +38,58 @@ bool BundleAdjuster::Solve(const ViewGraph& view_graph, // Set the solver options. ceres::Solver::Summary summary; + int num_images = images.size(); +#ifdef GLOMAP_CUDA_ENABLED + bool cuda_solver_enabled = false; + +#if (CERES_VERSION_MAJOR >= 3 || \ + (CERES_VERSION_MAJOR == 2 && CERES_VERSION_MINOR >= 2)) && \ + !defined(CERES_NO_CUDA) + if (options_.use_gpu && num_images >= options_.min_num_images_gpu_solver) { + cuda_solver_enabled = true; + options_.solver_options.dense_linear_algebra_library_type = ceres::CUDA; + } +#else + if (options_.use_gpu) { + LOG_FIRST_N(WARNING, 1) + << "Requested to use GPU for bundle adjustment, but Ceres was " + "compiled without CUDA support. Falling back to CPU-based dense " + "solvers."; + } +#endif + +#if (CERES_VERSION_MAJOR >= 3 || \ + (CERES_VERSION_MAJOR == 2 && CERES_VERSION_MINOR >= 3)) && \ + !defined(CERES_NO_CUDSS) + if (options_.use_gpu && num_images >= options_.min_num_images_gpu_solver) { + cuda_solver_enabled = true; + options_.solver_options.sparse_linear_algebra_library_type = + ceres::CUDA_SPARSE; + } +#else + if (options_.use_gpu) { + LOG_FIRST_N(WARNING, 1) + << "Requested to use GPU for bundle adjustment, but Ceres was " + "compiled without cuDSS support. Falling back to CPU-based sparse " + "solvers."; + } +#endif + + if (cuda_solver_enabled) { + const std::vector gpu_indices = + colmap::CSVToVector(options_.gpu_index); + THROW_CHECK_GT(gpu_indices.size(), 0); + colmap::SetBestCudaDevice(gpu_indices[0]); + } +#else + if (options_.use_gpu) { + LOG_FIRST_N(WARNING, 1) + << "Requested to use GPU for bundle adjustment, but COLMAP was " + "compiled without CUDA support. Falling back to CPU-based " + "solvers."; + } +#endif // GLOMAP_CUDA_ENABLED + // Do not use the iterative solver, as it does not seem to be helpful options_.solver_options.linear_solver_type = ceres::SPARSE_SCHUR; options_.solver_options.preconditioner_type = ceres::CLUSTER_TRIDIAGONAL; diff --git a/glomap/estimators/bundle_adjustment.h b/glomap/estimators/bundle_adjustment.h index 36a142f3..b78347ca 100644 --- a/glomap/estimators/bundle_adjustment.h +++ b/glomap/estimators/bundle_adjustment.h @@ -17,6 +17,10 @@ struct BundleAdjusterOptions : public OptimizationBaseOptions { bool optimize_principal_point = false; bool optimize_points = true; + bool use_gpu = true; + std::string gpu_index = "-1"; + int min_num_images_gpu_solver = 50; + // Constrain the minimum number of views per track int min_num_view_per_track = 3; diff --git a/glomap/estimators/global_positioning.cc b/glomap/estimators/global_positioning.cc index 8a9b064a..ebe1b8de 100644 --- a/glomap/estimators/global_positioning.cc +++ b/glomap/estimators/global_positioning.cc @@ -2,6 +2,9 @@ #include "glomap/estimators/cost_function.h" +#include +#include + namespace glomap { namespace { @@ -361,6 +364,58 @@ void GlobalPositioner::ParameterizeVariables( } } + int num_images = images.size(); +#ifdef GLOMAP_CUDA_ENABLED + bool cuda_solver_enabled = false; + +#if (CERES_VERSION_MAJOR >= 3 || \ + (CERES_VERSION_MAJOR == 2 && CERES_VERSION_MINOR >= 2)) && \ + !defined(CERES_NO_CUDA) + if (options_.use_gpu && num_images >= options_.min_num_images_gpu_solver) { + cuda_solver_enabled = true; + options_.solver_options.dense_linear_algebra_library_type = ceres::CUDA; + } +#else + if (options_.use_gpu) { + LOG_FIRST_N(WARNING, 1) + << "Requested to use GPU for bundle adjustment, but Ceres was " + "compiled without CUDA support. Falling back to CPU-based dense " + "solvers."; + } +#endif + +#if (CERES_VERSION_MAJOR >= 3 || \ + (CERES_VERSION_MAJOR == 2 && CERES_VERSION_MINOR >= 3)) && \ + !defined(CERES_NO_CUDSS) + if (options_.use_gpu && num_images >= options_.min_num_images_gpu_solver) { + cuda_solver_enabled = true; + options_.solver_options.sparse_linear_algebra_library_type = + ceres::CUDA_SPARSE; + } +#else + if (options_.use_gpu) { + LOG_FIRST_N(WARNING, 1) + << "Requested to use GPU for bundle adjustment, but Ceres was " + "compiled without cuDSS support. Falling back to CPU-based sparse " + "solvers."; + } +#endif + + if (cuda_solver_enabled) { + const std::vector gpu_indices = + colmap::CSVToVector(options_.gpu_index); + THROW_CHECK_GT(gpu_indices.size(), 0); + colmap::SetBestCudaDevice(gpu_indices[0]); + } +#else + if (options_.use_gpu) { + LOG_FIRST_N(WARNING, 1) + << "Requested to use GPU for bundle adjustment, but COLMAP was " + "compiled without CUDA support. Falling back to CPU-based " + "solvers."; + } +#endif // GLOMAP_CUDA_ENABLED + // Set up the options for the solver // Do not use iterative solvers, for its suboptimal performance. if (tracks.size() > 0) { diff --git a/glomap/estimators/global_positioning.h b/glomap/estimators/global_positioning.h index 3926ff7b..f318e8fa 100644 --- a/glomap/estimators/global_positioning.h +++ b/glomap/estimators/global_positioning.h @@ -29,6 +29,10 @@ struct GlobalPositionerOptions : public OptimizationBaseOptions { bool optimize_points = true; bool optimize_scales = true; + bool use_gpu = true; + std::string gpu_index = "-1"; + int min_num_images_gpu_solver = 50; + // Constrain the minimum number of views per track int min_num_view_per_track = 3; diff --git a/glomap/glomap.cc b/glomap/glomap.cc index aa300a19..19c77255 100644 --- a/glomap/glomap.cc +++ b/glomap/glomap.cc @@ -12,6 +12,12 @@ int ShowHelp( std::cout << "GLOMAP -- Global Structure-from-Motion" << std::endl << std::endl; +#ifdef GLOMAP_CUDA_ENABLED + std::cout << "This version was compiled with CUDA!" << std::endl << std::endl; +#else + std::cout << "This version was NOT compiled CUDA!" << std::endl << std::endl; +#endif + std::cout << "Usage:" << std::endl; std::cout << " glomap mapper --database_path DATABASE --output_path MODEL" << std::endl; diff --git a/vcpkg.json b/vcpkg.json index 4ec57780..a1b5010e 100644 --- a/vcpkg.json +++ b/vcpkg.json @@ -16,6 +16,7 @@ "name": "ceres", "features": [ "lapack", + "schur", "suitesparse" ] }, @@ -42,5 +43,12 @@ "suitesparse" ], "features": { + "cuda": { + "description": "Build with CUDA.", + "dependencies": [ + "glew", + "cuda" + ] + } } } From 58eaeaf880da66bf4ae3b78ca5ac3d28ba60d2ad Mon Sep 17 00:00:00 2001 From: Linfei Pan <36349740+lpanaf@users.noreply.github.com> Date: Thu, 20 Mar 2025 16:23:17 +0100 Subject: [PATCH 31/45] Add the rotation averager to GLOMAP (#113) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Fix conversion of colmap pose prior * d * restore the pose prior after rotation averaging * f * f * f * gravity refinement tested * add the stratified rotation averager and CLI * f * d * d * add timer * add options for gravity refinement * merge gravity_io to pose_io * add readme for the rotation averager * f * Update glomap/math/gravity.cc Co-authored-by: Johannes Schönberger * f * Update glomap/controllers/rotation_averager.cc Co-authored-by: Johannes Schönberger * Update glomap/controllers/rotation_averager.cc Co-authored-by: Johannes Schönberger * change rotation averager from class to struct * d * d * add weighting and corresponding IO. Tested * change the writing of relative pose to be sorted * unit test for rotation averager * inject noise to avoid local minima for error-free case * f * add more points in the unit test --------- Co-authored-by: Johannes Schönberger --- README.md | 1 + docs/rotation_averager.md | 62 +++++ glomap/CMakeLists.txt | 12 +- glomap/controllers/option_manager.cc | 15 ++ glomap/controllers/option_manager.h | 4 + glomap/controllers/rotation_averager.cc | 61 +++++ glomap/controllers/rotation_averager.h | 15 ++ glomap/controllers/rotation_averager_test.cc | 236 ++++++++++++++++++ .../estimators/global_rotation_averaging.cc | 45 +++- glomap/estimators/global_rotation_averaging.h | 5 +- glomap/estimators/gravity_refinement.cc | 5 + glomap/estimators/view_graph_calibration.cc | 2 +- glomap/exe/rotation_averager.cc | 105 ++++++++ glomap/exe/rotation_averager.h | 10 + glomap/glomap.cc | 4 +- glomap/io/colmap_converter.cc | 21 +- glomap/io/gravity_io.cc | 45 ---- glomap/io/gravity_io.h | 14 -- glomap/io/pose_io.cc | 212 ++++++++++++++++ glomap/io/pose_io.h | 37 +++ glomap/math/gravity.cc | 66 +++++ glomap/math/gravity.h | 5 + glomap/scene/image.h | 3 +- glomap/scene/image_pair.h | 2 +- 24 files changed, 904 insertions(+), 83 deletions(-) create mode 100644 docs/rotation_averager.md create mode 100644 glomap/controllers/rotation_averager.cc create mode 100644 glomap/controllers/rotation_averager.h create mode 100644 glomap/controllers/rotation_averager_test.cc create mode 100644 glomap/exe/rotation_averager.cc create mode 100644 glomap/exe/rotation_averager.h delete mode 100644 glomap/io/gravity_io.cc delete mode 100644 glomap/io/gravity_io.h create mode 100644 glomap/io/pose_io.cc create mode 100644 glomap/io/pose_io.h diff --git a/README.md b/README.md index e5f4455b..5c388af3 100644 --- a/README.md +++ b/README.md @@ -20,6 +20,7 @@ If you use this project for your research, please cite year={2024}, } ``` +To use the seperate rotation averaging module, refer to [this README](docs/rotation_averager.md). ## Getting Started diff --git a/docs/rotation_averager.md b/docs/rotation_averager.md new file mode 100644 index 00000000..5c4e9947 --- /dev/null +++ b/docs/rotation_averager.md @@ -0,0 +1,62 @@ +# Gravity-aligned Rotation Averaging with Circular Regression + +[Project page](https://lpanaf.github.io/eccv24_ra1dof/) | [Paper](https://www.ecva.net/papers/eccv_2024/papers_ECCV/papers/05651.pdf) | [Supp.](https://lpanaf.github.io/assets/pdf/eccv24_ra1dof_sm.pdf) +--- + +## About + +This project aims at solving the rotation averaging problem with gravity prior. +To achieve this, circular regression is leveraged. + +If you use this project for your research, please cite +``` +@inproceedings{pan2024ra1dof, + author={Pan, Linfei and Pollefeys, Marc and Barath, Daniel}, + title={{Gravity-aligned Rotation Averaging with Circular Regression}}, + booktitle={European Conference on Computer Vision (ECCV)}, + year={2024}, +} +``` + +## Getting Started +Install GLOMAP as instrcucted in [README](../README.md). +Then, call the rotation averager (3 degree-of-freedom) via +``` +glomap rotation_averager --relpose_path RELPOSE_PATH --output_path OUTPUT_PATH +``` + +If gravity directions are available, call the rotation averager (1 degree-of-freedom) via +``` +glomap rotation_averager \ + --relpose_path RELPOSE_PATH \ + --output_path OUTPUT_PATH \ + --gravity_path GRAVTIY PATH +``` +It is recommended to set `--use_stratified=1` if only a subset of images have gravity direction. +If gravity measurements are subject to i.i.d. noise, they can be refined by setting `--refine_gravity=1`. + + +## File Formats +### Relative Pose +The relative pose file is expected to be of the following format +``` +IMAGE_NAME_1 IMAGE_NAME_2 QW QX QY QZ TX TY TZ +``` +Only images contained in at least one relative pose will be included in the following procedure. +The relative pose should be 2R1 x1 + 2t1 = x2. + +### Gravity Direction +The gravity direction file is expected to be of the following format +``` +IMAGE_NAME GX GY GZ +``` +The gravity direction $g$ should $[0, 1, 0]$ if the image is parallel to the ground plane, and the estimated rotation would have the property that $R_i \cdot [0, 1, 0]^\top = g$. +If is acceptable if only a subset of all images have gravity direciton. +If the specified image name does not match any known image name from relative pose, it is ignored. + +### Output +The estimated global rotation will be in the following format +``` +IMAGE_NAME QW QX QY QZ +``` +Any images that are not within the largest connected component of the view-graph formed by the relative pose will be ignored. diff --git a/glomap/CMakeLists.txt b/glomap/CMakeLists.txt index 7d60ee17..e191049a 100644 --- a/glomap/CMakeLists.txt +++ b/glomap/CMakeLists.txt @@ -1,6 +1,7 @@ set(SOURCES controllers/global_mapper.cc controllers/option_manager.cc + controllers/rotation_averager.cc controllers/track_establishment.cc controllers/track_retriangulation.cc estimators/bundle_adjustment.cc @@ -11,7 +12,7 @@ set(SOURCES estimators/view_graph_calibration.cc io/colmap_converter.cc io/colmap_io.cc - io/gravity_io.cc + io/pose_io.cc math/gravity.cc math/rigid3d.cc math/tree.cc @@ -29,6 +30,7 @@ set(SOURCES set(HEADERS controllers/global_mapper.h controllers/option_manager.h + controllers/rotation_averager.h controllers/track_establishment.h controllers/track_retriangulation.h estimators/bundle_adjustment.h @@ -41,7 +43,7 @@ set(HEADERS estimators/view_graph_calibration.h io/colmap_converter.h io/colmap_io.h - io/gravity_io.h + io/pose_io.h math/gravity.h math/l1_solver.h math/rigid3d.h @@ -101,7 +103,10 @@ endif() add_executable(glomap_main glomap.cc exe/global_mapper.h - exe/global_mapper.cc) + exe/global_mapper.cc + exe/rotation_averager.h + exe/rotation_averager.cc +) target_link_libraries(glomap_main glomap) set_target_properties(glomap_main PROPERTIES OUTPUT_NAME glomap) @@ -111,6 +116,7 @@ install(TARGETS glomap_main DESTINATION bin) if(TESTS_ENABLED) add_executable(glomap_test controllers/global_mapper_test.cc + controllers/rotation_averager_test.cc ) target_link_libraries( glomap_test diff --git a/glomap/controllers/option_manager.cc b/glomap/controllers/option_manager.cc index d8041a62..3a7c8ff4 100644 --- a/glomap/controllers/option_manager.cc +++ b/glomap/controllers/option_manager.cc @@ -1,6 +1,7 @@ #include "option_manager.h" #include "glomap/controllers/global_mapper.h" +#include "glomap/estimators/gravity_refinement.h" #include #include @@ -14,6 +15,7 @@ OptionManager::OptionManager(bool add_project_options) { image_path = std::make_shared(); mapper = std::make_shared(); + gravity_refiner = std::make_shared(); Reset(); desc_->add_options()("help,h", ""); @@ -266,6 +268,18 @@ void OptionManager::AddInlierThresholdOptions() { &mapper->inlier_thresholds.max_rotation_error); } +void OptionManager::AddGravityRefinerOptions() { + if (added_gravity_refiner_options_) { + return; + } + added_gravity_refiner_options_ = true; + AddAndRegisterDefaultOption("GravityRefiner.max_outlier_ratio", + &gravity_refiner->max_outlier_ratio); + AddAndRegisterDefaultOption("GravityRefiner.max_gravity_error", + &gravity_refiner->max_gravity_error); + AddAndRegisterDefaultOption("GravityRefiner.min_num_neighbors", + &gravity_refiner->min_num_neighbors); +} void OptionManager::Reset() { const bool kResetPaths = true; ResetOptions(kResetPaths); @@ -294,6 +308,7 @@ void OptionManager::ResetOptions(const bool reset_paths) { *image_path = ""; } *mapper = GlobalMapperOptions(); + *gravity_refiner = GravityRefinerOptions(); } void OptionManager::Parse(const int argc, char** argv) { diff --git a/glomap/controllers/option_manager.h b/glomap/controllers/option_manager.h index 0926387d..030d38e3 100644 --- a/glomap/controllers/option_manager.h +++ b/glomap/controllers/option_manager.h @@ -18,6 +18,7 @@ struct GlobalPositionerOptions; struct BundleAdjusterOptions; struct TriangulatorOptions; struct InlierThresholdOptions; +struct GravityRefinerOptions; class OptionManager { public: @@ -37,6 +38,7 @@ class OptionManager { void AddBundleAdjusterOptions(); void AddTriangulatorOptions(); void AddInlierThresholdOptions(); + void AddGravityRefinerOptions(); template void AddRequiredOption(const std::string& name, @@ -56,6 +58,7 @@ class OptionManager { std::shared_ptr image_path; std::shared_ptr mapper; + std::shared_ptr gravity_refiner; private: template @@ -88,6 +91,7 @@ class OptionManager { bool added_bundle_adjustment_options_ = false; bool added_triangulation_options_ = false; bool added_inliers_options_ = false; + bool added_gravity_refiner_options_ = false; }; template diff --git a/glomap/controllers/rotation_averager.cc b/glomap/controllers/rotation_averager.cc new file mode 100644 index 00000000..09039e84 --- /dev/null +++ b/glomap/controllers/rotation_averager.cc @@ -0,0 +1,61 @@ +#include "glomap/controllers/rotation_averager.h" + +namespace glomap { + +bool SolveRotationAveraging(ViewGraph& view_graph, + std::unordered_map& images, + const RotationAveragerOptions& options) { + view_graph.KeepLargestConnectedComponents(images); + + bool solve_1dof_system = options.use_gravity && options.use_stratified; + + ViewGraph view_graph_grav; + image_pair_t total_pairs = 0; + image_pair_t grav_pairs = 0; + if (solve_1dof_system) { + // Prepare two sets: ones all with gravity, and one does not have gravity. + // Solve them separately first, then solve them in a single system + for (const auto& [pair_id, image_pair] : view_graph.image_pairs) { + if (!image_pair.is_valid) continue; + + image_t image_id1 = image_pair.image_id1; + image_t image_id2 = image_pair.image_id2; + + Image& image1 = images[image_id1]; + Image& image2 = images[image_id2]; + + if (!image1.is_registered || !image2.is_registered) continue; + + total_pairs++; + + if (image1.gravity_info.has_gravity && image2.gravity_info.has_gravity) { + view_graph_grav.image_pairs.emplace( + pair_id, + ImagePair(image_id1, image_id2, image_pair.cam2_from_cam1)); + grav_pairs++; + } + } + } + + // If there is no image pairs with gravity or most image pairs are with + // gravity, then just run the 3dof version + bool status = (grav_pairs == 0) || (grav_pairs > total_pairs * 0.95); + solve_1dof_system = solve_1dof_system && (!status); + + if (solve_1dof_system) { + // Run the 1dof optimization + LOG(INFO) << "Solving subset 1DoF rotation averaging problem in the mixed " + "prior system"; + int num_img_grv = view_graph_grav.KeepLargestConnectedComponents(images); + RotationEstimator rotation_estimator_grav(options); + if (!rotation_estimator_grav.EstimateRotations(view_graph_grav, images)) { + return false; + } + view_graph.KeepLargestConnectedComponents(images); + } + + RotationEstimator rotation_estimator(options); + return rotation_estimator.EstimateRotations(view_graph, images); +} + +} // namespace glomap \ No newline at end of file diff --git a/glomap/controllers/rotation_averager.h b/glomap/controllers/rotation_averager.h new file mode 100644 index 00000000..cdf73893 --- /dev/null +++ b/glomap/controllers/rotation_averager.h @@ -0,0 +1,15 @@ +#pragma once + +#include "glomap/estimators/global_rotation_averaging.h" + +namespace glomap { + +struct RotationAveragerOptions : public RotationEstimatorOptions { + bool use_stratified = true; +}; + +bool SolveRotationAveraging(ViewGraph& view_graph, + std::unordered_map& images, + const RotationAveragerOptions& options); + +} // namespace glomap \ No newline at end of file diff --git a/glomap/controllers/rotation_averager_test.cc b/glomap/controllers/rotation_averager_test.cc new file mode 100644 index 00000000..dbab164e --- /dev/null +++ b/glomap/controllers/rotation_averager_test.cc @@ -0,0 +1,236 @@ +#include "glomap/controllers/rotation_averager.h" + +#include "glomap/controllers/global_mapper.h" +#include "glomap/estimators/gravity_refinement.h" +#include "glomap/io/colmap_io.h" +#include "glomap/math/rigid3d.h" +#include "glomap/types.h" + +#include +#include +#include + +#include + +namespace glomap { +namespace { + +void CreateRandomRotation(const double stddev, Eigen::Quaterniond& q) { + std::random_device rd{}; + std::mt19937 gen{rd()}; + + // Construct a random axis + double theta = double(rand()) / RAND_MAX * 2 * M_PI; + double phi = double(rand()) / RAND_MAX * M_PI; + Eigen::Vector3d axis(std::cos(theta) * std::sin(phi), + std::sin(theta) * std::sin(phi), + std::cos(phi)); + + // Construct a random angle + std::normal_distribution d{0, stddev}; + double angle = d(gen); + q = Eigen::AngleAxisd(angle, axis); +} + +void PrepareGravity(const colmap::Reconstruction& gt, + std::unordered_map& images, + double stddev_gravity = 0.0, + double outlier_ratio = 0.0) { + for (auto& image_id : gt.RegImageIds()) { + Eigen::Vector3d gravity = + gt.Image(image_id).CamFromWorld().rotation * Eigen::Vector3d(0, 1, 0); + + if (stddev_gravity > 0.0) { + Eigen::Quaterniond q; + CreateRandomRotation(DegToRad(stddev_gravity), q); + gravity = q * gravity; + } + + if (outlier_ratio > 0.0 && double(rand()) / RAND_MAX < outlier_ratio) { + Eigen::Quaterniond q; + CreateRandomRotation(1., q); + gravity = + Rigid3dToAngleAxis(Rigid3d(q, Eigen::Vector3d::Zero())).normalized(); + } + images[image_id].gravity_info.SetGravity(gravity); + } +} + +GlobalMapperOptions CreateMapperTestOptions() { + GlobalMapperOptions options; + options.skip_view_graph_calibration = false; + options.skip_relative_pose_estimation = false; + options.skip_rotation_averaging = true; + options.skip_track_establishment = true; + options.skip_global_positioning = true; + options.skip_bundle_adjustment = true; + options.skip_retriangulation = true; + + return options; +} + +RotationAveragerOptions CreateRATestOptions(bool use_gravity = false) { + RotationAveragerOptions options; + options.skip_initialization = true; + options.use_gravity = use_gravity; + return options; +} + +void ExpectEqualRotations(const colmap::Reconstruction& gt, + const colmap::Reconstruction& computed, + const double max_rotation_error_deg) { + const std::set reg_image_ids_set = gt.RegImageIds(); + std::vector reg_image_ids(reg_image_ids_set.begin(), + reg_image_ids_set.end()); + for (size_t i = 0; i < reg_image_ids.size(); i++) { + const image_t image_id1 = reg_image_ids[i]; + for (size_t j = 0; j < reg_image_ids.size(); j++) { + if (i == j) continue; + const image_t image_id2 = reg_image_ids[j]; + + const Rigid3d cam2_from_cam1 = + computed.Image(image_id2).CamFromWorld() * + colmap::Inverse(computed.Image(image_id1).CamFromWorld()); + + const Rigid3d cam2_from_cam1_gt = + gt.Image(image_id2).CamFromWorld() * + colmap::Inverse(gt.Image(image_id1).CamFromWorld()); + + double rotation_error_deg = CalcAngle(cam2_from_cam1_gt, cam2_from_cam1); + EXPECT_LT(rotation_error_deg, max_rotation_error_deg); + } + } +} + +void ExpectEqualGravity( + const colmap::Reconstruction& gt, + const std::unordered_map& images_computed, + const double max_gravity_error_deg) { + for (const auto& image_id : gt.RegImageIds()) { + const Eigen::Vector3d gravity_gt = + gt.Image(image_id).CamFromWorld().rotation * Eigen::Vector3d(0, 1, 0); + const Eigen::Vector3d gravity_computed = + images_computed.at(image_id).gravity_info.GetGravity(); + + double gravity_error_deg = CalcAngle(gravity_gt, gravity_computed); + EXPECT_LT(gravity_error_deg, max_gravity_error_deg); + } +} + +TEST(RotationEstimator, WithoutNoise) { + const std::string database_path = colmap::CreateTestDir() + "/database.db"; + + colmap::Database database(database_path); + colmap::Reconstruction gt_reconstruction; + colmap::SyntheticDatasetOptions synthetic_dataset_options; + synthetic_dataset_options.num_cameras = 2; + synthetic_dataset_options.num_images = 9; + synthetic_dataset_options.num_points3D = 50; + synthetic_dataset_options.point2D_stddev = 0; + colmap::SynthesizeDataset( + synthetic_dataset_options, >_reconstruction, &database); + + ViewGraph view_graph; + std::unordered_map cameras; + std::unordered_map images; + std::unordered_map tracks; + + ConvertDatabaseToGlomap(database, view_graph, cameras, images); + + // PrepareRelativeRotations(view_graph, images); + PrepareGravity(gt_reconstruction, images); + + GlobalMapper global_mapper(CreateMapperTestOptions()); + global_mapper.Solve(database, view_graph, cameras, images, tracks); + + // Version with Gravity + for (bool use_gravity : {true, false}) { + SolveRotationAveraging( + view_graph, images, CreateRATestOptions(use_gravity)); + + colmap::Reconstruction reconstruction; + ConvertGlomapToColmap(cameras, images, tracks, reconstruction); + ExpectEqualRotations( + gt_reconstruction, reconstruction, /*max_rotation_error_deg=*/1e-2); + } +} + +TEST(RotationEstimator, WithNoiseAndOutliers) { + const std::string database_path = colmap::CreateTestDir() + "/database.db"; + + // FLAGS_v = 1; + colmap::Database database(database_path); + colmap::Reconstruction gt_reconstruction; + colmap::SyntheticDatasetOptions synthetic_dataset_options; + synthetic_dataset_options.num_cameras = 2; + synthetic_dataset_options.num_images = 7; + synthetic_dataset_options.num_points3D = 100; + synthetic_dataset_options.point2D_stddev = 1; + synthetic_dataset_options.inlier_match_ratio = 0.6; + colmap::SynthesizeDataset( + synthetic_dataset_options, >_reconstruction, &database); + + ViewGraph view_graph; + std::unordered_map cameras; + std::unordered_map images; + std::unordered_map tracks; + + ConvertDatabaseToGlomap(database, view_graph, cameras, images); + + PrepareGravity(gt_reconstruction, images, /*stddev_gravity=*/3e-1); + + GlobalMapper global_mapper(CreateMapperTestOptions()); + global_mapper.Solve(database, view_graph, cameras, images, tracks); + + for (bool use_gravity : {true, false}) { + SolveRotationAveraging( + view_graph, images, CreateRATestOptions(use_gravity)); + + colmap::Reconstruction reconstruction; + ConvertGlomapToColmap(cameras, images, tracks, reconstruction); + if (use_gravity) + ExpectEqualRotations( + gt_reconstruction, reconstruction, /*max_rotation_error_deg=*/1.); + else + ExpectEqualRotations( + gt_reconstruction, reconstruction, /*max_rotation_error_deg=*/2.); + } +} + +TEST(RotationEstimator, RefineGravity) { + const std::string database_path = colmap::CreateTestDir() + "/database.db"; + + // FLAGS_v = 2; + colmap::Database database(database_path); + colmap::Reconstruction gt_reconstruction; + colmap::SyntheticDatasetOptions synthetic_dataset_options; + synthetic_dataset_options.num_cameras = 2; + synthetic_dataset_options.num_images = 100; + synthetic_dataset_options.num_points3D = 200; + synthetic_dataset_options.point2D_stddev = 0; + colmap::SynthesizeDataset( + synthetic_dataset_options, >_reconstruction, &database); + + ViewGraph view_graph; + std::unordered_map cameras; + std::unordered_map images; + std::unordered_map tracks; + + ConvertDatabaseToGlomap(database, view_graph, cameras, images); + + PrepareGravity( + gt_reconstruction, images, /*stddev_gravity=*/0., /*outlier_ratio=*/0.3); + + GlobalMapper global_mapper(CreateMapperTestOptions()); + global_mapper.Solve(database, view_graph, cameras, images, tracks); + + GravityRefinerOptions opt_grav_refine; + GravityRefiner grav_refiner(opt_grav_refine); + grav_refiner.RefineGravity(view_graph, images); + + // Check whether the gravity does not have error after refinement + ExpectEqualGravity(gt_reconstruction, images, /*max_gravity_error_deg=*/1e-2); +} + +} // namespace +} // namespace glomap diff --git a/glomap/estimators/global_rotation_averaging.cc b/glomap/estimators/global_rotation_averaging.cc index 3f7d0319..e7122feb 100644 --- a/glomap/estimators/global_rotation_averaging.cc +++ b/glomap/estimators/global_rotation_averaging.cc @@ -16,6 +16,15 @@ double RelAngleError(double angle_12, double angle_1, double angle_2) { while (est < -EIGEN_PI) est += TWO_PI; + // Inject random noise if the angle is too close to the boundary to break the + // possible balance at the local minima + if (est > EIGEN_PI - 0.01 || est < -EIGEN_PI + 0.01) { + if (est < 0) + est += (rand() % 1000) / 1000.0 * 0.01; + else + est -= (rand() % 1000) / 1000.0 * 0.01; + } + return est; } } // namespace @@ -56,6 +65,9 @@ bool RotationEstimator::EstimateRotations( image.cam_from_world.rotation = Eigen::Quaterniond(AngleAxisToRotation( rotation_estimated_.segment(image_id_to_idx_[image_id], 3))); } + // Restore the prior position (t = -Rc = R * R_ori * t_ori = R * t_ori) + image.cam_from_world.translation = + (image.cam_from_world.rotation * image.cam_from_world.translation); } return true; @@ -207,6 +219,8 @@ void RotationEstimator::SetupLinearSystem( // Establish linear systems size_t curr_pos = 0; + std::vector weights; + weights.reserve(3 * view_graph.image_pairs.size()); for (const auto& [pair_id, image_pair] : view_graph.image_pairs) { if (!image_pair.is_valid) continue; @@ -221,6 +235,10 @@ void RotationEstimator::SetupLinearSystem( if (rel_temp_info_[pair_id].has_gravity) { coeffs.emplace_back(Eigen::Triplet(curr_pos, vector_idx1, -1)); coeffs.emplace_back(Eigen::Triplet(curr_pos, vector_idx2, 1)); + if (image_pair.weight >= 0) + weights.emplace_back(image_pair.weight); + else + weights.emplace_back(1); curr_pos++; } else { // If it is not gravity aligned, then we need to consider 3 dof @@ -245,6 +263,12 @@ void RotationEstimator::SetupLinearSystem( } else coeffs.emplace_back( Eigen::Triplet(curr_pos + 1, vector_idx2, 1)); + for (int i = 0; i < 3; i++) { + if (image_pair.weight >= 0) + weights.emplace_back(image_pair.weight); + else + weights.emplace_back(1); + } curr_pos += 3; } @@ -257,11 +281,13 @@ void RotationEstimator::SetupLinearSystem( images[fixed_camera_id_].gravity_info.has_gravity) { coeffs.emplace_back(Eigen::Triplet( curr_pos, image_id_to_idx_[fixed_camera_id_], 1)); + weights.emplace_back(1); curr_pos++; } else { for (int i = 0; i < 3; i++) { coeffs.emplace_back(Eigen::Triplet( curr_pos + i, image_id_to_idx_[fixed_camera_id_] + i, 1)); + weights.emplace_back(1); } curr_pos += 3; } @@ -269,6 +295,14 @@ void RotationEstimator::SetupLinearSystem( sparse_matrix_.resize(curr_pos, num_dof); sparse_matrix_.setFromTriplets(coeffs.begin(), coeffs.end()); + // Set up the weight matrix for the linear system + if (!options_.use_weight) { + weights_ = Eigen::ArrayXd::Ones(curr_pos); + } else { + weights_ = Eigen::ArrayXd(weights.size()); + for (size_t i = 0; i < weights.size(); i++) weights_[i] = weights[i]; + } + // Initialize x and b tangent_space_step_.resize(num_dof); tangent_space_residual_.resize(curr_pos); @@ -279,8 +313,8 @@ bool RotationEstimator::SolveL1Regression( L1SolverOptions opt_l1_solver; opt_l1_solver.max_num_iterations = 10; - L1Solver> l1_solver(opt_l1_solver, - sparse_matrix_); + L1Solver> l1_solver( + opt_l1_solver, weights_.matrix().asDiagonal() * sparse_matrix_); double last_norm = 0; double curr_norm = 0; @@ -295,7 +329,8 @@ bool RotationEstimator::SolveL1Regression( // use the current residual as b (Ax - b) tangent_space_step_.setZero(); - l1_solver.Solve(tangent_space_residual_, &tangent_space_step_); + l1_solver.Solve(weights_.matrix().asDiagonal() * tangent_space_residual_, + &tangent_space_step_); if (tangent_space_step_.array().isNaN().any()) { LOG(ERROR) << "nan error"; iteration++; @@ -393,7 +428,9 @@ bool RotationEstimator::SolveIRLS(const ViewGraph& view_graph, } // Update the factorization for the weighted values. - at_weight = sparse_matrix_.transpose() * weights_irls.matrix().asDiagonal(); + at_weight = sparse_matrix_.transpose() * + weights_irls.matrix().asDiagonal() * + weights_.matrix().asDiagonal(); llt.factorize(at_weight * sparse_matrix_); diff --git a/glomap/estimators/global_rotation_averaging.h b/glomap/estimators/global_rotation_averaging.h index 8899a1cb..599c7c5f 100644 --- a/glomap/estimators/global_rotation_averaging.h +++ b/glomap/estimators/global_rotation_averaging.h @@ -63,7 +63,7 @@ struct RotationEstimatorOptions { // Flg to use maximum spanning tree for initialization bool skip_initialization = false; - // TODO: Implement the weighted version for rotation averaging + // Flag to use weighting for rotation averaging bool use_weight = false; // Flag to use gravity for rotation averaging @@ -145,6 +145,9 @@ class RotationEstimator { // The fixed camera rotation (if with initialization, it would not be identity // matrix) Eigen::Vector3d fixed_camera_rotation_; + + // The weights for the edges + Eigen::ArrayXd weights_; }; } // namespace glomap diff --git a/glomap/estimators/gravity_refinement.cc b/glomap/estimators/gravity_refinement.cc index d1f96fa7..0f679bad 100644 --- a/glomap/estimators/gravity_refinement.cc +++ b/glomap/estimators/gravity_refinement.cc @@ -12,6 +12,10 @@ void GravityRefiner::RefineGravity(const ViewGraph& view_graph, view_graph.image_pairs; const std::unordered_map>& adjacency_list = view_graph.GetAdjacencyList(); + if (adjacency_list.empty()) { + LOG(INFO) << "Adjacency list not established" << std::endl; + return; + } // Identify the images that are error prone int counter_rect = 0; @@ -75,6 +79,7 @@ void GravityRefiner::RefineGravity(const ViewGraph& view_graph, if (gravities.size() < options_.min_num_neighbors) continue; // Then, run refinment + gravity = AverageGravity(gravities); colmap::SetSphereManifold<3>(&problem, gravity.data()); ceres::Solver::Summary summary_solver; ceres::Solve(options_.solver_options, &problem, &summary_solver); diff --git a/glomap/estimators/view_graph_calibration.cc b/glomap/estimators/view_graph_calibration.cc index 30c7d343..a66b5a2c 100644 --- a/glomap/estimators/view_graph_calibration.cc +++ b/glomap/estimators/view_graph_calibration.cc @@ -179,7 +179,7 @@ size_t ViewGraphCalibrator::FilterImagePairs(ViewGraph& view_graph) const { } LOG(INFO) << "invalid / total number of two view geometry: " - << invalid_counter << " / " << counter; + << invalid_counter << " / " << (counter / 2); return invalid_counter; } diff --git a/glomap/exe/rotation_averager.cc b/glomap/exe/rotation_averager.cc new file mode 100644 index 00000000..98460ab5 --- /dev/null +++ b/glomap/exe/rotation_averager.cc @@ -0,0 +1,105 @@ + +#include "glomap/controllers/rotation_averager.h" + +#include "glomap/controllers/option_manager.h" +#include "glomap/estimators/gravity_refinement.h" +#include "glomap/io/colmap_io.h" +#include "glomap/io/pose_io.h" +#include "glomap/types.h" + +#include +#include + +namespace glomap { +// ------------------------------------- +// Running Global Rotation Averager +// ------------------------------------- +int RunRotationAverager(int argc, char** argv) { + std::string relpose_path; + std::string output_path; + std::string gravity_path = ""; + std::string weight_path = ""; + + bool use_stratified = true; + bool refine_gravity = false; + bool use_weight = false; + + OptionManager options; + options.AddRequiredOption("relpose_path", &relpose_path); + options.AddRequiredOption("output_path", &output_path); + options.AddDefaultOption("gravity_path", &gravity_path); + options.AddDefaultOption("weight_path", &weight_path); + options.AddDefaultOption("use_stratified", &use_stratified); + options.AddDefaultOption("refine_gravity", &refine_gravity); + options.AddDefaultOption("use_weight", &use_weight); + options.AddGravityRefinerOptions(); + options.Parse(argc, argv); + + if (!colmap::ExistsFile(relpose_path)) { + LOG(ERROR) << "`relpose_path` is not a file"; + return EXIT_FAILURE; + } + + if (gravity_path != "" && !colmap::ExistsFile(gravity_path)) { + LOG(ERROR) << "`gravity_path` is not a file"; + return EXIT_FAILURE; + } + + if (weight_path != "" && !colmap::ExistsFile(weight_path)) { + LOG(ERROR) << "`weight_path` is not a file"; + return EXIT_FAILURE; + } + + if (use_weight && weight_path == "") { + LOG(ERROR) << "Weight path is required when use_weight is set to true"; + return EXIT_FAILURE; + } + + RotationAveragerOptions rotation_averager_options; + rotation_averager_options.skip_initialization = true; + rotation_averager_options.use_gravity = true; + + rotation_averager_options.use_stratified = use_stratified; + rotation_averager_options.use_weight = use_weight; + + // Load the database + ViewGraph view_graph; + std::unordered_map images; + + ReadRelPose(relpose_path, images, view_graph); + + if (gravity_path != "") { + ReadGravity(gravity_path, images); + } + + if (use_weight) { + ReadRelWeight(weight_path, images, view_graph); + } + + int num_img = view_graph.KeepLargestConnectedComponents(images); + LOG(INFO) << num_img << " / " << images.size() + << " are within the largest connected component"; + + if (refine_gravity && gravity_path != "") { + GravityRefiner grav_refiner(*options.gravity_refiner); + grav_refiner.RefineGravity(view_graph, images); + } + + colmap::Timer run_timer; + run_timer.Start(); + if (!SolveRotationAveraging(view_graph, images, rotation_averager_options)) { + LOG(ERROR) << "Failed to solve global rotation averaging"; + return EXIT_FAILURE; + } + run_timer.Pause(); + LOG(INFO) << "Global rotation averaging done in " + << run_timer.ElapsedSeconds() << " seconds"; + + // Write out the estimated rotation + WriteGlobalRotation(output_path, images); + LOG(INFO) << "Global rotation averaging done" << std::endl; + + return EXIT_SUCCESS; +} + +} // namespace glomap \ No newline at end of file diff --git a/glomap/exe/rotation_averager.h b/glomap/exe/rotation_averager.h new file mode 100644 index 00000000..0786b817 --- /dev/null +++ b/glomap/exe/rotation_averager.h @@ -0,0 +1,10 @@ +#pragma once + +#include "glomap/estimators/global_rotation_averaging.h" + +namespace glomap { + +// Use default values for most of the settings from database +int RunRotationAverager(int argc, char** argv); + +} // namespace glomap \ No newline at end of file diff --git a/glomap/glomap.cc b/glomap/glomap.cc index 19c77255..f9afc567 100644 --- a/glomap/glomap.cc +++ b/glomap/glomap.cc @@ -1,4 +1,5 @@ #include "glomap/exe/global_mapper.h" +#include "glomap/exe/rotation_averager.h" #include @@ -44,6 +45,7 @@ int main(int argc, char** argv) { std::vector> commands; commands.emplace_back("mapper", &glomap::RunMapper); commands.emplace_back("mapper_resume", &glomap::RunMapperResume); + commands.emplace_back("rotation_averager", &glomap::RunRotationAverager); if (argc == 1) { return ShowHelp(commands); @@ -62,7 +64,7 @@ int main(int argc, char** argv) { } if (matched_command_func == nullptr) { std::cout << "Command " << command << " not recognized. " - << "To list the available commands, run `colmap help`." + << "To list the available commands, run `glomap help`." << std::endl; return EXIT_FAILURE; } else { diff --git a/glomap/io/colmap_converter.cc b/glomap/io/colmap_converter.cc index a52455cd..abfe1ffc 100644 --- a/glomap/io/colmap_converter.cc +++ b/glomap/io/colmap_converter.cc @@ -188,14 +188,15 @@ void ConvertDatabaseToGlomap(const colmap::Database& database, << images_colmap.size() << std::flush; counter++; - image_t image_id = image.ImageId(); + const image_t image_id = image.ImageId(); if (image_id == colmap::kInvalidImageId) continue; auto ite = images.insert(std::make_pair( image_id, Image(image_id, image.CameraId(), image.Name()))); const colmap::PosePrior prior = database.ReadPosePrior(image_id); if (prior.IsValid()) { - ite.first->second.cam_from_world = Rigid3d( - colmap::Rigid3d(Eigen::Quaterniond::Identity(), prior.position)); + const colmap::Rigid3d world_from_cam_prior(Eigen::Quaterniond::Identity(), + prior.position); + ite.first->second.cam_from_world = Rigid3d(Inverse(world_from_cam_prior)); } else { ite.first->second.cam_from_world = Rigid3d(); } @@ -204,20 +205,18 @@ void ConvertDatabaseToGlomap(const colmap::Database& database, // Read keypoints for (auto& [image_id, image] : images) { - colmap::FeatureKeypoints keypoints = database.ReadKeypoints(image_id); - - image.features.reserve(keypoints.size()); - for (int i = 0; i < keypoints.size(); i++) { - image.features.emplace_back( - Eigen::Vector2d(keypoints[i].x, keypoints[i].y)); + const colmap::FeatureKeypoints keypoints = database.ReadKeypoints(image_id); + const int num_keypoints = keypoints.size(); + image.features.resize(num_keypoints); + for (int i = 0; i < num_keypoints; i++) { + image.features[i] = Eigen::Vector2d(keypoints[i].x, keypoints[i].y); } } // Add the cameras std::vector cameras_colmap = database.ReadAllCameras(); for (auto& camera : cameras_colmap) { - camera_t camera_id = camera.camera_id; - cameras[camera_id] = camera; + cameras[camera.camera_id] = camera; } // Add the matches diff --git a/glomap/io/gravity_io.cc b/glomap/io/gravity_io.cc deleted file mode 100644 index 7e974db2..00000000 --- a/glomap/io/gravity_io.cc +++ /dev/null @@ -1,45 +0,0 @@ -#include "gravity_io.h" - -#include - -namespace glomap { -void ReadGravity(const std::string& gravity_path, - std::unordered_map& images) { - std::unordered_map name_idx; - for (const auto& [image_id, image] : images) { - name_idx[image.file_name] = image_id; - } - - std::ifstream file(gravity_path); - - // Read in the file list - std::string line, item; - Eigen::Vector3d gravity; - int counter = 0; - while (std::getline(file, line)) { - std::stringstream line_stream(line); - - // file_name - std::string name; - std::getline(line_stream, name, ' '); - - // Gravity - for (double i = 0; i < 3; i++) { - std::getline(line_stream, item, ' '); - gravity[i] = std::stod(item); - } - - // Check whether the image present - auto ite = name_idx.find(name); - if (ite != name_idx.end()) { - counter++; - images[ite->second].gravity_info.SetGravity(gravity); - // Make sure the initialization is aligned with the gravity - images[ite->second].cam_from_world.rotation = - images[ite->second].gravity_info.GetRAlign().transpose(); - } - } - LOG(INFO) << counter << " images are loaded with gravity" << std::endl; -} - -} // namespace glomap \ No newline at end of file diff --git a/glomap/io/gravity_io.h b/glomap/io/gravity_io.h deleted file mode 100644 index 4dabda23..00000000 --- a/glomap/io/gravity_io.h +++ /dev/null @@ -1,14 +0,0 @@ -#pragma once - -#include "glomap/scene/image.h" - -#include - -namespace glomap { -// Require the gravity in the format: image_name, gravity (3 numbers) -// Gravity should be the direction of [0,1,0] in the image frame -// image.cam_from_world * [0,1,0]^T = g -void ReadGravity(const std::string& gravity_path, - std::unordered_map& images); - -} // namespace glomap \ No newline at end of file diff --git a/glomap/io/pose_io.cc b/glomap/io/pose_io.cc new file mode 100644 index 00000000..eeda2e6b --- /dev/null +++ b/glomap/io/pose_io.cc @@ -0,0 +1,212 @@ +#include "pose_io.h" + +#include +#include +#include + +namespace glomap { +void ReadRelPose(const std::string& file_path, + std::unordered_map& images, + ViewGraph& view_graph) { + std::unordered_map name_idx; + image_t max_image_id = 0; + for (const auto& [image_id, image] : images) { + name_idx[image.file_name] = image_id; + + max_image_id = std::max(max_image_id, image_id); + } + + std::ifstream file(file_path); + + // Read in data + std::string line; + std::string item; + + size_t counter = 0; + + // Required data structures + // IMAGE_NAME_1 IMAGE_NAME_2 QW QX QY QZ TX TY TZ + while (std::getline(file, line)) { + std::stringstream line_stream(line); + + std::string file1, file2; + std::getline(line_stream, item, ' '); + file1 = item; + std::getline(line_stream, item, ' '); + file2 = item; + + if (name_idx.find(file1) == name_idx.end()) { + max_image_id += 1; + images.insert( + std::make_pair(max_image_id, Image(max_image_id, -1, file1))); + name_idx[file1] = max_image_id; + } + if (name_idx.find(file2) == name_idx.end()) { + max_image_id += 1; + images.insert( + std::make_pair(max_image_id, Image(max_image_id, -1, file2))); + name_idx[file2] = max_image_id; + } + + image_t index1 = name_idx[file1]; + image_t index2 = name_idx[file2]; + + image_pair_t pair_id = ImagePair::ImagePairToPairId(index1, index2); + + // rotation + Rigid3d pose_rel; + for (int i = 0; i < 4; i++) { + std::getline(line_stream, item, ' '); + pose_rel.rotation.coeffs()[(i + 3) % 4] = std::stod(item); + } + + for (int i = 0; i < 3; i++) { + std::getline(line_stream, item, ' '); + pose_rel.translation[i] = std::stod(item); + } + + view_graph.image_pairs.insert( + std::make_pair(pair_id, ImagePair(index1, index2, pose_rel))); + counter++; + } + LOG(INFO) << counter << " relpose are loaded" << std::endl; +} + +void ReadRelWeight(const std::string& file_path, + const std::unordered_map& images, + ViewGraph& view_graph) { + std::unordered_map name_idx; + for (const auto& [image_id, image] : images) { + name_idx[image.file_name] = image_id; + } + + std::ifstream file(file_path); + + // Read in data + std::string line; + std::string item; + + size_t counter = 0; + + // Required data structures + // IMAGE_NAME_1 IMAGE_NAME_2 QW QX QY QZ TX TY TZ + while (std::getline(file, line)) { + std::stringstream line_stream(line); + + std::string file1, file2; + std::getline(line_stream, item, ' '); + file1 = item; + std::getline(line_stream, item, ' '); + file2 = item; + + if (name_idx.find(file1) == name_idx.end() || + name_idx.find(file2) == name_idx.end()) + continue; + + image_t index1 = name_idx[file1]; + image_t index2 = name_idx[file2]; + + image_pair_t pair_id = ImagePair::ImagePairToPairId(index1, index2); + + if (view_graph.image_pairs.find(pair_id) == view_graph.image_pairs.end()) + continue; + + std::getline(line_stream, item, ' '); + view_graph.image_pairs[pair_id].weight = std::stod(item); + counter++; + } + LOG(INFO) << counter << " weights are used are loaded" << std::endl; +} + +void ReadGravity(const std::string& gravity_path, + std::unordered_map& images) { + std::unordered_map name_idx; + for (const auto& [image_id, image] : images) { + name_idx[image.file_name] = image_id; + } + + std::ifstream file(gravity_path); + + // Read in the file list + std::string line, item; + Eigen::Vector3d gravity; + int counter = 0; + while (std::getline(file, line)) { + std::stringstream line_stream(line); + + // file_name + std::string name; + std::getline(line_stream, name, ' '); + + // Gravity + for (double i = 0; i < 3; i++) { + std::getline(line_stream, item, ' '); + gravity[i] = std::stod(item); + } + + // Check whether the image present + auto ite = name_idx.find(name); + if (ite != name_idx.end()) { + counter++; + images[ite->second].gravity_info.SetGravity(gravity); + // Make sure the initialization is aligned with the gravity + images[ite->second].cam_from_world.rotation = + images[ite->second].gravity_info.GetRAlign().transpose(); + } + } + LOG(INFO) << counter << " images are loaded with gravity" << std::endl; +} + +void WriteGlobalRotation(const std::string& file_path, + const std::unordered_map& images) { + std::ofstream file(file_path); + std::set existing_images; + for (const auto& [image_id, image] : images) { + if (image.is_registered) { + existing_images.insert(image_id); + } + } + for (const auto& image_id : existing_images) { + const auto image = images.at(image_id); + if (!image.is_registered) continue; + file << image.file_name; + for (int i = 0; i < 4; i++) { + file << " " << image.cam_from_world.rotation.coeffs()[(i + 3) % 4]; + } + file << "\n"; + } +} + +void WriteRelPose(const std::string& file_path, + const std::unordered_map& images, + const ViewGraph& view_graph) { + std::ofstream file(file_path); + + // Sort the image pairs by image name + std::map name_pair; + for (const auto& [pair_id, image_pair] : view_graph.image_pairs) { + if (image_pair.is_valid) { + const auto image1 = images.at(image_pair.image_id1); + const auto image2 = images.at(image_pair.image_id2); + name_pair[image1.file_name + " " + image2.file_name] = pair_id; + } + } + + // Write the image pairs + for (const auto& [name, pair_id] : name_pair) { + const auto image_pair = view_graph.image_pairs.at(pair_id); + if (!image_pair.is_valid) continue; + file << images.at(image_pair.image_id1).file_name << " " + << images.at(image_pair.image_id2).file_name; + for (int i = 0; i < 4; i++) { + file << " " << image_pair.cam2_from_cam1.rotation.coeffs()[(i + 3) % 4]; + } + for (int i = 0; i < 3; i++) { + file << " " << image_pair.cam2_from_cam1.translation[i]; + } + file << "\n"; + } + + LOG(INFO) << name_pair.size() << " relpose are written" << std::endl; +} +} // namespace glomap \ No newline at end of file diff --git a/glomap/io/pose_io.h b/glomap/io/pose_io.h new file mode 100644 index 00000000..e98112f7 --- /dev/null +++ b/glomap/io/pose_io.h @@ -0,0 +1,37 @@ +#pragma once + +#include "glomap/scene/types_sfm.h" + +#include + +namespace glomap { +// Required data structures +// IMAGE_NAME_1 IMAGE_NAME_2 QW QX QY QZ TX TY TZ +void ReadRelPose(const std::string& file_path, + std::unordered_map& images, + ViewGraph& view_graph); + +// Required data structures +// IMAGE_NAME_1 IMAGE_NAME_2 weight +void ReadRelWeight(const std::string& file_path, + const std::unordered_map& images, + ViewGraph& view_graph); + +// Require the gravity in the format: +// IMAGE_NAME GX GY GZ +// Gravity should be the direction of [0,1,0] in the image frame +// image.cam_from_world * [0,1,0]^T = g +void ReadGravity(const std::string& gravity_path, + std::unordered_map& images); + +// Output would be of the format: +// IMAGE_NAME QW QX QY QZ +void WriteGlobalRotation(const std::string& file_path, + const std::unordered_map& images); + +// Output would be of the format: +// IMAGE_NAME_1 IMAGE_NAME_2 QW QX QY QZ TX TY TZ +void WriteRelPose(const std::string& file_path, + const std::unordered_map& images, + const ViewGraph& view_graph); +} // namespace glomap \ No newline at end of file diff --git a/glomap/math/gravity.cc b/glomap/math/gravity.cc index 485a4cdb..30b83c66 100644 --- a/glomap/math/gravity.cc +++ b/glomap/math/gravity.cc @@ -31,4 +31,70 @@ Eigen::Matrix3d AngleToRotUp(double angle) { Eigen::Vector3d aa(0, angle, 0); return AngleAxisToRotation(aa); } + +// Code adapted from +// https://gist.github.com/PeteBlackerThe3rd/f73e9d569e29f23e8bd828d7886636a0 +Eigen::Vector3d AverageGravity(const std::vector& gravities) { + if (gravities.size() == 0) { + std::cerr + << "Error trying to calculate the average gravities of an empty set!\n"; + return Eigen::Vector3d::Zero(); + } + + // first build a 3x3 matrix which is the elementwise sum of the product of + // each quaternion with itself + Eigen::Matrix3d A = Eigen::Matrix3d::Zero(); + + for (int g = 0; g < gravities.size(); ++g) + A += gravities[g] * gravities[g].transpose(); + + // normalise with the number of gravities + A /= gravities.size(); + + // Compute the SVD of this 3x3 matrix + Eigen::JacobiSVD svd( + A, Eigen::ComputeThinU | Eigen::ComputeThinV); + + Eigen::VectorXd singular_values = svd.singularValues(); + Eigen::MatrixXd U = svd.matrixU(); + + // find the eigen vector corresponding to the largest eigen value + int largest_eigen_value_index = -1; + float largest_eigen_value; + bool first = true; + + for (int i = 0; i < singular_values.rows(); ++i) { + if (first) { + largest_eigen_value = singular_values(i); + largest_eigen_value_index = i; + first = false; + } else if (singular_values(i) > largest_eigen_value) { + largest_eigen_value = singular_values(i); + largest_eigen_value_index = i; + } + } + + Eigen::Vector3d average; + average(0) = U(0, largest_eigen_value_index); + average(1) = U(1, largest_eigen_value_index); + average(2) = U(2, largest_eigen_value_index); + + int negative_counter = 0; + for (int g = 0; g < gravities.size(); ++g) { + if (gravities[g].dot(average) < 0) negative_counter++; + } + if (negative_counter > gravities.size() / 2) { + average = -average; + } + + return average; +} + +double CalcAngle(const Eigen::Vector3d& gravity1, + const Eigen::Vector3d& gravity2) { + double cos_r = gravity1.dot(gravity2) / (gravity1.norm() * gravity2.norm()); + cos_r = std::min(std::max(cos_r, -1.), 1.); + + return std::acos(cos_r) * 180 / EIGEN_PI; +} } // namespace glomap \ No newline at end of file diff --git a/glomap/math/gravity.h b/glomap/math/gravity.h index 789ddd14..bf7a0406 100644 --- a/glomap/math/gravity.h +++ b/glomap/math/gravity.h @@ -14,4 +14,9 @@ double RotUpToAngle(const Eigen::Matrix3d& R_up); // Get the upright rotation matrix from a rotation angle Eigen::Matrix3d AngleToRotUp(double angle); +// Estimate the average gravity direction from a set of gravity directions +Eigen::Vector3d AverageGravity(const std::vector& gravities); + +double CalcAngle(const Eigen::Vector3d& gravity1, + const Eigen::Vector3d& gravity2); } // namespace glomap diff --git a/glomap/scene/image.h b/glomap/scene/image.h index 1497efae..9dd94f0a 100644 --- a/glomap/scene/image.h +++ b/glomap/scene/image.h @@ -14,7 +14,7 @@ struct GravityInfo { const Eigen::Matrix3d& GetRAlign() const { return R_align; } inline void SetGravity(const Eigen::Vector3d& g); - inline Eigen::Vector3d GetGravity(); + inline Eigen::Vector3d GetGravity() const { return gravity; }; private: // Direction of the gravity @@ -66,5 +66,4 @@ void GravityInfo::SetGravity(const Eigen::Vector3d& g) { has_gravity = true; } -Eigen::Vector3d GravityInfo::GetGravity() { return gravity; } } // namespace glomap diff --git a/glomap/scene/image_pair.h b/glomap/scene/image_pair.h index c913cab5..fba6534a 100644 --- a/glomap/scene/image_pair.h +++ b/glomap/scene/image_pair.h @@ -27,7 +27,7 @@ struct ImagePair { bool is_valid = true; // weight is the initial inlier rate - double weight = 0; + double weight = -1; // one of `ConfigurationType`. int config = colmap::TwoViewGeometry::UNDEFINED; From 164f43bb5f697364a976ed2df28ae449108d241b Mon Sep 17 00:00:00 2001 From: Linfei Pan <36349740+lpanaf@users.noreply.github.com> Date: Thu, 20 Mar 2025 17:07:56 +0100 Subject: [PATCH 32/45] loose threshold for the test (#177) --- glomap/controllers/rotation_averager_test.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/glomap/controllers/rotation_averager_test.cc b/glomap/controllers/rotation_averager_test.cc index dbab164e..4da6b98c 100644 --- a/glomap/controllers/rotation_averager_test.cc +++ b/glomap/controllers/rotation_averager_test.cc @@ -190,7 +190,7 @@ TEST(RotationEstimator, WithNoiseAndOutliers) { ConvertGlomapToColmap(cameras, images, tracks, reconstruction); if (use_gravity) ExpectEqualRotations( - gt_reconstruction, reconstruction, /*max_rotation_error_deg=*/1.); + gt_reconstruction, reconstruction, /*max_rotation_error_deg=*/1.5); else ExpectEqualRotations( gt_reconstruction, reconstruction, /*max_rotation_error_deg=*/2.); From bd0db55f731301c84644615c24f3470bb7dc8062 Mon Sep 17 00:00:00 2001 From: Jhacson Meza Date: Wed, 7 May 2025 20:30:00 +0200 Subject: [PATCH 33/45] Check if value is set to avoid out-of-bound indexing (#178) --- glomap/estimators/bundle_adjustment.cc | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/glomap/estimators/bundle_adjustment.cc b/glomap/estimators/bundle_adjustment.cc index 57f85b13..0e5ee5d3 100644 --- a/glomap/estimators/bundle_adjustment.cc +++ b/glomap/estimators/bundle_adjustment.cc @@ -208,11 +208,13 @@ void BundleAdjuster::ParameterizeVariables( } } - // Set the first camera to be fixed to remove the gauge ambiguity. - problem_->SetParameterBlockConstant( - images[center].cam_from_world.rotation.coeffs().data()); - problem_->SetParameterBlockConstant( - images[center].cam_from_world.translation.data()); + if (counter > 0) { + // Set the first camera to be fixed to remove the gauge ambiguity. + problem_->SetParameterBlockConstant( + images[center].cam_from_world.rotation.coeffs().data()); + problem_->SetParameterBlockConstant( + images[center].cam_from_world.translation.data()); + } // Parameterize the camera parameters, or set them to be constant if desired if (options_.optimize_intrinsics && !options_.optimize_principal_point) { From 5f40c0045cef30492fad59229da918d3a37695a3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Johannes=20Sch=C3=B6nberger?= Date: Mon, 12 May 2025 17:07:35 +0200 Subject: [PATCH 34/45] Update vcpkg and CI pipeline with latest changes in colmap (#193) * Update vcpkg to latest version * d * d * d --- .github/workflows/install-ccache.ps1 | 6 +-- .github/workflows/ubuntu.yml | 32 +++++++++---- .github/workflows/windows.yml | 53 +++++++++++++--------- cmake/FindDependencies.cmake | 5 +- scripts/format/{clang_format.sh => c++.sh} | 26 ++--------- 5 files changed, 66 insertions(+), 56 deletions(-) rename scripts/format/{clang_format.sh => c++.sh} (58%) diff --git a/.github/workflows/install-ccache.ps1 b/.github/workflows/install-ccache.ps1 index f56a9533..c9d15035 100644 --- a/.github/workflows/install-ccache.ps1 +++ b/.github/workflows/install-ccache.ps1 @@ -4,10 +4,10 @@ param ( [string] $Destination ) -$version = "4.8" -$folder = "ccache-$version-windows-x86_64" +$version = "4.10.2" +$folder="ccache-$version-windows-x86_64" $url = "https://github.com/ccache/ccache/releases/download/v$version/$folder.zip" -$expectedSha256 = "A2B3BAB4BB8318FFC5B3E4074DC25636258BC7E4B51261F7D9BEF8127FDA8309" +$expectedSha256 = "6252F081876A9A9F700FAE13A5AEC5D0D486B28261D7F1F72AC11C7AD9DF4DA9" $ErrorActionPreference = "Stop" diff --git a/.github/workflows/ubuntu.yml b/.github/workflows/ubuntu.yml index ebd56549..f3c2afcc 100644 --- a/.github/workflows/ubuntu.yml +++ b/.github/workflows/ubuntu.yml @@ -16,7 +16,13 @@ jobs: strategy: matrix: config: [ - + { + os: ubuntu-24.04, + cmakeBuildType: RelWithDebInfo, + asanEnabled: false, + cudaEnabled: false, + checkCodeFormat: true, + }, { os: ubuntu-22.04, cmakeBuildType: Release, @@ -35,7 +41,7 @@ jobs: os: ubuntu-22.04, cmakeBuildType: Release, asanEnabled: true, - cudaEnabled: true, + cudaEnabled: false, checkCodeFormat: false, }, { @@ -62,14 +68,15 @@ jobs: key: v${{ env.COMPILER_CACHE_VERSION }}-${{ matrix.config.os }}-${{ matrix.config.cmakeBuildType }}-${{ matrix.config.asanEnabled }}--${{ matrix.config.cudaEnabled }}-${{ github.run_id }}-${{ github.run_number }} restore-keys: v${{ env.COMPILER_CACHE_VERSION }}-${{ matrix.config.os }}-${{ matrix.config.cmakeBuildType }}-${{ matrix.config.asanEnabled }}--${{ matrix.config.cudaEnabled }} path: ${{ env.COMPILER_CACHE_DIR }} - - name: Install compiler cache run: | mkdir -p "$CCACHE_DIR" "$CTCACHE_DIR" echo "$COMPILER_CACHE_DIR/bin" >> $GITHUB_PATH + if [ -f "$COMPILER_CACHE_DIR/bin/ccache" ]; then exit 0 fi + set -x wget https://github.com/ccache/ccache/releases/download/v4.8.2/ccache-4.8.2-linux-x86_64.tar.xz echo "0b33f39766fe9db67f40418aed6a5b3d7b2f4f7fab025a8213264b77a2d0e1b1 ccache-4.8.2-linux-x86_64.tar.xz" | sha256sum --check @@ -81,14 +88,16 @@ jobs: echo "108b087f156a9fe7da0c796de1ef73f5855d2a33a27983769ea39061359a40fc ${ctcache_commit_id}.zip" | sha256sum --check unzip "${ctcache_commit_id}.zip" mv ctcache-${ctcache_commit_id}/clang-tidy* "$COMPILER_CACHE_DIR/bin" + - name: Check code format if: matrix.config.checkCodeFormat run: | set +x -euo pipefail - sudo apt-get update && sudo apt-get install -y clang-format-14 black - ./scripts/format/clang_format.sh + python -m pip install clang-format==19.1.0 + ./scripts/format/c++.sh git diff --name-only git diff --exit-code || (echo "Code formatting failed" && exit 1) + - name: Setup Ubuntu run: | sudo apt-get update && sudo apt-get install -y \ @@ -96,11 +105,9 @@ jobs: cmake \ ninja-build \ libboost-program-options-dev \ - libboost-filesystem-dev \ libboost-graph-dev \ libboost-system-dev \ libeigen3-dev \ - libsuitesparse-dev \ libceres-dev \ libflann-dev \ libfreeimage-dev \ @@ -116,7 +123,9 @@ jobs: libcgal-qt5-dev \ libgl1-mesa-dri \ libunwind-dev \ + libcurl4-openssl-dev \ xvfb + if [ "${{ matrix.config.cudaEnabled }}" == "true" ]; then if [ "${{ matrix.config.os }}" == "ubuntu-20.04" ]; then sudo apt-get install -y \ @@ -134,16 +143,19 @@ jobs: echo "CUDAHOSTCXX=/usr/bin/g++-10" >> $GITHUB_ENV fi fi + if [ "${{ matrix.config.asanEnabled }}" == "true" ]; then sudo apt-get install -y clang-15 libomp-15-dev echo "CC=/usr/bin/clang-15" >> $GITHUB_ENV echo "CXX=/usr/bin/clang++-15" >> $GITHUB_ENV fi + if [ "${{ matrix.config.cmakeBuildType }}" == "ClangTidy" ]; then sudo apt-get install -y clang-15 clang-tidy-15 libomp-15-dev echo "CC=/usr/bin/clang-15" >> $GITHUB_ENV echo "CXX=/usr/bin/clang++-15" >> $GITHUB_ENV fi + - name: Upgrade CMake run: | CMAKE_VERSION=3.28.6 @@ -152,6 +164,7 @@ jobs: tar -xzf ${CMAKE_DIR}.tar.gz sudo cp -r ${CMAKE_DIR}/* /usr/local/ rm -rf ${CMAKE_DIR}* + - name: Configure and build run: | set -x @@ -167,6 +180,7 @@ jobs: -DTESTS_ENABLED=ON \ -DASAN_ENABLED=${{ matrix.config.asanEnabled }} ninja -k 10000 + - name: Run tests if: ${{ matrix.config.cmakeBuildType != 'ClangTidy' }} run: | @@ -176,15 +190,17 @@ jobs: sleep 3 cd build ctest --output-on-failure -E .+colmap_.* + - name: Cleanup compiler cache run: | set -x ccache --show-stats --verbose ccache --evict-older-than 1d ccache --show-stats --verbose + echo "Size of ctcache before: $(du -sh $CTCACHE_DIR)" echo "Number of ctcache files before: $(find $CTCACHE_DIR | wc -l)" # Delete cache older than 10 days. find "$CTCACHE_DIR"/*/ -mtime +10 -print0 | xargs -0 rm -rf echo "Size of ctcache after: $(du -sh $CTCACHE_DIR)" - echo "Number of ctcache files after: $(find $CTCACHE_DIR | wc -l)"'' + echo "Number of ctcache files after: $(find $CTCACHE_DIR | wc -l)" diff --git a/.github/workflows/windows.yml b/.github/workflows/windows.yml index 1adc3635..2cfde898 100644 --- a/.github/workflows/windows.yml +++ b/.github/workflows/windows.yml @@ -16,13 +16,6 @@ jobs: strategy: matrix: config: [ - { - os: windows-2019, - cmakeBuildType: Release, - cudaEnabled: false, - testsEnabled: true, - exportPackage: false, - }, { os: windows-2022, cmakeBuildType: Release, @@ -44,18 +37,35 @@ jobs: COMPILER_CACHE_DIR: ${{ github.workspace }}/compiler-cache CCACHE_DIR: ${{ github.workspace }}/compiler-cache/ccache CCACHE_BASEDIR: ${{ github.workspace }} - VCPKG_COMMIT_ID: e01906b2ba7e645a76ee021a19de616edc98d29f - VCPKG_BINARY_SOURCES: "clear;x-gha,readwrite" + VCPKG_COMMIT_ID: bc3512a509f9d29b37346a7e7e929f9a26e66c7e + GLOG_v: 1 + GLOG_logtostderr: 1 steps: - uses: actions/checkout@v4 - - - name: Export GitHub Actions cache env - uses: actions/github-script@v7 - with: - script: | - core.exportVariable('ACTIONS_CACHE_URL', process.env.ACTIONS_CACHE_URL || ''); - core.exportVariable('ACTIONS_RUNTIME_TOKEN', process.env.ACTIONS_RUNTIME_TOKEN || ''); + + # We define the vcpkg binary sources using separate variables for read and + # write operations: + # * Read sources are defined as inline. These can be read by anyone and, + # in particular, pull requests from forks. Unfortunately, we cannot + # define these as action environment variables. See: + # https://github.com/orgs/community/discussions/44322 + # * Write sources are defined as action secret variables. These cannot be + # read by pull requests from forks but only from pull requests from + # within the target repository (i.e., created by a repository owner). + # This protects us from malicious actors accessing our secrets and + # gaining write access to our binary cache. For more information, see: + # https://securitylab.github.com/resources/github-actions-preventing-pwn-requests/ + - name: Setup vcpkg binary cache + shell: pwsh + run: | + # !!!PLEASE!!! be nice and don't use this cache for your own purposes. This is only meant for CI purposes in this repository. + $VCPKG_BINARY_SOURCES = "clear;x-azblob,https://colmap.blob.core.windows.net/github-actions-cache,sp=r&st=2024-12-10T17:29:32Z&se=2030-12-31T01:29:32Z&spr=https&sv=2022-11-02&sr=c&sig=bWydkilTMjRn3LHKTxLgdWrFpV4h%2Finzoe9QCOcPpYQ%3D,read" + if ("${{ secrets.VCPKG_BINARY_CACHE_AZBLOB_URL }}") { + # The secrets are only accessible in runs triggered from within the target repository and not forks. + $VCPKG_BINARY_SOURCES += ";x-azblob,${{ secrets.VCPKG_BINARY_CACHE_AZBLOB_URL }},${{ secrets.VCPKG_BINARY_CACHE_AZBLOB_SAS }},write" + } + echo "VCPKG_BINARY_SOURCES=${VCPKG_BINARY_SOURCES}" >> "${env:GITHUB_ENV}" - name: Compiler cache uses: actions/cache@v4 @@ -70,11 +80,9 @@ jobs: run: | New-Item -ItemType Directory -Force -Path "${{ env.CCACHE_DIR }}" echo "${{ env.COMPILER_CACHE_DIR }}/bin" | Out-File -Encoding utf8 -Append -FilePath $env:GITHUB_PATH - if (Test-Path -PathType Leaf "${{ env.COMPILER_CACHE_DIR }}/bin/ccache.exe") { exit } - .github/workflows/install-ccache.ps1 -Destination "${{ env.COMPILER_CACHE_DIR }}/bin" - name: Install CUDA @@ -86,9 +94,6 @@ jobs: sub-packages: '["nvcc", "nvtx", "cudart", "curand", "curand_dev", "nvrtc_dev"]' method: 'network' - - name: Install CMake and Ninja - uses: lukka/get-cmake@latest - - name: Setup vcpkg shell: pwsh run: | @@ -99,6 +104,12 @@ jobs: git reset --hard ${{ env.VCPKG_COMMIT_ID }} ./bootstrap-vcpkg.bat + - name: Install CMake and Ninja + uses: lukka/get-cmake@latest + with: + cmakeVersion: "3.31.0" + ninjaVersion: "1.12.1" + - name: Configure and build shell: pwsh run: | diff --git a/cmake/FindDependencies.cmake b/cmake/FindDependencies.cmake index a59323a1..caa2275c 100644 --- a/cmake/FindDependencies.cmake +++ b/cmake/FindDependencies.cmake @@ -1,7 +1,10 @@ set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake") find_package(Eigen3 3.4 REQUIRED) -find_package(SuiteSparse COMPONENTS CHOLMOD REQUIRED) +find_package(CHOLMOD QUIET) +if(NOT TARGET SuiteSparse::CHOLMOD) + find_package(SuiteSparse COMPONENTS CHOLMOD REQUIRED) +endif() find_package(Ceres REQUIRED COMPONENTS SuiteSparse) find_package(Boost REQUIRED) diff --git a/scripts/format/clang_format.sh b/scripts/format/c++.sh similarity index 58% rename from scripts/format/clang_format.sh rename to scripts/format/c++.sh index 29710188..07f09f36 100755 --- a/scripts/format/clang_format.sh +++ b/scripts/format/c++.sh @@ -2,28 +2,9 @@ # This script applies clang-format to the whole repository. -# Find clang-format -tools=' - clang-format -' - -clang_format='' -for tool in ${tools}; do - if type -p "${tool}" > /dev/null; then - clang_format=$tool - break - fi -done - -if [ -z "$clang_format" ]; then - echo "Could not locate clang-format" - exit 1 -fi -echo "Found clang-format: $(which ${clang_format})" - # Check version -version_string=$($clang_format --version | sed -E 's/^.*(\d+\.\d+\.\d+-.*).*$/\1/') -expected_version_string='14.0.0' +version_string=$(clang-format --version | sed -E 's/^.*(\d+\.\d+\.\d+-.*).*$/\1/') +expected_version_string='19.1.0' if [[ "$version_string" =~ "$expected_version_string" ]]; then echo "clang-format version '$version_string' matches '$expected_version_string'" else @@ -40,5 +21,4 @@ all_files=$( \ num_files=$(echo $all_files | wc -w) echo "Formatting ${num_files} files" -# Run clang-format -${clang_format} -i $all_files +clang-format -i $all_files From 0a87b96e70b18e55b33651393fb577d3da2716c6 Mon Sep 17 00:00:00 2001 From: Linfei Pan <36349740+lpanaf@users.noreply.github.com> Date: Mon, 12 May 2025 18:09:00 +0200 Subject: [PATCH 35/45] improved readme (#194) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * improved readme * Update docs/rotation_averager.md Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> * Update docs/getting_started.md Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> * Update docs/rotation_averager.md Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> * Update docs/getting_started.md Co-authored-by: Johannes Schönberger * Update docs/getting_started.md Co-authored-by: Johannes Schönberger --------- Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> Co-authored-by: Johannes Schönberger --- README.md | 2 +- docs/getting_started.md | 6 ++++++ docs/rotation_averager.md | 11 +++++++++-- 3 files changed, 16 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 5c388af3..d6250b51 100644 --- a/README.md +++ b/README.md @@ -41,7 +41,7 @@ glomap mapper --database_path DATABASE_PATH --output_path OUTPUT_PATH --image_pa ``` For more details on the command line interface, one can type `glomap -h` or `glomap mapper -h` for help. -We also provide a guide on improving the obtained reconstruction, which can be found [here](docs/getting_started.md) +We also provide a guide on improving the obtained reconstruction, which can be found [here](docs/getting_started.md). Note: - GLOMAP depends on two external libraries - [COLMAP](https://github.com/colmap/colmap) and [PoseLib](https://github.com/PoseLib/PoseLib). diff --git a/docs/getting_started.md b/docs/getting_started.md index 6ae06514..a6399f56 100644 --- a/docs/getting_started.md +++ b/docs/getting_started.md @@ -44,3 +44,9 @@ retriangulation should already been performed. The number of global positioning and bundle adjustment iterations can be limited using the `--GlobalPositioning.max_num_iterations` and `--BundleAdjustment.max_num_iterations` options. + +#### Enable GPU-based solver + +If Ceres 2.3 or above is installed and cuDSS is installed, GLOMAP supports GPU +accelerated optimization. The process can be largely sped up with flags +`--GlobalPositioning.use_gpu 1 --BundleAdjustment.use_gpu`. diff --git a/docs/rotation_averager.md b/docs/rotation_averager.md index 5c4e9947..1fc342de 100644 --- a/docs/rotation_averager.md +++ b/docs/rotation_averager.md @@ -50,8 +50,15 @@ The gravity direction file is expected to be of the following format ``` IMAGE_NAME GX GY GZ ``` -The gravity direction $g$ should $[0, 1, 0]$ if the image is parallel to the ground plane, and the estimated rotation would have the property that $R_i \cdot [0, 1, 0]^\top = g$. -If is acceptable if only a subset of all images have gravity direciton. +The gravity direction $g$ should $[0, 1, 0]$ if the image is orthogonal to the ground plane, and the estimated rotation would have the property that $R_i \cdot [0, 1, 0]^\top = g$. +More explicitly, suppose we can transpose a 3D point from the world coordinate to the image coordinate by RX + t = x. Here: +- `R` is a 3x3 rotation matrix that aligns the world coordinate system with the image coordinate system. +- `X` is a 3D point in the world coordinate system. +- `t` is a 3x1 translation vector that shifts the world coordinate system to the image coordinate system. +- `x` is the corresponding 3D point in the image coordinate system. +The gravity direction should be the second column of the rotation matrix `R`. + +It is acceptable if only a subset of all images have gravity direction. If the specified image name does not match any known image name from relative pose, it is ignored. ### Output From 032a40cfa9f59c16f2c7042f95c28c1fbac2e518 Mon Sep 17 00:00:00 2001 From: Gustavo Stahl Date: Sun, 22 Jun 2025 18:14:40 +0200 Subject: [PATCH 36/45] Fix compilation issues on MacOS (#196) --- cmake/FindDependencies.cmake | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/cmake/FindDependencies.cmake b/cmake/FindDependencies.cmake index caa2275c..a38b6276 100644 --- a/cmake/FindDependencies.cmake +++ b/cmake/FindDependencies.cmake @@ -47,6 +47,23 @@ message(STATUS "Configuring COLMAP...") set(UNINSTALL_ENABLED OFF CACHE INTERNAL "") if (FETCH_COLMAP) FetchContent_MakeAvailable(COLMAP) + + # Define where to store the patch + set(COLMAP_PATCH_PATH ${CMAKE_BINARY_DIR}/fix_poisson.patch) + + # Download the patch from GitHub + file(DOWNLOAD + https://github.com/colmap/colmap/commit/a586e7cb223cc86c609105246ecd3a10e0c55131.patch + ${COLMAP_PATCH_PATH} + SHOW_PROGRESS + STATUS PATCH_DOWNLOAD_STATUS + ) + # Apply the patch + execute_process( + COMMAND git apply ${COLMAP_PATCH_PATH} + WORKING_DIRECTORY ${colmap_SOURCE_DIR} + RESULT_VARIABLE PATCH_RESULT + ) else() find_package(COLMAP REQUIRED) endif() From 233540b55e8c1dfeeda99ee2aeb0fa0ad026017e Mon Sep 17 00:00:00 2001 From: Yeicor <4929005+yeicor@users.noreply.github.com> Date: Sat, 26 Jul 2025 23:45:14 +0200 Subject: [PATCH 37/45] Delete broken pybind11 submodules? (#203) --- pybind11 | 1 - pyglomap/pybind11 | 1 - 2 files changed, 2 deletions(-) delete mode 160000 pybind11 delete mode 160000 pyglomap/pybind11 diff --git a/pybind11 b/pybind11 deleted file mode 160000 index 3ebdc503..00000000 --- a/pybind11 +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 3ebdc503d29c7f089b9a0bc1823add0dda76f40d diff --git a/pyglomap/pybind11 b/pyglomap/pybind11 deleted file mode 160000 index 3ebdc503..00000000 --- a/pyglomap/pybind11 +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 3ebdc503d29c7f089b9a0bc1823add0dda76f40d From a62ba80953fe7b59d4797202bcaf813884b0653a Mon Sep 17 00:00:00 2001 From: Linfei Pan <36349740+lpanaf@users.noreply.github.com> Date: Thu, 30 Oct 2025 02:52:30 -0700 Subject: [PATCH 38/45] Add rig support and upgrade colmap to version 3.12 (#201) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * rigged global positioning. Not debugged yet * compilation debugged * the rigged GP debugged * debug code * dbugging rig GP * rig rotation averaging implemented * minor * rigged bundle adjustment * rigged pipeline * largest connect componenet establishment with rigs * test entry point * remove unnecessary dependency * add support for relative pose estimation only * change the snapshot logic * minor * migrate to COLMAP 3..12 (with rig support) * refactor rotation averaging. RIGGED version not working yet * d * rigged rotation averaging debugged. The convergence is poor. * rigged global positioning * minor * add stratefied rotation averaging for initialization * rotation initializer * gravity refinement with rig support * unit test for mapper. Include additional non-trivial rig test * unit test for RA. Add test for rig support * add gravity aligned RA for rigs * move the gravity property to frames * add frame struct * pose io with gravity support * d * remove the old version * rename the rigged version back to normal * name back the rigged version to default * normalization with rig support * cleanup and add support for cam-to-cam constraints * add reconstruction normalization * cleanup * f * d * d * f * d * d * d * d * d * skip unconnected images * f * Update glomap/controllers/global_mapper.cc Co-authored-by: Johannes Schönberger * Update glomap/controllers/global_mapper.cc Co-authored-by: Johannes Schönberger * renaming * remove redundant files * temp cam_from_world * change version match to only consider the first two numbers * changed todo * renaming * refactor the code so the is_registered is a frame property * formulate the pruning with respect to frames * m * f * d * d * Update to latest colmap and poselib * Update glomap/estimators/cost_function.h Co-authored-by: Johannes Schönberger * Update glomap/estimators/gravity_refinement.cc Co-authored-by: Johannes Schönberger * Update glomap/estimators/gravity_refinement.cc Co-authored-by: Johannes Schönberger * address minor issues * clean up * f * d * cleanup * expose optimize_rig_poses in cli * fix the bug for the reconstruction output when there are more than 1 cluster * f * make pair_id consistent with colmap * fix the bug for ba with rig calibration * f * f * stablize the reconstruction by removing nearly degenerate points * d * d * d * d * d * d * d * d * d * d * d * d --------- Co-authored-by: Johannes Schönberger Co-authored-by: Johannes Schönberger --- .clang-tidy | 2 + .github/workflows/mac.yml | 3 +- .github/workflows/ubuntu.yml | 35 +- .gitignore | 1 + cmake/FindDependencies.cmake | 23 +- glomap/CMakeLists.txt | 7 +- glomap/controllers/global_mapper.cc | 81 ++-- glomap/controllers/global_mapper.h | 4 +- glomap/controllers/global_mapper_test.cc | 126 ++++- glomap/controllers/option_manager.cc | 2 + glomap/controllers/rotation_averager.cc | 177 ++++++- glomap/controllers/rotation_averager.h | 5 + glomap/controllers/rotation_averager_test.cc | 340 ++++++++++--- glomap/controllers/track_establishment.cc | 2 +- glomap/controllers/track_retriangulation.cc | 20 +- glomap/controllers/track_retriangulation.h | 2 + glomap/estimators/bundle_adjustment.cc | 150 ++++-- glomap/estimators/bundle_adjustment.h | 17 +- glomap/estimators/cost_function.h | 95 ++++ glomap/estimators/global_positioning.cc | 282 ++++++++--- glomap/estimators/global_positioning.h | 19 +- .../estimators/global_rotation_averaging.cc | 449 ++++++++++++++---- glomap/estimators/global_rotation_averaging.h | 32 +- glomap/estimators/gravity_refinement.cc | 118 +++-- glomap/estimators/gravity_refinement.h | 2 + glomap/estimators/relpose_estimation.cc | 6 +- glomap/estimators/rotation_initializer.cc | 126 +++++ glomap/estimators/rotation_initializer.h | 14 + glomap/exe/global_mapper.cc | 41 +- glomap/exe/rotation_averager.cc | 25 +- glomap/io/colmap_converter.cc | 215 +++++++-- glomap/io/colmap_converter.h | 20 +- glomap/io/colmap_io.cc | 14 +- glomap/io/colmap_io.h | 2 + glomap/io/pose_io.cc | 37 +- glomap/math/gravity.cc | 2 +- glomap/math/rigid3d.cc | 17 +- glomap/math/rigid3d.h | 3 + glomap/math/tree.cc | 4 +- glomap/processors/image_undistorter.cc | 5 +- .../processors/reconstruction_normalizer.cc | 22 +- glomap/processors/reconstruction_normalizer.h | 2 + glomap/processors/reconstruction_pruning.cc | 73 ++- glomap/processors/reconstruction_pruning.h | 3 +- glomap/processors/relpose_filter.cc | 4 +- glomap/processors/track_filter.cc | 9 +- glomap/processors/view_graph_manipulation.cc | 30 +- glomap/processors/view_graph_manipulation.h | 2 + glomap/scene/frame.h | 52 ++ glomap/scene/image.h | 100 ++-- glomap/scene/image_pair.h | 12 +- glomap/scene/types.h | 20 +- glomap/scene/types_sfm.h | 2 + glomap/scene/view_graph.cc | 63 ++- glomap/scene/view_graph.h | 15 +- scripts/format/c++.sh | 10 +- 56 files changed, 2337 insertions(+), 607 deletions(-) create mode 100644 glomap/estimators/rotation_initializer.cc create mode 100644 glomap/estimators/rotation_initializer.h create mode 100644 glomap/scene/frame.h diff --git a/.clang-tidy b/.clang-tidy index 50b93153..fd1ab1b0 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -2,12 +2,14 @@ Checks: > performance-*, concurrency-*, bugprone-*, + -clang-analyzer-security.ArrayBound, -bugprone-easily-swappable-parameters, -bugprone-exception-escape, -bugprone-implicit-widening-of-multiplication-result, -bugprone-narrowing-conversions, -bugprone-reserved-identifier, -bugprone-unchecked-optional-access, + -performance-enum-size, cppcoreguidelines-virtual-class-destructor, google-explicit-constructor, google-build-using-namespace, diff --git a/.github/workflows/mac.yml b/.github/workflows/mac.yml index 6cfdbeb0..3f786675 100644 --- a/.github/workflows/mac.yml +++ b/.github/workflows/mac.yml @@ -40,8 +40,8 @@ jobs: - name: Setup Mac run: | + brew upgrade cmake || brew install cmake brew install \ - cmake \ ninja \ boost \ eigen \ @@ -56,6 +56,7 @@ jobs: cgal \ sqlite3 \ ccache + brew link --force libomp - name: Configure and build run: | diff --git a/.github/workflows/ubuntu.yml b/.github/workflows/ubuntu.yml index f3c2afcc..0ebc1e14 100644 --- a/.github/workflows/ubuntu.yml +++ b/.github/workflows/ubuntu.yml @@ -11,13 +11,14 @@ on: jobs: build: - name: ${{ matrix.config.os }} ${{ matrix.config.cmakeBuildType }} ${{ matrix.config.cudaEnabled && 'CUDA' || '' }} ${{ matrix.config.asanEnabled && 'ASan' || '' }} + name: ${{ matrix.config.os }} ${{ matrix.config.cmakeBuildType }} ${{ matrix.config.cudaEnabled && 'CUDA' || '' }} ${{ matrix.config.asanEnabled && 'ASan' || '' }} ${{ matrix.config.coverageEnabled && 'Coverage' || '' }} runs-on: ${{ matrix.config.os }} strategy: matrix: config: [ { os: ubuntu-24.04, + qtVersion: 6, cmakeBuildType: RelWithDebInfo, asanEnabled: false, cudaEnabled: false, @@ -25,6 +26,7 @@ jobs: }, { os: ubuntu-22.04, + qtVersion: 6, cmakeBuildType: Release, asanEnabled: false, cudaEnabled: false, @@ -32,20 +34,23 @@ jobs: }, { os: ubuntu-22.04, + qtVersion: 5, cmakeBuildType: Release, asanEnabled: false, cudaEnabled: true, checkCodeFormat: false, }, { - os: ubuntu-22.04, + os: ubuntu-24.04, + qtVersion: 6, cmakeBuildType: Release, asanEnabled: true, cudaEnabled: false, checkCodeFormat: false, }, { - os: ubuntu-22.04, + os: ubuntu-24.04, + qtVersion: 6, cmakeBuildType: ClangTidy, asanEnabled: false, cudaEnabled: false, @@ -59,6 +64,8 @@ jobs: CCACHE_DIR: ${{ github.workspace }}/compiler-cache/ccache CCACHE_BASEDIR: ${{ github.workspace }} CTCACHE_DIR: ${{ github.workspace }}/compiler-cache/ctcache + GLOG_v: 2 + GLOG_logtostderr: 1 steps: - uses: actions/checkout@v4 @@ -100,6 +107,12 @@ jobs: - name: Setup Ubuntu run: | + if [ "${{ matrix.config.qtVersion }}" == "5" ]; then + qt_packages="qtbase5-dev libqt5opengl5-dev libcgal-qt5-dev" + elif [ "${{ matrix.config.qtVersion }}" == "6" ]; then + qt_packages="qt6-base-dev libqt6opengl6-dev libqt6openglwidgets6" + fi + sudo apt-get update && sudo apt-get install -y \ build-essential \ cmake \ @@ -117,7 +130,7 @@ jobs: libgmock-dev \ libsqlite3-dev \ libglew-dev \ - qtbase5-dev \ + $qt_packages \ libqt5opengl5-dev \ libcgal-dev \ libcgal-qt5-dev \ @@ -145,15 +158,15 @@ jobs: fi if [ "${{ matrix.config.asanEnabled }}" == "true" ]; then - sudo apt-get install -y clang-15 libomp-15-dev - echo "CC=/usr/bin/clang-15" >> $GITHUB_ENV - echo "CXX=/usr/bin/clang++-15" >> $GITHUB_ENV + sudo apt-get install -y clang-18 libomp-18-dev + echo "CC=/usr/bin/clang-18" >> $GITHUB_ENV + echo "CXX=/usr/bin/clang++-18" >> $GITHUB_ENV fi if [ "${{ matrix.config.cmakeBuildType }}" == "ClangTidy" ]; then - sudo apt-get install -y clang-15 clang-tidy-15 libomp-15-dev - echo "CC=/usr/bin/clang-15" >> $GITHUB_ENV - echo "CXX=/usr/bin/clang++-15" >> $GITHUB_ENV + sudo apt-get install -y clang-18 clang-tidy-18 libomp-18-dev + echo "CC=/usr/bin/clang-18" >> $GITHUB_ENV + echo "CXX=/usr/bin/clang++-18" >> $GITHUB_ENV fi - name: Upgrade CMake @@ -183,7 +196,7 @@ jobs: - name: Run tests if: ${{ matrix.config.cmakeBuildType != 'ClangTidy' }} - run: | + run: | export DISPLAY=":99.0" export QT_QPA_PLATFORM="offscreen" Xvfb :99 & diff --git a/.gitignore b/.gitignore index 52abcd91..5066fb1b 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,4 @@ /build /data /.vscode +/compile_commands.json diff --git a/cmake/FindDependencies.cmake b/cmake/FindDependencies.cmake index a38b6276..c1d0110e 100644 --- a/cmake/FindDependencies.cmake +++ b/cmake/FindDependencies.cmake @@ -7,6 +7,7 @@ if(NOT TARGET SuiteSparse::CHOLMOD) endif() find_package(Ceres REQUIRED COMPONENTS SuiteSparse) find_package(Boost REQUIRED) +find_package(OpenMP REQUIRED COMPONENTS C CXX) if(CMAKE_CXX_COMPILER_ID STREQUAL "MSVC") find_package(Glog REQUIRED) @@ -26,7 +27,7 @@ endif() include(FetchContent) FetchContent_Declare(PoseLib GIT_REPOSITORY https://github.com/PoseLib/PoseLib.git - GIT_TAG 0439b2d361125915b8821043fca9376e6cc575b9 + GIT_TAG 7e9f5f53372e43f89655040d4dfc4a00e5ace11c # 2.0.5 EXCLUDE_FROM_ALL SYSTEM ) @@ -40,30 +41,14 @@ message(STATUS "Configuring PoseLib... done") FetchContent_Declare(COLMAP GIT_REPOSITORY https://github.com/colmap/colmap.git - GIT_TAG 78f1eefacae542d753c2e4f6a26771a0d976227d + GIT_TAG c5f9cefc87e5dd596b638e4cee0ff543c7d14755 # Oct 23 2025 EXCLUDE_FROM_ALL ) message(STATUS "Configuring COLMAP...") set(UNINSTALL_ENABLED OFF CACHE INTERNAL "") +set(GUI_ENABLED OFF CACHE INTERNAL "") if (FETCH_COLMAP) FetchContent_MakeAvailable(COLMAP) - - # Define where to store the patch - set(COLMAP_PATCH_PATH ${CMAKE_BINARY_DIR}/fix_poisson.patch) - - # Download the patch from GitHub - file(DOWNLOAD - https://github.com/colmap/colmap/commit/a586e7cb223cc86c609105246ecd3a10e0c55131.patch - ${COLMAP_PATCH_PATH} - SHOW_PROGRESS - STATUS PATCH_DOWNLOAD_STATUS - ) - # Apply the patch - execute_process( - COMMAND git apply ${COLMAP_PATCH_PATH} - WORKING_DIRECTORY ${colmap_SOURCE_DIR} - RESULT_VARIABLE PATCH_RESULT - ) else() find_package(COLMAP REQUIRED) endif() diff --git a/glomap/CMakeLists.txt b/glomap/CMakeLists.txt index e191049a..145adc4d 100644 --- a/glomap/CMakeLists.txt +++ b/glomap/CMakeLists.txt @@ -9,6 +9,7 @@ set(SOURCES estimators/global_rotation_averaging.cc estimators/gravity_refinement.cc estimators/relpose_estimation.cc + estimators/rotation_initializer.cc estimators/view_graph_calibration.cc io/colmap_converter.cc io/colmap_io.cc @@ -38,8 +39,9 @@ set(HEADERS estimators/global_positioning.h estimators/global_rotation_averaging.h estimators/gravity_refinement.h - estimators/relpose_estimation.h estimators/optimization_base.h + estimators/relpose_estimation.h + estimators/rotation_initializer.h estimators/view_graph_calibration.h io/colmap_converter.h io/colmap_io.h @@ -58,6 +60,7 @@ set(HEADERS processors/track_filter.h processors/view_graph_manipulation.h scene/camera.h + scene/frame.h scene/image_pair.h scene/image.h scene/track.h @@ -84,6 +87,7 @@ target_link_libraries( Eigen3::Eigen Ceres::ceres SuiteSparse::CHOLMOD + OpenMP::OpenMP_CXX ${BOOST_LIBRARIES} ) target_include_directories(glomap PUBLIC ..) @@ -112,7 +116,6 @@ target_link_libraries(glomap_main glomap) set_target_properties(glomap_main PROPERTIES OUTPUT_NAME glomap) install(TARGETS glomap_main DESTINATION bin) - if(TESTS_ENABLED) add_executable(glomap_test controllers/global_mapper_test.cc diff --git a/glomap/controllers/global_mapper.cc b/glomap/controllers/global_mapper.cc index 6de88d92..f5af2bbc 100644 --- a/glomap/controllers/global_mapper.cc +++ b/glomap/controllers/global_mapper.cc @@ -1,5 +1,6 @@ #include "global_mapper.h" +#include "glomap/controllers/rotation_averager.h" #include "glomap/io/colmap_converter.h" #include "glomap/processors/image_pair_inliers.h" #include "glomap/processors/image_undistorter.h" @@ -14,9 +15,12 @@ namespace glomap { +// TODO: Rig normalizaiton has not be done bool GlobalMapper::Solve(const colmap::Database& database, ViewGraph& view_graph, + std::unordered_map& rigs, std::unordered_map& cameras, + std::unordered_map& frames, std::unordered_map& images, std::unordered_map& tracks) { // 0. Preprocessing @@ -46,6 +50,7 @@ bool GlobalMapper::Solve(const colmap::Database& database, } // 2. Run relative pose estimation + // TODO: Use generalized relative pose estimation for rigs. if (!options_.skip_relative_pose_estimation) { std::cout << "-------------------------------------" << std::endl; std::cout << "Running relative pose estimation ..." << std::endl; @@ -66,7 +71,7 @@ bool GlobalMapper::Solve(const colmap::Database& database, RelPoseFilter::FilterInlierRatio( view_graph, options_.inlier_thresholds.min_inlier_ratio); - if (view_graph.KeepLargestConnectedComponents(images) == 0) { + if (view_graph.KeepLargestConnectedComponents(frames, images) == 0) { LOG(ERROR) << "no connected components are found"; return false; } @@ -83,24 +88,24 @@ bool GlobalMapper::Solve(const colmap::Database& database, colmap::Timer run_timer; run_timer.Start(); - RotationEstimator ra_engine(options_.opt_ra); // The first run is for filtering - ra_engine.EstimateRotations(view_graph, images); + SolveRotationAveraging(view_graph, rigs, frames, images, options_.opt_ra); RelPoseFilter::FilterRotations( view_graph, images, options_.inlier_thresholds.max_rotation_error); - if (view_graph.KeepLargestConnectedComponents(images) == 0) { + if (view_graph.KeepLargestConnectedComponents(frames, images) == 0) { LOG(ERROR) << "no connected components are found"; return false; } // The second run is for final estimation - if (!ra_engine.EstimateRotations(view_graph, images)) { + if (!SolveRotationAveraging( + view_graph, rigs, frames, images, options_.opt_ra)) { return false; } RelPoseFilter::FilterRotations( view_graph, images, options_.inlier_thresholds.max_rotation_error); - image_t num_img = view_graph.KeepLargestConnectedComponents(images); + image_t num_img = view_graph.KeepLargestConnectedComponents(frames, images); if (num_img == 0) { LOG(ERROR) << "no connected components are found"; return false; @@ -137,6 +142,12 @@ bool GlobalMapper::Solve(const colmap::Database& database, std::cout << "Running global positioning ..." << std::endl; std::cout << "-------------------------------------" << std::endl; + if (options_.opt_gp.constraint_type != + GlobalPositionerOptions::ConstraintType::ONLY_POINTS) { + LOG(ERROR) << "Only points are used for solving camera positions"; + return false; + } + colmap::Timer run_timer; run_timer.Start(); // Undistort images in case all previous steps are skipped @@ -144,24 +155,11 @@ bool GlobalMapper::Solve(const colmap::Database& database, UndistortImages(cameras, images, false); GlobalPositioner gp_engine(options_.opt_gp); - if (!gp_engine.Solve(view_graph, cameras, images, tracks)) { - return false; - } - // If only camera-to-camera constraints are used for solving camera - // positions, then points needs to be estimated separately - if (options_.opt_gp.constraint_type == - GlobalPositionerOptions::ConstraintType::ONLY_CAMERAS) { - GlobalPositionerOptions opt_gp_pt = options_.opt_gp; - opt_gp_pt.constraint_type = - GlobalPositionerOptions::ConstraintType::ONLY_POINTS; - opt_gp_pt.optimize_positions = false; - GlobalPositioner gp_engine_pt(opt_gp_pt); - if (!gp_engine_pt.Solve(view_graph, cameras, images, tracks)) { - return false; - } + // TODO: consider to support other modes as well + if (!gp_engine.Solve(view_graph, rigs, cameras, frames, images, tracks)) { + return false; } - // Filter tracks based on the estimation TrackFilter::FilterTracksByAngle( view_graph, @@ -170,8 +168,22 @@ bool GlobalMapper::Solve(const colmap::Database& database, tracks, options_.inlier_thresholds.max_angle_error); + // Filter tracks based on triangulation angle and reprojection error + TrackFilter::FilterTrackTriangulationAngle( + view_graph, + images, + tracks, + options_.inlier_thresholds.min_triangulation_angle); + // Set the threshold to be larger to avoid removing too many tracks + TrackFilter::FilterTracksByReprojection( + view_graph, + cameras, + images, + tracks, + 10 * options_.inlier_thresholds.max_reprojection_error); // Normalize the structure - NormalizeReconstruction(cameras, images, tracks); + // If the camera rig is used, the structure do not need to be normalized + NormalizeReconstruction(rigs, cameras, frames, images, tracks); run_timer.PrintSeconds(); } @@ -194,7 +206,7 @@ bool GlobalMapper::Solve(const colmap::Database& database, // Staged bundle adjustment // 6.1. First stage: optimize positions only ba_engine_options_inner.optimize_rotations = false; - if (!ba_engine.Solve(view_graph, cameras, images, tracks)) { + if (!ba_engine.Solve(rigs, cameras, frames, images, tracks)) { return false; } LOG(INFO) << "Global bundle adjustment iteration " << ite + 1 << " / " @@ -206,7 +218,7 @@ bool GlobalMapper::Solve(const colmap::Database& database, ba_engine_options_inner.optimize_rotations = options_.opt_ba.optimize_rotations; if (ba_engine_options_inner.optimize_rotations && - !ba_engine.Solve(view_graph, cameras, images, tracks)) { + !ba_engine.Solve(rigs, cameras, frames, images, tracks)) { return false; } LOG(INFO) << "Global bundle adjustment iteration " << ite + 1 << " / " @@ -216,7 +228,7 @@ bool GlobalMapper::Solve(const colmap::Database& database, run_timer.PrintSeconds(); // Normalize the structure - NormalizeReconstruction(cameras, images, tracks); + NormalizeReconstruction(rigs, cameras, frames, images, tracks); // 6.3. Filter tracks based on the estimation // For the filtering, in each round, the criteria for outlier is @@ -273,8 +285,13 @@ bool GlobalMapper::Solve(const colmap::Database& database, for (int ite = 0; ite < options_.num_iteration_retriangulation; ite++) { colmap::Timer run_timer; run_timer.Start(); - RetriangulateTracks( - options_.opt_triangulator, database, cameras, images, tracks); + RetriangulateTracks(options_.opt_triangulator, + database, + rigs, + cameras, + frames, + images, + tracks); run_timer.PrintSeconds(); std::cout << "-------------------------------------" << std::endl; @@ -282,7 +299,7 @@ bool GlobalMapper::Solve(const colmap::Database& database, std::cout << "-------------------------------------" << std::endl; LOG(INFO) << "Bundle adjustment start" << std::endl; BundleAdjuster ba_engine(options_.opt_ba); - if (!ba_engine.Solve(view_graph, cameras, images, tracks)) { + if (!ba_engine.Solve(rigs, cameras, frames, images, tracks)) { return false; } @@ -295,14 +312,14 @@ bool GlobalMapper::Solve(const colmap::Database& database, images, tracks, options_.inlier_thresholds.max_reprojection_error); - if (!ba_engine.Solve(view_graph, cameras, images, tracks)) { + if (!ba_engine.Solve(rigs, cameras, frames, images, tracks)) { return false; } run_timer.PrintSeconds(); } // Normalize the structure - NormalizeReconstruction(cameras, images, tracks); + NormalizeReconstruction(rigs, cameras, frames, images, tracks); // Filter tracks based on the estimation UndistortImages(cameras, images, true); @@ -330,7 +347,7 @@ bool GlobalMapper::Solve(const colmap::Database& database, run_timer.Start(); // Prune weakly connected images - PruneWeaklyConnectedImages(images, tracks); + PruneWeaklyConnectedImages(frames, images, tracks); run_timer.PrintSeconds(); } diff --git a/glomap/controllers/global_mapper.h b/glomap/controllers/global_mapper.h index 79c1779f..4f4c3443 100644 --- a/glomap/controllers/global_mapper.h +++ b/glomap/controllers/global_mapper.h @@ -1,5 +1,4 @@ #pragma once - #include "glomap/controllers/track_establishment.h" #include "glomap/controllers/track_retriangulation.h" #include "glomap/estimators/bundle_adjustment.h" @@ -42,13 +41,16 @@ struct GlobalMapperOptions { bool skip_pruning = true; }; +// TODO: Refactor the code to reuse the pipeline code more class GlobalMapper { public: GlobalMapper(const GlobalMapperOptions& options) : options_(options) {} bool Solve(const colmap::Database& database, ViewGraph& view_graph, + std::unordered_map& rigs, std::unordered_map& cameras, + std::unordered_map& frames, std::unordered_map& images, std::unordered_map& tracks); diff --git a/glomap/controllers/global_mapper_test.cc b/glomap/controllers/global_mapper_test.cc index b1a40628..dee5cada 100644 --- a/glomap/controllers/global_mapper_test.cc +++ b/glomap/controllers/global_mapper_test.cc @@ -53,28 +53,122 @@ GlobalMapperOptions CreateTestOptions() { TEST(GlobalMapper, WithoutNoise) { const std::string database_path = colmap::CreateTestDir() + "/database.db"; - colmap::Database database(database_path); + auto database = colmap::Database::Open(database_path); colmap::Reconstruction gt_reconstruction; colmap::SyntheticDatasetOptions synthetic_dataset_options; - synthetic_dataset_options.num_cameras = 2; - synthetic_dataset_options.num_images = 7; + synthetic_dataset_options.num_rigs = 2; + synthetic_dataset_options.num_cameras_per_rig = 1; + synthetic_dataset_options.num_frames_per_rig = 7; synthetic_dataset_options.num_points3D = 50; synthetic_dataset_options.point2D_stddev = 0; colmap::SynthesizeDataset( - synthetic_dataset_options, >_reconstruction, &database); + synthetic_dataset_options, >_reconstruction, database.get()); ViewGraph view_graph; + std::unordered_map rigs; std::unordered_map cameras; + std::unordered_map frames; std::unordered_map images; std::unordered_map tracks; - ConvertDatabaseToGlomap(database, view_graph, cameras, images); + ConvertDatabaseToGlomap(*database, view_graph, rigs, cameras, frames, images); GlobalMapper global_mapper(CreateTestOptions()); - global_mapper.Solve(database, view_graph, cameras, images, tracks); + global_mapper.Solve( + *database, view_graph, rigs, cameras, frames, images, tracks); colmap::Reconstruction reconstruction; - ConvertGlomapToColmap(cameras, images, tracks, reconstruction); + ConvertGlomapToColmap(rigs, cameras, frames, images, tracks, reconstruction); + + ExpectEqualReconstructions(gt_reconstruction, + reconstruction, + /*max_rotation_error_deg=*/1e-2, + /*max_proj_center_error=*/1e-4, + /*num_obs_tolerance=*/0); +} + +TEST(GlobalMapper, WithoutNoiseWithNonTrivialKnownRig) { + const std::string database_path = colmap::CreateTestDir() + "/database.db"; + + auto database = colmap::Database::Open(database_path); + colmap::Reconstruction gt_reconstruction; + colmap::SyntheticDatasetOptions synthetic_dataset_options; + synthetic_dataset_options.num_rigs = 2; + synthetic_dataset_options.num_cameras_per_rig = 2; + synthetic_dataset_options.num_frames_per_rig = 7; + synthetic_dataset_options.num_points3D = 50; + synthetic_dataset_options.point2D_stddev = 0; + synthetic_dataset_options.sensor_from_rig_translation_stddev = + 0.1; // No noise + synthetic_dataset_options.sensor_from_rig_rotation_stddev = 5.; // No noise + colmap::SynthesizeDataset( + synthetic_dataset_options, >_reconstruction, database.get()); + + ViewGraph view_graph; + std::unordered_map rigs; + std::unordered_map cameras; + std::unordered_map frames; + std::unordered_map images; + std::unordered_map tracks; + + ConvertDatabaseToGlomap(*database, view_graph, rigs, cameras, frames, images); + + GlobalMapper global_mapper(CreateTestOptions()); + global_mapper.Solve( + *database, view_graph, rigs, cameras, frames, images, tracks); + + colmap::Reconstruction reconstruction; + ConvertGlomapToColmap(rigs, cameras, frames, images, tracks, reconstruction); + + ExpectEqualReconstructions(gt_reconstruction, + reconstruction, + /*max_rotation_error_deg=*/1e-2, + /*max_proj_center_error=*/1e-4, + /*num_obs_tolerance=*/0); +} + +TEST(GlobalMapper, WithoutNoiseWithNonTrivialUnknownRig) { + const std::string database_path = colmap::CreateTestDir() + "/database.db"; + + auto database = colmap::Database::Open(database_path); + colmap::Reconstruction gt_reconstruction; + colmap::SyntheticDatasetOptions synthetic_dataset_options; + synthetic_dataset_options.num_rigs = 2; + synthetic_dataset_options.num_cameras_per_rig = 3; + synthetic_dataset_options.num_frames_per_rig = 7; + synthetic_dataset_options.num_points3D = 50; + synthetic_dataset_options.point2D_stddev = 0; + synthetic_dataset_options.sensor_from_rig_translation_stddev = + 0.1; // No noise + synthetic_dataset_options.sensor_from_rig_rotation_stddev = 5.; // No noise + + colmap::SynthesizeDataset( + synthetic_dataset_options, >_reconstruction, database.get()); + + ViewGraph view_graph; + std::unordered_map rigs; + std::unordered_map cameras; + std::unordered_map frames; + std::unordered_map images; + std::unordered_map tracks; + + ConvertDatabaseToGlomap(*database, view_graph, rigs, cameras, frames, images); + + // Set the rig sensors to be unknown + for (auto& [rig_id, rig] : rigs) { + for (auto& [sensor_id, sensor] : rig.NonRefSensors()) { + if (sensor.has_value()) { + rig.ResetSensorFromRig(sensor_id); + } + } + } + + GlobalMapper global_mapper(CreateTestOptions()); + global_mapper.Solve( + *database, view_graph, rigs, cameras, frames, images, tracks); + + colmap::Reconstruction reconstruction; + ConvertGlomapToColmap(rigs, cameras, frames, images, tracks, reconstruction); ExpectEqualReconstructions(gt_reconstruction, reconstruction, @@ -86,29 +180,33 @@ TEST(GlobalMapper, WithoutNoise) { TEST(GlobalMapper, WithNoiseAndOutliers) { const std::string database_path = colmap::CreateTestDir() + "/database.db"; - colmap::Database database(database_path); + auto database = colmap::Database::Open(database_path); colmap::Reconstruction gt_reconstruction; colmap::SyntheticDatasetOptions synthetic_dataset_options; - synthetic_dataset_options.num_cameras = 2; - synthetic_dataset_options.num_images = 7; + synthetic_dataset_options.num_rigs = 2; + synthetic_dataset_options.num_cameras_per_rig = 1; + synthetic_dataset_options.num_frames_per_rig = 4; synthetic_dataset_options.num_points3D = 100; synthetic_dataset_options.point2D_stddev = 0.5; synthetic_dataset_options.inlier_match_ratio = 0.6; colmap::SynthesizeDataset( - synthetic_dataset_options, >_reconstruction, &database); + synthetic_dataset_options, >_reconstruction, database.get()); ViewGraph view_graph; std::unordered_map cameras; + std::unordered_map rigs; std::unordered_map images; + std::unordered_map frames; std::unordered_map tracks; - ConvertDatabaseToGlomap(database, view_graph, cameras, images); + ConvertDatabaseToGlomap(*database, view_graph, rigs, cameras, frames, images); GlobalMapper global_mapper(CreateTestOptions()); - global_mapper.Solve(database, view_graph, cameras, images, tracks); + global_mapper.Solve( + *database, view_graph, rigs, cameras, frames, images, tracks); colmap::Reconstruction reconstruction; - ConvertGlomapToColmap(cameras, images, tracks, reconstruction); + ConvertGlomapToColmap(rigs, cameras, frames, images, tracks, reconstruction); ExpectEqualReconstructions(gt_reconstruction, reconstruction, diff --git a/glomap/controllers/option_manager.cc b/glomap/controllers/option_manager.cc index 3a7c8ff4..6035cbfa 100644 --- a/glomap/controllers/option_manager.cc +++ b/glomap/controllers/option_manager.cc @@ -209,6 +209,8 @@ void OptionManager::AddBundleAdjusterOptions() { &mapper->opt_ba.use_gpu); AddAndRegisterDefaultOption("BundleAdjustment.gpu_index", &mapper->opt_ba.gpu_index); + AddAndRegisterDefaultOption("BundleAdjustment.optimize_rig_poses", + &mapper->opt_ba.optimize_rig_poses); AddAndRegisterDefaultOption("BundleAdjustment.optimize_rotations", &mapper->opt_ba.optimize_rotations); AddAndRegisterDefaultOption("BundleAdjustment.optimize_translation", diff --git a/glomap/controllers/rotation_averager.cc b/glomap/controllers/rotation_averager.cc index 09039e84..287e45dd 100644 --- a/glomap/controllers/rotation_averager.cc +++ b/glomap/controllers/rotation_averager.cc @@ -1,61 +1,200 @@ #include "glomap/controllers/rotation_averager.h" +#include "glomap/estimators/rotation_initializer.h" +#include "glomap/io/colmap_converter.h" + namespace glomap { bool SolveRotationAveraging(ViewGraph& view_graph, + std::unordered_map& rigs, + std::unordered_map& frames, std::unordered_map& images, const RotationAveragerOptions& options) { - view_graph.KeepLargestConnectedComponents(images); + view_graph.KeepLargestConnectedComponents(frames, images); bool solve_1dof_system = options.use_gravity && options.use_stratified; ViewGraph view_graph_grav; image_pair_t total_pairs = 0; - image_pair_t grav_pairs = 0; if (solve_1dof_system) { // Prepare two sets: ones all with gravity, and one does not have gravity. // Solve them separately first, then solve them in a single system for (const auto& [pair_id, image_pair] : view_graph.image_pairs) { if (!image_pair.is_valid) continue; - image_t image_id1 = image_pair.image_id1; - image_t image_id2 = image_pair.image_id2; - - Image& image1 = images[image_id1]; - Image& image2 = images[image_id2]; + const Image& image1 = images[image_pair.image_id1]; + const Image& image2 = images[image_pair.image_id2]; - if (!image1.is_registered || !image2.is_registered) continue; + if (!image1.IsRegistered() || !image2.IsRegistered()) continue; total_pairs++; - if (image1.gravity_info.has_gravity && image2.gravity_info.has_gravity) { + if (image1.HasGravity() && image2.HasGravity()) { view_graph_grav.image_pairs.emplace( pair_id, - ImagePair(image_id1, image_id2, image_pair.cam2_from_cam1)); - grav_pairs++; + ImagePair(image_pair.image_id1, + image_pair.image_id2, + image_pair.cam2_from_cam1)); } } } + const size_t grav_pairs = view_graph_grav.image_pairs.size(); + + LOG(INFO) << "Total image pairs: " << total_pairs + << ", gravity image pairs: " << grav_pairs; + // If there is no image pairs with gravity or most image pairs are with // gravity, then just run the 3dof version - bool status = (grav_pairs == 0) || (grav_pairs > total_pairs * 0.95); - solve_1dof_system = solve_1dof_system && (!status); + const bool status = grav_pairs == 0 || grav_pairs > total_pairs * 0.95; + solve_1dof_system = solve_1dof_system && !status; if (solve_1dof_system) { // Run the 1dof optimization LOG(INFO) << "Solving subset 1DoF rotation averaging problem in the mixed " "prior system"; - int num_img_grv = view_graph_grav.KeepLargestConnectedComponents(images); + view_graph_grav.KeepLargestConnectedComponents(frames, images); RotationEstimator rotation_estimator_grav(options); - if (!rotation_estimator_grav.EstimateRotations(view_graph_grav, images)) { + if (!rotation_estimator_grav.EstimateRotations( + view_graph_grav, rigs, frames, images)) { return false; } - view_graph.KeepLargestConnectedComponents(images); + view_graph.KeepLargestConnectedComponents(frames, images); + } + + // By default, run trivial rotation averaging for cameras with unknown + // cam_from_rig. + std::unordered_set unknown_cams_from_rig; + rig_t max_rig_id = 0; + for (const auto& [rig_id, rig] : rigs) { + max_rig_id = std::max(max_rig_id, rig_id); + for (const auto& [sensor_id, sensor] : rig.NonRefSensors()) { + if (sensor_id.type != SensorType::CAMERA) continue; + if (!rig.MaybeSensorFromRig(sensor_id).has_value()) { + unknown_cams_from_rig.insert(sensor_id.id); + } + } } - RotationEstimator rotation_estimator(options); - return rotation_estimator.EstimateRotations(view_graph, images); + bool status_ra = false; + // If the trivial rotation averaging is enabled, run it + if (!unknown_cams_from_rig.empty() && !options.skip_initialization) { + LOG(INFO) << "Running trivial rotation averaging for rigged cameras"; + // Create a rig for each camera + std::unordered_map rigs_trivial; + std::unordered_map frames_trivial; + std::unordered_map images_trivial; + + // For cameras with known cam_from_rig, create rigs with only those sensors. + std::unordered_map camera_id_to_rig_id; + for (const auto& [rig_id, rig] : rigs) { + Rig rig_trivial; + rig_trivial.SetRigId(rig_id); + rig_trivial.AddRefSensor(rig.RefSensorId()); + camera_id_to_rig_id[rig.RefSensorId().id] = rig_id; + + for (const auto& [sensor_id, sensor] : rig.NonRefSensors()) { + if (sensor_id.type != SensorType::CAMERA) continue; + if (rig.MaybeSensorFromRig(sensor_id).has_value()) { + rig_trivial.AddSensor(sensor_id, sensor); + camera_id_to_rig_id[sensor_id.id] = rig_id; + } + } + rigs_trivial[rig_trivial.RigId()] = rig_trivial; + } + + // For each camera with unknown cam_from_rig, create a separate trivial rig. + for (const auto& camera_id : unknown_cams_from_rig) { + Rig rig_trivial; + rig_trivial.SetRigId(++max_rig_id); + rig_trivial.AddRefSensor(sensor_t(SensorType::CAMERA, camera_id)); + rigs_trivial[rig_trivial.RigId()] = rig_trivial; + camera_id_to_rig_id[camera_id] = rig_trivial.RigId(); + } + + frame_t max_frame_id = 0; + for (const auto& [frame_id, _] : frames) { + THROW_CHECK_NE(frame_id, colmap::kInvalidFrameId); + max_frame_id = std::max(max_frame_id, frame_id); + } + max_frame_id++; + + for (auto& [frame_id, frame] : frames) { + Frame frame_trivial = Frame(); + frame_trivial.SetFrameId(frame_id); + frame_trivial.SetRigId(frame.RigId()); + frame_trivial.SetRigPtr(rigs_trivial.find(frame.RigId()) != + rigs_trivial.end() + ? &rigs_trivial[frame.RigId()] + : nullptr); + frames_trivial[frame_id] = frame_trivial; + + for (const auto& data_id : frame.ImageIds()) { + const auto& image = images.at(data_id.id); + if (!image.IsRegistered()) continue; + auto& image_trivial = + images_trivial + .emplace(data_id.id, + Image(data_id.id, image.camera_id, image.file_name)) + .first->second; + + if (unknown_cams_from_rig.find(image_trivial.camera_id) == + unknown_cams_from_rig.end()) { + frames_trivial[frame_id].AddDataId(image_trivial.DataId()); + image_trivial.frame_id = frame_id; + image_trivial.frame_ptr = &frames_trivial[frame_id]; + } else { + // If the camera is not in any rig, then create a trivial frame + // for it + CreateFrameForImage(Rigid3d(), + image_trivial, + rigs_trivial, + frames_trivial, + camera_id_to_rig_id[image.camera_id], + max_frame_id); + max_frame_id++; + } + } + } + + view_graph.KeepLargestConnectedComponents(frames_trivial, images_trivial); + // Run the trivial rotation averaging + RotationEstimatorOptions options_trivial = options; + options_trivial.skip_initialization = options.skip_initialization; + RotationEstimator rotation_estimator_trivial(options_trivial); + rotation_estimator_trivial.EstimateRotations( + view_graph, rigs_trivial, frames_trivial, images_trivial); + + // Collect the results + std::unordered_map cams_from_world; + for (const auto& [image_id, image] : images_trivial) { + if (!image.IsRegistered()) continue; + cams_from_world[image_id] = image.CamFromWorld(); + } + + ConvertRotationsFromImageToRig(cams_from_world, images, rigs, frames); + + RotationEstimatorOptions options_ra = options; + options_ra.skip_initialization = true; + RotationEstimator rotation_estimator(options_ra); + status_ra = + rotation_estimator.EstimateRotations(view_graph, rigs, frames, images); + view_graph.KeepLargestConnectedComponents(frames, images); + } else { + RotationAveragerOptions options_ra = options; + // For cases where there are some cameras without known cam_from_rig + // transformation, we need to run the rotation averaging with the + // skip_initialization flag set to false for convergence + if (unknown_cams_from_rig.size() > 0) { + options_ra.skip_initialization = false; + } + + RotationEstimator rotation_estimator(options_ra); + status_ra = + rotation_estimator.EstimateRotations(view_graph, rigs, frames, images); + view_graph.KeepLargestConnectedComponents(frames, images); + } + return status_ra; } -} // namespace glomap \ No newline at end of file +} // namespace glomap diff --git a/glomap/controllers/rotation_averager.h b/glomap/controllers/rotation_averager.h index cdf73893..1ba93102 100644 --- a/glomap/controllers/rotation_averager.h +++ b/glomap/controllers/rotation_averager.h @@ -5,10 +5,15 @@ namespace glomap { struct RotationAveragerOptions : public RotationEstimatorOptions { + RotationAveragerOptions() = default; + RotationAveragerOptions(const RotationEstimatorOptions& options) + : RotationEstimatorOptions(options) {} bool use_stratified = true; }; bool SolveRotationAveraging(ViewGraph& view_graph, + std::unordered_map& rigs, + std::unordered_map& frames, std::unordered_map& images, const RotationAveragerOptions& options); diff --git a/glomap/controllers/rotation_averager_test.cc b/glomap/controllers/rotation_averager_test.cc index 4da6b98c..095ff437 100644 --- a/glomap/controllers/rotation_averager_test.cc +++ b/glomap/controllers/rotation_averager_test.cc @@ -7,6 +7,7 @@ #include "glomap/types.h" #include +#include #include #include @@ -20,8 +21,8 @@ void CreateRandomRotation(const double stddev, Eigen::Quaterniond& q) { std::mt19937 gen{rd()}; // Construct a random axis - double theta = double(rand()) / RAND_MAX * 2 * M_PI; - double phi = double(rand()) / RAND_MAX * M_PI; + double theta = colmap::RandomUniformReal(0, 2 * M_PI); + double phi = colmap::RandomUniformReal(0, M_PI); Eigen::Vector3d axis(std::cos(theta) * std::sin(phi), std::sin(theta) * std::sin(phi), std::cos(phi)); @@ -33,26 +34,31 @@ void CreateRandomRotation(const double stddev, Eigen::Quaterniond& q) { } void PrepareGravity(const colmap::Reconstruction& gt, - std::unordered_map& images, - double stddev_gravity = 0.0, + std::unordered_map& frames, + double gravity_noise_stddev = 0.0, double outlier_ratio = 0.0) { - for (auto& image_id : gt.RegImageIds()) { - Eigen::Vector3d gravity = - gt.Image(image_id).CamFromWorld().rotation * Eigen::Vector3d(0, 1, 0); - - if (stddev_gravity > 0.0) { - Eigen::Quaterniond q; - CreateRandomRotation(DegToRad(stddev_gravity), q); - gravity = q * gravity; + const Eigen::Vector3d kGravityInWorld = Eigen::Vector3d(0, 1, 0); + for (auto& frame_id : gt.RegFrameIds()) { + Eigen::Vector3d gravityInRig = + gt.Frame(frame_id).RigFromWorld().rotation * kGravityInWorld; + + if (gravity_noise_stddev > 0.0) { + Eigen::Quaterniond noise; + CreateRandomRotation(DegToRad(gravity_noise_stddev), noise); + gravityInRig = noise * gravityInRig; } - if (outlier_ratio > 0.0 && double(rand()) / RAND_MAX < outlier_ratio) { + if (outlier_ratio > 0.0 && + colmap::RandomUniformReal(0, 1) < outlier_ratio) { Eigen::Quaterniond q; CreateRandomRotation(1., q); - gravity = + gravityInRig = Rigid3dToAngleAxis(Rigid3d(q, Eigen::Vector3d::Zero())).normalized(); } - images[image_id].gravity_info.SetGravity(gravity); + + frames[frame_id].gravity_info.SetGravity(gravityInRig); + Rigid3d& cam_from_world = frames[frame_id].RigFromWorld(); + cam_from_world.rotation = frames[frame_id].gravity_info.GetRAlign(); } } @@ -65,38 +71,35 @@ GlobalMapperOptions CreateMapperTestOptions() { options.skip_global_positioning = true; options.skip_bundle_adjustment = true; options.skip_retriangulation = true; - return options; } RotationAveragerOptions CreateRATestOptions(bool use_gravity = false) { RotationAveragerOptions options; - options.skip_initialization = true; + options.skip_initialization = false; options.use_gravity = use_gravity; + options.use_stratified = true; return options; } void ExpectEqualRotations(const colmap::Reconstruction& gt, const colmap::Reconstruction& computed, const double max_rotation_error_deg) { - const std::set reg_image_ids_set = gt.RegImageIds(); - std::vector reg_image_ids(reg_image_ids_set.begin(), - reg_image_ids_set.end()); + const std::vector reg_image_ids = gt.RegImageIds(); for (size_t i = 0; i < reg_image_ids.size(); i++) { const image_t image_id1 = reg_image_ids[i]; - for (size_t j = 0; j < reg_image_ids.size(); j++) { - if (i == j) continue; + for (size_t j = 0; j < i; j++) { const image_t image_id2 = reg_image_ids[j]; const Rigid3d cam2_from_cam1 = computed.Image(image_id2).CamFromWorld() * - colmap::Inverse(computed.Image(image_id1).CamFromWorld()); - + Inverse(computed.Image(image_id1).CamFromWorld()); const Rigid3d cam2_from_cam1_gt = gt.Image(image_id2).CamFromWorld() * - colmap::Inverse(gt.Image(image_id1).CamFromWorld()); + Inverse(gt.Image(image_id1).CamFromWorld()); - double rotation_error_deg = CalcAngle(cam2_from_cam1_gt, cam2_from_cam1); + const double rotation_error_deg = + CalcAngle(cam2_from_cam1_gt, cam2_from_cam1); EXPECT_LT(rotation_error_deg, max_rotation_error_deg); } } @@ -107,10 +110,13 @@ void ExpectEqualGravity( const std::unordered_map& images_computed, const double max_gravity_error_deg) { for (const auto& image_id : gt.RegImageIds()) { + if (!images_computed.at(image_id).HasTrivialFrame()) { + continue; // Skip images that are not trivial frames + } const Eigen::Vector3d gravity_gt = gt.Image(image_id).CamFromWorld().rotation * Eigen::Vector3d(0, 1, 0); const Eigen::Vector3d gravity_computed = - images_computed.at(image_id).gravity_info.GetGravity(); + images_computed.at(image_id).frame_ptr->gravity_info.GetGravity(); double gravity_error_deg = CalcAngle(gravity_gt, gravity_computed); EXPECT_LT(gravity_error_deg, max_gravity_error_deg); @@ -118,76 +124,233 @@ void ExpectEqualGravity( } TEST(RotationEstimator, WithoutNoise) { + colmap::SetPRNGSeed(1); + + const std::string database_path = colmap::CreateTestDir() + "/database.db"; + + auto database = colmap::Database::Open(database_path); + colmap::Reconstruction gt_reconstruction; + colmap::SyntheticDatasetOptions synthetic_dataset_options; + synthetic_dataset_options.num_rigs = 1; + synthetic_dataset_options.num_cameras_per_rig = 1; + synthetic_dataset_options.num_frames_per_rig = 5; + synthetic_dataset_options.num_points3D = 50; + synthetic_dataset_options.point2D_stddev = 0; + synthetic_dataset_options.sensor_from_rig_rotation_stddev = 20.; + colmap::SynthesizeDataset( + synthetic_dataset_options, >_reconstruction, database.get()); + + ViewGraph view_graph; + std::unordered_map rigs; + std::unordered_map cameras; + std::unordered_map frames; + std::unordered_map images; + std::unordered_map tracks; + + ConvertDatabaseToGlomap(*database, view_graph, rigs, cameras, frames, images); + + PrepareGravity(gt_reconstruction, frames); + + GlobalMapper global_mapper(CreateMapperTestOptions()); + global_mapper.Solve( + *database, view_graph, rigs, cameras, frames, images, tracks); + + // TODO: The current 1-dof rotation averaging sometimes fails to pick the + // right solution (e.g., 180 deg flipped). + for (const bool use_gravity : {false}) { + SolveRotationAveraging( + view_graph, rigs, frames, images, CreateRATestOptions(use_gravity)); + + colmap::Reconstruction reconstruction; + ConvertGlomapToColmap( + rigs, cameras, frames, images, tracks, reconstruction); + ExpectEqualRotations( + gt_reconstruction, reconstruction, /*max_rotation_error_deg=*/1e-2); + } +} + +TEST(RotationEstimator, WithoutNoiseWithNoneTrivialKnownRig) { + colmap::SetPRNGSeed(1); + + const std::string database_path = colmap::CreateTestDir() + "/database.db"; + + auto database = colmap::Database::Open(database_path); + colmap::Reconstruction gt_reconstruction; + colmap::SyntheticDatasetOptions synthetic_dataset_options; + synthetic_dataset_options.num_rigs = 1; + synthetic_dataset_options.num_cameras_per_rig = 2; + synthetic_dataset_options.num_frames_per_rig = 4; + synthetic_dataset_options.num_points3D = 50; + synthetic_dataset_options.point2D_stddev = 0; + synthetic_dataset_options.sensor_from_rig_rotation_stddev = 20.; + colmap::SynthesizeDataset( + synthetic_dataset_options, >_reconstruction, database.get()); + + ViewGraph view_graph; + std::unordered_map rigs; + std::unordered_map cameras; + std::unordered_map frames; + std::unordered_map images; + std::unordered_map tracks; + + ConvertDatabaseToGlomap(*database, view_graph, rigs, cameras, frames, images); + + PrepareGravity(gt_reconstruction, frames); + + GlobalMapper global_mapper(CreateMapperTestOptions()); + global_mapper.Solve( + *database, view_graph, rigs, cameras, frames, images, tracks); + + for (const bool use_gravity : {true, false}) { + SolveRotationAveraging( + view_graph, rigs, frames, images, CreateRATestOptions(use_gravity)); + + colmap::Reconstruction reconstruction; + ConvertGlomapToColmap( + rigs, cameras, frames, images, tracks, reconstruction); + ExpectEqualRotations( + gt_reconstruction, reconstruction, /*max_rotation_error_deg=*/1e-2); + } +} + +TEST(RotationEstimator, WithoutNoiseWithNoneTrivialUnknownRig) { + colmap::SetPRNGSeed(1); + const std::string database_path = colmap::CreateTestDir() + "/database.db"; - colmap::Database database(database_path); + auto database = colmap::Database::Open(database_path); colmap::Reconstruction gt_reconstruction; colmap::SyntheticDatasetOptions synthetic_dataset_options; - synthetic_dataset_options.num_cameras = 2; - synthetic_dataset_options.num_images = 9; + synthetic_dataset_options.num_rigs = 1; + synthetic_dataset_options.num_cameras_per_rig = 2; + synthetic_dataset_options.num_frames_per_rig = 4; synthetic_dataset_options.num_points3D = 50; synthetic_dataset_options.point2D_stddev = 0; + synthetic_dataset_options.sensor_from_rig_rotation_stddev = 20.; colmap::SynthesizeDataset( - synthetic_dataset_options, >_reconstruction, &database); + synthetic_dataset_options, >_reconstruction, database.get()); ViewGraph view_graph; + std::unordered_map rigs; std::unordered_map cameras; + std::unordered_map frames; std::unordered_map images; std::unordered_map tracks; - ConvertDatabaseToGlomap(database, view_graph, cameras, images); + ConvertDatabaseToGlomap(*database, view_graph, rigs, cameras, frames, images); - // PrepareRelativeRotations(view_graph, images); - PrepareGravity(gt_reconstruction, images); + for (auto& [rig_id, rig] : rigs) { + for (auto& [sensor_id, sensor] : rig.NonRefSensors()) { + if (sensor.has_value()) { + rig.ResetSensorFromRig(sensor_id); + } + } + } + PrepareGravity(gt_reconstruction, frames); GlobalMapper global_mapper(CreateMapperTestOptions()); - global_mapper.Solve(database, view_graph, cameras, images, tracks); + global_mapper.Solve( + *database, view_graph, rigs, cameras, frames, images, tracks); - // Version with Gravity - for (bool use_gravity : {true, false}) { + // For unknown rigs, it is not supported to use gravity. + for (const bool use_gravity : {false}) { SolveRotationAveraging( - view_graph, images, CreateRATestOptions(use_gravity)); + view_graph, rigs, frames, images, CreateRATestOptions(use_gravity)); colmap::Reconstruction reconstruction; - ConvertGlomapToColmap(cameras, images, tracks, reconstruction); + ConvertGlomapToColmap( + rigs, cameras, frames, images, tracks, reconstruction); ExpectEqualRotations( gt_reconstruction, reconstruction, /*max_rotation_error_deg=*/1e-2); } } TEST(RotationEstimator, WithNoiseAndOutliers) { + colmap::SetPRNGSeed(1); + const std::string database_path = colmap::CreateTestDir() + "/database.db"; - // FLAGS_v = 1; - colmap::Database database(database_path); + auto database = colmap::Database::Open(database_path); colmap::Reconstruction gt_reconstruction; colmap::SyntheticDatasetOptions synthetic_dataset_options; - synthetic_dataset_options.num_cameras = 2; - synthetic_dataset_options.num_images = 7; + synthetic_dataset_options.num_rigs = 2; + synthetic_dataset_options.num_cameras_per_rig = 1; + synthetic_dataset_options.num_frames_per_rig = 7; synthetic_dataset_options.num_points3D = 100; synthetic_dataset_options.point2D_stddev = 1; synthetic_dataset_options.inlier_match_ratio = 0.6; colmap::SynthesizeDataset( - synthetic_dataset_options, >_reconstruction, &database); + synthetic_dataset_options, >_reconstruction, database.get()); ViewGraph view_graph; + std::unordered_map rigs; std::unordered_map cameras; std::unordered_map images; + std::unordered_map frames; std::unordered_map tracks; - ConvertDatabaseToGlomap(database, view_graph, cameras, images); + ConvertDatabaseToGlomap(*database, view_graph, rigs, cameras, frames, images); - PrepareGravity(gt_reconstruction, images, /*stddev_gravity=*/3e-1); + PrepareGravity(gt_reconstruction, frames, /*gravity_noise_stddev=*/3e-1); GlobalMapper global_mapper(CreateMapperTestOptions()); - global_mapper.Solve(database, view_graph, cameras, images, tracks); + global_mapper.Solve( + *database, view_graph, rigs, cameras, frames, images, tracks); - for (bool use_gravity : {true, false}) { + // TODO: The current 1-dof rotation averaging sometimes fails to pick the + // right solution (e.g., 180 deg flipped). + for (const bool use_gravity : {false}) { SolveRotationAveraging( - view_graph, images, CreateRATestOptions(use_gravity)); + view_graph, rigs, frames, images, CreateRATestOptions(use_gravity)); colmap::Reconstruction reconstruction; - ConvertGlomapToColmap(cameras, images, tracks, reconstruction); + ConvertGlomapToColmap( + rigs, cameras, frames, images, tracks, reconstruction); + ExpectEqualRotations( + gt_reconstruction, reconstruction, /*max_rotation_error_deg=*/3); + } +} + +TEST(RotationEstimator, WithNoiseAndOutliersWithNonTrivialKnownRigs) { + colmap::SetPRNGSeed(1); + + const std::string database_path = colmap::CreateTestDir() + "/database.db"; + + auto database = colmap::Database::Open(database_path); + colmap::Reconstruction gt_reconstruction; + colmap::SyntheticDatasetOptions synthetic_dataset_options; + synthetic_dataset_options.num_rigs = 2; + synthetic_dataset_options.num_cameras_per_rig = 2; + synthetic_dataset_options.num_frames_per_rig = 7; + synthetic_dataset_options.num_points3D = 100; + synthetic_dataset_options.point2D_stddev = 1; + synthetic_dataset_options.inlier_match_ratio = 0.6; + colmap::SynthesizeDataset( + synthetic_dataset_options, >_reconstruction, database.get()); + + ViewGraph view_graph; + std::unordered_map rigs; + std::unordered_map cameras; + std::unordered_map images; + std::unordered_map frames; + std::unordered_map tracks; + + ConvertDatabaseToGlomap(*database, view_graph, rigs, cameras, frames, images); + PrepareGravity(gt_reconstruction, frames, /*gravity_noise_stddev=*/3e-1); + + GlobalMapper global_mapper(CreateMapperTestOptions()); + global_mapper.Solve( + *database, view_graph, rigs, cameras, frames, images, tracks); + + // TODO: The current 1-dof rotation averaging sometimes fails to pick the + // right solution (e.g., 180 deg flipped). + for (const bool use_gravity : {true, false}) { + SolveRotationAveraging( + view_graph, rigs, frames, images, CreateRATestOptions(use_gravity)); + + colmap::Reconstruction reconstruction; + ConvertGlomapToColmap( + rigs, cameras, frames, images, tracks, reconstruction); if (use_gravity) ExpectEqualRotations( gt_reconstruction, reconstruction, /*max_rotation_error_deg=*/1.5); @@ -198,38 +361,91 @@ TEST(RotationEstimator, WithNoiseAndOutliers) { } TEST(RotationEstimator, RefineGravity) { + colmap::SetPRNGSeed(1); + + const std::string database_path = colmap::CreateTestDir() + "/database.db"; + + auto database = colmap::Database::Open(database_path); + colmap::Reconstruction gt_reconstruction; + colmap::SyntheticDatasetOptions synthetic_dataset_options; + synthetic_dataset_options.num_rigs = 2; + synthetic_dataset_options.num_cameras_per_rig = 1; + synthetic_dataset_options.num_frames_per_rig = 25; + synthetic_dataset_options.num_points3D = 100; + synthetic_dataset_options.point2D_stddev = 0; + colmap::SynthesizeDataset( + synthetic_dataset_options, >_reconstruction, database.get()); + + ViewGraph view_graph; + std::unordered_map rigs; + std::unordered_map cameras; + std::unordered_map frames; + std::unordered_map images; + std::unordered_map tracks; + + ConvertDatabaseToGlomap(*database, view_graph, rigs, cameras, frames, images); + + PrepareGravity(gt_reconstruction, + frames, + /*gravity_noise_stddev=*/0., + /*outlier_ratio=*/0.3); + + GlobalMapper global_mapper(CreateMapperTestOptions()); + global_mapper.Solve( + *database, view_graph, rigs, cameras, frames, images, tracks); + + GravityRefinerOptions opt_grav_refine; + GravityRefiner grav_refiner(opt_grav_refine); + grav_refiner.RefineGravity(view_graph, frames, images); + + // Check whether the gravity does not have error after refinement + ExpectEqualGravity(gt_reconstruction, + images, + /*max_gravity_error_deg=*/1e-2); +} + +TEST(RotationEstimator, RefineGravityWithNontrivialRigs) { + colmap::SetPRNGSeed(1); + const std::string database_path = colmap::CreateTestDir() + "/database.db"; - // FLAGS_v = 2; - colmap::Database database(database_path); + auto database = colmap::Database::Open(database_path); colmap::Reconstruction gt_reconstruction; colmap::SyntheticDatasetOptions synthetic_dataset_options; - synthetic_dataset_options.num_cameras = 2; - synthetic_dataset_options.num_images = 100; - synthetic_dataset_options.num_points3D = 200; + synthetic_dataset_options.num_rigs = 2; + synthetic_dataset_options.num_cameras_per_rig = 2; + synthetic_dataset_options.num_frames_per_rig = 25; + synthetic_dataset_options.num_points3D = 100; synthetic_dataset_options.point2D_stddev = 0; colmap::SynthesizeDataset( - synthetic_dataset_options, >_reconstruction, &database); + synthetic_dataset_options, >_reconstruction, database.get()); ViewGraph view_graph; + std::unordered_map rigs; std::unordered_map cameras; + std::unordered_map frames; std::unordered_map images; std::unordered_map tracks; - ConvertDatabaseToGlomap(database, view_graph, cameras, images); + ConvertDatabaseToGlomap(*database, view_graph, rigs, cameras, frames, images); - PrepareGravity( - gt_reconstruction, images, /*stddev_gravity=*/0., /*outlier_ratio=*/0.3); + PrepareGravity(gt_reconstruction, + frames, + /*gravity_noise_stddev=*/0., + /*outlier_ratio=*/0.3); GlobalMapper global_mapper(CreateMapperTestOptions()); - global_mapper.Solve(database, view_graph, cameras, images, tracks); + global_mapper.Solve( + *database, view_graph, rigs, cameras, frames, images, tracks); GravityRefinerOptions opt_grav_refine; GravityRefiner grav_refiner(opt_grav_refine); - grav_refiner.RefineGravity(view_graph, images); + grav_refiner.RefineGravity(view_graph, frames, images); // Check whether the gravity does not have error after refinement - ExpectEqualGravity(gt_reconstruction, images, /*max_gravity_error_deg=*/1e-2); + ExpectEqualGravity(gt_reconstruction, + images, + /*max_gravity_error_deg=*/1e-2); } } // namespace diff --git a/glomap/controllers/track_establishment.cc b/glomap/controllers/track_establishment.cc index 105f7763..d4396ed3 100644 --- a/glomap/controllers/track_establishment.cc +++ b/glomap/controllers/track_establishment.cc @@ -173,7 +173,7 @@ size_t TrackEngine::FindTracksForProblem( // corresponding to those images std::unordered_map tracks; for (const auto& [image_id, image] : images_) { - if (!image.is_registered) continue; + if (!image.IsRegistered()) continue; tracks_per_camera[image_id] = 0; } diff --git a/glomap/controllers/track_retriangulation.cc b/glomap/controllers/track_retriangulation.cc index 92d73e0f..0b133e0d 100644 --- a/glomap/controllers/track_retriangulation.cc +++ b/glomap/controllers/track_retriangulation.cc @@ -12,7 +12,9 @@ namespace glomap { bool RetriangulateTracks(const TriangulatorOptions& options, const colmap::Database& database, + std::unordered_map& rigs, std::unordered_map& cameras, + std::unordered_map& frames, std::unordered_map& images, std::unordered_map& tracks) { // Following code adapted from COLMAP @@ -28,16 +30,18 @@ bool RetriangulateTracks(const TriangulatorOptions& options, std::vector image_ids_notconnected; for (auto& image : images) { if (!database_cache->ExistsImage(image.first) && - image.second.is_registered) { - image.second.is_registered = false; + image.second.IsRegistered()) { image_ids_notconnected.push_back(image.first); + image.second.frame_ptr->is_registered = false; } } // Convert the glomap data structures to colmap data structures std::shared_ptr reconstruction_ptr = std::make_shared(); - ConvertGlomapToColmap(cameras, + ConvertGlomapToColmap(rigs, + cameras, + frames, images, std::unordered_map(), *reconstruction_ptr); @@ -59,7 +63,7 @@ bool RetriangulateTracks(const TriangulatorOptions& options, const auto tri_options = options_colmap.Triangulation(); const auto mapper_options = options_colmap.Mapper(); - const std::set& reg_image_ids = reconstruction_ptr->RegImageIds(); + const std::vector reg_image_ids = reconstruction_ptr->RegImageIds(); size_t image_idx = 0; for (const image_t image_id : reg_image_ids) { @@ -79,7 +83,8 @@ bool RetriangulateTracks(const TriangulatorOptions& options, ba_options.refine_focal_length = false; ba_options.refine_principal_point = false; ba_options.refine_extra_params = false; - ba_options.refine_extrinsics = false; + ba_options.refine_sensor_from_rig = false; + ba_options.refine_rig_from_world = false; // Configure bundle adjustment. colmap::BundleAdjustmentConfig ba_config; @@ -118,14 +123,15 @@ bool RetriangulateTracks(const TriangulatorOptions& options, // Add the removed images to the reconstruction for (const auto& image_id : image_ids_notconnected) { - images[image_id].is_registered = true; + images[image_id].frame_ptr->is_registered = true; colmap::Image image_colmap; ConvertGlomapToColmapImage(images[image_id], image_colmap, true); reconstruction_ptr->AddImage(std::move(image_colmap)); } // Convert the colmap data structures back to glomap data structures - ConvertColmapToGlomap(*reconstruction_ptr, cameras, images, tracks); + ConvertColmapToGlomap( + *reconstruction_ptr, rigs, cameras, frames, images, tracks); return true; } diff --git a/glomap/controllers/track_retriangulation.h b/glomap/controllers/track_retriangulation.h index 6b058515..169eae79 100644 --- a/glomap/controllers/track_retriangulation.h +++ b/glomap/controllers/track_retriangulation.h @@ -17,7 +17,9 @@ struct TriangulatorOptions { bool RetriangulateTracks(const TriangulatorOptions& options, const colmap::Database& database, + std::unordered_map& rigs, std::unordered_map& cameras, + std::unordered_map& frames, std::unordered_map& images, std::unordered_map& tracks); diff --git a/glomap/estimators/bundle_adjustment.cc b/glomap/estimators/bundle_adjustment.cc index 0e5ee5d3..8edffb01 100644 --- a/glomap/estimators/bundle_adjustment.cc +++ b/glomap/estimators/bundle_adjustment.cc @@ -8,8 +8,9 @@ namespace glomap { -bool BundleAdjuster::Solve(const ViewGraph& view_graph, +bool BundleAdjuster::Solve(std::unordered_map& rigs, std::unordered_map& cameras, + std::unordered_map& frames, std::unordered_map& images, std::unordered_map& tracks) { // Check if the input data is valid @@ -26,14 +27,14 @@ bool BundleAdjuster::Solve(const ViewGraph& view_graph, Reset(); // Add the constraints that the point tracks impose on the problem - AddPointToCameraConstraints(view_graph, cameras, images, tracks); + AddPointToCameraConstraints(rigs, cameras, frames, images, tracks); // Add the cameras and points to the parameter groups for schur-based // optimization - AddCamerasAndPointsToParameterGroups(cameras, images, tracks); + AddCamerasAndPointsToParameterGroups(rigs, cameras, frames, tracks); // Parameterize the variables - ParameterizeVariables(cameras, images, tracks); + ParameterizeVariables(rigs, cameras, frames, tracks); // Set the solver options. ceres::Solver::Summary summary; @@ -112,8 +113,9 @@ void BundleAdjuster::Reset() { } void BundleAdjuster::AddPointToCameraConstraints( - const ViewGraph& view_graph, + std::unordered_map& rigs, std::unordered_map& cameras, + std::unordered_map& frames, std::unordered_map& images, std::unordered_map& tracks) { for (auto& [track_id, track] : tracks) { @@ -123,20 +125,61 @@ void BundleAdjuster::AddPointToCameraConstraints( if (images.find(observation.first) == images.end()) continue; Image& image = images[observation.first]; - - ceres::CostFunction* cost_function = - colmap::CreateCameraCostFunction( - cameras[image.camera_id].model_id, - image.features[observation.second]); - - if (cost_function != nullptr) { + Frame* frame_ptr = image.frame_ptr; + camera_t camera_id = image.camera_id; + image_t rig_id = image.frame_ptr->RigId(); + + ceres::CostFunction* cost_function = nullptr; + // if (image_id_to_camera_rig_index_.find(observation.first) == + // image_id_to_camera_rig_index_.end()) { + if (image.HasTrivialFrame()) { + cost_function = + colmap::CreateCameraCostFunction( + cameras[image.camera_id].model_id, + image.features[observation.second]); + problem_->AddResidualBlock( + cost_function, + loss_function_.get(), + frame_ptr->RigFromWorld().rotation.coeffs().data(), + frame_ptr->RigFromWorld().translation.data(), + tracks[track_id].xyz.data(), + cameras[image.camera_id].params.data()); + } else if (!options_.optimize_rig_poses) { + const Rigid3d& cam_from_rig = rigs[rig_id].SensorFromRig( + sensor_t(SensorType::CAMERA, image.camera_id)); + cost_function = colmap::CreateCameraCostFunction< + colmap::RigReprojErrorConstantRigCostFunctor>( + cameras[image.camera_id].model_id, + image.features[observation.second], + cam_from_rig); + problem_->AddResidualBlock( + cost_function, + loss_function_.get(), + frame_ptr->RigFromWorld().rotation.coeffs().data(), + frame_ptr->RigFromWorld().translation.data(), + tracks[track_id].xyz.data(), + cameras[image.camera_id].params.data()); + } else { + // If the image is part of a camera rig, use the RigBATA error + // Down weight the uncalibrated cameras + Rigid3d& cam_from_rig = rigs[rig_id].SensorFromRig( + sensor_t(SensorType::CAMERA, image.camera_id)); + cost_function = + colmap::CreateCameraCostFunction( + cameras[image.camera_id].model_id, + image.features[observation.second]); problem_->AddResidualBlock( cost_function, loss_function_.get(), - image.cam_from_world.rotation.coeffs().data(), - image.cam_from_world.translation.data(), + cam_from_rig.rotation.coeffs().data(), + cam_from_rig.translation.data(), + frame_ptr->RigFromWorld().rotation.coeffs().data(), + frame_ptr->RigFromWorld().translation.data(), tracks[track_id].xyz.data(), cameras[image.camera_id].params.data()); + } + + if (cost_function != nullptr) { } else { LOG(ERROR) << "Camera model not supported: " << colmap::CameraModelIdToName( @@ -147,8 +190,9 @@ void BundleAdjuster::AddPointToCameraConstraints( } void BundleAdjuster::AddCamerasAndPointsToParameterGroups( + std::unordered_map& rigs, std::unordered_map& cameras, - std::unordered_map& images, + std::unordered_map& frames, std::unordered_map& tracks) { if (tracks.size() == 0) return; @@ -163,13 +207,30 @@ void BundleAdjuster::AddCamerasAndPointsToParameterGroups( parameter_ordering->AddElementToGroup(track.xyz.data(), 0); } - // Add camera parameters to group 1. - for (auto& [image_id, image] : images) { - if (problem_->HasParameterBlock(image.cam_from_world.translation.data())) { + // Add frame parameters to group 1. + for (auto& [frame_id, frame] : frames) { + if (!frame.HasPose()) continue; + if (problem_->HasParameterBlock(frame.RigFromWorld().translation.data())) { parameter_ordering->AddElementToGroup( - image.cam_from_world.translation.data(), 1); + frame.RigFromWorld().translation.data(), 1); parameter_ordering->AddElementToGroup( - image.cam_from_world.rotation.coeffs().data(), 1); + frame.RigFromWorld().rotation.coeffs().data(), 1); + } + } + + // Add the cam_from_rigs to be estimated into the parameter group + for (auto& [rig_id, rig] : rigs) { + for (const auto& [sensor_id, sensor] : rig.NonRefSensors()) { + if (sensor_id.type == SensorType::CAMERA) { + Eigen::Vector3d& translation = rig.SensorFromRig(sensor_id).translation; + if (problem_->HasParameterBlock(translation.data())) { + parameter_ordering->AddElementToGroup(translation.data(), 1); + } + Eigen::Quaterniond& rotation = rig.SensorFromRig(sensor_id).rotation; + if (problem_->HasParameterBlock(rotation.coeffs().data())) { + parameter_ordering->AddElementToGroup(rotation.coeffs().data(), 1); + } + } } } @@ -181,39 +242,31 @@ void BundleAdjuster::AddCamerasAndPointsToParameterGroups( } void BundleAdjuster::ParameterizeVariables( + std::unordered_map& rigs, std::unordered_map& cameras, - std::unordered_map& images, + std::unordered_map& frames, std::unordered_map& tracks) { - image_t center; + frame_t center; // Parameterize rotations, and set rotations and translations to be constant // if desired FUTURE: Consider fix the scale of the reconstruction int counter = 0; - for (auto& [image_id, image] : images) { + for (auto& [frame_id, frame] : frames) { + if (!frame.HasPose()) continue; if (problem_->HasParameterBlock( - image.cam_from_world.rotation.coeffs().data())) { + frame.RigFromWorld().rotation.coeffs().data())) { colmap::SetQuaternionManifold( - problem_.get(), image.cam_from_world.rotation.coeffs().data()); + problem_.get(), frame.RigFromWorld().rotation.coeffs().data()); - if (counter == 0) { - center = image_id; - counter++; - } - if (!options_.optimize_rotations) + if (!options_.optimize_rotations || counter == 0) problem_->SetParameterBlockConstant( - image.cam_from_world.rotation.coeffs().data()); - if (!options_.optimize_translation) + frame.RigFromWorld().rotation.coeffs().data()); + if (!options_.optimize_translation || counter == 0) problem_->SetParameterBlockConstant( - image.cam_from_world.translation.data()); - } - } + frame.RigFromWorld().translation.data()); - if (counter > 0) { - // Set the first camera to be fixed to remove the gauge ambiguity. - problem_->SetParameterBlockConstant( - images[center].cam_from_world.rotation.coeffs().data()); - problem_->SetParameterBlockConstant( - images[center].cam_from_world.translation.data()); + counter++; + } } // Parameterize the camera parameters, or set them to be constant if desired @@ -239,6 +292,21 @@ void BundleAdjuster::ParameterizeVariables( } } + // If we optimize the rig poses, then parameterize them + if (options_.optimize_rig_poses) { + for (auto& [rig_id, rig] : rigs) { + for (const auto& [sensor_id, sensor] : rig.NonRefSensors()) { + if (sensor_id.type == SensorType::CAMERA) { + Eigen::Quaterniond& rotation = rig.SensorFromRig(sensor_id).rotation; + if (problem_->HasParameterBlock(rotation.coeffs().data())) { + colmap::SetQuaternionManifold(problem_.get(), + rotation.coeffs().data()); + } + } + } + } + } + if (!options_.optimize_points) { for (auto& [track_id, track] : tracks) { if (problem_->HasParameterBlock(track.xyz.data())) { diff --git a/glomap/estimators/bundle_adjustment.h b/glomap/estimators/bundle_adjustment.h index b78347ca..5fdd92f6 100644 --- a/glomap/estimators/bundle_adjustment.h +++ b/glomap/estimators/bundle_adjustment.h @@ -1,5 +1,6 @@ #pragma once +// #include "glomap/estimators/bundle_adjustment.h" #include "glomap/estimators/optimization_base.h" #include "glomap/scene/types_sfm.h" #include "glomap/types.h" @@ -11,6 +12,7 @@ namespace glomap { struct BundleAdjusterOptions : public OptimizationBaseOptions { public: // Flags for which parameters to optimize + bool optimize_rig_poses = false; // Whether to optimize the rig poses bool optimize_rotations = true; bool optimize_translation = true; bool optimize_intrinsics = true; @@ -33,7 +35,6 @@ struct BundleAdjusterOptions : public OptimizationBaseOptions { return std::make_shared(thres_loss_function); } }; - class BundleAdjuster { public: BundleAdjuster(const BundleAdjusterOptions& options) : options_(options) {} @@ -41,8 +42,9 @@ class BundleAdjuster { // Returns true if the optimization was a success, false if there was a // failure. // Assume tracks here are already filtered - bool Solve(const ViewGraph& view_graph, + bool Solve(std::unordered_map& rigs, std::unordered_map& cameras, + std::unordered_map& frames, std::unordered_map& images, std::unordered_map& tracks); @@ -54,20 +56,23 @@ class BundleAdjuster { // Add tracks to the problem void AddPointToCameraConstraints( - const ViewGraph& view_graph, + std::unordered_map& rigs, std::unordered_map& cameras, + std::unordered_map& frames, std::unordered_map& images, std::unordered_map& tracks); // Set the parameter groups void AddCamerasAndPointsToParameterGroups( + std::unordered_map& rigs, std::unordered_map& cameras, - std::unordered_map& images, + std::unordered_map& frames, std::unordered_map& tracks); // Parameterize the variables, set some variables to be constant if desired - void ParameterizeVariables(std::unordered_map& cameras, - std::unordered_map& images, + void ParameterizeVariables(std::unordered_map& rigs, + std::unordered_map& cameras, + std::unordered_map& frames, std::unordered_map& tracks); BundleAdjusterOptions options_; diff --git a/glomap/estimators/cost_function.h b/glomap/estimators/cost_function.h index 64c3e466..ed21efdf 100644 --- a/glomap/estimators/cost_function.h +++ b/glomap/estimators/cost_function.h @@ -40,6 +40,101 @@ struct BATAPairwiseDirectionError { const Eigen::Vector3d translation_obs_; }; +// ---------------------------------------- +// RigBATAPairwiseDirectionError +// ---------------------------------------- +// Computes the error between a translation direction and the direction formed +// from two positions such that t_ij - scale * (c_j - c_i + scale_rig * t_rig) +// is minimized. +struct RigBATAPairwiseDirectionError { + RigBATAPairwiseDirectionError(const Eigen::Vector3d& translation_obs, + const Eigen::Vector3d& translation_rig) + : translation_obs_(translation_obs), translation_rig_(translation_rig) {} + + // The error is given by the position error described above. + template + bool operator()(const T* position1, + const T* position2, + const T* scale, + const T* scale_rig, + T* residuals) const { + Eigen::Map> residuals_vec(residuals); + residuals_vec = + translation_obs_.cast() - + scale[0] * (Eigen::Map>(position2) - + Eigen::Map>(position1) + + scale_rig[0] * translation_rig_.cast()); + return true; + } + + static ceres::CostFunction* Create(const Eigen::Vector3d& translation_obs, + const Eigen::Vector3d& translation_rig) { + return ( + new ceres:: + AutoDiffCostFunction( + new RigBATAPairwiseDirectionError(translation_obs, + translation_rig))); + } + + // TODO: add covariance + const Eigen::Vector3d translation_obs_; + const Eigen::Vector3d translation_rig_; // = c_R_w^T * c_t_r +}; + +// ---------------------------------------- +// RigUnknownBATAPairwiseDirectionError +// ---------------------------------------- +// Computes the error between a translation direction and the direction formed +// from three positions such that v - scale * ((X - r_c_w) - r_R_w^T * c_c_r) is +// minimized. +struct RigUnknownBATAPairwiseDirectionError { + RigUnknownBATAPairwiseDirectionError( + const Eigen::Vector3d& translation_obs, + const Eigen::Quaterniond& rig_from_world_rot) + : translation_obs_(translation_obs), + rig_from_world_rot_(rig_from_world_rot) {} + + // The error is given by the position error described above. + template + bool operator()(const T* point3d, + const T* rig_from_world_center, + const T* cam_from_rig_center, + const T* scale, + T* residuals) const { + Eigen::Map> residuals_vec(residuals); + + Eigen::Matrix translation_rig = + rig_from_world_rot_.toRotationMatrix().transpose() * + Eigen::Map>(cam_from_rig_center); + + residuals_vec = + translation_obs_.cast() - + scale[0] * + (Eigen::Map>(point3d) - + Eigen::Map>(rig_from_world_center) - + translation_rig); + return true; + } + + static ceres::CostFunction* Create( + const Eigen::Vector3d& translation_obs, + const Eigen::Quaterniond& rig_from_world_rot) { + return ( + new ceres::AutoDiffCostFunction( + new RigUnknownBATAPairwiseDirectionError(translation_obs, + rig_from_world_rot))); + } + + // TODO: add covariance + const Eigen::Vector3d translation_obs_; + const Eigen::Quaterniond rig_from_world_rot_; // = c_R_w^T * c_t_r +}; + // ---------------------------------------- // FetzerFocalLengthCost // ---------------------------------------- diff --git a/glomap/estimators/global_positioning.cc b/glomap/estimators/global_positioning.cc index ebe1b8de..298f23db 100644 --- a/glomap/estimators/global_positioning.cc +++ b/glomap/estimators/global_positioning.cc @@ -1,6 +1,7 @@ #include "glomap/estimators/global_positioning.h" #include "glomap/estimators/cost_function.h" +#include "glomap/math/rigid3d.h" #include #include @@ -25,9 +26,14 @@ GlobalPositioner::GlobalPositioner(const GlobalPositionerOptions& options) } bool GlobalPositioner::Solve(const ViewGraph& view_graph, + std::unordered_map& rigs, std::unordered_map& cameras, + std::unordered_map& frames, std::unordered_map& images, std::unordered_map& tracks) { + if (rigs.size() > 1) { + LOG(ERROR) << "Number of camera rigs = " << rigs.size(); + } if (images.empty()) { LOG(ERROR) << "Number of images = " << images.size(); return false; @@ -46,27 +52,29 @@ bool GlobalPositioner::Solve(const ViewGraph& view_graph, LOG(INFO) << "Setting up the global positioner problem"; // Setup the problem. - SetupProblem(view_graph, tracks); + SetupProblem(view_graph, rigs, tracks); // Initialize camera translations to be random. // Also, convert the camera pose translation to be the camera center. - InitializeRandomPositions(view_graph, images, tracks); + InitializeRandomPositions(view_graph, frames, images, tracks); // Add the camera to camera constraints to the problem. + // TODO: support the relative constraints with trivial frames to a non trivial + // frame if (options_.constraint_type != GlobalPositionerOptions::ONLY_POINTS) { AddCameraToCameraConstraints(view_graph, images); } // Add the point to camera constraints to the problem. if (options_.constraint_type != GlobalPositionerOptions::ONLY_CAMERAS) { - AddPointToCameraConstraints(cameras, images, tracks); + AddPointToCameraConstraints(rigs, cameras, frames, images, tracks); } - AddCamerasAndPointsToParameterGroups(images, tracks); + AddCamerasAndPointsToParameterGroups(rigs, frames, tracks); // Parameterize the variables, set image poses / tracks / scales to be // constant if desired - ParameterizeVariables(images, tracks); + ParameterizeVariables(rigs, frames, tracks); LOG(INFO) << "Solving the global positioner problem"; @@ -80,12 +88,13 @@ bool GlobalPositioner::Solve(const ViewGraph& view_graph, LOG(INFO) << summary.BriefReport(); } - ConvertResults(images); + ConvertResults(rigs, frames); return summary.IsSolutionUsable(); } void GlobalPositioner::SetupProblem( const ViewGraph& view_graph, + const std::unordered_map& rigs, const std::unordered_map& tracks) { ceres::Problem::Options problem_options; problem_options.loss_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; @@ -104,48 +113,52 @@ void GlobalPositioner::SetupProblem( [](int sum, const std::pair& track) { return sum + track.second.observations.size(); })); + + // Initialize the rig scales to be 1.0. + for (const auto& [rig_id, rig] : rigs) { + rig_scales_.emplace(rig_id, 1.0); + } } void GlobalPositioner::InitializeRandomPositions( const ViewGraph& view_graph, + std::unordered_map& frames, std::unordered_map& images, std::unordered_map& tracks) { std::unordered_set constrained_positions; - constrained_positions.reserve(images.size()); + constrained_positions.reserve(frames.size()); for (const auto& [pair_id, image_pair] : view_graph.image_pairs) { if (image_pair.is_valid == false) continue; - - constrained_positions.insert(image_pair.image_id1); - constrained_positions.insert(image_pair.image_id2); + constrained_positions.insert(images[image_pair.image_id1].frame_id); + constrained_positions.insert(images[image_pair.image_id2].frame_id); } - if (options_.constraint_type != GlobalPositionerOptions::ONLY_CAMERAS) { - for (const auto& [track_id, track] : tracks) { - if (track.observations.size() < options_.min_num_view_per_track) continue; - for (const auto& observation : tracks[track_id].observations) { - if (images.find(observation.first) == images.end()) continue; - Image& image = images[observation.first]; - if (!image.is_registered) continue; - constrained_positions.insert(observation.first); - } + for (const auto& [track_id, track] : tracks) { + if (track.observations.size() < options_.min_num_view_per_track) continue; + for (const auto& observation : tracks[track_id].observations) { + if (images.find(observation.first) == images.end()) continue; + Image& image = images[observation.first]; + if (!image.IsRegistered()) continue; + constrained_positions.insert(images[observation.first].frame_id); } } if (!options_.generate_random_positions || !options_.optimize_positions) { - for (auto& [image_id, image] : images) { - image.cam_from_world.translation = image.Center(); + for (auto& [frame_id, frame] : frames) { + if (constrained_positions.find(frame_id) != constrained_positions.end()) + frame.RigFromWorld().translation = CenterFromPose(frame.RigFromWorld()); } return; } // Generate random positions for the cameras centers. - for (auto& [image_id, image] : images) { + for (auto& [frame_id, frame] : frames) { // Only set the cameras to be random if they are needed to be optimized - if (constrained_positions.find(image_id) != constrained_positions.end()) - image.cam_from_world.translation = + if (constrained_positions.find(frame_id) != constrained_positions.end()) + frame.RigFromWorld().translation = 100.0 * RandVector3d(random_generator_, -1, 1); else - image.cam_from_world.translation = image.Center(); + frame.RigFromWorld().translation = CenterFromPose(frame.RigFromWorld()); } VLOG(2) << "Constrained positions: " << constrained_positions.size(); @@ -153,6 +166,15 @@ void GlobalPositioner::InitializeRandomPositions( void GlobalPositioner::AddCameraToCameraConstraints( const ViewGraph& view_graph, std::unordered_map& images) { + // For cam to cam constraint, only support the trivial frames now + for (const auto& [image_id, image] : images) { + if (!image.IsRegistered()) continue; + if (!image.HasTrivialFrame()) { + LOG(ERROR) << "Now, only trivial frames are supported for the camera to " + "camera constraints"; + } + } + for (const auto& [pair_id, image_pair] : view_graph.image_pairs) { if (image_pair.is_valid == false) continue; @@ -163,20 +185,20 @@ void GlobalPositioner::AddCameraToCameraConstraints( continue; } - CHECK_GT(scales_.capacity(), scales_.size()) + CHECK_GE(scales_.capacity(), scales_.size()) << "Not enough capacity was reserved for the scales."; double& scale = scales_.emplace_back(1); const Eigen::Vector3d translation = - -(images[image_id2].cam_from_world.rotation.inverse() * + -(images[image_id2].CamFromWorld().rotation.inverse() * image_pair.cam2_from_cam1.translation); ceres::CostFunction* cost_function = BATAPairwiseDirectionError::Create(translation); problem_->AddResidualBlock( cost_function, loss_function_.get(), - images[image_id1].cam_from_world.translation.data(), - images[image_id2].cam_from_world.translation.data(), + images[image_id1].frame_ptr->RigFromWorld().translation.data(), + images[image_id2].frame_ptr->RigFromWorld().translation.data(), &scale); problem_->SetParameterLowerBound(&scale, 0, 1e-5); @@ -188,7 +210,9 @@ void GlobalPositioner::AddCameraToCameraConstraints( } void GlobalPositioner::AddPointToCameraConstraints( + std::unordered_map& rigs, std::unordered_map& cameras, + std::unordered_map& frames, std::unordered_map& images, std::unordered_map& tracks) { // The number of camera-to-camera constraints coming from the relative poses @@ -239,13 +263,15 @@ void GlobalPositioner::AddPointToCameraConstraints( track.is_initialized = true; } - AddTrackToProblem(track_id, cameras, images, tracks); + AddTrackToProblem(track_id, rigs, cameras, frames, images, tracks); } } void GlobalPositioner::AddTrackToProblem( track_t track_id, + std::unordered_map& rigs, std::unordered_map& cameras, + std::unordered_map& frames, std::unordered_map& images, std::unordered_map& tracks) { // For each view in the track add the point to camera correspondences. @@ -253,7 +279,7 @@ void GlobalPositioner::AddTrackToProblem( if (images.find(observation.first) == images.end()) continue; Image& image = images[observation.first]; - if (!image.is_registered) continue; + if (!image.IsRegistered()) continue; const Eigen::Vector3d& feature_undist = image.features_undist[observation.second]; @@ -266,35 +292,82 @@ void GlobalPositioner::AddTrackToProblem( } const Eigen::Vector3d translation = - image.cam_from_world.rotation.inverse() * + image.CamFromWorld().rotation.inverse() * image.features_undist[observation.second]; - ceres::CostFunction* cost_function = - BATAPairwiseDirectionError::Create(translation); - CHECK_GT(scales_.capacity(), scales_.size()) - << "Not enough capacity was reserved for the scales."; double& scale = scales_.emplace_back(1); + if (!options_.generate_scales && tracks[track_id].is_initialized) { const Eigen::Vector3d trans_calc = - tracks[track_id].xyz - image.cam_from_world.translation; + tracks[track_id].xyz - image.CamFromWorld().translation; scale = std::max(1e-5, translation.dot(trans_calc) / trans_calc.squaredNorm()); } - // For calibrated and uncalibrated cameras, use different loss functions + CHECK_GE(scales_.capacity(), scales_.size()) + << "Not enough capacity was reserved for the scales."; + + // For calibrated and uncalibrated cameras, use different loss + // functions // Down weight the uncalibrated cameras - if (cameras[image.camera_id].has_prior_focal_length) { - problem_->AddResidualBlock(cost_function, - loss_function_ptcam_calibrated_.get(), - image.cam_from_world.translation.data(), - tracks[track_id].xyz.data(), - &scale); + ceres::LossFunction* loss_function = + (cameras[image.camera_id].has_prior_focal_length) + ? loss_function_ptcam_calibrated_.get() + : loss_function_ptcam_uncalibrated_.get(); + + // If the image is not part of a camera rig, use the standard BATA error + if (image.HasTrivialFrame()) { + ceres::CostFunction* cost_function = + BATAPairwiseDirectionError::Create(translation); + + problem_->AddResidualBlock( + cost_function, + loss_function, + image.frame_ptr->RigFromWorld().translation.data(), + tracks[track_id].xyz.data(), + &scale); + // If the image is part of a camera rig, use the RigBATA error } else { - problem_->AddResidualBlock(cost_function, - loss_function_ptcam_uncalibrated_.get(), - image.cam_from_world.translation.data(), - tracks[track_id].xyz.data(), - &scale); + rig_t rig_id = image.frame_ptr->RigId(); + // Otherwise, use the camera rig translation from the frame + Rigid3d& cam_from_rig = rigs.at(rig_id).SensorFromRig( + sensor_t(SensorType::CAMERA, image.camera_id)); + + Eigen::Vector3d cam_from_rig_translation = cam_from_rig.translation; + + if (!cam_from_rig_translation.hasNaN()) { + const Eigen::Vector3d translation_rig = + // image.cam_from_world.rotation.inverse() * + // cam_from_rig.translation; + image.CamFromWorld().rotation.inverse() * cam_from_rig_translation; + + ceres::CostFunction* cost_function = + RigBATAPairwiseDirectionError::Create(translation, translation_rig); + + problem_->AddResidualBlock( + cost_function, + loss_function, + image.frame_ptr->RigFromWorld().translation.data(), + tracks[track_id].xyz.data(), + &scale, + &rig_scales_[rig_id]); + } else { + // If the cam_from_rig contains nan values, it means that it needs to be + // re-estimated In this case, use the rigged cost NOTE: the scale for + // the rig is not needed, as it would natrually be consistent with the + // global one + ceres::CostFunction* cost_function = + RigUnknownBATAPairwiseDirectionError::Create( + translation, image.frame_ptr->RigFromWorld().rotation); + + problem_->AddResidualBlock( + cost_function, + loss_function, + tracks[track_id].xyz.data(), + image.frame_ptr->RigFromWorld().translation.data(), + cam_from_rig.translation.data(), + &scale); + } } problem_->SetParameterLowerBound(&scale, 0, 1e-5); @@ -302,7 +375,9 @@ void GlobalPositioner::AddTrackToProblem( } void GlobalPositioner::AddCamerasAndPointsToParameterGroups( - std::unordered_map& images, + // std::unordered_map& images, + std::unordered_map& rigs, + std::unordered_map& frames, std::unordered_map& tracks) { // Create a custom ordering for Schur-based problems. options_.solver_options.linear_solver_ordering.reset( @@ -325,27 +400,67 @@ void GlobalPositioner::AddCamerasAndPointsToParameterGroups( group_id++; } - // Add camera parameters to group 2 if there are tracks, otherwise group 1. - for (auto& [image_id, image] : images) { - if (problem_->HasParameterBlock(image.cam_from_world.translation.data())) { + for (auto& [frame_id, frame] : frames) { + if (!frame.HasPose()) continue; + if (problem_->HasParameterBlock(frame.RigFromWorld().translation.data())) { parameter_ordering->AddElementToGroup( - image.cam_from_world.translation.data(), group_id); + frame.RigFromWorld().translation.data(), group_id); + } + } + + // Add the cam_from_rigs to be estimated into the parameter group + for (auto& [rig_id, rig] : rigs) { + for (const auto& [sensor_id, sensor] : rig.NonRefSensors()) { + if (sensor_id.type == SensorType::CAMERA) { + Eigen::Vector3d& translation = rig.SensorFromRig(sensor_id).translation; + if (problem_->HasParameterBlock(translation.data())) { + parameter_ordering->AddElementToGroup(translation.data(), group_id); + } + } } } + + group_id++; + + // Also add the scales to the group + for (auto& [rig_id, scale] : rig_scales_) { + if (problem_->HasParameterBlock(&scale)) + parameter_ordering->AddElementToGroup(&scale, group_id); + } } void GlobalPositioner::ParameterizeVariables( - std::unordered_map& images, + // std::unordered_map& images, + std::unordered_map& rigs, + std::unordered_map& frames, std::unordered_map& tracks) { // For the global positioning, do not set any camera to be constant for easier // convergence + // First, for cam_from_rig that needs to be estimated, we need to initialize + // the center + if (options_.optimize_positions) { + for (auto& [rig_id, rig] : rigs) { + for (const auto& [sensor_id, sensor] : rig.NonRefSensors()) { + if (sensor_id.type == SensorType::CAMERA) { + Eigen::Vector3d& translation = + rig.SensorFromRig(sensor_id).translation; + if (problem_->HasParameterBlock(translation.data())) { + translation = RandVector3d(random_generator_, -1, 1); + } + } + } + } + } + // If do not optimize the positions, set the camera positions to be constant if (!options_.optimize_positions) { - for (auto& [image_id, image] : images) - if (problem_->HasParameterBlock(image.cam_from_world.translation.data())) + for (auto& [frame_id, frame] : frames) { + if (!frame.HasPose()) continue; + if (problem_->HasParameterBlock(frame.RigFromWorld().translation.data())) problem_->SetParameterBlockConstant( - image.cam_from_world.translation.data()); + frame.RigFromWorld().translation.data()); + } } // If do not optimize the rotations, set the camera rotations to be constant @@ -360,11 +475,28 @@ void GlobalPositioner::ParameterizeVariables( // If do not optimize the scales, set the scales to be constant if (!options_.optimize_scales) { for (double& scale : scales_) { + if (problem_->HasParameterBlock(&scale)) { + problem_->SetParameterBlockConstant(&scale); + } + } + } + // Set the first rig scale to be constant to remove the gauge ambiguity. + for (double& scale : scales_) { + if (problem_->HasParameterBlock(&scale)) { + problem_->SetParameterBlockConstant(&scale); + break; + } + } + // Set the rig scales to be constant + // TODO: add a flag to allow the scales to be optimized (if they are not in + // metric scale) + for (auto& [rig_id, scale] : rig_scales_) { + if (problem_->HasParameterBlock(&scale)) { problem_->SetParameterBlockConstant(&scale); } } - int num_images = images.size(); + int num_images = frames.size(); #ifdef GLOMAP_CUDA_ENABLED bool cuda_solver_enabled = false; @@ -428,12 +560,32 @@ void GlobalPositioner::ParameterizeVariables( } void GlobalPositioner::ConvertResults( - std::unordered_map& images) { - // translation now stores the camera position, needs to convert back to - // translation - for (auto& [image_id, image] : images) { - image.cam_from_world.translation = - -(image.cam_from_world.rotation * image.cam_from_world.translation); + std::unordered_map& rigs, + std::unordered_map& frames) { + // translation now stores the camera position, needs to convert back + for (auto& [frame_id, frame] : frames) { + frame.RigFromWorld().translation = + -(frame.RigFromWorld().rotation * frame.RigFromWorld().translation); + + rig_t idx_rig = frame.RigId(); + frame.RigFromWorld().translation *= rig_scales_[idx_rig]; + } + + // Update the rig scales + for (auto& [rig_id, rig] : rigs) { + for (auto& [sensor_id, cam_from_rig] : rig.NonRefSensors()) { + if (cam_from_rig.has_value()) { + if (problem_->HasParameterBlock( + rig.SensorFromRig(sensor_id).translation.data())) { + cam_from_rig->translation = + -(cam_from_rig->rotation * cam_from_rig->translation); + } else { + // If the camera is part of a rig, then scale the translation + // by the rig scale + cam_from_rig->translation *= rig_scales_[rig_id]; + } + } + } } } diff --git a/glomap/estimators/global_positioning.h b/glomap/estimators/global_positioning.h index f318e8fa..f5f508d3 100644 --- a/glomap/estimators/global_positioning.h +++ b/glomap/estimators/global_positioning.h @@ -61,7 +61,9 @@ class GlobalPositioner { // failure. // Assume tracks here are already filtered bool Solve(const ViewGraph& view_graph, + std::unordered_map& rigs, std::unordered_map& cameras, + std::unordered_map& frames, std::unordered_map& images, std::unordered_map& tracks); @@ -69,10 +71,12 @@ class GlobalPositioner { protected: void SetupProblem(const ViewGraph& view_graph, + const std::unordered_map& rigs, const std::unordered_map& tracks); // Initialize all cameras to be random. void InitializeRandomPositions(const ViewGraph& view_graph, + std::unordered_map& frames, std::unordered_map& images, std::unordered_map& tracks); @@ -82,28 +86,35 @@ class GlobalPositioner { // Add tracks to the problem void AddPointToCameraConstraints( + std::unordered_map& rigs, std::unordered_map& cameras, + std::unordered_map& frames, std::unordered_map& images, std::unordered_map& tracks); // Add a single track to the problem void AddTrackToProblem(track_t track_id, + std::unordered_map& rigs, std::unordered_map& cameras, + std::unordered_map& frames, std::unordered_map& images, std::unordered_map& tracks); // Set the parameter groups void AddCamerasAndPointsToParameterGroups( - std::unordered_map& images, + std::unordered_map& rigs, + std::unordered_map& frames, std::unordered_map& tracks); // Parameterize the variables, set some variables to be constant if desired - void ParameterizeVariables(std::unordered_map& images, + void ParameterizeVariables(std::unordered_map& rigs, + std::unordered_map& frames, std::unordered_map& tracks); // During the optimization, the camera translation is set to be the camera // center Convert the results back to camera poses - void ConvertResults(std::unordered_map& images); + void ConvertResults(std::unordered_map& rigs, + std::unordered_map& frames); GlobalPositionerOptions options_; @@ -117,6 +128,8 @@ class GlobalPositioner { // Auxiliary scale variables. std::vector scales_; + + std::unordered_map rig_scales_; }; } // namespace glomap diff --git a/glomap/estimators/global_rotation_averaging.cc b/glomap/estimators/global_rotation_averaging.cc index e7122feb..c78ee1cc 100644 --- a/glomap/estimators/global_rotation_averaging.cc +++ b/glomap/estimators/global_rotation_averaging.cc @@ -1,5 +1,6 @@ #include "global_rotation_averaging.h" +#include "glomap/estimators/rotation_initializer.h" #include "glomap/math/l1_solver.h" #include "glomap/math/rigid3d.h" #include "glomap/math/tree.h" @@ -7,8 +8,11 @@ #include #include +#include "colmap/geometry/pose.h" + namespace glomap { namespace { + double RelAngleError(double angle_12, double angle_1, double angle_2) { double est = (angle_2 - angle_1) - angle_12; @@ -27,54 +31,61 @@ double RelAngleError(double angle_12, double angle_1, double angle_2) { return est; } + } // namespace bool RotationEstimator::EstimateRotations( - const ViewGraph& view_graph, std::unordered_map& images) { + const ViewGraph& view_graph, + std::unordered_map& rigs, + std::unordered_map& frames, + std::unordered_map& images) { + // Now, for the gravity aligned case, we only support the trivial rigs or rigs + // with known sensor_from_rig + if (options_.use_gravity) { + for (auto& [rig_id, rig] : rigs) { + for (auto& [sensor_id, sensor] : rig.NonRefSensors()) { + if (!sensor.has_value()) { + LOG(ERROR) << "Rig " << rig_id << " has no sensor with ID " + << sensor_id.id + << ", but the gravity aligned rotation is " + "requested. Please add the rig calibration."; + return false; + } + } + } + } // Initialize the rotation from maximum spanning tree if (!options_.skip_initialization && !options_.use_gravity) { - InitializeFromMaximumSpanningTree(view_graph, images); + InitializeFromMaximumSpanningTree(view_graph, rigs, frames, images); } // Set up the linear system - SetupLinearSystem(view_graph, images); + SetupLinearSystem(view_graph, rigs, frames, images); // Solve the linear system for L1 norm optimization if (options_.max_num_l1_iterations > 0) { - if (!SolveL1Regression(view_graph, images)) { + if (!SolveL1Regression(view_graph, frames, images)) { return false; } } // Solve the linear system for IRLS optimization if (options_.max_num_irls_iterations > 0) { - if (!SolveIRLS(view_graph, images)) { + if (!SolveIRLS(view_graph, frames, images)) { return false; } } - // Convert the final results - for (auto& [image_id, image] : images) { - if (!image.is_registered) continue; - - if (options_.use_gravity && image.gravity_info.has_gravity) { - image.cam_from_world.rotation = Eigen::Quaterniond( - image.gravity_info.GetRAlign() * - AngleToRotUp(rotation_estimated_[image_id_to_idx_[image_id]])); - } else { - image.cam_from_world.rotation = Eigen::Quaterniond(AngleAxisToRotation( - rotation_estimated_.segment(image_id_to_idx_[image_id], 3))); - } - // Restore the prior position (t = -Rc = R * R_ori * t_ori = R * t_ori) - image.cam_from_world.translation = - (image.cam_from_world.rotation * image.cam_from_world.translation); - } + ConvertResults(rigs, frames, images); return true; } void RotationEstimator::InitializeFromMaximumSpanningTree( - const ViewGraph& view_graph, std::unordered_map& images) { + const ViewGraph& view_graph, + std::unordered_map& rigs, + std::unordered_map& frames, + std::unordered_map& images) { // Here, we assume that largest connected component is already retrieved, so // we do not need to do that again compute maximum spanning tree. std::unordered_map parents; @@ -84,7 +95,7 @@ void RotationEstimator::InitializeFromMaximumSpanningTree( // Establish child info std::unordered_map> children; for (const auto& [image_id, image] : images) { - if (!image.is_registered) continue; + if (!image.IsRegistered()) continue; children.insert(std::make_pair(image_id, std::vector())); } for (auto& [child, parent] : parents) { @@ -95,6 +106,7 @@ void RotationEstimator::InitializeFromMaximumSpanningTree( std::queue indexes; indexes.push(root); + std::unordered_map cam_from_worlds; while (!indexes.empty()) { image_t curr = indexes.front(); indexes.pop(); @@ -109,61 +121,134 @@ void RotationEstimator::InitializeFromMaximumSpanningTree( ImagePair::ImagePairToPairId(curr, parents[curr])); if (image_pair.image_id1 == curr) { // 1_R_w = 2_R_1^T * 2_R_w - images[curr].cam_from_world.rotation = - (Inverse(image_pair.cam2_from_cam1) * - images[parents[curr]].cam_from_world) + cam_from_worlds[curr].rotation = + (Inverse(image_pair.cam2_from_cam1) * cam_from_worlds[parents[curr]]) .rotation; } else { // 2_R_w = 2_R_1 * 1_R_w - images[curr].cam_from_world.rotation = - (image_pair.cam2_from_cam1 * images[parents[curr]].cam_from_world) - .rotation; + cam_from_worlds[curr].rotation = + (image_pair.cam2_from_cam1 * cam_from_worlds[parents[curr]]).rotation; } } + + ConvertRotationsFromImageToRig(cam_from_worlds, images, rigs, frames); } +// TODO: refine the code void RotationEstimator::SetupLinearSystem( - const ViewGraph& view_graph, std::unordered_map& images) { + const ViewGraph& view_graph, + std::unordered_map& rigs, + std::unordered_map& frames, + std::unordered_map& images) { // Clear all the structures sparse_matrix_.resize(0, 0); tangent_space_step_.resize(0); tangent_space_residual_.resize(0); rotation_estimated_.resize(0); image_id_to_idx_.clear(); + camera_id_to_idx_.clear(); rel_temp_info_.clear(); // Initialize the structures for estimated rotation image_id_to_idx_.reserve(images.size()); + camera_id_to_idx_.reserve(images.size()); rotation_estimated_.resize( - 3 * images.size()); // allocate more memory than needed + 6 * images.size()); // allocate more memory than needed image_t num_dof = 0; - for (auto& [image_id, image] : images) { - if (!image.is_registered) continue; - image_id_to_idx_[image_id] = num_dof; - if (options_.use_gravity && image.gravity_info.has_gravity) { + std::unordered_map camera_id_to_rig_id; + for (auto& [frame_id, frame] : frames) { + for (const auto& data_id : frame.ImageIds()) { + image_t image_id = data_id.id; + if (images.find(image_id) == images.end()) continue; + const auto& image = images.at(image_id); + if (!image.IsRegistered()) continue; + camera_id_to_rig_id[image.camera_id] = frame.RigId(); + } + } + + // First, we need to determine which cameras need to be estimated + std::unordered_map cam_from_rig_rotations; + for (auto& [camera_id, rig_id] : camera_id_to_rig_id) { + sensor_t sensor_id(SensorType::CAMERA, camera_id); + if (rigs[rig_id].IsRefSensor(sensor_id)) continue; + + auto cam_from_rig = rigs[rig_id].MaybeSensorFromRig(sensor_id); + if (!cam_from_rig.has_value() || + cam_from_rig.value().translation.hasNaN()) { + if (camera_id_to_idx_.find(camera_id) == camera_id_to_idx_.end()) { + camera_id_to_idx_[camera_id] = -1; + if (cam_from_rig.has_value()) { + // If the camera is not part of a rig, then we can use the first image + // to initialize the rotation + cam_from_rig_rotations[camera_id] = + Rigid3dToAngleAxis(cam_from_rig.value()); + } + } + } + } + + for (auto& [frame_id, frame] : frames) { + // Skip the unregistered frames + if (frames[frame_id].is_registered == false) continue; + frame_id_to_idx_[frame_id] = num_dof; + image_t image_id_ref = -1; + for (auto& data_id : frame.ImageIds()) { + image_t image_id = data_id.id; + if (images.find(image_id) == images.end()) continue; + image_id_to_idx_[image_id] = num_dof; // point to the first element + if (images[image_id].HasTrivialFrame()) { + image_id_ref = image_id; + } + } + + if (options_.use_gravity && frame.gravity_info.has_gravity) { rotation_estimated_[num_dof] = - RotUpToAngle(image.gravity_info.GetRAlign().transpose() * - image.cam_from_world.rotation.toRotationMatrix()); + RotUpToAngle(frame.gravity_info.GetRAlign().transpose() * + frame.RigFromWorld().rotation.toRotationMatrix()); num_dof++; if (fixed_camera_id_ == -1) { fixed_camera_rotation_ = Eigen::Vector3d(0, rotation_estimated_[num_dof - 1], 0); - fixed_camera_id_ = image_id; + fixed_camera_id_ = image_id_ref; } } else { + if (!frame.MaybeRigFromWorld().has_value()) { + // Initialize the frame's rig from world to identity + frame.SetRigFromWorld(Rigid3d()); + } rotation_estimated_.segment(num_dof, 3) = - Rigid3dToAngleAxis(image.cam_from_world); + Rigid3dToAngleAxis(frame.RigFromWorld()); num_dof += 3; } } + // Set the camera id to index mapping for cameras that need to be + // estimated. + for (auto& [camera_id, camera_idx] : camera_id_to_idx_) { + // If the camera is not part of a rig, then we can use the first image + // to initialize the rotation + camera_id_to_idx_[camera_id] = num_dof; + if (cam_from_rig_rotations.find(camera_id) != + cam_from_rig_rotations.end()) { + rotation_estimated_.segment(num_dof, 3) = + cam_from_rig_rotations[camera_id]; + } else { + // If the camera is part of a rig, then we can use the rig rotation + // to initialize the rotation + rotation_estimated_.segment(num_dof, 3) = Eigen::Vector3d::Zero(); + } + num_dof += 3; + } + // If no cameras are set to be fixed, then take the first camera if (fixed_camera_id_ == -1) { - for (auto& [image_id, image] : images) { - if (!image.is_registered) continue; - fixed_camera_id_ = image_id; - fixed_camera_rotation_ = Rigid3dToAngleAxis(image.cam_from_world); + for (auto& [frame_id, frame] : frames) { + if (frames[frame_id].is_registered == false) continue; + + fixed_camera_id_ = frame.DataIds().begin()->id; + fixed_camera_rotation_ = Rigid3dToAngleAxis(frame.RigFromWorld()); + break; } } @@ -175,29 +260,69 @@ void RotationEstimator::SetupLinearSystem( for (auto& [pair_id, image_pair] : view_graph.image_pairs) { if (!image_pair.is_valid) continue; - int image_id1 = image_pair.image_id1; - int image_id2 = image_pair.image_id2; + image_t image_id1 = image_pair.image_id1; + image_t image_id2 = image_pair.image_id2; + + camera_t camera_id1 = images[image_id1].camera_id; + camera_t camera_id2 = images[image_id2].camera_id; + + int vector_idx1 = image_id_to_idx_[image_id1]; + int vector_idx2 = image_id_to_idx_[image_id2]; + + Rigid3d cam1_from_rig1, cam2_from_rig2; + int idx_rig1 = frames[images[image_id1].frame_id].RigId(); + int idx_rig2 = frames[images[image_id2].frame_id].RigId(); + + // int idx_camera1 = -1, idx_camera2 = -1; + bool has_sensor_from_rig1 = false; + bool has_sensor_from_rig2 = false; + if (!images[image_id1].HasTrivialFrame()) { + auto cam1_from_rig1_opt = rigs[idx_rig1].MaybeSensorFromRig( + sensor_t(SensorType::CAMERA, camera_id1)); + if (camera_id_to_idx_.find(camera_id1) == camera_id_to_idx_.end()) { + cam1_from_rig1 = cam1_from_rig1_opt.value(); + has_sensor_from_rig1 = true; + } + } + if (!images[image_id2].HasTrivialFrame()) { + auto cam2_from_rig2_opt = rigs[idx_rig2].MaybeSensorFromRig( + sensor_t(SensorType::CAMERA, camera_id2)); + if (camera_id_to_idx_.find(camera_id2) == camera_id_to_idx_.end()) { + cam2_from_rig2 = cam2_from_rig2_opt.value(); + has_sensor_from_rig2 = true; + } + } + + // If both images are from the same rig and there is no need to estimate + // the cam_from_rig, skip the estimation + if (has_sensor_from_rig1 && has_sensor_from_rig2 && + vector_idx1 == vector_idx2) { + continue; // Skip the self loop + } rel_temp_info_[pair_id].R_rel = - image_pair.cam2_from_cam1.rotation.toRotationMatrix(); + (cam2_from_rig2.rotation.inverse() * + image_pair.cam2_from_cam1.rotation * cam1_from_rig1.rotation) + .toRotationMatrix(); // Align the relative rotation to the gravity + bool has_gravity1 = images[image_id1].HasGravity(); + bool has_gravity2 = images[image_id2].HasGravity(); if (options_.use_gravity) { - if (images[image_id1].gravity_info.has_gravity) { + if (has_gravity1) { rel_temp_info_[pair_id].R_rel = rel_temp_info_[pair_id].R_rel * - images[image_id1].gravity_info.GetRAlign(); + images[image_id1].frame_ptr->gravity_info.GetRAlign(); } - if (images[image_id2].gravity_info.has_gravity) { + if (has_gravity2) { rel_temp_info_[pair_id].R_rel = - images[image_id2].gravity_info.GetRAlign().transpose() * + images[image_id2].frame_ptr->gravity_info.GetRAlign().transpose() * rel_temp_info_[pair_id].R_rel; } } - if (options_.use_gravity && images[image_id1].gravity_info.has_gravity && - images[image_id2].gravity_info.has_gravity) { + if (options_.use_gravity && has_gravity1 && has_gravity2) { counter++; Eigen::Vector3d aa = RotationToAngleAxis(rel_temp_info_[pair_id].R_rel); double error = aa[0] * aa[0] + aa[2] * aa[2]; @@ -223,15 +348,39 @@ void RotationEstimator::SetupLinearSystem( weights.reserve(3 * view_graph.image_pairs.size()); for (const auto& [pair_id, image_pair] : view_graph.image_pairs) { if (!image_pair.is_valid) continue; + if (rel_temp_info_.find(pair_id) == rel_temp_info_.end()) continue; + + image_t image_id1 = image_pair.image_id1; + image_t image_id2 = image_pair.image_id2; - int image_id1 = image_pair.image_id1; - int image_id2 = image_pair.image_id2; + camera_t camera_id1 = images[image_id1].camera_id; + camera_t camera_id2 = images[image_id2].camera_id; + + frame_t frame_id1 = images[image_id1].frame_id; + frame_t frame_id2 = images[image_id2].frame_id; + + if (frames[frame_id1].is_registered == false || + frames[frame_id2].is_registered == false) { + continue; // skip unregistered frames + } int vector_idx1 = image_id_to_idx_[image_id1]; int vector_idx2 = image_id_to_idx_[image_id2]; + int vector_idx_cam1 = -1; + int vector_idx_cam2 = -1; + if (camera_id_to_idx_.find(camera_id1) != camera_id_to_idx_.end()) { + vector_idx_cam1 = camera_id_to_idx_[camera_id1]; + } + if (camera_id_to_idx_.find(camera_id2) != camera_id_to_idx_.end()) { + vector_idx_cam2 = camera_id_to_idx_[camera_id2]; + } + rel_temp_info_[pair_id].index = curr_pos; + rel_temp_info_[pair_id].idx_cam1 = vector_idx_cam1; + rel_temp_info_[pair_id].idx_cam2 = vector_idx_cam2; + // TODO: figure out the logic for the gravity aligned case if (rel_temp_info_[pair_id].has_gravity) { coeffs.emplace_back(Eigen::Triplet(curr_pos, vector_idx1, -1)); coeffs.emplace_back(Eigen::Triplet(curr_pos, vector_idx2, 1)); @@ -242,8 +391,7 @@ void RotationEstimator::SetupLinearSystem( curr_pos++; } else { // If it is not gravity aligned, then we need to consider 3 dof - if (!options_.use_gravity || - !images[image_id1].gravity_info.has_gravity) { + if (!options_.use_gravity || !images[image_id1].HasGravity()) { for (int i = 0; i < 3; i++) { coeffs.emplace_back( Eigen::Triplet(curr_pos + i, vector_idx1 + i, -1)); @@ -254,8 +402,7 @@ void RotationEstimator::SetupLinearSystem( Eigen::Triplet(curr_pos + 1, vector_idx1, -1)); // Similarly for the second componenet - if (!options_.use_gravity || - !images[image_id2].gravity_info.has_gravity) { + if (!options_.use_gravity || !images[image_id2].HasGravity()) { for (int i = 0; i < 3; i++) { coeffs.emplace_back( Eigen::Triplet(curr_pos + i, vector_idx2 + i, 1)); @@ -270,6 +417,25 @@ void RotationEstimator::SetupLinearSystem( weights.emplace_back(1); } + // If both camera share the same rig, the terms in the linear system would + // be cancelled + if (!(vector_idx_cam1 == -1 && vector_idx_cam2 == -1)) { + if (vector_idx_cam1 != -1) { + // If the camera is not part of a rig, then we can use the first image + // to initialize the rotation + for (int i = 0; i < 3; i++) { + coeffs.emplace_back( + Eigen::Triplet(curr_pos + i, vector_idx_cam1 + i, -1)); + } + } + if (vector_idx_cam2 != -1) { + for (int i = 0; i < 3; i++) { + coeffs.emplace_back( + Eigen::Triplet(curr_pos + i, vector_idx_cam2 + i, 1)); + } + } + } + curr_pos += 3; } } @@ -277,8 +443,7 @@ void RotationEstimator::SetupLinearSystem( // Set some cameras to be fixed // if some cameras have gravity, then add a single term constraint // Else, change to 3 constriants - if (options_.use_gravity && - images[fixed_camera_id_].gravity_info.has_gravity) { + if (options_.use_gravity && images[fixed_camera_id_].HasGravity()) { coeffs.emplace_back(Eigen::Triplet( curr_pos, image_id_to_idx_[fixed_camera_id_], 1)); weights.emplace_back(1); @@ -309,7 +474,9 @@ void RotationEstimator::SetupLinearSystem( } bool RotationEstimator::SolveL1Regression( - const ViewGraph& view_graph, std::unordered_map& images) { + const ViewGraph& view_graph, + std::unordered_map& frames, + std::unordered_map& images) { L1SolverOptions opt_l1_solver; opt_l1_solver.max_num_iterations = 10; @@ -346,12 +513,12 @@ bool RotationEstimator::SolveL1Regression( .sum(); curr_norm = tangent_space_step_.norm(); - UpdateGlobalRotations(view_graph, images); + UpdateGlobalRotations(view_graph, frames, images); ComputeResiduals(view_graph, images); // Check the residual. If it is small, stop // TODO: strange bug for the L1 solver: update norm state constant - if (ComputeAverageStepSize(images) < + if (ComputeAverageStepSize(frames) < options_.l1_step_convergence_threshold || std::abs(last_norm - curr_norm) < EPS) { if (std::abs(last_norm - curr_norm) < EPS) @@ -367,13 +534,11 @@ bool RotationEstimator::SolveL1Regression( } bool RotationEstimator::SolveIRLS(const ViewGraph& view_graph, + std::unordered_map& frames, std::unordered_map& images) { // TODO: Determine what is the best solver for this part Eigen::CholmodSupernodalLLT> llt; - // weight_matrix.setIdentity(); - // sparse_matrix_ = A_ori; - llt.analyzePattern(sparse_matrix_.transpose() * sparse_matrix_); const double sigma = DegToRad(options_.irls_loss_parameter_sigma); @@ -382,7 +547,7 @@ bool RotationEstimator::SolveIRLS(const ViewGraph& view_graph, Eigen::ArrayXd weights_irls(sparse_matrix_.rows()); Eigen::SparseMatrix at_weight; - if (options_.use_gravity && images[fixed_camera_id_].gravity_info.has_gravity) + if (options_.use_gravity && images[fixed_camera_id_].HasGravity()) weights_irls[sparse_matrix_.rows() - 1] = 1; else weights_irls.segment(sparse_matrix_.rows() - 3, 3).setConstant(1); @@ -437,11 +602,11 @@ bool RotationEstimator::SolveIRLS(const ViewGraph& view_graph, // Solve the least squares problem.. tangent_space_step_.setZero(); tangent_space_step_ = llt.solve(at_weight * tangent_space_residual_); - UpdateGlobalRotations(view_graph, images); + UpdateGlobalRotations(view_graph, frames, images); ComputeResiduals(view_graph, images); // Check the residual. If it is small, stop - if (ComputeAverageStepSize(images) < + if (ComputeAverageStepSize(frames) < options_.irls_step_convergence_threshold) { iteration++; break; @@ -453,12 +618,13 @@ bool RotationEstimator::SolveIRLS(const ViewGraph& view_graph, } void RotationEstimator::UpdateGlobalRotations( - const ViewGraph& view_graph, std::unordered_map& images) { - for (const auto& [image_id, image] : images) { - if (!image.is_registered) continue; - - image_t vector_idx = image_id_to_idx_[image_id]; - if (!(options_.use_gravity && image.gravity_info.has_gravity)) { + const ViewGraph& view_graph, + std::unordered_map& frames, + std::unordered_map& images) { + for (auto& [frame_id, frame] : frames) { + if (frames[frame_id].is_registered == false) continue; + image_t vector_idx = frame_id_to_idx_[frame_id]; + if (!(options_.use_gravity && frame.HasGravity())) { Eigen::Matrix3d R_ori = AngleAxisToRotation(rotation_estimated_.segment(vector_idx, 3)); @@ -469,6 +635,55 @@ void RotationEstimator::UpdateGlobalRotations( rotation_estimated_[vector_idx] -= tangent_space_step_[vector_idx]; } } + + std::unordered_map> cam_from_rigs; + for (auto& [camera_id, camera_idx] : camera_id_to_idx_) { + cam_from_rigs[camera_id] = std::vector(); + } + for (auto& [frame_id, frame] : frames) { + if (frames.at(frame_id).is_registered == false) continue; + // Update the rig from world for the frame + Eigen::Matrix3d R_ori; + if (!options_.use_gravity || !frame.HasGravity()) { + R_ori = AngleAxisToRotation( + rotation_estimated_.segment(frame_id_to_idx_[frame_id], 3)); + } else { + R_ori = AngleToRotUp(rotation_estimated_[frame_id_to_idx_[frame_id]]); + } + + // Update the cam_from_rig for the cameras in the frame + for (const auto& data_id : frame.ImageIds()) { + image_t image_id = data_id.id; + if (images.find(image_id) == images.end()) continue; + const auto& image = images.at(image_id); + if (camera_id_to_idx_.find(image.camera_id) != camera_id_to_idx_.end()) { + cam_from_rigs[image.camera_id].push_back(R_ori); + } + } + } + + // Update the global rotations for cam_from_rig cameras + // Note: the update is non trivial, and we need to average the rotations from + // all the frames + for (auto& [camera_id, camera_idx] : camera_id_to_idx_) { + Eigen::Matrix3d R_ori = + AngleAxisToRotation(rotation_estimated_.segment(camera_idx, 3)); + + std::vector rig_rotations; + Eigen::Matrix3d R_update = + AngleAxisToRotation(-tangent_space_step_.segment(camera_idx, 3)); + for (const auto& R : cam_from_rigs[camera_id]) { + // Update the rotation for the camera + rig_rotations.push_back( + Eigen::Quaterniond(R_ori * R * R_update * R.transpose())); + } + // Average the rotations for the rig + Eigen::Quaterniond R_ave = colmap::AverageQuaternions( + rig_rotations, std::vector(rig_rotations.size(), 1)); + + rotation_estimated_.segment(camera_idx, 3) = + RotationToAngleAxis(R_ave.toRotationMatrix()); + } } void RotationEstimator::ComputeResiduals( @@ -478,8 +693,11 @@ void RotationEstimator::ComputeResiduals( image_t image_id1 = view_graph.image_pairs.at(pair_id).image_id1; image_t image_id2 = view_graph.image_pairs.at(pair_id).image_id2; - image_t idx1 = image_id_to_idx_[image_id1]; - image_t idx2 = image_id_to_idx_[image_id2]; + int idx1 = image_id_to_idx_[image_id1]; + int idx2 = image_id_to_idx_[image_id2]; + + int idx_cam1 = pair_info.idx_cam1; + int idx_cam2 = pair_info.idx_cam2; if (pair_info.has_gravity) { tangent_space_residual_[pair_info.index] = @@ -488,26 +706,37 @@ void RotationEstimator::ComputeResiduals( rotation_estimated_[image_id_to_idx_[image_id2]])); } else { Eigen::Matrix3d R_1, R_2; - if (options_.use_gravity && images[image_id1].gravity_info.has_gravity) { + if (options_.use_gravity && images[image_id1].HasGravity()) { R_1 = AngleToRotUp(rotation_estimated_[image_id_to_idx_[image_id1]]); } else { R_1 = AngleAxisToRotation( rotation_estimated_.segment(image_id_to_idx_[image_id1], 3)); } - if (options_.use_gravity && images[image_id2].gravity_info.has_gravity) { + if (options_.use_gravity && images[image_id2].HasGravity()) { R_2 = AngleToRotUp(rotation_estimated_[image_id_to_idx_[image_id2]]); } else { R_2 = AngleAxisToRotation( rotation_estimated_.segment(image_id_to_idx_[image_id2], 3)); } + if (idx_cam1 != -1) { + // If the camera is not part of a rig, then we can use the first image + // to initialize the rotation + R_1 = + AngleAxisToRotation(rotation_estimated_.segment(idx_cam1, 3)) * R_1; + } + if (idx_cam2 != -1) { + R_2 = + AngleAxisToRotation(rotation_estimated_.segment(idx_cam2, 3)) * R_2; + } + tangent_space_residual_.segment(pair_info.index, 3) = -RotationToAngleAxis(R_2.transpose() * pair_info.R_rel * R_1); } } - if (options_.use_gravity && images[fixed_camera_id_].gravity_info.has_gravity) + if (options_.use_gravity && images[fixed_camera_id_].HasGravity()) tangent_space_residual_[tangent_space_residual_.size() - 1] = rotation_estimated_[image_id_to_idx_[fixed_camera_id_]] - fixed_camera_rotation_[1]; @@ -520,19 +749,63 @@ void RotationEstimator::ComputeResiduals( } double RotationEstimator::ComputeAverageStepSize( - const std::unordered_map& images) { + const std::unordered_map& frames) { double total_update = 0; - for (const auto& [image_id, image] : images) { - if (!image.is_registered) continue; + for (const auto& [frame_id, frame] : frames) { + if (frames.at(frame_id).is_registered) continue; - if (options_.use_gravity && image.gravity_info.has_gravity) { - total_update += std::abs(tangent_space_step_[image_id_to_idx_[image_id]]); + if (options_.use_gravity && frame.HasGravity()) { + total_update += std::abs(tangent_space_step_[frame_id_to_idx_[frame_id]]); } else { total_update += - tangent_space_step_.segment(image_id_to_idx_[image_id], 3).norm(); + tangent_space_step_.segment(frame_id_to_idx_[frame_id], 3).norm(); + } + } + return total_update / frame_id_to_idx_.size(); +} + +void RotationEstimator::ConvertResults( + std::unordered_map& rigs, + std::unordered_map& frames, + std::unordered_map& images) { + for (auto& [frame_id, frame] : frames) { + if (frames[frame_id].is_registered == false) continue; + + image_t image_id_begin = frame.DataIds().begin()->id; + + // Set the rig from world rotation + // If the frame has gravity, then use the first image's gravity + bool use_gravity = options_.use_gravity && frame.HasGravity(); + + if (use_gravity) { + frame.SetRigFromWorld(Rigid3d( + Eigen::Quaterniond( + frame.gravity_info.GetRAlign() * + AngleToRotUp( + rotation_estimated_[image_id_to_idx_[image_id_begin]])), + Eigen::Vector3d::Zero())); + } else { + frame.SetRigFromWorld(Rigid3d( + Eigen::Quaterniond(AngleAxisToRotation(rotation_estimated_.segment( + image_id_to_idx_[image_id_begin], 3))), + Eigen::Vector3d::Zero())); + } + } + + // add the estimated + for (auto& [rig_id, rig] : rigs) { + for (auto& [sensor_id, sensor] : rig.NonRefSensors()) { + if (camera_id_to_idx_.find(sensor_id.id) == camera_id_to_idx_.end()) { + continue; // Skip cameras that are not estimated + } + Rigid3d cam_from_rig; + cam_from_rig.rotation = AngleAxisToRotation( + rotation_estimated_.segment(camera_id_to_idx_[sensor_id.id], 3)); + cam_from_rig.translation.setConstant( + std::numeric_limits::quiet_NaN()); // No translation yet + rig.SetSensorFromRig(sensor_id, cam_from_rig); } } - return total_update / image_id_to_idx_.size(); } -} // namespace glomap \ No newline at end of file +} // namespace glomap diff --git a/glomap/estimators/global_rotation_averaging.h b/glomap/estimators/global_rotation_averaging.h index 599c7c5f..fa0bdab4 100644 --- a/glomap/estimators/global_rotation_averaging.h +++ b/glomap/estimators/global_rotation_averaging.h @@ -30,6 +30,9 @@ struct ImagePairTempInfo { // angle_rel is the converted angle if gravity prior is available for both // images double angle_rel = 0; + + int idx_cam1 = -1; // index of the first camera in the rig + int idx_cam2 = -1; // index of the second camera in the rig }; struct RotationEstimatorOptions { @@ -70,10 +73,6 @@ struct RotationEstimatorOptions { bool use_gravity = false; }; -// TODO: Implement the stratified camera rotation estimation -// TODO: Implement the HALF_NORM loss for IRLS -// TODO: Implement the weighted version for rotation averaging -// TODO: Implement the gravity as prior for rotation averaging class RotationEstimator { public: explicit RotationEstimator(const RotationEstimatorOptions& options) @@ -82,30 +81,40 @@ class RotationEstimator { // Estimates the global orientations of all views based on an initial // guess. Returns true on successful estimation and false otherwise. bool EstimateRotations(const ViewGraph& view_graph, + std::unordered_map& rigs, + std::unordered_map& frames, std::unordered_map& images); protected: // Initialize the rotation from the maximum spanning tree // Number of inliers serve as weights void InitializeFromMaximumSpanningTree( - const ViewGraph& view_graph, std::unordered_map& images); + const ViewGraph& view_graph, + std::unordered_map& rigs, + std::unordered_map& frames, + std::unordered_map& images); // Sets up the sparse linear system such that dR_ij = dR_j - dR_i. This is the // first-order approximation of the angle-axis rotations. This should only be // called once. void SetupLinearSystem(const ViewGraph& view_graph, + std::unordered_map& rigs, + std::unordered_map& frames, std::unordered_map& images); // Performs the L1 robust loss minimization. bool SolveL1Regression(const ViewGraph& view_graph, + std::unordered_map& frames, std::unordered_map& images); // Performs the iteratively reweighted least squares. bool SolveIRLS(const ViewGraph& view_graph, + std::unordered_map& frames, std::unordered_map& images); // Updates the global rotations based on the current rotation change. void UpdateGlobalRotations(const ViewGraph& view_graph, + std::unordered_map& frames, std::unordered_map& images); // Computes the relative rotation (tangent space) residuals based on the @@ -117,7 +126,13 @@ class RotationEstimator { // The is the average over all non-fixed global_orientations_ of their // rotation magnitudes. double ComputeAverageStepSize( - const std::unordered_map& images); + const std::unordered_map& frames); + + // Converts the results from the tangent space to the global rotations and + // updates the frames and images with the new rotations. + void ConvertResults(std::unordered_map& rigs, + std::unordered_map& frames, + std::unordered_map& images); // Data // Options for the solver. @@ -136,7 +151,10 @@ class RotationEstimator { Eigen::VectorXd rotation_estimated_; // Varaibles for intermidiate results - std::unordered_map image_id_to_idx_; + std::unordered_map image_id_to_idx_; + std::unordered_map frame_id_to_idx_; + std::unordered_map + camera_id_to_idx_; // Note: for reference cameras, it does not have this std::unordered_map rel_temp_info_; // The fixed camera id. This is used to remove the ambiguity of the linear diff --git a/glomap/estimators/gravity_refinement.cc b/glomap/estimators/gravity_refinement.cc index 0f679bad..bb19e28d 100644 --- a/glomap/estimators/gravity_refinement.cc +++ b/glomap/estimators/gravity_refinement.cc @@ -7,6 +7,7 @@ namespace glomap { void GravityRefiner::RefineGravity(const ViewGraph& view_graph, + std::unordered_map& frames, std::unordered_map& images) { const std::unordered_map& image_pairs = view_graph.image_pairs; @@ -19,26 +20,36 @@ void GravityRefiner::RefineGravity(const ViewGraph& view_graph, // Identify the images that are error prone int counter_rect = 0; - std::unordered_set error_prone_images; - IdentifyErrorProneGravity(view_graph, images, error_prone_images); + std::unordered_set error_prone_frames; + IdentifyErrorProneGravity(view_graph, frames, images, error_prone_frames); - if (error_prone_images.empty()) { - LOG(INFO) << "No error prone images found" << std::endl; + if (error_prone_frames.empty()) { + LOG(INFO) << "No error prone frames found" << std::endl; return; } + // Get the relevant pair ids for frames + std::unordered_map> + adjacency_list_frames_to_pair_id; + for (auto& [image_id, neighbors] : adjacency_list) { + for (auto neighbor : neighbors) { + adjacency_list_frames_to_pair_id[images[image_id].frame_id].insert( + ImagePair::ImagePairToPairId(image_id, neighbor)); + } + } loss_function_ = options_.CreateLossFunction(); int counter_progress = 0; // Iterate through the error prone images - for (auto image_id : error_prone_images) { + for (auto frame_id : error_prone_frames) { if ((counter_progress + 1) % 10 == 0 || - counter_progress == error_prone_images.size() - 1) { - std::cout << "\r Refining image " << counter_progress + 1 << " / " - << error_prone_images.size() << std::flush; + counter_progress == error_prone_frames.size() - 1) { + std::cout << "\r Refining frame " << counter_progress + 1 << " / " + << error_prone_frames.size() << std::flush; } counter_progress++; - const std::unordered_set& neighbors = adjacency_list.at(image_id); + const std::unordered_set& neighbors = + adjacency_list_frames_to_pair_id.at(frame_id); std::vector gravities; gravities.reserve(neighbors.size()); @@ -46,28 +57,42 @@ void GravityRefiner::RefineGravity(const ViewGraph& view_graph, problem_options.loss_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; ceres::Problem problem(problem_options); int counter = 0; - Eigen::Vector3d gravity = images[image_id].gravity_info.GetGravity(); - for (const auto& neighbor : neighbors) { - image_pair_t pair_id = ImagePair::ImagePairToPairId(image_id, neighbor); - + Eigen::Vector3d gravity = frames[frame_id].gravity_info.GetGravity(); + for (const auto& pair_id : neighbors) { image_t image_id1 = image_pairs.at(pair_id).image_id1; image_t image_id2 = image_pairs.at(pair_id).image_id2; - if (images.at(image_id1).gravity_info.has_gravity == false || - images.at(image_id2).gravity_info.has_gravity == false) + if (!images.at(image_id1).HasGravity() || + !images.at(image_id2).HasGravity()) continue; - if (image_id1 == image_id) { - gravities.emplace_back((image_pairs.at(pair_id) - .cam2_from_cam1.rotation.toRotationMatrix() - .transpose() * - images[image_id2].gravity_info.GetRAlign()) - .col(1)); - } else { + // Get the cam_from_rig + Rigid3d cam1_from_rig1, cam2_from_rig2; + if (!images.at(image_id1).HasTrivialFrame()) { + cam1_from_rig1 = + images.at(image_id1).frame_ptr->RigPtr()->SensorFromRig( + sensor_t(SensorType::CAMERA, images.at(image_id1).camera_id)); + } + if (!images.at(image_id2).HasTrivialFrame()) { + cam2_from_rig2 = + images.at(image_id2).frame_ptr->RigPtr()->SensorFromRig( + sensor_t(SensorType::CAMERA, images.at(image_id2).camera_id)); + } + + // Note: for the case where both cameras are from the same frames, we only + // consider a single cost term + if (images.at(image_id1).frame_id == frame_id) { gravities.emplace_back( - (image_pairs.at(pair_id) - .cam2_from_cam1.rotation.toRotationMatrix() * - images[image_id1].gravity_info.GetRAlign()) + (colmap::Inverse(image_pairs.at(pair_id).cam2_from_cam1 * + cam1_from_rig1) + .rotation.toRotationMatrix() * + images[image_id2].GetRAlign()) .col(1)); + } else if (images.at(image_id2).frame_id == frame_id) { + gravities.emplace_back(((colmap::Inverse(cam2_from_rig2) * + image_pairs.at(pair_id).cam2_from_cam1) + .rotation.toRotationMatrix() * + images[image_id1].GetRAlign()) + .col(1)); } ceres::CostFunction* coor_cost = @@ -95,67 +120,66 @@ void GravityRefiner::RefineGravity(const ViewGraph& view_graph, if (double(counter_outlier) / double(gravities.size()) < options_.max_outlier_ratio) { counter_rect++; - images[image_id].gravity_info.SetGravity(gravity); + frames[frame_id].gravity_info.SetGravity(gravity); } } std::cout << std::endl; - LOG(INFO) << "Number of rectified images: " << counter_rect << " / " - << error_prone_images.size() << std::endl; + LOG(INFO) << "Number of rectified frames: " << counter_rect << " / " + << error_prone_frames.size() << std::endl; } void GravityRefiner::IdentifyErrorProneGravity( const ViewGraph& view_graph, + const std::unordered_map& frames, const std::unordered_map& images, - std::unordered_set& error_prone_images) { - error_prone_images.clear(); + std::unordered_set& error_prone_frames) { + error_prone_frames.clear(); // image_id: (mistake, total) - std::unordered_map> image_counter; + std::unordered_map> frame_counter; + frame_counter.reserve(frames.size()); // Set the counter of all images to 0 - for (const auto& [image_id, image] : images) { - image_counter[image_id] = std::make_pair(0, 0); + for (const auto& [frame_id, frame] : frames) { + frame_counter[frame_id] = std::make_pair(0, 0); } for (const auto& [pair_id, image_pair] : view_graph.image_pairs) { if (!image_pair.is_valid) continue; const auto& image1 = images.at(image_pair.image_id1); const auto& image2 = images.at(image_pair.image_id2); - if (image1.gravity_info.has_gravity && image2.gravity_info.has_gravity) { + + if (image1.HasGravity() && image2.HasGravity()) { // Calculate the gravity aligned relative rotation const Eigen::Matrix3d R_rel = - image2.gravity_info.GetRAlign().transpose() * + image2.GetRAlign().transpose() * image_pair.cam2_from_cam1.rotation.toRotationMatrix() * - image1.gravity_info.GetRAlign(); + image1.GetRAlign(); // Convert it to the closest upright rotation const Eigen::Matrix3d R_rel_up = AngleToRotUp(RotUpToAngle(R_rel)); const double angle = CalcAngle(R_rel, R_rel_up); // increment the total count - image_counter[image_pair.image_id1].second++; - image_counter[image_pair.image_id2].second++; + frame_counter[image1.frame_id].second++; + frame_counter[image2.frame_id].second++; // increment the mistake count if (angle > options_.max_gravity_error) { - image_counter[image_pair.image_id1].first++; - image_counter[image_pair.image_id2].first++; + frame_counter[image1.frame_id].first++; + frame_counter[image2.frame_id].first++; } } } - const std::unordered_map>& - adjacency_list = view_graph.GetAdjacencyList(); - // Filter the images with too many mistakes - for (auto& [image_id, counter] : image_counter) { - if (images.at(image_id).gravity_info.has_gravity == false) continue; + for (const auto& [frame_id, counter] : frame_counter) { if (counter.second < options_.min_num_neighbors) continue; if (double(counter.first) / double(counter.second) >= options_.max_outlier_ratio) { - error_prone_images.insert(image_id); + error_prone_frames.insert(frame_id); } } - LOG(INFO) << "Number of error prone images: " << error_prone_images.size() + LOG(INFO) << "Number of error prone frames: " << error_prone_frames.size() << std::endl; } } // namespace glomap diff --git a/glomap/estimators/gravity_refinement.h b/glomap/estimators/gravity_refinement.h index b9667155..d05a7cd8 100644 --- a/glomap/estimators/gravity_refinement.h +++ b/glomap/estimators/gravity_refinement.h @@ -29,11 +29,13 @@ class GravityRefiner { public: GravityRefiner(const GravityRefinerOptions& options) : options_(options) {} void RefineGravity(const ViewGraph& view_graph, + std::unordered_map& frames, std::unordered_map& images); private: void IdentifyErrorProneGravity( const ViewGraph& view_graph, + const std::unordered_map& frames, const std::unordered_map& images, std::unordered_set& error_prone_images); GravityRefinerOptions options_; diff --git a/glomap/estimators/relpose_estimation.cc b/glomap/estimators/relpose_estimation.cc index 8cd3b380..0a8b5fc0 100644 --- a/glomap/estimators/relpose_estimation.cc +++ b/glomap/estimators/relpose_estimation.cc @@ -70,8 +70,10 @@ void EstimateRelativePoses(ViewGraph& view_graph, K2_new(0, 0) = camera2.FocalLengthX(); K2_new(1, 1) = camera2.FocalLengthY(); for (size_t idx = 0; idx < matches.rows(); idx++) { - points2D_1[idx] = K1_new * camera1.CamFromImg(points2D_1[idx]); - points2D_2[idx] = K2_new * camera2.CamFromImg(points2D_2[idx]); + points2D_1[idx] = K1_new * camera1.CamFromImg(points2D_1[idx]) + .value_or(Eigen::Vector2d::Zero()); + points2D_2[idx] = K2_new * camera2.CamFromImg(points2D_2[idx]) + .value_or(Eigen::Vector2d::Zero()); } // Reset the camera to be the pinhole camera with original focal diff --git a/glomap/estimators/rotation_initializer.cc b/glomap/estimators/rotation_initializer.cc new file mode 100644 index 00000000..3d1ca90e --- /dev/null +++ b/glomap/estimators/rotation_initializer.cc @@ -0,0 +1,126 @@ +#include "glomap/estimators/rotation_initializer.h" + +#include "colmap/geometry/pose.h" +namespace glomap { + +bool ConvertRotationsFromImageToRig( + const std::unordered_map& cam_from_worlds, + const std::unordered_map& images, + std::unordered_map& rigs, + std::unordered_map& frames) { + std::unordered_map camera_id_to_rig_id; + for (auto& [rig_id, rig] : rigs) { + for (auto& [sensor_id, sensor] : rig.NonRefSensors()) { + if (sensor_id.type != SensorType::CAMERA) continue; + camera_id_to_rig_id[sensor_id.id] = rig_id; + } + } + + std::unordered_map> + cam_from_ref_cam_rotations; + + std::unordered_map frame_to_ref_image_id; + for (auto& [frame_id, frame] : frames) { + // First, figure out the reference camera in the frame + image_t ref_img_id = -1; + for (const auto& data_id : frame.ImageIds()) { + image_t image_id = data_id.id; + if (images.find(image_id) == images.end()) continue; + const auto& image = images.at(image_id); + if (!image.IsRegistered()) continue; + + if (image.camera_id == frame.RigPtr()->RefSensorId().id) { + ref_img_id = image_id; + frame_to_ref_image_id[frame_id] = ref_img_id; + break; + } + } + + // If the reference image is not found, then skip the frame + if (ref_img_id == -1) { + continue; + } + + // Then, collect the rotations from the cameras to the reference camera + for (const auto& data_id : frame.ImageIds()) { + image_t image_id = data_id.id; + if (images.find(image_id) == images.end()) continue; + const auto& image = images.at(image_id); + if (!image.IsRegistered()) continue; + + Rig* rig_ptr = frame.RigPtr(); + + // If the camera is a reference camera, then skip it + if (image.camera_id == rig_ptr->RefSensorId().id) continue; + + if (rig_ptr + ->MaybeSensorFromRig( + sensor_t(SensorType::CAMERA, image.camera_id)) + .has_value()) + continue; + + if (cam_from_ref_cam_rotations.find(image.camera_id) == + cam_from_ref_cam_rotations.end()) + cam_from_ref_cam_rotations[image.camera_id] = + std::vector(); + + // Set the rotation from the camera to the world + cam_from_ref_cam_rotations[image.camera_id].push_back( + cam_from_worlds.at(image_id).rotation * + cam_from_worlds.at(ref_img_id).rotation.inverse()); + } + } + + Eigen::Vector3d nan_translation; + nan_translation.setConstant(std::numeric_limits::quiet_NaN()); + + // Use the average of the rotations to set the rotation from the camera + for (auto& [camera_id, cam_from_ref_cam_rotations_i] : + cam_from_ref_cam_rotations) { + const std::vector weights(cam_from_ref_cam_rotations_i.size(), 1.0); + Eigen::Quaterniond cam_from_ref_cam_rotation = + colmap::AverageQuaternions(cam_from_ref_cam_rotations_i, weights); + + rigs[camera_id_to_rig_id[camera_id]].SetSensorFromRig( + sensor_t(SensorType::CAMERA, camera_id), + Rigid3d(cam_from_ref_cam_rotation, nan_translation)); + } + + // Then, collect the rotations into frames and rigs + for (auto& [frame_id, frame] : frames) { + // Then, collect the rotations from the cameras to the reference camera + std::vector rig_from_world_rotations; + for (const auto& data_id : frame.ImageIds()) { + image_t image_id = data_id.id; + if (images.find(image_id) == images.end()) continue; + const auto& image = images.at(image_id); + if (!image.IsRegistered()) continue; + + // For images that not estimated directly, we need to skip it + if (cam_from_worlds.find(image_id) == cam_from_worlds.end()) continue; + + if (image_id == frame_to_ref_image_id[frame_id]) { + rig_from_world_rotations.push_back( + cam_from_worlds.at(image_id).rotation); + } else { + auto cam_from_rig_opt = + rigs[camera_id_to_rig_id[image.camera_id]].MaybeSensorFromRig( + sensor_t(SensorType::CAMERA, image.camera_id)); + if (!cam_from_rig_opt.has_value()) continue; + rig_from_world_rotations.push_back( + cam_from_rig_opt.value().rotation.inverse() * + cam_from_worlds.at(image_id).rotation); + } + + const std::vector rotation_weights( + rig_from_world_rotations.size(), 1); + Eigen::Quaterniond rig_from_world_rotation = colmap::AverageQuaternions( + rig_from_world_rotations, rotation_weights); + frame.SetRigFromWorld(Rigid3d(rig_from_world_rotation, nan_translation)); + } + } + + return true; +} + +} // namespace glomap diff --git a/glomap/estimators/rotation_initializer.h b/glomap/estimators/rotation_initializer.h new file mode 100644 index 00000000..bc470ded --- /dev/null +++ b/glomap/estimators/rotation_initializer.h @@ -0,0 +1,14 @@ +#pragma once + +#include "glomap/scene/types_sfm.h" + +namespace glomap { + +// Initialize the rotations of the rigs from the images +bool ConvertRotationsFromImageToRig( + const std::unordered_map& cam_from_worlds, + const std::unordered_map& images, + std::unordered_map& rigs, + std::unordered_map& frames); + +} // namespace glomap \ No newline at end of file diff --git a/glomap/exe/global_mapper.cc b/glomap/exe/global_mapper.cc index fb1e512c..f099f8bf 100644 --- a/glomap/exe/global_mapper.cc +++ b/glomap/exe/global_mapper.cc @@ -2,6 +2,7 @@ #include "glomap/controllers/option_manager.h" #include "glomap/io/colmap_io.h" +#include "glomap/io/pose_io.h" #include "glomap/types.h" #include @@ -63,12 +64,14 @@ int RunMapper(int argc, char** argv) { // Load the database ViewGraph view_graph; + std::unordered_map rigs; std::unordered_map cameras; + std::unordered_map frames; std::unordered_map images; std::unordered_map tracks; - const colmap::Database database(database_path); - ConvertDatabaseToGlomap(database, view_graph, cameras, images); + auto database = colmap::Database::Open(database_path); + ConvertDatabaseToGlomap(*database, view_graph, rigs, cameras, frames, images); if (view_graph.image_pairs.empty()) { LOG(ERROR) << "Can't continue without image pairs"; @@ -81,14 +84,21 @@ int RunMapper(int argc, char** argv) { LOG(INFO) << "Loaded database"; colmap::Timer run_timer; run_timer.Start(); - global_mapper.Solve(database, view_graph, cameras, images, tracks); + global_mapper.Solve( + *database, view_graph, rigs, cameras, frames, images, tracks); run_timer.Pause(); LOG(INFO) << "Reconstruction done in " << run_timer.ElapsedSeconds() << " seconds"; - WriteGlomapReconstruction( - output_path, cameras, images, tracks, output_format, image_path); + WriteGlomapReconstruction(output_path, + rigs, + cameras, + frames, + images, + tracks, + output_format, + image_path); LOG(INFO) << "Export to COLMAP reconstruction done"; return EXIT_SUCCESS; @@ -124,29 +134,38 @@ int RunMapperResume(int argc, char** argv) { } // Load the reconstruction - ViewGraph view_graph; // dummy variable - colmap::Database database; // dummy variable + ViewGraph view_graph; // dummy variable + std::shared_ptr database; // dummy variable + std::unordered_map rigs; std::unordered_map cameras; + std::unordered_map frames; std::unordered_map images; std::unordered_map tracks; colmap::Reconstruction reconstruction; reconstruction.Read(input_path); - ConvertColmapToGlomap(reconstruction, cameras, images, tracks); + ConvertColmapToGlomap(reconstruction, rigs, cameras, frames, images, tracks); GlobalMapper global_mapper(*options.mapper); // Main solver colmap::Timer run_timer; run_timer.Start(); - global_mapper.Solve(database, view_graph, cameras, images, tracks); + global_mapper.Solve( + *database, view_graph, rigs, cameras, frames, images, tracks); run_timer.Pause(); LOG(INFO) << "Reconstruction done in " << run_timer.ElapsedSeconds() << " seconds"; - WriteGlomapReconstruction( - output_path, cameras, images, tracks, output_format, image_path); + WriteGlomapReconstruction(output_path, + rigs, + cameras, + frames, + images, + tracks, + output_format, + image_path); LOG(INFO) << "Export to COLMAP reconstruction done"; return EXIT_SUCCESS; diff --git a/glomap/exe/rotation_averager.cc b/glomap/exe/rotation_averager.cc index 98460ab5..86403ded 100644 --- a/glomap/exe/rotation_averager.cc +++ b/glomap/exe/rotation_averager.cc @@ -1,4 +1,3 @@ - #include "glomap/controllers/rotation_averager.h" #include "glomap/controllers/option_manager.h" @@ -68,6 +67,23 @@ int RunRotationAverager(int argc, char** argv) { ReadRelPose(relpose_path, images, view_graph); + std::unordered_map rigs; + std::unordered_map cameras; + std::unordered_map frames; + + for (auto& [image_id, image] : images) { + image.camera_id = image.image_id; + cameras[image.camera_id] = Camera(); + } + + CreateOneRigPerCamera(cameras, rigs); + + // For frames that are not in any rig, add camera rigs + // For images without frames, initialize trivial frames + for (auto& [image_id, image] : images) { + CreateFrameForImage(Rigid3d(), image, rigs, frames); + } + if (gravity_path != "") { ReadGravity(gravity_path, images); } @@ -76,18 +92,19 @@ int RunRotationAverager(int argc, char** argv) { ReadRelWeight(weight_path, images, view_graph); } - int num_img = view_graph.KeepLargestConnectedComponents(images); + int num_img = view_graph.KeepLargestConnectedComponents(frames, images); LOG(INFO) << num_img << " / " << images.size() << " are within the largest connected component"; if (refine_gravity && gravity_path != "") { GravityRefiner grav_refiner(*options.gravity_refiner); - grav_refiner.RefineGravity(view_graph, images); + grav_refiner.RefineGravity(view_graph, frames, images); } colmap::Timer run_timer; run_timer.Start(); - if (!SolveRotationAveraging(view_graph, images, rotation_averager_options)) { + if (!SolveRotationAveraging( + view_graph, rigs, frames, images, rotation_averager_options)) { LOG(ERROR) << "Failed to solve global rotation averaging"; return EXIT_FAILURE; } diff --git a/glomap/io/colmap_converter.cc b/glomap/io/colmap_converter.cc index abfe1ffc..9fc1cc53 100644 --- a/glomap/io/colmap_converter.cc +++ b/glomap/io/colmap_converter.cc @@ -2,6 +2,8 @@ #include "glomap/math/two_view_geometry.h" +#include "colmap/scene/reconstruction_io_utils.h" + namespace glomap { void ConvertGlomapToColmapImage(const Image& image, @@ -10,16 +12,16 @@ void ConvertGlomapToColmapImage(const Image& image, image_colmap.SetImageId(image.image_id); image_colmap.SetCameraId(image.camera_id); image_colmap.SetName(image.file_name); - if (image.is_registered) { - image_colmap.SetCamFromWorld(image.cam_from_world); - } + image_colmap.SetFrameId(image.frame_id); if (keep_points) { image_colmap.SetPoints2D(image.features); } } -void ConvertGlomapToColmap(const std::unordered_map& cameras, +void ConvertGlomapToColmap(const std::unordered_map& rigs, + const std::unordered_map& cameras, + const std::unordered_map& frames, const std::unordered_map& images, const std::unordered_map& tracks, colmap::Reconstruction& reconstruction, @@ -33,14 +35,26 @@ void ConvertGlomapToColmap(const std::unordered_map& cameras, reconstruction.AddCamera(camera); } + // Add rigs + for (const auto& [rig_id, rig] : rigs) { + reconstruction.AddRig(rig); + } + + // Add frames + for (auto& [frame_id, frame] : frames) { + Frame frame_curr = frame; // Copy the frame to avoid dangling pointer + frame_curr.ResetRigPtr(); + reconstruction.AddFrame(frame_curr); + } + // Prepare the 2d-3d correspondences size_t min_supports = 2; std::unordered_map> image_to_point3D; if (tracks.size() > 0 || include_image_points) { // Initialize every point to corresponds to invalid point for (auto& [image_id, image] : images) { - if (!image.is_registered || - (cluster_id != -1 && image.cluster_id != cluster_id)) + if (!image.IsRegistered() || + (cluster_id != -1 && image.ClusterId() != cluster_id)) continue; image_to_point3D[image_id] = std::vector(image.features.size(), -1); @@ -71,8 +85,8 @@ void ConvertGlomapToColmap(const std::unordered_map& cameras, // Add track element for (auto& observation : track.observations) { const Image& image = images.at(observation.first); - if (!image.is_registered || - (cluster_id != -1 && image.cluster_id != cluster_id)) + if (!image.IsRegistered() || + (cluster_id != -1 && image.ClusterId() != cluster_id)) continue; colmap::TrackElement colmap_track_el; colmap_track_el.image_id = observation.first; @@ -81,7 +95,7 @@ void ConvertGlomapToColmap(const std::unordered_map& cameras, colmap_point.track.AddElement(colmap_track_el); } - if (track.observations.size() < min_supports) continue; + if (colmap_point.track.Length() < min_supports) continue; colmap_point.track.Compress(); reconstruction.AddPoint3D(track_id, std::move(colmap_point)); @@ -89,10 +103,6 @@ void ConvertGlomapToColmap(const std::unordered_map& cameras, // Add images for (const auto& [image_id, image] : images) { - if (!image.is_registered || - (cluster_id != -1 && image.cluster_id != cluster_id)) - continue; - colmap::Image image_colmap; bool keep_points = image_to_point3D.find(image_id) != image_to_point3D.end(); @@ -100,7 +110,7 @@ void ConvertGlomapToColmap(const std::unordered_map& cameras, if (keep_points) { std::vector& track_ids = image_to_point3D[image_id]; for (size_t i = 0; i < image.features.size(); i++) { - if (track_ids[i] != -1) { + if (track_ids[i] != -1 && reconstruction.ExistsPoint3D(track_ids[i])) { image_colmap.SetPoint3DForPoint2D(i, track_ids[i]); } } @@ -109,11 +119,21 @@ void ConvertGlomapToColmap(const std::unordered_map& cameras, reconstruction.AddImage(std::move(image_colmap)); } + // Deregister frames + for (auto& [frame_id, frame] : frames) { + if ((cluster_id != 0 && !frame.is_registered) || + (frame.cluster_id != cluster_id && cluster_id != -1)) { + reconstruction.DeRegisterFrame(frame_id); + } + } + reconstruction.UpdatePoint3DErrors(); } void ConvertColmapToGlomap(const colmap::Reconstruction& reconstruction, + std::unordered_map& rigs, std::unordered_map& cameras, + std::unordered_map& frames, std::unordered_map& images, std::unordered_map& tracks) { // Clear the glomap reconstruction @@ -125,6 +145,20 @@ void ConvertColmapToGlomap(const colmap::Reconstruction& reconstruction, cameras[camera_id] = camera; } + // Add rigs + for (const auto& [rig_id, rig] : reconstruction.Rigs()) { + rigs[rig_id] = rig; + } + + // Add frames + for (const auto& [frame_id, frame] : reconstruction.Frames()) { + frames[frame_id] = frame; + frames[frame_id].SetRigPtr(rigs.find(frame.RigId()) != rigs.end() + ? &rigs[frame.RigId()] + : nullptr); + frames[frame_id].is_registered = frame.HasPose(); + } + for (auto& [image_id, image_colmap] : reconstruction.Images()) { auto ite = images.insert(std::make_pair(image_colmap.ImageId(), Image(image_colmap.ImageId(), @@ -132,10 +166,10 @@ void ConvertColmapToGlomap(const colmap::Reconstruction& reconstruction, image_colmap.Name()))); Image& image = ite.first->second; - image.is_registered = image_colmap.HasPose(); - if (image_colmap.HasPose()) { - image.cam_from_world = static_cast(image_colmap.CamFromWorld()); - } + image.frame_id = image_colmap.FrameId(); + image.frame_ptr = frames.find(image.frame_id) != frames.end() + ? &frames[image.frame_id] + : nullptr; image.features.clear(); image.features.reserve(image_colmap.NumPoints2D()); @@ -174,11 +208,13 @@ void ConvertColmapPoints3DToGlomapTracks( } } -// For ease of debug, go through the database twice: first extract the available -// pairs, then read matches from pairs. +// For ease of debug, go through the database twice: first extract the +// available pairs, then read matches from pairs. void ConvertDatabaseToGlomap(const colmap::Database& database, ViewGraph& view_graph, + std::unordered_map& rigs, std::unordered_map& cameras, + std::unordered_map& frames, std::unordered_map& images) { // Add the images std::vector images_colmap = database.ReadAllImages(); @@ -190,16 +226,20 @@ void ConvertDatabaseToGlomap(const colmap::Database& database, const image_t image_id = image.ImageId(); if (image_id == colmap::kInvalidImageId) continue; - auto ite = images.insert(std::make_pair( + images.insert(std::make_pair( image_id, Image(image_id, image.CameraId(), image.Name()))); - const colmap::PosePrior prior = database.ReadPosePrior(image_id); - if (prior.IsValid()) { - const colmap::Rigid3d world_from_cam_prior(Eigen::Quaterniond::Identity(), - prior.position); - ite.first->second.cam_from_world = Rigid3d(Inverse(world_from_cam_prior)); - } else { - ite.first->second.cam_from_world = Rigid3d(); - } + + // TODO: Implement the logic of reading prior pose from the database + // const colmap::PosePrior prior = database.ReadPosePrior(image_id); + // if (prior.IsValid()) { + // const colmap::Rigid3d + // world_from_cam_prior(Eigen::Quaterniond::Identity(), + // prior.position); + // ite.first->second.cam_from_world = + // Rigid3d(Inverse(world_from_cam_prior)); + // } else { + // ite.first->second.cam_from_world = Rigid3d(); + // } } std::cout << std::endl; @@ -219,12 +259,90 @@ void ConvertDatabaseToGlomap(const colmap::Database& database, cameras[camera.camera_id] = camera; } + // Add the rigs + std::vector rigs_colmap = database.ReadAllRigs(); + for (auto& rig : rigs_colmap) { + rigs[rig.RigId()] = rig; + } + + // Add the frames + std::vector frames_colmap = database.ReadAllFrames(); + for (auto& frame : frames_colmap) { + frame_t frame_id = frame.FrameId(); + if (frame_id == colmap::kInvalidFrameId) continue; + frames[frame_id] = Frame(frame); + frames[frame_id].SetRigId(frame.RigId()); + frames[frame_id].SetRigPtr(rigs.find(frame.RigId()) != rigs.end() + ? &rigs[frame.RigId()] + : nullptr); + frames[frame_id].SetRigFromWorld(Rigid3d()); + + for (auto data_id : frame.ImageIds()) { + image_t image_id = data_id.id; + if (images.find(image_id) != images.end()) { + images[image_id].frame_id = frame_id; + images[image_id].frame_ptr = &frames[frame_id]; + } + } + } + + // cameras that are not used in any rig + rig_t max_rig_id = 0; + std::unordered_map cameras_id_to_rig_id; + for (const auto& [rig_id, rig] : rigs) { + max_rig_id = std::max(max_rig_id, rig_id); + + sensor_t sensor_id = rig.RefSensorId(); + if (sensor_id.type == SensorType::CAMERA) { + cameras_id_to_rig_id[rig.RefSensorId().id] = rig_id; + } + const std::map>& sensors = + rig.NonRefSensors(); + for (const auto& [sensor_id, sensor_pose] : sensors) { + if (sensor_id.type == SensorType::CAMERA) { + cameras_id_to_rig_id[sensor_id.id] = rig_id; + } + } + } + + // For cameras that are not in any rig, add camera rigs + for (const auto& [camera_id, camera] : cameras) { + if (cameras_id_to_rig_id.find(camera_id) == cameras_id_to_rig_id.end()) { + Rig rig; + rig.SetRigId(++max_rig_id); + rig.AddRefSensor(camera.SensorId()); + rigs[rig.RigId()] = rig; + cameras_id_to_rig_id[camera_id] = rig.RigId(); + } + } + + frame_t max_frame_id = 0; + // For frames that are not in any rig, add camera rigs + for (const auto& [frame_id, frame] : frames) { + if (frame_id == colmap::kInvalidFrameId) continue; + max_frame_id = std::max(max_frame_id, frame_id); + } + + // For images without frames, initialize trivial frames + for (auto& [image_id, image] : images) { + if (image.frame_id == colmap::kInvalidFrameId) { + frame_t frame_id = ++max_frame_id; + + CreateFrameForImage(Rigid3d(), + image, + rigs, + frames, + cameras_id_to_rig_id[image.camera_id], + frame_id); + } + } + // Add the matches std::vector> all_matches = database.ReadAllMatches(); - // Go through all matches and store the matche with enough observations in the - // view_graph + // Go through all matches and store the matche with enough observations in + // the view_graph size_t invalid_count = 0; std::unordered_map& image_pairs = view_graph.image_pairs; @@ -235,7 +353,7 @@ void ConvertDatabaseToGlomap(const colmap::Database& database, // Read the image pair from COLMAP database colmap::image_pair_t pair_id = all_matches[match_idx].first; std::pair image_pair_colmap = - database.PairIdToImagePair(pair_id); + colmap::PairIdToImagePair(pair_id); colmap::image_t image_id1 = image_pair_colmap.first; colmap::image_t image_id2 = image_pair_colmap.second; @@ -307,4 +425,37 @@ void ConvertDatabaseToGlomap(const colmap::Database& database, << view_graph.image_pairs.size() << " are invalid"; } +void CreateOneRigPerCamera(const std::unordered_map& cameras, + std::unordered_map& rigs) { + for (const auto& [camera_id, camera] : cameras) { + Rig rig; + rig.SetRigId(camera_id); + rig.AddRefSensor(camera.SensorId()); + } +} + +void CreateFrameForImage(const Rigid3d& cam_from_world, + Image& image, + std::unordered_map& rigs, + std::unordered_map& frames, + rig_t rig_id, + frame_t frame_id) { + Frame frame; + if (frame_id == colmap::kInvalidFrameId) { + frame_id = image.image_id; + } + if (rig_id == colmap::kInvalidRigId) { + rig_id = image.camera_id; + } + frame.SetFrameId(frame_id); + frame.SetRigId(rig_id); + frame.SetRigPtr(rigs.find(rig_id) != rigs.end() ? &rigs[rig_id] : nullptr); + frame.AddDataId(image.DataId()); + frame.SetRigFromWorld(cam_from_world); + frames[frame_id] = frame; + + image.frame_id = frame_id; + image.frame_ptr = &frames[frame_id]; +} + } // namespace glomap diff --git a/glomap/io/colmap_converter.h b/glomap/io/colmap_converter.h index 5bd4457e..486c7c04 100644 --- a/glomap/io/colmap_converter.h +++ b/glomap/io/colmap_converter.h @@ -8,10 +8,12 @@ namespace glomap { void ConvertGlomapToColmapImage(const Image& image, - colmap::Image& colmap_image, + colmap::Image& image_colmap, bool keep_points = false); -void ConvertGlomapToColmap(const std::unordered_map& cameras, +void ConvertGlomapToColmap(const std::unordered_map& rigs, + const std::unordered_map& cameras, + const std::unordered_map& frames, const std::unordered_map& images, const std::unordered_map& tracks, colmap::Reconstruction& reconstruction, @@ -19,7 +21,9 @@ void ConvertGlomapToColmap(const std::unordered_map& cameras, bool include_image_points = false); void ConvertColmapToGlomap(const colmap::Reconstruction& reconstruction, + std::unordered_map& rigs, std::unordered_map& cameras, + std::unordered_map& frames, std::unordered_map& images, std::unordered_map& tracks); @@ -29,7 +33,19 @@ void ConvertColmapPoints3DToGlomapTracks( void ConvertDatabaseToGlomap(const colmap::Database& database, ViewGraph& view_graph, + std::unordered_map& rigs, std::unordered_map& cameras, + std::unordered_map& frames, std::unordered_map& images); +void CreateOneRigPerCamera(const std::unordered_map& cameras, + std::unordered_map& rigs); + +void CreateFrameForImage(const Rigid3d& cam_from_world, + Image& image, + std::unordered_map& rigs, + std::unordered_map& frames, + rig_t rig_id = -1, + frame_t frame_id = -1); + } // namespace glomap diff --git a/glomap/io/colmap_io.cc b/glomap/io/colmap_io.cc index 5189e6d2..5afa23cd 100644 --- a/glomap/io/colmap_io.cc +++ b/glomap/io/colmap_io.cc @@ -7,7 +7,9 @@ namespace glomap { void WriteGlomapReconstruction( const std::string& reconstruction_path, + const std::unordered_map& rigs, const std::unordered_map& cameras, + const std::unordered_map& frames, const std::unordered_map& images, const std::unordered_map& tracks, const std::string output_format, @@ -15,14 +17,15 @@ void WriteGlomapReconstruction( // Check whether reconstruction pruning is applied. // If so, export seperate reconstruction int largest_component_num = -1; - for (const auto& [image_id, image] : images) { - if (image.cluster_id > largest_component_num) - largest_component_num = image.cluster_id; + for (const auto& [frame_id, frame] : frames) { + if (frame.cluster_id > largest_component_num) + largest_component_num = frame.cluster_id; } // If it is not seperated into several clusters, then output them as whole if (largest_component_num == -1) { colmap::Reconstruction reconstruction; - ConvertGlomapToColmap(cameras, images, tracks, reconstruction); + ConvertGlomapToColmap( + rigs, cameras, frames, images, tracks, reconstruction); // Read in colors if (image_path != "") { LOG(INFO) << "Extracting colors ..."; @@ -41,7 +44,8 @@ void WriteGlomapReconstruction( std::cout << "\r Exporting reconstruction " << comp + 1 << " / " << largest_component_num + 1 << std::flush; colmap::Reconstruction reconstruction; - ConvertGlomapToColmap(cameras, images, tracks, reconstruction, comp); + ConvertGlomapToColmap( + rigs, cameras, frames, images, tracks, reconstruction, comp); // Read in colors if (image_path != "") { reconstruction.ExtractColorsForAllImages(image_path); diff --git a/glomap/io/colmap_io.h b/glomap/io/colmap_io.h index 5d5cccb5..3df19cd3 100644 --- a/glomap/io/colmap_io.h +++ b/glomap/io/colmap_io.h @@ -7,7 +7,9 @@ namespace glomap { void WriteGlomapReconstruction( const std::string& reconstruction_path, + const std::unordered_map& rigs, const std::unordered_map& cameras, + const std::unordered_map& frames, const std::unordered_map& images, const std::unordered_map& tracks, const std::string output_format = "bin", diff --git a/glomap/io/pose_io.cc b/glomap/io/pose_io.cc index eeda2e6b..49835a8c 100644 --- a/glomap/io/pose_io.cc +++ b/glomap/io/pose_io.cc @@ -16,6 +16,11 @@ void ReadRelPose(const std::string& file_path, max_image_id = std::max(max_image_id, image_id); } + // Mark every edge in te view graph as invalid + for (auto& [pair_id, image_pair] : view_graph.image_pairs) { + image_pair.is_valid = false; + } + std::ifstream file(file_path); // Read in data @@ -65,8 +70,15 @@ void ReadRelPose(const std::string& file_path, pose_rel.translation[i] = std::stod(item); } - view_graph.image_pairs.insert( - std::make_pair(pair_id, ImagePair(index1, index2, pose_rel))); + if (view_graph.image_pairs.find(pair_id) == view_graph.image_pairs.end()) { + view_graph.image_pairs.insert( + std::make_pair(pair_id, ImagePair(index1, index2, pose_rel))); + } else { + view_graph.image_pairs[pair_id].cam2_from_cam1 = pose_rel; + view_graph.image_pairs[pair_id].is_valid = true; + view_graph.image_pairs[pair_id].config = + colmap::TwoViewGeometry::CALIBRATED; + } counter++; } LOG(INFO) << counter << " relpose are loaded" << std::endl; @@ -118,6 +130,8 @@ void ReadRelWeight(const std::string& file_path, LOG(INFO) << counter << " weights are used are loaded" << std::endl; } +// TODO: now, we only store 1 single gravity per rig. +// for ease of implementation, we only store from the image with trivial frame void ReadGravity(const std::string& gravity_path, std::unordered_map& images) { std::unordered_map name_idx; @@ -148,10 +162,14 @@ void ReadGravity(const std::string& gravity_path, auto ite = name_idx.find(name); if (ite != name_idx.end()) { counter++; - images[ite->second].gravity_info.SetGravity(gravity); - // Make sure the initialization is aligned with the gravity - images[ite->second].cam_from_world.rotation = - images[ite->second].gravity_info.GetRAlign().transpose(); + if (images[ite->second].HasTrivialFrame()) { + images[ite->second].frame_ptr->gravity_info.SetGravity(gravity); + Rigid3d& cam_from_world = images[ite->second].frame_ptr->RigFromWorld(); + // Set the rotation from the camera to the world + // Make sure the initialization is aligned with the gravity + cam_from_world.rotation = Eigen::Quaterniond( + images[ite->second].frame_ptr->gravity_info.GetRAlign()); + } } } LOG(INFO) << counter << " images are loaded with gravity" << std::endl; @@ -162,16 +180,17 @@ void WriteGlobalRotation(const std::string& file_path, std::ofstream file(file_path); std::set existing_images; for (const auto& [image_id, image] : images) { - if (image.is_registered) { + if (image.IsRegistered()) { existing_images.insert(image_id); } } for (const auto& image_id : existing_images) { const auto image = images.at(image_id); - if (!image.is_registered) continue; + if (!image.IsRegistered()) continue; file << image.file_name; + Rigid3d cam_from_world = image.CamFromWorld(); for (int i = 0; i < 4; i++) { - file << " " << image.cam_from_world.rotation.coeffs()[(i + 3) % 4]; + file << " " << cam_from_world.rotation.coeffs()[(i + 3) % 4]; } file << "\n"; } diff --git a/glomap/math/gravity.cc b/glomap/math/gravity.cc index 30b83c66..15db4000 100644 --- a/glomap/math/gravity.cc +++ b/glomap/math/gravity.cc @@ -97,4 +97,4 @@ double CalcAngle(const Eigen::Vector3d& gravity1, return std::acos(cos_r) * 180 / EIGEN_PI; } -} // namespace glomap \ No newline at end of file +} // namespace glomap diff --git a/glomap/math/rigid3d.cc b/glomap/math/rigid3d.cc index cc9f5c7f..e776e9b7 100644 --- a/glomap/math/rigid3d.cc +++ b/glomap/math/rigid3d.cc @@ -5,13 +5,7 @@ namespace glomap { double CalcAngle(const Rigid3d& pose1, const Rigid3d& pose2) { - double cos_r = - ((pose1.rotation.inverse() * pose2.rotation).toRotationMatrix().trace() - - 1) / - 2; - cos_r = std::min(std::max(cos_r, -1.), 1.); - - return std::acos(cos_r) * 180 / EIGEN_PI; + return pose1.rotation.angularDistance(pose2.rotation) * 180 / EIGEN_PI; } double CalcTrans(const Rigid3d& pose1, const Rigid3d& pose2) { @@ -22,7 +16,6 @@ double CalcTransAngle(const Rigid3d& pose1, const Rigid3d& pose2) { double cos_r = (pose1.translation).dot(pose2.translation) / (pose1.translation.norm() * pose2.translation.norm()); cos_r = std::min(std::max(cos_r, -1.), 1.); - return std::acos(cos_r) * 180 / EIGEN_PI; } @@ -30,7 +23,6 @@ double CalcAngle(const Eigen::Matrix3d& rotation1, const Eigen::Matrix3d& rotation2) { double cos_r = ((rotation1.transpose() * rotation2).trace() - 1) / 2; cos_r = std::min(std::max(cos_r, -1.), 1.); - return std::acos(cos_r) * 180 / EIGEN_PI; } @@ -69,4 +61,9 @@ Eigen::Matrix3d AngleAxisToRotation(const Eigen::Vector3d& aa_vec) { return R; } } -} // namespace glomap \ No newline at end of file + +Eigen::Vector3d CenterFromPose(const Rigid3d& pose) { + return pose.rotation.inverse() * -pose.translation; +} + +} // namespace glomap diff --git a/glomap/math/rigid3d.h b/glomap/math/rigid3d.h index 00f3b416..56bf62ae 100644 --- a/glomap/math/rigid3d.h +++ b/glomap/math/rigid3d.h @@ -35,4 +35,7 @@ Eigen::Vector3d RotationToAngleAxis(const Eigen::Matrix3d& rot); // Convert angle axis to rotation matrix Eigen::Matrix3d AngleAxisToRotation(const Eigen::Vector3d& aa); +// Calculate the center of the pose +Eigen::Vector3d CenterFromPose(const Rigid3d& pose); + } // namespace glomap diff --git a/glomap/math/tree.cc b/glomap/math/tree.cc index 5f329e64..15043ccd 100644 --- a/glomap/math/tree.cc +++ b/glomap/math/tree.cc @@ -84,7 +84,7 @@ image_t MaximumSpanningTree(const ViewGraph& view_graph, std::unordered_map idx_to_image_id; idx_to_image_id.reserve(images.size()); for (auto& [image_id, image] : images) { - if (image.is_registered == false) continue; + if (image.IsRegistered() == false) continue; idx_to_image_id[image_id_to_idx.size()] = image_id; image_id_to_idx[image_id] = image_id_to_idx.size(); } @@ -110,7 +110,7 @@ image_t MaximumSpanningTree(const ViewGraph& view_graph, const Image& image1 = images.at(image_pair.image_id1); const Image& image2 = images.at(image_pair.image_id2); - if (image1.is_registered == false || image2.is_registered == false) { + if (image1.IsRegistered() == false || image2.IsRegistered() == false) { continue; } diff --git a/glomap/processors/image_undistorter.cc b/glomap/processors/image_undistorter.cc index 012465d1..eb4bfe0d 100644 --- a/glomap/processors/image_undistorter.cc +++ b/glomap/processors/image_undistorter.cc @@ -32,7 +32,10 @@ void UndistortImages(std::unordered_map& cameras, image.features_undist.reserve(num_points); for (int i = 0; i < num_points; i++) { image.features_undist.emplace_back( - camera.CamFromImg(image.features[i]).homogeneous().normalized()); + camera.CamFromImg(image.features[i]) + .value_or(Eigen::Vector2d::Zero()) + .homogeneous() + .normalized()); } }); } diff --git a/glomap/processors/reconstruction_normalizer.cc b/glomap/processors/reconstruction_normalizer.cc index b2af04a7..04800a51 100644 --- a/glomap/processors/reconstruction_normalizer.cc +++ b/glomap/processors/reconstruction_normalizer.cc @@ -3,7 +3,9 @@ namespace glomap { colmap::Sim3d NormalizeReconstruction( + std::unordered_map& rigs, std::unordered_map& cameras, + std::unordered_map& frames, std::unordered_map& images, std::unordered_map& tracks, bool fixed_scale, @@ -19,7 +21,7 @@ colmap::Sim3d NormalizeReconstruction( coords_y.reserve(images.size()); coords_z.reserve(images.size()); for (const auto& [image_id, image] : images) { - if (!image.is_registered) continue; + if (!image.IsRegistered()) continue; const Eigen::Vector3d proj_center = image.Center(); coords_x.push_back(static_cast(proj_center(0))); coords_y.push_back(static_cast(proj_center(1))); @@ -59,9 +61,19 @@ colmap::Sim3d NormalizeReconstruction( colmap::Sim3d tform( scale, Eigen::Quaterniond::Identity(), -scale * mean_coord); - for (auto& [_, image] : images) { - if (image.is_registered) { - image.cam_from_world = TransformCameraWorld(tform, image.cam_from_world); + for (auto& [_, frame] : frames) { + if (!frame.HasPose()) continue; + Rigid3d& rig_from_world = frame.RigFromWorld(); + rig_from_world = TransformCameraWorld(tform, rig_from_world); + } + + for (auto& [_, rig] : rigs) { + for (auto& [sensor_id, sensor_from_rig_opt] : rig.NonRefSensors()) { + if (sensor_from_rig_opt.has_value()) { + Rigid3d sensor_from_rig = sensor_from_rig_opt.value(); + sensor_from_rig.translation *= scale; + rig.SetSensorFromRig(sensor_id, sensor_from_rig); + } } } @@ -72,4 +84,4 @@ colmap::Sim3d NormalizeReconstruction( return tform; } -} // namespace glomap \ No newline at end of file +} // namespace glomap diff --git a/glomap/processors/reconstruction_normalizer.h b/glomap/processors/reconstruction_normalizer.h index 51d51fd6..3d3c3193 100644 --- a/glomap/processors/reconstruction_normalizer.h +++ b/glomap/processors/reconstruction_normalizer.h @@ -7,7 +7,9 @@ namespace glomap { colmap::Sim3d NormalizeReconstruction( + std::unordered_map& rigs, std::unordered_map& cameras, + std::unordered_map& frames, std::unordered_map& images, std::unordered_map& tracks, bool fixed_scale = false, diff --git a/glomap/processors/reconstruction_pruning.cc b/glomap/processors/reconstruction_pruning.cc index 5ebbc205..014a10e5 100644 --- a/glomap/processors/reconstruction_pruning.cc +++ b/glomap/processors/reconstruction_pruning.cc @@ -3,24 +3,28 @@ #include "glomap/processors/view_graph_manipulation.h" namespace glomap { -image_t PruneWeaklyConnectedImages(std::unordered_map& images, +image_t PruneWeaklyConnectedImages(std::unordered_map& frames, + std::unordered_map& images, std::unordered_map& tracks, int min_num_images, int min_num_observations) { // Prepare the 2d-3d correspondences std::unordered_map pair_covisibility_count; - std::unordered_map image_observation_count; + std::unordered_map frame_observation_count; for (auto& [track_id, track] : tracks) { if (track.observations.size() <= 2) continue; for (size_t i = 0; i < track.observations.size(); i++) { - image_observation_count[track.observations[i].first]++; + image_t image_id1 = track.observations[i].first; + frame_t frame_id1 = images[image_id1].frame_id; + + frame_observation_count[frame_id1]++; for (size_t j = i + 1; j < track.observations.size(); j++) { - image_t image_id1 = track.observations[i].first; image_t image_id2 = track.observations[j].first; - if (image_id1 == image_id2) continue; + frame_t frame_id2 = images[image_id2].frame_id; + if (frame_id1 == frame_id2) continue; image_pair_t pair_id = - ImagePair::ImagePairToPairId(image_id1, image_id2); + ImagePair::ImagePairToPairId(frame_id1, frame_id2); if (pair_covisibility_count.find(pair_id) == pair_covisibility_count.end()) { pair_covisibility_count[pair_id] = 1; @@ -33,7 +37,7 @@ image_t PruneWeaklyConnectedImages(std::unordered_map& images, // Establish the visibility graph size_t counter = 0; - ViewGraph visibility_graph; + ViewGraph visibility_graph_frame; std::vector pair_count; for (auto& [pair_id, count] : pair_covisibility_count) { // since the relative pose is only fixed if there are more than 5 points, @@ -43,20 +47,64 @@ image_t PruneWeaklyConnectedImages(std::unordered_map& images, image_t image_id1, image_id2; ImagePair::PairIdToImagePair(pair_id, image_id1, image_id2); - if (image_observation_count[image_id1] < min_num_observations || - image_observation_count[image_id2] < min_num_observations) + if (frame_observation_count[image_id1] < min_num_observations || + frame_observation_count[image_id2] < min_num_observations) continue; - visibility_graph.image_pairs.insert( + visibility_graph_frame.image_pairs.insert( std::make_pair(pair_id, ImagePair(image_id1, image_id2))); pair_count.push_back(count); - visibility_graph.image_pairs[pair_id].is_valid = true; - visibility_graph.image_pairs[pair_id].weight = count; + visibility_graph_frame.image_pairs[pair_id].is_valid = true; + visibility_graph_frame.image_pairs[pair_id].weight = count; } } LOG(INFO) << "Established visibility graph with " << counter << " pairs"; + // Create the visibility graph + // Connect the reference image of each frame with other reference image + std::unordered_map frame_id_to_begin_img; + for (auto& [frame_id, frame] : frames) { + int counter = 0; + for (const auto& data_id : frame.ImageIds()) { + image_t image_id = data_id.id; + if (images.find(image_id) == images.end()) continue; + frame_id_to_begin_img[frame_id] = image_id; + break; + } + } + + ViewGraph visibility_graph; + for (auto& [pair_id, image_pair] : visibility_graph_frame.image_pairs) { + frame_t frame_id1, frame_id2; + ImagePair::PairIdToImagePair(pair_id, frame_id1, frame_id2); + image_t image_id1 = frame_id_to_begin_img[frame_id1]; + image_t image_id2 = frame_id_to_begin_img[frame_id2]; + visibility_graph.image_pairs.insert( + std::make_pair(pair_id, ImagePair(image_id1, image_id2))); + visibility_graph.image_pairs[pair_id].weight = image_pair.weight; + } + + int max_weight = std::max_element(pair_count.begin(), pair_count.end()) - + pair_count.begin(); + + // within each frame, connect the reference image with all other images + for (auto& [frame_id, frame] : frames) { + image_t begin_image_id = frame_id_to_begin_img[frame_id]; + for (const auto& data_id : frame.ImageIds()) { + image_t image_id = data_id.id; + if (image_id == begin_image_id || images.find(image_id) == images.end()) + continue; + image_pair_t pair_id = + ImagePair::ImagePairToPairId(begin_image_id, image_id); + visibility_graph.image_pairs.insert( + std::make_pair(pair_id, ImagePair(begin_image_id, image_id))); + + // Never break th inner edge + visibility_graph.image_pairs[pair_id].weight = max_weight; + } + } + // sort the pair count std::sort(pair_count.begin(), pair_count.end()); double median_count = pair_count[pair_count.size() / 2]; @@ -75,6 +123,7 @@ image_t PruneWeaklyConnectedImages(std::unordered_map& images, return ViewGraphManipulater::EstablishStrongClusters( visibility_graph, + frames, images, ViewGraphManipulater::WEIGHT, std::max(median_count - median_count_diff, 20.), diff --git a/glomap/processors/reconstruction_pruning.h b/glomap/processors/reconstruction_pruning.h index f527eb6c..9f9538e2 100644 --- a/glomap/processors/reconstruction_pruning.h +++ b/glomap/processors/reconstruction_pruning.h @@ -5,7 +5,8 @@ namespace glomap { -image_t PruneWeaklyConnectedImages(std::unordered_map& images, +image_t PruneWeaklyConnectedImages(std::unordered_map& frames, + std::unordered_map& images, std::unordered_map& tracks, int min_num_images = 2, int min_num_observations = 0); diff --git a/glomap/processors/relpose_filter.cc b/glomap/processors/relpose_filter.cc index 812e0b0c..8af7cf80 100644 --- a/glomap/processors/relpose_filter.cc +++ b/glomap/processors/relpose_filter.cc @@ -15,11 +15,11 @@ void RelPoseFilter::FilterRotations( const Image& image1 = images.at(image_pair.image_id1); const Image& image2 = images.at(image_pair.image_id2); - if (image1.is_registered == false || image2.is_registered == false) { + if (image1.IsRegistered() == false || image2.IsRegistered() == false) { continue; } - Rigid3d pose_calc = image2.cam_from_world * Inverse(image1.cam_from_world); + Rigid3d pose_calc = image2.CamFromWorld() * Inverse(image1.CamFromWorld()); double angle = CalcAngle(pose_calc, image_pair.cam2_from_cam1); if (angle > max_angle) { diff --git a/glomap/processors/track_filter.cc b/glomap/processors/track_filter.cc index c3a78f7d..04410c41 100644 --- a/glomap/processors/track_filter.cc +++ b/glomap/processors/track_filter.cc @@ -16,7 +16,7 @@ int TrackFilter::FilterTracksByReprojection( std::vector observation_new; for (auto& [image_id, feature_id] : track.observations) { const Image& image = images.at(image_id); - Eigen::Vector3d pt_calc = image.cam_from_world * track.xyz; + Eigen::Vector3d pt_calc = image.CamFromWorld() * track.xyz; if (pt_calc(2) < EPS) continue; double reprojection_error = max_reprojection_error; @@ -29,9 +29,10 @@ int TrackFilter::FilterTracksByReprojection( (pt_reproj - feature_undist.head(2) / (feature_undist(2) + EPS)) .norm(); } else { - Eigen::Vector2d pt_reproj = pt_calc.head(2) / pt_calc(2); Eigen::Vector2d pt_dist; - pt_dist = cameras.at(image.camera_id).ImgFromCam(pt_reproj); + pt_dist = cameras.at(image.camera_id) + .ImgFromCam(pt_calc) + .value_or(Eigen::Vector2d::Zero()); reprojection_error = (pt_dist - image.features.at(feature_id)).norm(); } @@ -66,7 +67,7 @@ int TrackFilter::FilterTracksByAngle( // const Camera& camera = image.camera; const Eigen::Vector3d& feature_undist = image.features_undist.at(feature_id); - Eigen::Vector3d pt_calc = image.cam_from_world * track.xyz; + Eigen::Vector3d pt_calc = image.CamFromWorld() * track.xyz; if (pt_calc(2) < EPS) continue; pt_calc = pt_calc.normalized(); diff --git a/glomap/processors/view_graph_manipulation.cc b/glomap/processors/view_graph_manipulation.cc index 38ec4dc6..db5bc3ca 100644 --- a/glomap/processors/view_graph_manipulation.cc +++ b/glomap/processors/view_graph_manipulation.cc @@ -9,9 +9,10 @@ namespace glomap { image_pair_t ViewGraphManipulater::SparsifyGraph( ViewGraph& view_graph, + std::unordered_map& frames, std::unordered_map& images, int expected_degree) { - image_t num_img = view_graph.KeepLargestConnectedComponents(images); + image_t num_img = view_graph.KeepLargestConnectedComponents(frames, images); // Keep track of chosen edges std::unordered_set chosen_edges; @@ -21,7 +22,7 @@ image_pair_t ViewGraphManipulater::SparsifyGraph( // Here, the average is the mean of the degrees double average_degree = 0; for (const auto& [image_id, neighbors] : adjacency_list) { - if (images[image_id].is_registered == false) continue; + if (images[image_id].IsRegistered() == false) continue; average_degree += neighbors.size(); } average_degree = average_degree / num_img; @@ -34,8 +35,8 @@ image_pair_t ViewGraphManipulater::SparsifyGraph( image_t image_id1 = image_pair.image_id1; image_t image_id2 = image_pair.image_id2; - if (images[image_id1].is_registered == false || - images[image_id2].is_registered == false) + if (images[image_id1].IsRegistered() == false || + images[image_id2].IsRegistered() == false) continue; int degree1 = adjacency_list.at(image_id1).size(); @@ -60,18 +61,20 @@ image_pair_t ViewGraphManipulater::SparsifyGraph( } // Keep the largest connected component - view_graph.KeepLargestConnectedComponents(images); + view_graph.KeepLargestConnectedComponents(frames, images); return chosen_edges.size(); } image_t ViewGraphManipulater::EstablishStrongClusters( ViewGraph& view_graph, + std::unordered_map& frames, std::unordered_map& images, StrongClusterCriteria criteria, double min_thres, int min_num_images) { - image_t num_img_before = view_graph.KeepLargestConnectedComponents(images); + image_t num_img_before = + view_graph.KeepLargestConnectedComponents(frames, images); // Construct the initial cluster by keeping the pairs with weight > min_thres UnionFind uf; @@ -84,8 +87,8 @@ image_t ViewGraphManipulater::EstablishStrongClusters( (criteria == INLIER_NUM && image_pair.inliers.size() > min_thres); status = status || (criteria == WEIGHT && image_pair.weight > min_thres); if (status) { - uf.Union(image_pair_t(image_pair.image_id1), - image_pair_t(image_pair.image_id2)); + uf.Union(image_pair_t(images[image_pair.image_id1].frame_id), + image_pair_t(images[image_pair.image_id2].frame_id)); } } @@ -118,8 +121,8 @@ image_t ViewGraphManipulater::EstablishStrongClusters( image_t image_id1 = image_pair.image_id1; image_t image_id2 = image_pair.image_id2; - image_pair_t root1 = uf.Find(image_pair_t(image_id1)); - image_pair_t root2 = uf.Find(image_pair_t(image_id2)); + image_pair_t root1 = uf.Find(image_pair_t(images[image_id1].frame_id)); + image_pair_t root2 = uf.Find(image_pair_t(images[image_id2].frame_id)); if (root1 == root2) { continue; @@ -154,11 +157,14 @@ image_t ViewGraphManipulater::EstablishStrongClusters( image_t image_id1 = image_pair.image_id1; image_t image_id2 = image_pair.image_id2; - if (uf.Find(image_pair_t(image_id1)) != uf.Find(image_pair_t(image_id2))) { + frame_t frame_id1 = images[image_id1].frame_id; + frame_t frame_id2 = images[image_id2].frame_id; + + if (uf.Find(image_pair_t(frame_id1)) != uf.Find(image_pair_t(frame_id2))) { image_pair.is_valid = false; } } - int num_comp = view_graph.MarkConnectedComponents(images); + int num_comp = view_graph.MarkConnectedComponents(frames, images); LOG(INFO) << "Clustering take " << iteration << " iterations. " << "Images are grouped into " << num_comp diff --git a/glomap/processors/view_graph_manipulation.h b/glomap/processors/view_graph_manipulation.h index 269d2305..067403ad 100644 --- a/glomap/processors/view_graph_manipulation.h +++ b/glomap/processors/view_graph_manipulation.h @@ -11,11 +11,13 @@ struct ViewGraphManipulater { }; static image_pair_t SparsifyGraph(ViewGraph& view_graph, + std::unordered_map& frames, std::unordered_map& images, int expected_degree = 50); static image_t EstablishStrongClusters( ViewGraph& view_graph, + std::unordered_map& frames, std::unordered_map& images, StrongClusterCriteria criteria = INLIER_NUM, double min_thres = 100, // require strong edges diff --git a/glomap/scene/frame.h b/glomap/scene/frame.h new file mode 100644 index 00000000..7843376f --- /dev/null +++ b/glomap/scene/frame.h @@ -0,0 +1,52 @@ +#pragma once + +#include "glomap/math/gravity.h" +#include "glomap/scene/types.h" +#include "glomap/types.h" + +#include + +namespace glomap { + +struct GravityInfo { + public: + // Whether the gravity information is available + bool has_gravity = false; + + const Eigen::Matrix3d& GetRAlign() const { return R_align_; } + + inline void SetGravity(const Eigen::Vector3d& g); + inline Eigen::Vector3d GetGravity() const { return gravity_in_rig_; }; + + private: + // Direction of the gravity + Eigen::Vector3d gravity_in_rig_ = Eigen::Vector3d::Zero(); + + // Alignment matrix, the second column is the gravity direction + Eigen::Matrix3d R_align_ = Eigen::Matrix3d::Identity(); +}; + +struct Frame : public colmap::Frame { + Frame() : colmap::Frame() {} + Frame(const colmap::Frame& frame) : colmap::Frame(frame) {} + + // whether the frame is within the largest connected component + bool is_registered = false; + int cluster_id = -1; + + // Gravity information + GravityInfo gravity_info; + + // Easy way to check if the image has gravity information + inline bool HasGravity() const; +}; + +bool Frame::HasGravity() const { return gravity_info.has_gravity; } + +void GravityInfo::SetGravity(const Eigen::Vector3d& g) { + gravity_in_rig_ = g; + R_align_ = GetAlignRot(g); + has_gravity = true; +} + +} // namespace glomap diff --git a/glomap/scene/image.h b/glomap/scene/image.h index 9dd94f0a..c5c6b278 100644 --- a/glomap/scene/image.h +++ b/glomap/scene/image.h @@ -1,29 +1,12 @@ #pragma once #include "glomap/math/gravity.h" +#include "glomap/scene/frame.h" #include "glomap/scene/types.h" #include "glomap/types.h" namespace glomap { -struct GravityInfo { - public: - // Whether the gravity information is available - bool has_gravity = false; - - const Eigen::Matrix3d& GetRAlign() const { return R_align; } - - inline void SetGravity(const Eigen::Vector3d& g); - inline Eigen::Vector3d GetGravity() const { return gravity; }; - - private: - // Direction of the gravity - Eigen::Vector3d gravity; - - // Alignment matrix, the second column is the gravity direction - Eigen::Matrix3d R_align; -}; - struct Image { Image() : image_id(-1), file_name("") {} Image(image_t img_id, camera_t cam_id, std::string file_name) @@ -37,15 +20,10 @@ struct Image { // The id of the camera camera_t camera_id; - // whether the image is within the largest connected component - bool is_registered = false; - int cluster_id = -1; - - // The pose of the image, defined as the transformation from world to camera. - Rigid3d cam_from_world; - - // Gravity information - GravityInfo gravity_info; + // Frame info + // By default, set it to be invalid index + frame_t frame_id = -1; + struct Frame* frame_ptr = nullptr; // Distorted feature points in pixels. std::vector features; @@ -54,16 +32,74 @@ struct Image { // Methods inline Eigen::Vector3d Center() const; + + // Methods to access the camera pose + inline Rigid3d CamFromWorld() const; + + // Check whether the frame is registered + inline bool IsRegistered() const; + + inline int ClusterId() const; + + // Check if cam_from_world needs to be composed with sensor_from_rig pose. + inline bool HasTrivialFrame() const; + + // Easy way to check if the image has gravity information + inline bool HasGravity() const; + + inline Eigen::Matrix3d GetRAlign() const; + + inline data_t DataId() const; }; Eigen::Vector3d Image::Center() const { - return cam_from_world.rotation.inverse() * -cam_from_world.translation; + return CamFromWorld().rotation.inverse() * -CamFromWorld().translation; +} + +// Concrete implementation of the methods +Rigid3d Image::CamFromWorld() const { + return THROW_CHECK_NOTNULL(frame_ptr)->SensorFromWorld( + sensor_t(SensorType::CAMERA, camera_id)); +} + +bool Image::IsRegistered() const { + return frame_ptr != nullptr && frame_ptr->is_registered; +} + +int Image::ClusterId() const { + return frame_ptr != nullptr ? frame_ptr->cluster_id : -1; +} + +bool Image::HasTrivialFrame() const { + return THROW_CHECK_NOTNULL(frame_ptr)->RigPtr()->IsRefSensor( + sensor_t(SensorType::CAMERA, camera_id)); +} + +bool Image::HasGravity() const { + return frame_ptr->HasGravity() && + (HasTrivialFrame() || + frame_ptr->RigPtr() + ->MaybeSensorFromRig(sensor_t(SensorType::CAMERA, camera_id)) + .has_value()); +} + +Eigen::Matrix3d Image::GetRAlign() const { + if (HasGravity()) { + if (HasTrivialFrame()) { + return frame_ptr->gravity_info.GetRAlign(); + } else { + return frame_ptr->RigPtr() + ->SensorFromRig(sensor_t(SensorType::CAMERA, camera_id)) + .rotation.toRotationMatrix() * + frame_ptr->gravity_info.GetRAlign(); + } + } else { + return Eigen::Matrix3d::Identity(); + } } -void GravityInfo::SetGravity(const Eigen::Vector3d& g) { - gravity = g; - R_align = GetAlignRot(g); - has_gravity = true; +data_t Image::DataId() const { + return data_t(sensor_t(SensorType::CAMERA, camera_id), image_id); } } // namespace glomap diff --git a/glomap/scene/image_pair.h b/glomap/scene/image_pair.h index fba6534a..67bf2ed3 100644 --- a/glomap/scene/image_pair.h +++ b/glomap/scene/image_pair.h @@ -60,18 +60,16 @@ struct ImagePair { image_pair_t ImagePair::ImagePairToPairId(const image_t image_id1, const image_t image_id2) { - if (image_id1 > image_id2) { - return static_cast(kMaxNumImages) * image_id2 + image_id1; - } else { - return static_cast(kMaxNumImages) * image_id1 + image_id2; - } + return colmap::ImagePairToPairId(image_id1, image_id2); } void ImagePair::PairIdToImagePair(const image_pair_t pair_id, image_t& image_id1, image_t& image_id2) { - image_id1 = static_cast(pair_id % kMaxNumImages); - image_id2 = static_cast((pair_id - image_id1) / kMaxNumImages); + std::pair image_id_pair = + colmap::PairIdToImagePair(pair_id); + image_id1 = image_id_pair.first; + image_id2 = image_id_pair.second; } } // namespace glomap diff --git a/glomap/scene/types.h b/glomap/scene/types.h index dc3aa038..c4631570 100644 --- a/glomap/scene/types.h +++ b/glomap/scene/types.h @@ -1,6 +1,7 @@ #pragma once #include +// #include #include #include @@ -21,6 +22,12 @@ using colmap::camera_t; // Unique identifier for images. using colmap::image_t; +// Unique identifier for frames. +using colmap::frame_t; + +// Unique identifier for camera rigs. +using colmap::rig_t; + // Each image pair gets a unique ID, see `Database::ImagePairToPairId`. typedef uint64_t image_pair_t; @@ -34,7 +41,18 @@ typedef uint64_t track_t; using colmap::Rigid3d; -const image_t kMaxNumImages = colmap::Database::kMaxNumImages; +// Unique identifier for sensors, which can be cameras or IMUs. +using colmap::sensor_t; + +// Sensor type, used to identify the type of sensor (e.g., camera, IMU). +using colmap::SensorType; + +// Unique identifier for sensor data +using colmap::data_t; + +// Rig +using colmap::Rig; + const image_pair_t kInvalidImagePairId = -1; } // namespace glomap diff --git a/glomap/scene/types_sfm.h b/glomap/scene/types_sfm.h index 4f03b5dc..b1e48dc0 100644 --- a/glomap/scene/types_sfm.h +++ b/glomap/scene/types_sfm.h @@ -1,6 +1,8 @@ +#pragma once // This files contains all the necessary includes for sfm // Types defined by GLOMAP #include "glomap/scene/camera.h" +#include "glomap/scene/frame.h" #include "glomap/scene/image.h" #include "glomap/scene/track.h" #include "glomap/scene/types.h" diff --git a/glomap/scene/view_graph.cc b/glomap/scene/view_graph.cc index 0443d90d..b1835b30 100644 --- a/glomap/scene/view_graph.cc +++ b/glomap/scene/view_graph.cc @@ -7,8 +7,10 @@ namespace glomap { int ViewGraph::KeepLargestConnectedComponents( + std::unordered_map& frames, std::unordered_map& images) { EstablishAdjacencyList(); + EstablishAdjacencyListFrame(images); int num_comp = FindConnectedComponent(); @@ -25,37 +27,41 @@ int ViewGraph::KeepLargestConnectedComponents( std::unordered_set largest_component = connected_components[max_idx]; - // Set all images to not registered - for (auto& [image_id, image] : images) image.is_registered = false; - - // Set the images in the largest component to registered - for (auto image_id : largest_component) images[image_id].is_registered = true; - + // Set all frames to not registered + for (auto& [frame_id, frame] : frames) { + frame.is_registered = false; + } + // Set the frames in the largest component to registered + for (auto frame_id : largest_component) { + frames[frame_id].is_registered = true; + } // set all pairs not in the largest component to invalid num_pairs = 0; for (auto& [pair_id, image_pair] : image_pairs) { - if (!images[image_pair.image_id1].is_registered || - !images[image_pair.image_id2].is_registered) { + if (!images[image_pair.image_id1].IsRegistered() || + !images[image_pair.image_id2].IsRegistered()) { image_pair.is_valid = false; } if (image_pair.is_valid) num_pairs++; } - num_images = largest_component.size(); + for (auto& [image_id, image] : images) { + if (image.IsRegistered()) max_img++; + } return max_img; } int ViewGraph::FindConnectedComponent() { connected_components.clear(); std::unordered_map visited; - for (auto& [image_id, neighbors] : adjacency_list) { - visited[image_id] = false; + for (auto& [frame_id, neighbors] : adjacency_list_frame) { + visited[frame_id] = false; } - for (auto& [image_id, neighbors] : adjacency_list) { - if (!visited[image_id]) { + for (auto& [frame_id, neighbors] : adjacency_list_frame) { + if (!visited[frame_id]) { std::unordered_set component; - BFS(image_id, visited, component); + BFS(frame_id, visited, component); connected_components.push_back(component); } } @@ -64,8 +70,11 @@ int ViewGraph::FindConnectedComponent() { } int ViewGraph::MarkConnectedComponents( - std::unordered_map& images, int min_num_img) { + std::unordered_map& frames, + std::unordered_map& images, + int min_num_img) { EstablishAdjacencyList(); + EstablishAdjacencyListFrame(images); int num_comp = FindConnectedComponent(); @@ -76,14 +85,15 @@ int ViewGraph::MarkConnectedComponents( } std::sort(cluster_num_img.begin(), cluster_num_img.end(), std::greater<>()); - // Set the cluster number of every image to be -1 - for (auto& [image_id, image] : images) image.cluster_id = -1; + // Set the cluster number of every frame to be -1 + for (auto& [frame_id, frame] : frames) frame.cluster_id = -1; int comp = 0; for (; comp < num_comp; comp++) { if (cluster_num_img[comp].first < min_num_img) break; - for (auto image_id : connected_components[cluster_num_img[comp].second]) - images[image_id].cluster_id = comp; + for (auto frame_id : connected_components[cluster_num_img[comp].second]) { + frames[frame_id].cluster_id = comp; + } } return comp; @@ -101,7 +111,7 @@ void ViewGraph::BFS(image_t root, image_t curr = q.front(); q.pop(); - for (image_t neighbor : adjacency_list[curr]) { + for (image_t neighbor : adjacency_list_frame[curr]) { if (!visited[neighbor]) { q.push(neighbor); visited[neighbor] = true; @@ -120,4 +130,17 @@ void ViewGraph::EstablishAdjacencyList() { } } } + +void ViewGraph::EstablishAdjacencyListFrame( + std::unordered_map& images) { + adjacency_list_frame.clear(); + for (auto& [pair_id, image_pair] : image_pairs) { + if (image_pair.is_valid) { + frame_t frame_id1 = images[image_pair.image_id1].frame_id; + frame_t frame_id2 = images[image_pair.image_id2].frame_id; + adjacency_list_frame[frame_id1].insert(frame_id2); + adjacency_list_frame[frame_id2].insert(frame_id1); + } + } +} } // namespace glomap diff --git a/glomap/scene/view_graph.h b/glomap/scene/view_graph.h index 7c28229d..21c1a16c 100644 --- a/glomap/scene/view_graph.h +++ b/glomap/scene/view_graph.h @@ -16,18 +16,25 @@ class ViewGraph { // Mark the image which is not connected to any other images as not registered // Return: the number of images in the largest connected component int KeepLargestConnectedComponents( + std::unordered_map& frames, std::unordered_map& images); // Mark the cluster of the cameras (cluster_id sort by the the number of // images) - int MarkConnectedComponents(std::unordered_map& images, + int MarkConnectedComponents(std::unordered_map& frames, + std::unordered_map& images, int min_num_img = -1); // Establish the adjacency list void EstablishAdjacencyList(); + // Establish the frame based adjacency list + void EstablishAdjacencyListFrame(std::unordered_map& images); + inline const std::unordered_map>& GetAdjacencyList() const; + inline const std::unordered_map>& + GetAdjacencyListFrame() const; // Data std::unordered_map image_pairs; @@ -44,6 +51,7 @@ class ViewGraph { // Data for processing std::unordered_map> adjacency_list; + std::unordered_map> adjacency_list_frame; std::vector> connected_components; }; @@ -52,6 +60,11 @@ ViewGraph::GetAdjacencyList() const { return adjacency_list; } +const std::unordered_map>& +ViewGraph::GetAdjacencyListFrame() const { + return adjacency_list_frame; +} + void ViewGraph::RemoveInvalidPair(image_pair_t pair_id) { ImagePair& pair = image_pairs.at(pair_id); pair.is_valid = false; diff --git a/scripts/format/c++.sh b/scripts/format/c++.sh index 07f09f36..2b5a64b9 100755 --- a/scripts/format/c++.sh +++ b/scripts/format/c++.sh @@ -3,12 +3,12 @@ # This script applies clang-format to the whole repository. # Check version -version_string=$(clang-format --version | sed -E 's/^.*(\d+\.\d+\.\d+-.*).*$/\1/') -expected_version_string='19.1.0' -if [[ "$version_string" =~ "$expected_version_string" ]]; then - echo "clang-format version '$version_string' matches '$expected_version_string'" +version_string=$(clang-format --version | sed -E 's/^.* ([0-9]+\.[0-9]+)\..*$/\1/') +expected_version_string='19.1' +if [[ "$version_string" == "$expected_version_string" ]]; then + echo "clang-format major.minor version '$version_string' matches expected '$expected_version_string'" else - echo "clang-format version '$version_string' doesn't match '$expected_version_string'" + echo "clang-format major.minor version '$version_string' doesn't match expected '$expected_version_string'" exit 1 fi From ca27e0effc24da523e2d1cf7ba99055abfd225ee Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Johannes=20Sch=C3=B6nberger?= Date: Sat, 22 Nov 2025 10:11:48 +0100 Subject: [PATCH 39/45] Update to latest colmap and use L1 solver + union-find from colmap (#223) * Update to latest colmap and use L1 solver + union-find from colmap * d * d * d * d * d * d * d * d * d * d * d * d --- .github/workflows/mac.yml | 2 +- .github/workflows/ubuntu.yml | 2 +- .github/workflows/windows.yml | 15 +- .gitignore | 1 + CMakeLists.txt | 15 +- cmake/FindDependencies.cmake | 36 +- cmake/FindSuiteSparse.cmake | 537 ------------------ glomap/CMakeLists.txt | 4 - glomap/controllers/global_mapper_test.cc | 8 +- glomap/controllers/rotation_averager_test.cc | 15 +- glomap/controllers/track_establishment.cc | 2 +- glomap/controllers/track_establishment.h | 5 +- .../estimators/global_rotation_averaging.cc | 23 +- glomap/estimators/global_rotation_averaging.h | 3 +- glomap/estimators/rotation_initializer.cc | 3 +- glomap/io/colmap_converter.cc | 2 +- glomap/math/l1_solver.h | 113 ---- glomap/math/union_find.h | 40 -- glomap/processors/reconstruction_normalizer.h | 3 +- glomap/processors/view_graph_manipulation.cc | 5 +- glomap/scene/view_graph.cc | 2 - scripts/format/c++.sh | 10 +- thirdparty/CMakeLists.txt | 37 ++ 23 files changed, 104 insertions(+), 779 deletions(-) delete mode 100644 cmake/FindSuiteSparse.cmake delete mode 100644 glomap/math/l1_solver.h delete mode 100644 glomap/math/union_find.h create mode 100644 thirdparty/CMakeLists.txt diff --git a/.github/workflows/mac.yml b/.github/workflows/mac.yml index 3f786675..a78a36de 100644 --- a/.github/workflows/mac.yml +++ b/.github/workflows/mac.yml @@ -17,7 +17,7 @@ jobs: matrix: config: [ { - os: macos-14, + os: macos-15, arch: arm64, cmakeBuildType: Release, }, diff --git a/.github/workflows/ubuntu.yml b/.github/workflows/ubuntu.yml index 0ebc1e14..29d05f07 100644 --- a/.github/workflows/ubuntu.yml +++ b/.github/workflows/ubuntu.yml @@ -100,7 +100,7 @@ jobs: if: matrix.config.checkCodeFormat run: | set +x -euo pipefail - python -m pip install clang-format==19.1.0 + python -m pip install clang-format==20.1.5 ./scripts/format/c++.sh git diff --name-only git diff --exit-code || (echo "Code formatting failed" && exit 1) diff --git a/.github/workflows/windows.yml b/.github/workflows/windows.yml index 2cfde898..3acfd2c6 100644 --- a/.github/workflows/windows.yml +++ b/.github/workflows/windows.yml @@ -17,14 +17,7 @@ jobs: matrix: config: [ { - os: windows-2022, - cmakeBuildType: Release, - cudaEnabled: true, - testsEnabled: true, - exportPackage: true, - }, - { - os: windows-2022, + os: windows-2025, cmakeBuildType: Release, cudaEnabled: false, testsEnabled: true, @@ -37,13 +30,13 @@ jobs: COMPILER_CACHE_DIR: ${{ github.workspace }}/compiler-cache CCACHE_DIR: ${{ github.workspace }}/compiler-cache/ccache CCACHE_BASEDIR: ${{ github.workspace }} - VCPKG_COMMIT_ID: bc3512a509f9d29b37346a7e7e929f9a26e66c7e + VCPKG_COMMIT_ID: 2ad7bd06128280e02bfe02361d8ffd7d465cfcf0 GLOG_v: 1 GLOG_logtostderr: 1 steps: - uses: actions/checkout@v4 - + # We define the vcpkg binary sources using separate variables for read and # write operations: # * Read sources are defined as inline. These can be read by anyone and, @@ -74,7 +67,7 @@ jobs: key: v${{ env.COMPILER_CACHE_VERSION }}-${{ matrix.config.os }}-${{ matrix.config.cmakeBuildType }}-${{ matrix.config.asanEnabled }}--${{ matrix.config.cudaEnabled }}-${{ github.run_id }}-${{ github.run_number }} restore-keys: v${{ env.COMPILER_CACHE_VERSION }}-${{ matrix.config.os }}-${{ matrix.config.cmakeBuildType }}-${{ matrix.config.asanEnabled }}--${{ matrix.config.cudaEnabled }} path: ${{ env.COMPILER_CACHE_DIR }} - + - name: Install ccache shell: pwsh run: | diff --git a/.gitignore b/.gitignore index 5066fb1b..9220f287 100644 --- a/.gitignore +++ b/.gitignore @@ -2,3 +2,4 @@ /data /.vscode /compile_commands.json +.cache diff --git a/CMakeLists.txt b/CMakeLists.txt index 1c0f9ab3..87c0992e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -20,6 +20,17 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON) set_property(GLOBAL PROPERTY GLOBAL_DEPENDS_NO_CYCLES ON) +# Determine project compiler. +if(CMAKE_CXX_COMPILER_ID STREQUAL "MSVC") + set(IS_MSVC TRUE) +endif() +if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") + set(IS_GNU TRUE) +endif() +if(CMAKE_CXX_COMPILER_ID MATCHES ".*Clang") + set(IS_CLANG TRUE) +endif() + include(cmake/FindDependencies.cmake) if (TESTS_ENABLED) @@ -45,8 +56,7 @@ else() message(STATUS "Disabling ccache support") endif() - -if(CMAKE_CXX_COMPILER_ID STREQUAL "MSVC") +if(IS_MSVC) # Some fixes for the Glog library. add_definitions("-DGLOG_USE_GLOG_EXPORT") add_definitions("-DGLOG_NO_ABBREVIATED_SEVERITIES") @@ -62,4 +72,5 @@ if(CMAKE_CXX_COMPILER_ID STREQUAL "MSVC") endif() endif() +add_subdirectory(thirdparty) add_subdirectory(glomap) diff --git a/cmake/FindDependencies.cmake b/cmake/FindDependencies.cmake index c1d0110e..a656c221 100644 --- a/cmake/FindDependencies.cmake +++ b/cmake/FindDependencies.cmake @@ -1,10 +1,6 @@ set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake") -find_package(Eigen3 3.4 REQUIRED) -find_package(CHOLMOD QUIET) -if(NOT TARGET SuiteSparse::CHOLMOD) - find_package(SuiteSparse COMPONENTS CHOLMOD REQUIRED) -endif() +find_package(Eigen3 REQUIRED) find_package(Ceres REQUIRED COMPONENTS SuiteSparse) find_package(Boost REQUIRED) find_package(OpenMP REQUIRED COMPONENTS C CXX) @@ -24,36 +20,6 @@ if(TESTS_ENABLED) find_package(GTest REQUIRED) endif() -include(FetchContent) -FetchContent_Declare(PoseLib - GIT_REPOSITORY https://github.com/PoseLib/PoseLib.git - GIT_TAG 7e9f5f53372e43f89655040d4dfc4a00e5ace11c # 2.0.5 - EXCLUDE_FROM_ALL - SYSTEM -) -message(STATUS "Configuring PoseLib...") -if (FETCH_POSELIB) - FetchContent_MakeAvailable(PoseLib) -else() - find_package(PoseLib REQUIRED) -endif() -message(STATUS "Configuring PoseLib... done") - -FetchContent_Declare(COLMAP - GIT_REPOSITORY https://github.com/colmap/colmap.git - GIT_TAG c5f9cefc87e5dd596b638e4cee0ff543c7d14755 # Oct 23 2025 - EXCLUDE_FROM_ALL -) -message(STATUS "Configuring COLMAP...") -set(UNINSTALL_ENABLED OFF CACHE INTERNAL "") -set(GUI_ENABLED OFF CACHE INTERNAL "") -if (FETCH_COLMAP) - FetchContent_MakeAvailable(COLMAP) -else() - find_package(COLMAP REQUIRED) -endif() -message(STATUS "Configuring COLMAP... done") - set(CUDA_MIN_VERSION "7.0") if(CUDA_ENABLED) if(CMAKE_VERSION VERSION_LESS 3.17) diff --git a/cmake/FindSuiteSparse.cmake b/cmake/FindSuiteSparse.cmake deleted file mode 100644 index bccd89fe..00000000 --- a/cmake/FindSuiteSparse.cmake +++ /dev/null @@ -1,537 +0,0 @@ -# Ceres Solver - A fast non-linear least squares minimizer -# Copyright 2023 Google Inc. All rights reserved. -# http://ceres-solver.org/ -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# * Redistributions of source code must retain the above copyright notice, -# this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# * Neither the name of Google Inc. nor the names of its contributors may be -# used to endorse or promote products derived from this software without -# specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: alexs.mac@gmail.com (Alex Stewart) -# - -#[=======================================================================[.rst: -FindSuiteSparse -=============== - -Module for locating SuiteSparse libraries and its dependencies. - -This module defines the following variables: - -``SuiteSparse_FOUND`` - ``TRUE`` iff SuiteSparse and all dependencies have been found. - -``SuiteSparse_VERSION`` - Extracted from ``SuiteSparse_config.h`` (>= v4). - -``SuiteSparse_VERSION_MAJOR`` - Equal to 4 if ``SuiteSparse_VERSION`` = 4.2.1 - -``SuiteSparse_VERSION_MINOR`` - Equal to 2 if ``SuiteSparse_VERSION`` = 4.2.1 - -``SuiteSparse_VERSION_PATCH`` - Equal to 1 if ``SuiteSparse_VERSION`` = 4.2.1 - -The following variables control the behaviour of this module: - -``SuiteSparse_NO_CMAKE`` - Do not attempt to use the native SuiteSparse CMake package configuration. - - -Targets -------- - -The following targets define the SuiteSparse components searched for. - -``SuiteSparse::AMD`` - Symmetric Approximate Minimum Degree (AMD) - -``SuiteSparse::CAMD`` - Constrained Approximate Minimum Degree (CAMD) - -``SuiteSparse::COLAMD`` - Column Approximate Minimum Degree (COLAMD) - -``SuiteSparse::CCOLAMD`` - Constrained Column Approximate Minimum Degree (CCOLAMD) - -``SuiteSparse::CHOLMOD`` - Sparse Supernodal Cholesky Factorization and Update/Downdate (CHOLMOD) - -``SuiteSparse::Partition`` - CHOLMOD with METIS support - -``SuiteSparse::SPQR`` - Multifrontal Sparse QR (SuiteSparseQR) - -``SuiteSparse::Config`` - Common configuration for all but CSparse (SuiteSparse version >= 4). - -Optional SuiteSparse dependencies: - -``METIS::METIS`` - Serial Graph Partitioning and Fill-reducing Matrix Ordering (METIS) -]=======================================================================] - -if (NOT SuiteSparse_NO_CMAKE) - find_package (SuiteSparse NO_MODULE QUIET) -endif (NOT SuiteSparse_NO_CMAKE) - -if (SuiteSparse_FOUND) - return () -endif (SuiteSparse_FOUND) - -# Push CMP0057 to enable support for IN_LIST, when cmake_minimum_required is -# set to <3.3. -cmake_policy (PUSH) -cmake_policy (SET CMP0057 NEW) - -if (NOT SuiteSparse_FIND_COMPONENTS) - set (SuiteSparse_FIND_COMPONENTS - AMD - CAMD - CCOLAMD - CHOLMOD - COLAMD - SPQR - ) - - foreach (component IN LISTS SuiteSparse_FIND_COMPONENTS) - set (SuiteSparse_FIND_REQUIRED_${component} TRUE) - endforeach (component IN LISTS SuiteSparse_FIND_COMPONENTS) -endif (NOT SuiteSparse_FIND_COMPONENTS) - -# Assume SuiteSparse was found and set it to false only if third-party -# dependencies could not be located. SuiteSparse components are handled by -# FindPackageHandleStandardArgs HANDLE_COMPONENTS option. -set (SuiteSparse_FOUND TRUE) - -include (CheckLibraryExists) -include (CheckSymbolExists) -include (CMakePushCheckState) - -# Config is a base component and thus always required -set (SuiteSparse_IMPLICIT_COMPONENTS Config) - -# CHOLMOD depends on AMD, CAMD, CCOLAMD, and COLAMD. -if (CHOLMOD IN_LIST SuiteSparse_FIND_COMPONENTS) - list (APPEND SuiteSparse_IMPLICIT_COMPONENTS AMD CAMD CCOLAMD COLAMD) -endif (CHOLMOD IN_LIST SuiteSparse_FIND_COMPONENTS) - -# SPQR depends on CHOLMOD. -if (SPQR IN_LIST SuiteSparse_FIND_COMPONENTS) - list (APPEND SuiteSparse_IMPLICIT_COMPONENTS CHOLMOD) -endif (SPQR IN_LIST SuiteSparse_FIND_COMPONENTS) - -# Implicit components are always required -foreach (component IN LISTS SuiteSparse_IMPLICIT_COMPONENTS) - set (SuiteSparse_FIND_REQUIRED_${component} TRUE) -endforeach (component IN LISTS SuiteSparse_IMPLICIT_COMPONENTS) - -list (APPEND SuiteSparse_FIND_COMPONENTS ${SuiteSparse_IMPLICIT_COMPONENTS}) - -# Do not list components multiple times. -list (REMOVE_DUPLICATES SuiteSparse_FIND_COMPONENTS) - -# Reset CALLERS_CMAKE_FIND_LIBRARY_PREFIXES to its value when -# FindSuiteSparse was invoked. -macro(SuiteSparse_RESET_FIND_LIBRARY_PREFIX) - if (MSVC) - set(CMAKE_FIND_LIBRARY_PREFIXES "${CALLERS_CMAKE_FIND_LIBRARY_PREFIXES}") - endif (MSVC) -endmacro(SuiteSparse_RESET_FIND_LIBRARY_PREFIX) - -# Called if we failed to find SuiteSparse or any of it's required dependencies, -# unsets all public (designed to be used externally) variables and reports -# error message at priority depending upon [REQUIRED/QUIET/] argument. -macro(SuiteSparse_REPORT_NOT_FOUND REASON_MSG) - # Will be set to FALSE by find_package_handle_standard_args - unset (SuiteSparse_FOUND) - - # Do NOT unset SuiteSparse_REQUIRED_VARS here, as it is used by - # FindPackageHandleStandardArgs() to generate the automatic error message on - # failure which highlights which components are missing. - - suitesparse_reset_find_library_prefix() - - # Note _FIND_[REQUIRED/QUIETLY] variables defined by FindPackage() - # use the camelcase library name, not uppercase. - if (SuiteSparse_FIND_QUIETLY) - message(STATUS "Failed to find SuiteSparse - " ${REASON_MSG} ${ARGN}) - elseif (SuiteSparse_FIND_REQUIRED) - message(FATAL_ERROR "Failed to find SuiteSparse - " ${REASON_MSG} ${ARGN}) - else() - # Neither QUIETLY nor REQUIRED, use no priority which emits a message - # but continues configuration and allows generation. - message("-- Failed to find SuiteSparse - " ${REASON_MSG} ${ARGN}) - endif (SuiteSparse_FIND_QUIETLY) - - # Do not call return(), s/t we keep processing if not called with REQUIRED - # and report all missing components, rather than bailing after failing to find - # the first. -endmacro(SuiteSparse_REPORT_NOT_FOUND) - -# Handle possible presence of lib prefix for libraries on MSVC, see -# also SuiteSparse_RESET_FIND_LIBRARY_PREFIX(). -if (MSVC) - # Preserve the caller's original values for CMAKE_FIND_LIBRARY_PREFIXES - # s/t we can set it back before returning. - set(CALLERS_CMAKE_FIND_LIBRARY_PREFIXES "${CMAKE_FIND_LIBRARY_PREFIXES}") - # The empty string in this list is important, it represents the case when - # the libraries have no prefix (shared libraries / DLLs). - set(CMAKE_FIND_LIBRARY_PREFIXES "lib" "" "${CMAKE_FIND_LIBRARY_PREFIXES}") -endif (MSVC) - -# Additional suffixes to try appending to each search path. -list(APPEND SuiteSparse_CHECK_PATH_SUFFIXES - suitesparse) # Windows/Ubuntu - -# Wrappers to find_path/library that pass the SuiteSparse search hints/paths. -# -# suitesparse_find_component( [FILES name1 [name2 ...]] -# [LIBRARIES name1 [name2 ...]]) -macro(suitesparse_find_component COMPONENT) - include(CMakeParseArguments) - set(MULTI_VALUE_ARGS FILES LIBRARIES) - cmake_parse_arguments(SuiteSparse_FIND_COMPONENT_${COMPONENT} - "" "" "${MULTI_VALUE_ARGS}" ${ARGN}) - - set(SuiteSparse_${COMPONENT}_FOUND TRUE) - if (SuiteSparse_FIND_COMPONENT_${COMPONENT}_FILES) - find_path(SuiteSparse_${COMPONENT}_INCLUDE_DIR - NAMES ${SuiteSparse_FIND_COMPONENT_${COMPONENT}_FILES} - PATH_SUFFIXES ${SuiteSparse_CHECK_PATH_SUFFIXES}) - if (SuiteSparse_${COMPONENT}_INCLUDE_DIR) - message(STATUS "Found ${COMPONENT} headers in: " - "${SuiteSparse_${COMPONENT}_INCLUDE_DIR}") - mark_as_advanced(SuiteSparse_${COMPONENT}_INCLUDE_DIR) - else() - # Specified headers not found. - set(SuiteSparse_${COMPONENT}_FOUND FALSE) - if (SuiteSparse_FIND_REQUIRED_${COMPONENT}) - suitesparse_report_not_found( - "Did not find ${COMPONENT} header (required SuiteSparse component).") - else() - message(STATUS "Did not find ${COMPONENT} header (optional " - "SuiteSparse component).") - # Hide optional vars from CMake GUI even if not found. - mark_as_advanced(SuiteSparse_${COMPONENT}_INCLUDE_DIR) - endif() - endif() - endif() - - if (SuiteSparse_FIND_COMPONENT_${COMPONENT}_LIBRARIES) - find_library(SuiteSparse_${COMPONENT}_LIBRARY - NAMES ${SuiteSparse_FIND_COMPONENT_${COMPONENT}_LIBRARIES} - PATH_SUFFIXES ${SuiteSparse_CHECK_PATH_SUFFIXES}) - if (SuiteSparse_${COMPONENT}_LIBRARY) - message(STATUS "Found ${COMPONENT} library: ${SuiteSparse_${COMPONENT}_LIBRARY}") - mark_as_advanced(SuiteSparse_${COMPONENT}_LIBRARY) - else () - # Specified libraries not found. - set(SuiteSparse_${COMPONENT}_FOUND FALSE) - if (SuiteSparse_FIND_REQUIRED_${COMPONENT}) - suitesparse_report_not_found( - "Did not find ${COMPONENT} library (required SuiteSparse component).") - else() - message(STATUS "Did not find ${COMPONENT} library (optional SuiteSparse " - "dependency)") - # Hide optional vars from CMake GUI even if not found. - mark_as_advanced(SuiteSparse_${COMPONENT}_LIBRARY) - endif() - endif() - endif() - - # A component can be optional (given to OPTIONAL_COMPONENTS). However, if the - # component is implicit (must be always present, such as the Config component) - # assume it be required as well. - if (SuiteSparse_FIND_REQUIRED_${COMPONENT}) - list (APPEND SuiteSparse_REQUIRED_VARS SuiteSparse_${COMPONENT}_INCLUDE_DIR) - list (APPEND SuiteSparse_REQUIRED_VARS SuiteSparse_${COMPONENT}_LIBRARY) - endif (SuiteSparse_FIND_REQUIRED_${COMPONENT}) - - # Define the target only if the include directory and the library were found - if (SuiteSparse_${COMPONENT}_INCLUDE_DIR AND SuiteSparse_${COMPONENT}_LIBRARY) - if (NOT TARGET SuiteSparse::${COMPONENT}) - add_library(SuiteSparse::${COMPONENT} IMPORTED UNKNOWN) - endif (NOT TARGET SuiteSparse::${COMPONENT}) - - set_property(TARGET SuiteSparse::${COMPONENT} PROPERTY - INTERFACE_INCLUDE_DIRECTORIES ${SuiteSparse_${COMPONENT}_INCLUDE_DIR}) - set_property(TARGET SuiteSparse::${COMPONENT} PROPERTY - IMPORTED_LOCATION ${SuiteSparse_${COMPONENT}_LIBRARY}) - endif (SuiteSparse_${COMPONENT}_INCLUDE_DIR AND SuiteSparse_${COMPONENT}_LIBRARY) -endmacro() - -# Given the number of components of SuiteSparse, and to ensure that the -# automatic failure message generated by FindPackageHandleStandardArgs() -# when not all required components are found is helpful, we maintain a list -# of all variables that must be defined for SuiteSparse to be considered found. -unset(SuiteSparse_REQUIRED_VARS) - -# BLAS. -find_package(BLAS QUIET) -if (NOT BLAS_FOUND) - suitesparse_report_not_found( - "Did not find BLAS library (required for SuiteSparse).") -endif (NOT BLAS_FOUND) - -# LAPACK. -find_package(LAPACK QUIET) -if (NOT LAPACK_FOUND) - suitesparse_report_not_found( - "Did not find LAPACK library (required for SuiteSparse).") -endif (NOT LAPACK_FOUND) - -foreach (component IN LISTS SuiteSparse_FIND_COMPONENTS) - if (component STREQUAL Partition) - # Partition is a meta component that neither provides additional headers nor - # a separate library. It is strictly part of CHOLMOD. - continue () - endif (component STREQUAL Partition) - string (TOLOWER ${component} component_library) - - if (component STREQUAL "Config") - set (component_header SuiteSparse_config.h) - set (component_library suitesparseconfig) - elseif (component STREQUAL "SPQR") - set (component_header SuiteSparseQR.hpp) - else (component STREQUAL "SPQR") - set (component_header ${component_library}.h) - endif (component STREQUAL "Config") - - suitesparse_find_component(${component} - FILES ${component_header} - LIBRARIES ${component_library}) -endforeach (component IN LISTS SuiteSparse_FIND_COMPONENTS) - -if (TARGET SuiteSparse::SPQR) - # SuiteSparseQR may be compiled with Intel Threading Building Blocks, - # we assume that if TBB is installed, SuiteSparseQR was compiled with - # support for it, this will do no harm if it wasn't. - find_package(TBB QUIET) - if (TBB_FOUND) - message(STATUS "Found Intel Thread Building Blocks (TBB) library " - "(${TBB_VERSION_MAJOR}.${TBB_VERSION_MINOR} / ${TBB_INTERFACE_VERSION}) " - "include location: ${TBB_INCLUDE_DIRS}. Assuming SuiteSparseQR was " - "compiled with TBB.") - # Add the TBB libraries to the SuiteSparseQR libraries (the only - # libraries to optionally depend on TBB). - if (TARGET TBB::tbb) - # Native TBB package configuration provides an imported target. Use it if - # available. - set_property (TARGET SuiteSparse::SPQR APPEND PROPERTY - INTERFACE_LINK_LIBRARIES TBB::tbb) - else (TARGET TBB::tbb) - set_property (TARGET SuiteSparse::SPQR APPEND PROPERTY - INTERFACE_INCLUDE_DIRECTORIES ${TBB_INCLUDE_DIRS}) - set_property (TARGET SuiteSparse::SPQR APPEND PROPERTY - INTERFACE_LINK_LIBRARIES ${TBB_LIBRARIES}) - endif (TARGET TBB::tbb) - else (TBB_FOUND) - message(STATUS "Did not find Intel TBB library, assuming SuiteSparseQR was " - "not compiled with TBB.") - endif (TBB_FOUND) -endif (TARGET SuiteSparse::SPQR) - -check_library_exists(rt shm_open "" HAVE_LIBRT) - -if (TARGET SuiteSparse::Config) - # SuiteSparse_config (SuiteSparse version >= 4) requires librt library for - # timing by default when compiled on Linux or Unix, but not on OSX (which - # does not have librt). - if (HAVE_LIBRT) - message(STATUS "Adding librt to " - "SuiteSparse_config libraries (required on Linux & Unix [not OSX] if " - "SuiteSparse is compiled with timing).") - set_property (TARGET SuiteSparse::Config APPEND PROPERTY - INTERFACE_LINK_LIBRARIES $) - else (HAVE_LIBRT) - message(STATUS "Could not find librt, but found SuiteSparse_config, " - "assuming that SuiteSparse was compiled without timing.") - endif (HAVE_LIBRT) - - # Add BLAS and LAPACK as dependencies of SuiteSparse::Config for convenience - # given that all components depend on it. - if (BLAS_FOUND) - if (TARGET BLAS::BLAS) - set_property (TARGET SuiteSparse::Config APPEND PROPERTY - INTERFACE_LINK_LIBRARIES $) - else (TARGET BLAS::BLAS) - set_property (TARGET SuiteSparse::Config APPEND PROPERTY - INTERFACE_LINK_LIBRARIES ${BLAS_LIBRARIES}) - endif (TARGET BLAS::BLAS) - endif (BLAS_FOUND) - - if (LAPACK_FOUND) - if (TARGET LAPACK::LAPACK) - set_property (TARGET SuiteSparse::Config APPEND PROPERTY - INTERFACE_LINK_LIBRARIES $) - else (TARGET LAPACK::LAPACK) - set_property (TARGET SuiteSparse::Config APPEND PROPERTY - INTERFACE_LINK_LIBRARIES ${LAPACK_LIBRARIES}) - endif (TARGET LAPACK::LAPACK) - endif (LAPACK_FOUND) - - # SuiteSparse version >= 4. - set(SuiteSparse_VERSION_FILE - ${SuiteSparse_Config_INCLUDE_DIR}/SuiteSparse_config.h) - if (NOT EXISTS ${SuiteSparse_VERSION_FILE}) - suitesparse_report_not_found( - "Could not find file: ${SuiteSparse_VERSION_FILE} containing version " - "information for >= v4 SuiteSparse installs, but SuiteSparse_config was " - "found (only present in >= v4 installs).") - else (NOT EXISTS ${SuiteSparse_VERSION_FILE}) - file(READ ${SuiteSparse_VERSION_FILE} Config_CONTENTS) - - string(REGEX MATCH "#define SUITESPARSE_MAIN_VERSION[ \t]+([0-9]+)" - SuiteSparse_VERSION_LINE "${Config_CONTENTS}") - set (SuiteSparse_VERSION_MAJOR ${CMAKE_MATCH_1}) - - string(REGEX MATCH "#define SUITESPARSE_SUB_VERSION[ \t]+([0-9]+)" - SuiteSparse_VERSION_LINE "${Config_CONTENTS}") - set (SuiteSparse_VERSION_MINOR ${CMAKE_MATCH_1}) - - string(REGEX MATCH "#define SUITESPARSE_SUBSUB_VERSION[ \t]+([0-9]+)" - SuiteSparse_VERSION_LINE "${Config_CONTENTS}") - set (SuiteSparse_VERSION_PATCH ${CMAKE_MATCH_1}) - - unset (SuiteSparse_VERSION_LINE) - - # This is on a single line s/t CMake does not interpret it as a list of - # elements and insert ';' separators which would result in 4.;2.;1 nonsense. - set(SuiteSparse_VERSION - "${SuiteSparse_VERSION_MAJOR}.${SuiteSparse_VERSION_MINOR}.${SuiteSparse_VERSION_PATCH}") - - if (SuiteSparse_VERSION MATCHES "[0-9]+\\.[0-9]+\\.[0-9]+") - set(SuiteSparse_VERSION_COMPONENTS 3) - else (SuiteSparse_VERSION MATCHES "[0-9]+\\.[0-9]+\\.[0-9]+") - message (WARNING "Could not parse SuiteSparse_config.h: SuiteSparse " - "version will not be available") - - unset (SuiteSparse_VERSION) - unset (SuiteSparse_VERSION_MAJOR) - unset (SuiteSparse_VERSION_MINOR) - unset (SuiteSparse_VERSION_PATCH) - endif (SuiteSparse_VERSION MATCHES "[0-9]+\\.[0-9]+\\.[0-9]+") - endif (NOT EXISTS ${SuiteSparse_VERSION_FILE}) -endif (TARGET SuiteSparse::Config) - -# CHOLMOD requires AMD CAMD CCOLAMD COLAMD -if (TARGET SuiteSparse::CHOLMOD) - foreach (component IN ITEMS AMD CAMD CCOLAMD COLAMD) - if (TARGET SuiteSparse::${component}) - set_property (TARGET SuiteSparse::CHOLMOD APPEND PROPERTY - INTERFACE_LINK_LIBRARIES SuiteSparse::${component}) - else (TARGET SuiteSparse::${component}) - # Consider CHOLMOD not found if COLAMD cannot be found - set (SuiteSparse_CHOLMOD_FOUND FALSE) - endif (TARGET SuiteSparse::${component}) - endforeach (component IN ITEMS AMD CAMD CCOLAMD COLAMD) -endif (TARGET SuiteSparse::CHOLMOD) - -# SPQR requires CHOLMOD -if (TARGET SuiteSparse::SPQR) - if (TARGET SuiteSparse::CHOLMOD) - set_property (TARGET SuiteSparse::SPQR APPEND PROPERTY - INTERFACE_LINK_LIBRARIES SuiteSparse::CHOLMOD) - else (TARGET SuiteSparse::CHOLMOD) - # Consider SPQR not found if CHOLMOD cannot be found - set (SuiteSparse_SQPR_FOUND FALSE) - endif (TARGET SuiteSparse::CHOLMOD) -endif (TARGET SuiteSparse::SPQR) - -# Add SuiteSparse::Config as dependency to all components -if (TARGET SuiteSparse::Config) - foreach (component IN LISTS SuiteSparse_FIND_COMPONENTS) - if (component STREQUAL Config) - continue () - endif (component STREQUAL Config) - - if (TARGET SuiteSparse::${component}) - set_property (TARGET SuiteSparse::${component} APPEND PROPERTY - INTERFACE_LINK_LIBRARIES SuiteSparse::Config) - endif (TARGET SuiteSparse::${component}) - endforeach (component IN LISTS SuiteSparse_FIND_COMPONENTS) -endif (TARGET SuiteSparse::Config) - -# Check whether CHOLMOD was compiled with METIS support. The check can be -# performed only after the main components have been set up. -if (TARGET SuiteSparse::CHOLMOD) - # NOTE If SuiteSparse was compiled as a static library we'll need to link - # against METIS already during the check. Otherwise, the check can fail due to - # undefined references even though SuiteSparse was compiled with METIS. - find_package (METIS) - - if (TARGET METIS::METIS) - cmake_push_check_state (RESET) - set (CMAKE_REQUIRED_LIBRARIES SuiteSparse::CHOLMOD METIS::METIS) - check_symbol_exists (cholmod_metis cholmod.h SuiteSparse_CHOLMOD_USES_METIS) - cmake_pop_check_state () - - if (SuiteSparse_CHOLMOD_USES_METIS) - set_property (TARGET SuiteSparse::CHOLMOD APPEND PROPERTY - INTERFACE_LINK_LIBRARIES $) - - # Provide the SuiteSparse::Partition component whose availability indicates - # that CHOLMOD was compiled with the Partition module. - if (NOT TARGET SuiteSparse::Partition) - add_library (SuiteSparse::Partition IMPORTED INTERFACE) - endif (NOT TARGET SuiteSparse::Partition) - - set_property (TARGET SuiteSparse::Partition APPEND PROPERTY - INTERFACE_LINK_LIBRARIES SuiteSparse::CHOLMOD) - endif (SuiteSparse_CHOLMOD_USES_METIS) - endif (TARGET METIS::METIS) -endif (TARGET SuiteSparse::CHOLMOD) - -# We do not use suitesparse_find_component to find Partition and therefore must -# handle the availability in an extra step. -if (TARGET SuiteSparse::Partition) - set (SuiteSparse_Partition_FOUND TRUE) -else (TARGET SuiteSparse::Partition) - set (SuiteSparse_Partition_FOUND FALSE) -endif (TARGET SuiteSparse::Partition) - -suitesparse_reset_find_library_prefix() - -# Handle REQUIRED and QUIET arguments to FIND_PACKAGE -include(FindPackageHandleStandardArgs) -if (SuiteSparse_FOUND) - find_package_handle_standard_args(SuiteSparse - REQUIRED_VARS ${SuiteSparse_REQUIRED_VARS} - VERSION_VAR SuiteSparse_VERSION - FAIL_MESSAGE "Failed to find some/all required components of SuiteSparse." - HANDLE_COMPONENTS) -else (SuiteSparse_FOUND) - # Do not pass VERSION_VAR to FindPackageHandleStandardArgs() if we failed to - # find SuiteSparse to avoid a confusing autogenerated failure message - # that states 'not found (missing: FOO) (found version: x.y.z)'. - find_package_handle_standard_args(SuiteSparse - REQUIRED_VARS ${SuiteSparse_REQUIRED_VARS} - FAIL_MESSAGE "Failed to find some/all required components of SuiteSparse." - HANDLE_COMPONENTS) -endif (SuiteSparse_FOUND) - -# Pop CMP0057. -cmake_policy (POP) diff --git a/glomap/CMakeLists.txt b/glomap/CMakeLists.txt index 145adc4d..69b7c761 100644 --- a/glomap/CMakeLists.txt +++ b/glomap/CMakeLists.txt @@ -47,11 +47,9 @@ set(HEADERS io/colmap_io.h io/pose_io.h math/gravity.h - math/l1_solver.h math/rigid3d.h math/tree.h math/two_view_geometry.h - math/union_find.h processors/image_pair_inliers.h processors/image_undistorter.h processors/reconstruction_normalizer.h @@ -86,7 +84,6 @@ target_link_libraries( PUBLIC Eigen3::Eigen Ceres::ceres - SuiteSparse::CHOLMOD OpenMP::OpenMP_CXX ${BOOST_LIBRARIES} ) @@ -97,7 +94,6 @@ if(MSVC) else() target_compile_options(glomap PRIVATE -Wall - -Werror -Wno-sign-compare -Wno-unused-variable ) diff --git a/glomap/controllers/global_mapper_test.cc b/glomap/controllers/global_mapper_test.cc index dee5cada..98b8fc2b 100644 --- a/glomap/controllers/global_mapper_test.cc +++ b/glomap/controllers/global_mapper_test.cc @@ -60,7 +60,6 @@ TEST(GlobalMapper, WithoutNoise) { synthetic_dataset_options.num_cameras_per_rig = 1; synthetic_dataset_options.num_frames_per_rig = 7; synthetic_dataset_options.num_points3D = 50; - synthetic_dataset_options.point2D_stddev = 0; colmap::SynthesizeDataset( synthetic_dataset_options, >_reconstruction, database.get()); @@ -97,7 +96,6 @@ TEST(GlobalMapper, WithoutNoiseWithNonTrivialKnownRig) { synthetic_dataset_options.num_cameras_per_rig = 2; synthetic_dataset_options.num_frames_per_rig = 7; synthetic_dataset_options.num_points3D = 50; - synthetic_dataset_options.point2D_stddev = 0; synthetic_dataset_options.sensor_from_rig_translation_stddev = 0.1; // No noise synthetic_dataset_options.sensor_from_rig_rotation_stddev = 5.; // No noise @@ -137,7 +135,6 @@ TEST(GlobalMapper, WithoutNoiseWithNonTrivialUnknownRig) { synthetic_dataset_options.num_cameras_per_rig = 3; synthetic_dataset_options.num_frames_per_rig = 7; synthetic_dataset_options.num_points3D = 50; - synthetic_dataset_options.point2D_stddev = 0; synthetic_dataset_options.sensor_from_rig_translation_stddev = 0.1; // No noise synthetic_dataset_options.sensor_from_rig_rotation_stddev = 5.; // No noise @@ -187,10 +184,13 @@ TEST(GlobalMapper, WithNoiseAndOutliers) { synthetic_dataset_options.num_cameras_per_rig = 1; synthetic_dataset_options.num_frames_per_rig = 4; synthetic_dataset_options.num_points3D = 100; - synthetic_dataset_options.point2D_stddev = 0.5; synthetic_dataset_options.inlier_match_ratio = 0.6; colmap::SynthesizeDataset( synthetic_dataset_options, >_reconstruction, database.get()); + colmap::SyntheticNoiseOptions synthetic_noise_options; + synthetic_noise_options.point2D_stddev = 0.5; + colmap::SynthesizeNoise( + synthetic_noise_options, >_reconstruction, database.get()); ViewGraph view_graph; std::unordered_map cameras; diff --git a/glomap/controllers/rotation_averager_test.cc b/glomap/controllers/rotation_averager_test.cc index 095ff437..1dac8abd 100644 --- a/glomap/controllers/rotation_averager_test.cc +++ b/glomap/controllers/rotation_averager_test.cc @@ -135,7 +135,6 @@ TEST(RotationEstimator, WithoutNoise) { synthetic_dataset_options.num_cameras_per_rig = 1; synthetic_dataset_options.num_frames_per_rig = 5; synthetic_dataset_options.num_points3D = 50; - synthetic_dataset_options.point2D_stddev = 0; synthetic_dataset_options.sensor_from_rig_rotation_stddev = 20.; colmap::SynthesizeDataset( synthetic_dataset_options, >_reconstruction, database.get()); @@ -181,7 +180,6 @@ TEST(RotationEstimator, WithoutNoiseWithNoneTrivialKnownRig) { synthetic_dataset_options.num_cameras_per_rig = 2; synthetic_dataset_options.num_frames_per_rig = 4; synthetic_dataset_options.num_points3D = 50; - synthetic_dataset_options.point2D_stddev = 0; synthetic_dataset_options.sensor_from_rig_rotation_stddev = 20.; colmap::SynthesizeDataset( synthetic_dataset_options, >_reconstruction, database.get()); @@ -225,7 +223,6 @@ TEST(RotationEstimator, WithoutNoiseWithNoneTrivialUnknownRig) { synthetic_dataset_options.num_cameras_per_rig = 2; synthetic_dataset_options.num_frames_per_rig = 4; synthetic_dataset_options.num_points3D = 50; - synthetic_dataset_options.point2D_stddev = 0; synthetic_dataset_options.sensor_from_rig_rotation_stddev = 20.; colmap::SynthesizeDataset( synthetic_dataset_options, >_reconstruction, database.get()); @@ -277,10 +274,13 @@ TEST(RotationEstimator, WithNoiseAndOutliers) { synthetic_dataset_options.num_cameras_per_rig = 1; synthetic_dataset_options.num_frames_per_rig = 7; synthetic_dataset_options.num_points3D = 100; - synthetic_dataset_options.point2D_stddev = 1; synthetic_dataset_options.inlier_match_ratio = 0.6; colmap::SynthesizeDataset( synthetic_dataset_options, >_reconstruction, database.get()); + colmap::SyntheticNoiseOptions synthetic_noise_options; + synthetic_noise_options.point2D_stddev = 1; + colmap::SynthesizeNoise( + synthetic_noise_options, >_reconstruction, database.get()); ViewGraph view_graph; std::unordered_map rigs; @@ -323,10 +323,13 @@ TEST(RotationEstimator, WithNoiseAndOutliersWithNonTrivialKnownRigs) { synthetic_dataset_options.num_cameras_per_rig = 2; synthetic_dataset_options.num_frames_per_rig = 7; synthetic_dataset_options.num_points3D = 100; - synthetic_dataset_options.point2D_stddev = 1; synthetic_dataset_options.inlier_match_ratio = 0.6; colmap::SynthesizeDataset( synthetic_dataset_options, >_reconstruction, database.get()); + colmap::SyntheticNoiseOptions synthetic_noise_options; + synthetic_noise_options.point2D_stddev = 1; + colmap::SynthesizeNoise( + synthetic_noise_options, >_reconstruction, database.get()); ViewGraph view_graph; std::unordered_map rigs; @@ -372,7 +375,6 @@ TEST(RotationEstimator, RefineGravity) { synthetic_dataset_options.num_cameras_per_rig = 1; synthetic_dataset_options.num_frames_per_rig = 25; synthetic_dataset_options.num_points3D = 100; - synthetic_dataset_options.point2D_stddev = 0; colmap::SynthesizeDataset( synthetic_dataset_options, >_reconstruction, database.get()); @@ -416,7 +418,6 @@ TEST(RotationEstimator, RefineGravityWithNontrivialRigs) { synthetic_dataset_options.num_cameras_per_rig = 2; synthetic_dataset_options.num_frames_per_rig = 25; synthetic_dataset_options.num_points3D = 100; - synthetic_dataset_options.point2D_stddev = 0; colmap::SynthesizeDataset( synthetic_dataset_options, >_reconstruction, database.get()); diff --git a/glomap/controllers/track_establishment.cc b/glomap/controllers/track_establishment.cc index d4396ed3..77c307ec 100644 --- a/glomap/controllers/track_establishment.cc +++ b/glomap/controllers/track_establishment.cc @@ -5,7 +5,7 @@ namespace glomap { size_t TrackEngine::EstablishFullTracks( std::unordered_map& tracks) { tracks.clear(); - uf_.Clear(); + uf_ = {}; // Blindly concatenate tracks if any matches occur BlindConcatenation(); diff --git a/glomap/controllers/track_establishment.h b/glomap/controllers/track_establishment.h index 7e0c3ef0..1eb6a58b 100644 --- a/glomap/controllers/track_establishment.h +++ b/glomap/controllers/track_establishment.h @@ -1,9 +1,10 @@ #pragma once -#include "glomap/math/union_find.h" #include "glomap/scene/types_sfm.h" +#include + namespace glomap { struct TrackEstablishmentOptions { @@ -53,7 +54,7 @@ class TrackEngine { const std::unordered_map& images_; // Internal structure used for concatenating tracks - UnionFind uf_; + colmap::UnionFind uf_; }; } // namespace glomap diff --git a/glomap/estimators/global_rotation_averaging.cc b/glomap/estimators/global_rotation_averaging.cc index c78ee1cc..75b46d04 100644 --- a/glomap/estimators/global_rotation_averaging.cc +++ b/glomap/estimators/global_rotation_averaging.cc @@ -1,14 +1,17 @@ #include "global_rotation_averaging.h" #include "glomap/estimators/rotation_initializer.h" -#include "glomap/math/l1_solver.h" #include "glomap/math/rigid3d.h" #include "glomap/math/tree.h" +#include +#include + #include #include -#include "colmap/geometry/pose.h" +#include +#include namespace glomap { namespace { @@ -477,11 +480,15 @@ bool RotationEstimator::SolveL1Regression( const ViewGraph& view_graph, std::unordered_map& frames, std::unordered_map& images) { - L1SolverOptions opt_l1_solver; - opt_l1_solver.max_num_iterations = 10; + colmap::LeastAbsoluteDeviationSolver::Options l1_solver_options; + l1_solver_options.max_num_iterations = 10; + l1_solver_options.solver_type = colmap::LeastAbsoluteDeviationSolver:: + Options::SolverType::SupernodalCholmodLLT; + + const Eigen::SparseMatrix A = + weights_.matrix().asDiagonal() * sparse_matrix_; - L1Solver> l1_solver( - opt_l1_solver, weights_.matrix().asDiagonal() * sparse_matrix_); + colmap::LeastAbsoluteDeviationSolver l1_solver(l1_solver_options, A); double last_norm = 0; double curr_norm = 0; @@ -526,8 +533,8 @@ bool RotationEstimator::SolveL1Regression( iteration++; break; } - opt_l1_solver.max_num_iterations = - std::min(opt_l1_solver.max_num_iterations * 2, 100); + l1_solver_options.max_num_iterations = + std::min(l1_solver_options.max_num_iterations * 2, 100); } VLOG(2) << "L1 ADMM total iteration: " << iteration; return true; diff --git a/glomap/estimators/global_rotation_averaging.h b/glomap/estimators/global_rotation_averaging.h index fa0bdab4..e3d5057c 100644 --- a/glomap/estimators/global_rotation_averaging.h +++ b/glomap/estimators/global_rotation_averaging.h @@ -1,12 +1,13 @@ #pragma once -#include "glomap/math/l1_solver.h" #include "glomap/scene/types_sfm.h" #include "glomap/types.h" #include #include +#include + // Code is adapted from Theia's RobustRotationEstimator // (http://www.theia-sfm.org/). For gravity aligned rotation averaging, refere // to the paper "Gravity Aligned Rotation Averaging" diff --git a/glomap/estimators/rotation_initializer.cc b/glomap/estimators/rotation_initializer.cc index 3d1ca90e..2fd6c5e1 100644 --- a/glomap/estimators/rotation_initializer.cc +++ b/glomap/estimators/rotation_initializer.cc @@ -1,6 +1,7 @@ #include "glomap/estimators/rotation_initializer.h" -#include "colmap/geometry/pose.h" +#include + namespace glomap { bool ConvertRotationsFromImageToRig( diff --git a/glomap/io/colmap_converter.cc b/glomap/io/colmap_converter.cc index 9fc1cc53..3fcb0959 100644 --- a/glomap/io/colmap_converter.cc +++ b/glomap/io/colmap_converter.cc @@ -2,7 +2,7 @@ #include "glomap/math/two_view_geometry.h" -#include "colmap/scene/reconstruction_io_utils.h" +#include namespace glomap { diff --git a/glomap/math/l1_solver.h b/glomap/math/l1_solver.h deleted file mode 100644 index 1327bcae..00000000 --- a/glomap/math/l1_solver.h +++ /dev/null @@ -1,113 +0,0 @@ -// This code is adapted from Theia library (http://theia-sfm.org/), -// with its original L1 solver adapted from -// "https://web.stanford.edu/~boyd/papers/admm/least_abs_deviations/lad.html" - -#pragma once - -#include - -#include -#include -#include - -// An L1 norm (|| A * x - b ||_1) approximation solver based on ADMM -// (alternating direction method of multipliers, -// https://web.stanford.edu/~boyd/papers/pdf/admm_distr_stats.pdf). -namespace glomap { - -// TODO: L1 solver for dense matrix -struct L1SolverOptions { - int max_num_iterations = 1000; - // Rho is the augmented Lagrangian parameter. - double rho = 1.0; - // Alpha is the over-relaxation parameter (typically between 1.0 and 1.8). - double alpha = 1.0; - - double absolute_tolerance = 1e-4; - double relative_tolerance = 1e-2; -}; - -template -class L1Solver { - public: - L1Solver(const L1SolverOptions& options, const MatrixType& mat) - : options_(options), a_(mat) { - // Pre-compute the sparsity pattern. - const MatrixType spd_mat = a_.transpose() * a_; - linear_solver_.compute(spd_mat); - } - - void Solve(const Eigen::VectorXd& rhs, Eigen::VectorXd* solution) { - Eigen::VectorXd& x = *solution; - Eigen::VectorXd z(a_.rows()), u(a_.rows()); - z.setZero(); - u.setZero(); - - Eigen::VectorXd a_times_x(a_.rows()), z_old(z.size()), ax_hat(a_.rows()); - // Precompute some convergence terms. - const double rhs_norm = rhs.norm(); - const double primal_abs_tolerance_eps = - std::sqrt(a_.rows()) * options_.absolute_tolerance; - const double dual_abs_tolerance_eps = - std::sqrt(a_.cols()) * options_.absolute_tolerance; - - const std::string row_format = - " % 4d % 4.4e % 4.4e % 4.4e % 4.4e"; - for (int i = 0; i < options_.max_num_iterations; i++) { - // Update x. - x.noalias() = linear_solver_.solve(a_.transpose() * (rhs + z - u)); - if (linear_solver_.info() != Eigen::Success) { - LOG(ERROR) << "L1 Minimization failed. Could not solve the sparse " - "linear system with Cholesky Decomposition"; - return; - } - - a_times_x.noalias() = a_ * x; - ax_hat.noalias() = options_.alpha * a_times_x; - ax_hat.noalias() += (1.0 - options_.alpha) * (z + rhs); - - // Update z and set z_old. - std::swap(z, z_old); - z.noalias() = Shrinkage(ax_hat - rhs + u, 1.0 / options_.rho); - - // Update u. - u.noalias() += ax_hat - z - rhs; - - // Compute the convergence terms. - const double r_norm = (a_times_x - z - rhs).norm(); - const double s_norm = - (-options_.rho * a_.transpose() * (z - z_old)).norm(); - const double max_norm = std::max({a_times_x.norm(), z.norm(), rhs_norm}); - const double primal_eps = - primal_abs_tolerance_eps + options_.relative_tolerance * max_norm; - const double dual_eps = dual_abs_tolerance_eps + - options_.relative_tolerance * - (options_.rho * a_.transpose() * u).norm(); - - // Determine if the minimizer has converged. - if (r_norm < primal_eps && s_norm < dual_eps) { - break; - } - } - } - - private: - const L1SolverOptions& options_; - - // Matrix A in || Ax - b ||_1 - const MatrixType a_; - - // Cholesky linear solver. Since our linear system will be a SPD matrix we can - // utilize the Cholesky factorization. - Eigen::CholmodSupernodalLLT> linear_solver_; - - static Eigen::VectorXd Shrinkage(const Eigen::VectorXd& vec, - const double kappa) { - Eigen::ArrayXd zero_vec(vec.size()); - zero_vec.setZero(); - return zero_vec.max(vec.array() - kappa) - - zero_vec.max(-vec.array() - kappa); - } -}; - -} // namespace glomap diff --git a/glomap/math/union_find.h b/glomap/math/union_find.h deleted file mode 100644 index 8c9687f9..00000000 --- a/glomap/math/union_find.h +++ /dev/null @@ -1,40 +0,0 @@ -#pragma once -#include -#include - -namespace glomap { - -// UnionFind class to maintain disjoint sets for creating tracks -template -class UnionFind { - public: - // Find the root of the element x - DataType Find(DataType x) { - // If x is not in parent map, initialize it with x as its parent - auto parentIt = parent_.find(x); - if (parentIt == parent_.end()) { - parent_.emplace_hint(parentIt, x, x); - return x; - } - // Path compression: set the parent of x to the root of the set containing x - if (parentIt->second != x) { - parentIt->second = Find(parentIt->second); - } - return parentIt->second; - } - - // Unite the sets containing x and y - void Union(DataType x, DataType y) { - DataType root_x = Find(x); - DataType root_y = Find(y); - if (root_x != root_y) parent_[root_x] = root_y; - } - - void Clear() { parent_.clear(); } - - private: - // Map to store the parent of each element - std::unordered_map parent_; -}; - -} // namespace glomap diff --git a/glomap/processors/reconstruction_normalizer.h b/glomap/processors/reconstruction_normalizer.h index 3d3c3193..6566b849 100644 --- a/glomap/processors/reconstruction_normalizer.h +++ b/glomap/processors/reconstruction_normalizer.h @@ -2,7 +2,7 @@ #include "glomap/scene/types_sfm.h" -#include "colmap/geometry/pose.h" +#include namespace glomap { @@ -16,4 +16,5 @@ colmap::Sim3d NormalizeReconstruction( double extent = 10., double p0 = 0.1, double p1 = 0.9); + } // namespace glomap diff --git a/glomap/processors/view_graph_manipulation.cc b/glomap/processors/view_graph_manipulation.cc index db5bc3ca..e63a57a2 100644 --- a/glomap/processors/view_graph_manipulation.cc +++ b/glomap/processors/view_graph_manipulation.cc @@ -1,8 +1,8 @@ #include "view_graph_manipulation.h" #include "glomap/math/two_view_geometry.h" -#include "glomap/math/union_find.h" +#include #include namespace glomap { @@ -77,7 +77,8 @@ image_t ViewGraphManipulater::EstablishStrongClusters( view_graph.KeepLargestConnectedComponents(frames, images); // Construct the initial cluster by keeping the pairs with weight > min_thres - UnionFind uf; + colmap::UnionFind uf; + uf.Reserve(frames.size()); // Go through the edges, and add the edge with weight > min_thres for (auto& [pair_id, image_pair] : view_graph.image_pairs) { if (image_pair.is_valid == false) continue; diff --git a/glomap/scene/view_graph.cc b/glomap/scene/view_graph.cc index b1835b30..60009116 100644 --- a/glomap/scene/view_graph.cc +++ b/glomap/scene/view_graph.cc @@ -1,7 +1,5 @@ #include "glomap/scene/view_graph.h" -#include "glomap/math/union_find.h" - #include namespace glomap { diff --git a/scripts/format/c++.sh b/scripts/format/c++.sh index 2b5a64b9..15ddc2f0 100755 --- a/scripts/format/c++.sh +++ b/scripts/format/c++.sh @@ -3,12 +3,12 @@ # This script applies clang-format to the whole repository. # Check version -version_string=$(clang-format --version | sed -E 's/^.* ([0-9]+\.[0-9]+)\..*$/\1/') -expected_version_string='19.1' -if [[ "$version_string" == "$expected_version_string" ]]; then - echo "clang-format major.minor version '$version_string' matches expected '$expected_version_string'" +version_string=$(clang-format --version | sed -E 's/^.*(\d+\.\d+\.\d+-.*).*$/\1/') +expected_version_string='20.1.5' +if [[ "$version_string" =~ "$expected_version_string" ]]; then + echo "clang-format version '$version_string' matches '$expected_version_string'" else - echo "clang-format major.minor version '$version_string' doesn't match expected '$expected_version_string'" + echo "clang-format version '$version_string' doesn't match '$expected_version_string'" exit 1 fi diff --git a/thirdparty/CMakeLists.txt b/thirdparty/CMakeLists.txt new file mode 100644 index 00000000..b9107f1c --- /dev/null +++ b/thirdparty/CMakeLists.txt @@ -0,0 +1,37 @@ +if(IS_GNU OR IS_CLANG) + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -w") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w") +endif() + +include(FetchContent) + +FetchContent_Declare(poselib + GIT_REPOSITORY https://github.com/PoseLib/PoseLib.git + GIT_TAG f119951fca625133112acde48daffa5f20eba451 + EXCLUDE_FROM_ALL + SYSTEM +) +message(STATUS "Configuring PoseLib...") +if(FETCH_POSELIB) + set(MARCH_NATIVE OFF CACHE BOOL "") + FetchContent_MakeAvailable(poselib) +else() + find_package(PoseLib REQUIRED) +endif() +message(STATUS "Configuring PoseLib... done") + +FetchContent_Declare(COLMAP + GIT_REPOSITORY https://github.com/colmap/colmap.git + GIT_TAG b6b7b54eca6078070f73a3f0a084f79c629a6f10 # Nov 20, 2025 + EXCLUDE_FROM_ALL + SYSTEM +) +message(STATUS "Configuring COLMAP...") +set(UNINSTALL_ENABLED OFF CACHE INTERNAL "") +set(GUI_ENABLED OFF CACHE INTERNAL "") +if (FETCH_COLMAP) + FetchContent_MakeAvailable(COLMAP) +else() + find_package(COLMAP REQUIRED) +endif() +message(STATUS "Configuring COLMAP... done") From ce08ebef3ea494ac8931e0124a659c8db2f77ad2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Johannes=20Sch=C3=B6nberger?= Date: Sat, 22 Nov 2025 18:08:50 +0100 Subject: [PATCH 40/45] View graph simplification (#225) * Update to latest colmap and use L1 solver + union-find from colmap * Simplify view graph * d * d --- .../estimators/global_rotation_averaging.cc | 2 +- glomap/estimators/gravity_refinement.cc | 35 +++-- glomap/io/colmap_converter.cc | 4 +- glomap/io/pose_io.cc | 20 +-- glomap/processors/image_pair_inliers.cc | 2 +- glomap/processors/reconstruction_pruning.cc | 18 ++- glomap/processors/relpose_filter.cc | 2 +- glomap/processors/view_graph_manipulation.cc | 7 +- glomap/scene/image_pair.h | 44 ++---- glomap/scene/view_graph.cc | 136 ++++++++++-------- glomap/scene/view_graph.h | 72 +++------- 11 files changed, 146 insertions(+), 196 deletions(-) diff --git a/glomap/estimators/global_rotation_averaging.cc b/glomap/estimators/global_rotation_averaging.cc index 75b46d04..8246b272 100644 --- a/glomap/estimators/global_rotation_averaging.cc +++ b/glomap/estimators/global_rotation_averaging.cc @@ -121,7 +121,7 @@ void RotationEstimator::InitializeFromMaximumSpanningTree( // Directly use the relative pose for estimation rotation const ImagePair& image_pair = view_graph.image_pairs.at( - ImagePair::ImagePairToPairId(curr, parents[curr])); + colmap::ImagePairToPairId(curr, parents[curr])); if (image_pair.image_id1 == curr) { // 1_R_w = 2_R_1^T * 2_R_w cam_from_worlds[curr].rotation = diff --git a/glomap/estimators/gravity_refinement.cc b/glomap/estimators/gravity_refinement.cc index bb19e28d..29f7adb6 100644 --- a/glomap/estimators/gravity_refinement.cc +++ b/glomap/estimators/gravity_refinement.cc @@ -9,12 +9,10 @@ namespace glomap { void GravityRefiner::RefineGravity(const ViewGraph& view_graph, std::unordered_map& frames, std::unordered_map& images) { - const std::unordered_map& image_pairs = - view_graph.image_pairs; const std::unordered_map>& - adjacency_list = view_graph.GetAdjacencyList(); + adjacency_list = view_graph.CreateImageAdjacencyList(); if (adjacency_list.empty()) { - LOG(INFO) << "Adjacency list not established" << std::endl; + LOG(INFO) << "Adjacency list not established"; return; } @@ -24,16 +22,16 @@ void GravityRefiner::RefineGravity(const ViewGraph& view_graph, IdentifyErrorProneGravity(view_graph, frames, images, error_prone_frames); if (error_prone_frames.empty()) { - LOG(INFO) << "No error prone frames found" << std::endl; + LOG(INFO) << "No error prone frames found"; return; } // Get the relevant pair ids for frames std::unordered_map> adjacency_list_frames_to_pair_id; for (auto& [image_id, neighbors] : adjacency_list) { - for (auto neighbor : neighbors) { + for (const auto& neighbor : neighbors) { adjacency_list_frames_to_pair_id[images[image_id].frame_id].insert( - ImagePair::ImagePairToPairId(image_id, neighbor)); + colmap::ImagePairToPairId(image_id, neighbor)); } } @@ -59,8 +57,8 @@ void GravityRefiner::RefineGravity(const ViewGraph& view_graph, int counter = 0; Eigen::Vector3d gravity = frames[frame_id].gravity_info.GetGravity(); for (const auto& pair_id : neighbors) { - image_t image_id1 = image_pairs.at(pair_id).image_id1; - image_t image_id2 = image_pairs.at(pair_id).image_id2; + const image_t image_id1 = view_graph.image_pairs.at(pair_id).image_id1; + const image_t image_id2 = view_graph.image_pairs.at(pair_id).image_id2; if (!images.at(image_id1).HasGravity() || !images.at(image_id2).HasGravity()) continue; @@ -82,17 +80,18 @@ void GravityRefiner::RefineGravity(const ViewGraph& view_graph, // consider a single cost term if (images.at(image_id1).frame_id == frame_id) { gravities.emplace_back( - (colmap::Inverse(image_pairs.at(pair_id).cam2_from_cam1 * + (colmap::Inverse(view_graph.image_pairs.at(pair_id).cam2_from_cam1 * cam1_from_rig1) .rotation.toRotationMatrix() * images[image_id2].GetRAlign()) .col(1)); } else if (images.at(image_id2).frame_id == frame_id) { - gravities.emplace_back(((colmap::Inverse(cam2_from_rig2) * - image_pairs.at(pair_id).cam2_from_cam1) - .rotation.toRotationMatrix() * - images[image_id1].GetRAlign()) - .col(1)); + gravities.emplace_back( + ((colmap::Inverse(cam2_from_rig2) * + view_graph.image_pairs.at(pair_id).cam2_from_cam1) + .rotation.toRotationMatrix() * + images[image_id1].GetRAlign()) + .col(1)); } ceres::CostFunction* coor_cost = @@ -123,9 +122,8 @@ void GravityRefiner::RefineGravity(const ViewGraph& view_graph, frames[frame_id].gravity_info.SetGravity(gravity); } } - std::cout << std::endl; LOG(INFO) << "Number of rectified frames: " << counter_rect << " / " - << error_prone_frames.size() << std::endl; + << error_prone_frames.size(); } void GravityRefiner::IdentifyErrorProneGravity( @@ -179,7 +177,6 @@ void GravityRefiner::IdentifyErrorProneGravity( error_prone_frames.insert(frame_id); } } - LOG(INFO) << "Number of error prone frames: " << error_prone_frames.size() - << std::endl; + LOG(INFO) << "Number of error prone frames: " << error_prone_frames.size(); } } // namespace glomap diff --git a/glomap/io/colmap_converter.cc b/glomap/io/colmap_converter.cc index 3fcb0959..13dc8308 100644 --- a/glomap/io/colmap_converter.cc +++ b/glomap/io/colmap_converter.cc @@ -361,7 +361,7 @@ void ConvertDatabaseToGlomap(const colmap::Database& database, // Initialize the image pair auto ite = image_pairs.insert( - std::make_pair(ImagePair::ImagePairToPairId(image_id1, image_id2), + std::make_pair(colmap::ImagePairToPairId(image_id1, image_id2), ImagePair(image_id1, image_id2))); ImagePair& image_pair = ite.first->second; @@ -419,8 +419,6 @@ void ConvertDatabaseToGlomap(const colmap::Database& database, } image_pair.matches.conservativeResize(count, 2); } - std::cout << std::endl; - LOG(INFO) << "Pairs read done. " << invalid_count << " / " << view_graph.image_pairs.size() << " are invalid"; } diff --git a/glomap/io/pose_io.cc b/glomap/io/pose_io.cc index 49835a8c..d2349f3c 100644 --- a/glomap/io/pose_io.cc +++ b/glomap/io/pose_io.cc @@ -56,7 +56,7 @@ void ReadRelPose(const std::string& file_path, image_t index1 = name_idx[file1]; image_t index2 = name_idx[file2]; - image_pair_t pair_id = ImagePair::ImagePairToPairId(index1, index2); + const image_pair_t pair_id = colmap::ImagePairToPairId(index1, index2); // rotation Rigid3d pose_rel; @@ -81,7 +81,7 @@ void ReadRelPose(const std::string& file_path, } counter++; } - LOG(INFO) << counter << " relpose are loaded" << std::endl; + LOG(INFO) << counter << " relative poses are loaded"; } void ReadRelWeight(const std::string& file_path, @@ -118,7 +118,7 @@ void ReadRelWeight(const std::string& file_path, image_t index1 = name_idx[file1]; image_t index2 = name_idx[file2]; - image_pair_t pair_id = ImagePair::ImagePairToPairId(index1, index2); + image_pair_t pair_id = colmap::ImagePairToPairId(index1, index2); if (view_graph.image_pairs.find(pair_id) == view_graph.image_pairs.end()) continue; @@ -127,7 +127,7 @@ void ReadRelWeight(const std::string& file_path, view_graph.image_pairs[pair_id].weight = std::stod(item); counter++; } - LOG(INFO) << counter << " weights are used are loaded" << std::endl; + LOG(INFO) << counter << " weights are used are loaded"; } // TODO: now, we only store 1 single gravity per rig. @@ -172,7 +172,7 @@ void ReadGravity(const std::string& gravity_path, } } } - LOG(INFO) << counter << " images are loaded with gravity" << std::endl; + LOG(INFO) << counter << " images are loaded with gravity"; } void WriteGlobalRotation(const std::string& file_path, @@ -185,7 +185,7 @@ void WriteGlobalRotation(const std::string& file_path, } } for (const auto& image_id : existing_images) { - const auto image = images.at(image_id); + const auto& image = images.at(image_id); if (!image.IsRegistered()) continue; file << image.file_name; Rigid3d cam_from_world = image.CamFromWorld(); @@ -205,8 +205,8 @@ void WriteRelPose(const std::string& file_path, std::map name_pair; for (const auto& [pair_id, image_pair] : view_graph.image_pairs) { if (image_pair.is_valid) { - const auto image1 = images.at(image_pair.image_id1); - const auto image2 = images.at(image_pair.image_id2); + const auto& image1 = images.at(image_pair.image_id1); + const auto& image2 = images.at(image_pair.image_id2); name_pair[image1.file_name + " " + image2.file_name] = pair_id; } } @@ -226,6 +226,6 @@ void WriteRelPose(const std::string& file_path, file << "\n"; } - LOG(INFO) << name_pair.size() << " relpose are written" << std::endl; + LOG(INFO) << name_pair.size() << " relpose are written"; } -} // namespace glomap \ No newline at end of file +} // namespace glomap diff --git a/glomap/processors/image_pair_inliers.cc b/glomap/processors/image_pair_inliers.cc index c32878f1..a0e14594 100644 --- a/glomap/processors/image_pair_inliers.cc +++ b/glomap/processors/image_pair_inliers.cc @@ -212,4 +212,4 @@ void ImagePairsInlierCount(ViewGraph& view_graph, } } -} // namespace glomap \ No newline at end of file +} // namespace glomap diff --git a/glomap/processors/reconstruction_pruning.cc b/glomap/processors/reconstruction_pruning.cc index 014a10e5..6fc0eb2f 100644 --- a/glomap/processors/reconstruction_pruning.cc +++ b/glomap/processors/reconstruction_pruning.cc @@ -23,8 +23,8 @@ image_t PruneWeaklyConnectedImages(std::unordered_map& frames, image_t image_id2 = track.observations[j].first; frame_t frame_id2 = images[image_id2].frame_id; if (frame_id1 == frame_id2) continue; - image_pair_t pair_id = - ImagePair::ImagePairToPairId(frame_id1, frame_id2); + const image_pair_t pair_id = + colmap::ImagePairToPairId(frame_id1, frame_id2); if (pair_covisibility_count.find(pair_id) == pair_covisibility_count.end()) { pair_covisibility_count[pair_id] = 1; @@ -44,8 +44,7 @@ image_t PruneWeaklyConnectedImages(std::unordered_map& frames, // then require each pair to have at least 5 points if (count >= 5) { counter++; - image_t image_id1, image_id2; - ImagePair::PairIdToImagePair(pair_id, image_id1, image_id2); + const auto [image_id1, image_id2] = colmap::PairIdToImagePair(pair_id); if (frame_observation_count[image_id1] < min_num_observations || frame_observation_count[image_id2] < min_num_observations) @@ -76,10 +75,9 @@ image_t PruneWeaklyConnectedImages(std::unordered_map& frames, ViewGraph visibility_graph; for (auto& [pair_id, image_pair] : visibility_graph_frame.image_pairs) { - frame_t frame_id1, frame_id2; - ImagePair::PairIdToImagePair(pair_id, frame_id1, frame_id2); - image_t image_id1 = frame_id_to_begin_img[frame_id1]; - image_t image_id2 = frame_id_to_begin_img[frame_id2]; + const auto [frame_id1, frame_id2] = colmap::PairIdToImagePair(pair_id); + const image_t image_id1 = frame_id_to_begin_img[frame_id1]; + const image_t image_id2 = frame_id_to_begin_img[frame_id2]; visibility_graph.image_pairs.insert( std::make_pair(pair_id, ImagePair(image_id1, image_id2))); visibility_graph.image_pairs[pair_id].weight = image_pair.weight; @@ -96,7 +94,7 @@ image_t PruneWeaklyConnectedImages(std::unordered_map& frames, if (image_id == begin_image_id || images.find(image_id) == images.end()) continue; image_pair_t pair_id = - ImagePair::ImagePairToPairId(begin_image_id, image_id); + colmap::ImagePairToPairId(begin_image_id, image_id); visibility_graph.image_pairs.insert( std::make_pair(pair_id, ImagePair(begin_image_id, image_id))); @@ -132,4 +130,4 @@ image_t PruneWeaklyConnectedImages(std::unordered_map& frames, // return visibility_graph.MarkConnectedComponents(images, min_num_images); } -} // namespace glomap \ No newline at end of file +} // namespace glomap diff --git a/glomap/processors/relpose_filter.cc b/glomap/processors/relpose_filter.cc index 8af7cf80..1f14cd07 100644 --- a/glomap/processors/relpose_filter.cc +++ b/glomap/processors/relpose_filter.cc @@ -64,4 +64,4 @@ void RelPoseFilter::FilterInlierRatio(ViewGraph& view_graph, << " relative poses with inlier ratio < " << min_inlier_ratio; } -} // namespace glomap \ No newline at end of file +} // namespace glomap diff --git a/glomap/processors/view_graph_manipulation.cc b/glomap/processors/view_graph_manipulation.cc index e63a57a2..24c36726 100644 --- a/glomap/processors/view_graph_manipulation.cc +++ b/glomap/processors/view_graph_manipulation.cc @@ -17,7 +17,7 @@ image_pair_t ViewGraphManipulater::SparsifyGraph( // Keep track of chosen edges std::unordered_set chosen_edges; const std::unordered_map>& - adjacency_list = view_graph.GetAdjacencyList(); + adjacency_list = view_graph.CreateImageAdjacencyList(); // Here, the average is the mean of the degrees double average_degree = 0; @@ -47,6 +47,7 @@ image_pair_t ViewGraphManipulater::SparsifyGraph( continue; } + // TODO: Replace rand() with thread-safe random number generator. if (rand() / double(RAND_MAX) < (expected_degree * average_degree) / (degree1 * degree2)) { chosen_edges.insert(pair_id); @@ -208,9 +209,7 @@ void ViewGraphManipulater::UpdateImagePairsConfig( // pairs are valid, then set the camera to valid std::unordered_map camera_validity; for (auto& [camera_id, counter] : camera_counter) { - if (counter.first == 0) { - camera_validity[camera_id] = false; - } else if (counter.second * 1. / counter.first > 0.5) { + if (counter.second * 1. / counter.first > 0.5) { camera_validity[camera_id] = true; } else { camera_validity[camera_id] = false; diff --git a/glomap/scene/image_pair.h b/glomap/scene/image_pair.h index 67bf2ed3..31b01386 100644 --- a/glomap/scene/image_pair.h +++ b/glomap/scene/image_pair.h @@ -9,19 +9,24 @@ namespace glomap { -// FUTURE: add covariance to the relative pose +// TODO: add covariance to the relative pose struct ImagePair { - ImagePair() : pair_id(-1), image_id1(-1), image_id2(-1) {} - ImagePair(image_t img_id1, image_t img_id2, Rigid3d pose_rel = Rigid3d()) - : pair_id(ImagePairToPairId(img_id1, img_id2)), - image_id1(img_id1), - image_id2(img_id2), - cam2_from_cam1(pose_rel) {} + ImagePair() + : image_id1(colmap::kInvalidImageId), + image_id2(colmap::kInvalidImageId), + pair_id(colmap::kInvalidImagePairId) {} + ImagePair(image_t image_id1, + image_t image_id2, + Rigid3d cam2_from_cam1 = Rigid3d()) + : image_id1(image_id1), + image_id2(image_id2), + pair_id(colmap::ImagePairToPairId(image_id1, image_id2)), + cam2_from_cam1(cam2_from_cam1) {} // Ids are kept constant - const image_pair_t pair_id; const image_t image_id1; const image_t image_id2; + const image_pair_t pair_id; // indicator whether the image pair is valid bool is_valid = true; @@ -40,7 +45,7 @@ struct ImagePair { Eigen::Matrix3d H = Eigen::Matrix3d::Zero(); // Relative pose. - Rigid3d cam2_from_cam1; + Rigid3d cam2_from_cam1 = Rigid3d(); // Matches between the two images. // First column is the index of the feature in the first image. @@ -49,27 +54,6 @@ struct ImagePair { // Row index of inliers in the matches matrix. std::vector inliers; - - static inline image_pair_t ImagePairToPairId(const image_t image_id1, - const image_t image_id2); - - static inline void PairIdToImagePair(const image_pair_t pair_id, - image_t& image_id1, - image_t& image_id2); }; -image_pair_t ImagePair::ImagePairToPairId(const image_t image_id1, - const image_t image_id2) { - return colmap::ImagePairToPairId(image_id1, image_id2); -} - -void ImagePair::PairIdToImagePair(const image_pair_t pair_id, - image_t& image_id1, - image_t& image_id2) { - std::pair image_id_pair = - colmap::PairIdToImagePair(pair_id); - image_id1 = image_id_pair.first; - image_id2 = image_id_pair.second; -} - } // namespace glomap diff --git a/glomap/scene/view_graph.cc b/glomap/scene/view_graph.cc index 60009116..5e8e855e 100644 --- a/glomap/scene/view_graph.cc +++ b/glomap/scene/view_graph.cc @@ -3,18 +3,65 @@ #include namespace glomap { +namespace { + +void BreadthFirstSearch( + const std::unordered_map>& + adjacency_list, + image_t root, + std::unordered_map& visited, + std::unordered_set& component) { + std::queue queue; + queue.push(root); + visited[root] = true; + component.insert(root); + + while (!queue.empty()) { + const image_t curr = queue.front(); + queue.pop(); + + for (const image_t neighbor : adjacency_list.at(curr)) { + if (!visited[neighbor]) { + queue.push(neighbor); + visited[neighbor] = true; + component.insert(neighbor); + } + } + } +} + +std::vector> FindConnectedComponents( + const std::unordered_map>& + adjacency_list) { + std::vector> connected_components; + std::unordered_map visited; + visited.reserve(adjacency_list.size()); + for (const auto& [frame_id, neighbors] : adjacency_list) { + visited[frame_id] = false; + } + + for (auto& [frame_id, _] : adjacency_list) { + if (!visited[frame_id]) { + std::unordered_set component; + BreadthFirstSearch(adjacency_list, frame_id, visited, component); + connected_components.push_back(std::move(component)); + } + } + + return connected_components; +} + +} // namespace int ViewGraph::KeepLargestConnectedComponents( std::unordered_map& frames, std::unordered_map& images) { - EstablishAdjacencyList(); - EstablishAdjacencyListFrame(images); - - int num_comp = FindConnectedComponent(); + const std::vector> connected_components = + FindConnectedComponents(CreateFrameAdjacencyList(images)); int max_idx = -1; int max_img = 0; - for (int comp = 0; comp < num_comp; comp++) { + for (int comp = 0; comp < connected_components.size(); comp++) { if (connected_components[comp].size() > max_img) { max_img = connected_components[comp].size(); max_idx = comp; @@ -23,7 +70,8 @@ int ViewGraph::KeepLargestConnectedComponents( if (max_img == 0) return 0; - std::unordered_set largest_component = connected_components[max_idx]; + const std::unordered_set& largest_component = + connected_components[max_idx]; // Set all frames to not registered for (auto& [frame_id, frame] : frames) { @@ -34,13 +82,11 @@ int ViewGraph::KeepLargestConnectedComponents( frames[frame_id].is_registered = true; } // set all pairs not in the largest component to invalid - num_pairs = 0; for (auto& [pair_id, image_pair] : image_pairs) { if (!images[image_pair.image_id1].IsRegistered() || !images[image_pair.image_id2].IsRegistered()) { image_pair.is_valid = false; } - if (image_pair.is_valid) num_pairs++; } for (auto& [image_id, image] : images) { @@ -49,32 +95,13 @@ int ViewGraph::KeepLargestConnectedComponents( return max_img; } -int ViewGraph::FindConnectedComponent() { - connected_components.clear(); - std::unordered_map visited; - for (auto& [frame_id, neighbors] : adjacency_list_frame) { - visited[frame_id] = false; - } - - for (auto& [frame_id, neighbors] : adjacency_list_frame) { - if (!visited[frame_id]) { - std::unordered_set component; - BFS(frame_id, visited, component); - connected_components.push_back(component); - } - } - - return connected_components.size(); -} - int ViewGraph::MarkConnectedComponents( std::unordered_map& frames, std::unordered_map& images, int min_num_img) { - EstablishAdjacencyList(); - EstablishAdjacencyListFrame(images); - - int num_comp = FindConnectedComponent(); + const std::vector> connected_components = + FindConnectedComponents(CreateFrameAdjacencyList(images)); + const int num_comp = connected_components.size(); std::vector> cluster_num_img(num_comp); for (int comp = 0; comp < num_comp; comp++) { @@ -97,48 +124,31 @@ int ViewGraph::MarkConnectedComponents( return comp; } -void ViewGraph::BFS(image_t root, - std::unordered_map& visited, - std::unordered_set& component) { - std::queue q; - q.push(root); - visited[root] = true; - component.insert(root); - - while (!q.empty()) { - image_t curr = q.front(); - q.pop(); - - for (image_t neighbor : adjacency_list_frame[curr]) { - if (!visited[neighbor]) { - q.push(neighbor); - visited[neighbor] = true; - component.insert(neighbor); - } - } - } -} - -void ViewGraph::EstablishAdjacencyList() { - adjacency_list.clear(); - for (auto& [pair_id, image_pair] : image_pairs) { +std::unordered_map> +ViewGraph::CreateImageAdjacencyList() const { + std::unordered_map> adjacency_list; + for (const auto& [_, image_pair] : image_pairs) { if (image_pair.is_valid) { adjacency_list[image_pair.image_id1].insert(image_pair.image_id2); adjacency_list[image_pair.image_id2].insert(image_pair.image_id1); } } + return adjacency_list; } -void ViewGraph::EstablishAdjacencyListFrame( - std::unordered_map& images) { - adjacency_list_frame.clear(); - for (auto& [pair_id, image_pair] : image_pairs) { +std::unordered_map> +ViewGraph::CreateFrameAdjacencyList( + const std::unordered_map& images) const { + std::unordered_map> adjacency_list; + for (const auto& [_, image_pair] : image_pairs) { if (image_pair.is_valid) { - frame_t frame_id1 = images[image_pair.image_id1].frame_id; - frame_t frame_id2 = images[image_pair.image_id2].frame_id; - adjacency_list_frame[frame_id1].insert(frame_id2); - adjacency_list_frame[frame_id2].insert(frame_id1); + const frame_t frame_id1 = images.at(image_pair.image_id1).frame_id; + const frame_t frame_id2 = images.at(image_pair.image_id2).frame_id; + adjacency_list[frame_id1].insert(frame_id2); + adjacency_list[frame_id2].insert(frame_id1); } } + return adjacency_list; } + } // namespace glomap diff --git a/glomap/scene/view_graph.h b/glomap/scene/view_graph.h index 21c1a16c..74497e24 100644 --- a/glomap/scene/view_graph.h +++ b/glomap/scene/view_graph.h @@ -1,73 +1,37 @@ #pragma once -#include "glomap/scene/camera.h" #include "glomap/scene/image.h" #include "glomap/scene/image_pair.h" #include "glomap/scene/types.h" -#include "glomap/types.h" + +#include +#include namespace glomap { -class ViewGraph { - public: - // Methods - inline void RemoveInvalidPair(image_pair_t pair_id); +struct ViewGraph { + std::unordered_map image_pairs; + + // Create the adjacency list for the images in the view graph. + std::unordered_map> + CreateImageAdjacencyList() const; + + // Create the adjacency list for the frames in the view graph. + std::unordered_map> + CreateFrameAdjacencyList( + const std::unordered_map& images) const; - // Mark the image which is not connected to any other images as not registered - // Return: the number of images in the largest connected component + // Mark the images which are not connected to any other images as not + // registered Returns the number of images in the largest connected component. int KeepLargestConnectedComponents( std::unordered_map& frames, std::unordered_map& images); - // Mark the cluster of the cameras (cluster_id sort by the the number of - // images) + // Mark connected clusters of images, where the cluster_id is sorted by the + // the number of images. int MarkConnectedComponents(std::unordered_map& frames, std::unordered_map& images, int min_num_img = -1); - - // Establish the adjacency list - void EstablishAdjacencyList(); - - // Establish the frame based adjacency list - void EstablishAdjacencyListFrame(std::unordered_map& images); - - inline const std::unordered_map>& - GetAdjacencyList() const; - inline const std::unordered_map>& - GetAdjacencyListFrame() const; - - // Data - std::unordered_map image_pairs; - - image_t num_images = 0; - image_pair_t num_pairs = 0; - - private: - int FindConnectedComponent(); - - void BFS(image_t root, - std::unordered_map& visited, - std::unordered_set& component); - - // Data for processing - std::unordered_map> adjacency_list; - std::unordered_map> adjacency_list_frame; - std::vector> connected_components; }; -const std::unordered_map>& -ViewGraph::GetAdjacencyList() const { - return adjacency_list; -} - -const std::unordered_map>& -ViewGraph::GetAdjacencyListFrame() const { - return adjacency_list_frame; -} - -void ViewGraph::RemoveInvalidPair(image_pair_t pair_id) { - ImagePair& pair = image_pairs.at(pair_id); - pair.is_valid = false; -} - } // namespace glomap From 60b065744e29abbf3e1150dfd3ad37efdf1061b9 Mon Sep 17 00:00:00 2001 From: Linfei Pan <36349740+lpanaf@users.noreply.github.com> Date: Thu, 11 Dec 2025 15:46:28 -0800 Subject: [PATCH 41/45] fix the rotation averaging bug (#227) * fix the rotation averaging bug * f --- glomap/estimators/global_rotation_averaging.cc | 2 +- glomap/exe/rotation_averager.cc | 1 + glomap/io/colmap_converter.cc | 1 + glomap/io/pose_io.cc | 12 ++++++++---- glomap/scene/view_graph.cc | 1 + 5 files changed, 12 insertions(+), 5 deletions(-) diff --git a/glomap/estimators/global_rotation_averaging.cc b/glomap/estimators/global_rotation_averaging.cc index 8246b272..0586f4bc 100644 --- a/glomap/estimators/global_rotation_averaging.cc +++ b/glomap/estimators/global_rotation_averaging.cc @@ -759,7 +759,7 @@ double RotationEstimator::ComputeAverageStepSize( const std::unordered_map& frames) { double total_update = 0; for (const auto& [frame_id, frame] : frames) { - if (frames.at(frame_id).is_registered) continue; + if (!frames.at(frame_id).is_registered) continue; if (options_.use_gravity && frame.HasGravity()) { total_update += std::abs(tangent_space_step_[frame_id_to_idx_[frame_id]]); diff --git a/glomap/exe/rotation_averager.cc b/glomap/exe/rotation_averager.cc index 86403ded..af8a7a3f 100644 --- a/glomap/exe/rotation_averager.cc +++ b/glomap/exe/rotation_averager.cc @@ -74,6 +74,7 @@ int RunRotationAverager(int argc, char** argv) { for (auto& [image_id, image] : images) { image.camera_id = image.image_id; cameras[image.camera_id] = Camera(); + cameras[image.camera_id].camera_id = image.camera_id; } CreateOneRigPerCamera(cameras, rigs); diff --git a/glomap/io/colmap_converter.cc b/glomap/io/colmap_converter.cc index 13dc8308..4c7fd030 100644 --- a/glomap/io/colmap_converter.cc +++ b/glomap/io/colmap_converter.cc @@ -429,6 +429,7 @@ void CreateOneRigPerCamera(const std::unordered_map& cameras, Rig rig; rig.SetRigId(camera_id); rig.AddRefSensor(camera.SensorId()); + rigs[rig.RigId()] = rig; } } diff --git a/glomap/io/pose_io.cc b/glomap/io/pose_io.cc index d2349f3c..b5c2fad9 100644 --- a/glomap/io/pose_io.cc +++ b/glomap/io/pose_io.cc @@ -10,10 +10,12 @@ void ReadRelPose(const std::string& file_path, ViewGraph& view_graph) { std::unordered_map name_idx; image_t max_image_id = 0; + camera_t max_camera_id = 0; for (const auto& [image_id, image] : images) { name_idx[image.file_name] = image_id; max_image_id = std::max(max_image_id, image_id); + max_camera_id = std::max(max_camera_id, image.camera_id); } // Mark every edge in te view graph as invalid @@ -42,14 +44,16 @@ void ReadRelPose(const std::string& file_path, if (name_idx.find(file1) == name_idx.end()) { max_image_id += 1; - images.insert( - std::make_pair(max_image_id, Image(max_image_id, -1, file1))); + max_camera_id += 1; + images.insert(std::make_pair(max_image_id, + Image(max_image_id, max_camera_id, file1))); name_idx[file1] = max_image_id; } if (name_idx.find(file2) == name_idx.end()) { max_image_id += 1; - images.insert( - std::make_pair(max_image_id, Image(max_image_id, -1, file2))); + max_camera_id += 1; + images.insert(std::make_pair(max_image_id, + Image(max_image_id, max_camera_id, file2))); name_idx[file2] = max_image_id; } diff --git a/glomap/scene/view_graph.cc b/glomap/scene/view_graph.cc index 5e8e855e..760593cc 100644 --- a/glomap/scene/view_graph.cc +++ b/glomap/scene/view_graph.cc @@ -89,6 +89,7 @@ int ViewGraph::KeepLargestConnectedComponents( } } + max_img = 0; for (auto& [image_id, image] : images) { if (image.IsRegistered()) max_img++; } From 4b17e6895b82876b4ae4047344ca13a1db174b52 Mon Sep 17 00:00:00 2001 From: Ryan Mack Date: Wed, 31 Dec 2025 19:29:10 -0500 Subject: [PATCH 42/45] Extract frames writes frames as it goes --- scripts/extract_frames.py | 34 +++++++++++++++++----------------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/scripts/extract_frames.py b/scripts/extract_frames.py index 97a9236b..f1833e13 100755 --- a/scripts/extract_frames.py +++ b/scripts/extract_frames.py @@ -4,31 +4,31 @@ import argparse import cv2 -import numpy as np from pathlib import Path -def extract_frames(cap, desired_fps: float | None) -> list[np.ndarray]: + +def extract_frames(video_path: Path, output_dir: Path, desired_fps: float | None): + output_dir.mkdir(parents=True, exist_ok=True) + + cap = cv2.VideoCapture(str(video_path)) fps = cap.get(cv2.CAP_PROP_FPS) - if desired_fps: - fps_ratio = desired_fps/fps - else: - fps_ratio = 1.0 - frames = [] + fps_ratio = (desired_fps / fps) if desired_fps else 1.0 + portion = 0.0 + frame_num = 0 + while True: - ret, rgb_image = cap.read() + ret, frame = cap.read() if not ret: break portion += fps_ratio if portion >= 1.0: portion -= 1.0 - frames.append(rgb_image) - return frames - -def save_frames(frames: list[np.ndarray], output_dir: Path): - output_dir.mkdir(parents=True, exist_ok=True) - for i, frame in enumerate(frames): - cv2.imwrite(str(output_dir / f"{i:0>5}.jpg"), frame) + cv2.imwrite(str(output_dir / f"{frame_num:05d}.jpg"), frame) + frame_num += 1 + + cap.release() + print(f"Extracted {frame_num} frames") def main(): @@ -37,8 +37,8 @@ def main(): parser.add_argument("-v", "--video-path", type=Path, required=True) parser.add_argument("--desired-fps", type=float, default=60.0) args = parser.parse_args() - frames = extract_frames(cv2.VideoCapture(str(args.video_path)), args.desired_fps) - save_frames(frames, args.output_dir) + extract_frames(args.video_path, args.output_dir, args.desired_fps) + if __name__ == "__main__": main() From 9d5a15b58cf7a1459f8d0b8247e245a1197c8b38 Mon Sep 17 00:00:00 2001 From: Ryan Mack Date: Wed, 31 Dec 2025 19:40:21 -0500 Subject: [PATCH 43/45] Update rerun, downgrade Ubuntu to LTS --- cmake/FindDependencies.cmake | 4 ++- docker/Dockerfile | 4 +-- glomap/estimators/global_positioning.cc | 29 ++++++++++-------- glomap/io/colmap_io.cc | 4 ++- glomap/io/recording.cc | 40 +++++++++++++------------ 5 files changed, 46 insertions(+), 35 deletions(-) diff --git a/cmake/FindDependencies.cmake b/cmake/FindDependencies.cmake index ce9320d5..7dc82b08 100644 --- a/cmake/FindDependencies.cmake +++ b/cmake/FindDependencies.cmake @@ -1,9 +1,11 @@ set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake") +include(FetchContent) + message(STATUS "Configuring Rerun...") if (FETCH_RERUN) FetchContent_Declare(rerun_sdk URL - https://github.com/rerun-io/rerun/releases/download/0.17.0/rerun_cpp_sdk.zip) + https://github.com/rerun-io/rerun/releases/download/0.28.1/rerun_cpp_sdk.zip) FetchContent_MakeAvailable(rerun_sdk) else() find_package(rerun_sdk REQUIRED) diff --git a/docker/Dockerfile b/docker/Dockerfile index c81329b2..a56b2756 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -1,4 +1,4 @@ -FROM ubuntu:24.10 +FROM ubuntu:24.04 RUN apt-get update && apt-get install -y --no-install-recommends --no-install-suggests \ curl \ @@ -34,7 +34,7 @@ RUN apt-get update && apt-get install -y --no-install-recommends --no-install-su WORKDIR /ws/ -RUN pipx install rerun-sdk==0.17.0 +RUN pipx install rerun-sdk==0.28.1 ENV PATH=$PATH:/root/.local/share/pipx/venvs/rerun-sdk/bin/ # RUN ./install_colmap.sh diff --git a/glomap/estimators/global_positioning.cc b/glomap/estimators/global_positioning.cc index 5ba9bb1d..f5f96a99 100644 --- a/glomap/estimators/global_positioning.cc +++ b/glomap/estimators/global_positioning.cc @@ -26,41 +26,46 @@ Eigen::Vector3d RandVector3d(std::mt19937& random_generator, class LoggingCallback : public ceres::IterationCallback { -public: - std::unordered_map& tracks; +public: + std::unordered_map& rigs; std::unordered_map& cameras; + std::unordered_map& frames; std::unordered_map& images; + std::unordered_map& tracks; std::string image_path; - LoggingCallback(std::unordered_map& tracks, + LoggingCallback(std::unordered_map& rigs, std::unordered_map& cameras, - std::unordered_map& images, + std::unordered_map& frames, + std::unordered_map& images, + std::unordered_map& tracks, std::string image_path - ) : tracks {tracks}, cameras {cameras}, images {images}, image_path {image_path} {} + ) : rigs{rigs}, cameras{cameras}, frames{frames}, images{images}, tracks{tracks}, image_path{image_path} {} ~LoggingCallback() {} ceres::CallbackReturnType operator()(const ceres::IterationSummary& summary) { std::unordered_map images_copy = images; for (auto& [image_id, image] : images_copy) { - image.cam_from_world.translation = - -(image.cam_from_world.rotation * image.cam_from_world.translation); + if (!image.IsRegistered()) continue; + image.CamFromWorld().translation = + -(image.CamFromWorld().rotation * image.CamFromWorld().translation); } rr_rec.set_time_sequence("step", algorithm_step++); if (summary.iteration == 0) { // This is a bit of a hack to extract the colors for the point cloud to make the visualization a bit prettier. - + std::unordered_map tmp_rigs; std::unordered_map tmp_cameras; + std::unordered_map tmp_frames; std::unordered_map tmp_images; std::unordered_map tmp_tracks; colmap::Reconstruction reconstruction; - // ConvertDatabaseToColmap(re) - ConvertGlomapToColmap(cameras, images_copy, tracks, reconstruction); + ConvertGlomapToColmap(rigs, cameras, frames, images_copy, tracks, reconstruction); reconstruction.ExtractColorsForAllImages(image_path); - ConvertColmapToGlomap(reconstruction, tmp_cameras, tmp_images, tmp_tracks); + ConvertColmapToGlomap(reconstruction, tmp_rigs, tmp_cameras, tmp_frames, tmp_images, tmp_tracks); for (auto &[track_id, track] : tmp_tracks) { tracks[track_id].color = track.color; } @@ -132,7 +137,7 @@ bool GlobalPositioner::Solve(const ViewGraph& view_graph, ParameterizeVariables(rigs, frames, tracks); LOG(INFO) << "Solving the global positioner problem"; - LoggingCallback callback {tracks, cameras, images, image_path_global}; + LoggingCallback callback {rigs, cameras, frames, images, tracks, image_path_global}; ceres::Solver::Summary summary; options_.solver_options.minimizer_progress_to_stdout = VLOG_IS_ON(2); diff --git a/glomap/io/colmap_io.cc b/glomap/io/colmap_io.cc index 546c5781..3ea576b6 100644 --- a/glomap/io/colmap_io.cc +++ b/glomap/io/colmap_io.cc @@ -34,10 +34,12 @@ void WriteGlomapReconstruction( } // Convert back to GLOMAP so that we can log the reconstruction with color. + std::unordered_map rigs_copy; std::unordered_map cameras_copy; + std::unordered_map frames_copy; std::unordered_map images_copy; std::unordered_map tracks_copy; - ConvertColmapToGlomap(reconstruction, cameras_copy, images_copy, tracks_copy); + ConvertColmapToGlomap(reconstruction, rigs_copy, cameras_copy, frames_copy, images_copy, tracks_copy); rr_rec.set_time_sequence("step", algorithm_step++); rr_rec.log("status", rerun::TextLog("Converted to Colmap and extracted colors")); log_reconstruction(rr_rec, cameras_copy, images_copy, tracks_copy); diff --git a/glomap/io/recording.cc b/glomap/io/recording.cc index a9dd49fc..3818de42 100644 --- a/glomap/io/recording.cc +++ b/glomap/io/recording.cc @@ -1,6 +1,6 @@ #include "recording.h" #include -// #include namespace glomap { @@ -17,7 +17,6 @@ void log_bitmap(rerun::RecordingStream &rec, std::string_view entity_path, colm size_t width = bitmap.Width(); size_t height = bitmap.Height(); size_t nchannels = bitmap.Channels(); - std::vector shape = {height, width, nchannels}; auto buffer = bitmap.ConvertToRowMajorArray(); LOG(INFO) << buffer.size(); LOG(INFO) << entity_path; @@ -25,8 +24,12 @@ void log_bitmap(rerun::RecordingStream &rec, std::string_view entity_path, colm for (size_t i = 0; i < buffer.size(); i+=3) { std::swap(buffer[i], buffer[i+2]); } + rec.log(entity_path, rerun::Image::from_rgb24(buffer, {width, height})); + } else if (nchannels == 4) { + rec.log(entity_path, rerun::Image::from_rgba32(buffer, {width, height})); + } else if (nchannels == 1) { + rec.log(entity_path, rerun::Image::from_grayscale8(buffer, {width, height})); } - rec.log(entity_path, rerun::Image(shape, std::move(buffer))); } std::unordered_map> get_observation_to_point_map( @@ -38,7 +41,7 @@ std::unordered_map> get_observation_to_point_map( if (tracks.size()) { // Initialize every point to corresponds to invalid point for (auto& [image_id, image] : images) { - if (!image.is_registered) + if (!image.IsRegistered()) continue; image_to_point3D[image_id] = std::vector(image.features.size(), -1); @@ -68,23 +71,22 @@ void log_reconstruction( std::vector points; std::vector colors; - + for (auto &[_, image] : images) { + if (!image.IsRegistered()) continue; // Skip images without valid poses + auto camera = cameras.at(image.camera_id); - Eigen::Vector3f translation = image.cam_from_world.translation.cast(); - Eigen::Matrix3f rotation = image.cam_from_world.rotation.toRotationMatrix().cast(); - rec.log("images/" + image.file_name, rerun::Transform3D( - rerun::datatypes::TranslationAndMat3x3( - rerun::Vec3D(translation.data()), - rerun::Mat3x3(rotation.data()), - true - ) - )); + Eigen::Vector3f translation = image.CamFromWorld().translation.cast(); + Eigen::Matrix3f rotation = image.CamFromWorld().rotation.toRotationMatrix().cast(); + rec.log("images/" + image.file_name, rerun::Transform3D() + .with_translation(rerun::Vec3D(translation.data())) + .with_mat3x3(rerun::Mat3x3(rotation.data())) + ); rec.log_static("images/" + image.file_name, rerun::ViewCoordinates::RDF); Eigen::Matrix3Xf K = camera.GetK().cast(); rec.log( - "images/" + image.file_name, + "images/" + image.file_name, rerun::Pinhole(rerun::components::PinholeProjection(rerun::datatypes::Mat3x3(K.data()))) .with_resolution(int(camera.width), int(camera.height)) ); @@ -94,7 +96,7 @@ void log_reconstruction( // Should actually be `track.observations.size() < options_.min_num_view_per_track`. if (track.observations.size() < 3) continue; - + auto xyz = track.xyz; points.emplace_back(xyz.x(), xyz.y(), xyz.z()); colors.emplace_back(track.color[0], track.color[1], track.color[2]); @@ -105,9 +107,9 @@ void log_reconstruction( void log_images(rerun::RecordingStream &rec, const std::unordered_map& images, const std::string image_path) { for (auto &[id, image] : images) { - std::string path = colmap::JoinPaths(image_path, image.file_name); + std::filesystem::path full_path = std::filesystem::path(image_path) / image.file_name; colmap::Bitmap bitmap; - if (!bitmap.Read(path)) { + if (!bitmap.Read(full_path.string())) { LOG(ERROR) << "Failed to read image path"; } std::string entity_path = "images/"; @@ -122,4 +124,4 @@ void log_images(rerun::RecordingStream &rec, const std::unordered_map Date: Thu, 1 Jan 2026 05:52:19 -0500 Subject: [PATCH 44/45] ENV[RERUN_CONNECT] to connect to a remote viewer --- glomap/io/recording.cc | 18 +++++++++++++++--- 1 file changed, 15 insertions(+), 3 deletions(-) diff --git a/glomap/io/recording.cc b/glomap/io/recording.cc index 3818de42..1a3c9f05 100644 --- a/glomap/io/recording.cc +++ b/glomap/io/recording.cc @@ -1,6 +1,7 @@ #include "recording.h" #include #include +#include namespace glomap { @@ -9,13 +10,24 @@ uint32_t algorithm_step = 0; std::string image_path_global = ""; void init_recording() { - rr_rec.spawn().exit_on_failure(); + // Check for RERUN_CONNECT environment variable to connect to remote viewer + // Format: "host:port" (e.g., "host.docker.internal:9876" or "192.168.1.100:9876") + // Default rerun port is 9876 + const char* rerun_connect = std::getenv("RERUN_CONNECT"); + + if (rerun_connect != nullptr && std::strlen(rerun_connect) > 0) { + LOG(INFO) << "Connecting to remote Rerun viewer at: " << rerun_connect; + rr_rec.connect_grpc(rerun_connect).exit_on_failure(); + } else { + // Spawn local viewer + rr_rec.spawn().exit_on_failure(); + } rr_rec.set_time_sequence("step", algorithm_step); } void log_bitmap(rerun::RecordingStream &rec, std::string_view entity_path, colmap::Bitmap& bitmap) { - size_t width = bitmap.Width(); - size_t height = bitmap.Height(); + uint32_t width = static_cast(bitmap.Width()); + uint32_t height = static_cast(bitmap.Height()); size_t nchannels = bitmap.Channels(); auto buffer = bitmap.ConvertToRowMajorArray(); LOG(INFO) << buffer.size(); From cd5b264ad47eb27b860b03436a87d1ca5e366e21 Mon Sep 17 00:00:00 2001 From: Ryan Mack Date: Thu, 1 Jan 2026 06:08:54 -0500 Subject: [PATCH 45/45] extract_frames.py output progress messages --- scripts/extract_frames.py | 28 +++++++++++++++++++++++----- 1 file changed, 23 insertions(+), 5 deletions(-) diff --git a/scripts/extract_frames.py b/scripts/extract_frames.py index f1833e13..9bba4bf8 100755 --- a/scripts/extract_frames.py +++ b/scripts/extract_frames.py @@ -9,26 +9,44 @@ def extract_frames(video_path: Path, output_dir: Path, desired_fps: float | None): output_dir.mkdir(parents=True, exist_ok=True) - + cap = cv2.VideoCapture(str(video_path)) fps = cap.get(cv2.CAP_PROP_FPS) + total_frames = int(cap.get(cv2.CAP_PROP_FRAME_COUNT)) fps_ratio = (desired_fps / fps) if desired_fps else 1.0 - + + # Estimate how many frames will be extracted + estimated_output = int(total_frames * fps_ratio) if fps_ratio < 1.0 else total_frames + + print(f"Video: {video_path.name}") + print(f"FPS: {fps:.2f}, Total frames: {total_frames}, Target FPS: {desired_fps or fps:.2f}") + print(f"Estimated output frames: ~{estimated_output}") + portion = 0.0 frame_num = 0 - + input_frame_num = 0 + last_percent = -1 + while True: ret, frame = cap.read() if not ret: break + input_frame_num += 1 portion += fps_ratio if portion >= 1.0: portion -= 1.0 cv2.imwrite(str(output_dir / f"{frame_num:05d}.jpg"), frame) frame_num += 1 - + + # Print progress every 1% + percent = int(100 * input_frame_num / total_frames) if total_frames > 0 else 0 + if percent != last_percent: + print(f"\rProgress: {percent}% ({frame_num} frames extracted)", end="", flush=True) + last_percent = percent + + print(f"\rProgress: 100% - Done! ") cap.release() - print(f"Extracted {frame_num} frames") + print(f"Extracted {frame_num} frames to {output_dir}") def main():