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