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/install-ccache.ps1 b/.github/workflows/install-ccache.ps1 new file mode 100644 index 00000000..c9d15035 --- /dev/null +++ b/.github/workflows/install-ccache.ps1 @@ -0,0 +1,38 @@ +[CmdletBinding()] +param ( + [Parameter(Mandatory = $true)] + [string] $Destination +) + +$version = "4.10.2" +$folder="ccache-$version-windows-x86_64" +$url = "https://github.com/ccache/ccache/releases/download/v$version/$folder.zip" +$expectedSha256 = "6252F081876A9A9F700FAE13A5AEC5D0D486B28261D7F1F72AC11C7AD9DF4DA9" + +$ErrorActionPreference = "Stop" + +try { + New-Item -Path "$Destination" -ItemType Container -ErrorAction SilentlyContinue + + Write-Host "Download CCache" + $zipFilePath = Join-Path "$env:TEMP" "$folder.zip" + Invoke-WebRequest -Uri $url -UseBasicParsing -OutFile "$zipFilePath" -MaximumRetryCount 3 + + $hash = Get-FileHash $zipFilePath -Algorithm "sha256" + if ($hash.Hash -ne $expectedSha256) { + throw "File $Path hash $hash.Hash did not match expected hash $expectedHash" + } + + Write-Host "Unzip CCache" + Expand-Archive -Path "$zipFilePath" -DestinationPath "$env:TEMP" + + Write-Host "Move CCache" + Move-Item -Force "$env:TEMP/$folder/ccache.exe" "$Destination" + Remove-Item "$zipFilePath" + Remove-Item -Recurse "$env:TEMP/$folder" +} +catch { + Write-Host "Installation failed with an error" + $_.Exception | Format-List + exit -1 +} diff --git a/.github/workflows/mac.yml b/.github/workflows/mac.yml new file mode 100644 index 00000000..a78a36de --- /dev/null +++ b/.github/workflows/mac.yml @@ -0,0 +1,84 @@ +name: Mac + +on: + push: + branches: + - main + pull_request: + types: [ assigned, opened, synchronize, reopened ] + release: + types: [ published, edited ] + +jobs: + build: + name: ${{ matrix.config.os }} ${{ matrix.config.arch }} ${{ matrix.config.cmakeBuildType }} + runs-on: ${{ matrix.config.os }} + strategy: + matrix: + config: [ + { + os: macos-15, + arch: arm64, + cmakeBuildType: Release, + }, + ] + + env: + COMPILER_CACHE_VERSION: 1 + COMPILER_CACHE_DIR: ${{ github.workspace }}/compiler-cache + CCACHE_DIR: ${{ github.workspace }}/compiler-cache/ccache + CCACHE_BASEDIR: ${{ github.workspace }} + + steps: + - uses: actions/checkout@v4 + - uses: actions/cache@v4 + id: cache-builds + with: + key: v${{ env.COMPILER_CACHE_VERSION }}-${{ matrix.config.os }}-${{ matrix.config.arch }}-${{ matrix.config.cmakeBuildType }}-${{ github.run_id }}-${{ github.run_number }} + restore-keys: v${{ env.COMPILER_CACHE_VERSION }}-${{ matrix.config.os }}-${{ matrix.config.arch }}-${{ matrix.config.cmakeBuildType }} + path: ${{ env.COMPILER_CACHE_DIR }} + + - name: Setup Mac + run: | + brew upgrade cmake || brew install cmake + brew install \ + ninja \ + boost \ + eigen \ + flann \ + freeimage \ + metis \ + glog \ + googletest \ + ceres-solver \ + qt5 \ + glew \ + cgal \ + sqlite3 \ + ccache + brew link --force libomp + + - name: Configure and build + run: | + cmake --version + mkdir build + cd build + cmake .. \ + -GNinja \ + -DCMAKE_BUILD_TYPE=${{ matrix.config.cmakeBuildType }} \ + -DTESTS_ENABLED=ON \ + -DCMAKE_PREFIX_PATH="$(brew --prefix qt@5)" + ninja + + - name: Run tests + run: | + cd build + set +e + ctest --output-on-failure -E .+colmap_.* + + - name: Cleanup compiler cache + run: | + set -x + ccache --show-stats --verbose + ccache --evict-older-than 1d + ccache --show-stats --verbose diff --git a/.github/workflows/ci.yml b/.github/workflows/ubuntu.yml similarity index 81% rename from .github/workflows/ci.yml rename to .github/workflows/ubuntu.yml index 888a383a..29d05f07 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ubuntu.yml @@ -11,14 +11,22 @@ 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, + checkCodeFormat: true, + }, { os: ubuntu-22.04, + qtVersion: 6, cmakeBuildType: Release, asanEnabled: false, cudaEnabled: false, @@ -26,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, @@ -53,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 @@ -62,14 +75,15 @@ jobs: key: v${{ env.COMPILER_CACHE_VERSION }}-${{ matrix.config.os }}-${{ matrix.config.cmakeBuildType }}-${{ matrix.config.asanEnabled }}--${{ matrix.config.cudaEnabled }}-${{ github.run_id }}-${{ github.run_number }} restore-keys: v${{ env.COMPILER_CACHE_VERSION }}-${{ matrix.config.os }}-${{ matrix.config.cmakeBuildType }}-${{ matrix.config.asanEnabled }}--${{ matrix.config.cudaEnabled }} path: ${{ env.COMPILER_CACHE_DIR }} - - name: Install compiler cache run: | mkdir -p "$CCACHE_DIR" "$CTCACHE_DIR" echo "$COMPILER_CACHE_DIR/bin" >> $GITHUB_PATH + if [ -f "$COMPILER_CACHE_DIR/bin/ccache" ]; then exit 0 fi + set -x wget https://github.com/ccache/ccache/releases/download/v4.8.2/ccache-4.8.2-linux-x86_64.tar.xz echo "0b33f39766fe9db67f40418aed6a5b3d7b2f4f7fab025a8213264b77a2d0e1b1 ccache-4.8.2-linux-x86_64.tar.xz" | sha256sum --check @@ -81,41 +95,50 @@ jobs: echo "108b087f156a9fe7da0c796de1ef73f5855d2a33a27983769ea39061359a40fc ${ctcache_commit_id}.zip" | sha256sum --check unzip "${ctcache_commit_id}.zip" mv ctcache-${ctcache_commit_id}/clang-tidy* "$COMPILER_CACHE_DIR/bin" + - name: Check code format if: matrix.config.checkCodeFormat run: | set +x -euo pipefail - sudo apt-get update && sudo apt-get install -y clang-format-14 black - ./scripts/format/clang_format.sh + python -m pip install clang-format==20.1.5 + ./scripts/format/c++.sh git diff --name-only git diff --exit-code || (echo "Code formatting failed" && exit 1) + - 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 \ ninja-build \ libboost-program-options-dev \ - libboost-filesystem-dev \ libboost-graph-dev \ libboost-system-dev \ libeigen3-dev \ - libsuitesparse-dev \ libceres-dev \ libflann-dev \ libfreeimage-dev \ libmetis-dev \ libgoogle-glog-dev \ libgtest-dev \ + libgmock-dev \ libsqlite3-dev \ libglew-dev \ - qtbase5-dev \ + $qt_packages \ libqt5opengl5-dev \ libcgal-dev \ libcgal-qt5-dev \ libgl1-mesa-dri \ libunwind-dev \ + libcurl4-openssl-dev \ xvfb + if [ "${{ matrix.config.cudaEnabled }}" == "true" ]; then if [ "${{ matrix.config.os }}" == "ubuntu-20.04" ]; then sudo apt-get install -y \ @@ -133,16 +156,19 @@ jobs: echo "CUDAHOSTCXX=/usr/bin/g++-10" >> $GITHUB_ENV fi fi + if [ "${{ matrix.config.asanEnabled }}" == "true" ]; then - sudo apt-get install -y clang-15 libomp-15-dev - echo "CC=/usr/bin/clang-15" >> $GITHUB_ENV - echo "CXX=/usr/bin/clang++-15" >> $GITHUB_ENV + 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 run: | CMAKE_VERSION=3.28.6 @@ -151,6 +177,7 @@ jobs: tar -xzf ${CMAKE_DIR}.tar.gz sudo cp -r ${CMAKE_DIR}/* /usr/local/ rm -rf ${CMAKE_DIR}* + - name: Configure and build run: | set -x @@ -162,29 +189,31 @@ jobs: -DCMAKE_BUILD_TYPE=${{ matrix.config.cmakeBuildType }} \ -DCMAKE_INSTALL_PREFIX=./install \ -DCMAKE_CUDA_ARCHITECTURES=50 \ - -DSuiteSparse_CHOLMOD_LIBRARY="/usr/lib/x86_64-linux-gnu/libcholmod.so" \ - -DSuiteSparse_CHOLMOD_INCLUDE_DIR="/usr/include/suitesparse" \ + -DCUDA_ENABLED=${{ matrix.config.cudaEnabled }} \ -DTESTS_ENABLED=ON \ -DASAN_ENABLED=${{ matrix.config.asanEnabled }} ninja -k 10000 + - name: Run tests if: ${{ matrix.config.cmakeBuildType != 'ClangTidy' }} - run: | + run: | export DISPLAY=":99.0" export QT_QPA_PLATFORM="offscreen" Xvfb :99 & sleep 3 cd build ctest --output-on-failure -E .+colmap_.* + - name: Cleanup compiler cache run: | set -x ccache --show-stats --verbose ccache --evict-older-than 1d ccache --show-stats --verbose + echo "Size of ctcache before: $(du -sh $CTCACHE_DIR)" echo "Number of ctcache files before: $(find $CTCACHE_DIR | wc -l)" # Delete cache older than 10 days. find "$CTCACHE_DIR"/*/ -mtime +10 -print0 | xargs -0 rm -rf echo "Size of ctcache after: $(du -sh $CTCACHE_DIR)" - echo "Number of ctcache files after: $(find $CTCACHE_DIR | wc -l)" \ No newline at end of file + echo "Number of ctcache files after: $(find $CTCACHE_DIR | wc -l)" diff --git a/.github/workflows/windows.yml b/.github/workflows/windows.yml new file mode 100644 index 00000000..3acfd2c6 --- /dev/null +++ b/.github/workflows/windows.yml @@ -0,0 +1,175 @@ +name: Windows + +on: + push: + branches: + - main + pull_request: + types: [ assigned, opened, synchronize, reopened ] + release: + types: [ published, edited ] + +jobs: + build: + name: ${{ matrix.config.os }} ${{ matrix.config.cmakeBuildType }} ${{ matrix.config.cudaEnabled && 'CUDA' || '' }} + runs-on: ${{ matrix.config.os }} + strategy: + matrix: + config: [ + { + os: windows-2025, + cmakeBuildType: Release, + cudaEnabled: false, + testsEnabled: true, + exportPackage: true, + }, + ] + + env: + COMPILER_CACHE_VERSION: 1 + COMPILER_CACHE_DIR: ${{ github.workspace }}/compiler-cache + CCACHE_DIR: ${{ github.workspace }}/compiler-cache/ccache + CCACHE_BASEDIR: ${{ github.workspace }} + VCPKG_COMMIT_ID: 2ad7bd06128280e02bfe02361d8ffd7d465cfcf0 + GLOG_v: 1 + GLOG_logtostderr: 1 + + steps: + - uses: actions/checkout@v4 + + # We define the vcpkg binary sources using separate variables for read and + # write operations: + # * Read sources are defined as inline. These can be read by anyone and, + # in particular, pull requests from forks. Unfortunately, we cannot + # define these as action environment variables. See: + # https://github.com/orgs/community/discussions/44322 + # * Write sources are defined as action secret variables. These cannot be + # read by pull requests from forks but only from pull requests from + # within the target repository (i.e., created by a repository owner). + # This protects us from malicious actors accessing our secrets and + # gaining write access to our binary cache. For more information, see: + # https://securitylab.github.com/resources/github-actions-preventing-pwn-requests/ + - name: Setup vcpkg binary cache + shell: pwsh + run: | + # !!!PLEASE!!! be nice and don't use this cache for your own purposes. This is only meant for CI purposes in this repository. + $VCPKG_BINARY_SOURCES = "clear;x-azblob,https://colmap.blob.core.windows.net/github-actions-cache,sp=r&st=2024-12-10T17:29:32Z&se=2030-12-31T01:29:32Z&spr=https&sv=2022-11-02&sr=c&sig=bWydkilTMjRn3LHKTxLgdWrFpV4h%2Finzoe9QCOcPpYQ%3D,read" + if ("${{ secrets.VCPKG_BINARY_CACHE_AZBLOB_URL }}") { + # The secrets are only accessible in runs triggered from within the target repository and not forks. + $VCPKG_BINARY_SOURCES += ";x-azblob,${{ secrets.VCPKG_BINARY_CACHE_AZBLOB_URL }},${{ secrets.VCPKG_BINARY_CACHE_AZBLOB_SAS }},write" + } + echo "VCPKG_BINARY_SOURCES=${VCPKG_BINARY_SOURCES}" >> "${env:GITHUB_ENV}" + + - name: Compiler cache + uses: actions/cache@v4 + id: cache-builds + with: + key: v${{ env.COMPILER_CACHE_VERSION }}-${{ matrix.config.os }}-${{ matrix.config.cmakeBuildType }}-${{ matrix.config.asanEnabled }}--${{ matrix.config.cudaEnabled }}-${{ github.run_id }}-${{ github.run_number }} + restore-keys: v${{ env.COMPILER_CACHE_VERSION }}-${{ matrix.config.os }}-${{ matrix.config.cmakeBuildType }}-${{ matrix.config.asanEnabled }}--${{ matrix.config.cudaEnabled }} + path: ${{ env.COMPILER_CACHE_DIR }} + + - name: Install ccache + shell: pwsh + run: | + New-Item -ItemType Directory -Force -Path "${{ env.CCACHE_DIR }}" + echo "${{ env.COMPILER_CACHE_DIR }}/bin" | Out-File -Encoding utf8 -Append -FilePath $env:GITHUB_PATH + if (Test-Path -PathType Leaf "${{ env.COMPILER_CACHE_DIR }}/bin/ccache.exe") { + exit + } + .github/workflows/install-ccache.ps1 -Destination "${{ env.COMPILER_CACHE_DIR }}/bin" + + - name: Install CUDA + uses: Jimver/cuda-toolkit@v0.2.18 + if: matrix.config.cudaEnabled + id: cuda-toolkit + with: + cuda: '12.6.2' + sub-packages: '["nvcc", "nvtx", "cudart", "curand", "curand_dev", "nvrtc_dev"]' + method: 'network' + + - name: Setup vcpkg + shell: pwsh + run: | + ./scripts/shell/enter_vs_dev_shell.ps1 + cd ${{ github.workspace }} + git clone https://github.com/microsoft/vcpkg + cd vcpkg + git reset --hard ${{ env.VCPKG_COMMIT_ID }} + ./bootstrap-vcpkg.bat + + - name: Install CMake and Ninja + uses: lukka/get-cmake@latest + with: + cmakeVersion: "3.31.0" + ninjaVersion: "1.12.1" + + - name: Configure and build + shell: pwsh + run: | + ./scripts/shell/enter_vs_dev_shell.ps1 + cd ${{ github.workspace }} + ./vcpkg/vcpkg.exe integrate install + mkdir build + cd build + cmake .. ` + -GNinja ` + -DCMAKE_MAKE_PROGRAM=ninja ` + -DCMAKE_BUILD_TYPE=Release ` + -DTESTS_ENABLED=ON ` + -DCUDA_ENABLED=${{ matrix.config.cudaEnabled }} ` + -DGUI_ENABLED=OFF ` + -DCGAL_ENABLED=OFF ` + -DCMAKE_CUDA_ARCHITECTURES=all-major ` + -DCUDAToolkit_ROOT="${{ steps.cuda-toolkit.outputs.CUDA_PATH }}" ` + -DCMAKE_TOOLCHAIN_FILE="${{ github.workspace }}/vcpkg/scripts/buildsystems/vcpkg.cmake" ` + -DVCPKG_TARGET_TRIPLET=x64-windows-release ` + -DCMAKE_INSTALL_PREFIX=install + ninja + + - name: Run tests + shell: pwsh + run: | + ./vcpkg/vcpkg.exe integrate install + cd build + ctest -E .+colmap_.* --output-on-failure + + - name: Export package + if: matrix.config.exportPackage + shell: pwsh + run: | + ./vcpkg/vcpkg.exe integrate install + + cd build + ninja install + + ../vcpkg/vcpkg.exe install ` + --triplet=x64-windows-release + $(if ($${{ matrix.config.cudaEnabled }}) { echo "--x-feature=cuda" }) + ../vcpkg/vcpkg.exe export --raw --output-dir vcpkg_export --output glomap + cp vcpkg_export/glomap/installed/x64-windows/bin/*.dll install/bin + cp vcpkg_export/glomap/installed/x64-windows-release/bin/*.dll install/bin + if ($${{ matrix.config.cudaEnabled }}) { + cp "${{ steps.cuda-toolkit.outputs.CUDA_PATH }}/bin/cudart64_*.dll" install/bin + cp "${{ steps.cuda-toolkit.outputs.CUDA_PATH }}/bin/curand64_*.dll" install/bin + } + + - name: Upload package + uses: actions/upload-artifact@v4 + if: ${{ matrix.config.exportPackage && matrix.config.cudaEnabled }} + with: + name: glomap-x64-windows-cuda + path: build/install + + - name: Upload package + uses: actions/upload-artifact@v4 + if: ${{ matrix.config.exportPackage && !matrix.config.cudaEnabled }} + with: + name: glomap-x64-windows-nocuda + path: build/install + + - name: Cleanup compiler cache + shell: pwsh + run: | + ccache --show-stats --verbose + ccache --evict-older-than 1d + ccache --show-stats --verbose diff --git a/.gitignore b/.gitignore index 55e9e67e..b88c9bff 100644 --- a/.gitignore +++ b/.gitignore @@ -1,12 +1,14 @@ -build -data +/build +/data .pixi venv *.mp4 -.vscode +/.vscode output shell.nix /run.sh +/compile_commands.json +.cache # pixi environments .pixi diff --git a/CMakeLists.txt b/CMakeLists.txt index 1cbeefa9..9eb97cb4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,20 +1,37 @@ cmake_minimum_required(VERSION 3.28) -project(glomap VERSION 0.0.1) +option(CUDA_ENABLED "Whether to enable CUDA, if available" ON) +option(TESTS_ENABLED "Whether to build test binaries" OFF) +option(ASAN_ENABLED "Whether to enable AddressSanitizer flags" OFF) +option(CCACHE_ENABLED "Whether to enable compiler caching, if available" ON) +option(FETCH_COLMAP "Whether to use COLMAP with FetchContent or with self-installed software" ON) +option(FETCH_POSELIB "Whether to use PoseLib with FetchContent or with self-installed software" ON) + +# Propagate options to vcpkg manifest. +if(CUDA_ENABLED) + list(APPEND VCPKG_MANIFEST_FEATURES "cuda") +endif() + +# Initialize the project. +project(glomap VERSION 1.1.0) set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) set_property(GLOBAL PROPERTY GLOBAL_DEPENDS_NO_CYCLES ON) -option(OPENMP_ENABLED "Whether to enable OpenMP parallelization" ON) -option(TESTS_ENABLED "Whether to build test binaries" OFF) -option(ASAN_ENABLED "Whether to enable AddressSanitizer flags" OFF) -option(CCACHE_ENABLED "Whether to enable compiler caching, if available" ON) -option(FETCH_COLMAP "Whether to use COLMAP with FetchContent or with self-installed software" ON) -option(FETCH_POSELIB "Whether to use PoseLib with FetchContent or with self-installed software" ON) option(FETCH_RERUN "Whether to use Rerun with FetchContent or with self-installed software" ON) +# Determine project compiler. +if(CMAKE_CXX_COMPILER_ID STREQUAL "MSVC") + set(IS_MSVC TRUE) +endif() +if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") + set(IS_GNU TRUE) +endif() +if(CMAKE_CXX_COMPILER_ID MATCHES ".*Clang") + set(IS_CLANG TRUE) +endif() include(cmake/FindDependencies.cmake) @@ -41,4 +58,21 @@ else() message(STATUS "Disabling ccache support") endif() +if(IS_MSVC) + # Some fixes for the Glog library. + add_definitions("-DGLOG_USE_GLOG_EXPORT") + add_definitions("-DGLOG_NO_ABBREVIATED_SEVERITIES") + add_definitions("-DGL_GLEXT_PROTOTYPES") + add_definitions("-DNOMINMAX") + add_compile_options(/EHsc) + # Disable warning: 'initializing': conversion from 'X' to 'Y', possible loss of data + add_compile_options(/wd4244 /wd4267 /wd4305) + # Enable object level parallel builds in Visual Studio. + add_compile_options(/MP) + if("${CMAKE_BUILD_TYPE}" STREQUAL "Debug" OR "${CMAKE_BUILD_TYPE}" STREQUAL "RelWithDebInfo") + add_compile_options(/bigobj) + endif() +endif() + +add_subdirectory(thirdparty) add_subdirectory(glomap) diff --git a/README.md b/README.md index b65e3bd6..4475f801 100644 --- a/README.md +++ b/README.md @@ -20,6 +20,7 @@ If you use this project for your research, please cite year={2024}, } ``` +To use the seperate rotation averaging module, refer to [this README](docs/rotation_averager.md). ## Using pixi Easiest way to run this example is using pixi, you can easily install it using curl explained here @@ -31,32 +32,52 @@ then pixi run example ``` +## Installation -## End-to-End with Docker. +To build GLOMAP, first install [COLMAP](https://colmap.github.io/install.html#build-from-source) +dependencies and then build GLOMAP using the following commands: +```shell +mkdir build +cd build +cmake .. -GNinja +ninja && ninja install +``` +Pre-compiled Windows binaries can be downloaded from the official +[release page](https://github.com/colmap/glomap/releases). -Download one of the [official datasets](https://colmap.github.io/datasets.html) unzip it. -For example if we download the `south-building.zip` -```bash -mkdir -p data -unzip south-building.zip -d data/ +After installation, one can run GLOMAP by (starting from a database) +```shell +glomap mapper --database_path DATABASE_PATH --output_path OUTPUT_PATH --image_path IMAGE_PATH ``` +For more details on the command line interface, one can type `glomap -h` or `glomap mapper -h` for help. -Build and enter the docker container by running the following scripts: -```bash -cd docker -./build.sh # Builds the docker image -./start.sh # Starts the docker container -./attach.sh # Enter the container +We also provide a guide on improving the obtained reconstruction, which can be found [here](docs/getting_started.md). -``` +Note: +- GLOMAP depends on two external libraries - [COLMAP](https://github.com/colmap/colmap) and [PoseLib](https://github.com/PoseLib/PoseLib). + With the default setting, the library is built automatically by GLOMAP via `FetchContent`. + However, if a self-installed version is preferred, one can also disable the `FETCH_COLMAP` and `FETCH_POSELIB` CMake options. +- To use `FetchContent`, the minimum required version of `cmake` is 3.28. If a self-installed version is used, `cmake` can be downgraded to 3.10. +- If your system does not provide a recent enough CMake version, you can install it as: + ```shell + wget https://github.com/Kitware/CMake/releases/download/v3.30.1/cmake-3.30.1.tar.gz + tar xfvz cmake-3.30.1.tar.gz && cd cmake-3.30.1 + ./bootstrap && make -j$(nproc) && sudo make install + ``` -When we're inside the container we can build `glomap` and install it. -```bash -mkdir -p build -cd build -cmake .. -GNinja -ninja -ninja install +## End-to-End Example + +In this section, we will use datasets from [this link](https://demuc.de/colmap/datasets) as examples. +Download the datasets and put them under `data` folder. + +### From database + +If a COLMAP database already exists, GLOMAP can directly use it to perform mapping: +```shell +glomap mapper \ + --database_path ./data/gerrard-hall/database.db \ + --image_path ./data/gerrard-hall/images \ + --output_path ./output/gerrard-hall/sparse ``` To run it we must first extract the features and match them with `colmap`. @@ -107,6 +128,22 @@ glomap mapper \ --output_path colosseum/sparse ``` +### Visualize and use the results + +The results are written out in the COLMAP sparse reconstruction format. Please +refer to [COLMAP](https://colmap.github.io/format.html#sparse-reconstruction) +for more details. + +The reconstruction can be visualized using the COLMAP GUI, for example: +```shell +colmap gui --import_path ./output/south-building/sparse/0 +``` +Alternatives like [rerun.io](https://rerun.io/examples/3d-reconstruction/glomap) +also enable visualization of COLMAP and GLOMAP outputs. + +If you want to inspect the reconstruction programmatically, you can use +`pycolmap` in Python or link against COLMAP's C++ library interface. + ### Notes - For larger scale datasets, it is recommended to use `sequential_matcher` or diff --git a/cmake/FindDependencies.cmake b/cmake/FindDependencies.cmake index 73b3ae85..7dc82b08 100644 --- a/cmake/FindDependencies.cmake +++ b/cmake/FindDependencies.cmake @@ -1,51 +1,97 @@ +set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake") + include(FetchContent) -FetchContent_Declare(PoseLib - GIT_REPOSITORY https://github.com/PoseLib/PoseLib.git - GIT_TAG b3691b791bcedccd5451621b2275a1df0d9dcdeb - EXCLUDE_FROM_ALL -) -message(STATUS "Configuring PoseLib...") -if (FETCH_POSELIB) - FetchContent_MakeAvailable(PoseLib) -else() - find_package(PoseLib REQUIRED) -endif() -message(STATUS "Configuring PoseLib... done") - -FetchContent_Declare(COLMAP - GIT_REPOSITORY https://github.com/colmap/colmap.git - GIT_TAG 66fd8e56a0d160d68af2f29e9ac6941d442d2322 - EXCLUDE_FROM_ALL -) -message(STATUS "Configuring COLMAP...") -set(UNINSTALL_ENABLED OFF CACHE INTERNAL "") -if (FETCH_COLMAP) - FetchContent_MakeAvailable(COLMAP) -else() - find_package(COLMAP REQUIRED) -endif() -message(STATUS "Configuring COLMAP... done") message(STATUS "Configuring Rerun...") if (FETCH_RERUN) FetchContent_Declare(rerun_sdk URL - https://github.com/rerun-io/rerun/releases/download/0.17.0/rerun_cpp_sdk.zip) + https://github.com/rerun-io/rerun/releases/download/0.28.1/rerun_cpp_sdk.zip) FetchContent_MakeAvailable(rerun_sdk) else() find_package(rerun_sdk REQUIRED) endif() message(STATUS "Configuring Rerun... done") -find_package(Eigen3 3.4 REQUIRED) +find_package(Eigen3 REQUIRED) 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) +endif() + +if(DEFINED glog_VERSION_MAJOR) + # Older versions of glog don't export version variables. + add_definitions("-DGLOG_VERSION_MAJOR=${glog_VERSION_MAJOR}") + add_definitions("-DGLOG_VERSION_MINOR=${glog_VERSION_MINOR}") +endif() if(TESTS_ENABLED) message(STATUS "Enabling tests") find_package(GTest REQUIRED) endif() -if (OPENMP_ENABLED) - message(STATUS "Enabling OpenMP") - find_package(OpenMP REQUIRED) +set(CUDA_MIN_VERSION "7.0") +if(CUDA_ENABLED) + if(CMAKE_VERSION VERSION_LESS 3.17) + find_package(CUDA QUIET) + if(CUDA_FOUND) + message(STATUS "Found CUDA version ${CUDA_VERSION} installed in " + "${CUDA_TOOLKIT_ROOT_DIR} via legacy CMake (<3.17) module. " + "Using the legacy CMake module means that any installation of " + "COLMAP will require that the CUDA libraries are " + "available under LD_LIBRARY_PATH.") + message(STATUS "Found CUDA ") + message(STATUS " Includes : ${CUDA_INCLUDE_DIRS}") + message(STATUS " Libraries : ${CUDA_LIBRARIES}") + + enable_language(CUDA) + + macro(declare_imported_cuda_target module) + add_library(CUDA::${module} INTERFACE IMPORTED) + target_include_directories( + CUDA::${module} INTERFACE ${CUDA_INCLUDE_DIRS}) + target_link_libraries( + CUDA::${module} INTERFACE ${CUDA_${module}_LIBRARY} ${ARGN}) + endmacro() + + declare_imported_cuda_target(cudart ${CUDA_LIBRARIES}) + declare_imported_cuda_target(curand ${CUDA_LIBRARIES}) + + set(CUDAToolkit_VERSION "${CUDA_VERSION_STRING}") + set(CUDAToolkit_BIN_DIR "${CUDA_TOOLKIT_ROOT_DIR}/bin") + else() + message(STATUS "CUDA not found") + endif() + else() + find_package(CUDAToolkit QUIET) + if(CUDAToolkit_FOUND) + set(CUDA_FOUND ON) + enable_language(CUDA) + else() + message(STATUS "CUDA not found") + endif() + endif() +endif() + +if(CUDA_ENABLED AND CUDA_FOUND) + if(NOT DEFINED CMAKE_CUDA_ARCHITECTURES) + set(CMAKE_CUDA_ARCHITECTURES "native") + endif() + + add_definitions("-DGLOMAP_CUDA_ENABLED") + + # Do not show warnings if the architectures are deprecated. + set(CMAKE_CUDA_FLAGS "${CMAKE_CUDA_FLAGS} -Wno-deprecated-gpu-targets") + # Explicitly set PIC flags for CUDA targets. + if(NOT IS_MSVC) + set(CMAKE_CUDA_FLAGS "${CMAKE_CUDA_FLAGS} --compiler-options -fPIC") + endif() + + message(STATUS "Enabling CUDA support (version: ${CUDAToolkit_VERSION}, " + "archs: ${CMAKE_CUDA_ARCHITECTURES})") +else() + set(CUDA_ENABLED OFF) + message(STATUS "Disabling CUDA support") endif() diff --git a/cmake/FindGlog.cmake b/cmake/FindGlog.cmake new file mode 100644 index 00000000..8bf2ea49 --- /dev/null +++ b/cmake/FindGlog.cmake @@ -0,0 +1,118 @@ +# Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of +# its contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + + +# Find package module for Glog library. +# +# The following variables are set by this module: +# +# GLOG_FOUND: TRUE if Glog is found. +# glog::glog: Imported target to link against. +# +# The following variables control the behavior of this module: +# +# GLOG_INCLUDE_DIR_HINTS: List of additional directories in which to +# search for Glog includes. +# GLOG_LIBRARY_DIR_HINTS: List of additional directories in which to +# search for Glog libraries. + +set(GLOG_INCLUDE_DIR_HINTS "" CACHE PATH "Glog include directory") +set(GLOG_LIBRARY_DIR_HINTS "" CACHE PATH "Glog library directory") + +unset(GLOG_FOUND) + +find_package(glog CONFIG QUIET) +if(TARGET glog::glog) + set(GLOG_FOUND TRUE) + message(STATUS "Found Glog") + message(STATUS " Target : glog::glog") +else() + # Older versions of glog don't come with a find_package config. + # Fall back to custom logic to find the library and remap to imported target. + + include(FindPackageHandleStandardArgs) + + list(APPEND GLOG_CHECK_INCLUDE_DIRS + /usr/local/include + /usr/local/homebrew/include + /opt/local/var/macports/software + /opt/local/include + /usr/include) + list(APPEND GLOG_CHECK_PATH_SUFFIXES + glog/include + glog/Include + Glog/include + Glog/Include + src/windows) + + list(APPEND GLOG_CHECK_LIBRARY_DIRS + /usr/local/lib + /usr/local/homebrew/lib + /opt/local/lib + /usr/lib) + list(APPEND GLOG_CHECK_LIBRARY_SUFFIXES + glog/lib + glog/Lib + Glog/lib + Glog/Lib + x64/Release) + + find_path(GLOG_INCLUDE_DIRS + NAMES + glog/logging.h + PATHS + ${GLOG_INCLUDE_DIR_HINTS} + ${GLOG_CHECK_INCLUDE_DIRS} + PATH_SUFFIXES + ${GLOG_CHECK_PATH_SUFFIXES}) + find_library(GLOG_LIBRARIES + NAMES + glog + libglog + PATHS + ${GLOG_LIBRARY_DIR_HINTS} + ${GLOG_CHECK_LIBRARY_DIRS} + PATH_SUFFIXES + ${GLOG_CHECK_LIBRARY_SUFFIXES}) + + if(GLOG_INCLUDE_DIRS AND GLOG_LIBRARIES) + set(GLOG_FOUND TRUE) + message(STATUS "Found Glog") + message(STATUS " Includes : ${GLOG_INCLUDE_DIRS}") + message(STATUS " Libraries : ${GLOG_LIBRARIES}") + endif() + + add_library(glog::glog INTERFACE IMPORTED) + target_include_directories(glog::glog INTERFACE ${GLOG_INCLUDE_DIRS}) + target_link_libraries(glog::glog INTERFACE ${GLOG_LIBRARIES}) +endif() + +if(NOT GLOG_FOUND AND GLOG_FIND_REQUIRED) + message(FATAL_ERROR "Could not find Glog") +endif() diff --git a/cmake/FindMETIS.cmake b/cmake/FindMETIS.cmake new file mode 100644 index 00000000..5f41792d --- /dev/null +++ b/cmake/FindMETIS.cmake @@ -0,0 +1,110 @@ +# +# Copyright (c) 2022 Sergiu Deitsch +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTMETISLAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. +# +#[=======================================================================[.rst: +Module for locating METIS +========================= + +Read-only variables: + +``METIS_FOUND`` + Indicates whether the library has been found. + +``METIS_VERSION`` + Indicates library version. + +Targets +------- + +``METIS::METIS`` + Specifies targets that should be passed to target_link_libararies. +]=======================================================================] + +include (FindPackageHandleStandardArgs) + +find_path (METIS_INCLUDE_DIR NAMES metis.h + PATH_SUFFIXES include + DOC "METIS include directory") +find_library (METIS_LIBRARY_DEBUG NAMES metis + PATH_SUFFIXES Debug + DOC "METIS debug library") +find_library (METIS_LIBRARY_RELEASE NAMES metis + PATH_SUFFIXES Release + DOC "METIS release library") + +if (METIS_LIBRARY_RELEASE) + if (METIS_LIBRARY_DEBUG) + set (METIS_LIBRARY debug ${METIS_LIBRARY_DEBUG} optimized + ${METIS_LIBRARY_RELEASE} CACHE STRING "METIS library") + else (METIS_LIBRARY_DEBUG) + set (METIS_LIBRARY ${METIS_LIBRARY_RELEASE} CACHE FILEPATH "METIS library") + endif (METIS_LIBRARY_DEBUG) +elseif (METIS_LIBRARY_DEBUG) + set (METIS_LIBRARY ${METIS_LIBRARY_DEBUG} CACHE FILEPATH "METIS library") +endif (METIS_LIBRARY_RELEASE) + +set (_METIS_VERSION_HEADER ${METIS_INCLUDE_DIR}/metis.h) + +if (EXISTS ${_METIS_VERSION_HEADER}) + file (READ ${_METIS_VERSION_HEADER} _METIS_VERSION_CONTENTS) + + string (REGEX REPLACE ".*#define METIS_VER_MAJOR[ \t]+([0-9]+).*" "\\1" + METIS_VERSION_MAJOR "${_METIS_VERSION_CONTENTS}") + string (REGEX REPLACE ".*#define METIS_VER_MINOR[ \t]+([0-9]+).*" "\\1" + METIS_VERSION_MINOR "${_METIS_VERSION_CONTENTS}") + string (REGEX REPLACE ".*#define METIS_VER_SUBMINOR[ \t]+([0-9]+).*" "\\1" + METIS_VERSION_PATCH "${_METIS_VERSION_CONTENTS}") + + set (METIS_VERSION + ${METIS_VERSION_MAJOR}.${METIS_VERSION_MINOR}.${METIS_VERSION_PATCH}) + set (METIS_VERSION_COMPONENTS 3) +endif (EXISTS ${_METIS_VERSION_HEADER}) + +mark_as_advanced (METIS_INCLUDE_DIR METIS_LIBRARY_DEBUG METIS_LIBRARY_RELEASE + METIS_LIBRARY) + +if (NOT TARGET METIS::METIS) + if (METIS_INCLUDE_DIR OR METIS_LIBRARY) + add_library (METIS::METIS IMPORTED UNKNOWN) + endif (METIS_INCLUDE_DIR OR METIS_LIBRARY) +endif (NOT TARGET METIS::METIS) + +if (METIS_INCLUDE_DIR) + set_property (TARGET METIS::METIS PROPERTY INTERFACE_INCLUDE_DIRECTORIES + ${METIS_INCLUDE_DIR}) +endif (METIS_INCLUDE_DIR) + +if (METIS_LIBRARY_RELEASE) + set_property (TARGET METIS::METIS PROPERTY IMPORTED_LOCATION_RELEASE + ${METIS_LIBRARY_RELEASE}) + set_property (TARGET METIS::METIS APPEND PROPERTY IMPORTED_CONFIGURATIONS + RELEASE) +endif (METIS_LIBRARY_RELEASE) + +if (METIS_LIBRARY_DEBUG) + set_property (TARGET METIS::METIS PROPERTY IMPORTED_LOCATION_DEBUG + ${METIS_LIBRARY_DEBUG}) + set_property (TARGET METIS::METIS APPEND PROPERTY IMPORTED_CONFIGURATIONS + DEBUG) +endif (METIS_LIBRARY_DEBUG) + +find_package_handle_standard_args (METIS REQUIRED_VARS + METIS_INCLUDE_DIR METIS_LIBRARY VERSION_VAR METIS_VERSION) diff --git a/docker/Dockerfile b/docker/Dockerfile index c81329b2..a56b2756 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -1,4 +1,4 @@ -FROM ubuntu:24.10 +FROM ubuntu:24.04 RUN apt-get update && apt-get install -y --no-install-recommends --no-install-suggests \ curl \ @@ -34,7 +34,7 @@ RUN apt-get update && apt-get install -y --no-install-recommends --no-install-su WORKDIR /ws/ -RUN pipx install rerun-sdk==0.17.0 +RUN pipx install rerun-sdk==0.28.1 ENV PATH=$PATH:/root/.local/share/pipx/venvs/rerun-sdk/bin/ # RUN ./install_colmap.sh diff --git a/docs/getting_started.md b/docs/getting_started.md index 653b4364..a6399f56 100644 --- a/docs/getting_started.md +++ b/docs/getting_started.md @@ -1,20 +1,52 @@ # Getting started -### Installation and End-to-End Examples -Please refer to the main `README.md` -### Recommended practice +## Installation and end-to-end examples + +Please refer to the main `README.md`. + +## Recommended settings + The default parameters do not always gaurantee satisfying reconstructions. -Regarding this, there are several things which can generally help +Regarding this, there are several things which can generally help. + +### Share camera parameters + +If images are known to be taken with the same physical camera under identical +camera settings, or images are well organized and known to be taken by several +cameras, it is higly recommended to share the camera intrinsics as appropriate. +To achieve this, one can set `--ImageReader.single_camera_per_folder` or +`--ImageReader.single_camera_per_image` in `colmap feature_extractor`. + +### Handle high-resolution or blurry images -#### Share camera parameters as much as possible -If images are known to be taken with the same camera, or images are well organized and known to be taken by several cameras, it is higly recommended to share the camera intrinsics -To achieve this, one can set `--ImageReader.single_camera_per_folder` or `--ImageReader.single_camera_per_image` in `colmap feature_extractor` to be 1. +If images have high resolution or are blurry, it is worth trying to increase the +allowed epipolar error by modifying `--RelPoseEstimation.max_epipolar_error`. +For example, increase it to 4 or 10. -#### Allow larger epipolar error -If images are of high resolution, or are blurry, it is worth trying to increase the allowed epipolar error by modifying `--RelPoseEstimation.max_epipolar_error`. For example, make it 4, or 10. +### Speedup reconstruction process #### Cap the number of tracks -If the number of images and points are large, the run-time of global bundle adjustment can be long. In this case, to further speed up the overall reconstruction process, the total number of points can be capped, by changing `--TrackEstablishment.max_num_tracks`. Typically, one image should not need more than 1000 tracks to achieve good performance, so this number can be adjusted to $1000 \times n$. -Afterwards, if a full point cloud is desired (for example, to initialize a Gaussian Splatting), points can be triangulated directly by calling `colmap point_triangulator`. -Note, if the `--skip_retriangulation` is not set true when calling `glomap mapper`, retriangulation should already been performed. +If the number of images and points are large, the run-time of global bundle +adjustment can be long. In this case, to further speed up the overall +reconstruction process, the total number of points can be capped, by changing +`--TrackEstablishment.max_num_tracks`. Typically, one image should not need more +than 1000 tracks to achieve good performance, so this number can be adjusted to +$1000 \times n$. Afterwards, if a full point cloud is desired (for example, to +initialize a Gaussian Splatting), points can be triangulated directly by calling +`colmap point_triangulator`. + +Note, if the `--skip_retriangulation` is not set when calling `glomap mapper`, +retriangulation should already been performed. + +#### Limit optimization iterations + +The number of global positioning and bundle adjustment iterations can be limited +using the `--GlobalPositioning.max_num_iterations` and +`--BundleAdjustment.max_num_iterations` options. + +#### Enable GPU-based solver + +If Ceres 2.3 or above is installed and cuDSS is installed, GLOMAP supports GPU +accelerated optimization. The process can be largely sped up with flags +`--GlobalPositioning.use_gpu 1 --BundleAdjustment.use_gpu`. diff --git a/docs/rotation_averager.md b/docs/rotation_averager.md new file mode 100644 index 00000000..1fc342de --- /dev/null +++ b/docs/rotation_averager.md @@ -0,0 +1,69 @@ +# Gravity-aligned Rotation Averaging with Circular Regression + +[Project page](https://lpanaf.github.io/eccv24_ra1dof/) | [Paper](https://www.ecva.net/papers/eccv_2024/papers_ECCV/papers/05651.pdf) | [Supp.](https://lpanaf.github.io/assets/pdf/eccv24_ra1dof_sm.pdf) +--- + +## About + +This project aims at solving the rotation averaging problem with gravity prior. +To achieve this, circular regression is leveraged. + +If you use this project for your research, please cite +``` +@inproceedings{pan2024ra1dof, + author={Pan, Linfei and Pollefeys, Marc and Barath, Daniel}, + title={{Gravity-aligned Rotation Averaging with Circular Regression}}, + booktitle={European Conference on Computer Vision (ECCV)}, + year={2024}, +} +``` + +## Getting Started +Install GLOMAP as instrcucted in [README](../README.md). +Then, call the rotation averager (3 degree-of-freedom) via +``` +glomap rotation_averager --relpose_path RELPOSE_PATH --output_path OUTPUT_PATH +``` + +If gravity directions are available, call the rotation averager (1 degree-of-freedom) via +``` +glomap rotation_averager \ + --relpose_path RELPOSE_PATH \ + --output_path OUTPUT_PATH \ + --gravity_path GRAVTIY PATH +``` +It is recommended to set `--use_stratified=1` if only a subset of images have gravity direction. +If gravity measurements are subject to i.i.d. noise, they can be refined by setting `--refine_gravity=1`. + + +## File Formats +### Relative Pose +The relative pose file is expected to be of the following format +``` +IMAGE_NAME_1 IMAGE_NAME_2 QW QX QY QZ TX TY TZ +``` +Only images contained in at least one relative pose will be included in the following procedure. +The relative pose should be 2R1 x1 + 2t1 = x2. + +### Gravity Direction +The gravity direction file is expected to be of the following format +``` +IMAGE_NAME GX GY GZ +``` +The gravity direction $g$ should $[0, 1, 0]$ if the image is orthogonal to the ground plane, and the estimated rotation would have the property that $R_i \cdot [0, 1, 0]^\top = g$. +More explicitly, suppose we can transpose a 3D point from the world coordinate to the image coordinate by RX + t = x. Here: +- `R` is a 3x3 rotation matrix that aligns the world coordinate system with the image coordinate system. +- `X` is a 3D point in the world coordinate system. +- `t` is a 3x1 translation vector that shifts the world coordinate system to the image coordinate system. +- `x` is the corresponding 3D point in the image coordinate system. +The gravity direction should be the second column of the rotation matrix `R`. + +It is acceptable if only a subset of all images have gravity direction. +If the specified image name does not match any known image name from relative pose, it is ignored. + +### Output +The estimated global rotation will be in the following format +``` +IMAGE_NAME QW QX QY QZ +``` +Any images that are not within the largest connected component of the view-graph formed by the relative pose will be ignored. diff --git a/glomap/CMakeLists.txt b/glomap/CMakeLists.txt index 856e20da..f73bd7f2 100644 --- a/glomap/CMakeLists.txt +++ b/glomap/CMakeLists.txt @@ -1,6 +1,7 @@ set(SOURCES controllers/global_mapper.cc controllers/option_manager.cc + controllers/rotation_averager.cc controllers/track_establishment.cc controllers/track_retriangulation.cc estimators/bundle_adjustment.cc @@ -8,10 +9,11 @@ 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 - io/gravity_io.cc + io/pose_io.cc io/recording.cc math/gravity.cc math/rigid3d.cc @@ -19,6 +21,7 @@ set(SOURCES math/two_view_geometry.cc processors/image_pair_inliers.cc processors/image_undistorter.cc + processors/reconstruction_normalizer.cc processors/reconstruction_pruning.cc processors/relpose_filter.cc processors/track_filter.cc @@ -29,6 +32,7 @@ set(SOURCES set(HEADERS controllers/global_mapper.h controllers/option_manager.h + controllers/rotation_averager.h controllers/track_establishment.h controllers/track_retriangulation.h estimators/bundle_adjustment.h @@ -36,26 +40,27 @@ 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 - io/gravity_io.h + io/pose_io.h io/recording.h math/gravity.h - math/l1_solver.h math/rigid3d.h math/tree.h math/two_view_geometry.h - math/union_find.h processors/image_pair_inliers.h processors/image_undistorter.h + processors/reconstruction_normalizer.h processors/reconstruction_pruning.h processors/relpose_filter.h 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 @@ -83,26 +88,16 @@ target_link_libraries( PUBLIC Eigen3::Eigen Ceres::ceres + OpenMP::OpenMP_CXX ${BOOST_LIBRARIES} - ${SuiteSparse_CHOLMOD_LIBRARY} -) -target_include_directories( - glomap - PUBLIC - .. - ${SuiteSparse_CHOLMOD_INCLUDE_DIR} ) - -if(OPENMP_FOUND) - target_link_libraries(glomap PUBLIC OpenMP::OpenMP_CXX) -endif() +target_include_directories(glomap PUBLIC ..) if(MSVC) target_compile_options(glomap PRIVATE /bigobj) else() target_compile_options(glomap PRIVATE -Wall - -Werror -Wno-sign-compare -Wno-unused-variable ) @@ -112,16 +107,19 @@ endif() add_executable(glomap_main glomap.cc exe/global_mapper.h - exe/global_mapper.cc) + exe/global_mapper.cc + exe/rotation_averager.h + exe/rotation_averager.cc +) target_link_libraries(glomap_main glomap) set_target_properties(glomap_main PROPERTIES OUTPUT_NAME glomap) install(TARGETS glomap_main DESTINATION bin) - if(TESTS_ENABLED) add_executable(glomap_test controllers/global_mapper_test.cc + controllers/rotation_averager_test.cc ) target_link_libraries( glomap_test diff --git a/glomap/controllers/global_mapper.cc b/glomap/controllers/global_mapper.cc index 75615780..c433f840 100644 --- a/glomap/controllers/global_mapper.cc +++ b/glomap/controllers/global_mapper.cc @@ -1,20 +1,27 @@ #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" +#include "glomap/processors/reconstruction_normalizer.h" #include "glomap/processors/reconstruction_pruning.h" #include "glomap/processors/relpose_filter.h" #include "glomap/processors/track_filter.h" #include "glomap/processors/view_graph_manipulation.h" #include "glomap/io/recording.h" +#include #include 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 @@ -44,6 +51,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; @@ -64,7 +72,10 @@ bool GlobalMapper::Solve(const colmap::Database& database, RelPoseFilter::FilterInlierRatio( view_graph, options_.inlier_thresholds.min_inlier_ratio); - view_graph.KeepLargestConnectedComponents(images); + if (view_graph.KeepLargestConnectedComponents(frames, images) == 0) { + LOG(ERROR) << "no connected components are found"; + return false; + } run_timer.PrintSeconds(); } @@ -78,21 +89,28 @@ 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); - view_graph.KeepLargestConnectedComponents(images); + 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; + } LOG(INFO) << num_img << " / " << images.size() << " images are within the connected component." << std::endl; @@ -125,6 +143,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 @@ -134,24 +158,11 @@ bool GlobalMapper::Solve(const colmap::Database& database, GlobalPositioner gp_engine(options_.opt_gp); rr_rec.set_time_sequence("step", algorithm_step); rr_rec.log("status", rerun::TextLog("Starting global positioning")); - 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, @@ -159,6 +170,24 @@ bool GlobalMapper::Solve(const colmap::Database& database, images, 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 + // If the camera rig is used, the structure do not need to be normalized + NormalizeReconstruction(rigs, cameras, frames, images, tracks); + run_timer.PrintSeconds(); rr_rec.set_time_sequence("step", algorithm_step++); rr_rec.log("status", rerun::TextLog("Filter tracks based on the estimation")); @@ -184,7 +213,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 << " / " @@ -200,7 +229,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 << " / " @@ -209,6 +238,9 @@ bool GlobalMapper::Solve(const colmap::Database& database, if (ite != options_.num_iteration_bundle_adjustment - 1) run_timer.PrintSeconds(); + // Normalize the structure + NormalizeReconstruction(rigs, cameras, frames, images, tracks); + rr_rec.set_time_sequence("step", algorithm_step++); rr_rec.log("status", rerun::TextLog("Global bundle adjustment iteration " + std::to_string(ite+1) + ", stage 2 finished")); log_reconstruction(rr_rec, cameras, images, tracks); @@ -277,8 +309,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; @@ -286,7 +323,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; } @@ -299,12 +336,15 @@ 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(rigs, cameras, frames, images, tracks); + // Filter tracks based on the estimation UndistortImages(cameras, images, true); LOG(INFO) << "Filtering tracks by reprojection ..."; @@ -334,7 +374,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(); } @@ -346,4 +386,4 @@ bool GlobalMapper::Solve(const colmap::Database& database, return true; } -} // namespace glomap \ No newline at end of file +} // namespace glomap diff --git a/glomap/controllers/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 044c917e..98b8fc2b 100644 --- a/glomap/controllers/global_mapper_test.cc +++ b/glomap/controllers/global_mapper_test.cc @@ -40,13 +40,6 @@ void ExpectEqualReconstructions(const colmap::Reconstruction& gt, GlobalMapperOptions CreateTestOptions() { GlobalMapperOptions options; - // Control the verbosity of the global sfm - options.opt_vgcalib.verbose = false; - options.opt_ra.verbose = false; - options.opt_gp.verbose = false; - options.opt_ba.verbose = false; - - // Control the flow of the global sfm options.skip_view_graph_calibration = false; options.skip_relative_pose_estimation = false; options.skip_rotation_averaging = false; @@ -60,28 +53,119 @@ 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_rigs = 2; + synthetic_dataset_options.num_cameras_per_rig = 1; + synthetic_dataset_options.num_frames_per_rig = 7; + synthetic_dataset_options.num_points3D = 50; + 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, 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.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_cameras = 2; - synthetic_dataset_options.num_images = 7; + 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); + 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); + + // 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, 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, @@ -93,29 +177,36 @@ 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()); + colmap::SyntheticNoiseOptions synthetic_noise_options; + synthetic_noise_options.point2D_stddev = 0.5; + colmap::SynthesizeNoise( + synthetic_noise_options, >_reconstruction, database.get()); ViewGraph view_graph; std::unordered_map cameras; + 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 39c17f8f..6035cbfa 100644 --- a/glomap/controllers/option_manager.cc +++ b/glomap/controllers/option_manager.cc @@ -1,6 +1,7 @@ #include "option_manager.h" #include "glomap/controllers/global_mapper.h" +#include "glomap/estimators/gravity_refinement.h" #include #include @@ -14,9 +15,13 @@ OptionManager::OptionManager(bool add_project_options) { image_path = std::make_shared(); mapper = std::make_shared(); + gravity_refiner = std::make_shared(); Reset(); desc_->add_options()("help,h", ""); + + AddAndRegisterDefaultOption("log_to_stderr", &FLAGS_logtostderr); + AddAndRegisterDefaultOption("log_level", &FLAGS_v); } void OptionManager::AddAllOptions() { @@ -177,6 +182,10 @@ void OptionManager::AddGlobalPositionerOptions() { return; } added_global_positioning_options_ = true; + AddAndRegisterDefaultOption("GlobalPositioning.use_gpu", + &mapper->opt_gp.use_gpu); + AddAndRegisterDefaultOption("GlobalPositioning.gpu_index", + &mapper->opt_gp.gpu_index); AddAndRegisterDefaultOption("GlobalPositioning.optimize_positions", &mapper->opt_gp.optimize_positions); AddAndRegisterDefaultOption("GlobalPositioning.optimize_points", @@ -196,12 +205,20 @@ void OptionManager::AddBundleAdjusterOptions() { return; } added_bundle_adjustment_options_ = true; + AddAndRegisterDefaultOption("BundleAdjustment.use_gpu", + &mapper->opt_ba.use_gpu); + AddAndRegisterDefaultOption("BundleAdjustment.gpu_index", + &mapper->opt_ba.gpu_index); + AddAndRegisterDefaultOption("BundleAdjustment.optimize_rig_poses", + &mapper->opt_ba.optimize_rig_poses); AddAndRegisterDefaultOption("BundleAdjustment.optimize_rotations", &mapper->opt_ba.optimize_rotations); AddAndRegisterDefaultOption("BundleAdjustment.optimize_translation", &mapper->opt_ba.optimize_translation); AddAndRegisterDefaultOption("BundleAdjustment.optimize_intrinsics", &mapper->opt_ba.optimize_intrinsics); + AddAndRegisterDefaultOption("BundleAdjustment.optimize_principal_point", + &mapper->opt_ba.optimize_principal_point); AddAndRegisterDefaultOption("BundleAdjustment.optimize_points", &mapper->opt_ba.optimize_points); AddAndRegisterDefaultOption("BundleAdjustment.thres_loss_function", @@ -231,6 +248,14 @@ void OptionManager::AddInlierThresholdOptions() { return; } added_inliers_options_ = true; + AddAndRegisterDefaultOption("Thresholds.max_angle_error", + &mapper->inlier_thresholds.max_angle_error); + AddAndRegisterDefaultOption( + "Thresholds.max_reprojection_error", + &mapper->inlier_thresholds.max_reprojection_error); + AddAndRegisterDefaultOption( + "Thresholds.min_triangulation_angle", + &mapper->inlier_thresholds.min_triangulation_angle); AddAndRegisterDefaultOption("Thresholds.max_epipolar_error_E", &mapper->inlier_thresholds.max_epipolar_error_E); AddAndRegisterDefaultOption("Thresholds.max_epipolar_error_F", @@ -245,6 +270,18 @@ void OptionManager::AddInlierThresholdOptions() { &mapper->inlier_thresholds.max_rotation_error); } +void OptionManager::AddGravityRefinerOptions() { + if (added_gravity_refiner_options_) { + return; + } + added_gravity_refiner_options_ = true; + AddAndRegisterDefaultOption("GravityRefiner.max_outlier_ratio", + &gravity_refiner->max_outlier_ratio); + AddAndRegisterDefaultOption("GravityRefiner.max_gravity_error", + &gravity_refiner->max_gravity_error); + AddAndRegisterDefaultOption("GravityRefiner.min_num_neighbors", + &gravity_refiner->min_num_neighbors); +} void OptionManager::Reset() { const bool kResetPaths = true; ResetOptions(kResetPaths); @@ -273,6 +310,7 @@ void OptionManager::ResetOptions(const bool reset_paths) { *image_path = ""; } *mapper = GlobalMapperOptions(); + *gravity_refiner = GravityRefinerOptions(); } void OptionManager::Parse(const int argc, char** argv) { diff --git a/glomap/controllers/option_manager.h b/glomap/controllers/option_manager.h index 0926387d..030d38e3 100644 --- a/glomap/controllers/option_manager.h +++ b/glomap/controllers/option_manager.h @@ -18,6 +18,7 @@ struct GlobalPositionerOptions; struct BundleAdjusterOptions; struct TriangulatorOptions; struct InlierThresholdOptions; +struct GravityRefinerOptions; class OptionManager { public: @@ -37,6 +38,7 @@ class OptionManager { void AddBundleAdjusterOptions(); void AddTriangulatorOptions(); void AddInlierThresholdOptions(); + void AddGravityRefinerOptions(); template void AddRequiredOption(const std::string& name, @@ -56,6 +58,7 @@ class OptionManager { std::shared_ptr image_path; std::shared_ptr mapper; + std::shared_ptr gravity_refiner; private: template @@ -88,6 +91,7 @@ class OptionManager { bool added_bundle_adjustment_options_ = false; bool added_triangulation_options_ = false; bool added_inliers_options_ = false; + bool added_gravity_refiner_options_ = false; }; template diff --git a/glomap/controllers/rotation_averager.cc b/glomap/controllers/rotation_averager.cc new file mode 100644 index 00000000..287e45dd --- /dev/null +++ b/glomap/controllers/rotation_averager.cc @@ -0,0 +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(frames, images); + + bool solve_1dof_system = options.use_gravity && options.use_stratified; + + ViewGraph view_graph_grav; + image_pair_t total_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; + + const Image& image1 = images[image_pair.image_id1]; + const Image& image2 = images[image_pair.image_id2]; + + if (!image1.IsRegistered() || !image2.IsRegistered()) continue; + + total_pairs++; + + if (image1.HasGravity() && image2.HasGravity()) { + view_graph_grav.image_pairs.emplace( + pair_id, + 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 + 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"; + view_graph_grav.KeepLargestConnectedComponents(frames, images); + RotationEstimator rotation_estimator_grav(options); + if (!rotation_estimator_grav.EstimateRotations( + view_graph_grav, rigs, frames, images)) { + return false; + } + 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); + } + } + } + + 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 diff --git a/glomap/controllers/rotation_averager.h b/glomap/controllers/rotation_averager.h new file mode 100644 index 00000000..1ba93102 --- /dev/null +++ b/glomap/controllers/rotation_averager.h @@ -0,0 +1,20 @@ +#pragma once + +#include "glomap/estimators/global_rotation_averaging.h" + +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); + +} // namespace glomap \ No newline at end of file diff --git a/glomap/controllers/rotation_averager_test.cc b/glomap/controllers/rotation_averager_test.cc new file mode 100644 index 00000000..1dac8abd --- /dev/null +++ b/glomap/controllers/rotation_averager_test.cc @@ -0,0 +1,453 @@ +#include "glomap/controllers/rotation_averager.h" + +#include "glomap/controllers/global_mapper.h" +#include "glomap/estimators/gravity_refinement.h" +#include "glomap/io/colmap_io.h" +#include "glomap/math/rigid3d.h" +#include "glomap/types.h" + +#include +#include +#include +#include + +#include + +namespace glomap { +namespace { + +void CreateRandomRotation(const double stddev, Eigen::Quaterniond& q) { + std::random_device rd{}; + std::mt19937 gen{rd()}; + + // Construct a random axis + double theta = 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)); + + // Construct a random angle + std::normal_distribution d{0, stddev}; + double angle = d(gen); + q = Eigen::AngleAxisd(angle, axis); +} + +void PrepareGravity(const colmap::Reconstruction& gt, + std::unordered_map& frames, + double gravity_noise_stddev = 0.0, + double outlier_ratio = 0.0) { + 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 && + colmap::RandomUniformReal(0, 1) < outlier_ratio) { + Eigen::Quaterniond q; + CreateRandomRotation(1., q); + gravityInRig = + Rigid3dToAngleAxis(Rigid3d(q, Eigen::Vector3d::Zero())).normalized(); + } + + 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(); + } +} + +GlobalMapperOptions CreateMapperTestOptions() { + GlobalMapperOptions options; + options.skip_view_graph_calibration = false; + options.skip_relative_pose_estimation = false; + options.skip_rotation_averaging = true; + options.skip_track_establishment = true; + options.skip_global_positioning = true; + options.skip_bundle_adjustment = true; + options.skip_retriangulation = true; + return options; +} + +RotationAveragerOptions CreateRATestOptions(bool use_gravity = false) { + RotationAveragerOptions options; + options.skip_initialization = 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::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 < i; j++) { + const image_t image_id2 = reg_image_ids[j]; + + const Rigid3d cam2_from_cam1 = + computed.Image(image_id2).CamFromWorld() * + Inverse(computed.Image(image_id1).CamFromWorld()); + const Rigid3d cam2_from_cam1_gt = + gt.Image(image_id2).CamFromWorld() * + Inverse(gt.Image(image_id1).CamFromWorld()); + + const double rotation_error_deg = + CalcAngle(cam2_from_cam1_gt, cam2_from_cam1); + EXPECT_LT(rotation_error_deg, max_rotation_error_deg); + } + } +} + +void ExpectEqualGravity( + const colmap::Reconstruction& gt, + const std::unordered_map& images_computed, + const double max_gravity_error_deg) { + for (const auto& image_id : gt.RegImageIds()) { + 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).frame_ptr->gravity_info.GetGravity(); + + double gravity_error_deg = CalcAngle(gravity_gt, gravity_computed); + EXPECT_LT(gravity_error_deg, max_gravity_error_deg); + } +} + +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.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.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"; + + 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.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); + + 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, rigs, cameras, frames, images, tracks); + + // For unknown rigs, it is not supported to use gravity. + 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, WithNoiseAndOutliers) { + 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 = 7; + synthetic_dataset_options.num_points3D = 100; + synthetic_dataset_options.inlier_match_ratio = 0.6; + colmap::SynthesizeDataset( + synthetic_dataset_options, >_reconstruction, database.get()); + colmap::SyntheticNoiseOptions synthetic_noise_options; + synthetic_noise_options.point2D_stddev = 1; + colmap::SynthesizeNoise( + synthetic_noise_options, >_reconstruction, database.get()); + + ViewGraph view_graph; + std::unordered_map rigs; + 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 : {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=*/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.inlier_match_ratio = 0.6; + colmap::SynthesizeDataset( + synthetic_dataset_options, >_reconstruction, database.get()); + colmap::SyntheticNoiseOptions synthetic_noise_options; + synthetic_noise_options.point2D_stddev = 1; + colmap::SynthesizeNoise( + synthetic_noise_options, >_reconstruction, database.get()); + + ViewGraph view_graph; + std::unordered_map rigs; + 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); + else + ExpectEqualRotations( + gt_reconstruction, reconstruction, /*max_rotation_error_deg=*/2.); + } +} + +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; + 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"; + + 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 = 25; + synthetic_dataset_options.num_points3D = 100; + 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); +} + +} // namespace +} // namespace glomap diff --git a/glomap/controllers/track_establishment.cc b/glomap/controllers/track_establishment.cc index 105f7763..77c307ec 100644 --- a/glomap/controllers/track_establishment.cc +++ b/glomap/controllers/track_establishment.cc @@ -5,7 +5,7 @@ namespace glomap { size_t TrackEngine::EstablishFullTracks( std::unordered_map& tracks) { tracks.clear(); - uf_.Clear(); + uf_ = {}; // Blindly concatenate tracks if any matches occur BlindConcatenation(); @@ -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_establishment.h b/glomap/controllers/track_establishment.h index 7e0c3ef0..1eb6a58b 100644 --- a/glomap/controllers/track_establishment.h +++ b/glomap/controllers/track_establishment.h @@ -1,9 +1,10 @@ #pragma once -#include "glomap/math/union_find.h" #include "glomap/scene/types_sfm.h" +#include + namespace glomap { struct TrackEstablishmentOptions { @@ -53,7 +54,7 @@ class TrackEngine { const std::unordered_map& images_; // Internal structure used for concatenating tracks - UnionFind uf_; + colmap::UnionFind uf_; }; } // namespace glomap diff --git a/glomap/controllers/track_retriangulation.cc b/glomap/controllers/track_retriangulation.cc index 18559c20..0b133e0d 100644 --- a/glomap/controllers/track_retriangulation.cc +++ b/glomap/controllers/track_retriangulation.cc @@ -2,15 +2,19 @@ #include "glomap/io/colmap_converter.h" -#include +#include #include #include +#include + 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 @@ -26,21 +30,23 @@ 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); - colmap::IncrementalMapperOptions options_colmap; + colmap::IncrementalPipelineOptions options_colmap; options_colmap.triangulation.complete_max_reproj_error = options.tri_complete_max_reproj_error; options_colmap.triangulation.merge_max_reproj_error = @@ -57,13 +63,13 @@ bool RetriangulateTracks(const TriangulatorOptions& options, const auto tri_options = options_colmap.Triangulation(); const auto mapper_options = options_colmap.Mapper(); - const std::vector& reg_image_ids = reconstruction_ptr->RegImageIds(); + const std::vector reg_image_ids = reconstruction_ptr->RegImageIds(); - for (size_t i = 0; i < reg_image_ids.size(); ++i) { - std::cout << "\r Triangulating image " << i + 1 << " / " + size_t image_idx = 0; + for (const image_t image_id : reg_image_ids) { + std::cout << "\r Triangulating image " << image_idx++ + 1 << " / " << reg_image_ids.size() << std::flush; - const image_t image_id = reg_image_ids[i]; const auto& image = reconstruction_ptr->Image(image_id); int num_tris = mapper.TriangulateImage(tri_options, image_id); @@ -77,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; @@ -96,10 +103,10 @@ bool RetriangulateTracks(const TriangulatorOptions& options, const size_t num_observations = reconstruction_ptr->ComputeNumObservations(); - // PrintHeading1("Bundle adjustment"); - colmap::BundleAdjuster bundle_adjuster(ba_options, ba_config); - // THROW_CHECK(bundle_adjuster.Solve(reconstruction.get())); - if (!bundle_adjuster.Solve(reconstruction_ptr.get())) { + std::unique_ptr bundle_adjuster; + bundle_adjuster = + CreateDefaultBundleAdjuster(ba_options, ba_config, *reconstruction_ptr); + if (bundle_adjuster->Solve().termination_type == ceres::FAILURE) { return false; } @@ -116,16 +123,17 @@ 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; } -} // namespace glomap \ No newline at end of file +} // namespace glomap 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 a03bea5c..8edffb01 100644 --- a/glomap/estimators/bundle_adjustment.cc +++ b/glomap/estimators/bundle_adjustment.cc @@ -3,11 +3,14 @@ #include #include #include +#include +#include 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 @@ -24,25 +27,77 @@ 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; + int num_images = images.size(); +#ifdef GLOMAP_CUDA_ENABLED + bool cuda_solver_enabled = false; + +#if (CERES_VERSION_MAJOR >= 3 || \ + (CERES_VERSION_MAJOR == 2 && CERES_VERSION_MINOR >= 2)) && \ + !defined(CERES_NO_CUDA) + if (options_.use_gpu && num_images >= options_.min_num_images_gpu_solver) { + cuda_solver_enabled = true; + options_.solver_options.dense_linear_algebra_library_type = ceres::CUDA; + } +#else + if (options_.use_gpu) { + LOG_FIRST_N(WARNING, 1) + << "Requested to use GPU for bundle adjustment, but Ceres was " + "compiled without CUDA support. Falling back to CPU-based dense " + "solvers."; + } +#endif + +#if (CERES_VERSION_MAJOR >= 3 || \ + (CERES_VERSION_MAJOR == 2 && CERES_VERSION_MINOR >= 3)) && \ + !defined(CERES_NO_CUDSS) + if (options_.use_gpu && num_images >= options_.min_num_images_gpu_solver) { + cuda_solver_enabled = true; + options_.solver_options.sparse_linear_algebra_library_type = + ceres::CUDA_SPARSE; + } +#else + if (options_.use_gpu) { + LOG_FIRST_N(WARNING, 1) + << "Requested to use GPU for bundle adjustment, but Ceres was " + "compiled without cuDSS support. Falling back to CPU-based sparse " + "solvers."; + } +#endif + + if (cuda_solver_enabled) { + const std::vector gpu_indices = + colmap::CSVToVector(options_.gpu_index); + THROW_CHECK_GT(gpu_indices.size(), 0); + colmap::SetBestCudaDevice(gpu_indices[0]); + } +#else + if (options_.use_gpu) { + LOG_FIRST_N(WARNING, 1) + << "Requested to use GPU for bundle adjustment, but COLMAP was " + "compiled without CUDA support. Falling back to CPU-based " + "solvers."; + } +#endif // GLOMAP_CUDA_ENABLED + // Do not use the iterative solver, as it does not seem to be helpful options_.solver_options.linear_solver_type = ceres::SPARSE_SCHUR; options_.solver_options.preconditioner_type = ceres::CLUSTER_TRIDIAGONAL; - options_.solver_options.minimizer_progress_to_stdout = options_.verbose; + options_.solver_options.minimizer_progress_to_stdout = VLOG_IS_ON(2); ceres::Solve(options_.solver_options, problem_.get(), &summary); - if (options_.verbose) + if (VLOG_IS_ON(2)) LOG(INFO) << summary.FullReport(); else LOG(INFO) << summary.BriefReport(); @@ -54,11 +109,13 @@ void BundleAdjuster::Reset() { ceres::Problem::Options problem_options; problem_options.loss_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; problem_ = std::make_unique(problem_options); + loss_function_ = options_.CreateLossFunction(); } void BundleAdjuster::AddPointToCameraConstraints( - 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) { @@ -68,20 +125,61 @@ void BundleAdjuster::AddPointToCameraConstraints( if (images.find(observation.first) == images.end()) continue; Image& image = images[observation.first]; + 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 = - colmap::CameraCostFunction( - cameras[image.camera_id].model_id, - image.features[observation.second]); - - if (cost_function != nullptr) { + 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, - options_.loss_function.get(), - image.cam_from_world.rotation.coeffs().data(), - image.cam_from_world.translation.data(), + loss_function_.get(), + 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( @@ -92,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; @@ -108,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); + } + } } } @@ -126,41 +242,35 @@ 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()); + + counter++; } } - // Set the first camera to be fixed to remove the gauge ambiguity. - problem_->SetParameterBlockConstant( - images[center].cam_from_world.rotation.coeffs().data()); - problem_->SetParameterBlockConstant( - images[center].cam_from_world.translation.data()); - // Parameterize the camera parameters, or set them to be constant if desired - if (options_.optimize_intrinsics) { + if (options_.optimize_intrinsics && !options_.optimize_principal_point) { for (auto& [camera_id, camera] : cameras) { if (problem_->HasParameterBlock(camera.params.data())) { std::vector principal_point_idxs; @@ -173,8 +283,8 @@ void BundleAdjuster::ParameterizeVariables( camera.params.data()); } } - - } else { + } else if (!options_.optimize_intrinsics && + !options_.optimize_principal_point) { for (auto& [camera_id, camera] : cameras) { if (problem_->HasParameterBlock(camera.params.data())) { problem_->SetParameterBlockConstant(camera.params.data()); @@ -182,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 97419a5d..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,21 +12,29 @@ 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; + bool optimize_principal_point = false; bool optimize_points = true; + bool use_gpu = true; + std::string gpu_index = "-1"; + int min_num_images_gpu_solver = 50; + // Constrain the minimum number of views per track int min_num_view_per_track = 3; BundleAdjusterOptions() : OptimizationBaseOptions() { thres_loss_function = 1.; - loss_function = std::make_shared(thres_loss_function); solver_options.max_num_iterations = 200; } -}; + std::shared_ptr CreateLossFunction() { + return std::make_shared(thres_loss_function); + } +}; class BundleAdjuster { public: BundleAdjuster(const BundleAdjusterOptions& options) : options_(options) {} @@ -33,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); @@ -46,25 +56,29 @@ 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_; std::unique_ptr problem_; + std::shared_ptr loss_function_; }; } // namespace glomap diff --git a/glomap/estimators/cost_function.h b/glomap/estimators/cost_function.h index d0a8ce48..ed21efdf 100644 --- a/glomap/estimators/cost_function.h +++ b/glomap/estimators/cost_function.h @@ -14,7 +14,7 @@ namespace glomap { // from two positions such that t_ij - scale * (c_j - c_i) is minimized. struct BATAPairwiseDirectionError { BATAPairwiseDirectionError(const Eigen::Vector3d& translation_obs) - : translation_obs_(translation_obs){}; + : translation_obs_(translation_obs) {} // The error is given by the position error described above. template @@ -22,30 +22,117 @@ struct BATAPairwiseDirectionError { const T* position2, const T* scale, T* residuals) const { - Eigen::Matrix translation; - translation[0] = position2[0] - position1[0]; - translation[1] = position2[1] - position1[1]; - translation[2] = position2[2] - position1[2]; + Eigen::Map> residuals_vec(residuals); + residuals_vec = + translation_obs_.cast() - + scale[0] * (Eigen::Map>(position2) - + Eigen::Map>(position1)); + return true; + } - Eigen::Matrix residual_vec; + static ceres::CostFunction* Create(const Eigen::Vector3d& translation_obs) { + return ( + new ceres::AutoDiffCostFunction( + new BATAPairwiseDirectionError(translation_obs))); + } - residual_vec = translation_obs_.cast() - scale[0] * translation; + // TODO: add covariance + const Eigen::Vector3d translation_obs_; +}; - residuals[0] = residual_vec(0); - residuals[1] = residual_vec(1); - residuals[2] = residual_vec(2); +// ---------------------------------------- +// 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) { + static ceres::CostFunction* Create(const Eigen::Vector3d& translation_obs, + const Eigen::Vector3d& translation_rig) { return ( - new ceres::AutoDiffCostFunction( - new BATAPairwiseDirectionError(translation_obs))); + 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 }; // ---------------------------------------- diff --git a/glomap/estimators/global_positioning.cc b/glomap/estimators/global_positioning.cc index 9c566f6c..f5f96a99 100644 --- a/glomap/estimators/global_positioning.cc +++ b/glomap/estimators/global_positioning.cc @@ -2,9 +2,11 @@ #include "glomap/estimators/cost_function.h" #include "glomap/io/recording.h" -#include #include "glomap/io/colmap_io.h" +#include "glomap/math/rigid3d.h" +#include +#include #include namespace glomap { @@ -24,41 +26,46 @@ Eigen::Vector3d RandVector3d(std::mt19937& random_generator, class LoggingCallback : public ceres::IterationCallback { -public: - std::unordered_map& tracks; +public: + std::unordered_map& rigs; std::unordered_map& cameras; + std::unordered_map& frames; std::unordered_map& images; + std::unordered_map& tracks; std::string image_path; - LoggingCallback(std::unordered_map& tracks, + LoggingCallback(std::unordered_map& rigs, std::unordered_map& cameras, - std::unordered_map& images, + std::unordered_map& frames, + std::unordered_map& images, + std::unordered_map& tracks, std::string image_path - ) : tracks {tracks}, cameras {cameras}, images {images}, image_path {image_path} {} + ) : rigs{rigs}, cameras{cameras}, frames{frames}, images{images}, tracks{tracks}, image_path{image_path} {} ~LoggingCallback() {} ceres::CallbackReturnType operator()(const ceres::IterationSummary& summary) { std::unordered_map images_copy = images; for (auto& [image_id, image] : images_copy) { - image.cam_from_world.translation = - -(image.cam_from_world.rotation * image.cam_from_world.translation); + if (!image.IsRegistered()) continue; + image.CamFromWorld().translation = + -(image.CamFromWorld().rotation * image.CamFromWorld().translation); } rr_rec.set_time_sequence("step", algorithm_step++); if (summary.iteration == 0) { // This is a bit of a hack to extract the colors for the point cloud to make the visualization a bit prettier. - + std::unordered_map tmp_rigs; std::unordered_map tmp_cameras; + std::unordered_map tmp_frames; std::unordered_map tmp_images; std::unordered_map tmp_tracks; colmap::Reconstruction reconstruction; - // ConvertDatabaseToColmap(re) - ConvertGlomapToColmap(cameras, images_copy, tracks, reconstruction); + ConvertGlomapToColmap(rigs, cameras, frames, images_copy, tracks, reconstruction); reconstruction.ExtractColorsForAllImages(image_path); - ConvertColmapToGlomap(reconstruction, tmp_cameras, tmp_images, tmp_tracks); + ConvertColmapToGlomap(reconstruction, tmp_rigs, tmp_cameras, tmp_frames, tmp_images, tmp_tracks); for (auto &[track_id, track] : tmp_tracks) { tracks[track_id].color = track.color; } @@ -79,9 +86,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; @@ -99,141 +111,174 @@ bool GlobalPositioner::Solve(const ViewGraph& view_graph, LOG(INFO) << "Setting up the global positioner problem"; - // Initialize the problem - Reset(); + // Setup the problem. + SetupProblem(view_graph, 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"; - LoggingCallback callback {tracks, cameras, images, image_path_global}; + LoggingCallback callback {rigs, cameras, frames, images, tracks, image_path_global}; ceres::Solver::Summary summary; - options_.solver_options.minimizer_progress_to_stdout = options_.verbose; + options_.solver_options.minimizer_progress_to_stdout = VLOG_IS_ON(2); auto solver_options = options_.solver_options; solver_options.callbacks.push_back(&callback); solver_options.update_state_every_iteration = true; - + ceres::Solve(solver_options, problem_.get(), &summary); - if (options_.verbose) { + if (VLOG_IS_ON(2)) { LOG(INFO) << summary.FullReport(); } else { LOG(INFO) << summary.BriefReport(); } - ConvertResults(images); + ConvertResults(rigs, frames); return summary.IsSolutionUsable(); } -void GlobalPositioner::Reset() { +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; problem_ = std::make_unique(problem_options); + loss_function_ = options_.CreateLossFunction(); + + // Allocate enough memory for the scales. One for each residual. + // Due to possibly invalid image pairs or tracks, the actual number of + // residuals may be smaller. scales_.clear(); + scales_.reserve( + view_graph.image_pairs.size() + + std::accumulate(tracks.begin(), + tracks.end(), + 0, + [](int sum, const std::pair& track) { + return sum + track.second.observations.size(); + })); + + // 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()); } - if (options_.verbose) - LOG(INFO) << "Constrained positions: " << constrained_positions.size(); + VLOG(2) << "Constrained positions: " << constrained_positions.size(); } 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; const image_t image_id1 = image_pair.image_id1; const image_t image_id2 = image_pair.image_id2; if (images.find(image_id1) == images.end() || - images.find(image_id2) == images.end()) + images.find(image_id2) == images.end()) { continue; + } - track_t counter = scales_.size(); - scales_.insert(std::make_pair(counter, 1)); + CHECK_GE(scales_.capacity(), scales_.size()) + << "Not enough capacity was reserved for the scales."; + double& scale = scales_.emplace_back(1); - Eigen::Vector3d translation = - -(images[image_id2].cam_from_world.rotation.inverse() * + const Eigen::Vector3d translation = + -(images[image_id2].CamFromWorld().rotation.inverse() * image_pair.cam2_from_cam1.translation); ceres::CostFunction* cost_function = BATAPairwiseDirectionError::Create(translation); problem_->AddResidualBlock( cost_function, - options_.loss_function.get(), - images[image_id1].cam_from_world.translation.data(), - images[image_id2].cam_from_world.translation.data(), - &(scales_[counter])); + loss_function_.get(), + images[image_id1].frame_ptr->RigFromWorld().translation.data(), + images[image_id2].frame_ptr->RigFromWorld().translation.data(), + &scale); - problem_->SetParameterLowerBound(&(scales_[counter]), 0, 1e-5); + problem_->SetParameterLowerBound(&scale, 0, 1e-5); } - if (options_.verbose) - LOG(INFO) << problem_->NumResidualBlocks() - << " camera to camera constraints were added to the position " - "estimation problem."; + VLOG(2) << problem_->NumResidualBlocks() + << " camera to camera constraints were added to the position " + "estimation problem."; } void GlobalPositioner::AddPointToCameraConstraints( + 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 @@ -242,10 +287,9 @@ void GlobalPositioner::AddPointToCameraConstraints( // Find the tracks that are relevant to the current set of cameras const size_t num_pt_to_cam = tracks.size(); - if (options_.verbose) - LOG(INFO) << num_pt_to_cam - << " point to camera constriants were added to the position " - "estimation problem."; + VLOG(2) << num_pt_to_cam + << " point to camera constriants were added to the position " + "estimation problem."; if (num_pt_to_cam == 0) return; @@ -259,24 +303,21 @@ void GlobalPositioner::AddPointToCameraConstraints( static_cast(num_cam_to_cam) / static_cast(num_pt_to_cam); } - if (options_.verbose) - LOG(INFO) << "Point to camera weight scaled: " << weight_scale_pt; + VLOG(2) << "Point to camera weight scaled: " << weight_scale_pt; if (loss_function_ptcam_uncalibrated_ == nullptr) { loss_function_ptcam_uncalibrated_ = - std::make_shared(options_.loss_function.get(), + std::make_shared(loss_function_.get(), 0.5 * weight_scale_pt, ceres::DO_NOT_TAKE_OWNERSHIP); } if (options_.constraint_type == GlobalPositionerOptions::POINTS_AND_CAMERAS_BALANCED) { - loss_function_ptcam_calibrated_ = - std::make_shared(options_.loss_function.get(), - weight_scale_pt, - ceres::DO_NOT_TAKE_OWNERSHIP); + loss_function_ptcam_calibrated_ = std::make_shared( + loss_function_.get(), weight_scale_pt, ceres::DO_NOT_TAKE_OWNERSHIP); } else { - loss_function_ptcam_calibrated_ = options_.loss_function; + loss_function_ptcam_calibrated_ = loss_function_; } for (auto& [track_id, track] : tracks) { @@ -288,13 +329,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( - const track_t& track_id, + 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. @@ -302,43 +345,105 @@ 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]; + if (feature_undist.array().isNaN().any()) { + LOG(WARNING) + << "Ignoring feature because it failed to undistort: track_id=" + << track_id << ", image_id=" << observation.first + << ", feature_id=" << observation.second; + continue; + } - Eigen::Vector3d translation = image.cam_from_world.rotation.inverse() * - image.features_undist[observation.second]; - ceres::CostFunction* cost_function = - BATAPairwiseDirectionError::Create(translation); + const Eigen::Vector3d translation = + image.CamFromWorld().rotation.inverse() * + image.features_undist[observation.second]; - track_t counter = scales_.size(); - if (options_.generate_scales || !tracks[track_id].is_initialized) { - scales_.insert(std::make_pair(counter, 1)); - } else { - Eigen::Vector3d trans_calc = - tracks[track_id].xyz - image.cam_from_world.translation; - double scale = translation.dot(trans_calc) / trans_calc.squaredNorm(); - scales_.insert(std::make_pair(counter, std::max(scale, 1e-5))); + 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.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 - (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(), - &(scales_[counter])) - : problem_->AddResidualBlock(cost_function, - loss_function_ptcam_uncalibrated_.get(), - image.cam_from_world.translation.data(), - tracks[track_id].xyz.data(), - &(scales_[counter])); - - problem_->SetParameterLowerBound(&(scales_[counter]), 0, 1e-5); + 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 { + 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); } } 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( @@ -347,8 +452,8 @@ void GlobalPositioner::AddCamerasAndPointsToParameterGroups( options_.solver_options.linear_solver_ordering.get(); // Add scale parameters to group 0 (large and independent) - for (auto& [i, scale] : scales_) { - parameter_ordering->AddElementToGroup(&(scales_[i]), 0); + for (double& scale : scales_) { + parameter_ordering->AddElementToGroup(&scale, 0); } // Add point parameters to group 1. @@ -361,27 +466,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 @@ -395,10 +540,79 @@ void GlobalPositioner::ParameterizeVariables( // If do not optimize the scales, set the scales to be constant if (!options_.optimize_scales) { - for (auto& [i, scale] : scales_) { - problem_->SetParameterBlockConstant(&(scales_[i])); + for (double& scale : scales_) { + 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 = frames.size(); +#ifdef GLOMAP_CUDA_ENABLED + bool cuda_solver_enabled = false; + +#if (CERES_VERSION_MAJOR >= 3 || \ + (CERES_VERSION_MAJOR == 2 && CERES_VERSION_MINOR >= 2)) && \ + !defined(CERES_NO_CUDA) + if (options_.use_gpu && num_images >= options_.min_num_images_gpu_solver) { + cuda_solver_enabled = true; + options_.solver_options.dense_linear_algebra_library_type = ceres::CUDA; + } +#else + if (options_.use_gpu) { + LOG_FIRST_N(WARNING, 1) + << "Requested to use GPU for bundle adjustment, but Ceres was " + "compiled without CUDA support. Falling back to CPU-based dense " + "solvers."; + } +#endif + +#if (CERES_VERSION_MAJOR >= 3 || \ + (CERES_VERSION_MAJOR == 2 && CERES_VERSION_MINOR >= 3)) && \ + !defined(CERES_NO_CUDSS) + if (options_.use_gpu && num_images >= options_.min_num_images_gpu_solver) { + cuda_solver_enabled = true; + options_.solver_options.sparse_linear_algebra_library_type = + ceres::CUDA_SPARSE; + } +#else + if (options_.use_gpu) { + LOG_FIRST_N(WARNING, 1) + << "Requested to use GPU for bundle adjustment, but Ceres was " + "compiled without cuDSS support. Falling back to CPU-based sparse " + "solvers."; + } +#endif + + if (cuda_solver_enabled) { + const std::vector gpu_indices = + colmap::CSVToVector(options_.gpu_index); + THROW_CHECK_GT(gpu_indices.size(), 0); + colmap::SetBestCudaDevice(gpu_indices[0]); + } +#else + if (options_.use_gpu) { + LOG_FIRST_N(WARNING, 1) + << "Requested to use GPU for bundle adjustment, but COLMAP was " + "compiled without CUDA support. Falling back to CPU-based " + "solvers."; + } +#endif // GLOMAP_CUDA_ENABLED // Set up the options for the solver // Do not use iterative solvers, for its suboptimal performance. @@ -412,13 +626,33 @@ 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]; + } + } + } } } -} // namespace glomap \ No newline at end of file +} // namespace glomap diff --git a/glomap/estimators/global_positioning.h b/glomap/estimators/global_positioning.h index 24260dc2..f5f508d3 100644 --- a/glomap/estimators/global_positioning.h +++ b/glomap/estimators/global_positioning.h @@ -29,6 +29,10 @@ struct GlobalPositionerOptions : public OptimizationBaseOptions { bool optimize_points = true; bool optimize_scales = true; + bool use_gpu = true; + std::string gpu_index = "-1"; + int min_num_images_gpu_solver = 50; + // Constrain the minimum number of views per track int min_num_view_per_track = 3; @@ -42,7 +46,10 @@ struct GlobalPositionerOptions : public OptimizationBaseOptions { GlobalPositionerOptions() : OptimizationBaseOptions() { thres_loss_function = 1e-1; - loss_function = std::make_shared(thres_loss_function); + } + + std::shared_ptr CreateLossFunction() { + return std::make_shared(thres_loss_function); } }; @@ -54,18 +61,22 @@ 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); GlobalPositionerOptions& GetOptions() { return options_; } protected: - // Reset the problem - void Reset(); + 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); @@ -75,40 +86,50 @@ 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(const track_t& track_id, + 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); - // Data members GlobalPositionerOptions options_; std::mt19937 random_generator_; std::unique_ptr problem_; - // loss functions for reweighted terms + // Loss functions for reweighted terms. + std::shared_ptr loss_function_; std::shared_ptr loss_function_ptcam_uncalibrated_; std::shared_ptr loss_function_ptcam_calibrated_; - std::unordered_map scales_; + // 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 b3edaad8..0586f4bc 100644 --- a/glomap/estimators/global_rotation_averaging.cc +++ b/glomap/estimators/global_rotation_averaging.cc @@ -1,68 +1,94 @@ #include "global_rotation_averaging.h" -#include "glomap/math/l1_solver.h" +#include "glomap/estimators/rotation_initializer.h" #include "glomap/math/rigid3d.h" #include "glomap/math/tree.h" +#include +#include + #include #include +#include +#include + namespace glomap { namespace { + double RelAngleError(double angle_12, double angle_1, double angle_2) { double est = (angle_2 - angle_1) - angle_12; - while (est >= M_PI) est -= TWO_PI; + while (est >= EIGEN_PI) est -= TWO_PI; - while (est < -M_PI) est += TWO_PI; + while (est < -EIGEN_PI) est += TWO_PI; + + // Inject random noise if the angle is too close to the boundary to break the + // possible balance at the local minima + if (est > EIGEN_PI - 0.01 || est < -EIGEN_PI + 0.01) { + if (est < 0) + est += (rand() % 1000) / 1000.0 * 0.01; + else + est -= (rand() % 1000) / 1000.0 * 0.01; + } return est; } + } // namespace 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))); - } - } + 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; @@ -72,7 +98,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) { @@ -83,6 +109,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(); @@ -94,72 +121,141 @@ void RotationEstimator::InitializeFromMaximumSpanningTree( // Directly use the relative pose for estimation rotation const ImagePair& image_pair = view_graph.image_pairs.at( - ImagePair::ImagePairToPairId(curr, parents[curr])); + colmap::ImagePairToPairId(curr, parents[curr])); if (image_pair.image_id1 == curr) { // 1_R_w = 2_R_1^T * 2_R_w - 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; } } - if (options_.verbose) - LOG(INFO) << "num_img: " << image_id_to_idx_.size() - << ", num_dof: " << num_dof; - rotation_estimated_.conservativeResize(num_dof); // Prepare the relative information @@ -167,29 +263,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]; @@ -204,33 +340,61 @@ void RotationEstimator::SetupLinearSystem( } } - if (options_.verbose) - LOG(INFO) << counter << " image pairs are gravity aligned" << std::endl; + VLOG(2) << counter << " image pairs are gravity aligned" << std::endl; std::vector> coeffs; coeffs.reserve(rel_temp_info_.size() * 6 + 3); // Establish linear systems size_t curr_pos = 0; + std::vector weights; + weights.reserve(3 * view_graph.image_pairs.size()); for (const auto& [pair_id, image_pair] : view_graph.image_pairs) { if (!image_pair.is_valid) continue; + 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)); + if (image_pair.weight >= 0) + weights.emplace_back(image_pair.weight); + else + weights.emplace_back(1); curr_pos++; } else { // If it is not gravity aligned, then we need to consider 3 dof - 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)); @@ -241,8 +405,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)); @@ -250,6 +413,31 @@ void RotationEstimator::SetupLinearSystem( } else coeffs.emplace_back( Eigen::Triplet(curr_pos + 1, vector_idx2, 1)); + for (int i = 0; i < 3; i++) { + if (image_pair.weight >= 0) + weights.emplace_back(image_pair.weight); + else + weights.emplace_back(1); + } + + // 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; } @@ -258,15 +446,16 @@ 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); curr_pos++; } else { for (int i = 0; i < 3; i++) { coeffs.emplace_back(Eigen::Triplet( curr_pos + i, image_id_to_idx_[fixed_camera_id_] + i, 1)); + weights.emplace_back(1); } curr_pos += 3; } @@ -274,40 +463,55 @@ void RotationEstimator::SetupLinearSystem( sparse_matrix_.resize(curr_pos, num_dof); sparse_matrix_.setFromTriplets(coeffs.begin(), coeffs.end()); + // Set up the weight matrix for the linear system + if (!options_.use_weight) { + weights_ = Eigen::ArrayXd::Ones(curr_pos); + } else { + weights_ = Eigen::ArrayXd(weights.size()); + for (size_t i = 0; i < weights.size(); i++) weights_[i] = weights[i]; + } + // Initialize x and b tangent_space_step_.resize(num_dof); tangent_space_residual_.resize(curr_pos); } bool RotationEstimator::SolveL1Regression( - const ViewGraph& view_graph, std::unordered_map& images) { - L1SolverOptions opt_l1_solver; - opt_l1_solver.max_num_iterations = 10; - - L1Solver> l1_solver(opt_l1_solver, - sparse_matrix_); + const ViewGraph& view_graph, + std::unordered_map& frames, + std::unordered_map& images) { + colmap::LeastAbsoluteDeviationSolver::Options l1_solver_options; + l1_solver_options.max_num_iterations = 10; + l1_solver_options.solver_type = colmap::LeastAbsoluteDeviationSolver:: + Options::SolverType::SupernodalCholmodLLT; + + const Eigen::SparseMatrix A = + weights_.matrix().asDiagonal() * sparse_matrix_; + + colmap::LeastAbsoluteDeviationSolver l1_solver(l1_solver_options, A); double last_norm = 0; double curr_norm = 0; ComputeResiduals(view_graph, images); - if (options_.verbose) LOG(INFO) << "ComputeResiduals done "; + VLOG(2) << "ComputeResiduals done"; int iteration = 0; for (iteration = 0; iteration < options_.max_num_l1_iterations; iteration++) { - if (options_.verbose) LOG(INFO) << "L1 ADMM iteration: " << iteration; + VLOG(2) << "L1 ADMM iteration: " << iteration; last_norm = curr_norm; // use the current residual as b (Ax - b) tangent_space_step_.setZero(); - l1_solver.Solve(tangent_space_residual_, &tangent_space_step_); - if (tangent_space_step_.array().isNaN().sum() > 0) { + l1_solver.Solve(weights_.matrix().asDiagonal() * tangent_space_residual_, + &tangent_space_step_); + if (tangent_space_step_.array().isNaN().any()) { LOG(ERROR) << "nan error"; iteration++; return false; } - if (options_.verbose) + if (VLOG_IS_ON(2)) LOG(INFO) << "residual:" << (sparse_matrix_ * tangent_space_step_ - tangent_space_residual_) @@ -316,12 +520,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) @@ -329,31 +533,28 @@ bool RotationEstimator::SolveL1Regression( iteration++; break; } - opt_l1_solver.max_num_iterations = - std::min(opt_l1_solver.max_num_iterations * 2, 100); + l1_solver_options.max_num_iterations = + std::min(l1_solver_options.max_num_iterations * 2, 100); } - if (options_.verbose) LOG(INFO) << "L1 ADMM total iteration: " << iteration; + VLOG(2) << "L1 ADMM total iteration: " << iteration; return true; } 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); - if (options_.verbose) - LOG(INFO) << "sigma: " << options_.irls_loss_parameter_sigma; + VLOG(2) << "sigma: " << options_.irls_loss_parameter_sigma; Eigen::ArrayXd weights_irls(sparse_matrix_.rows()); Eigen::SparseMatrix at_weight; - 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); @@ -362,7 +563,7 @@ bool RotationEstimator::SolveIRLS(const ViewGraph& view_graph, int iteration = 0; for (iteration = 0; iteration < options_.max_num_irls_iterations; iteration++) { - if (options_.verbose) LOG(INFO) << "IRLS iteration: " << iteration; + VLOG(2) << "IRLS iteration: " << iteration; // Compute the weights for IRLS for (auto& [pair_id, pair_info] : rel_temp_info_) { @@ -399,35 +600,38 @@ bool RotationEstimator::SolveIRLS(const ViewGraph& view_graph, } // Update the factorization for the weighted values. - at_weight = sparse_matrix_.transpose() * weights_irls.matrix().asDiagonal(); + at_weight = sparse_matrix_.transpose() * + weights_irls.matrix().asDiagonal() * + weights_.matrix().asDiagonal(); llt.factorize(at_weight * sparse_matrix_); // 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; } } - if (options_.verbose) LOG(INFO) << "IRLS total iteration: " << iteration; + VLOG(2) << "IRLS total iteration: " << iteration; return true; } 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)); @@ -438,6 +642,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( @@ -447,8 +700,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] = @@ -457,26 +713,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]; @@ -489,19 +756,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 7b73d344..e3d5057c 100644 --- a/glomap/estimators/global_rotation_averaging.h +++ b/glomap/estimators/global_rotation_averaging.h @@ -1,12 +1,13 @@ #pragma once -#include "glomap/math/l1_solver.h" #include "glomap/scene/types_sfm.h" #include "glomap/types.h" #include #include +#include + // Code is adapted from Theia's RobustRotationEstimator // (http://www.theia-sfm.org/). For gravity aligned rotation averaging, refere // to the paper "Gravity Aligned Rotation Averaging" @@ -30,6 +31,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 { @@ -63,20 +67,13 @@ struct RotationEstimatorOptions { // Flg to use maximum spanning tree for initialization bool skip_initialization = false; - // TODO: Implement the weighted version for rotation averaging + // Flag to use weighting for rotation averaging bool use_weight = false; // Flag to use gravity for rotation averaging bool use_gravity = false; - - // Flag whether report the verbose information - bool verbose = 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) @@ -85,30 +82,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 @@ -120,7 +127,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. @@ -139,7 +152,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 @@ -148,6 +164,9 @@ class RotationEstimator { // The fixed camera rotation (if with initialization, it would not be identity // matrix) Eigen::Vector3d fixed_camera_rotation_; + + // The weights for the edges + Eigen::ArrayXd weights_; }; } // namespace glomap diff --git a/glomap/estimators/gravity_refinement.cc b/glomap/estimators/gravity_refinement.cc index c4b2e2d0..29f7adb6 100644 --- a/glomap/estimators/gravity_refinement.cc +++ b/glomap/estimators/gravity_refinement.cc @@ -7,32 +7,47 @@ namespace glomap { void GravityRefiner::RefineGravity(const ViewGraph& view_graph, + std::unordered_map& frames, std::unordered_map& images) { - const std::unordered_map& image_pairs = - view_graph.image_pairs; const std::unordered_map>& - adjacency_list = view_graph.GetAdjacencyList(); + adjacency_list = view_graph.CreateImageAdjacencyList(); + if (adjacency_list.empty()) { + LOG(INFO) << "Adjacency list not established"; + return; + } // 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"; 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 (const auto& neighbor : neighbors) { + adjacency_list_frames_to_pair_id[images[image_id].frame_id].insert( + colmap::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()); @@ -40,40 +55,55 @@ 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); - - 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) + Eigen::Vector3d gravity = frames[frame_id].gravity_info.GetGravity(); + for (const auto& pair_id : neighbors) { + const image_t image_id1 = view_graph.image_pairs.at(pair_id).image_id1; + const image_t image_id2 = view_graph.image_pairs.at(pair_id).image_id2; + if (!images.at(image_id1).HasGravity() || + !images.at(image_id2).HasGravity()) continue; - 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( + (colmap::Inverse(view_graph.image_pairs.at(pair_id).cam2_from_cam1 * + cam1_from_rig1) + .rotation.toRotationMatrix() * + images[image_id2].GetRAlign()) + .col(1)); + } else if (images.at(image_id2).frame_id == frame_id) { gravities.emplace_back( - (image_pairs.at(pair_id) - .cam2_from_cam1.rotation.toRotationMatrix() * - images[image_id1].gravity_info.GetRAlign()) + ((colmap::Inverse(cam2_from_rig2) * + view_graph.image_pairs.at(pair_id).cam2_from_cam1) + .rotation.toRotationMatrix() * + images[image_id1].GetRAlign()) .col(1)); } ceres::CostFunction* coor_cost = GravError::CreateCost(gravities[counter]); - problem.AddResidualBlock( - coor_cost, options_.loss_function.get(), gravity.data()); + problem.AddResidualBlock(coor_cost, loss_function_.get(), gravity.data()); counter++; } if (gravities.size() < options_.min_num_neighbors) continue; // Then, run refinment + gravity = AverageGravity(gravities); colmap::SetSphereManifold<3>(&problem, gravity.data()); ceres::Solver::Summary summary_solver; ceres::Solve(options_.solver_options, &problem, &summary_solver); @@ -89,67 +119,64 @@ 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(); } 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 - Eigen::Matrix3d R_rel = - image2.gravity_info.GetRAlign().transpose() * + const Eigen::Matrix3d R_rel = + image2.GetRAlign().transpose() * image_pair.cam2_from_cam1.rotation.toRotationMatrix() * - image1.gravity_info.GetRAlign(); + image1.GetRAlign(); // Convert it to the closest upright rotation - Eigen::Matrix3d R_rel_up = AngleToRotUp(RotUpToAngle(R_rel)); + const Eigen::Matrix3d R_rel_up = AngleToRotUp(RotUpToAngle(R_rel)); - double angle = CalcAngle(R_rel, R_rel_up); + const double angle = CalcAngle(R_rel, R_rel_up); // increment the total count - image_counter[image_pair.image_id1].second++; - 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() - << std::endl; + LOG(INFO) << "Number of error prone frames: " << error_prone_frames.size(); } } // namespace glomap diff --git a/glomap/estimators/gravity_refinement.h b/glomap/estimators/gravity_refinement.h index 40a3b37f..d05a7cd8 100644 --- a/glomap/estimators/gravity_refinement.h +++ b/glomap/estimators/gravity_refinement.h @@ -13,12 +13,14 @@ struct GravityRefinerOptions : public OptimizationBaseOptions { // The minimal ratio that the gravity vector should be consistent with double max_outlier_ratio = 0.5; // The maximum allowed angle error in degree - bool max_gravity_error = 1.; + double max_gravity_error = 1.; // Only refine the gravity of the images with more than min_neighbors int min_num_neighbors = 7; - GravityRefinerOptions() : OptimizationBaseOptions() { - loss_function = std::make_shared( + GravityRefinerOptions() : OptimizationBaseOptions() {} + + std::shared_ptr CreateLossFunction() { + return std::make_shared( 1 - std::cos(DegToRad(max_gravity_error))); } }; @@ -27,14 +29,17 @@ 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_; + std::shared_ptr loss_function_; }; } // namespace glomap diff --git a/glomap/estimators/optimization_base.h b/glomap/estimators/optimization_base.h index 6234ca26..c6a67abb 100644 --- a/glomap/estimators/optimization_base.h +++ b/glomap/estimators/optimization_base.h @@ -9,22 +9,16 @@ namespace glomap { struct OptimizationBaseOptions { - // Logging control - bool verbose = false; - // The threshold for the loss function double thres_loss_function = 1e-1; - // The loss function for the calibration - std::shared_ptr loss_function; - // The options for the solver ceres::Solver::Options solver_options; OptimizationBaseOptions() { solver_options.num_threads = std::thread::hardware_concurrency(); solver_options.max_num_iterations = 100; - solver_options.minimizer_progress_to_stdout = verbose; + solver_options.minimizer_progress_to_stdout = false; solver_options.function_tolerance = 1e-5; } }; diff --git a/glomap/estimators/relpose_estimation.cc b/glomap/estimators/relpose_estimation.cc index 1b9b9626..0a8b5fc0 100644 --- a/glomap/estimators/relpose_estimation.cc +++ b/glomap/estimators/relpose_estimation.cc @@ -1,5 +1,7 @@ #include "glomap/estimators/relpose_estimation.h" +#include + #include namespace glomap { @@ -14,55 +16,106 @@ void EstimateRelativePoses(ViewGraph& view_graph, valid_pair_ids.push_back(image_pair_id); } - // Define outside loop to reuse memory and avoid reallocation. - std::vector points2D_1, points2D_2; - std::vector inliers; + const int64_t num_image_pairs = valid_pair_ids.size(); + const int64_t kNumChunks = 10; + const int64_t interval = + std::ceil(static_cast(num_image_pairs) / kNumChunks); + + colmap::ThreadPool thread_pool(colmap::ThreadPool::kMaxNumThreads); - const size_t kNumChunks = 10; - size_t inverval = std::ceil(valid_pair_ids.size() / kNumChunks); - LOG(INFO) << "Estimating relative pose for " << valid_pair_ids.size() - << " pairs"; - for (size_t chunk_id = 0; chunk_id < kNumChunks; chunk_id++) { + LOG(INFO) << "Estimating relative pose for " << num_image_pairs << " pairs"; + for (int64_t chunk_id = 0; chunk_id < kNumChunks; chunk_id++) { std::cout << "\r Estimating relative pose: " << chunk_id * kNumChunks << "%" << std::flush; - const size_t start = chunk_id * inverval; - const size_t end = - std::min((chunk_id + 1) * inverval, valid_pair_ids.size()); - -#pragma omp parallel for schedule(dynamic) private( \ - points2D_1, points2D_2, inliers) - for (size_t pair_idx = start; pair_idx < end; pair_idx++) { - ImagePair& image_pair = view_graph.image_pairs[valid_pair_ids[pair_idx]]; - const Image& image1 = images[image_pair.image_id1]; - const Image& image2 = images[image_pair.image_id2]; - const Eigen::MatrixXi& matches = image_pair.matches; - - // Collect the original 2D points - points2D_1.clear(); - points2D_2.clear(); - for (size_t idx = 0; idx < matches.rows(); idx++) { - points2D_1.push_back(image1.features[matches(idx, 0)]); - points2D_2.push_back(image2.features[matches(idx, 1)]); - } - - inliers.clear(); - poselib::CameraPose pose_rel_calc; - poselib::estimate_relative_pose( - points2D_1, - points2D_2, - ColmapCameraToPoseLibCamera(cameras[image1.camera_id]), - ColmapCameraToPoseLibCamera(cameras[image2.camera_id]), - options.ransac_options, - options.bundle_options, - &pose_rel_calc, - &inliers); - // Convert the relative pose to the glomap format - for (int i = 0; i < 4; i++) { - image_pair.cam2_from_cam1.rotation.coeffs()[i] = - pose_rel_calc.q[(i + 1) % 4]; - } - image_pair.cam2_from_cam1.translation = pose_rel_calc.t; + const int64_t start = chunk_id * interval; + const int64_t end = + std::min((chunk_id + 1) * interval, num_image_pairs); + + for (int64_t pair_idx = start; pair_idx < end; pair_idx++) { + thread_pool.AddTask([&, pair_idx]() { + // Define as thread-local to reuse memory allocation in different tasks. + thread_local std::vector points2D_1; + thread_local std::vector points2D_2; + thread_local std::vector inliers; + + ImagePair& image_pair = + view_graph.image_pairs[valid_pair_ids[pair_idx]]; + const Image& image1 = images[image_pair.image_id1]; + const Image& image2 = images[image_pair.image_id2]; + const Eigen::MatrixXi& matches = image_pair.matches; + + const Camera& camera1 = cameras[image1.camera_id]; + const Camera& camera2 = cameras[image2.camera_id]; + poselib::Camera camera_poselib1 = ColmapCameraToPoseLibCamera(camera1); + poselib::Camera camera_poselib2 = ColmapCameraToPoseLibCamera(camera2); + bool valid_camera_model = + (camera_poselib1.model_id >= 0 && camera_poselib2.model_id >= 0); + + // Collect the original 2D points + points2D_1.clear(); + points2D_2.clear(); + for (size_t idx = 0; idx < matches.rows(); idx++) { + points2D_1.push_back(image1.features[matches(idx, 0)]); + points2D_2.push_back(image2.features[matches(idx, 1)]); + } + // If the camera model is not supported by poselib + if (!valid_camera_model) { + // Undistort points + // Note that here, we still rescale by the focal length (to avoid + // change the RANSAC threshold) + Eigen::Matrix2d K1_new = Eigen::Matrix2d::Zero(); + Eigen::Matrix2d K2_new = Eigen::Matrix2d::Zero(); + K1_new(0, 0) = camera1.FocalLengthX(); + K1_new(1, 1) = camera1.FocalLengthY(); + K2_new(0, 0) = camera2.FocalLengthX(); + K2_new(1, 1) = camera2.FocalLengthY(); + for (size_t idx = 0; idx < matches.rows(); idx++) { + points2D_1[idx] = K1_new * camera1.CamFromImg(points2D_1[idx]) + .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 + // length and zero principal point + camera_poselib1 = poselib::Camera( + "PINHOLE", + {camera1.FocalLengthX(), camera1.FocalLengthY(), 0., 0.}, + camera1.width, + camera1.height); + camera_poselib2 = poselib::Camera( + "PINHOLE", + {camera2.FocalLengthX(), camera2.FocalLengthY(), 0., 0.}, + camera2.width, + camera2.height); + } + inliers.clear(); + poselib::CameraPose pose_rel_calc; + try { + poselib::estimate_relative_pose(points2D_1, + points2D_2, + camera_poselib1, + camera_poselib2, + options.ransac_options, + options.bundle_options, + &pose_rel_calc, + &inliers); + } catch (const std::exception& e) { + LOG(ERROR) << "Error in relative pose estimation: " << e.what(); + image_pair.is_valid = false; + return; + } + + // Convert the relative pose to the glomap format + for (int i = 0; i < 4; i++) { + image_pair.cam2_from_cam1.rotation.coeffs()[i] = + pose_rel_calc.q[(i + 1) % 4]; + } + image_pair.cam2_from_cam1.translation = pose_rel_calc.t; + }); } + + thread_pool.Wait(); } std::cout << "\r Estimating relative pose: 100%" << std::endl; diff --git a/glomap/estimators/rotation_initializer.cc b/glomap/estimators/rotation_initializer.cc new file mode 100644 index 00000000..2fd6c5e1 --- /dev/null +++ b/glomap/estimators/rotation_initializer.cc @@ -0,0 +1,127 @@ +#include "glomap/estimators/rotation_initializer.h" + +#include + +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/estimators/view_graph_calibration.cc b/glomap/estimators/view_graph_calibration.cc index 79910554..a66b5a2c 100644 --- a/glomap/estimators/view_graph_calibration.cc +++ b/glomap/estimators/view_graph_calibration.cc @@ -36,13 +36,10 @@ bool ViewGraphCalibrator::Solve(ViewGraph& view_graph, // Solve the problem ceres::Solver::Summary summary; - options_.solver_options.minimizer_progress_to_stdout = options_.verbose; + options_.solver_options.minimizer_progress_to_stdout = VLOG_IS_ON(2); ceres::Solve(options_.solver_options, problem_.get(), &summary); - // Print the summary only if verbose - if (options_.verbose) { - LOG(INFO) << summary.FullReport(); - } + VLOG(2) << summary.FullReport(); // Convert the results back to the camera CopyBackResults(cameras); @@ -64,8 +61,7 @@ void ViewGraphCalibrator::Reset( ceres::Problem::Options problem_options; problem_options.loss_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; problem_ = std::make_unique(problem_options); - options_.loss_function = - std::make_shared(options_.thres_loss_function); + loss_function_ = options_.CreateLossFunction(); } void ViewGraphCalibrator::AddImagePairsToProblem( @@ -93,14 +89,14 @@ void ViewGraphCalibrator::AddImagePair( problem_->AddResidualBlock( FetzerFocalLengthSameCameraCost::Create( image_pair.F, cameras.at(camera_id1).PrincipalPoint()), - options_.loss_function.get(), + loss_function_.get(), &(focals_[camera_id1])); } else { problem_->AddResidualBlock( FetzerFocalLengthCost::Create(image_pair.F, cameras.at(camera_id1).PrincipalPoint(), cameras.at(camera_id2).PrincipalPoint()), - options_.loss_function.get(), + loss_function_.get(), &(focals_[camera_id1]), &(focals_[camera_id2])); } @@ -132,10 +128,9 @@ void ViewGraphCalibrator::CopyBackResults( // if the estimated parameter is too crazy, reject it if (focals_[camera_id] / camera.Focal() > options_.thres_higher_ratio || focals_[camera_id] / camera.Focal() < options_.thres_lower_ratio) { - if (options_.verbose) - LOG(INFO) << "NOT ACCEPTED: Camera " << camera_id - << " focal: " << focals_[camera_id] - << " original focal: " << camera.Focal(); + VLOG(2) << "Ignoring degenerate camera camera " << camera_id + << " focal: " << focals_[camera_id] + << " original focal: " << camera.Focal(); counter++; continue; @@ -147,9 +142,6 @@ void ViewGraphCalibrator::CopyBackResults( // Update the focal length for (const size_t idx : camera.FocalLengthIdxs()) { camera.params[idx] = focals_[camera_id]; - if (options_.verbose) - LOG(INFO) << "Camera " << idx << " focal: " << focals_[camera_id] - << std::endl; } } LOG(INFO) << counter << " cameras are rejected in view graph calibration"; @@ -187,7 +179,7 @@ size_t ViewGraphCalibrator::FilterImagePairs(ViewGraph& view_graph) const { } LOG(INFO) << "invalid / total number of two view geometry: " - << invalid_counter << " / " << counter; + << invalid_counter << " / " << (counter / 2); return invalid_counter; } diff --git a/glomap/estimators/view_graph_calibration.h b/glomap/estimators/view_graph_calibration.h index f7f5104e..c1cebd65 100644 --- a/glomap/estimators/view_graph_calibration.h +++ b/glomap/estimators/view_graph_calibration.h @@ -21,6 +21,10 @@ struct ViewGraphCalibratorOptions : public OptimizationBaseOptions { ViewGraphCalibratorOptions() : OptimizationBaseOptions() { thres_loss_function = 1e-2; } + + std::shared_ptr CreateLossFunction() { + return std::make_shared(thres_loss_function); + } }; class ViewGraphCalibrator { @@ -61,6 +65,7 @@ class ViewGraphCalibrator { ViewGraphCalibratorOptions options_; std::unique_ptr problem_; std::unordered_map focals_; + std::shared_ptr loss_function_; }; } // namespace glomap diff --git a/glomap/exe/global_mapper.cc b/glomap/exe/global_mapper.cc index 7761a99e..f851b79a 100644 --- a/glomap/exe/global_mapper.cc +++ b/glomap/exe/global_mapper.cc @@ -3,9 +3,11 @@ #include "glomap/controllers/option_manager.h" #include "colmap/sensor/bitmap.h" #include "glomap/io/colmap_io.h" +#include "glomap/io/pose_io.h" #include "glomap/io/recording.h" #include "glomap/types.h" +#include #include #include #include @@ -70,12 +72,19 @@ 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"; + return EXIT_FAILURE; + } log_images(rr_rec, images, image_path); @@ -86,14 +95,21 @@ int RunMapper(int argc, char** argv) { 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; @@ -129,10 +145,12 @@ 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; @@ -141,7 +159,7 @@ int RunMapperResume(int argc, char** argv) { image_path_global = image_path; reconstruction.Read(input_path); - ConvertColmapToGlomap(reconstruction, cameras, images, tracks); + ConvertColmapToGlomap(reconstruction, rigs, cameras, frames, images, tracks); log_images(rr_rec, images, image_path); @@ -150,17 +168,24 @@ int RunMapperResume(int argc, char** argv) { // 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; } -} // namespace glomap \ No newline at end of file +} // namespace glomap diff --git a/glomap/exe/rotation_averager.cc b/glomap/exe/rotation_averager.cc new file mode 100644 index 00000000..af8a7a3f --- /dev/null +++ b/glomap/exe/rotation_averager.cc @@ -0,0 +1,123 @@ +#include "glomap/controllers/rotation_averager.h" + +#include "glomap/controllers/option_manager.h" +#include "glomap/estimators/gravity_refinement.h" +#include "glomap/io/colmap_io.h" +#include "glomap/io/pose_io.h" +#include "glomap/types.h" + +#include +#include + +namespace glomap { +// ------------------------------------- +// Running Global Rotation Averager +// ------------------------------------- +int RunRotationAverager(int argc, char** argv) { + std::string relpose_path; + std::string output_path; + std::string gravity_path = ""; + std::string weight_path = ""; + + bool use_stratified = true; + bool refine_gravity = false; + bool use_weight = false; + + OptionManager options; + options.AddRequiredOption("relpose_path", &relpose_path); + options.AddRequiredOption("output_path", &output_path); + options.AddDefaultOption("gravity_path", &gravity_path); + options.AddDefaultOption("weight_path", &weight_path); + options.AddDefaultOption("use_stratified", &use_stratified); + options.AddDefaultOption("refine_gravity", &refine_gravity); + options.AddDefaultOption("use_weight", &use_weight); + options.AddGravityRefinerOptions(); + options.Parse(argc, argv); + + if (!colmap::ExistsFile(relpose_path)) { + LOG(ERROR) << "`relpose_path` is not a file"; + return EXIT_FAILURE; + } + + if (gravity_path != "" && !colmap::ExistsFile(gravity_path)) { + LOG(ERROR) << "`gravity_path` is not a file"; + return EXIT_FAILURE; + } + + if (weight_path != "" && !colmap::ExistsFile(weight_path)) { + LOG(ERROR) << "`weight_path` is not a file"; + return EXIT_FAILURE; + } + + if (use_weight && weight_path == "") { + LOG(ERROR) << "Weight path is required when use_weight is set to true"; + return EXIT_FAILURE; + } + + RotationAveragerOptions rotation_averager_options; + rotation_averager_options.skip_initialization = true; + rotation_averager_options.use_gravity = true; + + rotation_averager_options.use_stratified = use_stratified; + rotation_averager_options.use_weight = use_weight; + + // Load the database + ViewGraph view_graph; + std::unordered_map images; + + ReadRelPose(relpose_path, images, view_graph); + + 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(); + cameras[image.camera_id].camera_id = image.camera_id; + } + + 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); + } + + if (use_weight) { + ReadRelWeight(weight_path, images, view_graph); + } + + 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, frames, images); + } + + colmap::Timer run_timer; + run_timer.Start(); + if (!SolveRotationAveraging( + view_graph, rigs, frames, images, rotation_averager_options)) { + LOG(ERROR) << "Failed to solve global rotation averaging"; + return EXIT_FAILURE; + } + run_timer.Pause(); + LOG(INFO) << "Global rotation averaging done in " + << run_timer.ElapsedSeconds() << " seconds"; + + // Write out the estimated rotation + WriteGlobalRotation(output_path, images); + LOG(INFO) << "Global rotation averaging done" << std::endl; + + return EXIT_SUCCESS; +} + +} // namespace glomap \ No newline at end of file diff --git a/glomap/exe/rotation_averager.h b/glomap/exe/rotation_averager.h new file mode 100644 index 00000000..0786b817 --- /dev/null +++ b/glomap/exe/rotation_averager.h @@ -0,0 +1,10 @@ +#pragma once + +#include "glomap/estimators/global_rotation_averaging.h" + +namespace glomap { + +// Use default values for most of the settings from database +int RunRotationAverager(int argc, char** argv); + +} // namespace glomap \ No newline at end of file diff --git a/glomap/glomap.cc b/glomap/glomap.cc index d2a5230a..f9afc567 100644 --- a/glomap/glomap.cc +++ b/glomap/glomap.cc @@ -1,4 +1,5 @@ #include "glomap/exe/global_mapper.h" +#include "glomap/exe/rotation_averager.h" #include @@ -12,6 +13,12 @@ int ShowHelp( std::cout << "GLOMAP -- Global Structure-from-Motion" << std::endl << std::endl; +#ifdef GLOMAP_CUDA_ENABLED + std::cout << "This version was compiled with CUDA!" << std::endl << std::endl; +#else + std::cout << "This version was NOT compiled CUDA!" << std::endl << std::endl; +#endif + std::cout << "Usage:" << std::endl; std::cout << " glomap mapper --database_path DATABASE --output_path MODEL" << std::endl; @@ -33,10 +40,12 @@ int ShowHelp( int main(int argc, char** argv) { colmap::InitializeGlog(argv); + FLAGS_alsologtostderr = true; std::vector> commands; commands.emplace_back("mapper", &glomap::RunMapper); commands.emplace_back("mapper_resume", &glomap::RunMapperResume); + commands.emplace_back("rotation_averager", &glomap::RunRotationAverager); if (argc == 1) { return ShowHelp(commands); @@ -55,7 +64,7 @@ int main(int argc, char** argv) { } if (matched_command_func == nullptr) { std::cout << "Command " << command << " not recognized. " - << "To list the available commands, run `colmap help`." + << "To list the available commands, run `glomap help`." << std::endl; return EXIT_FAILURE; } else { diff --git a/glomap/io/colmap_converter.cc b/glomap/io/colmap_converter.cc index 6a838c22..4c7fd030 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 + namespace glomap { void ConvertGlomapToColmapImage(const Image& image, @@ -9,16 +11,17 @@ void ConvertGlomapToColmapImage(const Image& image, bool keep_points) { image_colmap.SetImageId(image.image_id); image_colmap.SetCameraId(image.camera_id); - image_colmap.SetRegistered(image.is_registered); image_colmap.SetName(image.file_name); - image_colmap.CamFromWorld() = image.cam_from_world; + 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, @@ -32,13 +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); @@ -46,7 +62,7 @@ void ConvertGlomapToColmap(const std::unordered_map& cameras, if (tracks.size() > 0) { for (auto& [track_id, track] : tracks) { - if (track.observations.size() < 3) { + if (track.observations.size() < min_supports) { continue; } for (auto& observation : track.observations) { @@ -69,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; @@ -79,7 +95,7 @@ void ConvertGlomapToColmap(const std::unordered_map& cameras, colmap_point.track.AddElement(colmap_track_el); } - if (colmap_point.track.Length() < 2) continue; + if (colmap_point.track.Length() < min_supports) continue; colmap_point.track.Compress(); reconstruction.AddPoint3D(track_id, std::move(colmap_point)); @@ -87,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(); @@ -98,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]); } } @@ -106,10 +118,22 @@ 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 @@ -121,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(), @@ -128,8 +166,10 @@ void ConvertColmapToGlomap(const colmap::Reconstruction& reconstruction, image_colmap.Name()))); Image& image = ite.first->second; - image.is_registered = image_colmap.IsRegistered(); - image.cam_from_world = static_cast(image_colmap.CamFromWorld()); + image.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()); @@ -168,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(); @@ -182,44 +224,125 @@ void ConvertDatabaseToGlomap(const colmap::Database& database, << images_colmap.size() << std::flush; counter++; - image_t image_id = image.ImageId(); + const image_t image_id = image.ImageId(); if (image_id == colmap::kInvalidImageId) continue; - auto ite = images.insert(std::make_pair( + images.insert(std::make_pair( image_id, Image(image_id, image.CameraId(), image.Name()))); - const colmap::PosePrior prior = database.ReadPosePrior(image_id); - if (prior.IsValid()) { - ite.first->second.cam_from_world = Rigid3d( - colmap::Rigid3d(Eigen::Quaterniond::Identity(), prior.position)); - } 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; // Read keypoints for (auto& [image_id, image] : images) { - colmap::FeatureKeypoints keypoints = database.ReadKeypoints(image_id); - - image.features.reserve(keypoints.size()); - for (int i = 0; i < keypoints.size(); i++) { - image.features.emplace_back( - Eigen::Vector2d(keypoints[i].x, keypoints[i].y)); + const colmap::FeatureKeypoints keypoints = database.ReadKeypoints(image_id); + const int num_keypoints = keypoints.size(); + image.features.resize(num_keypoints); + for (int i = 0; i < num_keypoints; i++) { + image.features[i] = Eigen::Vector2d(keypoints[i].x, keypoints[i].y); } } // Add the cameras std::vector cameras_colmap = database.ReadAllCameras(); for (auto& camera : cameras_colmap) { - camera_t camera_id = camera.camera_id; - cameras[camera_id] = camera; + cameras[camera.camera_id] = camera; + } + + // Add the 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; @@ -230,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; @@ -238,7 +361,7 @@ void ConvertDatabaseToGlomap(const colmap::Database& database, // Initialize the image pair auto ite = image_pairs.insert( - std::make_pair(ImagePair::ImagePairToPairId(image_id1, image_id2), + std::make_pair(colmap::ImagePairToPairId(image_id1, image_id2), ImagePair(image_id1, image_id2))); ImagePair& image_pair = ite.first->second; @@ -296,10 +419,42 @@ void ConvertDatabaseToGlomap(const colmap::Database& database, } image_pair.matches.conservativeResize(count, 2); } - std::cout << std::endl; - LOG(INFO) << "Pairs read done. " << invalid_count << " / " << view_graph.image_pairs.size() << " are invalid"; } +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()); + rigs[rig.RigId()] = rig; + } +} + +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 325a081f..3ea576b6 100644 --- a/glomap/io/colmap_io.cc +++ b/glomap/io/colmap_io.cc @@ -1,12 +1,16 @@ #include "glomap/io/colmap_io.h" -#include +#include "glomap/io/recording.h" + +#include #include 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, @@ -14,15 +18,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 ..."; @@ -30,10 +34,12 @@ void WriteGlomapReconstruction( } // Convert back to GLOMAP so that we can log the reconstruction with color. + std::unordered_map rigs_copy; std::unordered_map cameras_copy; + std::unordered_map frames_copy; std::unordered_map images_copy; std::unordered_map tracks_copy; - ConvertColmapToGlomap(reconstruction, cameras_copy, images_copy, tracks_copy); + ConvertColmapToGlomap(reconstruction, rigs_copy, cameras_copy, frames_copy, images_copy, tracks_copy); rr_rec.set_time_sequence("step", algorithm_step++); rr_rec.log("status", rerun::TextLog("Converted to Colmap and extracted colors")); log_reconstruction(rr_rec, cameras_copy, images_copy, tracks_copy); @@ -51,7 +57,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/gravity_io.cc b/glomap/io/gravity_io.cc deleted file mode 100644 index 7e974db2..00000000 --- a/glomap/io/gravity_io.cc +++ /dev/null @@ -1,45 +0,0 @@ -#include "gravity_io.h" - -#include - -namespace glomap { -void ReadGravity(const std::string& gravity_path, - std::unordered_map& images) { - std::unordered_map name_idx; - for (const auto& [image_id, image] : images) { - name_idx[image.file_name] = image_id; - } - - std::ifstream file(gravity_path); - - // Read in the file list - std::string line, item; - Eigen::Vector3d gravity; - int counter = 0; - while (std::getline(file, line)) { - std::stringstream line_stream(line); - - // file_name - std::string name; - std::getline(line_stream, name, ' '); - - // Gravity - for (double i = 0; i < 3; i++) { - std::getline(line_stream, item, ' '); - gravity[i] = std::stod(item); - } - - // Check whether the image present - auto ite = name_idx.find(name); - if (ite != name_idx.end()) { - counter++; - images[ite->second].gravity_info.SetGravity(gravity); - // Make sure the initialization is aligned with the gravity - images[ite->second].cam_from_world.rotation = - images[ite->second].gravity_info.GetRAlign().transpose(); - } - } - LOG(INFO) << counter << " images are loaded with gravity" << std::endl; -} - -} // namespace glomap \ No newline at end of file diff --git a/glomap/io/gravity_io.h b/glomap/io/gravity_io.h deleted file mode 100644 index 4dabda23..00000000 --- a/glomap/io/gravity_io.h +++ /dev/null @@ -1,14 +0,0 @@ -#pragma once - -#include "glomap/scene/image.h" - -#include - -namespace glomap { -// Require the gravity in the format: image_name, gravity (3 numbers) -// Gravity should be the direction of [0,1,0] in the image frame -// image.cam_from_world * [0,1,0]^T = g -void ReadGravity(const std::string& gravity_path, - std::unordered_map& images); - -} // namespace glomap \ No newline at end of file diff --git a/glomap/io/pose_io.cc b/glomap/io/pose_io.cc new file mode 100644 index 00000000..b5c2fad9 --- /dev/null +++ b/glomap/io/pose_io.cc @@ -0,0 +1,235 @@ +#include "pose_io.h" + +#include +#include +#include + +namespace glomap { +void ReadRelPose(const std::string& file_path, + std::unordered_map& images, + ViewGraph& view_graph) { + std::unordered_map name_idx; + image_t max_image_id = 0; + camera_t max_camera_id = 0; + for (const auto& [image_id, image] : images) { + name_idx[image.file_name] = image_id; + + max_image_id = std::max(max_image_id, image_id); + max_camera_id = std::max(max_camera_id, image.camera_id); + } + + // Mark every edge in te view graph as invalid + for (auto& [pair_id, image_pair] : view_graph.image_pairs) { + image_pair.is_valid = false; + } + + std::ifstream file(file_path); + + // Read in data + std::string line; + std::string item; + + size_t counter = 0; + + // Required data structures + // IMAGE_NAME_1 IMAGE_NAME_2 QW QX QY QZ TX TY TZ + while (std::getline(file, line)) { + std::stringstream line_stream(line); + + std::string file1, file2; + std::getline(line_stream, item, ' '); + file1 = item; + std::getline(line_stream, item, ' '); + file2 = item; + + if (name_idx.find(file1) == name_idx.end()) { + max_image_id += 1; + max_camera_id += 1; + images.insert(std::make_pair(max_image_id, + Image(max_image_id, max_camera_id, file1))); + name_idx[file1] = max_image_id; + } + if (name_idx.find(file2) == name_idx.end()) { + max_image_id += 1; + max_camera_id += 1; + images.insert(std::make_pair(max_image_id, + Image(max_image_id, max_camera_id, file2))); + name_idx[file2] = max_image_id; + } + + image_t index1 = name_idx[file1]; + image_t index2 = name_idx[file2]; + + const image_pair_t pair_id = colmap::ImagePairToPairId(index1, index2); + + // rotation + Rigid3d pose_rel; + for (int i = 0; i < 4; i++) { + std::getline(line_stream, item, ' '); + pose_rel.rotation.coeffs()[(i + 3) % 4] = std::stod(item); + } + + for (int i = 0; i < 3; i++) { + std::getline(line_stream, item, ' '); + pose_rel.translation[i] = std::stod(item); + } + + 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 << " relative poses are loaded"; +} + +void ReadRelWeight(const std::string& file_path, + const std::unordered_map& images, + ViewGraph& view_graph) { + std::unordered_map name_idx; + for (const auto& [image_id, image] : images) { + name_idx[image.file_name] = image_id; + } + + std::ifstream file(file_path); + + // Read in data + std::string line; + std::string item; + + size_t counter = 0; + + // Required data structures + // IMAGE_NAME_1 IMAGE_NAME_2 QW QX QY QZ TX TY TZ + while (std::getline(file, line)) { + std::stringstream line_stream(line); + + std::string file1, file2; + std::getline(line_stream, item, ' '); + file1 = item; + std::getline(line_stream, item, ' '); + file2 = item; + + if (name_idx.find(file1) == name_idx.end() || + name_idx.find(file2) == name_idx.end()) + continue; + + image_t index1 = name_idx[file1]; + image_t index2 = name_idx[file2]; + + image_pair_t pair_id = colmap::ImagePairToPairId(index1, index2); + + if (view_graph.image_pairs.find(pair_id) == view_graph.image_pairs.end()) + continue; + + std::getline(line_stream, item, ' '); + view_graph.image_pairs[pair_id].weight = std::stod(item); + counter++; + } + LOG(INFO) << counter << " weights are used are loaded"; +} + +// 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; + for (const auto& [image_id, image] : images) { + name_idx[image.file_name] = image_id; + } + + std::ifstream file(gravity_path); + + // Read in the file list + std::string line, item; + Eigen::Vector3d gravity; + int counter = 0; + while (std::getline(file, line)) { + std::stringstream line_stream(line); + + // file_name + std::string name; + std::getline(line_stream, name, ' '); + + // Gravity + for (double i = 0; i < 3; i++) { + std::getline(line_stream, item, ' '); + gravity[i] = std::stod(item); + } + + // Check whether the image present + auto ite = name_idx.find(name); + if (ite != name_idx.end()) { + counter++; + 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"; +} + +void WriteGlobalRotation(const std::string& file_path, + const std::unordered_map& images) { + std::ofstream file(file_path); + std::set existing_images; + for (const auto& [image_id, image] : images) { + if (image.IsRegistered()) { + existing_images.insert(image_id); + } + } + for (const auto& image_id : existing_images) { + const auto& image = images.at(image_id); + if (!image.IsRegistered()) continue; + file << image.file_name; + Rigid3d cam_from_world = image.CamFromWorld(); + for (int i = 0; i < 4; i++) { + file << " " << cam_from_world.rotation.coeffs()[(i + 3) % 4]; + } + file << "\n"; + } +} + +void WriteRelPose(const std::string& file_path, + const std::unordered_map& images, + const ViewGraph& view_graph) { + std::ofstream file(file_path); + + // Sort the image pairs by image name + std::map name_pair; + for (const auto& [pair_id, image_pair] : view_graph.image_pairs) { + if (image_pair.is_valid) { + const auto& image1 = images.at(image_pair.image_id1); + const auto& image2 = images.at(image_pair.image_id2); + name_pair[image1.file_name + " " + image2.file_name] = pair_id; + } + } + + // Write the image pairs + for (const auto& [name, pair_id] : name_pair) { + const auto image_pair = view_graph.image_pairs.at(pair_id); + if (!image_pair.is_valid) continue; + file << images.at(image_pair.image_id1).file_name << " " + << images.at(image_pair.image_id2).file_name; + for (int i = 0; i < 4; i++) { + file << " " << image_pair.cam2_from_cam1.rotation.coeffs()[(i + 3) % 4]; + } + for (int i = 0; i < 3; i++) { + file << " " << image_pair.cam2_from_cam1.translation[i]; + } + file << "\n"; + } + + LOG(INFO) << name_pair.size() << " relpose are written"; +} +} // namespace glomap diff --git a/glomap/io/pose_io.h b/glomap/io/pose_io.h new file mode 100644 index 00000000..e98112f7 --- /dev/null +++ b/glomap/io/pose_io.h @@ -0,0 +1,37 @@ +#pragma once + +#include "glomap/scene/types_sfm.h" + +#include + +namespace glomap { +// Required data structures +// IMAGE_NAME_1 IMAGE_NAME_2 QW QX QY QZ TX TY TZ +void ReadRelPose(const std::string& file_path, + std::unordered_map& images, + ViewGraph& view_graph); + +// Required data structures +// IMAGE_NAME_1 IMAGE_NAME_2 weight +void ReadRelWeight(const std::string& file_path, + const std::unordered_map& images, + ViewGraph& view_graph); + +// Require the gravity in the format: +// IMAGE_NAME GX GY GZ +// Gravity should be the direction of [0,1,0] in the image frame +// image.cam_from_world * [0,1,0]^T = g +void ReadGravity(const std::string& gravity_path, + std::unordered_map& images); + +// Output would be of the format: +// IMAGE_NAME QW QX QY QZ +void WriteGlobalRotation(const std::string& file_path, + const std::unordered_map& images); + +// Output would be of the format: +// IMAGE_NAME_1 IMAGE_NAME_2 QW QX QY QZ TX TY TZ +void WriteRelPose(const std::string& file_path, + const std::unordered_map& images, + const ViewGraph& view_graph); +} // namespace glomap \ No newline at end of file diff --git a/glomap/io/recording.cc b/glomap/io/recording.cc index a9dd49fc..1a3c9f05 100644 --- a/glomap/io/recording.cc +++ b/glomap/io/recording.cc @@ -1,6 +1,7 @@ #include "recording.h" #include -// #include +#include namespace glomap { @@ -9,15 +10,25 @@ uint32_t algorithm_step = 0; std::string image_path_global = ""; void init_recording() { - rr_rec.spawn().exit_on_failure(); + // Check for RERUN_CONNECT environment variable to connect to remote viewer + // Format: "host:port" (e.g., "host.docker.internal:9876" or "192.168.1.100:9876") + // Default rerun port is 9876 + const char* rerun_connect = std::getenv("RERUN_CONNECT"); + + if (rerun_connect != nullptr && std::strlen(rerun_connect) > 0) { + LOG(INFO) << "Connecting to remote Rerun viewer at: " << rerun_connect; + rr_rec.connect_grpc(rerun_connect).exit_on_failure(); + } else { + // Spawn local viewer + rr_rec.spawn().exit_on_failure(); + } rr_rec.set_time_sequence("step", algorithm_step); } void log_bitmap(rerun::RecordingStream &rec, std::string_view entity_path, colmap::Bitmap& bitmap) { - size_t width = bitmap.Width(); - size_t height = bitmap.Height(); + uint32_t width = static_cast(bitmap.Width()); + uint32_t height = static_cast(bitmap.Height()); size_t nchannels = bitmap.Channels(); - std::vector shape = {height, width, nchannels}; auto buffer = bitmap.ConvertToRowMajorArray(); LOG(INFO) << buffer.size(); LOG(INFO) << entity_path; @@ -25,8 +36,12 @@ void log_bitmap(rerun::RecordingStream &rec, std::string_view entity_path, colm for (size_t i = 0; i < buffer.size(); i+=3) { std::swap(buffer[i], buffer[i+2]); } + rec.log(entity_path, rerun::Image::from_rgb24(buffer, {width, height})); + } else if (nchannels == 4) { + rec.log(entity_path, rerun::Image::from_rgba32(buffer, {width, height})); + } else if (nchannels == 1) { + rec.log(entity_path, rerun::Image::from_grayscale8(buffer, {width, height})); } - rec.log(entity_path, rerun::Image(shape, std::move(buffer))); } std::unordered_map> get_observation_to_point_map( @@ -38,7 +53,7 @@ std::unordered_map> get_observation_to_point_map( if (tracks.size()) { // Initialize every point to corresponds to invalid point for (auto& [image_id, image] : images) { - if (!image.is_registered) + if (!image.IsRegistered()) continue; image_to_point3D[image_id] = std::vector(image.features.size(), -1); @@ -68,23 +83,22 @@ void log_reconstruction( std::vector points; std::vector colors; - + for (auto &[_, image] : images) { + if (!image.IsRegistered()) continue; // Skip images without valid poses + auto camera = cameras.at(image.camera_id); - Eigen::Vector3f translation = image.cam_from_world.translation.cast(); - Eigen::Matrix3f rotation = image.cam_from_world.rotation.toRotationMatrix().cast(); - rec.log("images/" + image.file_name, rerun::Transform3D( - rerun::datatypes::TranslationAndMat3x3( - rerun::Vec3D(translation.data()), - rerun::Mat3x3(rotation.data()), - true - ) - )); + Eigen::Vector3f translation = image.CamFromWorld().translation.cast(); + Eigen::Matrix3f rotation = image.CamFromWorld().rotation.toRotationMatrix().cast(); + rec.log("images/" + image.file_name, rerun::Transform3D() + .with_translation(rerun::Vec3D(translation.data())) + .with_mat3x3(rerun::Mat3x3(rotation.data())) + ); rec.log_static("images/" + image.file_name, rerun::ViewCoordinates::RDF); Eigen::Matrix3Xf K = camera.GetK().cast(); rec.log( - "images/" + image.file_name, + "images/" + image.file_name, rerun::Pinhole(rerun::components::PinholeProjection(rerun::datatypes::Mat3x3(K.data()))) .with_resolution(int(camera.width), int(camera.height)) ); @@ -94,7 +108,7 @@ void log_reconstruction( // Should actually be `track.observations.size() < options_.min_num_view_per_track`. if (track.observations.size() < 3) continue; - + auto xyz = track.xyz; points.emplace_back(xyz.x(), xyz.y(), xyz.z()); colors.emplace_back(track.color[0], track.color[1], track.color[2]); @@ -105,9 +119,9 @@ void log_reconstruction( void log_images(rerun::RecordingStream &rec, const std::unordered_map& images, const std::string image_path) { for (auto &[id, image] : images) { - std::string path = colmap::JoinPaths(image_path, image.file_name); + std::filesystem::path full_path = std::filesystem::path(image_path) / image.file_name; colmap::Bitmap bitmap; - if (!bitmap.Read(path)) { + if (!bitmap.Read(full_path.string())) { LOG(ERROR) << "Failed to read image path"; } std::string entity_path = "images/"; @@ -122,4 +136,4 @@ void log_images(rerun::RecordingStream &rec, const std::unordered_map& gravities) { + if (gravities.size() == 0) { + std::cerr + << "Error trying to calculate the average gravities of an empty set!\n"; + return Eigen::Vector3d::Zero(); + } + + // first build a 3x3 matrix which is the elementwise sum of the product of + // each quaternion with itself + Eigen::Matrix3d A = Eigen::Matrix3d::Zero(); + + for (int g = 0; g < gravities.size(); ++g) + A += gravities[g] * gravities[g].transpose(); + + // normalise with the number of gravities + A /= gravities.size(); + + // Compute the SVD of this 3x3 matrix + Eigen::JacobiSVD svd( + A, Eigen::ComputeThinU | Eigen::ComputeThinV); + + Eigen::VectorXd singular_values = svd.singularValues(); + Eigen::MatrixXd U = svd.matrixU(); + + // find the eigen vector corresponding to the largest eigen value + int largest_eigen_value_index = -1; + float largest_eigen_value; + bool first = true; + + for (int i = 0; i < singular_values.rows(); ++i) { + if (first) { + largest_eigen_value = singular_values(i); + largest_eigen_value_index = i; + first = false; + } else if (singular_values(i) > largest_eigen_value) { + largest_eigen_value = singular_values(i); + largest_eigen_value_index = i; + } + } + + Eigen::Vector3d average; + average(0) = U(0, largest_eigen_value_index); + average(1) = U(1, largest_eigen_value_index); + average(2) = U(2, largest_eigen_value_index); + + int negative_counter = 0; + for (int g = 0; g < gravities.size(); ++g) { + if (gravities[g].dot(average) < 0) negative_counter++; + } + if (negative_counter > gravities.size() / 2) { + average = -average; + } + + return average; +} + +double CalcAngle(const Eigen::Vector3d& gravity1, + const Eigen::Vector3d& gravity2) { + double cos_r = gravity1.dot(gravity2) / (gravity1.norm() * gravity2.norm()); + cos_r = std::min(std::max(cos_r, -1.), 1.); + + return std::acos(cos_r) * 180 / EIGEN_PI; +} +} // namespace glomap diff --git a/glomap/math/gravity.h b/glomap/math/gravity.h index 789ddd14..bf7a0406 100644 --- a/glomap/math/gravity.h +++ b/glomap/math/gravity.h @@ -14,4 +14,9 @@ double RotUpToAngle(const Eigen::Matrix3d& R_up); // Get the upright rotation matrix from a rotation angle Eigen::Matrix3d AngleToRotUp(double angle); +// Estimate the average gravity direction from a set of gravity directions +Eigen::Vector3d AverageGravity(const std::vector& gravities); + +double CalcAngle(const Eigen::Vector3d& gravity1, + const Eigen::Vector3d& gravity2); } // namespace glomap diff --git a/glomap/math/l1_solver.h b/glomap/math/l1_solver.h deleted file mode 100644 index 1327bcae..00000000 --- a/glomap/math/l1_solver.h +++ /dev/null @@ -1,113 +0,0 @@ -// This code is adapted from Theia library (http://theia-sfm.org/), -// with its original L1 solver adapted from -// "https://web.stanford.edu/~boyd/papers/admm/least_abs_deviations/lad.html" - -#pragma once - -#include - -#include -#include -#include - -// An L1 norm (|| A * x - b ||_1) approximation solver based on ADMM -// (alternating direction method of multipliers, -// https://web.stanford.edu/~boyd/papers/pdf/admm_distr_stats.pdf). -namespace glomap { - -// TODO: L1 solver for dense matrix -struct L1SolverOptions { - int max_num_iterations = 1000; - // Rho is the augmented Lagrangian parameter. - double rho = 1.0; - // Alpha is the over-relaxation parameter (typically between 1.0 and 1.8). - double alpha = 1.0; - - double absolute_tolerance = 1e-4; - double relative_tolerance = 1e-2; -}; - -template -class L1Solver { - public: - L1Solver(const L1SolverOptions& options, const MatrixType& mat) - : options_(options), a_(mat) { - // Pre-compute the sparsity pattern. - const MatrixType spd_mat = a_.transpose() * a_; - linear_solver_.compute(spd_mat); - } - - void Solve(const Eigen::VectorXd& rhs, Eigen::VectorXd* solution) { - Eigen::VectorXd& x = *solution; - Eigen::VectorXd z(a_.rows()), u(a_.rows()); - z.setZero(); - u.setZero(); - - Eigen::VectorXd a_times_x(a_.rows()), z_old(z.size()), ax_hat(a_.rows()); - // Precompute some convergence terms. - const double rhs_norm = rhs.norm(); - const double primal_abs_tolerance_eps = - std::sqrt(a_.rows()) * options_.absolute_tolerance; - const double dual_abs_tolerance_eps = - std::sqrt(a_.cols()) * options_.absolute_tolerance; - - const std::string row_format = - " % 4d % 4.4e % 4.4e % 4.4e % 4.4e"; - for (int i = 0; i < options_.max_num_iterations; i++) { - // Update x. - x.noalias() = linear_solver_.solve(a_.transpose() * (rhs + z - u)); - if (linear_solver_.info() != Eigen::Success) { - LOG(ERROR) << "L1 Minimization failed. Could not solve the sparse " - "linear system with Cholesky Decomposition"; - return; - } - - a_times_x.noalias() = a_ * x; - ax_hat.noalias() = options_.alpha * a_times_x; - ax_hat.noalias() += (1.0 - options_.alpha) * (z + rhs); - - // Update z and set z_old. - std::swap(z, z_old); - z.noalias() = Shrinkage(ax_hat - rhs + u, 1.0 / options_.rho); - - // Update u. - u.noalias() += ax_hat - z - rhs; - - // Compute the convergence terms. - const double r_norm = (a_times_x - z - rhs).norm(); - const double s_norm = - (-options_.rho * a_.transpose() * (z - z_old)).norm(); - const double max_norm = std::max({a_times_x.norm(), z.norm(), rhs_norm}); - const double primal_eps = - primal_abs_tolerance_eps + options_.relative_tolerance * max_norm; - const double dual_eps = dual_abs_tolerance_eps + - options_.relative_tolerance * - (options_.rho * a_.transpose() * u).norm(); - - // Determine if the minimizer has converged. - if (r_norm < primal_eps && s_norm < dual_eps) { - break; - } - } - } - - private: - const L1SolverOptions& options_; - - // Matrix A in || Ax - b ||_1 - const MatrixType a_; - - // Cholesky linear solver. Since our linear system will be a SPD matrix we can - // utilize the Cholesky factorization. - Eigen::CholmodSupernodalLLT> linear_solver_; - - static Eigen::VectorXd Shrinkage(const Eigen::VectorXd& vec, - const double kappa) { - Eigen::ArrayXd zero_vec(vec.size()); - zero_vec.setZero(); - return zero_vec.max(vec.array() - kappa) - - zero_vec.max(-vec.array() - kappa); - } -}; - -} // namespace glomap diff --git a/glomap/math/rigid3d.cc b/glomap/math/rigid3d.cc index 98869a5a..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 / M_PI; + return pose1.rotation.angularDistance(pose2.rotation) * 180 / EIGEN_PI; } double CalcTrans(const Rigid3d& pose1, const Rigid3d& pose2) { @@ -22,21 +16,19 @@ 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 / M_PI; + return std::acos(cos_r) * 180 / EIGEN_PI; } 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 / M_PI; + return std::acos(cos_r) * 180 / EIGEN_PI; } -double DegToRad(double degree) { return degree * M_PI / 180; } +double DegToRad(double degree) { return degree * EIGEN_PI / 180; } -double RadToDeg(double radian) { return radian * 180 / M_PI; } +double RadToDeg(double radian) { return radian * 180 / EIGEN_PI; } Eigen::Vector3d Rigid3dToAngleAxis(const Rigid3d& pose) { Eigen::AngleAxis aa(pose.rotation); @@ -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 1a0ad4e5..15043ccd 100644 --- a/glomap/math/tree.cc +++ b/glomap/math/tree.cc @@ -84,11 +84,21 @@ 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(); } + double max_weight = 0; + for (const auto& [pair_id, image_pair] : view_graph.image_pairs) { + if (image_pair.is_valid == false) continue; + if (type == INLIER_RATIO) + max_weight = std::max(max_weight, image_pair.weight); + else + max_weight = + std::max(max_weight, static_cast(image_pair.inliers.size())); + } + // establish graph weighted_graph G(image_id_to_idx.size()); weight_map weights_boost = boost::get(boost::edge_weight, G); @@ -100,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; } @@ -111,11 +121,11 @@ image_t MaximumSpanningTree(const ViewGraph& view_graph, // spanning tree e = boost::add_edge(idx1, idx2, G).first; if (type == INLIER_NUM) - weights_boost[e] = -image_pair.inliers.size(); + weights_boost[e] = max_weight - image_pair.inliers.size(); else if (type == INLIER_RATIO) - weights_boost[e] = -image_pair.weight; + weights_boost[e] = max_weight - image_pair.weight; else - weights_boost[e] = -image_pair.inliers.size(); + weights_boost[e] = max_weight - image_pair.inliers.size(); } std::vector @@ -142,4 +152,4 @@ image_t MaximumSpanningTree(const ViewGraph& view_graph, return idx_to_image_id[0]; } -}; // namespace glomap \ No newline at end of file +}; // namespace glomap diff --git a/glomap/math/two_view_geometry.cc b/glomap/math/two_view_geometry.cc index 3d9a5d1d..b7d5f5d1 100644 --- a/glomap/math/two_view_geometry.cc +++ b/glomap/math/two_view_geometry.cc @@ -51,7 +51,7 @@ void FundamentalFromMotionAndCameras(const Camera& camera1, Eigen::Matrix3d* F) { Eigen::Matrix3d E; EssentialFromMotion(pose, &E); - *F = camera1.GetK().transpose().inverse() * E * camera2.GetK().inverse(); + *F = camera2.GetK().transpose().inverse() * E * camera1.GetK().inverse(); } double SampsonError(const Eigen::Matrix3d& E, diff --git a/glomap/math/union_find.h b/glomap/math/union_find.h deleted file mode 100644 index 8c9687f9..00000000 --- a/glomap/math/union_find.h +++ /dev/null @@ -1,40 +0,0 @@ -#pragma once -#include -#include - -namespace glomap { - -// UnionFind class to maintain disjoint sets for creating tracks -template -class UnionFind { - public: - // Find the root of the element x - DataType Find(DataType x) { - // If x is not in parent map, initialize it with x as its parent - auto parentIt = parent_.find(x); - if (parentIt == parent_.end()) { - parent_.emplace_hint(parentIt, x, x); - return x; - } - // Path compression: set the parent of x to the root of the set containing x - if (parentIt->second != x) { - parentIt->second = Find(parentIt->second); - } - return parentIt->second; - } - - // Unite the sets containing x and y - void Union(DataType x, DataType y) { - DataType root_x = Find(x); - DataType root_y = Find(y); - if (root_x != root_y) parent_[root_x] = root_y; - } - - void Clear() { parent_.clear(); } - - private: - // Map to store the parent of each element - std::unordered_map parent_; -}; - -} // namespace glomap diff --git a/glomap/processors/image_pair_inliers.cc b/glomap/processors/image_pair_inliers.cc index c32878f1..a0e14594 100644 --- a/glomap/processors/image_pair_inliers.cc +++ b/glomap/processors/image_pair_inliers.cc @@ -212,4 +212,4 @@ void ImagePairsInlierCount(ViewGraph& view_graph, } } -} // namespace glomap \ No newline at end of file +} // namespace glomap diff --git a/glomap/processors/image_undistorter.cc b/glomap/processors/image_undistorter.cc index ae397484..eb4bfe0d 100644 --- a/glomap/processors/image_undistorter.cc +++ b/glomap/processors/image_undistorter.cc @@ -1,5 +1,7 @@ #include "glomap/processors/image_undistorter.h" +#include + namespace glomap { void UndistortImages(std::unordered_map& cameras, @@ -7,39 +9,38 @@ void UndistortImages(std::unordered_map& cameras, bool clean_points) { std::vector image_ids; for (auto& [image_id, image] : images) { - int num_points = image.features.size(); - + const int num_points = image.features.size(); if (image.features_undist.size() == num_points && !clean_points) continue; // already undistorted image_ids.push_back(image_id); } - LOG(INFO) << "Undistorting images.."; -#pragma omp parallel for - for (size_t i = 0; i < image_ids.size(); i++) { - Eigen::Vector2d pt_undist; - Eigen::Vector3d pt_undist_norm; - - image_t image_id = image_ids[i]; - Image& image = images[image_id]; - - int camera_id = image.camera_id; - Camera& camera = cameras[camera_id]; - int num_points = image.features.size(); + colmap::ThreadPool thread_pool(colmap::ThreadPool::kMaxNumThreads); + LOG(INFO) << "Undistorting images.."; + const int num_images = image_ids.size(); + for (int image_idx = 0; image_idx < num_images; image_idx++) { + Image& image = images[image_ids[image_idx]]; + const int num_points = image.features.size(); if (image.features_undist.size() == num_points && !clean_points) continue; // already undistorted - image.features_undist.clear(); - image.features_undist.reserve(num_points); - for (int i = 0; i < num_points; i++) { - // Undistort point in image - pt_undist = camera.CamFromImg(image.features[i]); - - pt_undist_norm = pt_undist.homogeneous().normalized(); - image.features_undist.emplace_back(pt_undist_norm); - } + const Camera& camera = cameras[image.camera_id]; + + thread_pool.AddTask([&image, &camera, num_points]() { + image.features_undist.clear(); + image.features_undist.reserve(num_points); + for (int i = 0; i < num_points; i++) { + image.features_undist.emplace_back( + camera.CamFromImg(image.features[i]) + .value_or(Eigen::Vector2d::Zero()) + .homogeneous() + .normalized()); + } + }); } + + thread_pool.Wait(); LOG(INFO) << "Image undistortion done"; } diff --git a/glomap/processors/reconstruction_normalizer.cc b/glomap/processors/reconstruction_normalizer.cc new file mode 100644 index 00000000..04800a51 --- /dev/null +++ b/glomap/processors/reconstruction_normalizer.cc @@ -0,0 +1,87 @@ +#include "reconstruction_normalizer.h" + +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, + double extent, + double p0, + double p1) { + // Coordinates of image centers or point locations. + std::vector coords_x; + std::vector coords_y; + std::vector coords_z; + + coords_x.reserve(images.size()); + coords_y.reserve(images.size()); + coords_z.reserve(images.size()); + for (const auto& [image_id, image] : images) { + if (!image.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))); + coords_z.push_back(static_cast(proj_center(2))); + } + + // Determine robust bounding box and mean. + std::sort(coords_x.begin(), coords_x.end()); + std::sort(coords_y.begin(), coords_y.end()); + std::sort(coords_z.begin(), coords_z.end()); + + const size_t P0 = static_cast( + (coords_x.size() > 3) ? p0 * (coords_x.size() - 1) : 0); + const size_t P1 = static_cast( + (coords_x.size() > 3) ? p1 * (coords_x.size() - 1) : coords_x.size() - 1); + + const Eigen::Vector3d bbox_min(coords_x[P0], coords_y[P0], coords_z[P0]); + const Eigen::Vector3d bbox_max(coords_x[P1], coords_y[P1], coords_z[P1]); + + Eigen::Vector3d mean_coord(0, 0, 0); + for (size_t i = P0; i <= P1; ++i) { + mean_coord(0) += coords_x[i]; + mean_coord(1) += coords_y[i]; + mean_coord(2) += coords_z[i]; + } + mean_coord /= P1 - P0 + 1; + + // Calculate scale and translation, such that + // translation is applied before scaling. + double scale = 1.; + if (!fixed_scale) { + const double old_extent = (bbox_max - bbox_min).norm(); + if (old_extent >= std::numeric_limits::epsilon()) { + scale = extent / old_extent; + } + } + colmap::Sim3d tform( + scale, Eigen::Quaterniond::Identity(), -scale * mean_coord); + + for (auto& [_, 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); + } + } + } + + for (auto& [_, track] : tracks) { + track.xyz = tform * track.xyz; + } + + return tform; +} + +} // namespace glomap diff --git a/glomap/processors/reconstruction_normalizer.h b/glomap/processors/reconstruction_normalizer.h new file mode 100644 index 00000000..6566b849 --- /dev/null +++ b/glomap/processors/reconstruction_normalizer.h @@ -0,0 +1,20 @@ +#pragma once + +#include "glomap/scene/types_sfm.h" + +#include + +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, + double extent = 10., + double p0 = 0.1, + double p1 = 0.9); + +} // namespace glomap diff --git a/glomap/processors/reconstruction_pruning.cc b/glomap/processors/reconstruction_pruning.cc index 5ebbc205..6fc0eb2f 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; - image_pair_t pair_id = - ImagePair::ImagePairToPairId(image_id1, image_id2); + frame_t frame_id2 = images[image_id2].frame_id; + if (frame_id1 == frame_id2) continue; + const image_pair_t pair_id = + colmap::ImagePairToPairId(frame_id1, frame_id2); if (pair_covisibility_count.find(pair_id) == pair_covisibility_count.end()) { pair_covisibility_count[pair_id] = 1; @@ -33,30 +37,72 @@ 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, // then require each pair to have at least 5 points if (count >= 5) { counter++; - image_t image_id1, image_id2; - ImagePair::PairIdToImagePair(pair_id, image_id1, image_id2); + const auto [image_id1, image_id2] = colmap::PairIdToImagePair(pair_id); - if (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) { + const auto [frame_id1, frame_id2] = colmap::PairIdToImagePair(pair_id); + const image_t image_id1 = frame_id_to_begin_img[frame_id1]; + const image_t image_id2 = frame_id_to_begin_img[frame_id2]; + visibility_graph.image_pairs.insert( + std::make_pair(pair_id, ImagePair(image_id1, image_id2))); + visibility_graph.image_pairs[pair_id].weight = image_pair.weight; + } + + 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 = + colmap::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 +121,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.), @@ -83,4 +130,4 @@ image_t PruneWeaklyConnectedImages(std::unordered_map& images, // return visibility_graph.MarkConnectedComponents(images, min_num_images); } -} // namespace glomap \ No newline at end of file +} // namespace glomap 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..1f14cd07 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) { @@ -64,4 +64,4 @@ void RelPoseFilter::FilterInlierRatio(ViewGraph& view_graph, << " relative poses with inlier ratio < " << min_inlier_ratio; } -} // namespace glomap \ No newline at end of file +} // namespace glomap diff --git a/glomap/processors/track_filter.cc b/glomap/processors/track_filter.cc index 226af020..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,15 +29,16 @@ 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(); } // If the reprojection error is smaller than the threshold, then keep it if (reprojection_error < max_reprojection_error) { - observation_new.emplace_back(std::make_pair(image_id, feature_id)); + observation_new.emplace_back(image_id, feature_id); } } if (observation_new.size() != track.observations.size()) { @@ -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 4571d452..24c36726 100644 --- a/glomap/processors/view_graph_manipulation.cc +++ b/glomap/processors/view_graph_manipulation.cc @@ -1,24 +1,28 @@ #include "view_graph_manipulation.h" #include "glomap/math/two_view_geometry.h" -#include "glomap/math/union_find.h" + +#include +#include 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; const std::unordered_map>& - adjacency_list = view_graph.GetAdjacencyList(); + adjacency_list = view_graph.CreateImageAdjacencyList(); // Here, the average is the mean of the degrees double average_degree = 0; 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; @@ -31,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(); @@ -43,6 +47,7 @@ image_pair_t ViewGraphManipulater::SparsifyGraph( continue; } + // TODO: Replace rand() with thread-safe random number generator. if (rand() / double(RAND_MAX) < (expected_degree * average_degree) / (degree1 * degree2)) { chosen_edges.insert(pair_id); @@ -57,21 +62,24 @@ 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; + colmap::UnionFind uf; + uf.Reserve(frames.size()); // Go through the edges, and add the edge with weight > min_thres for (auto& [pair_id, image_pair] : view_graph.image_pairs) { if (image_pair.is_valid == false) continue; @@ -81,8 +89,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)); } } @@ -115,8 +123,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; @@ -151,11 +159,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 @@ -198,9 +209,7 @@ void ViewGraphManipulater::UpdateImagePairsConfig( // pairs are valid, then set the camera to valid std::unordered_map camera_validity; for (auto& [camera_id, counter] : camera_counter) { - if (counter.first == 0) { - camera_validity[camera_id] = false; - } else if (counter.second * 1. / counter.first > 0.5) { + if (counter.second * 1. / counter.first > 0.5) { camera_validity[camera_id] = true; } else { camera_validity[camera_id] = false; @@ -241,49 +250,55 @@ void ViewGraphManipulater::DecomposeRelPose( continue; image_pair_ids.push_back(pair_id); } - LOG(INFO) << "Decompose relative pose for " << image_pair_ids.size() - << " pairs"; - -#pragma omp parallel for - for (size_t idx = 0; idx < image_pair_ids.size(); idx++) { - ImagePair& image_pair = view_graph.image_pairs.at(image_pair_ids[idx]); - image_t image_id1 = image_pair.image_id1; - image_t image_id2 = image_pair.image_id2; - camera_t camera_id1 = images.at(image_id1).camera_id; - camera_t camera_id2 = images.at(image_id2).camera_id; - - // Use the two-view geometry to re-estimate the relative pose - colmap::TwoViewGeometry two_view_geometry; - two_view_geometry.E = image_pair.E; - two_view_geometry.F = image_pair.F; - two_view_geometry.H = image_pair.H; - two_view_geometry.config = image_pair.config; - - colmap::EstimateTwoViewGeometryPose(cameras[camera_id1], - images[image_id1].features, - cameras[camera_id2], - images[image_id2].features, - &two_view_geometry); - - // if it planar, then use the estimated relative pose - if (image_pair.config == colmap::TwoViewGeometry::PLANAR && - cameras[camera_id1].has_prior_focal_length && - cameras[camera_id2].has_prior_focal_length) { - image_pair.config = colmap::TwoViewGeometry::CALIBRATED; - continue; - } else if (!(cameras[camera_id1].has_prior_focal_length && - cameras[camera_id2].has_prior_focal_length)) - continue; + const int64_t num_image_pairs = image_pair_ids.size(); + LOG(INFO) << "Decompose relative pose for " << num_image_pairs << " pairs"; - image_pair.config = two_view_geometry.config; - image_pair.cam2_from_cam1 = two_view_geometry.cam2_from_cam1; + colmap::ThreadPool thread_pool(colmap::ThreadPool::kMaxNumThreads); + for (int64_t idx = 0; idx < num_image_pairs; idx++) { + thread_pool.AddTask([&, idx]() { + ImagePair& image_pair = view_graph.image_pairs.at(image_pair_ids[idx]); + image_t image_id1 = image_pair.image_id1; + image_t image_id2 = image_pair.image_id2; - if (image_pair.cam2_from_cam1.translation.norm() > EPS) { - image_pair.cam2_from_cam1.translation = - image_pair.cam2_from_cam1.translation.normalized(); - } + camera_t camera_id1 = images.at(image_id1).camera_id; + camera_t camera_id2 = images.at(image_id2).camera_id; + + // Use the two-view geometry to re-estimate the relative pose + colmap::TwoViewGeometry two_view_geometry; + two_view_geometry.E = image_pair.E; + two_view_geometry.F = image_pair.F; + two_view_geometry.H = image_pair.H; + two_view_geometry.config = image_pair.config; + + colmap::EstimateTwoViewGeometryPose(cameras[camera_id1], + images[image_id1].features, + cameras[camera_id2], + images[image_id2].features, + &two_view_geometry); + + // if it planar, then use the estimated relative pose + if (image_pair.config == colmap::TwoViewGeometry::PLANAR && + cameras[camera_id1].has_prior_focal_length && + cameras[camera_id2].has_prior_focal_length) { + image_pair.config = colmap::TwoViewGeometry::CALIBRATED; + return; + } else if (!(cameras[camera_id1].has_prior_focal_length && + cameras[camera_id2].has_prior_focal_length)) + return; + + image_pair.config = two_view_geometry.config; + image_pair.cam2_from_cam1 = two_view_geometry.cam2_from_cam1; + + if (image_pair.cam2_from_cam1.translation.norm() > EPS) { + image_pair.cam2_from_cam1.translation = + image_pair.cam2_from_cam1.translation.normalized(); + } + }); } + + thread_pool.Wait(); + size_t counter = 0; for (size_t idx = 0; idx < image_pair_ids.size(); idx++) { ImagePair& image_pair = view_graph.image_pairs.at(image_pair_ids[idx]); 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 a191371a..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(); - - 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,35 +20,86 @@ 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; + // Frame info + // By default, set it to be invalid index + frame_t frame_id = -1; + struct Frame* frame_ptr = nullptr; - // Gravity information - GravityInfo gravity_info; - - // Features + // Distorted feature points in pixels. std::vector features; - std::vector - features_undist; // store the normalized features, can be obtained by - // calling UndistortImages + // Normalized feature rays, can be obtained by calling UndistortImages. + std::vector features_undist; // Methods inline Eigen::Vector3d Center() const; + + // 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); } -Eigen::Vector3d GravityInfo::GetGravity() { return gravity; } } // namespace glomap diff --git a/glomap/scene/image_pair.h b/glomap/scene/image_pair.h index c913cab5..31b01386 100644 --- a/glomap/scene/image_pair.h +++ b/glomap/scene/image_pair.h @@ -9,25 +9,30 @@ namespace glomap { -// FUTURE: add covariance to the relative pose +// TODO: add covariance to the relative pose struct ImagePair { - ImagePair() : pair_id(-1), image_id1(-1), image_id2(-1) {} - ImagePair(image_t img_id1, image_t img_id2, Rigid3d pose_rel = Rigid3d()) - : pair_id(ImagePairToPairId(img_id1, img_id2)), - image_id1(img_id1), - image_id2(img_id2), - cam2_from_cam1(pose_rel) {} + ImagePair() + : image_id1(colmap::kInvalidImageId), + image_id2(colmap::kInvalidImageId), + pair_id(colmap::kInvalidImagePairId) {} + ImagePair(image_t image_id1, + image_t image_id2, + Rigid3d cam2_from_cam1 = Rigid3d()) + : image_id1(image_id1), + image_id2(image_id2), + pair_id(colmap::ImagePairToPairId(image_id1, image_id2)), + cam2_from_cam1(cam2_from_cam1) {} // Ids are kept constant - const image_pair_t pair_id; const image_t image_id1; const image_t image_id2; + const image_pair_t pair_id; // indicator whether the image pair is valid bool is_valid = true; // weight is the initial inlier rate - double weight = 0; + double weight = -1; // one of `ConfigurationType`. int config = colmap::TwoViewGeometry::UNDEFINED; @@ -40,7 +45,7 @@ struct ImagePair { Eigen::Matrix3d H = Eigen::Matrix3d::Zero(); // Relative pose. - Rigid3d cam2_from_cam1; + Rigid3d cam2_from_cam1 = Rigid3d(); // Matches between the two images. // First column is the index of the feature in the first image. @@ -49,29 +54,6 @@ struct ImagePair { // Row index of inliers in the matches matrix. std::vector inliers; - - static inline image_pair_t ImagePairToPairId(const image_t image_id1, - const image_t image_id2); - - static inline void PairIdToImagePair(const image_pair_t pair_id, - image_t& image_id1, - image_t& image_id2); }; -image_pair_t ImagePair::ImagePairToPairId(const image_t image_id1, - const image_t image_id2) { - if (image_id1 > image_id2) { - return static_cast(kMaxNumImages) * image_id2 + image_id1; - } else { - return static_cast(kMaxNumImages) * 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); -} - } // namespace glomap diff --git a/glomap/scene/types.h b/glomap/scene/types.h index 3499a10f..c4631570 100644 --- a/glomap/scene/types.h +++ b/glomap/scene/types.h @@ -1,6 +1,8 @@ #pragma once #include +// #include +#include #include #include @@ -20,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; @@ -33,7 +41,18 @@ typedef uint64_t track_t; using colmap::Rigid3d; -const image_t kMaxNumImages = std::numeric_limits::max(); +// 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 d3a540f6..760593cc 100644 --- a/glomap/scene/view_graph.cc +++ b/glomap/scene/view_graph.cc @@ -1,71 +1,108 @@ #include "glomap/scene/view_graph.h" -#include "glomap/math/union_find.h" - #include namespace glomap { +namespace { + +void BreadthFirstSearch( + const std::unordered_map>& + adjacency_list, + image_t root, + std::unordered_map& visited, + std::unordered_set& component) { + std::queue queue; + queue.push(root); + visited[root] = true; + component.insert(root); + + while (!queue.empty()) { + const image_t curr = queue.front(); + queue.pop(); + + for (const image_t neighbor : adjacency_list.at(curr)) { + if (!visited[neighbor]) { + queue.push(neighbor); + visited[neighbor] = true; + component.insert(neighbor); + } + } + } +} + +std::vector> FindConnectedComponents( + const std::unordered_map>& + adjacency_list) { + std::vector> connected_components; + std::unordered_map visited; + visited.reserve(adjacency_list.size()); + for (const auto& [frame_id, neighbors] : adjacency_list) { + visited[frame_id] = false; + } + + for (auto& [frame_id, _] : adjacency_list) { + if (!visited[frame_id]) { + std::unordered_set component; + BreadthFirstSearch(adjacency_list, frame_id, visited, component); + connected_components.push_back(std::move(component)); + } + } + + return connected_components; +} + +} // namespace int ViewGraph::KeepLargestConnectedComponents( + std::unordered_map& frames, std::unordered_map& images) { - EstablishAdjacencyList(); - - int num_comp = FindConnectedComponent(); + const std::vector> connected_components = + FindConnectedComponents(CreateFrameAdjacencyList(images)); int max_idx = -1; int max_img = 0; - for (int comp = 0; comp < num_comp; comp++) { + for (int comp = 0; comp < connected_components.size(); comp++) { if (connected_components[comp].size() > max_img) { max_img = connected_components[comp].size(); max_idx = comp; } } - std::unordered_set largest_component = connected_components[max_idx]; + if (max_img == 0) return 0; - // 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; + const std::unordered_set& largest_component = + connected_components[max_idx]; + // 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(); - return max_img; -} - -int ViewGraph::FindConnectedComponent() { - connected_components.clear(); - std::unordered_map visited; - for (auto& [image_id, neighbors] : adjacency_list) { - visited[image_id] = false; + max_img = 0; + for (auto& [image_id, image] : images) { + if (image.IsRegistered()) max_img++; } - - for (auto& [image_id, neighbors] : adjacency_list) { - if (!visited[image_id]) { - std::unordered_set component; - BFS(image_id, visited, component); - connected_components.push_back(component); - } - } - - return connected_components.size(); + return max_img; } int ViewGraph::MarkConnectedComponents( - std::unordered_map& images, int min_num_img) { - EstablishAdjacencyList(); - - int num_comp = FindConnectedComponent(); + std::unordered_map& frames, + std::unordered_map& images, + int min_num_img) { + const std::vector> connected_components = + FindConnectedComponents(CreateFrameAdjacencyList(images)); + const int num_comp = connected_components.size(); std::vector> cluster_num_img(num_comp); for (int comp = 0; comp < num_comp; comp++) { @@ -74,48 +111,45 @@ 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; } -void ViewGraph::BFS(image_t root, - std::unordered_map& visited, - std::unordered_set& component) { - std::queue q; - q.push(root); - visited[root] = true; - component.insert(root); - - while (!q.empty()) { - image_t curr = q.front(); - q.pop(); - - for (image_t neighbor : adjacency_list[curr]) { - if (!visited[neighbor]) { - q.push(neighbor); - visited[neighbor] = true; - component.insert(neighbor); - } +std::unordered_map> +ViewGraph::CreateImageAdjacencyList() const { + std::unordered_map> adjacency_list; + for (const auto& [_, image_pair] : image_pairs) { + if (image_pair.is_valid) { + adjacency_list[image_pair.image_id1].insert(image_pair.image_id2); + adjacency_list[image_pair.image_id2].insert(image_pair.image_id1); } } + return adjacency_list; } -void ViewGraph::EstablishAdjacencyList() { - adjacency_list.clear(); - for (auto& [pair_id, image_pair] : image_pairs) { +std::unordered_map> +ViewGraph::CreateFrameAdjacencyList( + const std::unordered_map& images) const { + std::unordered_map> adjacency_list; + for (const auto& [_, image_pair] : image_pairs) { if (image_pair.is_valid) { - adjacency_list[image_pair.image_id1].insert(image_pair.image_id2); - adjacency_list[image_pair.image_id2].insert(image_pair.image_id1); + const frame_t frame_id1 = images.at(image_pair.image_id1).frame_id; + const frame_t frame_id2 = images.at(image_pair.image_id2).frame_id; + adjacency_list[frame_id1].insert(frame_id2); + adjacency_list[frame_id2].insert(frame_id1); } } + return adjacency_list; } + } // namespace glomap diff --git a/glomap/scene/view_graph.h b/glomap/scene/view_graph.h index 7c28229d..74497e24 100644 --- a/glomap/scene/view_graph.h +++ b/glomap/scene/view_graph.h @@ -1,60 +1,37 @@ #pragma once -#include "glomap/scene/camera.h" #include "glomap/scene/image.h" #include "glomap/scene/image_pair.h" #include "glomap/scene/types.h" -#include "glomap/types.h" -namespace glomap { - -class ViewGraph { - public: - // Methods - inline void RemoveInvalidPair(image_pair_t pair_id); - - // 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& images); - - // Mark the cluster of the cameras (cluster_id sort by the the number of - // images) - int MarkConnectedComponents(std::unordered_map& images, - int min_num_img = -1); +#include +#include - // Establish the adjacency list - void EstablishAdjacencyList(); - - inline const std::unordered_map>& - GetAdjacencyList() const; +namespace glomap { - // Data +struct ViewGraph { std::unordered_map image_pairs; - image_t num_images = 0; - image_pair_t num_pairs = 0; + // Create the adjacency list for the images in the view graph. + std::unordered_map> + CreateImageAdjacencyList() const; - private: - int FindConnectedComponent(); + // Create the adjacency list for the frames in the view graph. + std::unordered_map> + CreateFrameAdjacencyList( + const std::unordered_map& images) const; - void BFS(image_t root, - std::unordered_map& visited, - std::unordered_set& component); + // Mark the images which are not connected to any other images as not + // registered Returns the number of images in the largest connected component. + int KeepLargestConnectedComponents( + std::unordered_map& frames, + std::unordered_map& images); - // Data for processing - std::unordered_map> adjacency_list; - std::vector> connected_components; + // Mark connected clusters of images, where the cluster_id is sorted by the + // the number of images. + int MarkConnectedComponents(std::unordered_map& frames, + std::unordered_map& images, + int min_num_img = -1); }; -const std::unordered_map>& -ViewGraph::GetAdjacencyList() const { - return adjacency_list; -} - -void ViewGraph::RemoveInvalidPair(image_pair_t pair_id) { - ImagePair& pair = image_pairs.at(pair_id); - pair.is_valid = false; -} - } // namespace glomap diff --git a/glomap/version.h.in b/glomap/version.h.in deleted file mode 100644 index aa41e55b..00000000 --- a/glomap/version.h.in +++ /dev/null @@ -1,9 +0,0 @@ -#ifndef @PROJECT_NAME_UPPERCASE@_VERSION_ -#define @PROJECT_NAME_UPPERCASE@_VERSION_ - -#define @PROJECT_NAME_UPPERCASE@_MAJOR_VERSION (@MAJOR_VERSION@) -#define @PROJECT_NAME_UPPERCASE@_MINOR_VERSION (@MINOR_VERSION@) -#define @PROJECT_NAME_UPPERCASE@_PATCH_VERSION (@PATCH_VERSION@) -#define @PROJECT_NAME_UPPERCASE@_VERSION "@PROJECT_VERSION@" - -#endif // @PROJECT_NAME_UPPERCASE@_VERSION_ \ No newline at end of file diff --git a/scripts/extract_frames.py b/scripts/extract_frames.py index 97a9236b..9bba4bf8 100755 --- a/scripts/extract_frames.py +++ b/scripts/extract_frames.py @@ -4,31 +4,49 @@ import argparse import cv2 -import numpy as np from pathlib import Path -def extract_frames(cap, desired_fps: float | None) -> list[np.ndarray]: + +def extract_frames(video_path: Path, output_dir: Path, desired_fps: float | None): + output_dir.mkdir(parents=True, exist_ok=True) + + cap = cv2.VideoCapture(str(video_path)) fps = cap.get(cv2.CAP_PROP_FPS) - if desired_fps: - fps_ratio = desired_fps/fps - else: - fps_ratio = 1.0 - frames = [] + total_frames = int(cap.get(cv2.CAP_PROP_FRAME_COUNT)) + fps_ratio = (desired_fps / fps) if desired_fps else 1.0 + + # Estimate how many frames will be extracted + estimated_output = int(total_frames * fps_ratio) if fps_ratio < 1.0 else total_frames + + print(f"Video: {video_path.name}") + print(f"FPS: {fps:.2f}, Total frames: {total_frames}, Target FPS: {desired_fps or fps:.2f}") + print(f"Estimated output frames: ~{estimated_output}") + portion = 0.0 + frame_num = 0 + input_frame_num = 0 + last_percent = -1 + while True: - ret, rgb_image = cap.read() + ret, frame = cap.read() if not ret: break + input_frame_num += 1 portion += fps_ratio if portion >= 1.0: portion -= 1.0 - frames.append(rgb_image) - return frames + cv2.imwrite(str(output_dir / f"{frame_num:05d}.jpg"), frame) + frame_num += 1 -def save_frames(frames: list[np.ndarray], output_dir: Path): - output_dir.mkdir(parents=True, exist_ok=True) - for i, frame in enumerate(frames): - cv2.imwrite(str(output_dir / f"{i:0>5}.jpg"), frame) + # Print progress every 1% + percent = int(100 * input_frame_num / total_frames) if total_frames > 0 else 0 + if percent != last_percent: + print(f"\rProgress: {percent}% ({frame_num} frames extracted)", end="", flush=True) + last_percent = percent + + print(f"\rProgress: 100% - Done! ") + cap.release() + print(f"Extracted {frame_num} frames to {output_dir}") def main(): @@ -37,8 +55,8 @@ def main(): parser.add_argument("-v", "--video-path", type=Path, required=True) parser.add_argument("--desired-fps", type=float, default=60.0) args = parser.parse_args() - frames = extract_frames(cv2.VideoCapture(str(args.video_path)), args.desired_fps) - save_frames(frames, args.output_dir) + extract_frames(args.video_path, args.output_dir, args.desired_fps) + if __name__ == "__main__": main() diff --git a/scripts/format/clang_format.sh b/scripts/format/c++.sh similarity index 58% rename from scripts/format/clang_format.sh rename to scripts/format/c++.sh index 29710188..15ddc2f0 100755 --- a/scripts/format/clang_format.sh +++ b/scripts/format/c++.sh @@ -2,28 +2,9 @@ # This script applies clang-format to the whole repository. -# Find clang-format -tools=' - clang-format -' - -clang_format='' -for tool in ${tools}; do - if type -p "${tool}" > /dev/null; then - clang_format=$tool - break - fi -done - -if [ -z "$clang_format" ]; then - echo "Could not locate clang-format" - exit 1 -fi -echo "Found clang-format: $(which ${clang_format})" - # Check version -version_string=$($clang_format --version | sed -E 's/^.*(\d+\.\d+\.\d+-.*).*$/\1/') -expected_version_string='14.0.0' +version_string=$(clang-format --version | sed -E 's/^.*(\d+\.\d+\.\d+-.*).*$/\1/') +expected_version_string='20.1.5' if [[ "$version_string" =~ "$expected_version_string" ]]; then echo "clang-format version '$version_string' matches '$expected_version_string'" else @@ -40,5 +21,4 @@ all_files=$( \ num_files=$(echo $all_files | wc -w) echo "Formatting ${num_files} files" -# Run clang-format -${clang_format} -i $all_files +clang-format -i $all_files diff --git a/scripts/shell/enter_vs_dev_shell.ps1 b/scripts/shell/enter_vs_dev_shell.ps1 new file mode 100644 index 00000000..9396c5c6 --- /dev/null +++ b/scripts/shell/enter_vs_dev_shell.ps1 @@ -0,0 +1,25 @@ +if (!$env:VisualStudioDevShell) { + $vswhere = "${Env:ProgramFiles(x86)}/Microsoft Visual Studio/Installer/vswhere.exe" + if (!(Test-Path $vswhere)) { + throw "Failed to find vswhere.exe" + } + + & $vswhere -latest -format json + $vsInstance = & $vswhere -latest -format json | ConvertFrom-Json + if ($LASTEXITCODE) { + throw "vswhere.exe returned exit code $LASTEXITCODE" + } + + Import-Module "$($vsInstance.installationPath)/Common7/Tools/Microsoft.VisualStudio.DevShell.dll" + $prevCwd = Get-Location + try { + Enter-VsDevShell $vsInstance.instanceId -DevCmdArguments "-no_logo -host_arch=amd64 -arch=amd64" + } catch { + Write-Host $_ + Write-Error "Failed to enter Visual Studio Dev Shell" + exit 1 + } + Set-Location $prevCwd + + $env:VisualStudioDevShell = $true +} diff --git a/thirdparty/CMakeLists.txt b/thirdparty/CMakeLists.txt new file mode 100644 index 00000000..b9107f1c --- /dev/null +++ b/thirdparty/CMakeLists.txt @@ -0,0 +1,37 @@ +if(IS_GNU OR IS_CLANG) + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -w") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w") +endif() + +include(FetchContent) + +FetchContent_Declare(poselib + GIT_REPOSITORY https://github.com/PoseLib/PoseLib.git + GIT_TAG f119951fca625133112acde48daffa5f20eba451 + EXCLUDE_FROM_ALL + SYSTEM +) +message(STATUS "Configuring PoseLib...") +if(FETCH_POSELIB) + set(MARCH_NATIVE OFF CACHE BOOL "") + FetchContent_MakeAvailable(poselib) +else() + find_package(PoseLib REQUIRED) +endif() +message(STATUS "Configuring PoseLib... done") + +FetchContent_Declare(COLMAP + GIT_REPOSITORY https://github.com/colmap/colmap.git + GIT_TAG b6b7b54eca6078070f73a3f0a084f79c629a6f10 # Nov 20, 2025 + EXCLUDE_FROM_ALL + SYSTEM +) +message(STATUS "Configuring COLMAP...") +set(UNINSTALL_ENABLED OFF CACHE INTERNAL "") +set(GUI_ENABLED OFF CACHE INTERNAL "") +if (FETCH_COLMAP) + FetchContent_MakeAvailable(COLMAP) +else() + find_package(COLMAP REQUIRED) +endif() +message(STATUS "Configuring COLMAP... done") diff --git a/vcpkg.json b/vcpkg.json new file mode 100644 index 00000000..a1b5010e --- /dev/null +++ b/vcpkg.json @@ -0,0 +1,54 @@ +{ + "name": "glomap", + "description": "GLOMAP is a general purpose global structure-from-motion pipeline for image-based reconstruction. GLOMAP requires a COLMAP database as input and outputs a COLMAP sparse reconstruction. As compared to COLMAP, this project provides a much more efficient and scalable reconstruction process, typically 1-2 orders of magnitude faster, with on-par or superior reconstruction quality.", + "homepage": "https://github.com/colmap/glomap", + "license": "BSD-3-Clause", + "supports": "(linux | (windows & !static) | osx) & (x86 | x64 | arm64)", + "dependencies": [ + "boost-algorithm", + "boost-filesystem", + "boost-graph", + "boost-heap", + "boost-program-options", + "boost-property-map", + "boost-property-tree", + { + "name": "ceres", + "features": [ + "lapack", + "schur", + "suitesparse" + ] + }, + "eigen3", + "flann", + "freeimage", + "gflags", + "glog", + { + "name": "jasper", + "default-features": false + }, + "metis", + "sqlite3", + { + "name": "vcpkg-cmake", + "host": true + }, + { + "name": "vcpkg-cmake-config", + "host": true + }, + "gtest", + "suitesparse" + ], + "features": { + "cuda": { + "description": "Build with CUDA.", + "dependencies": [ + "glew", + "cuda" + ] + } + } +}