diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 27a5d7d..4f24ff3 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -29,7 +29,7 @@ jobs: run: shell: bash -l {0} container: - image: openxrlab/xrsfm_runtime:ubuntu1804_x64_cu116 + image: openxrlab/xrsfm_runtime:ubuntu20.04_cu11.6.1 credentials: username: ${{secrets.DOCKERHUB_USERNAME}} password: ${{secrets.DOCKERHUB_PWD}} diff --git a/3rdparty/SiftGPU/CuTexImage.cpp b/3rdparty/SiftGPU/CuTexImage.cpp index bcbcaf4..2e36c67 100644 --- a/3rdparty/SiftGPU/CuTexImage.cpp +++ b/3rdparty/SiftGPU/CuTexImage.cpp @@ -9,7 +9,7 @@ // Permission to use, copy, modify and distribute this software and its // documentation for educational, research and non-profit purposes, without // fee, and without a written agreement is hereby granted, provided that -// the above copyright notice and the following paragraph appear in all copies. +// the above copyright notice and the following paragraph appear in all copies. // // The University of North Carolina at Chapel Hill make no representations // about the suitability of this software for any purpose. It is provided diff --git a/3rdparty/apriltag/CMakeLists.txt b/3rdparty/apriltag/CMakeLists.txt index 79d4ec4..ad788f9 100644 --- a/3rdparty/apriltag/CMakeLists.txt +++ b/3rdparty/apriltag/CMakeLists.txt @@ -133,7 +133,6 @@ if(BUILD_PYTHON_WRAPPER) endif(BUILD_PYTHON_WRAPPER) if (NOT Python3_NOT_FOUND AND NOT Numpy_NOT_FOUND AND PYTHONLIBS_FOUND AND BUILD_PYTHON_WRAPPER) - # TODO deal with both python2/3 execute_process(COMMAND python3 ${CMAKE_CURRENT_SOURCE_DIR}/python_build_flags.py OUTPUT_VARIABLE PY_OUT) set(PY_VARS CFLAGS LDFLAGS LINKER EXT_SUFFIX) cmake_parse_arguments(PY "" "${PY_VARS}" "" ${PY_OUT}) @@ -160,23 +159,3 @@ execute_process(COMMAND python3 -m site --user-site OUTPUT_VARIABLE PY_DEST) string(STRIP ${PY_DEST} PY_DEST) install(FILES ${PROJECT_BINARY_DIR}/apriltag${PY_EXT_SUFFIX} DESTINATION ${PY_DEST}) endif (NOT Python3_NOT_FOUND AND NOT Numpy_NOT_FOUND AND PYTHONLIBS_FOUND AND BUILD_PYTHON_WRAPPER) - - -# Examples -# apriltag_demo -# add_executable(apriltag_demo example/apriltag_demo.c) -# target_link_libraries(apriltag_demo apriltag) - -# # opencv_demo -# find_package(OpenCV 4.0) -# if(OpenCV_FOUND) -# add_executable(opencv_demo example/opencv_demo.cc) -# target_link_libraries(opencv_demo apriltag ${OpenCV_LIBRARIES} stdc++fs) -# set_target_properties(opencv_demo PROPERTIES CXX_STANDARD 17) -# install(TARGETS opencv_demo RUNTIME DESTINATION bin) -# else() -# message(WARNING "OpenCV not found: Not building demo") -# endif(OpenCV_FOUND) - -# # install example programs -# install(TARGETS apriltag_demo RUNTIME DESTINATION bin) diff --git a/3rdparty/apriltag/apriltag.c b/3rdparty/apriltag/apriltag.c index 342cd5e..4cc6108 100644 --- a/3rdparty/apriltag/apriltag.c +++ b/3rdparty/apriltag/apriltag.c @@ -902,7 +902,6 @@ static void refine_edges(apriltag_detector_t *td, image_u8_t *im_orig, double Cxy = Mxy / N - Ex * Ey; double Cyy = Myy / N - Ey * Ey; - // TODO: Can replace this with same code as in fit_line. double normal_theta = .5 * atan2f(-2 * Cxy, (Cyy - Cxx)); nx = cosf(normal_theta); ny = sinf(normal_theta); diff --git a/3rdparty/apriltag/common/getopt.c b/3rdparty/apriltag/common/getopt.c index 87f23ba..dd547ca 100644 --- a/3rdparty/apriltag/common/getopt.c +++ b/3rdparty/apriltag/common/getopt.c @@ -218,7 +218,6 @@ int getopt_parse(getopt_t *gopt, int argc, char *argv[], int showErrors) { } if (goo->type == GOO_STRING_TYPE) { - // TODO: check whether next argument is an option, denoting // missing argument if ((i + 1) < zarray_size(toks)) { char *val = NULL; @@ -272,7 +271,6 @@ int getopt_parse(getopt_t *gopt, int argc, char *argv[], int showErrors) { if ((i + 1) < zarray_size(toks)) { char *val = NULL; zarray_get(toks, i + 1, &val); - // TODO: allow negative numerical values for short-name // options ? if (val[0] == '-') { okay = 0; diff --git a/3rdparty/apriltag/common/homography.c b/3rdparty/apriltag/common/homography.c index bd2dbbc..53e2ca0 100644 --- a/3rdparty/apriltag/common/homography.c +++ b/3rdparty/apriltag/common/homography.c @@ -396,8 +396,6 @@ matd_t *homography_to_model_view(const matd_t *H, double F, double G, double A, double R12 = R20 * R01 - R00 * R21; double R22 = R00 * R11 - R10 * R01; - // TODO XXX: Improve rotation matrix by applying polar decomposition. - return matd_create_data(4, 4, (double[]){R00, R01, R02, TX, R10, R11, R12, TY, R20, R21, R22, TZ, 0, 0, 0, 1}); diff --git a/3rdparty/apriltag/common/matd.c b/3rdparty/apriltag/common/matd.c index 8f6dfd9..40f4f04 100644 --- a/3rdparty/apriltag/common/matd.c +++ b/3rdparty/apriltag/common/matd.c @@ -516,14 +516,6 @@ matd_t *matd_inverse(const matd_t *x) { return NULL; // unreachable } -// TODO Optimization: Some operations we could perform in-place, -// saving some memory allocation work. E.g., ADD, SUBTRACT. Just need -// to make sure that we don't do an in-place modification on a matrix -// that was an input argument! - -// handle right-associative operators, greedily consuming them. These -// include transpose and inverse. This is called by the main recursion -// method. static inline matd_t *matd_op_gobble_right(const char *expr, int *pos, matd_t *acc, matd_t **garb, int *garbpos) { @@ -1091,7 +1083,6 @@ static matd_svd_t matd_svd_tall(matd_t *A, int flags) { for (int i = 0; i < vlen; i++) v[i] /= mag; - // TODO: optimize these multiplications // matd_t *Q = matd_identity(A->ncols); // for (int i = 0; i < vlen; i++) // for (int j = 0; j < vlen; j++) diff --git a/3rdparty/apriltag/common/pjpeg.c b/3rdparty/apriltag/common/pjpeg.c index f7b433b..734db95 100644 --- a/3rdparty/apriltag/common/pjpeg.c +++ b/3rdparty/apriltag/common/pjpeg.c @@ -238,7 +238,6 @@ static inline uint32_t bd_get_offset(struct bit_decoder *bd) { } static int pjpeg_decode_buffer(struct pjpeg_decode_state *pjd) { - // XXX TODO Include sanity check that this is actually a JPG struct bit_decoder bd; memset(&bd, 0, sizeof(struct bit_decoder)); diff --git a/3rdparty/apriltag/common/time_util.c b/3rdparty/apriltag/common/time_util.c index 74cd07b..c55e360 100644 --- a/3rdparty/apriltag/common/time_util.c +++ b/3rdparty/apriltag/common/time_util.c @@ -67,8 +67,6 @@ int32_t timeutil_usleep(int64_t useconds) { Sleep(useconds / 1000); return 0; #else - // unistd.h function, but usleep is obsoleted in POSIX.1-2008. - // TODO: Eventually, rewrite this to use nanosleep return usleep(useconds); #endif } diff --git a/3rdparty/json/json.hpp b/3rdparty/json/json.hpp index 67f59fa..ded00b9 100644 --- a/3rdparty/json/json.hpp +++ b/3rdparty/json/json.hpp @@ -9888,7 +9888,7 @@ class binary_reader { */ bool get_ubjson_string(string_t &result, const bool get_char = true) { if (get_char) { - get(); // TODO(niels): may we ignore N here? + get(); } if (JSON_HEDLEY_UNLIKELY( diff --git a/CMakeLists.txt b/CMakeLists.txt index 029f8d4..eb5fd14 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(xrsfm) -option(XRPRIMER_ENABLED "Buinding based on XRPRIMER, if available" ON) +option(XRPRIMER_ENABLED "Buinding based on XRPRIMER, if available" OFF) option(CUDA_ENABLED "Whether to enable CUDA, if available" ON) set(CUDA_ARCHS "Auto" CACHE STRING "List of CUDA architectures for which to \ generate code, e.g., Auto, All, Maxwell, Pascal, ...") @@ -28,11 +28,17 @@ find_package(OpenGL REQUIRED) find_package(GLEW REQUIRED) +find_package(Qt5 5.4 COMPONENTS Core OpenGL Widgets) + +set(CUDA_MIN_VERSION "7.0") if (CUDA_ENABLED) - find_package(CUDA ${CUDA_MIN_VERSION} QUIET) - message(STATUS "found") + find_package(CUDA QUIET) + message(STATUS "Found CUDA ") + message(STATUS " Includes : ${CUDA_INCLUDE_DIRS}") + message(STATUS " Libraries : ${CUDA_LIBRARIES}") endif () + ################################################################################ # Compiler specific configuration ################################################################################ @@ -53,10 +59,9 @@ endif () if (CUDA_FOUND) if (CUDA_ENABLED) add_definitions("-DCUDA_ENABLED") - - include(${CMAKE_CURRENT_SOURCE_DIR}/SelectCudaComputeArch.cmake) - - CUDA_SELECT_NVCC_ARCH_FLAGS(CUDA_ARCH_FLAGS ${CUDA_ARCHS}) + if(NOT DEFINED CMAKE_CUDA_ARCHITECTURES) + set(CMAKE_CUDA_ARCHITECTURES "native") + endif() set(CUDA_NVCC_FLAGS "${CUDA_NVCC_FLAGS} ${CUDA_ARCH_FLAGS}") set(CUDA_NVCC_FLAGS "${CUDA_NVCC_FLAGS} -D_FORCE_INLINES") @@ -67,7 +72,8 @@ if (CUDA_FOUND) add_definitions("-D_MWAITXINTRIN_H_INCLUDED") message(STATUS "Enabling CUDA support (version: ${CUDA_VERSION_STRING}," - " archs: ${CUDA_ARCH_FLAGS_readable})") + " archs: ${CMAKE_CUDA_ARCHITECTURES})") + else () set(CUDA_FOUND OFF) message(STATUS "Disabling CUDA support") @@ -98,6 +104,7 @@ add_library(xrsfm src/base/map.cc src/feature/sift_extractor.cc + src/feature/feature_processor.cc src/feature/feature_extraction.cc src/feature/feature_processing.cc src/feature/match_expansion.cc @@ -108,49 +115,62 @@ add_library(xrsfm src/geometry/pnp.cc src/geometry/essential.cc - src/geometry/triangluate_svd.cc + src/geometry/triangulate_light.cc src/geometry/track_processor.cc - src/geometry/colmap/base/triangulation.cc - src/geometry/colmap/estimators/absolute_pose.cc - src/geometry/colmap/estimators/triangulation.cc - src/geometry/colmap/estimators/polynomial.cc - src/geometry/colmap/estimators/fundamental_matrix.cc - src/geometry/colmap/estimators/utils.cc - src/geometry/colmap/util/random.cc - src/geometry/colmap/optim/random_sampler.cc - src/geometry/colmap/optim/combination_sampler.cc - src/geometry/colmap/optim/support_measurement.cc - src/optimization/ba_solver.cc src/mapper/incremental_mapper.cc src/utility/io_ecim.cc + + src/colmap/estimators/absolute_pose.cc + src/colmap/estimators/polynomial.cc + src/colmap/estimators/fundamental_matrix.cc + src/colmap/ransac/random_sampler.cc + src/colmap/ransac/combination_sampler.cc + src/colmap/ransac/support_measurement.cc + src/colmap/ransac/random.cc + src/colmap/ui/point_painter.cc + src/colmap/ui/project_widget.cc + src/colmap/ui/model_viewer_widget.cc + src/colmap/ui/resources.qrc ) + +qt5_add_resources(RESOURCES src/colmap/ui/resources.qrc) + target_include_directories(xrsfm PUBLIC ${CMAKE_SOURCE_DIR} ${CMAKE_SOURCE_DIR}/src - ${CMAKE_SOURCE_DIR}/src/geometry/colmap + ${CMAKE_SOURCE_DIR}/src/colmap ${CMAKE_SOURCE_DIR}/3rdparty/SiftGPU ${CMAKE_SOURCE_DIR}/3rdparty/apriltag ${EIGEN3_INCLUDE_DIRS} + ${Qt5Core_INCLUDE_DIRS} + ${Qt5OpenGL_INCLUDE_DIRS} + ${Qt5Widgets_INCLUDE_DIRS} ) if(XRPRIMER_ENABLED) target_link_libraries(xrsfm stdc++fs + sift_gpu XRPrimer::xrprimer ${OpenCV_LIBS} ${GTEST_LIBRARIES} - sift_gpu + ${Qt5Core_LIBRARIES} + ${Qt5OpenGL_LIBRARIES} + ${Qt5Widgets_LIBRARIES} ) else () target_link_libraries(xrsfm stdc++fs + sift_gpu ${OpenCV_LIBS} ${CERES_LIBRARIES} - sift_gpu + ${Qt5Core_LIBRARIES} + ${Qt5OpenGL_LIBRARIES} + ${Qt5Widgets_LIBRARIES} ) endif() @@ -174,3 +194,7 @@ target_link_libraries(rec_1dsfm xrsfm) add_executable(rec_kitti src/rec_kitti.cc) target_link_libraries(rec_kitti xrsfm) + +add_executable(xrsfm_gui src/xrsfm_gui.cc) +target_link_libraries(xrsfm_gui xrsfm) +target_sources(xrsfm_gui PRIVATE ${RESOURCES}) diff --git a/Dockerfile b/Dockerfile index 5d6559c..18091a1 100644 --- a/Dockerfile +++ b/Dockerfile @@ -1,4 +1,4 @@ -FROM nvidia/cuda:10.2-devel-ubuntu18.04 +FROM nvidia/cuda:11.6.1-devel-ubuntu20.04 ENV DEBIAN_FRONTEND=noninteractive @@ -14,6 +14,11 @@ RUN apt-get install -y \ python3-pip \ libglew-dev \ libatlas-base-dev \ - libgtest-dev + libgtest-dev \ + qtbase5-dev \ + libqt5opengl5-dev \ + libeigen3-dev \ + libceres-dev \ + libopencv-dev Run pip3 install numpy diff --git a/SelectCudaComputeArch.cmake b/SelectCudaComputeArch.cmake deleted file mode 100755 index 5eedb05..0000000 --- a/SelectCudaComputeArch.cmake +++ /dev/null @@ -1,294 +0,0 @@ -# CMake - Cross Platform Makefile Generator -# Copyright 2000-2017 Kitware, Inc. and Contributors -# 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 Kitware, Inc. nor the names of 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 -# HOLDER 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. - -# Synopsis: -# CUDA_SELECT_NVCC_ARCH_FLAGS(out_variable [target_CUDA_architectures]) -# -- Selects GPU arch flags for nvcc based on target_CUDA_architectures -# target_CUDA_architectures : Auto | Common | All | LIST(ARCH_AND_PTX ...) -# - "Auto" detects local machine GPU compute arch at runtime. -# - "Common" and "All" cover common and entire subsets of architectures -# ARCH_AND_PTX : NAME | NUM.NUM | NUM.NUM(NUM.NUM) | NUM.NUM+PTX -# NAME: Fermi Kepler Maxwell Kepler+Tegra Kepler+Tesla Maxwell+Tegra Pascal -# NUM: Any number. Only those pairs are currently accepted by NVCC though: -# 2.0 2.1 3.0 3.2 3.5 3.7 5.0 5.2 5.3 6.0 6.2 -# Returns LIST of flags to be added to CUDA_NVCC_FLAGS in ${out_variable} -# Additionally, sets ${out_variable}_readable to the resulting numeric list -# Example: -# CUDA_SELECT_NVCC_ARCH_FLAGS(ARCH_FLAGS 3.0 3.5+PTX 5.2(5.0) Maxwell) -# LIST(APPEND CUDA_NVCC_FLAGS ${ARCH_FLAGS}) -# -# More info on CUDA architectures: https://en.wikipedia.org/wiki/CUDA -# - -if(CMAKE_CUDA_COMPILER_LOADED) # CUDA as a language - if(CMAKE_CUDA_COMPILER_ID STREQUAL "NVIDIA") - set(CUDA_VERSION "${CMAKE_CUDA_COMPILER_VERSION}") - endif() -endif() - -# See: https://docs.nvidia.com/cuda/cuda-compiler-driver-nvcc/index.html#gpu-feature-list - -# This list will be used for CUDA_ARCH_NAME = All option -set(CUDA_KNOWN_GPU_ARCHITECTURES "") - -# CUDA 9.X and later do not support the Fermi architecture anymore. -if(CUDA_VERSION VERSION_LESS "9.0") - list(APPEND CUDA_KNOWN_GPU_ARCHITECTURES "Fermi") -endif() -list(APPEND CUDA_KNOWN_GPU_ARCHITECTURES "Kepler" "Maxwell") - -# This list will be used for CUDA_ARCH_NAME = Common option (enabled by default) -# set(CUDA_COMMON_GPU_ARCHITECTURES "3.0" "3.5" "5.0") - -if(CUDA_VERSION VERSION_LESS "7.0") - set(CUDA_LIMIT_GPU_ARCHITECTURE "5.2") -endif() - -# This list is used to filter CUDA archs when autodetecting -set(CUDA_ALL_GPU_ARCHITECTURES "3.0" "3.2" "3.5" "5.0") - -if(CUDA_VERSION VERSION_GREATER "7.0" OR CUDA_VERSION VERSION_EQUAL "7.0") - list(APPEND CUDA_KNOWN_GPU_ARCHITECTURES "Kepler+Tegra" "Kepler+Tesla" "Maxwell+Tegra") - list(APPEND CUDA_COMMON_GPU_ARCHITECTURES "5.2") - - if(CUDA_VERSION VERSION_LESS "8.0") - list(APPEND CUDA_COMMON_GPU_ARCHITECTURES "5.2+PTX") - set(CUDA_LIMIT_GPU_ARCHITECTURE "6.0") - endif() -endif() - -if(CUDA_VERSION VERSION_GREATER "8.0" OR CUDA_VERSION VERSION_EQUAL "8.0") - list(APPEND CUDA_KNOWN_GPU_ARCHITECTURES "Pascal") - list(APPEND CUDA_COMMON_GPU_ARCHITECTURES "6.0" "6.1") - list(APPEND CUDA_ALL_GPU_ARCHITECTURES "6.0" "6.1" "6.2") - - if(CUDA_VERSION VERSION_LESS "9.0") - list(APPEND CUDA_COMMON_GPU_ARCHITECTURES "6.1+PTX") - set(CUDA_LIMIT_GPU_ARCHITECTURE "7.0") - endif() -endif() - -if(CUDA_VERSION VERSION_GREATER "9.0" OR CUDA_VERSION VERSION_EQUAL "9.0") - list(APPEND CUDA_KNOWN_GPU_ARCHITECTURES "Volta") - list(APPEND CUDA_COMMON_GPU_ARCHITECTURES "7.0" "7.0+PTX") - - if(CUDA_VERSION VERSION_LESS "10.0") - set(CUDA_LIMIT_GPU_ARCHITECTURE "8.0") - endif() -endif() - -################################################################################################ -# A function for automatic detection of GPUs installed (if autodetection is enabled) -# Usage: -# CUDA_DETECT_INSTALLED_GPUS(OUT_VARIABLE) -# -function(CUDA_DETECT_INSTALLED_GPUS OUT_VARIABLE) - if(NOT CUDA_GPU_DETECT_OUTPUT) - if(CMAKE_CUDA_COMPILER_LOADED) # CUDA as a language - set(file "${PROJECT_BINARY_DIR}/detect_cuda_compute_capabilities.cu") - else() - set(file "${PROJECT_BINARY_DIR}/detect_cuda_compute_capabilities.cpp") - endif() - - file(WRITE ${file} "" - "#include \n" - "#include \n" - "int main()\n" - "{\n" - " int count = 0;\n" - " if (cudaSuccess != cudaGetDeviceCount(&count)) return -1;\n" - " if (count == 0) return -1;\n" - " for (int device = 0; device < count; ++device)\n" - " {\n" - " cudaDeviceProp prop;\n" - " if (cudaSuccess == cudaGetDeviceProperties(&prop, device))\n" - " std::printf(\"%d.%d \", prop.major, prop.minor);\n" - " }\n" - " return 0;\n" - "}\n") - - if(CMAKE_CUDA_COMPILER_LOADED) # CUDA as a language - try_run(run_result compile_result ${PROJECT_BINARY_DIR} ${file} - RUN_OUTPUT_VARIABLE compute_capabilities) - else() - try_run(run_result compile_result ${PROJECT_BINARY_DIR} ${file} - CMAKE_FLAGS "-DINCLUDE_DIRECTORIES=${CUDA_INCLUDE_DIRS}" - LINK_LIBRARIES ${CUDA_LIBRARIES} - RUN_OUTPUT_VARIABLE compute_capabilities) - endif() - - if(run_result EQUAL 0) - string(REPLACE "2.1" "2.1(2.0)" compute_capabilities "${compute_capabilities}") - set(CUDA_GPU_DETECT_OUTPUT ${compute_capabilities} - CACHE INTERNAL "Returned GPU architectures from detect_gpus tool" FORCE) - endif() - endif() - - if(NOT CUDA_GPU_DETECT_OUTPUT) - message(STATUS "Automatic GPU detection failed. Building for common architectures.") - set(${OUT_VARIABLE} ${CUDA_COMMON_GPU_ARCHITECTURES} PARENT_SCOPE) - else() - # Filter based on CUDA version supported archs - set(CUDA_GPU_DETECT_OUTPUT_FILTERED "") - separate_arguments(CUDA_GPU_DETECT_OUTPUT) - foreach(ITEM IN ITEMS ${CUDA_GPU_DETECT_OUTPUT}) - if(CUDA_LIMIT_GPU_ARCHITECTURE) - if(ITEM VERSION_GREATER CUDA_LIMIT_GPU_ARCHITECTURE OR ITEM VERSION_EQUAL CUDA_LIMIT_GPU_ARCHITECTURE) - list(GET CUDA_COMMON_GPU_ARCHITECTURES -1 NEWITEM) - string(APPEND CUDA_GPU_DETECT_OUTPUT_FILTERED " ${NEWITEM}") - else() - string(APPEND CUDA_GPU_DETECT_OUTPUT_FILTERED " ${ITEM}") - endif() - else() - string(APPEND CUDA_GPU_DETECT_OUTPUT_FILTERED " ${ITEM}") - endif() - endforeach() - - set(${OUT_VARIABLE} ${CUDA_GPU_DETECT_OUTPUT_FILTERED} PARENT_SCOPE) - endif() -endfunction() - - -################################################################################################ -# Function for selecting GPU arch flags for nvcc based on CUDA architectures from parameter list -# Usage: -# SELECT_NVCC_ARCH_FLAGS(out_variable [list of CUDA compute archs]) -function(CUDA_SELECT_NVCC_ARCH_FLAGS out_variable) - set(CUDA_ARCH_LIST "${ARGN}") - - if("X${CUDA_ARCH_LIST}" STREQUAL "X" ) - set(CUDA_ARCH_LIST "Auto") - endif() - - set(cuda_arch_bin) - set(cuda_arch_ptx) - - if("${CUDA_ARCH_LIST}" STREQUAL "All") - set(CUDA_ARCH_LIST ${CUDA_KNOWN_GPU_ARCHITECTURES}) - elseif("${CUDA_ARCH_LIST}" STREQUAL "Common") - set(CUDA_ARCH_LIST ${CUDA_COMMON_GPU_ARCHITECTURES}) - elseif("${CUDA_ARCH_LIST}" STREQUAL "Auto") - CUDA_DETECT_INSTALLED_GPUS(CUDA_ARCH_LIST) - message(STATUS "Autodetected CUDA architecture(s): ${CUDA_ARCH_LIST}") - endif() - - # Now process the list and look for names - string(REGEX REPLACE "[ \t]+" ";" CUDA_ARCH_LIST "${CUDA_ARCH_LIST}") - list(REMOVE_DUPLICATES CUDA_ARCH_LIST) - foreach(arch_name ${CUDA_ARCH_LIST}) - set(arch_bin) - set(arch_ptx) - set(add_ptx FALSE) - # Check to see if we are compiling PTX - if(arch_name MATCHES "(.*)\\+PTX$") - set(add_ptx TRUE) - set(arch_name ${CMAKE_MATCH_1}) - endif() - if(arch_name MATCHES "^([0-9]\\.[0-9](\\([0-9]\\.[0-9]\\))?)$") - set(arch_bin ${CMAKE_MATCH_1}) - set(arch_ptx ${arch_bin}) - else() - # Look for it in our list of known architectures - if(${arch_name} STREQUAL "Fermi") - set(arch_bin 2.0 "2.1(2.0)") - elseif(${arch_name} STREQUAL "Kepler+Tegra") - set(arch_bin 3.2) - elseif(${arch_name} STREQUAL "Kepler+Tesla") - set(arch_bin 3.7) - elseif(${arch_name} STREQUAL "Kepler") - set(arch_bin 3.0 3.5) - set(arch_ptx 3.5) - elseif(${arch_name} STREQUAL "Maxwell+Tegra") - set(arch_bin 5.3) - elseif(${arch_name} STREQUAL "Maxwell") - set(arch_bin 5.0 5.2) - set(arch_ptx 5.2) - elseif(${arch_name} STREQUAL "Pascal") - set(arch_bin 6.0 6.1) - set(arch_ptx 6.1) - elseif(${arch_name} STREQUAL "Volta") - set(arch_bin 7.0 7.0) - set(arch_ptx 7.0) - else() - message(SEND_ERROR "Unknown CUDA Architecture Name ${arch_name} in CUDA_SELECT_NVCC_ARCH_FLAGS") - endif() - endif() - if(NOT arch_bin) - message(SEND_ERROR "arch_bin wasn't set for some reason") - endif() - list(APPEND cuda_arch_bin ${arch_bin}) - if(add_ptx) - if (NOT arch_ptx) - set(arch_ptx ${arch_bin}) - endif() - list(APPEND cuda_arch_ptx ${arch_ptx}) - endif() - endforeach() - - # remove dots and convert to lists - string(REGEX REPLACE "\\." "" cuda_arch_bin "${cuda_arch_bin}") - string(REGEX REPLACE "\\." "" cuda_arch_ptx "${cuda_arch_ptx}") - string(REGEX MATCHALL "[0-9()]+" cuda_arch_bin "${cuda_arch_bin}") - string(REGEX MATCHALL "[0-9]+" cuda_arch_ptx "${cuda_arch_ptx}") - - if(cuda_arch_bin) - list(REMOVE_DUPLICATES cuda_arch_bin) - endif() - if(cuda_arch_ptx) - list(REMOVE_DUPLICATES cuda_arch_ptx) - endif() - - set(nvcc_flags "") - set(nvcc_archs_readable "") - - # Tell NVCC to add binaries for the specified GPUs - foreach(arch ${cuda_arch_bin}) - if(arch MATCHES "([0-9]+)\\(([0-9]+)\\)") - # User explicitly specified ARCH for the concrete CODE - list(APPEND nvcc_flags -gencode arch=compute_${CMAKE_MATCH_2},code=sm_${CMAKE_MATCH_1}) - list(APPEND nvcc_archs_readable sm_${CMAKE_MATCH_1}) - else() - # User didn't explicitly specify ARCH for the concrete CODE, we assume ARCH=CODE - list(APPEND nvcc_flags -gencode arch=compute_${arch},code=sm_${arch}) - list(APPEND nvcc_archs_readable sm_${arch}) - endif() - endforeach() - - # Tell NVCC to add PTX intermediate code for the specified architectures - foreach(arch ${cuda_arch_ptx}) - list(APPEND nvcc_flags -gencode arch=compute_${arch},code=compute_${arch}) - list(APPEND nvcc_archs_readable compute_${arch}) - endforeach() - - string(REPLACE ";" " " nvcc_archs_readable "${nvcc_archs_readable}") - set(${out_variable} ${nvcc_flags} PARENT_SCOPE) - set(${out_variable}_readable ${nvcc_archs_readable} PARENT_SCOPE) -endfunction() diff --git a/config_mat.json b/config_mat.json deleted file mode 100644 index 679d12a..0000000 --- a/config_mat.json +++ /dev/null @@ -1,6 +0,0 @@ -{ - "image_dir_path": "/path/images/", - "retrieval_path": "/path/retrieval.txt", - "matching_type": "sequential", - "output_path": "/output_path/" -} diff --git a/config_seq.json b/config_seq.json deleted file mode 100644 index c678e48..0000000 --- a/config_seq.json +++ /dev/null @@ -1,5 +0,0 @@ -{ - "bin_path": "/matching_output_path/", - "camera_path": "/path/camera.txt", - "output_path": "/output_path/" -} diff --git a/config_tri.json b/config_tri.json deleted file mode 100644 index 09e9453..0000000 --- a/config_tri.json +++ /dev/null @@ -1,6 +0,0 @@ -{ - "bin_path": "/path/refine/images.bin", - "feature_path": "/path/features.bin", - "matches_path": "/path/matches.bin", - "output_path": "/output_path/" -} diff --git a/docs/en/installation.md b/docs/en/installation.md index 1a9226c..6ddf61f 100644 --- a/docs/en/installation.md +++ b/docs/en/installation.md @@ -23,7 +23,12 @@ Dependencies from the default Ubuntu repositories: libatlas-base-dev \ libgtest-dev \ libgflags-dev \ - libgoogle-glog-dev + libgoogle-glog-dev \ + qtbase5-dev \ + libqt5opengl5-dev \ + libeigen3-dev \ + libceres-dev \ + libopencv-dev ``` If the version of CMake is less than 3.16, update it. @@ -34,39 +39,12 @@ cp -r cmake-3.21.0-linux-x86_64 /usr/share/ ln -sf /usr/share/cmake-3.21.0-linux-x86_64/bin/cmake /usr/bin/cmake ``` -Install [xrprimer](https://github.com/openxrlab/xrprimer) -```shell -git clone git@github.com:openxrlab/xrprimer.git -cd xrprimer -git checkout xrslam-opencv3.4.7 -cmake -S. -Bbuild -DBUILD_EXTERNAL=ON -DCMAKE_BUILD_TYPE=Release -DENABLE_PRECOMPILED_HEADERS=OFF -cmake --build build --target install -j4 -``` - -Ensure that the root directories of xrsfm and xrprimer remain the same. - -``` -xrprimer -├── -... -xrsfm -├── docs -├── scripts -├── src -... -``` - Compile xrsfm ```shell -git clone git@github.com:openxrlab/xrsfm.git +git clone https://github.com/openxrlab/xrsfm.git cd xrsfm && cmake -B build && cmake --build build -j4 ``` -Note: If you encounter difficulties during the compilation of xrprimer, you can try installing xrsfm without xrprimer. This requires OpenCV and Ceres-Solver. Then you can install xrsfm using the following command: -```shell -cd xrsfm && cmake -B build -DXRPRIMER_ENABLED=OFF && cmake --build build -j4 -``` - ### Dockerfile We provide a [Dockerfile](../../Dockerfile) to build an image. Ensure that you are using [docker version](https://docs.docker.com/engine/install/) >=19.03 and `"default-runtime": "nvidia"` in daemon.json. @@ -78,5 +56,12 @@ docker build -t xrsfm . Run it with ```shell -docker run --gpus all --network=host -it xrsfm +docker run --name xrsfm-container --gpus all --network=host -it xrsfm +``` + +Compile xrsfm + +```shell +git clone https://github.com/openxrlab/xrsfm.git +cd xrsfm && cmake -B build && cmake --build build -j4 ``` diff --git a/docs/en/tutorial.md b/docs/en/tutorial.md index 7393b10..fda2899 100644 --- a/docs/en/tutorial.md +++ b/docs/en/tutorial.md @@ -51,7 +51,7 @@ Run reconsturction stage with the following command line "init_id1" and "init_id2" indicates two frames for the initialization of SfM. In general, these two frames can be set to 0 and 5. -The format of output binary files is is consistent with COLMAP, you can use colmap gui to view the reconstruction result. +You can view the reconstruction results using ```./bin/xrsfm_gui```. The file format is the same as the well-known open-source project COLMAP. You can also use COLMAP's graphical user interface to view the reconstruction results of this project. ### 3. Estimate scale with apriltag (Optional) Input: images, reconstruction results diff --git a/docs/zh/installation.md b/docs/zh/installation.md index 022ad4a..4f6ae74 100644 --- a/docs/zh/installation.md +++ b/docs/zh/installation.md @@ -4,7 +4,7 @@ ## 从源码安装 (Linux) -版本要求 +版本要求: + C++17 + GCC 7.5+ + CMake 3.16+ @@ -23,10 +23,15 @@ libatlas-base-dev \ libgtest-dev \ libgflags-dev \ - libgoogle-glog-dev + libgoogle-glog-dev \ + qtbase5-dev \ + libqt5opengl5-dev \ + libeigen3-dev \ + libceres-dev \ + libopencv-dev ``` -如果cmake版本小于3.16，按照以下步骤更新 +如果cmake版本小于3.16，按照以下步骤更新: ```shell wget https://cmake.org/files/v3.21/cmake-3.21.0-linux-x86_64.tar.gz tar -xf cmake-3.21.0-linux-x86_64.tar.gz @@ -34,49 +39,27 @@ cp -r cmake-3.21.0-linux-x86_64 /usr/share/ ln -sf /usr/share/cmake-3.21.0-linux-x86_64/bin/cmake /usr/bin/cmake ``` -安装 [XRPRimer](https://github.com/openxrlab/xrprimer) +编译 XRSfM: ```shell -git clone git@github.com:openxrlab/xrprimer.git -cd xrprimer -git checkout xrslam-opencv3.4.7 -cmake -S. -Bbuild -DBUILD_EXTERNAL=ON -DCMAKE_BUILD_TYPE=Release -DENABLE_PRECOMPILED_HEADERS=OFF -cmake --build build --target install -j4 -``` - -确保xrsfm和xrprimer的根目录保持一致。 -``` -xrprimer -├── -... -xrsfm -├── docs -├── scripts -├── src -... -``` - - -编译 XRSfM -```shell -git clone git@github.com:openxrlab/xrsfm.git +git clone https://github.com/openxrlab/xrsfm.git cd xrsfm && cmake -B build && cmake --build build -j4 ``` -注意：如果您在编译xrprimer时遇到困难，可以尝试在没有xrprimer的情况下安装xrsfm。这需要OpenCV和Ceres-Solver。然后，您可以使用以下命令安装xrsfm： -```shell -cd xrsfm && cmake -B build -DXRPRIMER_ENABLED=OFF && cmake --build build -j4 -``` +### Dockerfile -### 通过Docker镜像运行 - -We provide a [Dockerfile](../../Dockerfile) to build an image. Ensure that you are using [docker version](https://docs.docker.com/engine/install/) >=19.03 and `"default-runtime": "nvidia"` in daemon.json. +我们提供了[Dockerfile](../../Dockerfile)文件来方便环境配置。 +使用前请确认[docker version](https://docs.docker.com/engine/install/) >=19.03，并且在daemon.json文件中已经设置`"default-runtime": "nvidia"`。 ```shell docker build -t xrsfm . ``` +启动容器: +```shell +docker run --name xrsfm-container --gpus all --network=host -it xrsfm +``` -Run it with - +编译 XRSfM: ```shell -docker run --gpus all --network=host -it xrsfm +git clone https://github.com/openxrlab/xrsfm.git +cd xrsfm && cmake -B build && cmake --build build -j4 ``` diff --git a/docs/zh/tutorial.md b/docs/zh/tutorial.md index 7af3206..d3802cd 100644 --- a/docs/zh/tutorial.md +++ b/docs/zh/tutorial.md @@ -49,7 +49,7 @@ XRSfM支持顺序匹配("sequential")、基于检索的匹配("retrieval")和基 "init_id1" and "init_id2" 是可选的输入参数，指定重建阶段的初始帧，当不输入该参数时，程序将会自行选择初始帧。 输出的重建结果由三部分组成“cameras.bin”,“images.bin”,"points.bin"。 -文件格式与知名的开源工作COLMAP相同，用户可以使用COLMAP的图像用户界面查看本项目的重建结果。 +使用```./bin/xrsfm_gui```可以查看重建结果，文件格式与知名的开源工作COLMAP相同，也可以使用COLMAP的图像用户界面查看本项目的重建结果。 ### 3. 尺度估计（可选） 输入: 图像文件夹，重建结果 diff --git a/src/base/camera.hpp b/src/base/camera.hpp index eb74d90..95ad81a 100644 --- a/src/base/camera.hpp +++ b/src/base/camera.hpp @@ -34,11 +34,10 @@ class Camera { params_.resize(camera_model_param_size(model_id_)); is_valid = true; } - Camera(int _id, double fx, double fy, double cx, double cy, double d) { + Camera(int _id, double fxy, double cx, double cy) { id_ = _id; model_id_ = 2; - params_ = {fx, cx, cy, d}; - std::cout << params_.size() << std::endl; + params_ = {fxy, cx, cy, 0}; is_valid = true; } @@ -86,8 +85,8 @@ inline void ImageToNormalized(const Camera &c, const Eigen::Vector2d &p2d, CAMERA_MODEL_CASES #undef CAMERA_MODEL_CASE - std::cout << "ERROR: (ImageToNormalized) unsupported model type: " - << c.model_id_ << "\n"; + printf("ERROR: (ImageToNormalized) unsupported model type: %d\n", + c.model_id_); exit(-1); } @@ -102,8 +101,8 @@ inline void NormalizedToImage(const Camera &c, const Eigen::Vector2d &p2d_n, CAMERA_MODEL_CASES #undef CAMERA_MODEL_CASE - std::cout << "ERROR: (NormalizedToImage) unsupported model type: " - << c.model_id_ << "\n"; + printf("ERROR: (NormalizedToImage) unsupported model type: %d", + int(c.model_id_)); exit(-1); } diff --git a/src/base/camera_model.hpp b/src/base/camera_model.hpp index 0f06959..cf536e4 100644 --- a/src/base/camera_model.hpp +++ b/src/base/camera_model.hpp @@ -26,7 +26,6 @@ inline void IterativeUndistortion(const double *params, double *xy) { Eigen::Vector2d xy_plus; for (size_t i = 0; i < kNumIterations; ++i) { - // std::cout << i << " " << kNumIterations << std::endl; const double step0 = std::max(std::numeric_limits::epsilon(), std::abs(kRelStepSize * x(0))); const double step1 = std::max(std::numeric_limits::epsilon(), diff --git a/src/base/map.cc b/src/base/map.cc index 709640f..a90d94d 100644 --- a/src/base/map.cc +++ b/src/base/map.cc @@ -27,15 +27,7 @@ int CorrespondenceGraph::GetMatch(int frame_id1, int frame_id2, }; void Map::Init() { - // 0 0 it may bug - assert(frames_.size() < 32768); // 2^15 INT_MAX=2147483647=2^31 - frameid2pairid_.clear(); - for (int i = 0; i < frame_pairs_.size(); ++i) { - const auto &fp = frame_pairs_.at(i); - int idpair = - fp.id1 < fp.id2 ? fp.id1 * 32768 + fp.id2 : fp.id2 * 32768 + fp.id1; - frameid2pairid_[idpair] = i; - } + assert(NumFrames() < 32768); // 2^15 INT_MAX=2147483647=2^31 frameid2framepairids_.clear(); frameid2matched_frameids_.clear(); frameid2covisible_frameids_.clear(); @@ -52,7 +44,7 @@ void Map::Init() { frameid2matched_frameids_.at(fp.id1).emplace_back(fp.id2); frameid2matched_frameids_.at(fp.id2).emplace_back(fp.id1); } - corr_graph_.frame_node_vec_.resize(frames_.size() + 1); // TODO here a bug + corr_graph_.frame_node_vec_.resize(NumFrames() + 1); // TODO here a bug for (const auto &frame : frames_) { corr_graph_.frame_node_vec_.at(frame.id).num_observations = 0; corr_graph_.frame_node_vec_.at(frame.id).num_visible_point3d = 0; @@ -89,12 +81,10 @@ void Map::Init() { void Map::RemoveRedundancyPoints() { Init(); // remove unused frame points - std::vector> id2nid_vec(frames_.size(), - std::vector(0)); - std::vector> nid2id_vec(frames_.size(), - std::vector(0)); - for (int i = 0; i < frames_.size(); ++i) { - auto &frame = frames_.at(i); + std::vector> id2nid_vec(NumFrames(), std::vector(0)); + std::vector> nid2id_vec(NumFrames(), std::vector(0)); + for (int i = 0; i < NumFrames(); ++i) { + auto &frame = this->frame(i); auto &id2nid = id2nid_vec.at(i); auto &nid2id = nid2id_vec.at(i); id2nid.assign(frame.points.size(), -1); @@ -109,11 +99,8 @@ void Map::RemoveRedundancyPoints() { for (int k = 0; k < nid2id.size(); ++k) { frame.points.at(k) = frame.points.at(nid2id.at(k)); - // frame.points_normalized.at(k) = - // frame.points_normalized.at(nid2id.at(k)); } frame.points.resize(nid2id.size()); - // frame.points_normalized.resize(nid2id.size()); frame.track_ids_.assign(nid2id.size(), -1); } for (auto &fp : frame_pairs_) { @@ -131,127 +118,19 @@ int Map::MaxPoint3dFrameId() { for (const auto &frame : frames_) { if (frame.registered || frame.registered_fail) continue; - // skip bad camera in unorder - // if (cameras_[frame.camera_id].distort_params[0] == 0) continue; - // only use sequence image - // if (frame.camera_id != cameras_.back().id) continue; - - // if (bad_focal.count(frame.id) != 0) continue; - int num_p3d = frames_[frame.id].num_visible_points3D_; + int num_p3d = this->frame(frame.id).num_visible_points3D_; if (num_p3d > max_num_p3d) { max_num_p3d = num_p3d; best_id = frame.id; } } - // bool exit_good_neibor = false; - // for (const auto id : frameid2framepairids_[best_id]) { - // auto &fp = frame_pairs_[id]; - // const auto &frame1 = frames_[fp.id1]; - // const auto &frame2 = frames_[fp.id2]; - // if (fp.matches.size() < 100) continue; - // if ((fp.id1 == best_id && frame2.registered) || (fp.id2 == best_id && - // frame1.registered)) { - // exit_good_neibor = true; - // break; - // } - // } - - // for (const auto &frame : frames_) { - // if (frame.registered) continue; - // int num_p3d = 0; - // const auto &corrs_vector = - // corr_graph_.frame_node_vec_[frame.id].corrs_vector; for (const auto - // &corrs : corrs_vector) { - // for (const auto &[t_frame_id, t_p2d_id] : corrs) { - // const auto &t_frame = frames_[t_frame_id]; - // if (!t_frame.registered || t_frame.track_ids_[t_p2d_id] == -1) - // continue; const auto &track = - // tracks_[t_frame.track_ids_[t_p2d_id]]; if (track.outlier) continue; - // num_p3d++; - // break; - // } - // } - // if (num_p3d > max_num_p3d1) { - // max_num_p3d1 = num_p3d; - // best_id1 = frame.id; - // } - // } - - // for (const auto &frame : frames_) { - // if (frame.registered) continue; - // const auto &corrs_vector = - // corr_graph_.frame_node_vec_[frame.id].corrs_vector; for (int i = 0; i < - // corrs_vector.size(); ++i) { - // int num_p3d = 0; - // const auto &corrs = corrs_vector[i]; - // for (const auto &[t_frame_id, t_p2d_id] : corrs) { - // const auto &t_frame = frames_[t_frame_id]; - // if (!t_frame.registered || t_frame.track_ids_[t_p2d_id] == -1) - // continue; const auto &track = - // tracks_[t_frame.track_ids_[t_p2d_id]]; if (track.outlier) continue; - // num_p3d++; - // } - - // if (frame.num_correspondences_have_point3D_[i] != num_p3d) { - // printf("error %d %d %d %d %d\n", frame.id, i, - // frame.num_correspondences_have_point3D_[i], num_p3d); - // CHECK(frame.num_correspondences_have_point3D_[i] == num_p3d); - // } - // } - // } - // CHECK(max_num_p3d1 == frames_[best_id1].num_visible_points3D_); printf("Frame id: %d visible point3d num: %d \n", best_id, max_num_p3d); if (max_num_p3d < 20) return -1; return best_id; } -int Map::get_num_p3d(const int frame_id) { - int num_p3d = 0; - const auto &corrs_vector = - corr_graph_.frame_node_vec_[frame_id].corrs_vector; - for (const auto &corrs : corrs_vector) { - bool visit = false, visit_key = false; - for (const auto &[t_frame_id, t_p2d_id] : corrs) { - const auto &t_frame = frames_[t_frame_id]; - if (!t_frame.registered) - continue; - const int track_id = t_frame.track_ids_[t_p2d_id]; - if (track_id == -1 || tracks_[track_id].outlier) - continue; - visit = true; - num_p3d++; - break; - } - } - return num_p3d; -} - -std::pair Map::MaxPoint3dFrameIdSeq() { - // get seq head and tail - int best_id = -1, max_num_p3d = 0; - size_t num_frames = frames_.size(); - for (int i = 0; i < num_frames; ++i) { - const auto &frame = frames_[i]; - if (frame.registered || frame.registered_fail) - continue; - if ((i - 1 >= 0 && frames_[i - 1].registered && - frames_[i - 1].camera_id == frame.camera_id) || - (i + 1 < num_frames && frames_[i + 1].registered && - frames_[i + 1].camera_id == frame.camera_id)) { - const int num_p3d = get_num_p3d(frame.id); - int num_p3d2 = frame.num_visible_points3D_; - CHECK(num_p3d == num_p3d2) << num_p3d << " " << num_p3d2 << "\n"; - if (num_p3d > max_num_p3d) { - max_num_p3d = num_p3d; - best_id = frame.id; - } - } - } - return std::pair(best_id, max_num_p3d); -} - void Map::SearchCorrespondences(const Frame &frame, std::vector &points2d, std::vector &points3d, @@ -264,15 +143,13 @@ void Map::SearchCorrespondences(const Frame &frame, auto &corrs_vector = corr_graph_.frame_node_vec_[frame.id].corrs_vector; for (int p2d_id = 0; p2d_id < corrs_vector.size(); ++p2d_id) { auto &corrs = corrs_vector[p2d_id]; - for (auto &corr : corrs) { - int t_frame_id = corr.first; - int t_p2d_id = corr.second; - if (!frames_[t_frame_id].registered) + for (auto &[t_frame_id, t_p2d_id] : corrs) { + if (!this->frame(t_frame_id).registered) continue; - int p3d_id = frames_[t_frame_id].track_ids_[t_p2d_id]; - if (p3d_id != -1 && !tracks_[p3d_id].outlier) { + int p3d_id = this->frame(t_frame_id).track_ids_[t_p2d_id]; + if (p3d_id != -1 && !track(p3d_id).outlier) { points2d.emplace_back(frame.points[p2d_id]); - points3d.emplace_back(tracks_[p3d_id].point3d_); + points3d.emplace_back(track(p3d_id).point3d_); cor_2d_3d_ids.emplace_back(std::pair(p2d_id, p3d_id)); break; } @@ -303,15 +180,15 @@ void Map::SearchCorrespondences1( for (auto &corr : corrs) { int t_frame_id = corr.first; int t_p2d_id = corr.second; - if (!frames_[t_frame_id].registered) + if (!this->frame(t_frame_id).registered) continue; if (cor_frame_id.count(t_frame_id) == 0) continue; - int p3d_id = frames_[t_frame_id].track_ids_[t_p2d_id]; - if (p3d_id != -1 && !tracks_[p3d_id].outlier) { + int p3d_id = this->frame(t_frame_id).track_ids_[t_p2d_id]; + if (p3d_id != -1 && !track(p3d_id).outlier) { points2d.emplace_back(frame.points[p2d_id]); - points3d.emplace_back(tracks_[p3d_id].point3d_); + points3d.emplace_back(track(p3d_id).point3d_); cor_2d_3d_ids.emplace_back(std::pair(p2d_id, p3d_id)); break; } @@ -327,36 +204,6 @@ void Map::SearchCorrespondences1( } } -void Map::SearchCorrespondencesOrder( - const Frame &frame, std::vector &points2d, - std::vector &points3d, - std::vector> &cor_2d_3d_ids) { - points2d.clear(); - points3d.clear(); - cor_2d_3d_ids.clear(); - - FramePair frame_pair; - for (auto &t_frame_pair : frame_pairs_) { - if (t_frame_pair.id1 == frame.id - 1 && t_frame_pair.id2 == frame.id) { - frame_pair = t_frame_pair; - break; - } - } - - auto &last_frame = frames_[frame.id - 1]; - for (int i = 0; i < frame_pair.matches.size(); ++i) { - if (!frame_pair.inlier_mask[i]) - continue; - auto &match = frame_pair.matches[i]; - int track_id = last_frame.track_ids_[match.id1]; - if (track_id == -1) - continue; - points2d.emplace_back(frame.points[match.id2]); - points3d.emplace_back(tracks_[track_id].point3d_); - cor_2d_3d_ids.emplace_back(std::pair(match.id2, track_id)); - } -} - bool FindPair(const std::vector &frame_pairs, const int id1, const int id2, FramePair &frame_pair) { for (auto &t_frame_pair : frame_pairs) { @@ -378,13 +225,13 @@ FramePair FindPair(const std::vector &frame_pairs, const int id1, } bool UpdateCovisiblity(Map &map, int frame_id) { - auto &frame = map.frames_[frame_id]; + auto &frame = map.frame(frame_id); // add a covisible edge if two neighbor frame have 10+ covisible p3d std::map id2num_covisible_pt; for (const auto &track_id : frame.track_ids_) { if (track_id == -1) continue; - const auto &track = map.tracks_[track_id]; + const auto &track = map.track(track_id); for (const auto [t_frame_id, t_p2d_id] : track.observations_) { if (id2num_covisible_pt.count(t_frame_id) == 0) { @@ -406,7 +253,7 @@ bool UpdateCovisiblity(Map &map, int frame_id) { if (count_covisibile_images == 0) { map.DeregistrationFrame(frame_id); - map.frames_[frame_id].registered_fail = true; + map.frame(frame_id).registered_fail = true; LOG(ERROR) << "Frame " << frame_id << ": fail to registered , no enough covisibility\n"; return false; @@ -415,10 +262,10 @@ bool UpdateCovisiblity(Map &map, int frame_id) { // update number for registered neighbor frame int num_neighbors_registered = 0; for (const auto &id : map.frameid2matched_frameids_[frame_id]) { - if (!map.frames_[id].registered) + if (!map.frame(id).registered) continue; num_neighbors_registered++; - map.frames_[id].num_neighbors_registered++; + map.frame(id).num_neighbors_registered++; } frame.num_neighbors_registered = num_neighbors_registered; @@ -446,12 +293,12 @@ void KeyFrameSelection(Map &map, std::vector loop_matched_frame_id, for (const auto track_id : frame.track_ids_) { if (track_id == -1) continue; - const auto &track = map.tracks_[track_id]; + const auto &track = map.track(track_id); num_p3d++; int count = 0; for (const auto &[t_frame_id, t_p2d_id] : track.observations_) { - const auto &t_frame = map.frames_[t_frame_id]; + const auto &t_frame = map.frame(t_frame_id); if (t_frame_id != frame.id && t_frame.is_keyframe) count++; } @@ -466,7 +313,7 @@ void KeyFrameSelection(Map &map, std::vector loop_matched_frame_id, std::set id_covisible_key; const auto &id_covisibility = map.frameid2covisible_frameids_[frame.id]; for (const auto &id : id_covisibility) { - if (map.frames_[id].is_keyframe && id != frame.id) + if (map.frame(id).is_keyframe && id != frame.id) id_covisible_key.insert(id); } if (id_covisibility.empty()) @@ -480,10 +327,10 @@ void KeyFrameSelection(Map &map, std::vector loop_matched_frame_id, int id1 = *it, id2 = *next(it); if (id1 < frame.id && id2 > frame.id) { int count = 0; - for (auto &track_id : map.frames_[id1].track_ids_) { + for (auto &track_id : map.frame(id1).track_ids_) { if (track_id == -1) continue; - const auto &track = map.tracks_[track_id]; + const auto &track = map.track(track_id); if (track.observations_.count(id2) == 0) continue; count++; @@ -497,67 +344,6 @@ void KeyFrameSelection(Map &map, std::vector loop_matched_frame_id, continue; } - // step4: keep hierarchical structure - // std::map track2change_level; - // for (const auto track_id : frame.track_ids_) { - // if (track_id == -1) continue; - // const auto &track = map.tracks_[track_id]; - // if (track.hierarchical_level < frame.hierarchical_level) continue; - - // int min_level1 = -1, min_level2 = -1; - // for (const auto &[t_frame_id, t_p2d_id] : track.observations_) { - // const auto &t_frame = map.frames_[t_frame_id]; - // if (t_frame_id == frame.id || !t_frame.is_keyframe) continue; - - // int level = t_frame.hierarchical_level; - // if (min_level1 == -1) { - // min_level1 = level; - // } else if (min_level2 == -1 || level < min_level2) { - // if (level < min_level1) { - // min_level2 = min_level1; - // min_level1 = level; - // } else { - // min_level2 = level; - // } - // if (min_level2 <= track.hierarchical_level) break; - // } - // } - // if (min_level2 > track.hierarchical_level) - // track2change_level[track_id] = min_level2; - // } - - // bool keep_hierarchicy = true; - // for (auto &id : id_covisible_key) { - // auto &cov_frame = map.frames_[id]; - // if (cov_frame.hierarchical_level <= frame.hierarchical_level) - // continue; - - // int count = 0, count1 = 0; - // const int level = cov_frame.hierarchical_level; - // for (auto &track_id : map.frames_[id].track_ids_) { - // if (track_id == -1) continue; - // const auto &track = map.tracks_[track_id]; - // int track_level = track.hierarchical_level; - // if (track_level < level) { - // count1++; - // } - // if (track2change_level.count(track_id) != 0) { - // track_level = track2change_level[track_id]; - // } - // if (track_level < level) { - // count++; - // if (count == MIN_OBS_NUM_LEVEL) break; - // } - // } - // std::cout << frame.id << " " << cov_frame.id << " " << count << " " - // << count1 << std::endl; if (count < MIN_OBS_NUM_LEVEL && count != - // count1) { - // keep_hierarchicy = false; - // break; - // } - // } - // if (!keep_hierarchicy) continue; - frame.is_keyframe = false; printf("!!! init remove: %d %d %d\n", frame.id, num_p3d_redundant, num_p3d); @@ -565,8 +351,8 @@ void KeyFrameSelection(Map &map, std::vector loop_matched_frame_id, for (auto &frame_id : loop_matched_frame_id) { std::cout << "|" << frame_id << std::endl; - map.frames_[frame_id].ref_id = -1; // fix bug - map.frames_[frame_id].is_keyframe = true; + map.frame(frame_id).ref_id = -1; // fix bug + map.frame(frame_id).is_keyframe = true; } for (auto &frame : map.frames_) { @@ -581,11 +367,11 @@ void KeyFrameSelection(Map &map, std::vector loop_matched_frame_id, for (const auto &track_id : frame.track_ids_) { if (track_id == -1) continue; - const auto &track = map.tracks_[track_id]; + const auto &track = map.track(track_id); if (track.outlier) continue; for (const auto &[t_frame_id, t_p2d_id] : track.observations_) { - if (map.frames_[t_frame_id].is_keyframe && + if (map.frame(t_frame_id).is_keyframe && t_frame_id != frame.id) { // DIF(ORBSLAM use scalelevel) if (covisiblity.count(t_frame_id) == 0) covisiblity[t_frame_id] = 1; @@ -598,15 +384,15 @@ void KeyFrameSelection(Map &map, std::vector loop_matched_frame_id, if (covisiblity.empty()) { // todo fix it LOG(ERROR) << "no covisiblity key frame"; - for (int i = 1; i < map.frames_.size(); ++i) { - if (frame.id + i < map.frames_.size()) { - if (map.frames_[frame.id + i].is_keyframe) { + for (int i = 1; i < map.NumFrames(); ++i) { + if (frame.id + i < map.NumFrames()) { + if (map.frame(frame.id + i).is_keyframe) { frame.ref_id = frame.id + i; break; } } if (frame.id - i > 0) { - if (map.frames_[frame.id - i].is_keyframe) { + if (map.frame(frame.id - i).is_keyframe) { frame.ref_id = frame.id - i; break; } @@ -631,7 +417,7 @@ void KeyFrameSelection(Map &map, std::vector loop_matched_frame_id, continue; int count = 0; for (const auto &[t_frame_id, t_p2d_id] : track.observations_) { - if (map.frames_[t_frame_id].is_keyframe) { + if (map.frame(t_frame_id).is_keyframe) { count++; } } @@ -645,10 +431,10 @@ void UpdateByRefFrame(Map &map) { continue; if (frame.is_keyframe) continue; - Frame *ref_frame = &map.frames_[frame.ref_id]; + Frame *ref_frame = &map.frame(frame.ref_id); int count = 0; while (!ref_frame->is_keyframe) { - ref_frame = &map.frames_[ref_frame->ref_id]; + ref_frame = &map.frame(ref_frame->ref_id); count++; if (count > 100) { LOG(ERROR) << "too many loop " << ref_frame->id << " " @@ -663,14 +449,14 @@ void UpdateByRefFrame(Map &map) { } void Map::DeregistrationFrame(int frame_id) { - auto &frame = frames_[frame_id]; + auto &frame = this->frame(frame_id); frame.registered = false; for (int i = 0; i < frame.track_ids_.size(); ++i) { auto &id = frame.track_ids_[i]; // for (int &id : frame.track_ids_) { if (id == -1) continue; - auto &track = tracks_[id]; + auto &track = this->track(id); if (track.outlier) continue; track.observations_.erase(frame.id); diff --git a/src/base/map.h b/src/base/map.h index 834de60..5822906 100644 --- a/src/base/map.h +++ b/src/base/map.h @@ -45,7 +45,6 @@ class Frame { // reconstruction std::vector points; - std::vector points_normalized; std::vector track_ids_; Pose Tcw, tcw_old; @@ -117,14 +116,12 @@ class Map { public: std::vector tracks_; std::vector frames_; - std::vector frame_pairs_; std::map camera_map_; std::map frame_map_; std::map track_map_; + std::vector frame_pairs_; - // for build - std::unordered_map frameid2pairid_; std::unordered_map> frameid2framepairids_; std::unordered_map> frameid2matched_frameids_; std::unordered_map> frameid2covisible_frameids_; @@ -155,39 +152,40 @@ class Map { std::vector> &cor_2d_3d_ids, const bool use_p2d_normalized); - void - SearchCorrespondencesOrder(const Frame &frame, - std::vector &points2d, - std::vector &points3d, - std::vector> &cor_2d_3d_ids); - inline const class Camera &Camera(int camera_id) const { return camera_map_.at(camera_id); }; inline class Camera &Camera(int camera_id) { return camera_map_.at(camera_id); }; + inline const class Frame &frame(int frame_id) const { + return frames_.at(frame_id); + }; + inline class Frame &frame(int frame_id) { return frames_.at(frame_id); }; + inline const class Track &track(int track_id) const { + return tracks_.at(track_id); + }; + inline class Track &track(int track_id) { return tracks_.at(track_id); }; + inline int NumFrames() { return frames_.size(); }; int MaxPoint3dFrameId(); - int get_num_p3d(const int frame_id); - std::pair MaxPoint3dFrameIdSeq(); inline void AddNumCorHavePoint3D(int frame_id, int p2d_id) { for (const auto &[t_frame_id, t_p2d_id] : corr_graph_.frame_node_vec_[frame_id].corrs_vector[p2d_id]) { - frames_[t_frame_id].AddNumCorHavePoint3D(t_p2d_id); + frame(t_frame_id).AddNumCorHavePoint3D(t_p2d_id); } } inline void DeleteNumCorHavePoint3D(int frame_id, int p2d_id) { for (const auto &[t_frame_id, t_p2d_id] : corr_graph_.frame_node_vec_[frame_id].corrs_vector[p2d_id]) { - frames_[t_frame_id].DeleteNumCorHavePoint3D(t_p2d_id); + frame(t_frame_id).DeleteNumCorHavePoint3D(t_p2d_id); } } inline vector2 GetNormalizedPoint(const int frame_id, const int p2d_id) { Eigen::Vector2d ptn; - const auto &frame = frames_.at(frame_id); + const auto &frame = this->frame(frame_id); ImageToNormalized(Camera(frame.camera_id), frame.points.at(p2d_id), ptn); return ptn; @@ -215,6 +213,6 @@ void KeyFrameSelection(Map &map, std::vector loop_matched_frame_id, void UpdateByRefFrame(Map &map); -bool CheckMeaNumber(Map &map, int frame_id); } // namespace xrsfm + #endif // XRSFM_SRC_BASE_MAP_H diff --git a/src/geometry/colmap/estimators/absolute_pose.cc b/src/colmap/estimators/absolute_pose.cc similarity index 92% rename from src/geometry/colmap/estimators/absolute_pose.cc rename to src/colmap/estimators/absolute_pose.cc index 6765f10..9f80b61 100644 --- a/src/geometry/colmap/estimators/absolute_pose.cc +++ b/src/colmap/estimators/absolute_pose.cc @@ -32,14 +32,64 @@ #include "absolute_pose.h" #include +#include -#include "geometry/colmap/util/logging.h" #include "polynomial.h" -#include "utils.h" namespace colmap { namespace { +void ComputeSquaredReprojectionError( + const std::vector &points2D, + const std::vector &points3D, + const Eigen::Matrix3x4d &proj_matrix, std::vector *residuals) { + CHECK_EQ(points2D.size(), points3D.size()); + + residuals->resize(points2D.size()); + + // Note that this code might not be as nice as Eigen expressions, + // but it is significantly faster in various tests. + + const double P_00 = proj_matrix(0, 0); + const double P_01 = proj_matrix(0, 1); + const double P_02 = proj_matrix(0, 2); + const double P_03 = proj_matrix(0, 3); + const double P_10 = proj_matrix(1, 0); + const double P_11 = proj_matrix(1, 1); + const double P_12 = proj_matrix(1, 2); + const double P_13 = proj_matrix(1, 3); + const double P_20 = proj_matrix(2, 0); + const double P_21 = proj_matrix(2, 1); + const double P_22 = proj_matrix(2, 2); + const double P_23 = proj_matrix(2, 3); + + for (size_t i = 0; i < points2D.size(); ++i) { + const double X_0 = points3D[i](0); + const double X_1 = points3D[i](1); + const double X_2 = points3D[i](2); + + // Project 3D point from world to camera. + const double px_2 = P_20 * X_0 + P_21 * X_1 + P_22 * X_2 + P_23; + + // Check if 3D point is in front of camera. + if (px_2 > std::numeric_limits::epsilon()) { + const double px_0 = P_00 * X_0 + P_01 * X_1 + P_02 * X_2 + P_03; + const double px_1 = P_10 * X_0 + P_11 * X_1 + P_12 * X_2 + P_13; + + const double x_0 = points2D[i](0); + const double x_1 = points2D[i](1); + + const double inv_px_2 = 1.0 / px_2; + const double dx_0 = x_0 - px_0 * inv_px_2; + const double dx_1 = x_1 - px_1 * inv_px_2; + + (*residuals)[i] = dx_0 * dx_0 + dx_1 * dx_1; + } else { + (*residuals)[i] = std::numeric_limits::max(); + } + } +} + Eigen::Vector3d LiftImagePoint(const Eigen::Vector2d &point) { return point.homogeneous() / std::sqrt(point.squaredNorm() + 1); } diff --git a/src/geometry/colmap/estimators/absolute_pose.h b/src/colmap/estimators/absolute_pose.h similarity index 98% rename from src/geometry/colmap/estimators/absolute_pose.h rename to src/colmap/estimators/absolute_pose.h index bc494f5..538fadf 100644 --- a/src/geometry/colmap/estimators/absolute_pose.h +++ b/src/colmap/estimators/absolute_pose.h @@ -36,8 +36,11 @@ #include #include -//#include "util/alignment.h" -#include "geometry/colmap/util/types.h" +// #include util/types.h" + +namespace Eigen { +typedef Eigen::Matrix Matrix3x4d; +} namespace colmap { diff --git a/src/geometry/colmap/estimators/fundamental_matrix.cc b/src/colmap/estimators/fundamental_matrix.cc similarity index 99% rename from src/geometry/colmap/estimators/fundamental_matrix.cc rename to src/colmap/estimators/fundamental_matrix.cc index a787283..4486526 100644 --- a/src/geometry/colmap/estimators/fundamental_matrix.cc +++ b/src/colmap/estimators/fundamental_matrix.cc @@ -39,8 +39,6 @@ #include #include "estimators/polynomial.h" -#include "estimators/utils.h" -//#include "util/logging.h" namespace colmap { diff --git a/src/geometry/colmap/estimators/fundamental_matrix.h b/src/colmap/estimators/fundamental_matrix.h similarity index 98% rename from src/geometry/colmap/estimators/fundamental_matrix.h rename to src/colmap/estimators/fundamental_matrix.h index 2f19239..24e2f2a 100644 --- a/src/geometry/colmap/estimators/fundamental_matrix.h +++ b/src/colmap/estimators/fundamental_matrix.h @@ -35,10 +35,7 @@ #include #include -//#include "estimators/homography_matrix.h" -//#include "util/alignment.h" -#include "optim/ransac.h" -#include "util/types.h" +#include "ransac/ransac.h" namespace colmap { diff --git a/src/geometry/colmap/estimators/polynomial.cc b/src/colmap/estimators/polynomial.cc similarity index 99% rename from src/geometry/colmap/estimators/polynomial.cc rename to src/colmap/estimators/polynomial.cc index 1a315c8..76ddf18 100644 --- a/src/geometry/colmap/estimators/polynomial.cc +++ b/src/colmap/estimators/polynomial.cc @@ -32,8 +32,7 @@ #include "polynomial.h" #include - -#include "geometry/colmap/util/logging.h" +#include namespace colmap { namespace { diff --git a/src/geometry/colmap/util/math.cc b/src/colmap/estimators/polynomial.h old mode 100755 new mode 100644 similarity index 71% rename from src/geometry/colmap/util/math.cc rename to src/colmap/estimators/polynomial.h index e0db348..2d17f4f --- a/src/geometry/colmap/util/math.cc +++ b/src/colmap/estimators/polynomial.h @@ -29,16 +29,24 @@ // // Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) -#include "math.h" +#ifndef COLMAP_SRC_BASE_POLYNOMIAL_H_ +#define COLMAP_SRC_BASE_POLYNOMIAL_H_ -namespace colmap { +#include -size_t NChooseK(const size_t n, const size_t k) { - if (k == 0) { - return 1; - } +namespace colmap { - return (n * NChooseK(n - 1, k - 1)) / k; -} +// Find the roots of a polynomial using the companion matrix method, based on: +// +// R. A. Horn & C. R. Johnson, Matrix Analysis. Cambridge, +// UK: Cambridge University Press, 1999, pp. 146-7. +// +// Compared to Durand-Kerner, this method is slower but more stable/accurate. +// The real and/or imaginary variable may be NULL if the output is not needed. +bool FindPolynomialRootsCompanionMatrix(const Eigen::VectorXd &coeffs, + Eigen::VectorXd *real, + Eigen::VectorXd *imag); } // namespace colmap + +#endif // COLMAP_SRC_BASE_POLYNOMIAL_H_ diff --git a/src/geometry/colmap/optim/combination_sampler.cc b/src/colmap/ransac/combination_sampler.cc similarity index 97% rename from src/geometry/colmap/optim/combination_sampler.cc rename to src/colmap/ransac/combination_sampler.cc index 4ed8352..d565da7 100644 --- a/src/geometry/colmap/optim/combination_sampler.cc +++ b/src/colmap/ransac/combination_sampler.cc @@ -33,8 +33,8 @@ #include -#include "geometry/colmap/util/math.h" -#include "geometry/colmap/util/random.h" +#include "random.h" +#include "math.h" namespace colmap { diff --git a/src/geometry/colmap/optim/combination_sampler.h b/src/colmap/ransac/combination_sampler.h similarity index 100% rename from src/geometry/colmap/optim/combination_sampler.h rename to src/colmap/ransac/combination_sampler.h diff --git a/src/geometry/colmap/optim/loransac.h b/src/colmap/ransac/loransac.h similarity index 99% rename from src/geometry/colmap/optim/loransac.h rename to src/colmap/ransac/loransac.h index f05beca..c7ab108 100644 --- a/src/geometry/colmap/optim/loransac.h +++ b/src/colmap/ransac/loransac.h @@ -40,8 +40,6 @@ #include "random_sampler.h" #include "ransac.h" #include "support_measurement.h" -//#include "util/alignment.h" -#include "../util/logging.h" namespace colmap { diff --git a/src/geometry/colmap/util/math.h b/src/colmap/ransac/math.h similarity index 98% rename from src/geometry/colmap/util/math.h rename to src/colmap/ransac/math.h index 6cd4754..18e5deb 100644 --- a/src/geometry/colmap/util/math.h +++ b/src/colmap/ransac/math.h @@ -40,8 +40,6 @@ #include #include -#include "logging.h" - #ifndef M_PI #define M_PI 3.14159265358979323846264338327950288 #endif @@ -110,9 +108,6 @@ template T Sigmoid(const T x, const T alpha = 1); // @return The scaled value in the range [0, 1]. template T ScaleSigmoid(T x, const T alpha = 1, const T x0 = 10); -// Binomial coefficient or all combinations, defined as n! / ((n - k)! k!). -size_t NChooseK(const size_t n, const size_t k); - // Cast value from one type to another and truncate instead of overflow, if the // input value is out of range of the output data type. template T2 TruncateCast(const T1 value); diff --git a/src/geometry/colmap/util/random.cc b/src/colmap/ransac/random.cc similarity index 100% rename from src/geometry/colmap/util/random.cc rename to src/colmap/ransac/random.cc diff --git a/src/geometry/colmap/util/random.h b/src/colmap/ransac/random.h similarity index 98% rename from src/geometry/colmap/util/random.h rename to src/colmap/ransac/random.h index 233ee2a..a50a416 100644 --- a/src/geometry/colmap/util/random.h +++ b/src/colmap/ransac/random.h @@ -36,9 +36,6 @@ #include #include -#include "logging.h" -//#include "util/threading.h" - namespace colmap { extern thread_local std::mt19937 *PRNG; diff --git a/src/geometry/colmap/optim/random_sampler.cc b/src/colmap/ransac/random_sampler.cc similarity index 98% rename from src/geometry/colmap/optim/random_sampler.cc rename to src/colmap/ransac/random_sampler.cc index 4368804..7ebb80e 100644 --- a/src/geometry/colmap/optim/random_sampler.cc +++ b/src/colmap/ransac/random_sampler.cc @@ -33,7 +33,7 @@ #include -#include "../util/random.h" +#include "random.h" namespace colmap { diff --git a/src/geometry/colmap/optim/random_sampler.h b/src/colmap/ransac/random_sampler.h similarity index 100% rename from src/geometry/colmap/optim/random_sampler.h rename to src/colmap/ransac/random_sampler.h diff --git a/src/geometry/colmap/optim/ransac.h b/src/colmap/ransac/ransac.h similarity index 99% rename from src/geometry/colmap/optim/ransac.h rename to src/colmap/ransac/ransac.h index d993520..1e2b631 100644 --- a/src/geometry/colmap/optim/ransac.h +++ b/src/colmap/ransac/ransac.h @@ -36,11 +36,10 @@ #include #include #include +#include #include "random_sampler.h" #include "support_measurement.h" -//#include "util/alignment.h" -#include "../util/logging.h" namespace colmap { diff --git a/src/geometry/colmap/optim/sampler.h b/src/colmap/ransac/sampler.h similarity index 99% rename from src/geometry/colmap/optim/sampler.h rename to src/colmap/ransac/sampler.h index 7546a20..2d54d57 100644 --- a/src/geometry/colmap/optim/sampler.h +++ b/src/colmap/ransac/sampler.h @@ -35,7 +35,7 @@ #include #include -#include "../util/logging.h" +#include namespace colmap { diff --git a/src/geometry/colmap/optim/support_measurement.cc b/src/colmap/ransac/support_measurement.cc similarity index 100% rename from src/geometry/colmap/optim/support_measurement.cc rename to src/colmap/ransac/support_measurement.cc diff --git a/src/geometry/colmap/optim/support_measurement.h b/src/colmap/ransac/support_measurement.h similarity index 100% rename from src/geometry/colmap/optim/support_measurement.h rename to src/colmap/ransac/support_measurement.h diff --git a/src/colmap/ui/model_viewer_widget.cc b/src/colmap/ui/model_viewer_widget.cc new file mode 100644 index 0000000..47658a2 --- /dev/null +++ b/src/colmap/ui/model_viewer_widget.cc @@ -0,0 +1,887 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of +// its contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#include +#include "model_viewer_widget.h" + +#define SELECTION_BUFFER_IMAGE_IDX 0 +#define SELECTION_BUFFER_POINT_IDX 1 + +const Eigen::Vector4f kSelectedPointColor(0.0f, 1.0f, 0.0f, 1.0f); + +const Eigen::Vector4f kSelectedImagePlaneColor(1.0f, 0.0f, 1.0f, 0.6f); +const Eigen::Vector4f kSelectedImageFrameColor(0.8f, 0.0f, 0.8f, 1.0f); + +const Eigen::Vector4f kMovieGrabberImagePlaneColor(0.0f, 1.0f, 1.0f, 0.6f); +const Eigen::Vector4f kMovieGrabberImageFrameColor(0.0f, 0.8f, 0.8f, 1.0f); + +const Eigen::Vector4f kGridColor(0.2f, 0.2f, 0.2f, 0.6f); +const Eigen::Vector4f kXAxisColor(0.9f, 0.0f, 0.0f, 0.5f); +const Eigen::Vector4f kYAxisColor(0.0f, 0.9f, 0.0f, 0.5f); +const Eigen::Vector4f kZAxisColor(0.0f, 0.0f, 0.9f, 0.5f); + +namespace xrsfm { +namespace { + +double RadToDeg(const double rad) { + return rad * 57.29577951308232286464772187173366546630859375; +} + +// Generate unique index from RGB color in the range [0, 256^3]. +inline size_t RGBToIndex(const uint8_t r, const uint8_t g, const uint8_t b) { + return static_cast(r) + static_cast(g) * 256 + + static_cast(b) * 65536; +} + +// Derive color from unique index, generated by `RGBToIndex`. +inline Eigen::Vector4f IndexToRGB(const size_t index) { + Eigen::Vector4f color; + color(0) = ((index & 0x000000FF) >> 0) / 255.0f; + color(1) = ((index & 0x0000FF00) >> 8) / 255.0f; + color(2) = ((index & 0x00FF0000) >> 16) / 255.0f; + color(3) = 1.0f; + return color; +} + +void BuildImageModel(const Eigen::Matrix &inv_proj_matrix, + const Camera &camera, const float image_size, + const Eigen::Vector4f &plane_color, + const Eigen::Vector4f &frame_color, + std::vector *triangle_data, + std::vector *line_data) { + // Generate camera dimensions in OpenGL (world) coordinate space. + const float kBaseCameraWidth = 1024.0f; + const float image_width = image_size * camera.width_ / kBaseCameraWidth; + const float image_height = image_width * + static_cast(camera.height_) / + static_cast(camera.width_); + const float image_extent = std::max(image_width, image_height); + const float camera_extent = std::max(camera.width_, camera.height_); + const float camera_extent_world = + static_cast(camera_extent / camera.fx()); + const float focal_length = 2.0f * image_extent / camera_extent_world; + + // const Eigen::Matrix inv_proj_matrix = + // image.InverseProjectionMatrix().cast(); + + // Projection center, top-left, top-right, bottom-right, bottom-left + // corners. + + const Eigen::Vector3f pc = inv_proj_matrix.rightCols<1>(); + const Eigen::Vector3f tl = + inv_proj_matrix * + Eigen::Vector4f(-image_width, image_height, focal_length, 1); + const Eigen::Vector3f tr = + inv_proj_matrix * + Eigen::Vector4f(image_width, image_height, focal_length, 1); + const Eigen::Vector3f br = + inv_proj_matrix * + Eigen::Vector4f(image_width, -image_height, focal_length, 1); + const Eigen::Vector3f bl = + inv_proj_matrix * + Eigen::Vector4f(-image_width, -image_height, focal_length, 1); + + // Image plane as two triangles. + if (triangle_data != nullptr) { + triangle_data->emplace_back( + PointPainter::Data(tl(0), tl(1), tl(2), plane_color(0), + plane_color(1), plane_color(2), plane_color(3)), + PointPainter::Data(tr(0), tr(1), tr(2), plane_color(0), + plane_color(1), plane_color(2), plane_color(3)), + PointPainter::Data(bl(0), bl(1), bl(2), plane_color(0), + plane_color(1), plane_color(2), plane_color(3))); + + triangle_data->emplace_back( + PointPainter::Data(bl(0), bl(1), bl(2), plane_color(0), + plane_color(1), plane_color(2), plane_color(3)), + PointPainter::Data(tr(0), tr(1), tr(2), plane_color(0), + plane_color(1), plane_color(2), plane_color(3)), + PointPainter::Data(br(0), br(1), br(2), plane_color(0), + plane_color(1), plane_color(2), plane_color(3))); + } + + if (line_data != nullptr) { + // Frame around image plane and connecting lines to projection center. + + line_data->emplace_back( + PointPainter::Data(pc(0), pc(1), pc(2), frame_color(0), + frame_color(1), frame_color(2), frame_color(3)), + PointPainter::Data(tl(0), tl(1), tl(2), frame_color(0), + frame_color(1), frame_color(2), frame_color(3))); + + line_data->emplace_back( + PointPainter::Data(pc(0), pc(1), pc(2), frame_color(0), + frame_color(1), frame_color(2), frame_color(3)), + PointPainter::Data(tr(0), tr(1), tr(2), frame_color(0), + frame_color(1), frame_color(2), frame_color(3))); + + line_data->emplace_back( + PointPainter::Data(pc(0), pc(1), pc(2), frame_color(0), + frame_color(1), frame_color(2), frame_color(3)), + PointPainter::Data(br(0), br(1), br(2), frame_color(0), + frame_color(1), frame_color(2), frame_color(3))); + + line_data->emplace_back( + PointPainter::Data(pc(0), pc(1), pc(2), frame_color(0), + frame_color(1), frame_color(2), frame_color(3)), + PointPainter::Data(bl(0), bl(1), bl(2), frame_color(0), + frame_color(1), frame_color(2), frame_color(3))); + + line_data->emplace_back( + PointPainter::Data(tl(0), tl(1), tl(2), frame_color(0), + frame_color(1), frame_color(2), frame_color(3)), + PointPainter::Data(tr(0), tr(1), tr(2), frame_color(0), + frame_color(1), frame_color(2), frame_color(3))); + + line_data->emplace_back( + PointPainter::Data(tr(0), tr(1), tr(2), frame_color(0), + frame_color(1), frame_color(2), frame_color(3)), + PointPainter::Data(br(0), br(1), br(2), frame_color(0), + frame_color(1), frame_color(2), frame_color(3))); + + line_data->emplace_back( + PointPainter::Data(br(0), br(1), br(2), frame_color(0), + frame_color(1), frame_color(2), frame_color(3)), + PointPainter::Data(bl(0), bl(1), bl(2), frame_color(0), + frame_color(1), frame_color(2), frame_color(3))); + + line_data->emplace_back( + PointPainter::Data(bl(0), bl(1), bl(2), frame_color(0), + frame_color(1), frame_color(2), frame_color(3)), + PointPainter::Data(tl(0), tl(1), tl(2), frame_color(0), + frame_color(1), frame_color(2), frame_color(3))); + } +} + +} // namespace + +ModelViewerWidget::ModelViewerWidget(QWidget *parent) + : QOpenGLWidget(parent), mouse_is_pressed_(false), + focus_distance_(kInitFocusDistance), selected_image_id_(-1), + selected_point3D_id_(-1), coordinate_grid_enabled_(true), + near_plane_(kInitNearPlane) { + background_color_[0] = 1.0f; + background_color_[1] = 1.0f; + background_color_[2] = 1.0f; + + QSurfaceFormat format; + format.setDepthBufferSize(24); + format.setMajorVersion(3); + format.setMinorVersion(2); + format.setSamples(4); + format.setProfile(QSurfaceFormat::CoreProfile); +#ifdef DEBUG + format.setOption(QSurfaceFormat::DebugContext); +#endif + setFormat(format); + QSurfaceFormat::setDefaultFormat(format); + + image_size_ = static_cast(devicePixelRatio() * image_size_); + point_size_ = static_cast(devicePixelRatio() * point_size_); +} + +void ModelViewerWidget::initializeGL() { + initializeOpenGLFunctions(); + glEnable(GL_DEPTH_TEST); + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + glEnable(GL_VERTEX_PROGRAM_POINT_SIZE); + SetupPainters(); + SetupView(); + + UploadPointData(false); +} + +Eigen::Matrix4f QMatrixToEigen(const QMatrix4x4 &matrix) { + Eigen::Matrix4f eigen; + for (size_t r = 0; r < 4; ++r) { + for (size_t c = 0; c < 4; ++c) { + eigen(r, c) = matrix(r, c); + } + } + return eigen; +} + +void ModelViewerWidget::paintGL() { + glClearColor(background_color_[0], background_color_[1], + background_color_[2], 1.0f); + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + + const QMatrix4x4 pmv_matrix = projection_matrix_ * model_view_matrix_; + + // // Model view matrix for center of view + // QMatrix4x4 model_view_center_matrix = model_view_matrix_; + // const Eigen::Vector4f rot_center = + // QMatrixToEigen(model_view_matrix_).inverse() * + // Eigen::Vector4f(0, 0, -focus_distance_, 1); + // model_view_center_matrix.translate(rot_center(0), rot_center(1), + // rot_center(2)); + + point_painter_.Render(pmv_matrix, point_size_); + image_line_painter_.Render(pmv_matrix, width(), height(), 1); + image_triangle_painter_.Render(pmv_matrix); +} + +void ModelViewerWidget::resizeGL(int width, int height) { + glViewport(0, 0, width, height); + ComposeProjectionMatrix(); + // UploadCoordinateGridData(); +} + +// void ModelViewerWidget::ReloadReconstruction() { +// if (reconstruction == nullptr) { +// return; +// } + +// cameras = reconstruction->Cameras(); +// points3D = reconstruction->Points3D(); +// reg_image_ids = reconstruction->RegImageIds(); + +// images.clear(); +// for (const image_t image_id : reg_image_ids) { +// images[image_id] = reconstruction->Image(image_id); +// } + +// statusbar_status_label->setText(QString().asprintf( +// "%d Images - %d Points", static_cast(reg_image_ids.size()), +// static_cast(points3D.size()))); + +// Upload(); +// } + +// void ModelViewerWidget::ClearReconstruction() { +// cameras.clear(); +// images.clear(); +// points3D.clear(); +// reg_image_ids.clear(); +// reconstruction = nullptr; +// Upload(); +// } + +// int ModelViewerWidget::GetProjectionType() const { +// return options_->render->projection_type; +// } + +// void ModelViewerWidget::SetPointColormap(PointColormapBase *colormap) { +// point_colormap_.reset(colormap); +// } + +// void ModelViewerWidget::SetImageColormap(ImageColormapBase *colormap) { +// image_colormap_.reset(colormap); +// } + +void ModelViewerWidget::ChangeFocusDistance(const float delta) { + if (delta == 0.0f) { + return; + } + const float prev_focus_distance = focus_distance_; + float diff = delta * ZoomScale() * kFocusSpeed; + focus_distance_ -= diff; + if (focus_distance_ < kMinFocusDistance) { + focus_distance_ = kMinFocusDistance; + diff = prev_focus_distance - focus_distance_; + } else if (focus_distance_ > kMaxFocusDistance) { + focus_distance_ = kMaxFocusDistance; + diff = prev_focus_distance - focus_distance_; + } + const Eigen::Matrix4f vm_mat = QMatrixToEigen(model_view_matrix_).inverse(); + const Eigen::Vector3f tvec(0, 0, diff); + const Eigen::Vector3f tvec_rot = vm_mat.block<3, 3>(0, 0) * tvec; + model_view_matrix_.translate(tvec_rot(0), tvec_rot(1), tvec_rot(2)); + ComposeProjectionMatrix(); + // UploadCoordinateGridData(); + update(); +} + +void ModelViewerWidget::ChangeNearPlane(const float delta) { + if (delta == 0.0f) { + return; + } + near_plane_ *= (1.0f + delta / 100.0f * kNearPlaneScaleSpeed); + near_plane_ = std::max(kMinNearPlane, std::min(kMaxNearPlane, near_plane_)); + ComposeProjectionMatrix(); + // UploadCoordinateGridData(); + update(); +} + +void ModelViewerWidget::ChangePointSize(const float delta) { + if (delta == 0.0f) { + return; + } + point_size_ *= (1.0f + delta / 100.0f * kPointScaleSpeed); + point_size_ = std::max(kMinPointSize, std::min(kMaxPointSize, point_size_)); + update(); +} + +void ModelViewerWidget::RotateView(const float x, const float y, + const float prev_x, const float prev_y) { + if (x - prev_x == 0 && y - prev_y == 0) { + return; + } + + // Rotation according to the Arcball method "ARCBALL: A User Interface for + // Specifying Three-Dimensional Orientation Using a Mouse", Ken Shoemake, + // University of Pennsylvania, 1992. + + // Determine Arcball vector on unit sphere. + const Eigen::Vector3f u = PositionToArcballVector(x, y); + const Eigen::Vector3f v = PositionToArcballVector(prev_x, prev_y); + + // Angle between vectors. + const float angle = 2.0f * std::acos(std::min(1.0f, u.dot(v))); + + const float kMinAngle = 1e-3f; + if (angle > kMinAngle) { + const Eigen::Matrix4f vm_mat = + QMatrixToEigen(model_view_matrix_).inverse(); + + // Rotation axis. + Eigen::Vector3f axis = vm_mat.block<3, 3>(0, 0) * v.cross(u); + axis = axis.normalized(); + // Center of rotation is current focus. + const Eigen::Vector4f rot_center = + vm_mat * Eigen::Vector4f(0, 0, -focus_distance_, 1); + // First shift to rotation center, then rotate and shift back. + model_view_matrix_.translate(rot_center(0), rot_center(1), + rot_center(2)); + model_view_matrix_.rotate(RadToDeg(angle), axis(0), axis(1), axis(2)); + model_view_matrix_.translate(-rot_center(0), -rot_center(1), + -rot_center(2)); + update(); + } +} + +void ModelViewerWidget::TranslateView(const float x, const float y, + const float prev_x, const float prev_y) { + if (x - prev_x == 0 && y - prev_y == 0) { + return; + } + + Eigen::Vector3f tvec(x - prev_x, prev_y - y, 0.0f); + + // if (options_->render->projection_type == + // RenderOptions::ProjectionType::PERSPECTIVE) { + // tvec *= ZoomScale(); + // } else if (options_->render->projection_type == + // RenderOptions::ProjectionType::ORTHOGRAPHIC) { + // tvec *= 2.0f * OrthographicWindowExtent() / height(); + // } + tvec *= ZoomScale(); + + const Eigen::Matrix4f vm_mat = QMatrixToEigen(model_view_matrix_).inverse(); + + const Eigen::Vector3f tvec_rot = vm_mat.block<3, 3>(0, 0) * tvec; + model_view_matrix_.translate(tvec_rot(0), tvec_rot(1), tvec_rot(2)); + + update(); +} + +void ModelViewerWidget::ChangeCameraSize(const float delta) { + if (delta == 0.0f) { + return; + } + image_size_ *= (1.0f + delta / 100.0f * kImageScaleSpeed); + image_size_ = std::max(kMinImageSize, std::min(kMaxImageSize, image_size_)); + UploadImageData(); + update(); +} + +// void ModelViewerWidget::ResetView() { +// SetupView(); +// Upload(); +// } + +// QMatrix4x4 ModelViewerWidget::ModelViewMatrix() const { +// return model_view_matrix_; +// } + +// void ModelViewerWidget::SetModelViewMatrix(const QMatrix4x4 &matrix) { +// model_view_matrix_ = matrix; +// update(); +// } + +// void ModelViewerWidget::SelectObject(const int x, const int y) { +// makeCurrent(); + +// // Ensure that anti-aliasing does not change the colors of objects. +// glDisable(GL_MULTISAMPLE); + +// glClearColor(1.0f, 1.0f, 1.0f, 1.0f); +// glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + +// // Upload data in selection mode (one color per object). +// UploadImageData(true); +// UploadPointData(true); + +// // Render in selection mode, with larger points to improve selection +// // accuracy. +// const QMatrix4x4 pmv_matrix = projection_matrix_ * +// model_view_matrix_; image_triangle_painter_.Render(pmv_matrix); +// point_painter_.Render(pmv_matrix, 2 * point_size_); + +// const int scaled_x = devicePixelRatio() * x; +// const int scaled_y = devicePixelRatio() * (height() - y - 1); + +// QOpenGLFramebufferObjectFormat fbo_format; +// fbo_format.setSamples(0); +// QOpenGLFramebufferObject fbo(1, 1, fbo_format); + +// glBindFramebuffer(GL_READ_FRAMEBUFFER, defaultFramebufferObject()); +// glBindFramebuffer(GL_DRAW_FRAMEBUFFER, fbo.handle()); +// glBlitFramebuffer(scaled_x, scaled_y, scaled_x + 1, scaled_y + 1, 0, +// 0, 1, +// 1, GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT, +// GL_NEAREST); + +// fbo.bind(); +// std::array color; +// glReadPixels(0, 0, 1, 1, GL_RGB, GL_UNSIGNED_BYTE, color.data()); +// fbo.release(); + +// const size_t index = RGBToIndex(color[0], color[1], color[2]); + +// if (index < selection_buffer_.size()) { +// const char buffer_type = selection_buffer_[index].second; +// if (buffer_type == SELECTION_BUFFER_IMAGE_IDX) { +// selected_image_id_ = +// static_cast(selection_buffer_[index].first); +// selected_point3D_id_ = kInvalidPoint3DId; +// ShowImageInfo(selected_image_id_); +// } else if (buffer_type == SELECTION_BUFFER_POINT_IDX) { +// selected_image_id_ = kInvalidImageId; +// selected_point3D_id_ = selection_buffer_[index].first; +// ShowPointInfo(selection_buffer_[index].first); +// } else { +// selected_image_id_ = kInvalidImageId; +// selected_point3D_id_ = kInvalidPoint3DId; +// image_viewer_widget_->hide(); +// } +// } else { +// selected_image_id_ = kInvalidImageId; +// selected_point3D_id_ = kInvalidPoint3DId; +// image_viewer_widget_->hide(); +// } + +// // Re-enable, since temporarily disabled above. +// glEnable(GL_MULTISAMPLE); + +// selection_buffer_.clear(); + +// UploadPointData(); +// UploadImageData(); +// UploadPointConnectionData(); +// UploadImageConnectionData(); + +// update(); +// } + +// void ModelViewerWidget::SelectMoviewGrabberView(const size_t view_idx) { +// selected_movie_grabber_view_ = view_idx; +// UploadMovieGrabberData(); +// update(); +// } + +// void ModelViewerWidget::ShowPointInfo(const point3D_t point3D_id) { +// point_viewer_widget_->Show(point3D_id); +// } + +// void ModelViewerWidget::ShowImageInfo(const image_t image_id) { +// image_viewer_widget_->ShowImageWithId(image_id); +// } + +// void ModelViewerWidget::SetBackgroundColor(const float r, const float g, +// const float b) { +// background_color_[0] = r; +// background_color_[1] = g; +// background_color_[2] = b; +// update(); +// } + +void ModelViewerWidget::mousePressEvent(QMouseEvent *event) { + if (mouse_press_timer_.isActive()) { // Select objects (2. click) + mouse_is_pressed_ = false; + mouse_press_timer_.stop(); + selection_buffer_.clear(); + // SelectObject(event->pos().x(), event->pos().y()); + } else { // Set timer to remember 1. click + mouse_press_timer_.setSingleShot(true); + mouse_press_timer_.start(kDoubleClickInterval); + mouse_is_pressed_ = true; + prev_mouse_pos_ = event->pos(); + } + event->accept(); +} + +void ModelViewerWidget::mouseReleaseEvent(QMouseEvent *event) { + mouse_is_pressed_ = false; + event->accept(); +} + +void ModelViewerWidget::mouseMoveEvent(QMouseEvent *event) { + if (mouse_is_pressed_) { + if (event->buttons() & Qt::RightButton || + (event->buttons() & Qt::LeftButton && + event->modifiers() & Qt::ControlModifier)) { + TranslateView(event->pos().x(), event->pos().y(), + prev_mouse_pos_.x(), prev_mouse_pos_.y()); + } else if (event->buttons() & Qt::LeftButton) { + RotateView(event->pos().x(), event->pos().y(), prev_mouse_pos_.x(), + prev_mouse_pos_.y()); + } + } + prev_mouse_pos_ = event->pos(); + event->accept(); +} + +void ModelViewerWidget::wheelEvent(QWheelEvent *event) { + // We don't mind whether horizontal or vertical scroll. + const float delta = event->angleDelta().x() + event->angleDelta().y(); + if (event->modifiers().testFlag(Qt::ControlModifier)) { + ChangePointSize(delta); + } else if (event->modifiers().testFlag(Qt::AltModifier)) { + ChangeCameraSize(delta); + } else if (event->modifiers().testFlag(Qt::ShiftModifier)) { + ChangeNearPlane(delta); + } else { + ChangeFocusDistance(delta); + } + event->accept(); +} + +void ModelViewerWidget::SetupPainters() { + makeCurrent(); + + // coordinate_axes_painter_.Setup(); + // coordinate_grid_painter_.Setup(); + + point_painter_.Setup(); + image_line_painter_.Setup(); + image_triangle_painter_.Setup(); + // point_connection_painter_.Setup(); + + // image_connection_painter_.Setup(); + + // movie_grabber_path_painter_.Setup(); + // movie_grabber_line_painter_.Setup(); + // movie_grabber_triangle_painter_.Setup(); +} + +void ModelViewerWidget::SetupView() { + point_size_ = kInitPointSize; + image_size_ = kInitImageSize; + focus_distance_ = kInitFocusDistance; + model_view_matrix_.setToIdentity(); + model_view_matrix_.translate(0, 0, -focus_distance_); + model_view_matrix_.rotate(225, 1, 0, 0); + model_view_matrix_.rotate(-45, 0, 1, 0); + projection_matrix_.setToIdentity(); +} + +void ModelViewerWidget::Upload() { + // point_colormap_->Prepare(cameras, images, points3D, reg_image_ids); + // image_colormap_->Prepare(cameras, images, points3D, reg_image_ids); + + // ComposeProjectionMatrix(); + + UploadPointData(); + UploadImageData(); + // UploadMovieGrabberData(); + // UploadPointConnectionData(); + // UploadImageConnectionData(); + + update(); +} + +// void ModelViewerWidget::UploadCoordinateGridData() { +// makeCurrent(); + +// const float scale = ZoomScale(); + +// // View center grid. +// std::vector grid_data(3); + +// grid_data[0].point1 = +// PointPainter::Data(-20 * scale, 0, 0, kGridColor(0), +// kGridColor(1), +// kGridColor(2), kGridColor(3)); +// grid_data[0].point2 = +// PointPainter::Data(20 * scale, 0, 0, kGridColor(0), +// kGridColor(1), +// kGridColor(2), kGridColor(3)); + +// grid_data[1].point1 = +// PointPainter::Data(0, -20 * scale, 0, kGridColor(0), +// kGridColor(1), +// kGridColor(2), kGridColor(3)); +// grid_data[1].point2 = +// PointPainter::Data(0, 20 * scale, 0, kGridColor(0), +// kGridColor(1), +// kGridColor(2), kGridColor(3)); + +// grid_data[2].point1 = +// PointPainter::Data(0, 0, -20 * scale, kGridColor(0), +// kGridColor(1), +// kGridColor(2), kGridColor(3)); +// grid_data[2].point2 = +// PointPainter::Data(0, 0, 20 * scale, kGridColor(0), +// kGridColor(1), +// kGridColor(2), kGridColor(3)); + +// coordinate_grid_painter_.Upload(grid_data); + +// // Coordinate axes. +// std::vector axes_data(3); + +// axes_data[0].point1 = +// PointPainter::Data(0, 0, 0, kXAxisColor(0), kXAxisColor(1), +// kXAxisColor(2), kXAxisColor(3)); +// axes_data[0].point2 = +// PointPainter::Data(50 * scale, 0, 0, kXAxisColor(0), +// kXAxisColor(1), +// kXAxisColor(2), kXAxisColor(3)); + +// axes_data[1].point1 = +// PointPainter::Data(0, 0, 0, kYAxisColor(0), kYAxisColor(1), +// kYAxisColor(2), kYAxisColor(3)); +// axes_data[1].point2 = +// PointPainter::Data(0, 50 * scale, 0, kYAxisColor(0), +// kYAxisColor(1), +// kYAxisColor(2), kYAxisColor(3)); + +// axes_data[2].point1 = +// PointPainter::Data(0, 0, 0, kZAxisColor(0), kZAxisColor(1), +// kZAxisColor(2), kZAxisColor(3)); +// axes_data[2].point2 = +// PointPainter::Data(0, 0, 50 * scale, kZAxisColor(0), +// kZAxisColor(1), +// kZAxisColor(2), kZAxisColor(3)); + +// coordinate_axes_painter_.Upload(axes_data); +// } + +void ModelViewerWidget::UploadPointData(const bool selection_mode) { + makeCurrent(); + + if (map == nullptr) + return; + + std::vector data; + for (const auto &[id, track] : map->track_map_) { + if (track.outlier) + continue; + PointPainter::Data painter_point; + painter_point.x = track.point3d_.x(); + painter_point.y = track.point3d_.y(); + painter_point.z = track.point3d_.z(); + painter_point.r = 0; + painter_point.g = 0; + painter_point.b = 0; + painter_point.a = 1; + data.push_back(painter_point); + } + + point_painter_.Upload(data); +} + +// void ModelViewerWidget::UploadPointConnectionData() { +// makeCurrent(); + +// std::vector line_data; + +// if (selected_point3D_id_ == kInvalidPoint3DId) { +// // No point selected, so upload empty data +// point_connection_painter_.Upload(line_data); +// return; +// } + +// const auto &point3D = points3D[selected_point3D_id_]; + +// // 3D point position. +// LinePainter::Data line; +// line.point1 = PointPainter::Data( +// static_cast(point3D.XYZ(0)), +// static_cast(point3D.XYZ(1)), +// static_cast(point3D.XYZ(2)), kSelectedPointColor(0), +// kSelectedPointColor(1), kSelectedPointColor(2), 0.8f); + +// // All images in which 3D point is observed. +// for (const auto &track_el : point3D.Track().Elements()) { +// const Image &conn_image = images[track_el.image_id]; +// const Eigen::Vector3f conn_proj_center = +// conn_image.ProjectionCenter().cast(); +// line.point2 = PointPainter::Data( +// conn_proj_center(0), conn_proj_center(1), +// conn_proj_center(2), kSelectedPointColor(0), +// kSelectedPointColor(1), kSelectedPointColor(2), 0.8f); +// line_data.push_back(line); +// } + +// point_connection_painter_.Upload(line_data); +// } + +void ModelViewerWidget::UploadImageData(const bool selection_mode) { + makeCurrent(); + if (map == nullptr) + return; + + const int num_frame = map->frame_map_.size(); + + std::vector line_data; + line_data.reserve(8 * num_frame); + + std::vector triangle_data; + triangle_data.reserve(2 * num_frame); + + for (const auto &[image_id, frame] : map->frame_map_) { + const Camera &camera = map->Camera(frame.camera_id); + + Eigen::Vector4f plane_color; + Eigen::Vector4f frame_color; + if (selection_mode) { + const size_t index = selection_buffer_.size(); + selection_buffer_.push_back( + std::make_pair(image_id, SELECTION_BUFFER_IMAGE_IDX)); + plane_color = frame_color = IndexToRGB(index); + } else { + if (image_id == selected_image_id_) { + plane_color = kSelectedImagePlaneColor; + frame_color = kSelectedImageFrameColor; + } else { + plane_color = Eigen::Vector4f(0.8, 0, 0, 1); + frame_color = Eigen::Vector4f(0.8, 0, 0, 1); + // image_colormap_->ComputeColor(image, &plane_color, + // &frame_color); + } + } + Eigen::Matrix inv_proj_matrix; + inv_proj_matrix.leftCols<3>() = + frame.qwc().toRotationMatrix().cast(); + inv_proj_matrix.rightCols<1>() = frame.twc().cast(); + + BuildImageModel(inv_proj_matrix, camera, image_size_, plane_color, + frame_color, &triangle_data, + selection_mode ? nullptr : &line_data); + } + + image_line_painter_.Upload(line_data); + image_triangle_painter_.Upload(triangle_data); +} + +// void ModelViewerWidget::UploadImageConnectionData() { +// makeCurrent(); + +// std::vector line_data; +// std::vector image_ids; + +// if (selected_image_id_ != kInvalidImageId) { +// // Show connections to selected images +// image_ids.push_back(selected_image_id_); +// } else if (options_->render->image_connections) { +// // Show all connections +// image_ids = reg_image_ids; +// } else { // Disabled, so upload empty data +// image_connection_painter_.Upload(line_data); +// return; +// } + +// for (const image_t image_id : image_ids) { +// const Image &image = images.at(image_id); + +// const Eigen::Vector3f proj_center = +// image.ProjectionCenter().cast(); + +// // Collect all connected images +// std::unordered_set conn_image_ids; + +// for (const Point2D &point2D : image.Points2D()) { +// if (point2D.HasPoint3D()) { +// const Point3D &point3D = points3D[point2D.Point3DId()]; +// for (const auto &track_elem : point3D.Track().Elements()) +// { +// conn_image_ids.insert(track_elem.image_id); +// } +// } +// } + +// // Selected image in the center. +// LinePainter::Data line; +// line.point1 = PointPainter::Data( +// proj_center(0), proj_center(1), proj_center(2), +// kSelectedImageFrameColor(0), kSelectedImageFrameColor(1), +// kSelectedImageFrameColor(2), 0.8f); + +// // All connected images to the selected image. +// for (const image_t conn_image_id : conn_image_ids) { +// const Image &conn_image = images[conn_image_id]; +// const Eigen::Vector3f conn_proj_center = +// conn_image.ProjectionCenter().cast(); +// line.point2 = PointPainter::Data( +// conn_proj_center(0), conn_proj_center(1), +// conn_proj_center(2), kSelectedImageFrameColor(0), +// kSelectedImageFrameColor(1), kSelectedImageFrameColor(2), +// 0.8f); +// line_data.push_back(line); +// } +// } + +// image_connection_painter_.Upload(line_data); +// } + +void ModelViewerWidget::ComposeProjectionMatrix() { + projection_matrix_.setToIdentity(); + projection_matrix_.perspective(kFieldOfView, AspectRatio(), near_plane_, + kFarPlane); +} + +float ModelViewerWidget::ZoomScale() const { + // "Constant" scale factor w.r.t. zoom-level. + float rad = kFieldOfView * + 0.0174532925199432954743716805978692718781530857086181640625; + return 2.0f * std::tan(rad / 2.0f) * std::abs(focus_distance_) / height(); +} + +float ModelViewerWidget::AspectRatio() const { + return static_cast(width()) / static_cast(height()); +} + +Eigen::Vector3f +ModelViewerWidget::PositionToArcballVector(const float x, const float y) const { + Eigen::Vector3f vec(2.0f * x / width() - 1, 1 - 2.0f * y / height(), 0.0f); + const float norm2 = vec.squaredNorm(); + if (norm2 <= 1.0f) { + vec.z() = std::sqrt(1.0f - norm2); + } else { + vec = vec.normalized(); + } + return vec; +} + +} // namespace xrsfm diff --git a/src/colmap/ui/model_viewer_widget.h b/src/colmap/ui/model_viewer_widget.h new file mode 100644 index 0000000..23a2ee3 --- /dev/null +++ b/src/colmap/ui/model_viewer_widget.h @@ -0,0 +1,155 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of +// its contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UI_MODEL_VIEWER_WIDGET_H_ +#define COLMAP_SRC_UI_MODEL_VIEWER_WIDGET_H_ + +#include +#include + +#include + +#include "base/map.h" +#include "ui/point_painter.h" + +namespace xrsfm { + +class ModelViewerWidget : public QOpenGLWidget, + protected QOpenGLFunctions_3_2_Core { + public: + const float kInitNearPlane = 1.0f; + const float kMinNearPlane = 1e-3f; + const float kMaxNearPlane = 1e5f; + const float kNearPlaneScaleSpeed = 0.02f; + const float kFarPlane = 1e5f; + const float kInitFocusDistance = 100.0f; + const float kMinFocusDistance = 1e-5f; + const float kMaxFocusDistance = 1e8f; + const float kFieldOfView = 25.0f; + const float kFocusSpeed = 2.0f; + const float kInitPointSize = 1.0f; + const float kMinPointSize = 0.5f; + const float kMaxPointSize = 100.0f; + const float kPointScaleSpeed = 0.1f; + const float kInitImageSize = 0.2f; + const float kMinImageSize = 1e-6f; + const float kMaxImageSize = 1e3f; + const float kImageScaleSpeed = 0.1f; + const int kDoubleClickInterval = 250; + + ModelViewerWidget(QWidget *parent); + + void ChangeFocusDistance(const float delta); + void ChangeNearPlane(const float delta); + void ChangePointSize(const float delta); + void ChangeCameraSize(const float delta); + + void ResetView(); + void RotateView(const float x, const float y, const float prev_x, + const float prev_y); + void TranslateView(const float x, const float y, const float prev_x, + const float prev_y); + + // void ReloadReconstruction(); + // void ClearReconstruction(); + // void SelectObject(const int x, const int y); + // void SelectMoviewGrabberView(const size_t view_idx); + + // void ShowPointInfo(const point3D_t point3D_id); + // void ShowImageInfo(const image_t image_id); + + Map *map = nullptr; + + // QLabel *statusbar_status_label; + + // protected: + void initializeGL() override; + void resizeGL(int width, int height) override; + void paintGL() override; + + void Upload(); + + private: + void mousePressEvent(QMouseEvent *event) override; + void mouseReleaseEvent(QMouseEvent *event) override; + void mouseMoveEvent(QMouseEvent *event) override; + void wheelEvent(QWheelEvent *event) override; + + void SetupPainters(); + void SetupView(); + + void UploadCoordinateGridData(); + void UploadPointData(const bool selection_mode = false); + // void UploadPointConnectionData(); + void UploadImageData(const bool selection_mode = false); + // void UploadImageConnectionData(); + // void UploadMovieGrabberData(); + + void ComposeProjectionMatrix(); + + float ZoomScale() const; + float AspectRatio() const; + // float OrthographicWindowExtent() const; + + Eigen::Vector3f PositionToArcballVector(const float x, const float y) const; + + QMatrix4x4 model_view_matrix_; + QMatrix4x4 projection_matrix_; + + PointPainter point_painter_; + LinePainter image_line_painter_; + TrianglePainter image_triangle_painter_; + + bool mouse_is_pressed_; + QTimer mouse_press_timer_; + QPoint prev_mouse_pos_; + + float focus_distance_; + + std::vector> selection_buffer_; + int selected_image_id_; + long int selected_point3D_id_; + size_t selected_movie_grabber_view_; + + bool coordinate_grid_enabled_; + + float point_size_; + float image_size_; + float near_plane_; + + float background_color_[3]; + + QOpenGLShaderProgram *shaderProgram; +}; + +} // namespace xrsfm + +#endif // COLMAP_SRC_UI_MODEL_VIEWER_WIDGET_H_ diff --git a/src/colmap/ui/point_painter.cc b/src/colmap/ui/point_painter.cc new file mode 100644 index 0000000..6d017e9 --- /dev/null +++ b/src/colmap/ui/point_painter.cc @@ -0,0 +1,297 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of +// its contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#include "point_painter.h" + +namespace xrsfm { + +PointPainter::PointPainter() : num_geoms_(0) {} + +PointPainter::~PointPainter() { + vao_.destroy(); + vbo_.destroy(); +} + +void PointPainter::Setup() { + vao_.destroy(); + vbo_.destroy(); + if (shader_program_.isLinked()) { + shader_program_.release(); + shader_program_.removeAllShaders(); + } + + shader_program_.addShaderFromSourceFile(QOpenGLShader::Vertex, + ":/shaders/points.v.glsl"); + shader_program_.addShaderFromSourceFile(QOpenGLShader::Fragment, + ":/shaders/points.f.glsl"); + + shader_program_.link(); + shader_program_.bind(); + + vao_.create(); + vbo_.create(); + +#if DEBUG + glDebugLog(); +#endif +} + +void PointPainter::Upload(const std::vector &data) { + num_geoms_ = data.size(); + if (num_geoms_ == 0) { + return; + } + + vao_.bind(); + vbo_.bind(); + + // Upload data array to GPU + vbo_.setUsagePattern(QOpenGLBuffer::DynamicDraw); + vbo_.allocate(data.data(), + static_cast(data.size() * sizeof(PointPainter::Data))); + + // in_position + shader_program_.enableAttributeArray("a_position"); + shader_program_.setAttributeBuffer("a_position", GL_FLOAT, 0, 3, + sizeof(PointPainter::Data)); + + // in_color + shader_program_.enableAttributeArray("a_color"); + shader_program_.setAttributeBuffer("a_color", GL_FLOAT, 3 * sizeof(GLfloat), + 4, sizeof(PointPainter::Data)); + + // Make sure they are not changed from the outside + vbo_.release(); + vao_.release(); + +#if DEBUG + glDebugLog(); +#endif +} + +void PointPainter::Render(const QMatrix4x4 &pmv_matrix, + const float point_size) { + if (num_geoms_ == 0) { + return; + } + + shader_program_.bind(); + vao_.bind(); + + shader_program_.setUniformValue("u_pmv_matrix", pmv_matrix); + shader_program_.setUniformValue("u_point_size", point_size); + + QOpenGLFunctions *gl_funcs = QOpenGLContext::currentContext()->functions(); + gl_funcs->glDrawArrays(GL_POINTS, 0, (GLsizei)num_geoms_); + + // Make sure the VAO is not changed from the outside + vao_.release(); + +#if DEBUG + glDebugLog(); +#endif +} + +TrianglePainter::TrianglePainter() : num_geoms_(0) {} + +TrianglePainter::~TrianglePainter() { + vao_.destroy(); + vbo_.destroy(); +} + +void TrianglePainter::Setup() { + vao_.destroy(); + vbo_.destroy(); + if (shader_program_.isLinked()) { + shader_program_.release(); + shader_program_.removeAllShaders(); + } + + shader_program_.addShaderFromSourceFile(QOpenGLShader::Vertex, + ":/shaders/triangles.v.glsl"); + shader_program_.addShaderFromSourceFile(QOpenGLShader::Fragment, + ":/shaders/triangles.f.glsl"); + shader_program_.link(); + shader_program_.bind(); + + vao_.create(); + vbo_.create(); + +#if DEBUG + glDebugLog(); +#endif +} + +void TrianglePainter::Upload(const std::vector &data) { + num_geoms_ = data.size(); + if (num_geoms_ == 0) { + return; + } + + vao_.bind(); + vbo_.bind(); + + // Upload data array to GPU + vbo_.setUsagePattern(QOpenGLBuffer::DynamicDraw); + vbo_.allocate(data.data(), static_cast(data.size() * + sizeof(TrianglePainter::Data))); + + // in_position + shader_program_.enableAttributeArray("a_position"); + shader_program_.setAttributeBuffer("a_position", GL_FLOAT, 0, 3, + sizeof(PointPainter::Data)); + + // in_color + shader_program_.enableAttributeArray("a_color"); + shader_program_.setAttributeBuffer("a_color", GL_FLOAT, 3 * sizeof(GLfloat), + 4, sizeof(PointPainter::Data)); + + // Make sure they are not changed from the outside + vbo_.release(); + vao_.release(); + +#if DEBUG + glDebugLog(); +#endif +} + +void TrianglePainter::Render(const QMatrix4x4 &pmv_matrix) { + if (num_geoms_ == 0) { + return; + } + + shader_program_.bind(); + vao_.bind(); + + shader_program_.setUniformValue("u_pmv_matrix", pmv_matrix); + + QOpenGLFunctions *gl_funcs = QOpenGLContext::currentContext()->functions(); + gl_funcs->glDrawArrays(GL_TRIANGLES, 0, (GLsizei)(3 * num_geoms_)); + + // Make sure the VAO is not changed from the outside + vao_.release(); + +#if DEBUG + glDebugLog(); +#endif +} + +LinePainter::LinePainter() : num_geoms_(0) {} + +LinePainter::~LinePainter() { + vao_.destroy(); + vbo_.destroy(); +} + +void LinePainter::Setup() { + vao_.destroy(); + vbo_.destroy(); + if (shader_program_.isLinked()) { + shader_program_.release(); + shader_program_.removeAllShaders(); + } + + shader_program_.addShaderFromSourceFile(QOpenGLShader::Vertex, + ":/shaders/lines.v.glsl"); + shader_program_.addShaderFromSourceFile(QOpenGLShader::Geometry, + ":/shaders/lines.g.glsl"); + shader_program_.addShaderFromSourceFile(QOpenGLShader::Fragment, + ":/shaders/lines.f.glsl"); + shader_program_.link(); + shader_program_.bind(); + + vao_.create(); + vbo_.create(); + +#if DEBUG + glDebugLog(); +#endif +} + +void LinePainter::Upload(const std::vector &data) { + num_geoms_ = data.size(); + if (num_geoms_ == 0) { + return; + } + + vao_.bind(); + vbo_.bind(); + + // Upload data array to GPU + vbo_.setUsagePattern(QOpenGLBuffer::DynamicDraw); + vbo_.allocate(data.data(), + static_cast(data.size() * sizeof(LinePainter::Data))); + + // in_position + shader_program_.enableAttributeArray("a_pos"); + shader_program_.setAttributeBuffer("a_pos", GL_FLOAT, 0, 3, + sizeof(PointPainter::Data)); + + // in_color + shader_program_.enableAttributeArray("a_color"); + shader_program_.setAttributeBuffer("a_color", GL_FLOAT, 3 * sizeof(GLfloat), + 4, sizeof(PointPainter::Data)); + + // Make sure they are not changed from the outside + vbo_.release(); + vao_.release(); + +#if DEBUG + glDebugLog(); +#endif +} + +void LinePainter::Render(const QMatrix4x4 &pmv_matrix, const int width, + const int height, const float line_width) { + if (num_geoms_ == 0) { + return; + } + + shader_program_.bind(); + vao_.bind(); + + shader_program_.setUniformValue("u_pmv_matrix", pmv_matrix); + shader_program_.setUniformValue("u_inv_viewport", + QVector2D(1.0f / width, 1.0f / height)); + shader_program_.setUniformValue("u_line_width", line_width); + + QOpenGLFunctions *gl_funcs = QOpenGLContext::currentContext()->functions(); + gl_funcs->glDrawArrays(GL_LINES, 0, (GLsizei)(2 * num_geoms_)); + + // Make sure the VAO is not changed from the outside + vao_.release(); + +#if DEBUG + glDebugLog(); +#endif +} + +} // namespace xrsfm diff --git a/src/colmap/ui/point_painter.h b/src/colmap/ui/point_painter.h new file mode 100644 index 0000000..d4a51b6 --- /dev/null +++ b/src/colmap/ui/point_painter.h @@ -0,0 +1,124 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of +// its contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#ifndef COLMAP_SRC_UI_POINT_PAINTER_H_ +#define COLMAP_SRC_UI_POINT_PAINTER_H_ + +#include +#include + +namespace xrsfm { + +class PointPainter { + public: + PointPainter(); + ~PointPainter(); + + struct Data { + Data() : x(0), y(0), z(0), r(0), g(0), b(0), a(0) {} + Data(const float x_, const float y_, const float z_, const float r_, + const float g_, const float b_, const float a_) + : x(x_), y(y_), z(z_), r(r_), g(g_), b(b_), a(a_) {} + + float x, y, z; + float r, g, b, a; + }; + + void Setup(); + void Upload(const std::vector &data); + void Render(const QMatrix4x4 &pmv_matrix, const float point_size); + + private: + QOpenGLShaderProgram shader_program_; + QOpenGLVertexArrayObject vao_; + QOpenGLBuffer vbo_; + + size_t num_geoms_; +}; + +class TrianglePainter { + public: + TrianglePainter(); + ~TrianglePainter(); + + struct Data { + Data() {} + Data(const PointPainter::Data &p1, const PointPainter::Data &p2, + const PointPainter::Data &p3) + : point1(p1), point2(p2), point3(p3) {} + + PointPainter::Data point1; + PointPainter::Data point2; + PointPainter::Data point3; + }; + + void Setup(); + void Upload(const std::vector &data); + void Render(const QMatrix4x4 &pmv_matrix); + + private: + QOpenGLShaderProgram shader_program_; + QOpenGLVertexArrayObject vao_; + QOpenGLBuffer vbo_; + + size_t num_geoms_; +}; + +class LinePainter { + public: + LinePainter(); + ~LinePainter(); + + struct Data { + Data() {} + Data(const PointPainter::Data &p1, const PointPainter::Data &p2) + : point1(p1), point2(p2) {} + + PointPainter::Data point1; + PointPainter::Data point2; + }; + + void Setup(); + void Upload(const std::vector &data); + void Render(const QMatrix4x4 &pmv_matrix, int width, int height, + float line_width); + + private: + QOpenGLShaderProgram shader_program_; + QOpenGLVertexArrayObject vao_; + QOpenGLBuffer vbo_; + + size_t num_geoms_; +}; + +} // namespace xrsfm + +#endif // COLMAP_SRC_UI_POINT_PAINTER_H_ diff --git a/src/colmap/ui/project_widget.cc b/src/colmap/ui/project_widget.cc new file mode 100644 index 0000000..568eb49 --- /dev/null +++ b/src/colmap/ui/project_widget.cc @@ -0,0 +1,127 @@ +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of +// its contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) + +#include "project_widget.h" + +ProjectWidget::ProjectWidget(QWidget *parent) + : QWidget(parent), prev_selected_(false) { + setWindowFlags(Qt::Dialog); + setWindowModality(Qt::ApplicationModal); + setWindowTitle("Project"); + + // Image path. + QPushButton *image_path_select = new QPushButton(tr("Select"), this); + connect(image_path_select, &QPushButton::released, this, + &ProjectWidget::SelectImagePath); + image_path_text_ = new QLineEdit(this); + image_path_text_->setText(QString::fromStdString(image_path_)); + + // Workspace path. + QPushButton *databse_path_open = new QPushButton(tr("Open"), this); + connect(databse_path_open, &QPushButton::released, this, + &ProjectWidget::SelectWorkspacePath); + workspace_path_text_ = new QLineEdit(this); + workspace_path_text_->setText(QString::fromStdString(workspace_path_)); + + QGridLayout *grid = new QGridLayout(this); + grid->addWidget(new QLabel(tr("Workspace"), this), 0, 0); + grid->addWidget(workspace_path_text_, 0, 1); + grid->addWidget(databse_path_open, 0, 2); + grid->addWidget(new QLabel(tr("Images"), this), 1, 0); + grid->addWidget(image_path_text_, 1, 1); + grid->addWidget(image_path_select, 1, 2); +} + +void ProjectWidget::Reset() { + workspace_path_text_->clear(); + image_path_text_->clear(); +} + +std::string ProjectWidget::GetWorkspacePath() const { + std::string path = workspace_path_text_->text().toUtf8().constData(); + if (path.back() != "/") + path = path + "/"; + return path; +} + +std::string ProjectWidget::GetImagePath() const { + std::string path = image_path_text_->text().toUtf8().constData(); + if (path.back() != "/") + path = path + "/"; + return path; +} + +void ProjectWidget::SelectImagePath() { + const auto image_path = QFileDialog::getExistingDirectory( + this, tr("Select image path..."), DefaultDirectory(), + QFileDialog::ShowDirsOnly); + if (image_path != "") { + image_path_text_->setText(image_path); + } +} + +void ProjectWidget::SelectWorkspacePath() { + const auto workspace_path = QFileDialog::getExistingDirectory( + this, tr("Select image path..."), DefaultDirectory(), + QFileDialog::ShowDirsOnly); + if (workspace_path != "") { + workspace_path_text_->setText(workspace_path); + } +} + +QString ProjectWidget::DefaultDirectory() { + // if (prev_selected_) { + // return ""; + // } + + // prev_selected_ = true; + + // if (!options_->project_path->empty()) { + // const auto parent_path = GetParentDir(*options_->project_path); + // if (ExistsDir(parent_path)) { + // return QString::fromStdString(parent_path); + // } + // } + + // if (!workspace_path_text_->text().isEmpty()) { + // const auto parent_path = + // GetParentDir(workspace_path_text_->text().toUtf8().constData()); + // if (ExistsDir(parent_path)) { + // return QString::fromStdString(parent_path); + // } + // } + + // if (!image_path_text_->text().isEmpty()) { + // return image_path_text_->text(); + // } + + return ""; +} diff --git a/src/geometry/colmap/util/logging.cc b/src/colmap/ui/project_widget.h similarity index 65% rename from src/geometry/colmap/util/logging.cc rename to src/colmap/ui/project_widget.h index 65560e8..c35a83b 100644 --- a/src/geometry/colmap/util/logging.cc +++ b/src/colmap/ui/project_widget.h @@ -1,4 +1,4 @@ -// Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. +// Copyright (c) 2023, ETH Zurich and UNC Chapel Hill. // All rights reserved. // // Redistribution and use in source and binary forms, with or without @@ -29,35 +29,35 @@ // // Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) -#include "logging.h" - -namespace colmap { - -void InitializeGlog(char **argv) { -#ifndef _MSC_VER // Broken in MSVC - google::InstallFailureSignalHandler(); -#endif - google::InitGoogleLogging(argv[0]); -} - -const char *__GetConstFileBaseName(const char *file) { - const char *base = strrchr(file, '/'); - if (!base) { - base = strrchr(file, '\\'); - } - return base ? (base + 1) : file; -} - -bool __CheckOptionImpl(const char *file, const int line, const bool result, - const char *expr_str) { - if (result) { - return true; - } else { - std::cerr << StringPrintf("[%s:%d] Check failed: %s", - __GetConstFileBaseName(file), line, expr_str) - << std::endl; - return false; - } -} - -} // namespace colmap +#ifndef COLMAP_SRC_UI_PROJECT_WIDGET_H_ +#define COLMAP_SRC_UI_PROJECT_WIDGET_H_ + +#include +#include + +class ProjectWidget : public QWidget { + public: + ProjectWidget(QWidget *parent); + + void Reset(); + + std::string GetWorkspacePath() const; + std::string GetImagePath() const; + + private: + void SelectImagePath(); + void SelectWorkspacePath(); + QString DefaultDirectory(); + + // Whether file dialog was opened previously. + bool prev_selected_; + + // Text boxes that hold the currently selected paths. + QLineEdit *image_path_text_; + QLineEdit *workspace_path_text_; + + std::string image_path_; + std::string workspace_path_; +}; + +#endif // COLMAP_SRC_UI_PROJECT_WIDGET_H_ diff --git a/src/colmap/ui/resources.qrc b/src/colmap/ui/resources.qrc new file mode 100755 index 0000000..a9df1c4 --- /dev/null +++ b/src/colmap/ui/resources.qrc @@ -0,0 +1,11 @@ + + + shaders/points.v.glsl + shaders/points.f.glsl + shaders/lines.v.glsl + shaders/lines.g.glsl + shaders/lines.f.glsl + shaders/triangles.v.glsl + shaders/triangles.f.glsl + + diff --git a/src/colmap/ui/shaders/lines.f.glsl b/src/colmap/ui/shaders/lines.f.glsl new file mode 100755 index 0000000..1e628ac --- /dev/null +++ b/src/colmap/ui/shaders/lines.f.glsl @@ -0,0 +1,8 @@ +#version 150 + +in vec4 g_color; +out vec4 f_color; + +void main(void) { + f_color = g_color; +} diff --git a/src/colmap/ui/shaders/lines.g.glsl b/src/colmap/ui/shaders/lines.g.glsl new file mode 100755 index 0000000..8814239 --- /dev/null +++ b/src/colmap/ui/shaders/lines.g.glsl @@ -0,0 +1,35 @@ +#version 150 + +layout(lines) in; +layout(triangle_strip, max_vertices = 4) out; + +uniform vec2 u_inv_viewport; +uniform float u_line_width; + +in vec4 v_pos[2]; +in vec4 v_color[2]; +out vec4 g_color; + +void main() { + vec2 dir = normalize(v_pos[1].xy / v_pos[1].w - v_pos[0].xy / v_pos[0].w); + vec2 normal_dir = vec2(-dir.y, dir.x); + vec2 offset = (vec2(u_line_width) * u_inv_viewport) * normal_dir; + + gl_Position = vec4(v_pos[0].xy + offset * v_pos[0].w, v_pos[0].z, v_pos[0].w); + g_color = v_color[0]; + EmitVertex(); + + gl_Position = vec4(v_pos[1].xy + offset * v_pos[1].w, v_pos[1].z, v_pos[1].w); + g_color = v_color[1]; + EmitVertex(); + + gl_Position = vec4(v_pos[0].xy - offset * v_pos[0].w, v_pos[0].z, v_pos[0].w); + g_color = v_color[0]; + EmitVertex(); + + gl_Position = vec4(v_pos[1].xy - offset * v_pos[1].w, v_pos[1].z, v_pos[1].w); + g_color = v_color[1]; + EmitVertex(); + + EndPrimitive(); +} diff --git a/src/colmap/ui/shaders/lines.v.glsl b/src/colmap/ui/shaders/lines.v.glsl new file mode 100755 index 0000000..c4ccdc2 --- /dev/null +++ b/src/colmap/ui/shaders/lines.v.glsl @@ -0,0 +1,13 @@ +#version 150 + +uniform mat4 u_pmv_matrix; + +in vec3 a_pos; +in vec4 a_color; +out vec4 v_pos; +out vec4 v_color; + +void main(void) { + v_pos = u_pmv_matrix * vec4(a_pos, 1); + v_color = a_color; +} diff --git a/src/colmap/ui/shaders/points.f.glsl b/src/colmap/ui/shaders/points.f.glsl new file mode 100755 index 0000000..f9351d7 --- /dev/null +++ b/src/colmap/ui/shaders/points.f.glsl @@ -0,0 +1,8 @@ +#version 150 + +in vec4 v_color; +out vec4 f_color; + +void main(void) { + f_color = v_color; +} diff --git a/src/colmap/ui/shaders/points.v.glsl b/src/colmap/ui/shaders/points.v.glsl new file mode 100755 index 0000000..6b2f853 --- /dev/null +++ b/src/colmap/ui/shaders/points.v.glsl @@ -0,0 +1,14 @@ +#version 150 + +uniform float u_point_size; +uniform mat4 u_pmv_matrix; + +in vec3 a_position; +in vec4 a_color; +out vec4 v_color; + +void main(void) { + gl_Position = u_pmv_matrix * vec4(a_position, 1); + gl_PointSize = u_point_size; + v_color = a_color; +} diff --git a/src/colmap/ui/shaders/triangles.f.glsl b/src/colmap/ui/shaders/triangles.f.glsl new file mode 100755 index 0000000..f9351d7 --- /dev/null +++ b/src/colmap/ui/shaders/triangles.f.glsl @@ -0,0 +1,8 @@ +#version 150 + +in vec4 v_color; +out vec4 f_color; + +void main(void) { + f_color = v_color; +} diff --git a/src/colmap/ui/shaders/triangles.v.glsl b/src/colmap/ui/shaders/triangles.v.glsl new file mode 100755 index 0000000..eff1fe5 --- /dev/null +++ b/src/colmap/ui/shaders/triangles.v.glsl @@ -0,0 +1,12 @@ +#version 150 + +uniform mat4 u_pmv_matrix; + +in vec3 a_position; +in vec4 a_color; +out vec4 v_color; + +void main(void) { + gl_Position = u_pmv_matrix * vec4(a_position, 1); + v_color = a_color; +} diff --git a/src/estimate_scale.cc b/src/estimate_scale.cc index 6bca900..33081d2 100644 --- a/src/estimate_scale.cc +++ b/src/estimate_scale.cc @@ -3,7 +3,7 @@ #include -#include "base/camera.h" +#include "base/camera.hpp" #include "base/map.h" #include "geometry/track_processor.h" #include "optimization/ba_solver.h" diff --git a/src/feature/feature_extraction.cc b/src/feature/feature_extraction.cc index 4e5d91c..9bde7c5 100644 --- a/src/feature/feature_extraction.cc +++ b/src/feature/feature_extraction.cc @@ -9,21 +9,20 @@ #include "base/map.h" #include "feature_processing.h" -#include "geometry/colmap/estimators/fundamental_matrix.h" +#include "estimators/fundamental_matrix.h" #include "geometry/essential.h" -#include "optim/loransac.h" -#include "optim/ransac.h" +#include "ransac/loransac.h" +#include "ransac/ransac.h" #include "sift_extractor.h" #include "utility/timer.h" namespace xrsfm { void FeatureExtract(const std::string &image_dir_path, - std::vector &frames) { + std::vector &frames, + std::vector &image_size) { #ifndef USE_ORB constexpr int _feature_num = 8192; - // std::cout << "FeatureExtract: feature number = " << _feature_num << - // "\n"; SiftExtractor sift(_feature_num); #else const int _feature_num = 2048; @@ -34,20 +33,22 @@ void FeatureExtract(const std::string &image_dir_path, ORB_SLAM2::OrbExtractor orb(_feature_num, _lever_ratio, _lever_num, _initTh, _minTh); #endif + for (int i = 0; i < frames.size(); i++) { Frame &frame = frames[i]; frame.keypoints_.clear(); const cv::Mat image = cv::imread(image_dir_path + frame.name, cv::IMREAD_IGNORE_ORIENTATION); if (image.rows == 0) { - std::cout << "Can't read " << image_dir_path + frame.name - << std::endl; + std::cout << "ERROR: fail to read image: " + << image_dir_path + frame.name << std::endl; exit(0); } + + image_size.push_back(ImageSize(image.cols, image.rows)); + #ifndef USE_ORB - // std::cout << image_dir_path + frame.name << std::endl; sift.ExtractUINT8(image, frame.keypoints_, frame.uint_descs_); - // std::cout << i << " " << frame.uint_descs_.rows() << std::endl; #else orb(image, cv::Mat(), frame.keypoints_, frame.orb_descs_); #endif diff --git a/src/feature/feature_processing.cc b/src/feature/feature_processing.cc index 687b443..ec32751 100644 --- a/src/feature/feature_processing.cc +++ b/src/feature/feature_processing.cc @@ -9,12 +9,12 @@ #include #include -#include "geometry/colmap/estimators/fundamental_matrix.h" +#include "estimators/fundamental_matrix.h" #include "geometry/epipolar_geometry.hpp" #include "geometry/essential.h" #include "base/map.h" -#include "optim/loransac.h" -#include "optim/ransac.h" +#include "ransac/loransac.h" +#include "ransac/ransac.h" #include "sift_extractor.h" #include "utility/timer.h" #include "match_expansion.h" @@ -41,7 +41,6 @@ void SetUpFramePoints(std::vector &frames) { for (auto &frame : frames) { const int num_points = frame.keypoints_.size(); frame.points.clear(); - frame.points_normalized.clear(); for (const auto &kpt : frame.keypoints_) { const auto &pt = kpt.pt; frame.points.emplace_back(pt.x, pt.y); @@ -270,16 +269,6 @@ void FeatureMatching(const std::vector &frames, SolveFundamnetalCOLMAP(points1, points2, frame_pair); } else { CHECK(false); // TODO it only work with known camera parameters - std::vector points1_normalized, points2_normalized; - for (const auto &match : frame_pair.matches) { - points1_normalized.push_back( - frame1.points_normalized[match.id1]); - points2_normalized.push_back( - frame2.points_normalized[match.id2]); - } - solve_essential(points1_normalized, points2_normalized, 10.0 / 525, - frame_pair.E, frame_pair.inlier_num, - frame_pair.inlier_mask); } const int inlier_threshold = std::max(min_num_inlier, @@ -333,7 +322,7 @@ void ExpansionAndMatching(Map &map, std::cout << "init: " << map.frame_pairs_.size() << " " << init_candidates.size() << std::endl; - std::vector> id2matched_ids(map.frames_.size()); + std::vector> id2matched_ids(map.NumFrames()); for (auto &[id1, id2] : init_candidates) { id2matched_ids[id1].insert(id2); id2matched_ids[id2].insert(id1); diff --git a/src/feature/feature_processing.h b/src/feature/feature_processing.h index 3a9c1e7..7394e92 100644 --- a/src/feature/feature_processing.h +++ b/src/feature/feature_processing.h @@ -32,7 +32,8 @@ void OrbMatch(const cv::Mat &descs1, const cv::Mat &descs2, std::vector &matches); void FeatureExtract(const std::string &image_dir_path, - std::vector &frames); + std::vector &frames, + std::vector &image_size); void FeatureMatching(const std::vector &frames, const std::vector> &candidate_pairs, diff --git a/src/feature/feature_processor.cc b/src/feature/feature_processor.cc new file mode 100644 index 0000000..92d571f --- /dev/null +++ b/src/feature/feature_processor.cc @@ -0,0 +1,233 @@ + +#include "feature_processor.h" + +#include +#include +#include +#include + +#include "base/map.h" +#include "feature/feature_processing.h" +#include "utility/io_ecim.hpp" +#include "utility/timer.h" + +namespace xrsfm { + +FeatureProcessor::FeatureProcessor(std::string images_path, + std::string output_path, + std::string matching_type, + std::string retrieval_path) + : images_path_(images_path), output_path_(output_path), + matching_type_(matching_type), retrieval_path_(retrieval_path) { + if (!std::experimental::filesystem::exists(images_path)) { + std::cout << "image path not exists :" << images_path << "\n"; + exit(-1); + } + if (!std::experimental::filesystem::exists(output_path)) { + std::cout << "output path not exists :" << output_path << "\n"; + exit(-1); + } +} + +void FeatureProcessor::GetFeatures(const std::string &images_path, + const std::vector &image_names, + const std::string &ftr_path, + const std::string &size_path, + std::vector &frames, + std::vector &image_size) { + + std::ifstream ftr_bin(ftr_path); + std::ifstream size_bin(size_path); + if (ftr_bin.good() && size_bin.good()) { + ReadFeatures(ftr_path, frames); + SetUpFramePoints(frames); + LoadImageSize(size_path, image_size); + } else { + FeatureExtract(images_path, frames, image_size); + SaveFeatures(ftr_path, frames, true); + SaveImageSize(size_path, image_size); + } +} + +void FeatureProcessor::GetInitFramePairs( + const std::string &path, const std::vector &frames, + const std::vector> id_pairs, + std::vector &frame_pairs) { + std::ifstream bin(path); + if (bin.good()) { + ReadFramePairs(path, frame_pairs); + } else { + FeatureMatching(frames, id_pairs, frame_pairs, true); + SaveFramePairs(path, frame_pairs); + } +} + +void FeatureProcessor::ExtractNearestImagePairs( + const std::map> &id2rank, const int num_candidates, + std::vector> &image_id_pairs) { + std::set> image_pair_set; + // search nv5 in candidates except for nearest images + for (const auto &[id, retrieval_results] : id2rank) { + int count = 0; + for (const auto &id2 : retrieval_results) { + auto ret = image_pair_set.insert( + std::make_pair(std::min(id, id2), std::max(id, id2))); + count++; + if (count >= num_candidates) + break; + } + } + image_id_pairs.insert(image_id_pairs.end(), image_pair_set.begin(), + image_pair_set.end()); + std::sort(image_id_pairs.begin(), image_id_pairs.end(), + [](auto &a, auto &b) { + if (a.first < b.first || + (a.first == b.first && a.second < b.second)) + return true; + return false; + }); +} + +std::tuple +FeatureProcessor::GetInitId(const int num_image, + std::vector &frame_pairs) { + std::vector> id2cor_num_vec(num_image); + std::vector> connect_number_vec(num_image); + std::map> connect_id_vec; + for (int i = 0; i < num_image; ++i) { + connect_number_vec[i] = {i, 0}; + } + + for (auto &fp : frame_pairs) { + if (fp.inlier_num < 100) + continue; // for Trafalgar + connect_number_vec[fp.id1].second++; + connect_number_vec[fp.id2].second++; + id2cor_num_vec[fp.id1][fp.id2] = fp.matches.size(); + id2cor_num_vec[fp.id2][fp.id1] = fp.matches.size(); + } + std::sort(connect_number_vec.begin(), connect_number_vec.end(), + [](const std::pair &a, const std::pair &b) { + return a.second > b.second; + }); + const int init_id1 = connect_number_vec[0].first; + int init_id2 = -1; + for (auto &[id, number] : connect_number_vec) { + if (id2cor_num_vec[init_id1].count(id) != 0 && + id2cor_num_vec[init_id1][id] >= 100) { + init_id2 = id; + break; + } + } + return std::tuple(init_id1, init_id2); +} + +void FeatureProcessor::MatchingSeq( + std::vector &frames, const std::string &fp_path, + const std::map> &id2rank, + std::vector> &id_pairs) { + const int num_frame = frames.size(); + std::set> set_pairs; + for (int i = 0; i < num_frame; ++i) { + for (int k = 1; k < 20 && i + k < num_frame; ++k) + set_pairs.insert(std::pair(i, i + k)); + } + + for (const auto &[id1, vec] : id2rank) { + if (id1 % 5 != 0) + continue; + for (auto &id2 : vec) { + if (id1 < id2) + set_pairs.insert(std::pair(id1, id2)); + else if (id2 < id1) + set_pairs.insert(std::pair(id2, id1)); + } + } + + id_pairs.assign(set_pairs.begin(), set_pairs.end()); +} + +void FeatureProcessor::Run() { + const std::string ftr_path = output_path_ + "ftr.bin"; + const std::string size_path = output_path_ + "size.bin"; + const std::string fp_init_path = output_path_ + "fp_init.bin"; + const std::string fp_path = output_path_ + "fp.bin"; + + // 2.read images + std::vector image_names; + LoadImageNames(images_path_, image_names); + const int num_image = image_names.size(); + std::cout << "Load Image Info Done.\n"; + + std::vector frames; + frames.resize(num_image); + for (int i = 0; i < num_image; ++i) { + frames[i].id = i; + frames[i].name = image_names[i]; + } + + // 2.feature extraction + std::vector image_size_vec; + GetFeatures(images_path_, image_names, ftr_path, size_path, frames, + image_size_vec); + std::cout << "Extract Features Done.\n"; + + // 3.image matching + std::map> id2rank; + std::map name2id; + for (int i = 0; i < num_image; ++i) { + name2id[image_names[i]] = i; + } + bool have_retrieval_info = + LoadRetrievalRank(retrieval_path_, name2id, id2rank); + std::cout << "Load Retrieval Info Done.\n"; + + Timer timer("%lf s\n"); + timer.start(); + + std::vector frame_pairs; + if (matching_type_ == "covisibility") { + if (!have_retrieval_info) { + std::cout << "The retrieval file is required when using " + "covisibility-based matching!!!\n"; + return 0; + } + std::vector> id_pairs; + ExtractNearestImagePairs(id2rank, 5, id_pairs); + GetInitFramePairs(fp_init_path, frames, id_pairs, frame_pairs); + std::cout << "Init Matching Done.\n"; + + constexpr int num_iteration = 5; + constexpr bool use_fundamental = true; + const auto [init_id1, init_id2] = GetInitId(num_image, frame_pairs); + + Map map; + map.frames_ = std::move(frames); + map.frame_pairs_ = std::move(frame_pairs); + ExpansionAndMatching(map, id2rank, num_iteration, image_size_vec, + init_id1, init_id2, use_fundamental, id_pairs); + } else { + std::vector> id_pairs; + if (matching_type_ == "sequential") { + if (!have_retrieval_info) { + std::cout << "Without the retrieval file, sequential matching " + "method only matches adjacent frames.\n"; + } + MatchingSeq(frames, fp_path, id2rank, id_pairs); + } else if (matching_type_ == "retrieval") { + if (!have_retrieval_info) { + std::cout << "ERROR: The retrieval file is required when using " + "retrieval-based matching!!!\n"; + return 0; + } + ExtractNearestImagePairs(id2rank, 25, id_pairs); + } + FeatureMatching(frames, id_pairs, frame_pairs, true); + } + SaveFramePairs(fp_path, frame_pairs); + + timer.stop(); + timer.print(); +} + +} // namespace xrsfm diff --git a/src/feature/feature_processor.h b/src/feature/feature_processor.h new file mode 100644 index 0000000..c51228a --- /dev/null +++ b/src/feature/feature_processor.h @@ -0,0 +1,46 @@ + +#ifndef XRSFM_SRC_FEATURE_FEATURE_PROCESSOR_H +#define XRSFM_SRC_FEATURE_FEATURE_PROCESSOR_H + +#include "base/map.h" +#include "feature/feature_processing.h" +#include "utility/io_ecim.hpp" +#include "utility/timer.h" + +namespace xrsfm { +class FeatureProcessor { + public: + FeatureProcessor(std::string images_path, std::string output_path, + std::string matching_type = "sequential", + std::string retrieval_path = ""); + + void Run(); + + private: + void GetFeatures(const std::string &images_path, + const std::vector &image_names, + const std::string &ftr_path, const std::string &size_path, + std::vector &frames, + std::vector &image_size); + void GetInitFramePairs(const std::string &path, + const std::vector &frames, + const std::vector> id_pairs, + std::vector &frame_pairs); + void + ExtractNearestImagePairs(const std::map> &id2rank, + const int num_candidates, + std::vector> &image_id_pairs); + std::tuple GetInitId(const int num_image, + std::vector &frame_pairs); + void MatchingSeq(std::vector &frames, const std::string &fp_path, + const std::map> &id2rank, + std::vector> &id_pairs); + + std::string images_path_; + std::string output_path_; + std::string matching_type_; + std::string retrieval_path_; +}; +} // namespace xrsfm + +#endif // XRSFM_SRC_FEATURE_FEATURE_PROCESSOR_H diff --git a/src/feature/match_expansion.cc b/src/feature/match_expansion.cc index 2ff8e3e..26afe13 100644 --- a/src/feature/match_expansion.cc +++ b/src/feature/match_expansion.cc @@ -3,20 +3,17 @@ namespace xrsfm { void MatchMap::PatchInit(const Map &map) { m_patch_track_num = 0; - m_patch_tracks.clear(); - m_patch_kpt_ids.resize(map.frames_.size()); - pt2patch_id_vec.resize(map.frames_.size()); - m_frames_patch_track_ids.resize(map.frames_.size()); - for (size_t i = 0; i < map.frames_.size(); ++i) { + m_patch_kpt_ids.resize(map.NumFrames()); + pt2patch_id_vec.resize(map.NumFrames()); + for (size_t i = 0; i < map.NumFrames(); ++i) { const int step_x = image_size[i].width / col_num; const int step_y = image_size[i].height / row_num; - auto &kps = map.frames_[i].keypoints_; + auto &kps = map.frame(i).keypoints_; m_patch_kpt_ids[i].resize(patch_num); pt2patch_id_vec[i].resize(kps.size()); - m_frames_patch_track_ids[i].assign(patch_num, -1); for (size_t k = 0; k < patch_num; ++k) { m_patch_kpt_ids[i][k].clear(); } @@ -35,16 +32,6 @@ void MatchMap::PatchInit(const Map &map) { } } -void MatchMap::PrintImageSize() { - std::cout << "Image Size:\n"; - for (size_t i = 0; i < image_size.size(); ++i) { - // int step_x =image_size[i].width / col_num; - // int step_y =image_size[i].height / row_num; - std::cout << "(" << i << "):(" << image_size[i].width << ", " - << image_size[i].height << ")\n"; - } -} - void MatchMap::AddTrack(int frame_id1, int frame_id2, const Match &match, const size_t num_keypoint1, const size_t num_keypoints2) { @@ -64,10 +51,8 @@ void MatchMap::AddTrack(int frame_id1, int frame_id2, const Match &match, frame2_track[idx2] = m_track_num; TrackExpansionWrapper track; track.invalid = false; - // printf("%f %f %f\n", p(0), p(1), p(2)); track.observations_.insert(std::pair(frame_id1, idx1)); track.observations_.insert(std::pair(frame_id2, idx2)); - // printf("%d\n",m_tracks.size()); m_tracks.push_back(track); m_track_num++; } else if (frame1_track[idx1] == -1) { @@ -107,40 +92,15 @@ void MatchMap::AddTrack(int frame_id1, int frame_id2, const Match &match, void MatchMap::MakeTrack(const Map &map) { // map:: m_tracks.clear(); - m_frames_track_ids.resize(map.frames_.size()); - // for (size_t i = 0; i < map.frames_.size(); i++) { - // m_frames_track_ids[i].assign(map.frames_[i].keypoints_.size(), -1); - // } - // for (auto &frame_pair : m_frame_pairs) { - // const int id1 = frame_pair.id1, id2 = frame_pair.id2; - // for (auto &match : frame_pair.matches) { - // AddTrack(id1, id2, match, map.frames_[id1].keypoints_.size(), - // map.frames_[id2].keypoints_.size()); - // } - // } - for (size_t i = 0; i < map.frames_.size(); i++) { - m_frames_track_ids[i].assign(map.frames_[i].track_ids_.size(), -1); - } - for (auto &frame_pair : m_frame_pairs) { - const int id1 = frame_pair.id1, id2 = frame_pair.id2; - for (auto &match : frame_pair.matches) { - AddTrack(id1, id2, match, map.frames_[id1].track_ids_.size(), - map.frames_[id2].track_ids_.size()); - } - } -} - -void MatchMap::MakeTrack(const std::vector &frames) { // map:: - m_tracks.clear(); - m_frames_track_ids.resize(frames.size()); - for (size_t i = 0; i < frames.size(); i++) { - m_frames_track_ids[i].assign(frames[i].track_ids_.size(), -1); + m_frames_track_ids.resize(map.NumFrames()); + for (size_t i = 0; i < map.NumFrames(); i++) { + m_frames_track_ids[i].assign(map.frame(i).track_ids_.size(), -1); } for (auto &frame_pair : m_frame_pairs) { const int id1 = frame_pair.id1, id2 = frame_pair.id2; for (auto &match : frame_pair.matches) { - AddTrack(id1, id2, match, frames[id1].track_ids_.size(), - frames[id2].track_ids_.size()); + AddTrack(id1, id2, match, map.frame(id1).track_ids_.size(), + map.frame(id2).track_ids_.size()); } } } @@ -256,20 +216,11 @@ void MatchMap::LoadRetrievalRank( } } -void MatchMap::LoadMatch(const Map &map, std::vector &frame_pairs) { - frame_pairs.reserve(map.frame_pairs_.size()); - for (const auto &frame_pair : map.frame_pairs_) { - frame_pairs.emplace_back(frame_pair); - frame_pairs.back().inlier_num_f = frame_pair.matches.size(); - } -} - void MatchMap::MakeCorrGraph(const Map &map) { // map:: MatchExpansionSolver::CheckConsistenceOfFrameIdAndIndex(map); if (corr_graph.frame_node_vec_.size() == m_num_frame) { for (const auto &frame : map.frames_) { - corr_graph.frame_node_vec_[frame.id].num_observations = - 0; //(TODO)Is it ok to use frame.id + corr_graph.frame_node_vec_[frame.id].num_observations = 0; corr_graph.frame_node_vec_[frame.id].num_visible_point3d = 0; } } else { @@ -301,29 +252,6 @@ void MatchMap::MakeCorrGraph(const Map &map) { // map:: } } -int MatchMap::GetMatches(int frame_id1, int frame_id2, - std::vector &matches) { - const auto &track_ids = m_frames_track_ids[frame_id1]; - int count = 0; - matches.resize(0); - for (size_t i = 0; i < track_ids.size(); ++i) { - if (track_ids[i] == -1) - continue; - TrackExpansionWrapper &track = m_tracks[track_ids[i]]; - if (track.invalid) - continue; - auto &obs_infos = track.observations_; - for (auto &obs_info : obs_infos) { - if (obs_info.first == frame_id2) { - count++; - matches.emplace_back(Match(i, obs_info.second)); - break; - } - } - } - return count; -} - int MatchMap::GetMatcheNum(int frame_id1, int frame_id2) { int count = 0; for (const auto &track_id : m_frames_track_ids[frame_id1]) { @@ -344,9 +272,9 @@ int MatchMap::GetMatcheNum(int frame_id1, int frame_id2) { //////////////////////////////////////// void MatchExpansionSolver::CheckConsistenceOfFrameIdAndIndex(const Map &map) { - for (int i = 0; i < map.frames_.size(); i++) { - CHECK(i == map.frames_[i].id) - << "Error: frame id" << map.frames_[i].id << " != " << i << "\n"; + for (int i = 0; i < map.NumFrames(); i++) { + CHECK(i == map.frame(i).id) + << "Error: frame id" << map.frame(i).id << " != " << i << "\n"; } } @@ -358,7 +286,7 @@ void MatchExpansionSolver::SetUp( m_initial_frame2 = initial_frame2; m_matchmap.SetUpImageSize(image_size_vec); - m_matchmap.SetUpFrameSize(map.frames_.size()); + m_matchmap.SetUpFrameSize(map.NumFrames()); m_matchmap.m_frame_pairs = map.frame_pairs_; for (auto &fp : m_matchmap.m_frame_pairs) { fp.inlier_num_f = fp.matches.size(); @@ -437,7 +365,7 @@ void MatchExpansionSolver::Run(const Map &map, std::cout << "#full candidates: " << candidates.size() << "\n"; // remove duplicate frame pairs - std::vector> t_pair_list(map.frames_.size()); + std::vector> t_pair_list(map.NumFrames()); for (const auto &fp : candidates) { if (fp.id1 < fp.id2) t_pair_list[fp.id1].insert(fp.id2); @@ -514,15 +442,6 @@ void MatchExpansionSolver::GetConnectedFrames(const Map &map, } } -void MatchExpansionSolver::PrintRetrievalMap() { - std::cout << "retrievalframepairs_map"; - for (size_t i = 0; i < id2rank_vec.size(); i++) { - for (const auto &item : id2rank_vec[i]) { - std::cout << i << " - " << item.first << ":" << item.second << "\n"; - } - } -} - int MatchExpansionSolver::GetInitFramePairId( const std::vector> &id_pair) { CHECK(id_pair[m_initial_frame1].count(m_initial_frame2) != 0) diff --git a/src/feature/match_expansion.h b/src/feature/match_expansion.h index 5f05477..0f1ad6a 100644 --- a/src/feature/match_expansion.h +++ b/src/feature/match_expansion.h @@ -33,11 +33,7 @@ class MatchMap { void MakeTrack(const Map &map); - void MakeTrack(const std::vector &frames); - - std::vector> MakeIdPair(); // map:: - - int GetMatches(int frame_id1, int frame_id2, std::vector &matches); + std::vector> MakeIdPair(); int GetMatcheNum(int frame_id1, int frame_id2); @@ -48,8 +44,6 @@ class MatchMap { std::map> &retrieval_rank_of_frames, bool skip_matches = true); - static void LoadMatch(const Map &map, std::vector &frame_pairs); - void MakeCorrGraph(const Map &map); void SetUpImageSize(const std::vector &images) { @@ -65,16 +59,14 @@ class MatchMap { void PrintFramesTrack(); void PrintPatchIdOfFeatures(); void PrintFeaturesOfPatch(); - void PrintImageSize(); + size_t m_num_frame; + size_t m_track_num; std::vector image_size; - std::vector m_frame_pairs; - size_t m_num_frame; - CorrespondenceGraph corr_graph; std::vector> m_frames_track_ids; std::vector m_tracks; - size_t m_track_num; + CorrespondenceGraph corr_graph; struct PatchTrack { std::map m_observations; @@ -86,10 +78,8 @@ class MatchMap { int row_num; int patch_num; int m_patch_track_num; - std::vector m_patch_tracks; std::vector> pt2patch_id_vec; std::vector>> m_patch_kpt_ids; - std::vector> m_frames_patch_track_ids; std::vector> id2pair_id; bool verbose; @@ -111,8 +101,6 @@ class MatchExpansionSolver { private: void Print(const std::vector &x_set); - void PrintRetrievalMap(); - std::vector> GetId2RankVec( const std::map> &retrieval_rank_of_frames); @@ -140,9 +128,9 @@ class MatchExpansionSolver { std::map> GetCovisibilityInfo(int frame_id); size_t m_initial_frame1, m_initial_frame2; - std::vector> - id2rank_vec; // retrievalframepairs_map[frame_i][frame_j] = the - // rank of frame_j in candidates of frame_i + std::vector> id2rank_vec; + // retrievalframepairs_map[frame_i][frame_j] = the + // rank of frame_j in candidates of frame_i MatchMap m_matchmap; const bool verbose = false; diff --git a/src/geometry/colmap/base/camera.h b/src/geometry/colmap/base/camera.h deleted file mode 100644 index 0183773..0000000 --- a/src/geometry/colmap/base/camera.h +++ /dev/null @@ -1,208 +0,0 @@ -// Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of -// its contributors may be used to endorse or promote products derived -// from this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// -// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) - -#ifndef COLMAP_SRC_BASE_CAMERA_H_ -#define COLMAP_SRC_BASE_CAMERA_H_ - -#include - -#include "geometry/colmap/util/types.h" - -namespace colmap { - -// Camera class that holds the intrinsic parameters. Cameras may be shared -// between multiple images, e.g., if the same "physical" camera took multiple -// pictures with the exact same lens and intrinsics (focal length, etc.). -// This class has a specific distortion model defined by a camera model class. -class Camera { - public: - Camera(); - - // Access the unique identifier of the camera. - inline camera_t CameraId() const; - inline void SetCameraId(const camera_t camera_id); - - // Access the camera model. - inline int ModelId() const; - std::string ModelName() const; - void SetModelId(const int model_id); - void SetModelIdFromName(const std::string &model_name); - - // Access dimensions of the camera sensor. - inline size_t Width() const; - inline size_t Height() const; - inline void SetWidth(const size_t width); - inline void SetHeight(const size_t height); - - // Access focal length parameters. - double MeanFocalLength() const; - double FocalLength() const; - double FocalLengthX() const; - double FocalLengthY() const; - void SetFocalLength(const double focal_length); - void SetFocalLengthX(const double focal_length_x); - void SetFocalLengthY(const double focal_length_y); - - // Check if camera has prior focal length. - inline bool HasPriorFocalLength() const; - inline void SetPriorFocalLength(const bool prior); - - // Access principal point parameters. Only works if there are two - // principal point parameters. - double PrincipalPointX() const; - double PrincipalPointY() const; - void SetPrincipalPointX(const double ppx); - void SetPrincipalPointY(const double ppy); - - // Get the indices of the parameter groups in the parameter vector. - const std::vector &FocalLengthIdxs() const; - const std::vector &PrincipalPointIdxs() const; - const std::vector &ExtraParamsIdxs() const; - - // Get intrinsic calibration matrix composed from focal length and principal - // point parameters, excluding distortion parameters. - Eigen::Matrix3d CalibrationMatrix() const; - - // Get human-readable information about the parameter vector ordering. - std::string ParamsInfo() const; - - // Access the raw parameter vector. - inline size_t NumParams() const; - inline const std::vector &Params() const; - inline std::vector &Params(); - inline double Params(const size_t idx) const; - inline double &Params(const size_t idx); - inline const double *ParamsData() const; - inline double *ParamsData(); - inline void SetParams(const std::vector ¶ms); - - // Concatenate parameters as comma-separated list. - std::string ParamsToString() const; - - // Set camera parameters from comma-separated list. - bool SetParamsFromString(const std::string &string); - - // Check whether parameters are valid, i.e. the parameter vector has - // the correct dimensions that match the specified camera model. - bool VerifyParams() const; - - // Check whether camera has bogus parameters. - bool HasBogusParams(const double min_focal_length_ratio, - const double max_focal_length_ratio, - const double max_extra_param) const; - - // Initialize parameters for given camera model and focal length, and set - // the principal point to be the image center. - void InitializeWithId(const int model_id, const double focal_length, - const size_t width, const size_t height); - void InitializeWithName(const std::string &model_name, - const double focal_length, const size_t width, - const size_t height); - - // Project point in image plane to world / infinity. - Eigen::Vector2d ImageToWorld(const Eigen::Vector2d &image_point) const; - - // Convert pixel threshold in image plane to world space. - double ImageToWorldThreshold(const double threshold) const; - - // Project point from world / infinity to image plane. - Eigen::Vector2d WorldToImage(const Eigen::Vector2d &world_point) const; - - // Rescale camera dimensions and accordingly the focal length and - // and the principal point. - void Rescale(const double scale); - void Rescale(const size_t width, const size_t height); - - private: - // The unique identifier of the camera. If the identifier is not specified - // it is set to `kInvalidCameraId`. - camera_t camera_id_; - - // The identifier of the camera model. If the camera model is not specified - // the identifier is `kInvalidCameraModelId`. - int model_id_; - - // The dimensions of the image, 0 if not initialized. - size_t width_; - size_t height_; - - // The focal length, principal point, and extra parameters. If the camera - // model is not specified, this vector is empty. - std::vector params_; - - // Whether there is a safe prior for the focal length, - // e.g. manually provided or extracted from EXIF - bool prior_focal_length_; -}; - -//////////////////////////////////////////////////////////////////////////////// -// Implementation -//////////////////////////////////////////////////////////////////////////////// - -camera_t Camera::CameraId() const { return camera_id_; } - -void Camera::SetCameraId(const camera_t camera_id) { camera_id_ = camera_id; } - -int Camera::ModelId() const { return model_id_; } - -size_t Camera::Width() const { return width_; } - -size_t Camera::Height() const { return height_; } - -void Camera::SetWidth(const size_t width) { width_ = width; } - -void Camera::SetHeight(const size_t height) { height_ = height; } - -bool Camera::HasPriorFocalLength() const { return prior_focal_length_; } - -void Camera::SetPriorFocalLength(const bool prior) { - prior_focal_length_ = prior; -} - -size_t Camera::NumParams() const { return params_.size(); } - -const std::vector &Camera::Params() const { return params_; } - -std::vector &Camera::Params() { return params_; } - -double Camera::Params(const size_t idx) const { return params_[idx]; } - -double &Camera::Params(const size_t idx) { return params_[idx]; } - -const double *Camera::ParamsData() const { return params_.data(); } - -double *Camera::ParamsData() { return params_.data(); } - -void Camera::SetParams(const std::vector ¶ms) { params_ = params; } - -} // namespace colmap - -#endif // COLMAP_SRC_BASE_CAMERA_H_ diff --git a/src/geometry/colmap/base/pose.cc b/src/geometry/colmap/base/pose.cc deleted file mode 100644 index b1c2606..0000000 --- a/src/geometry/colmap/base/pose.cc +++ /dev/null @@ -1,250 +0,0 @@ -// Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of -// its contributors may be used to endorse or promote products derived -// from this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// -// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) - -#include "base/pose.h" - -#include - -#include "base/projection.h" -#include "base/triangulation.h" - -namespace colmap { - -Eigen::Matrix3d CrossProductMatrix(const Eigen::Vector3d &vector) { - Eigen::Matrix3d matrix; - matrix << 0, -vector(2), vector(1), vector(2), 0, -vector(0), -vector(1), - vector(0), 0; - return matrix; -} - -void RotationMatrixToEulerAngles(const Eigen::Matrix3d &R, double *rx, - double *ry, double *rz) { - *rx = std::atan2(-R(1, 2), R(2, 2)); - *ry = std::asin(R(0, 2)); - *rz = std::atan2(-R(0, 1), R(0, 0)); - - *rx = IsNaN(*rx) ? 0 : *rx; - *ry = IsNaN(*ry) ? 0 : *ry; - *rz = IsNaN(*rz) ? 0 : *rz; -} - -Eigen::Matrix3d EulerAnglesToRotationMatrix(const double rx, const double ry, - const double rz) { - const Eigen::Matrix3d Rx = - Eigen::AngleAxisd(rx, Eigen::Vector3d::UnitX()).toRotationMatrix(); - const Eigen::Matrix3d Ry = - Eigen::AngleAxisd(ry, Eigen::Vector3d::UnitY()).toRotationMatrix(); - const Eigen::Matrix3d Rz = - Eigen::AngleAxisd(rz, Eigen::Vector3d::UnitZ()).toRotationMatrix(); - return Rz * Ry * Rx; -} - -Eigen::Vector4d RotationMatrixToQuaternion(const Eigen::Matrix3d &rot_mat) { - const Eigen::Quaterniond quat(rot_mat); - return Eigen::Vector4d(quat.w(), quat.x(), quat.y(), quat.z()); -} - -Eigen::Matrix3d QuaternionToRotationMatrix(const Eigen::Vector4d &qvec) { - const Eigen::Vector4d normalized_qvec = NormalizeQuaternion(qvec); - const Eigen::Quaterniond quat(normalized_qvec(0), normalized_qvec(1), - normalized_qvec(2), normalized_qvec(3)); - return quat.toRotationMatrix(); -} - -Eigen::Vector4d NormalizeQuaternion(const Eigen::Vector4d &qvec) { - const double norm = qvec.norm(); - if (norm == 0) { - // We do not just use (1, 0, 0, 0) because that is a constant and when - // used for automatic differentiation that would lead to a zero - // derivative. - return Eigen::Vector4d(1.0, qvec(1), qvec(2), qvec(3)); - } else { - return qvec / norm; - } -} - -Eigen::Vector4d InvertQuaternion(const Eigen::Vector4d &qvec) { - return Eigen::Vector4d(qvec(0), -qvec(1), -qvec(2), -qvec(3)); -} - -Eigen::Vector4d ConcatenateQuaternions(const Eigen::Vector4d &qvec1, - const Eigen::Vector4d &qvec2) { - const Eigen::Vector4d normalized_qvec1 = NormalizeQuaternion(qvec1); - const Eigen::Vector4d normalized_qvec2 = NormalizeQuaternion(qvec2); - const Eigen::Quaterniond quat1(normalized_qvec1(0), normalized_qvec1(1), - normalized_qvec1(2), normalized_qvec1(3)); - const Eigen::Quaterniond quat2(normalized_qvec2(0), normalized_qvec2(1), - normalized_qvec2(2), normalized_qvec2(3)); - const Eigen::Quaterniond cat_quat = quat2 * quat1; - return NormalizeQuaternion(Eigen::Vector4d(cat_quat.w(), cat_quat.x(), - cat_quat.y(), cat_quat.z())); -} - -Eigen::Vector3d QuaternionRotatePoint(const Eigen::Vector4d &qvec, - const Eigen::Vector3d &point) { - const Eigen::Vector4d normalized_qvec = NormalizeQuaternion(qvec); - const Eigen::Quaterniond quat(normalized_qvec(0), normalized_qvec(1), - normalized_qvec(2), normalized_qvec(3)); - return quat * point; -} - -Eigen::Vector4d AverageQuaternions(const std::vector &qvecs, - const std::vector &weights) { - CHECK_EQ(qvecs.size(), weights.size()); - CHECK_GT(qvecs.size(), 0); - - if (qvecs.size() == 1) { - return qvecs[0]; - } - - Eigen::Matrix4d A = Eigen::Matrix4d::Zero(); - double weight_sum = 0; - - for (size_t i = 0; i < qvecs.size(); ++i) { - CHECK_GT(weights[i], 0); - const Eigen::Vector4d qvec = NormalizeQuaternion(qvecs[i]); - A += weights[i] * qvec * qvec.transpose(); - weight_sum += weights[i]; - } - - A.array() /= weight_sum; - - const Eigen::Matrix4d eigenvectors = - Eigen::SelfAdjointEigenSolver(A).eigenvectors(); - - return eigenvectors.col(3); -} - -Eigen::Matrix3d RotationFromUnitVectors(const Eigen::Vector3d &vector1, - const Eigen::Vector3d &vector2) { - const Eigen::Vector3d v1 = vector1.normalized(); - const Eigen::Vector3d v2 = vector2.normalized(); - const Eigen::Vector3d v = v1.cross(v2); - const Eigen::Matrix3d v_x = CrossProductMatrix(v); - const double c = v1.dot(v2); - if (c == -1) { - return Eigen::Matrix3d::Identity(); - } else { - return Eigen::Matrix3d::Identity() + v_x + 1 / (1 + c) * (v_x * v_x); - } -} - -Eigen::Vector3d -ProjectionCenterFromMatrix(const Eigen::Matrix3x4d &proj_matrix) { - return -proj_matrix.leftCols<3>().transpose() * proj_matrix.rightCols<1>(); -} - -Eigen::Vector3d ProjectionCenterFromPose(const Eigen::Vector4d &qvec, - const Eigen::Vector3d &tvec) { - // Inverse rotation as conjugate quaternion. - const Eigen::Vector4d normalized_qvec = NormalizeQuaternion(qvec); - const Eigen::Quaterniond quat(normalized_qvec(0), -normalized_qvec(1), - -normalized_qvec(2), -normalized_qvec(3)); - return quat * -tvec; -} - -void ComputeRelativePose(const Eigen::Vector4d &qvec1, - const Eigen::Vector3d &tvec1, - const Eigen::Vector4d &qvec2, - const Eigen::Vector3d &tvec2, Eigen::Vector4d *qvec12, - Eigen::Vector3d *tvec12) { - const Eigen::Vector4d inv_qvec1 = InvertQuaternion(qvec1); - *qvec12 = ConcatenateQuaternions(inv_qvec1, qvec2); - *tvec12 = tvec2 - QuaternionRotatePoint(*qvec12, tvec1); -} - -void ConcatenatePoses(const Eigen::Vector4d &qvec1, - const Eigen::Vector3d &tvec1, - const Eigen::Vector4d &qvec2, - const Eigen::Vector3d &tvec2, Eigen::Vector4d *qvec12, - Eigen::Vector3d *tvec12) { - *qvec12 = ConcatenateQuaternions(qvec1, qvec2); - *tvec12 = tvec2 + QuaternionRotatePoint(qvec2, tvec1); -} - -void InvertPose(const Eigen::Vector4d &qvec, const Eigen::Vector3d &tvec, - Eigen::Vector4d *inv_qvec, Eigen::Vector3d *inv_tvec) { - *inv_qvec = InvertQuaternion(qvec); - *inv_tvec = -QuaternionRotatePoint(*inv_qvec, tvec); -} - -void InterpolatePose(const Eigen::Vector4d &qvec1, const Eigen::Vector3d &tvec1, - const Eigen::Vector4d &qvec2, const Eigen::Vector3d &tvec2, - const double t, Eigen::Vector4d *qveci, - Eigen::Vector3d *tveci) { - const Eigen::Vector4d normalized_qvec1 = NormalizeQuaternion(qvec1); - const Eigen::Vector4d normalized_qvec2 = NormalizeQuaternion(qvec2); - const Eigen::Quaterniond quat1(normalized_qvec1(0), normalized_qvec1(1), - normalized_qvec1(2), normalized_qvec1(3)); - const Eigen::Quaterniond quat2(normalized_qvec2(0), normalized_qvec2(1), - normalized_qvec2(2), normalized_qvec2(3)); - const Eigen::Vector3d tvec12 = tvec2 - tvec1; - - const Eigen::Quaterniond quati = quat1.slerp(t, quat2); - - *qveci = Eigen::Vector4d(quati.w(), quati.x(), quati.y(), quati.z()); - *tveci = tvec1 + tvec12 * t; -} - -Eigen::Vector3d CalculateBaseline(const Eigen::Vector4d &qvec1, - const Eigen::Vector3d &tvec1, - const Eigen::Vector4d &qvec2, - const Eigen::Vector3d &tvec2) { - const Eigen::Vector3d center1 = ProjectionCenterFromPose(qvec1, tvec1); - const Eigen::Vector3d center2 = ProjectionCenterFromPose(qvec2, tvec2); - return center2 - center1; -} - -bool CheckCheirality(const Eigen::Matrix3d &R, const Eigen::Vector3d &t, - const std::vector &points1, - const std::vector &points2, - std::vector *points3D) { - CHECK_EQ(points1.size(), points2.size()); - const Eigen::Matrix3x4d proj_matrix1 = Eigen::Matrix3x4d::Identity(); - const Eigen::Matrix3x4d proj_matrix2 = ComposeProjectionMatrix(R, t); - const double kMinDepth = std::numeric_limits::epsilon(); - const double max_depth = 1000.0f * (R.transpose() * t).norm(); - points3D->clear(); - for (size_t i = 0; i < points1.size(); ++i) { - const Eigen::Vector3d point3D = TriangulatePoint( - proj_matrix1, proj_matrix2, points1[i], points2[i]); - const double depth1 = CalculateDepth(proj_matrix1, point3D); - if (depth1 > kMinDepth && depth1 < max_depth) { - const double depth2 = CalculateDepth(proj_matrix2, point3D); - if (depth2 > kMinDepth && depth2 < max_depth) { - points3D->push_back(point3D); - } - } - } - return !points3D->empty(); -} - -} // namespace colmap diff --git a/src/geometry/colmap/base/pose.h b/src/geometry/colmap/base/pose.h deleted file mode 100644 index 0df5fbe..0000000 --- a/src/geometry/colmap/base/pose.h +++ /dev/null @@ -1,229 +0,0 @@ -// Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of -// its contributors may be used to endorse or promote products derived -// from this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// -// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) - -#ifndef COLMAP_SRC_BASE_POSE_H_ -#define COLMAP_SRC_BASE_POSE_H_ - -#include -#include - -//#include "util/alignment.h" -#include "geometry/colmap/util/types.h" - -namespace colmap { - -// Compose the skew symmetric cross product matrix from a vector. -Eigen::Matrix3d CrossProductMatrix(const Eigen::Vector3d &vector); - -// Convert 3D rotation matrix to Euler angles. -// -// The convention `R = Rx * Ry * Rz` is used, -// using a right-handed coordinate system. -// -// @param R 3x3 rotation matrix. -// @param rx, ry, rz Euler angles in radians. -void RotationMatrixToEulerAngles(const Eigen::Matrix3d &R, double *rx, - double *ry, double *rz); - -// Convert Euler angles to 3D rotation matrix. -// -// The convention `R = Rz * Ry * Rx` is used, -// using a right-handed coordinate system. -// -// @param rx, ry, rz Euler angles in radians. -// -// @return 3x3 rotation matrix. -Eigen::Matrix3d EulerAnglesToRotationMatrix(const double rx, const double ry, - const double rz); - -// Convert 3D rotation matrix to Quaternion representation. -// -// @param rot_mat 3x3 rotation matrix. -// -// @return Unit Quaternion rotation coefficients (w, x, y, z). -Eigen::Vector4d RotationMatrixToQuaternion(const Eigen::Matrix3d &rot_mat); - -// Convert Quaternion representation to 3D rotation matrix. -// -// @param qvec Unit Quaternion rotation coefficients (w, x, y, z). -// -// @return 3x3 rotation matrix. -Eigen::Matrix3d QuaternionToRotationMatrix(const Eigen::Vector4d &qvec); - -// Compose the Quaternion vector corresponding to a identity transformation. -inline Eigen::Vector4d ComposeIdentityQuaternion(); - -// Normalize Quaternion vector. -// -// @param qvec Quaternion rotation coefficients (w, x, y, z). -// -// @return Unit Quaternion rotation coefficients (w, x, y, z). -Eigen::Vector4d NormalizeQuaternion(const Eigen::Vector4d &qvec); - -// Invert Quaternion vector to return Quaternion of inverse rotation. -// -// @param qvec Quaternion rotation coefficients (w, x, y, z). -// -// @return Inverse Quaternion rotation coefficients (w, x, y, z). -Eigen::Vector4d InvertQuaternion(const Eigen::Vector4d &qvec); - -// Concatenate Quaternion rotations such that the rotation of `qvec1` is applied -// before the rotation of `qvec2`. -// -// @param qvec1 Quaternion rotation coefficients (w, x, y, z). -// @param qvec2 Quaternion rotation coefficients (w, x, y, z). -// -// @return Concatenated Quaternion coefficients (w, x, y, z). -Eigen::Vector4d ConcatenateQuaternions(const Eigen::Vector4d &qvec1, - const Eigen::Vector4d &qvec2); - -// Transform point by quaternion rotation. -// -// @param qvec Quaternion rotation coefficients (w, x, y, z). -// @param point Point to rotate. -// -// @return Rotated point. -Eigen::Vector3d QuaternionRotatePoint(const Eigen::Vector4d &qvec, - const Eigen::Vector3d &point); - -// Compute the weighted average of multiple Quaternions according to: -// -// Markley, F. Landis, et al. "Averaging quaternions." -// Journal of Guidance, Control, and Dynamics 30.4 (2007): 1193-1197. -// -// @param qvecs The Quaternions to be averaged. -// @param weights Non-negative weights. -// -// @return The average Quaternion. -Eigen::Vector4d AverageQuaternions(const std::vector &qvecs, - const std::vector &weights); - -// Compose rotation matrix that rotates unit vector 1 to unit vector 2. -// Note that when vector 1 points into the opposite direction of vector 2, -// the function returns an identity rotation. -Eigen::Matrix3d RotationFromUnitVectors(const Eigen::Vector3d &vec1, - const Eigen::Vector3d &vec2); - -// Extract camera projection center from projection matrix, i.e. the projection -// center in world coordinates `-R^T t`. -Eigen::Vector3d -ProjectionCenterFromMatrix(const Eigen::Matrix3x4d &proj_matrix); - -// Extract camera projection center from projection parameters. -// -// @param qvec Unit Quaternion rotation coefficients (w, x, y, z). -// @param tvec 3x1 translation vector. -// -// @return 3x1 camera projection center. -Eigen::Vector3d ProjectionCenterFromPose(const Eigen::Vector4d &qvec, - const Eigen::Vector3d &tvec); - -// Compute the relative transformation from pose 1 to 2. -// -// @param qvec1, tvec1 First camera pose. -// @param qvec2, tvec2 Second camera pose. -// @param qvec12, tvec12 Relative pose. -void ComputeRelativePose(const Eigen::Vector4d &qvec1, - const Eigen::Vector3d &tvec1, - const Eigen::Vector4d &qvec2, - const Eigen::Vector3d &tvec2, Eigen::Vector4d *qvec12, - Eigen::Vector3d *tvec12); - -// Concatenate the transformations of the two poses. -// -// @param qvec1, tvec1 First camera pose. -// @param qvec2, tvec2 Second camera pose. -// @param qvec12, tvec12 Concatenated pose. -void ConcatenatePoses(const Eigen::Vector4d &qvec1, - const Eigen::Vector3d &tvec1, - const Eigen::Vector4d &qvec2, - const Eigen::Vector3d &tvec2, Eigen::Vector4d *qvec12, - Eigen::Vector3d *tvec12); - -// Invert transformation of the pose. -// @param qvec, tvec Input camera pose. -// @param inv_qvec, inv_tvec Inverse camera pose. -void InvertPose(const Eigen::Vector4d &qvec, const Eigen::Vector3d &tvec, - Eigen::Vector4d *inv_qvec, Eigen::Vector3d *inv_tvec); - -// Linearly interpolate camera pose. -// -// @param qvec1, tvec1 Camera pose at t0 = 0. -// @param qvec2, tvec2 Camera pose at t1 = 1. -// @param t Interpolation time. -// @param qveci, tveci Camera pose at time t. -void InterpolatePose(const Eigen::Vector4d &qvec1, const Eigen::Vector3d &tvec1, - const Eigen::Vector4d &qvec2, const Eigen::Vector3d &tvec2, - const double t, Eigen::Vector4d *qveci, - Eigen::Vector3d *tveci); - -// Calculate baseline vector from first to second pose. -// -// The given rotation and orientation is expected as the -// world to camera transformation. -// -// @param qvec1 Unit Quaternion rotation coefficients (w, x, y, z). -// @param tvec1 3x1 translation vector. -// @param qvec2 Unit Quaternion rotation coefficients (w, x, y, z). -// @param tvec2 3x1 translation vector. -// -// @return Baseline vector from 1 to 2. -Eigen::Vector3d CalculateBaseline(const Eigen::Vector4d &qvec1, - const Eigen::Vector3d &tvec1, - const Eigen::Vector4d &qvec2, - const Eigen::Vector3d &tvec2); - -// Perform cheirality constraint test, i.e., determine which of the triangulated -// correspondences lie in front of of both cameras. The first camera has the -// projection matrix P1 = [I | 0] and the second camera has the projection -// matrix P2 = [R | t]. -// -// @param R 3x3 rotation matrix of second projection matrix. -// @param t 3x1 translation vector of second projection matrix. -// @param points1 First set of corresponding points. -// @param points2 Second set of corresponding points. -// @param points3D Points that lie in front of both cameras. -bool CheckCheirality(const Eigen::Matrix3d &R, const Eigen::Vector3d &t, - const std::vector &points1, - const std::vector &points2, - std::vector *points3D); - -//////////////////////////////////////////////////////////////////////////////// -// Implementation -//////////////////////////////////////////////////////////////////////////////// - -Eigen::Vector4d ComposeIdentityQuaternion() { - return Eigen::Vector4d(1, 0, 0, 0); -} - -} // namespace colmap - -#endif // COLMAP_SRC_BASE_POSE_H_ diff --git a/src/geometry/colmap/base/triangulation.cc b/src/geometry/colmap/base/triangulation.cc deleted file mode 100644 index 202bc49..0000000 --- a/src/geometry/colmap/base/triangulation.cc +++ /dev/null @@ -1,186 +0,0 @@ -// Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of -// its contributors may be used to endorse or promote products derived -// from this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// -// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) - -#include "triangulation.h" - -#include -//#include "base/essential_matrix.h" -//#include "base/pose.h" - -namespace colmap { - -Eigen::Vector3d TriangulatePoint(const Eigen::Matrix3x4d &proj_matrix1, - const Eigen::Matrix3x4d &proj_matrix2, - const Eigen::Vector2d &point1, - const Eigen::Vector2d &point2) { - Eigen::Matrix4d A; - - A.row(0) = point1(0) * proj_matrix1.row(2) - proj_matrix1.row(0); - A.row(1) = point1(1) * proj_matrix1.row(2) - proj_matrix1.row(1); - A.row(2) = point2(0) * proj_matrix2.row(2) - proj_matrix2.row(0); - A.row(3) = point2(1) * proj_matrix2.row(2) - proj_matrix2.row(1); - - Eigen::JacobiSVD svd(A, Eigen::ComputeFullV); - - return svd.matrixV().col(3).hnormalized(); -} - -std::vector -TriangulatePoints(const Eigen::Matrix3x4d &proj_matrix1, - const Eigen::Matrix3x4d &proj_matrix2, - const std::vector &points1, - const std::vector &points2) { - CHECK_EQ(points1.size(), points2.size()); - - std::vector points3D(points1.size()); - - for (size_t i = 0; i < points3D.size(); ++i) { - points3D[i] = TriangulatePoint(proj_matrix1, proj_matrix2, points1[i], - points2[i]); - } - - return points3D; -} - -Eigen::Vector3d -TriangulateMultiViewPoint(const std::vector &proj_matrices, - const std::vector &points) { - CHECK_EQ(proj_matrices.size(), points.size()); - - Eigen::Matrix4d A = Eigen::Matrix4d::Zero(); - - for (size_t i = 0; i < points.size(); i++) { - const Eigen::Vector3d point = points[i].homogeneous().normalized(); - const Eigen::Matrix3x4d term = - proj_matrices[i] - point * point.transpose() * proj_matrices[i]; - A += term.transpose() * term; - } - - Eigen::SelfAdjointEigenSolver eigen_solver(A); - - return eigen_solver.eigenvectors().col(0).hnormalized(); -} - -// Eigen::Vector3d TriangulateOptimalPoint(const Eigen::Matrix3x4d& -// proj_matrix1, -// const Eigen::Matrix3x4d& proj_matrix2, -// const Eigen::Vector2d& point1, -// const Eigen::Vector2d& point2) { -// const Eigen::Matrix3d E = -// EssentialMatrixFromAbsolutePoses(proj_matrix1, proj_matrix2); -// -// Eigen::Vector2d optimal_point1; -// Eigen::Vector2d optimal_point2; -// FindOptimalImageObservations(E, point1, point2, &optimal_point1, -// &optimal_point2); -// -// return TriangulatePoint(proj_matrix1, proj_matrix2, optimal_point1, -// optimal_point2); -//} - -// std::vector TriangulateOptimalPoints( -// const Eigen::Matrix3x4d& proj_matrix1, -// const Eigen::Matrix3x4d& proj_matrix2, -// const std::vector& points1, -// const std::vector& points2) { -// std::vector points3D(points1.size()); -// -// for (size_t i = 0; i < points3D.size(); ++i) { -// points3D[i] = -// TriangulatePoint(proj_matrix1, proj_matrix2, points1[i], points2[i]); -// } -// -// return points3D; -//} - -double CalculateTriangulationAngle(const Eigen::Vector3d &proj_center1, - const Eigen::Vector3d &proj_center2, - const Eigen::Vector3d &point3D) { - const double baseline_length_squared = - (proj_center1 - proj_center2).squaredNorm(); - - const double ray_length_squared1 = (point3D - proj_center1).squaredNorm(); - const double ray_length_squared2 = (point3D - proj_center2).squaredNorm(); - - // Using "law of cosines" to compute the enclosing angle between rays. - const double denominator = - 2.0 * std::sqrt(ray_length_squared1 * ray_length_squared2); - if (denominator == 0.0) { - return 0.0; - } - const double nominator = - ray_length_squared1 + ray_length_squared2 - baseline_length_squared; - const double angle = std::abs(std::acos(nominator / denominator)); - - // Triangulation is unstable for acute angles (far away points) and - // obtuse angles (close points), so always compute the minimum angle - // between the two intersecting rays. - return std::min(angle, M_PI - angle); -} - -std::vector -CalculateTriangulationAngles(const Eigen::Vector3d &proj_center1, - const Eigen::Vector3d &proj_center2, - const std::vector &points3D) { - // Baseline length between camera centers. - const double baseline_length_squared = - (proj_center1 - proj_center2).squaredNorm(); - - std::vector angles(points3D.size()); - - for (size_t i = 0; i < points3D.size(); ++i) { - // Ray lengths from cameras to point. - const double ray_length_squared1 = - (points3D[i] - proj_center1).squaredNorm(); - const double ray_length_squared2 = - (points3D[i] - proj_center2).squaredNorm(); - - // Using "law of cosines" to compute the enclosing angle between rays. - const double denominator = - 2.0 * std::sqrt(ray_length_squared1 * ray_length_squared2); - if (denominator == 0.0) { - angles[i] = 0.0; - continue; - } - const double nominator = - ray_length_squared1 + ray_length_squared2 - baseline_length_squared; - const double angle = std::abs(std::acos(nominator / denominator)); - - // Triangulation is unstable for acute angles (far away points) and - // obtuse angles (close points), so always compute the minimum angle - // between the two intersecting rays. - angles[i] = std::min(angle, M_PI - angle); - } - - return angles; -} - -} // namespace colmap diff --git a/src/geometry/colmap/base/triangulation.h b/src/geometry/colmap/base/triangulation.h deleted file mode 100644 index 07cb84a..0000000 --- a/src/geometry/colmap/base/triangulation.h +++ /dev/null @@ -1,120 +0,0 @@ -// Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of -// its contributors may be used to endorse or promote products derived -// from this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// -// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) - -#ifndef COLMAP_SRC_BASE_TRIANGULATION_H_ -#define COLMAP_SRC_BASE_TRIANGULATION_H_ - -#include -#include - -#include "geometry/colmap/base/camera.h" -//#include "util/alignment.h" -#include "geometry/colmap/util/math.h" -#include "geometry/colmap/util/types.h" - -namespace colmap { - -// Triangulate 3D point from corresponding image point observations. -// -// Implementation of the direct linear transform triangulation method in -// R. Hartley and A. Zisserman, Multiple View Geometry in Computer Vision, -// Cambridge Univ. Press, 2003. -// -// @param proj_matrix1 Projection matrix of the first image as 3x4 matrix. -// @param proj_matrix2 Projection matrix of the second image as 3x4 matrix. -// @param point1 Corresponding 2D point in first image. -// @param point2 Corresponding 2D point in second image. -// -// @return Triangulated 3D point. -Eigen::Vector3d TriangulatePoint(const Eigen::Matrix3x4d &proj_matrix1, - const Eigen::Matrix3x4d &proj_matrix2, - const Eigen::Vector2d &point1, - const Eigen::Vector2d &point2); - -// Triangulate multiple 3D points from multiple image correspondences. -std::vector -TriangulatePoints(const Eigen::Matrix3x4d &proj_matrix1, - const Eigen::Matrix3x4d &proj_matrix2, - const std::vector &points1, - const std::vector &points2); - -// Triangulate point from multiple views minimizing the L2 error. -// -// @param proj_matrices Projection matrices of multi-view observations. -// @param points Image observations of multi-view observations. -// -// @return Estimated 3D point. -Eigen::Vector3d -TriangulateMultiViewPoint(const std::vector &proj_matrices, - const std::vector &points); - -// Triangulate optimal 3D point from corresponding image point observations by -// finding the optimal image observations. -// -// Note that camera poses should be very good in order for this method to yield -// good results. Otherwise just use `TriangulatePoint`. -// -// Implementation of the method described in -// P. Lindstrom, "Triangulation Made Easy," IEEE Computer Vision and Pattern -// Recognition 2010, pp. 1554-1561, June 2010. -// -// @param proj_matrix1 Projection matrix of the first image as 3x4 matrix. -// @param proj_matrix2 Projection matrix of the second image as 3x4 matrix. -// @param point1 Corresponding 2D point in first image. -// @param point2 Corresponding 2D point in second image. -// -// @return Triangulated optimal 3D point. -// Eigen::Vector3d TriangulateOptimalPoint(const Eigen::Matrix3x4d& -// proj_matrix1, -// const Eigen::Matrix3x4d& proj_matrix2, -// const Eigen::Vector2d& point1, -// const Eigen::Vector2d& point2); - -// Triangulate multiple optimal 3D points from multiple image correspondences. -std::vector -TriangulateOptimalPoints(const Eigen::Matrix3x4d &proj_matrix1, - const Eigen::Matrix3x4d &proj_matrix2, - const std::vector &points1, - const std::vector &points2); - -// Calculate angle in radians between the two rays of a triangulated point. -double CalculateTriangulationAngle(const Eigen::Vector3d &proj_center1, - const Eigen::Vector3d &proj_center2, - const Eigen::Vector3d &point3D); - -std::vector -CalculateTriangulationAngles(const Eigen::Vector3d &proj_center1, - const Eigen::Vector3d &proj_center2, - const std::vector &points3D); - -} // namespace colmap - -#endif // COLMAP_SRC_BASE_TRIANGULATION_H_ diff --git a/src/geometry/colmap/estimators/polynomial.h b/src/geometry/colmap/estimators/polynomial.h deleted file mode 100644 index 9f64324..0000000 --- a/src/geometry/colmap/estimators/polynomial.h +++ /dev/null @@ -1,101 +0,0 @@ -// Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of -// its contributors may be used to endorse or promote products derived -// from this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// -// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) - -#ifndef COLMAP_SRC_BASE_POLYNOMIAL_H_ -#define COLMAP_SRC_BASE_POLYNOMIAL_H_ - -#include - -namespace colmap { - -// All polynomials are assumed to be the form: -// -// sum_{i=0}^N polynomial(i) x^{N-i}. -// -// and are given by a vector of coefficients of size N + 1. -// -// The implementation is based on COLMAP's old polynomial functionality and is -// inspired by Ceres-Solver's/Theia's implementation to support complex -// polynomials. The companion matrix implementation is based on NumPy. - -// Evaluate the polynomial for the given coefficients at x using the Horner -// scheme. This function is templated such that the polynomial may be evaluated -// at real and/or imaginary points. -template -T EvaluatePolynomial(const Eigen::VectorXd &coeffs, const T &x); - -// Find the root of polynomials of the form: a * x + b = 0. -// The real and/or imaginary variable may be NULL if the output is not needed. -bool FindLinearPolynomialRoots(const Eigen::VectorXd &coeffs, - Eigen::VectorXd *real, Eigen::VectorXd *imag); - -// Find the roots of polynomials of the form: a * x^2 + b * x + c = 0. -// The real and/or imaginary variable may be NULL if the output is not needed. -bool FindQuadraticPolynomialRoots(const Eigen::VectorXd &coeffs, - Eigen::VectorXd *real, Eigen::VectorXd *imag); - -// Find the roots of a polynomial using the Durand-Kerner method, based on: -// -// https://en.wikipedia.org/wiki/Durand%E2%80%93Kerner_method -// -// The Durand-Kerner is comparatively fast but often unstable/inaccurate. -// The real and/or imaginary variable may be NULL if the output is not needed. -bool FindPolynomialRootsDurandKerner(const Eigen::VectorXd &coeffs, - Eigen::VectorXd *real, - Eigen::VectorXd *imag); - -// Find the roots of a polynomial using the companion matrix method, based on: -// -// R. A. Horn & C. R. Johnson, Matrix Analysis. Cambridge, -// UK: Cambridge University Press, 1999, pp. 146-7. -// -// Compared to Durand-Kerner, this method is slower but more stable/accurate. -// The real and/or imaginary variable may be NULL if the output is not needed. -bool FindPolynomialRootsCompanionMatrix(const Eigen::VectorXd &coeffs, - Eigen::VectorXd *real, - Eigen::VectorXd *imag); - -//////////////////////////////////////////////////////////////////////////////// -// Implementation -//////////////////////////////////////////////////////////////////////////////// - -template -T EvaluatePolynomial(const Eigen::VectorXd &coeffs, const T &x) { - T value = 0.0; - for (Eigen::VectorXd::Index i = 0; i < coeffs.size(); ++i) { - value = value * x + coeffs(i); - } - return value; -} - -} // namespace colmap - -#endif // COLMAP_SRC_BASE_POLYNOMIAL_H_ diff --git a/src/geometry/colmap/estimators/triangulation.cc b/src/geometry/colmap/estimators/triangulation.cc deleted file mode 100644 index 7a6fb6e..0000000 --- a/src/geometry/colmap/estimators/triangulation.cc +++ /dev/null @@ -1,197 +0,0 @@ -// Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of -// its contributors may be used to endorse or promote products derived -// from this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// -// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) - -#include "triangulation.h" - -#include - -//#include "geometry/colmap/base/projection.h" -#include "geometry/colmap/base/triangulation.h" -//#include "estimators/essential_matrix.h" -#include "geometry/colmap/optim/combination_sampler.h" -#include "geometry/colmap/optim/loransac.h" -#include "geometry/colmap/util/logging.h" -#include "geometry/colmap/util/math.h" - -namespace colmap { -double CalculateSquaredReprojectionError(const Eigen::Vector2d &point2D, - const Eigen::Vector3d &point3D, - const Eigen::Matrix3x4d &proj_matrix, - const Camera &camera) { - const double proj_z = proj_matrix.row(2).dot(point3D.homogeneous()); - - // Check that point is infront of camera. - if (proj_z < std::numeric_limits::epsilon()) { - return std::numeric_limits::max(); - } - - const double proj_x = proj_matrix.row(0).dot(point3D.homogeneous()); - const double proj_y = proj_matrix.row(1).dot(point3D.homogeneous()); - const double inv_proj_z = 1.0 / proj_z; - - const Eigen::Vector2d - proj_point2D; // = camera.WorldToImage(Eigen::Vector2d(inv_proj_z * - // proj_x, inv_proj_z * proj_y)); - - return (proj_point2D - point2D).squaredNorm(); -} - -double CalculateNormalizedAngularError(const Eigen::Vector2d &point2D, - const Eigen::Vector3d &point3D, - const Eigen::Matrix3x4d &proj_matrix) { - const Eigen::Vector3d ray1 = point2D.homogeneous(); - const Eigen::Vector3d ray2 = proj_matrix * point3D.homogeneous(); - return std::acos(ray1.normalized().transpose() * ray2.normalized()); -} - -void TriangulationEstimator::SetMinTriAngle(const double min_tri_angle) { - CHECK_GE(min_tri_angle, 0); - min_tri_angle_ = min_tri_angle; -} - -void TriangulationEstimator::SetResidualType(const ResidualType residual_type) { - residual_type_ = residual_type; -} - -bool HasPointPositiveDepth(const Eigen::Matrix3x4d &proj_matrix, - const Eigen::Vector3d &point3D) { - return proj_matrix.row(2).dot(point3D.homogeneous()) >= - std::numeric_limits::epsilon(); -} - -std::vector -TriangulationEstimator::Estimate(const std::vector &point_data, - const std::vector &pose_data) const { - CHECK_GE(point_data.size(), 2); - CHECK_EQ(point_data.size(), pose_data.size()); - - if (point_data.size() == 2) { - // Two-view triangulation. - - const M_t xyz = TriangulatePoint( - pose_data[0].proj_matrix, pose_data[1].proj_matrix, - point_data[0].point_normalized, point_data[1].point_normalized); - - if (HasPointPositiveDepth(pose_data[0].proj_matrix, xyz) && - HasPointPositiveDepth(pose_data[1].proj_matrix, xyz) && - CalculateTriangulationAngle(pose_data[0].proj_center, - pose_data[1].proj_center, - xyz) >= min_tri_angle_) { - return std::vector{xyz}; - } - } else { - // Multi-view triangulation. - - std::vector proj_matrices; - proj_matrices.reserve(point_data.size()); - std::vector points; - points.reserve(point_data.size()); - for (size_t i = 0; i < point_data.size(); ++i) { - proj_matrices.push_back(pose_data[i].proj_matrix); - points.push_back(point_data[i].point_normalized); - } - - const M_t xyz = TriangulateMultiViewPoint(proj_matrices, points); - - // Check for cheirality constraint. - for (const auto &pose : pose_data) { - if (!HasPointPositiveDepth(pose.proj_matrix, xyz)) { - return std::vector(); - } - } - - // Check for sufficient triangulation angle. - for (size_t i = 0; i < pose_data.size(); ++i) { - for (size_t j = 0; j < i; ++j) { - const double tri_angle = CalculateTriangulationAngle( - pose_data[i].proj_center, pose_data[j].proj_center, xyz); - if (tri_angle >= min_tri_angle_) { - return std::vector{xyz}; - } - } - } - } - - return std::vector(); -} - -void TriangulationEstimator::Residuals(const std::vector &point_data, - const std::vector &pose_data, - const M_t &xyz, - std::vector *residuals) const { - CHECK_EQ(point_data.size(), pose_data.size()); - - residuals->resize(point_data.size()); - - for (size_t i = 0; i < point_data.size(); ++i) { - if (residual_type_ == ResidualType::REPROJECTION_ERROR) { - (*residuals)[i] = CalculateSquaredReprojectionError( - point_data[i].point, xyz, pose_data[i].proj_matrix, - *pose_data[i].camera); - } else if (residual_type_ == ResidualType::ANGULAR_ERROR) { - const double angular_error = CalculateNormalizedAngularError( - point_data[i].point_normalized, xyz, pose_data[i].proj_matrix); - (*residuals)[i] = angular_error * angular_error; - } - } -} - -bool EstimateTriangulation( - const EstimateTriangulationOptions &options, - const std::vector &point_data, - const std::vector &pose_data, - std::vector *inlier_mask, Eigen::Vector3d *xyz) { - CHECK_NOTNULL(inlier_mask); - CHECK_NOTNULL(xyz); - CHECK_GE(point_data.size(), 2); - CHECK_EQ(point_data.size(), pose_data.size()); - options.Check(); - - // Robustly estimate track using LORANSAC. - LORANSAC - ransac(options.ransac_options); - ransac.estimator.SetMinTriAngle(options.min_tri_angle); - ransac.estimator.SetResidualType(options.residual_type); - ransac.local_estimator.SetMinTriAngle(options.min_tri_angle); - ransac.local_estimator.SetResidualType(options.residual_type); - const auto report = ransac.Estimate(point_data, pose_data); - if (!report.success) { - return false; - } - - *inlier_mask = report.inlier_mask; - *xyz = report.model; - - return report.success; -} - -} // namespace colmap diff --git a/src/geometry/colmap/estimators/triangulation.h b/src/geometry/colmap/estimators/triangulation.h deleted file mode 100644 index b8c226a..0000000 --- a/src/geometry/colmap/estimators/triangulation.h +++ /dev/null @@ -1,157 +0,0 @@ -// Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of -// its contributors may be used to endorse or promote products derived -// from this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// -// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) - -#ifndef COLMAP_SRC_ESTIMATORS_TRIANGULATION_H_ -#define COLMAP_SRC_ESTIMATORS_TRIANGULATION_H_ - -#include -#include - -#include "geometry/colmap/base/camera.h" -#include "geometry/colmap/optim/ransac.h" -//#include "util/alignment.h" -//#include "util/math.h" -#include "geometry/colmap/util/types.h" - -namespace colmap { - -// Triangulation estimator to estimate 3D point from multiple observations. -// The triangulation must satisfy the following constraints: -// - Sufficient triangulation angle between observation pairs. -// - All observations must satisfy cheirality constraint. -// -// An observation is composed of an image measurement and the corresponding -// camera pose and calibration. -class TriangulationEstimator { - public: - enum class ResidualType { - ANGULAR_ERROR, - REPROJECTION_ERROR, - }; - - struct PointData { - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - PointData() {} - PointData(const Eigen::Vector2d &point_, - const Eigen::Vector2d &point_N_) - : point(point_), point_normalized(point_N_) {} - // Image observation in pixels. Only needs to be set for - // REPROJECTION_ERROR. - Eigen::Vector2d point; - // Normalized image observation. Must always be set. - Eigen::Vector2d point_normalized; - }; - - struct PoseData { - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - PoseData() : camera(nullptr) {} - PoseData(const Eigen::Matrix3x4d &proj_matrix_, - const Eigen::Vector3d &pose_, const Camera *camera_) - : proj_matrix(proj_matrix_), proj_center(pose_), camera(camera_) {} - // The projection matrix for the image of the observation. - Eigen::Matrix3x4d proj_matrix; - // The projection center for the image of the observation. - Eigen::Vector3d proj_center; - // The camera for the image of the observation. - const Camera *camera; - }; - - typedef PointData X_t; - typedef PoseData Y_t; - typedef Eigen::Vector3d M_t; - - // Specify settings for triangulation estimator. - void SetMinTriAngle(const double min_tri_angle); - void SetResidualType(const ResidualType residual_type); - - // The minimum number of samples needed to estimate a model. - static const int kMinNumSamples = 2; - - // Estimate a 3D point from a two-view observation. - // - // @param point_data Image measurement. - // @param point_data Camera poses. - // - // @return Triangulated point if successful, otherwise - // none. - std::vector Estimate(const std::vector &point_data, - const std::vector &pose_data) const; - - // Calculate residuals in terms of squared reprojection or angular error. - // - // @param point_data Image measurements. - // @param point_data Camera poses. - // @param xyz 3D point. - // - // @return Residual for each observation. - void Residuals(const std::vector &point_data, - const std::vector &pose_data, const M_t &xyz, - std::vector *residuals) const; - - private: - ResidualType residual_type_ = ResidualType::REPROJECTION_ERROR; - double min_tri_angle_ = 0.0; -}; - -struct EstimateTriangulationOptions { - // Minimum triangulation angle in radians. - double min_tri_angle = 0.0; - - // The employed residual type. - TriangulationEstimator::ResidualType residual_type = - TriangulationEstimator::ResidualType::ANGULAR_ERROR; - - // RANSAC options for TriangulationEstimator. - RANSACOptions ransac_options; - - void Check() const { - CHECK_GE(min_tri_angle, 0.0); - ransac_options.Check(); - } -}; - -// Robustly estimate 3D point from observations in multiple views using RANSAC -// and a subsequent non-linear refinement using all inliers. Returns true -// if the estimated number of inliers has more than two views. -bool EstimateTriangulation( - const EstimateTriangulationOptions &options, - const std::vector &point_data, - const std::vector &pose_data, - std::vector *inlier_mask, Eigen::Vector3d *xyz); - -} // namespace colmap - -// EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM( -// colmap::TriangulationEstimator::PointData) -// EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM( -// colmap::TriangulationEstimator::PoseData) - -#endif // COLMAP_SRC_ESTIMATORS_TRIANGULATION_H_ diff --git a/src/geometry/colmap/estimators/utils.cc b/src/geometry/colmap/estimators/utils.cc deleted file mode 100644 index 92412d6..0000000 --- a/src/geometry/colmap/estimators/utils.cc +++ /dev/null @@ -1,184 +0,0 @@ -// Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of -// its contributors may be used to endorse or promote products derived -// from this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// -// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) - -#include "utils.h" - -#include "geometry/colmap/util/logging.h" - -namespace colmap { - -// void CenterAndNormalizeImagePoints(const std::vector& -// points, -// std::vector* -// normed_points, Eigen::Matrix3d* matrix) { -// // Calculate centroid -// Eigen::Vector2d centroid(0, 0); -// for (const auto point : points) { -// centroid += point; -// } -// centroid /= points.size(); - -// // Root mean square error to centroid of all points -// double rms_mean_dist = 0; -// for (const auto point : points) { -// rms_mean_dist += (point - centroid).squaredNorm(); -// } -// rms_mean_dist = std::sqrt(rms_mean_dist / points.size()); - -// // Compose normalization matrix -// const double norm_factor = std::sqrt(2.0) / rms_mean_dist; -// *matrix << norm_factor, 0, -norm_factor * centroid(0), 0, norm_factor, -// -norm_factor * centroid(1), 0, 0, 1; - -// // Apply normalization matrix -// normed_points->resize(points.size()); - -// const double M_00 = (*matrix)(0, 0); -// const double M_01 = (*matrix)(0, 1); -// const double M_02 = (*matrix)(0, 2); -// const double M_10 = (*matrix)(1, 0); -// const double M_11 = (*matrix)(1, 1); -// const double M_12 = (*matrix)(1, 2); -// const double M_20 = (*matrix)(2, 0); -// const double M_21 = (*matrix)(2, 1); -// const double M_22 = (*matrix)(2, 2); - -// for (size_t i = 0; i < points.size(); ++i) { -// const double p_0 = points[i](0); -// const double p_1 = points[i](1); - -// const double np_0 = M_00 * p_0 + M_01 * p_1 + M_02; -// const double np_1 = M_10 * p_0 + M_11 * p_1 + M_12; -// const double np_2 = M_20 * p_0 + M_21 * p_1 + M_22; - -// const double inv_np_2 = 1.0 / np_2; -// (*normed_points)[i](0) = np_0 * inv_np_2; -// (*normed_points)[i](1) = np_1 * inv_np_2; -// } -// } - -// void ComputeSquaredSampsonError(const std::vector& points1, -// const std::vector& points2, -// const Eigen::Matrix3d& E, -// std::vector* residuals) { -// CHECK_EQ(points1.size(), points2.size()); - -// residuals->resize(points1.size()); - -// // Note that this code might not be as nice as Eigen expressions, -// // but it is significantly faster in various tests - -// const double E_00 = E(0, 0); -// const double E_01 = E(0, 1); -// const double E_02 = E(0, 2); -// const double E_10 = E(1, 0); -// const double E_11 = E(1, 1); -// const double E_12 = E(1, 2); -// const double E_20 = E(2, 0); -// const double E_21 = E(2, 1); -// const double E_22 = E(2, 2); - -// for (size_t i = 0; i < points1.size(); ++i) { -// const double x1_0 = points1[i](0); -// const double x1_1 = points1[i](1); -// const double x2_0 = points2[i](0); -// const double x2_1 = points2[i](1); - -// // Ex1 = E * points1[i].homogeneous(); -// const double Ex1_0 = E_00 * x1_0 + E_01 * x1_1 + E_02; -// const double Ex1_1 = E_10 * x1_0 + E_11 * x1_1 + E_12; -// const double Ex1_2 = E_20 * x1_0 + E_21 * x1_1 + E_22; - -// // Etx2 = E.transpose() * points2[i].homogeneous(); -// const double Etx2_0 = E_00 * x2_0 + E_10 * x2_1 + E_20; -// const double Etx2_1 = E_01 * x2_0 + E_11 * x2_1 + E_21; - -// // x2tEx1 = points2[i].homogeneous().transpose() * Ex1; -// const double x2tEx1 = x2_0 * Ex1_0 + x2_1 * Ex1_1 + Ex1_2; - -// // Sampson distance -// (*residuals)[i] = x2tEx1 * x2tEx1 / (Ex1_0 * Ex1_0 + Ex1_1 * Ex1_1 + -// Etx2_0 * Etx2_0 + Etx2_1 * Etx2_1); -// } -// } - -void ComputeSquaredReprojectionError( - const std::vector &points2D, - const std::vector &points3D, - const Eigen::Matrix3x4d &proj_matrix, std::vector *residuals) { - CHECK_EQ(points2D.size(), points3D.size()); - - residuals->resize(points2D.size()); - - // Note that this code might not be as nice as Eigen expressions, - // but it is significantly faster in various tests. - - const double P_00 = proj_matrix(0, 0); - const double P_01 = proj_matrix(0, 1); - const double P_02 = proj_matrix(0, 2); - const double P_03 = proj_matrix(0, 3); - const double P_10 = proj_matrix(1, 0); - const double P_11 = proj_matrix(1, 1); - const double P_12 = proj_matrix(1, 2); - const double P_13 = proj_matrix(1, 3); - const double P_20 = proj_matrix(2, 0); - const double P_21 = proj_matrix(2, 1); - const double P_22 = proj_matrix(2, 2); - const double P_23 = proj_matrix(2, 3); - - for (size_t i = 0; i < points2D.size(); ++i) { - const double X_0 = points3D[i](0); - const double X_1 = points3D[i](1); - const double X_2 = points3D[i](2); - - // Project 3D point from world to camera. - const double px_2 = P_20 * X_0 + P_21 * X_1 + P_22 * X_2 + P_23; - - // Check if 3D point is in front of camera. - if (px_2 > std::numeric_limits::epsilon()) { - const double px_0 = P_00 * X_0 + P_01 * X_1 + P_02 * X_2 + P_03; - const double px_1 = P_10 * X_0 + P_11 * X_1 + P_12 * X_2 + P_13; - - const double x_0 = points2D[i](0); - const double x_1 = points2D[i](1); - - const double inv_px_2 = 1.0 / px_2; - const double dx_0 = x_0 - px_0 * inv_px_2; - const double dx_1 = x_1 - px_1 * inv_px_2; - - (*residuals)[i] = dx_0 * dx_0 + dx_1 * dx_1; - } else { - (*residuals)[i] = std::numeric_limits::max(); - } - } -} - -} // namespace colmap diff --git a/src/geometry/colmap/estimators/utils.h b/src/geometry/colmap/estimators/utils.h deleted file mode 100644 index d8d2737..0000000 --- a/src/geometry/colmap/estimators/utils.h +++ /dev/null @@ -1,92 +0,0 @@ -// Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of -// its contributors may be used to endorse or promote products derived -// from this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// -// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) - -#ifndef COLMAP_SRC_ESTIMATORS_UTILS_H_ -#define COLMAP_SRC_ESTIMATORS_UTILS_H_ - -#include -#include - -//#include "util/alignment.h" -#include "geometry/colmap/util/types.h" - -namespace colmap { - -// Center and normalize image points. -// -// The points are transformed in a two-step procedure that is expressed -// as a transformation matrix. The matrix of the resulting points is usually -// better conditioned than the matrix of the original points. -// -// Center the image points, such that the new coordinate system has its -// origin at the centroid of the image points. -// -// Normalize the image points, such that the mean distance from the points -// to the coordinate system is sqrt(2). -// -// @param points Image coordinates. -// @param normed_points Transformed image coordinates. -// @param matrix 3x3 transformation matrix. -// void CenterAndNormalizeImagePoints(const std::vector& -// points, -// std::vector* -// normed_points, Eigen::Matrix3d* matrix); - -// Calculate the residuals of a set of corresponding points and a given -// fundamental or essential matrix. -// -// Residuals are defined as the squared Sampson error. -// -// @param points1 First set of corresponding points as Nx2 matrix. -// @param points2 Second set of corresponding points as Nx2 matrix. -// @param E 3x3 fundamental or essential matrix. -// @param residuals Output vector of residuals. -// void ComputeSquaredSampsonError(const std::vector& points1, -// const std::vector& points2, -// const Eigen::Matrix3d& E, -// std::vector* residuals); - -// Calculate the squared reprojection error given a set of 2D-3D point -// correspondences and a projection matrix. Returns DBL_MAX if a 3D point is -// behind the given camera. -// -// @param points2D Normalized 2D image points. -// @param points3D 3D world points. -// @param proj_matrix 3x4 projection matrix. -// @param residuals Output vector of residuals. -void ComputeSquaredReprojectionError( - const std::vector &points2D, - const std::vector &points3D, - const Eigen::Matrix3x4d &proj_matrix, std::vector *residuals); - -} // namespace colmap - -#endif // COLMAP_SRC_ESTIMATORS_UTILS_H_ diff --git a/src/geometry/colmap/util/alignment.h b/src/geometry/colmap/util/alignment.h deleted file mode 100644 index 2f55911..0000000 --- a/src/geometry/colmap/util/alignment.h +++ /dev/null @@ -1,99 +0,0 @@ -// Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of -// its contributors may be used to endorse or promote products derived -// from this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// -// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) - -#ifndef COLMAP_SRC_UTIL_ALIGNMENT_H_ -#define COLMAP_SRC_UTIL_ALIGNMENT_H_ - -#include -#include -#include -#include -#include -#include - -#ifndef EIGEN_ALIGNED_ALLOCATOR -#define EIGEN_ALIGNED_ALLOCATOR Eigen::aligned_allocator -#endif - -// Equivalent to EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION but with support for -// initializer lists, which is a C++11 feature and not supported by the Eigen. -// The initializer list extension is inspired by Theia and StackOverflow code. -#define EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(...) \ - namespace std { \ - template <> \ - class vector<__VA_ARGS__, std::allocator<__VA_ARGS__>> \ - : public vector<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__>> { \ - typedef vector<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__>> \ - vector_base; \ - \ - public: \ - typedef __VA_ARGS__ value_type; \ - typedef vector_base::allocator_type allocator_type; \ - typedef vector_base::size_type size_type; \ - typedef vector_base::iterator iterator; \ - explicit vector(const allocator_type &a = allocator_type()) \ - : vector_base(a) {} \ - template \ - vector(InputIterator first, InputIterator last, \ - const allocator_type &a = allocator_type()) \ - : vector_base(first, last, a) {} \ - vector(const vector &c) : vector_base(c) {} \ - explicit vector(size_type num, const value_type &val = value_type()) \ - : vector_base(num, val) {} \ - vector(iterator start, iterator end) : vector_base(start, end) {} \ - vector &operator=(const vector &x) { \ - vector_base::operator=(x); \ - return *this; \ - } \ - vector(initializer_list<__VA_ARGS__> list) \ - : vector_base(list.begin(), list.end()) {} \ - }; \ - } // namespace std - -EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Vector2d) -EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Vector4d) -EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Vector4f) -EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Matrix2d) -EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Matrix2f) -EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Matrix4d) -EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Matrix4f) -EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Affine3d) -EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Affine3f) -EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Quaterniond) -EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Quaternionf) -EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Matrix) -EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(Eigen::Matrix) - -#define EIGEN_STL_UMAP(KEY, VALUE) \ - std::unordered_map, std::equal_to, \ - Eigen::aligned_allocator>> - -#endif // COLMAP_SRC_UTIL_ALIGNMENT_H_ diff --git a/src/geometry/colmap/util/endian.h b/src/geometry/colmap/util/endian.h deleted file mode 100644 index 93a41d6..0000000 --- a/src/geometry/colmap/util/endian.h +++ /dev/null @@ -1,153 +0,0 @@ -// Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of -// its contributors may be used to endorse or promote products derived -// from this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// -// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) - -#ifndef COLMAP_SRC_UTIL_ENDIAN_H_ -#define COLMAP_SRC_UTIL_ENDIAN_H_ - -#include - -namespace colmap { - -// Reverse the order of each byte. -template T ReverseBytes(const T &data); - -// Check the order in which bytes are stored in computer memory. -bool IsLittleEndian(); -bool IsBigEndian(); - -// Convert data between endianness and the native format. Note that, for float -// and double types, these functions are only valid if the format is IEEE-754. -// This is the case for pretty much most processors. -template T LittleEndianToNative(const T x); -template T BigEndianToNative(const T x); -template T NativeToLittleEndian(const T x); -template T NativeToBigEndian(const T x); - -// Read data in little endian format for cross-platform support. -template T ReadBinaryLittleEndian(std::istream *stream); -template -void ReadBinaryLittleEndian(std::istream *stream, std::vector *data); - -// Write data in little endian format for cross-platform support. -template -void WriteBinaryLittleEndian(std::ostream *stream, const T &data); -template -void WriteBinaryLittleEndian(std::ostream *stream, const std::vector &data); - -//////////////////////////////////////////////////////////////////////////////// -// Implementation -//////////////////////////////////////////////////////////////////////////////// - -template T ReverseBytes(const T &data) { - T data_reversed = data; - std::reverse(reinterpret_cast(&data_reversed), - reinterpret_cast(&data_reversed) + sizeof(T)); - return data_reversed; -} - -inline bool IsLittleEndian() { -#ifdef BOOST_BIG_ENDIAN - return false; -#else - return true; -#endif -} - -inline bool IsBigEndian() { -#ifdef BOOST_BIG_ENDIAN - return true; -#else - return false; -#endif -} - -template T LittleEndianToNative(const T x) { - if (IsLittleEndian()) { - return x; - } else { - return ReverseBytes(x); - } -} - -template T BigEndianToNative(const T x) { - if (IsBigEndian()) { - return x; - } else { - return ReverseBytes(x); - } -} - -template T NativeToLittleEndian(const T x) { - if (IsLittleEndian()) { - return x; - } else { - return ReverseBytes(x); - } -} - -template T NativeToBigEndian(const T x) { - if (IsBigEndian()) { - return x; - } else { - return ReverseBytes(x); - } -} - -template T ReadBinaryLittleEndian(std::istream *stream) { - T data_little_endian; - stream->read(reinterpret_cast(&data_little_endian), sizeof(T)); - return LittleEndianToNative(data_little_endian); -} - -template -void ReadBinaryLittleEndian(std::istream *stream, std::vector *data) { - for (size_t i = 0; i < data->size(); ++i) { - (*data)[i] = ReadBinaryLittleEndian(stream); - } -} - -template -void WriteBinaryLittleEndian(std::ostream *stream, const T &data) { - const T data_little_endian = NativeToLittleEndian(data); - stream->write(reinterpret_cast(&data_little_endian), - sizeof(T)); -} - -template -void WriteBinaryLittleEndian(std::ostream *stream, const std::vector &data) { - for (const auto &elem : data) { - WriteBinaryLittleEndian(stream, elem); - } -} - -} // namespace colmap - -#endif // COLMAP_SRC_UTIL_ENDIAN_H_ diff --git a/src/geometry/colmap/util/logging.h b/src/geometry/colmap/util/logging.h deleted file mode 100644 index c1de402..0000000 --- a/src/geometry/colmap/util/logging.h +++ /dev/null @@ -1,94 +0,0 @@ -// Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of -// its contributors may be used to endorse or promote products derived -// from this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// -// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) - -#ifndef COLMAP_SRC_UTIL_LOGGING_H_ -#define COLMAP_SRC_UTIL_LOGGING_H_ - -#include - -#include - -#include "string.h" - -// Option checker macros. In contrast to glog, this function does not abort the -// program, but simply returns false on failure. -#define CHECK_OPTION_IMPL(expr) \ - __CheckOptionImpl(__FILE__, __LINE__, (expr), #expr) -#define CHECK_OPTION(expr) \ - if (!__CheckOptionImpl(__FILE__, __LINE__, (expr), #expr)) { \ - return false; \ - } -#define CHECK_OPTION_OP(name, op, val1, val2) \ - if (!__CheckOptionOpImpl(__FILE__, __LINE__, (val1 op val2), val1, val2, \ - #val1, #val2, #op)) { \ - return false; \ - } -#define CHECK_OPTION_EQ(val1, val2) CHECK_OPTION_OP(_EQ, ==, val1, val2) -#define CHECK_OPTION_NE(val1, val2) CHECK_OPTION_OP(_NE, !=, val1, val2) -#define CHECK_OPTION_LE(val1, val2) CHECK_OPTION_OP(_LE, <=, val1, val2) -#define CHECK_OPTION_LT(val1, val2) CHECK_OPTION_OP(_LT, <, val1, val2) -#define CHECK_OPTION_GE(val1, val2) CHECK_OPTION_OP(_GE, >=, val1, val2) -#define CHECK_OPTION_GT(val1, val2) CHECK_OPTION_OP(_GT, >, val1, val2) - -namespace colmap { - -// Initialize glog at the beginning of the program. -void InitializeGlog(char **argv); - -//////////////////////////////////////////////////////////////////////////////// -// Implementation -//////////////////////////////////////////////////////////////////////////////// - -const char *__GetConstFileBaseName(const char *file); - -bool __CheckOptionImpl(const char *file, const int line, const bool result, - const char *expr_str); - -template -bool __CheckOptionOpImpl(const char *file, const int line, const bool result, - const T1 &val1, const T2 &val2, const char *val1_str, - const char *val2_str, const char *op_str) { - if (result) { - return true; - } else { - std::cerr << StringPrintf("[%s:%d] Check failed: %s %s %s (%s vs. %s)", - __GetConstFileBaseName(file), line, val1_str, - op_str, val2_str, - std::to_string(val1).c_str(), - std::to_string(val2).c_str()) - << std::endl; - return false; - } -} - -} // namespace colmap - -#endif // COLMAP_SRC_UTIL_LOGGING_H_ diff --git a/src/geometry/colmap/util/misc.cc b/src/geometry/colmap/util/misc.cc deleted file mode 100644 index 0aa0fef..0000000 --- a/src/geometry/colmap/util/misc.cc +++ /dev/null @@ -1,321 +0,0 @@ -// Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of -// its contributors may be used to endorse or promote products derived -// from this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// -// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) - -#include "misc.h" - -#include -#include - -namespace colmap { - -std::string EnsureTrailingSlash(const std::string &str) { - if (str.length() > 0) { - if (str.back() != '/') { - return str + "/"; - } - } else { - return str + "/"; - } - return str; -} - -bool HasFileExtension(const std::string &file_name, const std::string &ext) { - CHECK(!ext.empty()); - CHECK_EQ(ext.at(0), '.'); - std::string ext_lower = ext; - StringToLower(&ext_lower); - if (file_name.size() >= ext_lower.size() && - file_name.substr(file_name.size() - ext_lower.size(), - ext_lower.size()) == ext_lower) { - return true; - } - return false; -} - -void SplitFileExtension(const std::string &path, std::string *root, - std::string *ext) { - const auto parts = StringSplit(path, "."); - CHECK_GT(parts.size(), 0); - if (parts.size() == 1) { - *root = parts[0]; - *ext = ""; - } else { - *root = ""; - for (size_t i = 0; i < parts.size() - 1; ++i) { - *root += parts[i] + "."; - } - *root = root->substr(0, root->length() - 1); - if (parts.back() == "") { - *ext = ""; - } else { - *ext = "." + parts.back(); - } - } -} - -bool ExistsFile(const std::string &path) { - return boost::filesystem::is_regular_file(path); -} - -bool ExistsDir(const std::string &path) { - return boost::filesystem::is_directory(path); -} - -bool ExistsPath(const std::string &path) { - return boost::filesystem::exists(path); -} - -void CreateDirIfNotExists(const std::string &path) { - if (!ExistsDir(path)) { - CHECK(boost::filesystem::create_directory(path)); - } -} - -std::string GetPathBaseName(const std::string &path) { - const std::vector names = - StringSplit(StringReplace(path, "\\", "/"), "/"); - if (names.size() > 1 && names.back() == "") { - return names[names.size() - 2]; - } else { - return names.back(); - } -} - -std::string GetParentDir(const std::string &path) { - return boost::filesystem::path(path).parent_path().string(); -} - -std::string GetRelativePath(const std::string &from, const std::string &to) { - // This implementation is adapted from: - // https://stackoverflow.com/questions/10167382 - // A native implementation in boost::filesystem is only available starting - // from boost version 1.60. - using namespace boost::filesystem; - - path from_path = canonical(path(from)); - path to_path = canonical(path(to)); - - // Start at the root path and while they are the same then do nothing then - // when they first diverge take the entire from path, swap it with '..' - // segments, and then append the remainder of the to path. - path::const_iterator from_iter = from_path.begin(); - path::const_iterator to_iter = to_path.begin(); - - // Loop through both while they are the same to find nearest common - // directory - while (from_iter != from_path.end() && to_iter != to_path.end() && - (*to_iter) == (*from_iter)) { - ++to_iter; - ++from_iter; - } - - // Replace from path segments with '..' (from => nearest common directory) - path rel_path; - while (from_iter != from_path.end()) { - rel_path /= ".."; - ++from_iter; - } - - // Append the remainder of the to path (nearest common directory => to) - while (to_iter != to_path.end()) { - rel_path /= *to_iter; - ++to_iter; - } - - return rel_path.string(); -} - -std::vector GetFileList(const std::string &path) { - std::vector file_list; - for (auto it = boost::filesystem::directory_iterator(path); - it != boost::filesystem::directory_iterator(); ++it) { - if (boost::filesystem::is_regular_file(*it)) { - const boost::filesystem::path file_path = *it; - file_list.push_back(file_path.string()); - } - } - return file_list; -} - -std::vector GetRecursiveFileList(const std::string &path) { - std::vector file_list; - for (auto it = boost::filesystem::recursive_directory_iterator(path); - it != boost::filesystem::recursive_directory_iterator(); ++it) { - if (boost::filesystem::is_regular_file(*it)) { - const boost::filesystem::path file_path = *it; - file_list.push_back(file_path.string()); - } - } - return file_list; -} - -std::vector GetDirList(const std::string &path) { - std::vector dir_list; - for (auto it = boost::filesystem::directory_iterator(path); - it != boost::filesystem::directory_iterator(); ++it) { - if (boost::filesystem::is_directory(*it)) { - const boost::filesystem::path dir_path = *it; - dir_list.push_back(dir_path.string()); - } - } - return dir_list; -} - -std::vector GetRecursiveDirList(const std::string &path) { - std::vector dir_list; - for (auto it = boost::filesystem::recursive_directory_iterator(path); - it != boost::filesystem::recursive_directory_iterator(); ++it) { - if (boost::filesystem::is_directory(*it)) { - const boost::filesystem::path dir_path = *it; - dir_list.push_back(dir_path.string()); - } - } - return dir_list; -} - -size_t GetFileSize(const std::string &path) { - std::ifstream file(path, std::ifstream::ate | std::ifstream::binary); - CHECK(file.is_open()) << path; - return file.tellg(); -} - -void PrintHeading1(const std::string &heading) { - std::cout << std::endl << std::string(78, '=') << std::endl; - std::cout << heading << std::endl; - std::cout << std::string(78, '=') << std::endl << std::endl; -} - -void PrintHeading2(const std::string &heading) { - std::cout << std::endl << heading << std::endl; - std::cout << std::string(std::min(heading.size(), 78), '-') - << std::endl; -} - -template <> std::vector CSVToVector(const std::string &csv) { - auto elems = StringSplit(csv, ",;"); - std::vector values; - values.reserve(elems.size()); - for (auto &elem : elems) { - StringTrim(&elem); - if (elem.empty()) { - continue; - } - values.push_back(elem); - } - return values; -} - -template <> std::vector CSVToVector(const std::string &csv) { - auto elems = StringSplit(csv, ",;"); - std::vector values; - values.reserve(elems.size()); - for (auto &elem : elems) { - StringTrim(&elem); - if (elem.empty()) { - continue; - } - try { - values.push_back(std::stoi(elem)); - } catch (std::exception) { - return std::vector(0); - } - } - return values; -} - -template <> std::vector CSVToVector(const std::string &csv) { - auto elems = StringSplit(csv, ",;"); - std::vector values; - values.reserve(elems.size()); - for (auto &elem : elems) { - StringTrim(&elem); - if (elem.empty()) { - continue; - } - try { - values.push_back(std::stod(elem)); - } catch (std::exception) { - return std::vector(0); - } - } - return values; -} - -template <> std::vector CSVToVector(const std::string &csv) { - auto elems = StringSplit(csv, ",;"); - std::vector values; - values.reserve(elems.size()); - for (auto &elem : elems) { - StringTrim(&elem); - if (elem.empty()) { - continue; - } - try { - values.push_back(std::stold(elem)); - } catch (std::exception) { - return std::vector(0); - } - } - return values; -} - -std::vector ReadTextFileLines(const std::string &path) { - std::ifstream file(path); - CHECK(file.is_open()) << path; - - std::string line; - std::vector lines; - while (std::getline(file, line)) { - StringTrim(&line); - - if (line.empty()) { - continue; - } - - lines.push_back(line); - } - - return lines; -} - -void RemoveCommandLineArgument(const std::string &arg, int *argc, char **argv) { - for (int i = 0; i < *argc; ++i) { - if (argv[i] == arg) { - for (int j = i + 1; j < *argc; ++j) { - argv[i] = argv[j]; - } - *argc -= 1; - break; - } - } -} - -} // namespace colmap diff --git a/src/geometry/colmap/util/misc.h b/src/geometry/colmap/util/misc.h deleted file mode 100644 index a4f43a6..0000000 --- a/src/geometry/colmap/util/misc.h +++ /dev/null @@ -1,191 +0,0 @@ -// Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of -// its contributors may be used to endorse or promote products derived -// from this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// -// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) - -#ifndef COLMAP_SRC_UTIL_MISC_H_ -#define COLMAP_SRC_UTIL_MISC_H_ - -#include -#include -#include -#include -#include - -#include "endian.h" -#include "logging.h" -#include "string.h" - -namespace colmap { - -#ifndef STRINGIFY -#define STRINGIFY(s) STRINGIFY_(s) -#define STRINGIFY_(s) #s -#endif // STRINGIFY - -// Append trailing slash to string if it does not yet end with a slash. -std::string EnsureTrailingSlash(const std::string &str); - -// Check whether file name has the file extension (case insensitive). -bool HasFileExtension(const std::string &file_name, const std::string &ext); - -// Split the path into its root and extension, for example, -// "dir/file.jpg" into "dir/file" and ".jpg". -void SplitFileExtension(const std::string &path, std::string *root, - std::string *ext); - -// Check if the path points to an existing directory. -bool ExistsFile(const std::string &path); - -// Check if the path points to an existing directory. -bool ExistsDir(const std::string &path); - -// Check if the path points to an existing file or directory. -bool ExistsPath(const std::string &path); - -// Create the directory if it does not exist. -void CreateDirIfNotExists(const std::string &path); - -// Extract the base name of a path, e.g., "image.jpg" for "/dir/image.jpg". -std::string GetPathBaseName(const std::string &path); - -// Get the path of the parent directory for the given path. -std::string GetParentDir(const std::string &path); - -// Get the relative path between from and to. Both the from and to paths must -// exist. -std::string GetRelativePath(const std::string &from, const std::string &to); - -// Join multiple paths into one path. -template std::string JoinPaths(T const &...paths); - -// Return list of files in directory. -std::vector GetFileList(const std::string &path); - -// Return list of files, recursively in all sub-directories. -std::vector GetRecursiveFileList(const std::string &path); - -// Return list of directories, recursively in all sub-directories. -std::vector GetDirList(const std::string &path); - -// Return list of directories, recursively in all sub-directories. -std::vector GetRecursiveDirList(const std::string &path); - -// Get the size in bytes of a file. -size_t GetFileSize(const std::string &path); - -// Print first-order heading with over- and underscores to `std::cout`. -void PrintHeading1(const std::string &heading); - -// Print second-order heading with underscores to `std::cout`. -void PrintHeading2(const std::string &heading); - -// Check if vector contains elements. -template -bool VectorContainsValue(const std::vector &vector, const T value); - -template -bool VectorContainsDuplicateValues(const std::vector &vector); - -// Parse CSV line to a list of values. -template std::vector CSVToVector(const std::string &csv); - -// Concatenate values in list to comma-separated list. -template std::string VectorToCSV(const std::vector &values); - -// Read contiguous binary blob from file. -template -void ReadBinaryBlob(const std::string &path, std::vector *data); - -// Write contiguous binary blob to file. -template -void WriteBinaryBlob(const std::string &path, const std::vector &data); - -// Read each line of a text file into a separate element. Empty lines are -// ignored and leading/trailing whitespace is removed. -std::vector ReadTextFileLines(const std::string &path); - -// Remove an argument from the list of command-line arguments. -void RemoveCommandLineArgument(const std::string &arg, int *argc, char **argv); - -//////////////////////////////////////////////////////////////////////////////// -// Implementation -//////////////////////////////////////////////////////////////////////////////// - -template std::string JoinPaths(T const &...paths) { - boost::filesystem::path result; - int unpack[]{0, (result = result / boost::filesystem::path(paths), 0)...}; - static_cast(unpack); - return result.string(); -} - -template -bool VectorContainsValue(const std::vector &vector, const T value) { - return std::find_if(vector.begin(), vector.end(), [value](const T element) { - return element == value; - }) != vector.end(); -} - -template -bool VectorContainsDuplicateValues(const std::vector &vector) { - std::vector unique_vector = vector; - return std::unique(unique_vector.begin(), unique_vector.end()) != - unique_vector.end(); -} - -template std::string VectorToCSV(const std::vector &values) { - std::string string; - for (const T value : values) { - string += std::to_string(value) + ", "; - } - return string.substr(0, string.length() - 2); -} - -template -void ReadBinaryBlob(const std::string &path, std::vector *data) { - std::ifstream file(path, std::ios::binary | std::ios::ate); - CHECK(file.is_open()) << path; - file.seekg(0, std::ios::end); - const size_t num_bytes = file.tellg(); - CHECK_EQ(num_bytes % sizeof(T), 0); - data->resize(num_bytes / sizeof(T)); - file.seekg(0, std::ios::beg); - ReadBinaryLittleEndian(&file, data); -} - -template -void WriteBinaryBlob(const std::string &path, const std::vector &data) { - std::ofstream file(path, std::ios::binary); - CHECK(file.is_open()) << path; - WriteBinaryLittleEndian(&file, data); -} - -} // namespace colmap - -#endif // COLMAP_SRC_UTIL_MISC_H_ diff --git a/src/geometry/colmap/util/string.cc b/src/geometry/colmap/util/string.cc deleted file mode 100644 index ddd557b..0000000 --- a/src/geometry/colmap/util/string.cc +++ /dev/null @@ -1,204 +0,0 @@ -// Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of -// its contributors may be used to endorse or promote products derived -// from this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// -// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) - -#include "string.h" - -#include -#include -#include -#include -#include - -namespace colmap { -namespace { - -// The StringAppendV function is borrowed from Google under the BSD license: -// -// Copyright 2012 Google Inc. All rights reserved. -// https://developers.google.com/protocol-buffers/ -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// * Redistributions in binary form must reproduce the above -// copyright notice, this list of conditions and the following disclaimer -// in the documentation and/or other materials provided with the -// distribution. -// * Neither the name of Google Inc. nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR -// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT -// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, -// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT -// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY -// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -void StringAppendV(std::string *dst, const char *format, va_list ap) { - // First try with a small fixed size buffer. - static const int kFixedBufferSize = 1024; - char fixed_buffer[kFixedBufferSize]; - - // It is possible for methods that use a va_list to invalidate - // the data in it upon use. The fix is to make a copy - // of the structure before using it and use that copy instead. - va_list backup_ap; - va_copy(backup_ap, ap); - int result = vsnprintf(fixed_buffer, kFixedBufferSize, format, backup_ap); - va_end(backup_ap); - - if (result < kFixedBufferSize) { - if (result >= 0) { - // Normal case - everything fits. - dst->append(fixed_buffer, result); - return; - } - -#ifdef _MSC_VER - // Error or MSVC running out of space. MSVC 8.0 and higher - // can be asked about space needed with the special idiom below: - va_copy(backup_ap, ap); - result = vsnprintf(nullptr, 0, format, backup_ap); - va_end(backup_ap); -#endif - - if (result < 0) { - // Just an error. - return; - } - } - - // Increase the buffer size to the size requested by vsnprintf, - // plus one for the closing \0. - const int variable_buffer_size = result + 1; - std::unique_ptr variable_buffer(new char[variable_buffer_size]); - - // Restore the va_list before we use it again. - va_copy(backup_ap, ap); - result = vsnprintf(variable_buffer.get(), variable_buffer_size, format, - backup_ap); - va_end(backup_ap); - - if (result >= 0 && result < variable_buffer_size) { - dst->append(variable_buffer.get(), result); - } -} - -bool IsNotWhiteSpace(const int character) { - return character != ' ' && character != '\n' && character != '\r' && - character != '\t'; -} - -} // namespace - -std::string StringPrintf(const char *format, ...) { - va_list ap; - va_start(ap, format); - std::string result; - StringAppendV(&result, format, ap); - va_end(ap); - return result; -} - -std::string StringReplace(const std::string &str, const std::string &old_str, - const std::string &new_str) { - if (old_str.empty()) { - return str; - } - size_t position = 0; - std::string mod_str = str; - while ((position = mod_str.find(old_str, position)) != std::string::npos) { - mod_str.replace(position, old_str.size(), new_str); - position += new_str.size(); - } - return mod_str; -} - -std::string StringGetAfter(const std::string &str, const std::string &key) { - if (key.empty()) { - return str; - } - std::size_t found = str.rfind(key); - if (found != std::string::npos) { - return str.substr(found + key.length(), - str.length() - (found + key.length())); - } - return ""; -} - -std::vector StringSplit(const std::string &str, - const std::string &delim) { - std::vector elems; - boost::split(elems, str, boost::is_any_of(delim), boost::token_compress_on); - return elems; -} - -bool StringStartsWith(const std::string &str, const std::string &prefix) { - return !prefix.empty() && prefix.size() <= str.size() && - str.substr(0, prefix.size()) == prefix; -} - -void StringLeftTrim(std::string *str) { - str->erase(str->begin(), - std::find_if(str->begin(), str->end(), IsNotWhiteSpace)); -} - -void StringRightTrim(std::string *str) { - str->erase(std::find_if(str->rbegin(), str->rend(), IsNotWhiteSpace).base(), - str->end()); -} - -void StringTrim(std::string *str) { - StringLeftTrim(str); - StringRightTrim(str); -} - -void StringToLower(std::string *str) { - std::transform(str->begin(), str->end(), str->begin(), ::tolower); -} - -void StringToUpper(std::string *str) { - std::transform(str->begin(), str->end(), str->begin(), ::toupper); -} - -bool StringContains(const std::string &str, const std::string &sub_str) { - return str.find(sub_str) != std::string::npos; -} - -} // namespace colmap diff --git a/src/geometry/colmap/util/string.h b/src/geometry/colmap/util/string.h deleted file mode 100644 index dc5d7eb..0000000 --- a/src/geometry/colmap/util/string.h +++ /dev/null @@ -1,73 +0,0 @@ -// Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of -// its contributors may be used to endorse or promote products derived -// from this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// -// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) - -#ifndef COLMAP_SRC_UTIL_STRING_H_ -#define COLMAP_SRC_UTIL_STRING_H_ - -#include -#include - -namespace colmap { - -// Format string by replacing embedded format specifiers with their respective -// values, see `printf` for more details. This is a modified implementation -// of Google's BSD-licensed StringPrintf function. -std::string StringPrintf(const char *format, ...); - -// Replace all occurrences of `old_str` with `new_str` in the given string. -std::string StringReplace(const std::string &str, const std::string &old_str, - const std::string &new_str); - -// Get substring of string after search key -std::string StringGetAfter(const std::string &str, const std::string &key); - -// Split string into list of words using the given delimiters. -std::vector StringSplit(const std::string &str, - const std::string &delim); - -// Check whether a string starts with a certain prefix. -bool StringStartsWith(const std::string &str, const std::string &prefix); - -// Remove whitespace from string on both, left, or right sides. -void StringTrim(std::string *str); -void StringLeftTrim(std::string *str); -void StringRightTrim(std::string *str); - -// Convert string to lower/upper case. -void StringToLower(std::string *str); -void StringToUpper(std::string *str); - -// Check whether the sub-string is contained in the given string. -bool StringContains(const std::string &str, const std::string &sub_str); - -} // namespace colmap - -#endif // COLMAP_SRC_UTIL_STRING_H_ diff --git a/src/geometry/colmap/util/types.h b/src/geometry/colmap/util/types.h deleted file mode 100644 index edd251e..0000000 --- a/src/geometry/colmap/util/types.h +++ /dev/null @@ -1,120 +0,0 @@ -// Copyright (c) 2018, ETH Zurich and UNC Chapel Hill. -// All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of -// its contributors may be used to endorse or promote products derived -// from this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// -// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) - -#ifndef COLMAP_SRC_UTIL_TYPES_H_ -#define COLMAP_SRC_UTIL_TYPES_H_ - -//#include "util/alignment.h" - -#ifdef _MSC_VER -#if _MSC_VER >= 1600 -#include -#else -typedef __int8 int8_t; -typedef __int16 int16_t; -typedef __int32 int32_t; -typedef __int64 int64_t; -typedef unsigned __int8 uint8_t; -typedef unsigned __int16 uint16_t; -typedef unsigned __int32 uint32_t; -typedef unsigned __int64 uint64_t; -#endif -#elif __GNUC__ >= 3 -#include -#endif - -// Define non-copyable or non-movable classes. -#define NON_COPYABLE(class_name) \ - class_name(class_name const &) = delete; \ - void operator=(class_name const &obj) = delete; -#define NON_MOVABLE(class_name) class_name(class_name &&) = delete; - -#include - -namespace Eigen { - -typedef Eigen::Matrix Matrix3x4f; -typedef Eigen::Matrix Matrix3x4d; -typedef Eigen::Matrix Vector3ub; -typedef Eigen::Matrix Vector4ub; -typedef Eigen::Matrix Vector6d; - -} // namespace Eigen - -namespace colmap { - -//////////////////////////////////////////////////////////////////////////////// -// Index types, determines the maximum number of objects. -//////////////////////////////////////////////////////////////////////////////// - -// Unique identifier for cameras. -typedef uint32_t camera_t; - -// Unique identifier for images. -typedef uint32_t image_t; - -// Each image pair gets a unique ID, see `Database::ImagePairToPairId`. -typedef uint64_t image_pair_t; - -// Index per image, i.e. determines maximum number of 2D points per image. -typedef uint32_t point2D_t; - -// Unique identifier per added 3D point. Since we add many 3D points, -// delete them, and possibly re-add them again, the maximum number of allowed -// unique indices should be large. -typedef uint64_t point3D_t; - -// Values for invalid identifiers or indices. -const camera_t kInvalidCameraId = std::numeric_limits::max(); -const image_t kInvalidImageId = std::numeric_limits::max(); -const image_pair_t kInvalidImagePairId = - std::numeric_limits::max(); -const point2D_t kInvalidPoint2DIdx = std::numeric_limits::max(); -const point3D_t kInvalidPoint3DId = std::numeric_limits::max(); - -} // namespace colmap - -// This file provides specializations of the templated hash function for -// custom types. These are used for comparison in unordered sets/maps. -namespace std { - -// Hash function specialization for uint32_t pairs, e.g., image_t or camera_t. -template <> struct hash> { - std::size_t operator()(const std::pair &p) const { - const uint64_t s = (static_cast(p.first) << 32) + - static_cast(p.second); - return std::hash()(s); - } -}; - -} // namespace std - -#endif // COLMAP_SRC_UTIL_TYPES_H_ diff --git a/src/geometry/epipolar_geometry.hpp b/src/geometry/epipolar_geometry.hpp index 987fb58..b8e3257 100644 --- a/src/geometry/epipolar_geometry.hpp +++ b/src/geometry/epipolar_geometry.hpp @@ -4,7 +4,7 @@ #include "base/map.h" #include "estimators/fundamental_matrix.h" -#include "optim/loransac.h" +#include "ransac/loransac.h" namespace xrsfm { inline void SolveFundamnetalCOLMAP(const std::vector &points1, diff --git a/src/geometry/error_corrector.cc b/src/geometry/error_corrector.cc index 41f53d0..b6ab9ae 100644 --- a/src/geometry/error_corrector.cc +++ b/src/geometry/error_corrector.cc @@ -7,7 +7,7 @@ namespace xrsfm { inline std::vector GetMatchedFrameIds(Map &map, int frame_id) { std::vector matched_frame_ids; for (const int &id : map.frameid2matched_frameids_[frame_id]) { - if (!map.frames_[id].registered) + if (!map.frame(id).registered) continue; matched_frame_ids.emplace_back(id); } @@ -27,8 +27,7 @@ std::vector> DivideMatchedFrames(Map &map, Frame &frame, for (auto &track_id : frame.track_ids_) { if (track_id == -1) continue; - for (auto &[t_frame_id, t_p2d_id] : - map.tracks_[track_id].observations_) { + for (auto &[t_frame_id, t_p2d_id] : map.track(track_id).observations_) { if (id2num_covisible1.count(t_frame_id) != 0) { id2num_covisible1[t_frame_id]++; } @@ -37,8 +36,7 @@ std::vector> DivideMatchedFrames(Map &map, Frame &frame, for (auto &track_id : frame2.track_ids_) { if (track_id == -1) continue; - for (auto &[t_frame_id, t_p2d_id] : - map.tracks_[track_id].observations_) { + for (auto &[t_frame_id, t_p2d_id] : map.track(track_id).observations_) { if (id2num_covisible2.count(t_frame_id) != 0) { id2num_covisible2[t_frame_id]++; } @@ -77,8 +75,8 @@ LoopInfo GetLoopInfo(Map &map, Frame &frame1, Frame &frame2) { track_id2 = frame2.track_ids_[i]; if (track_id1 == -1 || track_id2 == -1) continue; - const auto &track1 = map.tracks_[track_id1], - &track2 = map.tracks_[track_id2]; + const auto &track1 = map.track(track_id1), + &track2 = map.track(track_id2); vector3 p3d1 = frame1.Tcw.q * track1.point3d_ + frame1.Tcw.t; vector3 p3d2 = frame2.Tcw.q * track2.point3d_ + frame2.Tcw.t; @@ -99,7 +97,7 @@ bool CheckNegtiveDepth(const Map &map, const Frame &frame1, for (auto &track_id : frame1.track_ids_) { if (track_id == -1) continue; - auto &track = map.tracks_[track_id]; + auto &track = map.track(track_id); const vector3 p3d = frame2.Tcw.q * track.point3d_ + frame2.Tcw.t; if (p3d.z() < 0) { return true; @@ -108,7 +106,7 @@ bool CheckNegtiveDepth(const Map &map, const Frame &frame1, for (auto &track_id : frame2.track_ids_) { if (track_id == -1) continue; - auto &track = map.tracks_[track_id]; + auto &track = map.track(track_id); const vector3 p3d = frame1.Tcw.q * track.point3d_ + frame1.Tcw.t; if (p3d.z() < 0) { return true; @@ -120,67 +118,55 @@ bool CheckNegtiveDepth(const Map &map, const Frame &frame1, inline bool TryLocate(Map &map, const int frame_id, const std::set &local_frame_ids, Point3dProcessor *p3d_processor_) { - bool reg_success = RegisterNextImageLocal(frame_id, local_frame_ids, map); + bool reg_success = RegisterImageLocal(frame_id, local_frame_ids, map); if (!reg_success) { bool have_neighbor = false; std::vector adjacent_frame_ids = {frame_id - 1, frame_id + 1}; for (auto &id : adjacent_frame_ids) { if (local_frame_ids.count(id) != 0) { - map.frames_[frame_id].registered = false; + map.frame(frame_id).registered = false; p3d_processor_->TriangulateFramePoint( map, id, p3d_processor_->th_rpe_lba_); - map.frames_[frame_id].registered = true; + map.frame(frame_id).registered = true; have_neighbor = true; } } if (have_neighbor) - reg_success = - RegisterNextImageLocal(frame_id, local_frame_ids, map); + reg_success = RegisterImageLocal(frame_id, local_frame_ids, map); } return reg_success; } inline void MergeTrackLoop(Map &map, Frame &frame1, Frame &frame2) { for (size_t i = 0; i < frame1.track_ids_.size(); ++i) { - const int track_id = - frame2.track_ids_[i]; // not add num cor have point 3d + // merge track in two local_map + const int track_id = frame2.track_ids_[i]; if (track_id == -1) continue; - auto &track = map.tracks_[track_id]; + auto &track = map.track(track_id); if (track.observations_.count(frame1.id) != 0) - continue; // may compare - - vector3 p3d = frame2.Tcw.q * track.point3d_ + frame2.Tcw.t; - vector3 p3d1 = p3d.z() * frame1.points_normalized[i].homogeneous(); - vector3 p3d2 = frame1.Tcw.q.inverse() * (p3d1 - frame1.Tcw.t); + continue; - bool merge = false; const int track_id1 = frame1.track_ids_[i]; - if (track_id1 != -1) { // merge - auto &track1 = map.tracks_[track_id1]; - // track.point3d_ = (track.point3d_ + track1.point3d_) / 2; + if (track_id1 != -1) { // try merge track1 into track + auto &track1 = map.track(track_id1); for (const auto &[t_frame_id, t_p2d_id] : track1.observations_) { - if (track.observations_.count(t_frame_id) == - 0) { // if track have t_frame_id obs, it map injective - // injective + if (track.observations_.count(t_frame_id) == 0) { track.observations_[t_frame_id] = t_p2d_id; - map.frames_[t_frame_id].track_ids_[t_p2d_id] = track_id; + map.frame(t_frame_id).track_ids_[t_p2d_id] = track_id; } else { - map.frames_[t_frame_id].track_ids_[t_p2d_id] = -1; + map.frame(t_frame_id).track_ids_[t_p2d_id] = -1; map.DeleteNumCorHavePoint3D(t_frame_id, t_p2d_id); } } track1.outlier = true; - merge = true; + continue; } - if (!merge) { - // track.point3d_ = (track.point3d_ + p3d2) / 2; - frame1.track_ids_[i] = track_id; - track.observations_[frame1.id] = i; - map.AddNumCorHavePoint3D(frame1.id, i); - } + frame1.track_ids_[i] = track_id; + track.observations_[frame1.id] = i; + map.AddNumCorHavePoint3D(frame1.id, i); } } @@ -192,18 +178,13 @@ bool ErrorCorrector::CheckAndCorrectPose(Map &map, int frame_id, int iter) { if (!TryLocate(map, frame_id, bad_matched_frame_ids, p3d_processor_)) return false; - auto &frame = map.frames_[frame_id]; + auto &frame = map.frame(frame_id); const std::vector matched_frame_ids = GetMatchedFrameIds(map, frame_id); for (auto &id : matched_frame_ids) { std::cout << "|" << id << std::endl; } - // error_detector.viewerTh_->update_map(map); - // cv::Mat img(10,10,CV_8U); - // cv::imshow("",img); - // cv::waitKey(); - // PoseGraph const double dist = (frame.Tcw.center() - map.tmp_frame.center()).norm(); const bool negtive_depth = CheckNegtiveDepth(map, map.tmp_frame, frame); @@ -220,10 +201,6 @@ bool ErrorCorrector::CheckAndCorrectPose(Map &map, int frame_id, int iter) { ba_solver_->ScalePoseGraphUnorder(loop_info, map, true); } - // error_detector.viewerTh_->update_map(map); - // cv::imshow("",img); - // cv::waitKey(); - MergeTrackLoop(map, frame, map.tmp_frame); // BA @@ -236,7 +213,6 @@ bool ErrorCorrector::CheckAndCorrectPose(Map &map, int frame_id, int iter) { ba_solver_->KGBA(map, matched_frame_ids, true); p3d_processor_->FilterPoints3d(map, p3d_processor_->th_rpe_gba_, p3d_processor_->th_angle_gba_); - // TODO num_image_reg_pre = num_image_reg; } error_detector.CheckAllRelativePose(map, frame_id, bad_matched_frame_ids); diff --git a/src/geometry/error_corrector.h b/src/geometry/error_corrector.h index 9e2105a..89ddda4 100644 --- a/src/geometry/error_corrector.h +++ b/src/geometry/error_corrector.h @@ -4,8 +4,6 @@ #include "base/map.h" #include "geometry/track_processor.h" #include "optimization/ba_solver.h" -// #include "utility/view.h" -// #include "utility/viewer.h" namespace xrsfm { inline double ToDeg(double theta) { return theta * 180 / M_PI; } @@ -16,8 +14,6 @@ class ErrorDetector { std::set &bad_matched_frame_ids); bool IsGoodRelativePose(const Map &map, const FramePair &fp, std::vector &inlier_mask); - - std::string image_dir_; }; class ErrorCorrector { diff --git a/src/geometry/error_detector.cc b/src/geometry/error_detector.cc index d54e6cb..9604530 100644 --- a/src/geometry/error_detector.cc +++ b/src/geometry/error_detector.cc @@ -15,7 +15,7 @@ bool ErrorDetector::IsGoodRelativePose(const Map &map, const FramePair &fp, if ((std::abs(fp.id1 - fp.id2) != 1) && fp.matches.size() < num_min_matches) return true; - const auto &frame1 = map.frames_[fp.id1], &frame2 = map.frames_[fp.id2]; + const auto &frame1 = map.frame(fp.id1), &frame2 = map.frame(fp.id2); const vector3 relative_motion = frame2.center() - frame1.center(); const double distance = relative_motion.norm(); const bool is_pure_rotation = distance < pure_rotation_th; @@ -92,9 +92,6 @@ bool ErrorDetector::IsGoodRelativePose(const Map &map, const FramePair &fp, num_matches); if (ratio < ratio_th) { - // std::cout << frame1.tcw.q.coeffs().transpose() << " " << - // frame1.tcw.t.transpose() << std::endl;nn - return false; } return true; @@ -103,51 +100,48 @@ bool ErrorDetector::IsGoodRelativePose(const Map &map, const FramePair &fp, bool ErrorDetector::CheckAllRelativePose(Map &map, int frame_id, std::set &bad_matched_frame_ids) { bad_matched_frame_ids.clear(); - const auto &frame = map.frames_[frame_id]; + const auto &frame = map.frame(frame_id); std::map id2num_covisible_obs; for (const auto &track_id : frame.track_ids_) { if (track_id == -1) continue; - const auto &track = map.tracks_[track_id]; + const auto &track = map.track(track_id); for (const auto &[t_frame_id, t_p2d_id] : track.observations_) { if (id2num_covisible_obs.count(t_frame_id) == 0) { id2num_covisible_obs[t_frame_id] = 1; } else { - id2num_covisible_obs[t_frame_id]++; + id2num_covisible_obs.at(t_frame_id)++; } } } int num_good = 0, num_all = 0; for (const auto id : map.frameid2framepairids_[frame_id]) { - auto &fp = map.frame_pairs_[id]; + const auto &fp = map.frame_pairs_[id]; + const auto &frame1 = map.frame(fp.id1); + const auto &frame2 = map.frame(fp.id2); + if (!(frame1.registered && frame2.registered)) + continue; + int num_covise = -1; if (fp.id1 == frame_id) { - num_covise = id2num_covisible_obs[fp.id2]; + if (id2num_covisible_obs.count(fp.id2) != 0) + num_covise = id2num_covisible_obs.at(fp.id2); } else { - num_covise = id2num_covisible_obs[fp.id1]; + if (id2num_covisible_obs.count(fp.id1) != 0) + num_covise = id2num_covisible_obs.at(fp.id1); } if (num_covise >= 10) continue; + // matched but not covisible - const auto &frame1 = map.frames_[fp.id1]; - const auto &frame2 = map.frames_[fp.id2]; - // std::cout< inlier_mask; - if (IsGoodRelativePose(map, fp, inlier_mask)) { - ++num_good; - } else { - int bad_neighbor_id = fp.id1 == frame_id ? fp.id2 : fp.id1; - bad_matched_frame_ids.insert(bad_neighbor_id); - } + ++num_all; + std::vector inlier_mask; + if (IsGoodRelativePose(map, fp, inlier_mask)) { // TODO use tri angle + ++num_good; + } else { + int bad_neighbor_id = fp.id1 == frame_id ? fp.id2 : fp.id1; + bad_matched_frame_ids.insert(bad_neighbor_id); } } diff --git a/src/geometry/essential.cc b/src/geometry/essential.cc index 87baabd..faa5a50 100644 --- a/src/geometry/essential.cc +++ b/src/geometry/essential.cc @@ -2,7 +2,7 @@ #include -#include "utility/random.h" +#include "random.h" namespace xrsfm { namespace { diff --git a/src/geometry/map_initializer.cc b/src/geometry/map_initializer.cc index b55aeaf..124d582 100644 --- a/src/geometry/map_initializer.cc +++ b/src/geometry/map_initializer.cc @@ -4,16 +4,15 @@ #include "geometry/map_initializer.h" -#include "geometry/colmap/base/triangulation.h" #include "geometry/essential.h" -#include "geometry/triangluate_svd.h" +#include "geometry/triangulate_light.h" namespace xrsfm { bool CheckInitFramePair(const Map &map, FramePair &frame_pair, double min_angle = 16.0) { - auto &frame1 = map.frames_[frame_pair.id1]; - auto &frame2 = map.frames_[frame_pair.id2]; + auto &frame1 = map.frame(frame_pair.id1); + auto &frame2 = map.frame(frame_pair.id2); int inlier_num; std::vector inlier_mask; @@ -44,7 +43,7 @@ bool CheckInitFramePair(const Map &map, FramePair &frame_pair, for (size_t i = 0; i < num_matches; ++i) { if (frame_pair.inlier_mask[i]) { // assert track valid if (inlier_mask[id_p3d]) { - double angle = colmap::CalculateTriangulationAngle( + double angle = CalculateTriangulationAngleLight( pose1.center(), pose2.center(), point3ds[id_p3d]); if (angle > DegToRad(min_angle)) { num_p3d_valid++; @@ -110,7 +109,6 @@ bool FindInitFramePair(const Map &map, FramePair &init_frame_pair) { } for (const auto &init_id2 : init_id2_candidate) { - // std::cout< points2ds; @@ -79,13 +78,14 @@ bool RegisterImage(const int frame_id, Map &map) { continue; num_inlier++; const auto &[p2d_id, track_id] = id_pair_vec[id]; - auto &track = map.tracks_[track_id]; - // TODO do something if this track has been observed by this frame + auto &track = map.track(track_id); if (track.observations_.count(frame_id) == 0) { frame.track_ids_[p2d_id] = track_id; track.observations_[frame_id] = p2d_id; map.AddNumCorHavePoint3D(frame_id, p2d_id); level_vec.emplace_back(track.hierarchical_level); + } else { // if this track has been observed + // TODO compare the reprojection error } } std::sort(level_vec.begin(), level_vec.end()); @@ -98,41 +98,9 @@ bool RegisterImage(const int frame_id, Map &map) { return true; } -bool RegisterNextImageLocal(const int _frame_id, const std::set cor_set, - Map &map, Pose &tcw, - std::vector> &cor_2d_3d_ids) { - Frame &next_frame = map.frames_[_frame_id]; - // search for 2D-3D correspondences - std::vector cor_points2ds; - std::vector cor_points3ds; - map.SearchCorrespondences1(next_frame, cor_set, cor_points2ds, - cor_points3ds, cor_2d_3d_ids, true); - printf("frame: %d cor num: %zu\n", _frame_id, cor_2d_3d_ids.size()); - if (cor_2d_3d_ids.size() < 20) - return false; - const double max_error = 16.0 / map.Camera(next_frame.camera_id).fx(); - std::vector inlier_mask; - if (SolvePnP_colmap(cor_points2ds, cor_points3ds, max_error, tcw, - inlier_mask)) { - // Continue tracks - std::vector> inlier_cor_2d_3d_ids; - for (int id = 0; id < inlier_mask.size(); ++id) { - if (!inlier_mask[id]) - continue; - inlier_cor_2d_3d_ids.emplace_back(cor_2d_3d_ids[id]); - } - printf("PnP %zu/%zu\n", inlier_cor_2d_3d_ids.size(), - cor_2d_3d_ids.size()); - cor_2d_3d_ids = inlier_cor_2d_3d_ids; - return true; - } - LOG(WARNING) << "fail to register frame " << _frame_id; - return false; -} - -bool RegisterNextImageLocal(const int _frame_id, const std::set cor_set, - Map &map) { - Frame &next_frame = map.frames_[_frame_id]; +bool RegisterImageLocal(const int _frame_id, const std::set cor_set, + Map &map) { + Frame &next_frame = map.frame(_frame_id); // search for 2D-3D correspondences std::vector cor_points2ds; std::vector cor_points3ds; @@ -167,151 +135,28 @@ bool RegisterNextImageLocal(const int _frame_id, const std::set cor_set, return false; } -bool SolvePnP(Camera cam, std::vector cor_points2ds, - std::vector cor_points3ds, Frame &frame, - std::vector &inlier) { - size_t cor_num = cor_points2ds.size(); - if (cor_num < 20) { - printf("there are no enough correspondence\n"); - return false; - } - std::vector cvpt3d(cor_num); - std::vector cvpt2d(cor_num); - for (int i = 0; i < cor_num; ++i) { - cvpt2d[i] = cv::Point2d(cor_points2ds[i](0), cor_points2ds[i](1)); - cvpt3d[i] = cv::Point3d(cor_points3ds[i](0), cor_points3ds[i](1), - cor_points3ds[i](2)); - } - cv::Mat camera_matrix = cv::Mat(3, 3, CV_64FC1, cv::Scalar::all(0)); - camera_matrix.ptr(0)[0] = cam.fx(); - camera_matrix.ptr(0)[2] = cam.cx(); - camera_matrix.ptr(1)[1] = cam.fy(); - camera_matrix.ptr(1)[2] = cam.cy(); - camera_matrix.ptr(2)[2] = 1.0f; - - cv::Mat distortion_coeffs = cv::Mat(5, 1, CV_64FC1, cv::Scalar::all(0)); - cv::Mat rvec, tvec, rmat; - - // there are randomness in cv::solvePnPRansac - srand(0); - bool success = - solvePnPRansac(cvpt3d, cvpt2d, camera_matrix, distortion_coeffs, rvec, - tvec, false, 50, 3.5, 0.99, inlier, cv::SOLVEPNP_EPNP); - if (success) { - Rodrigues(rvec, rmat); - matrix3 rmatrix; - rmatrix << rmat.ptr(0)[0], rmat.ptr(0)[1], - rmat.ptr(0)[2], rmat.ptr(1)[0], - rmat.ptr(1)[1], rmat.ptr(1)[2], - rmat.ptr(2)[0], rmat.ptr(2)[1], - rmat.ptr(2)[2]; - vector3 t(tvec.at(0), tvec.at(1), tvec.at(2)); - frame.Tcw = Pose(quaternion(rmatrix), t); - frame.registered = true; - printf("PnP %d/%d\n", inlier.size(), cvpt2d.size()); - } - return success; -} - -using namespace colmap; - -struct AbsolutePoseEstimationOptions { - // Whether to estimate the focal length. - bool estimate_focal_length = false; - - // Number of discrete samples for focal length estimation. - size_t num_focal_length_samples = 30; - - // Minimum focal length ratio for discrete focal length sampling - // around focal length of given camera. - double min_focal_length_ratio = 0.2; - - // Maximum focal length ratio for discrete focal length sampling - // around focal length of given camera. - double max_focal_length_ratio = 5; - - // Number of threads for parallel estimation of focal length. - int num_threads = 1; - - // Options used for P3P RANSAC. - RANSACOptions ransac_options; - - void Check() const { - CHECK_GT(num_focal_length_samples, 0); - CHECK_GT(min_focal_length_ratio, 0); - CHECK_GT(max_focal_length_ratio, 0); - CHECK_LT(min_focal_length_ratio, max_focal_length_ratio); - ransac_options.Check(); - } -}; - -bool EstimateAbsolutePose(const AbsolutePoseEstimationOptions &options, - const std::vector &points2D, - const std::vector &points3D, Pose &pose, - std::vector *inlier_mask); - bool SolvePnP_colmap(const std::vector &cor_points2ds, const std::vector &cor_points3ds, const double max_error, Pose &tcw, std::vector &inlier_mask) { - AbsolutePoseEstimationOptions abs_pose_options; - abs_pose_options.num_threads = 1; - abs_pose_options.num_focal_length_samples = 30; - abs_pose_options.ransac_options.max_error = max_error; - abs_pose_options.ransac_options.min_inlier_ratio = 0.25; - // Use high confidence to avoid preemptive termination of P3P RANSAC - // - too early termination may lead to bad registration. - abs_pose_options.ransac_options.min_num_trials = 100; - abs_pose_options.ransac_options.confidence = 0.9999; - abs_pose_options.estimate_focal_length = false; - - bool success = EstimateAbsolutePose(abs_pose_options, cor_points2ds, - cor_points3ds, tcw, &inlier_mask); - - return success; -} - -typedef LORANSAC AbsolutePoseRANSAC; - -void EstimateAbsolutePoseKernel(const std::vector &points2D, - const std::vector &points3D, - const RANSACOptions &options, - AbsolutePoseRANSAC::Report *report) { - // Estimate pose for given focal length. - AbsolutePoseRANSAC ransac(options); - - std::vector points2D_N(points2D.size()); - for (size_t i = 0; i < points2D.size(); ++i) { - points2D_N[i] = points2D[i]; - } - - *report = ransac.Estimate(points2D_N, points3D); -} - -bool EstimateAbsolutePose(const AbsolutePoseEstimationOptions &options, - const std::vector &points2D, - const std::vector &points3D, Pose &tcw, - std::vector *inlier_mask) { - AbsolutePoseRANSAC::Report report; - - EstimateAbsolutePoseKernel(points2D, points3D, options.ransac_options, - &report); - + using namespace colmap; + RANSACOptions ransac_options; + ransac_options.max_error = max_error; + ransac_options.min_inlier_ratio = 0.25; + ransac_options.min_num_trials = 100; + ransac_options.confidence = 0.9999; + LORANSAC ransac(ransac_options); + auto report = ransac.Estimate(cor_points2ds, cor_points3ds); if (!report.success || report.support.num_inliers == 0) { return false; } - Eigen::Matrix3x4d proj_matrix; - proj_matrix = report.model; - *inlier_mask = report.inlier_mask; + inlier_mask = report.inlier_mask; + Eigen::Matrix3x4d proj_matrix = report.model; quaternion quat(proj_matrix.leftCols<3>()); - tcw.q = quat; tcw.t = proj_matrix.rightCols<1>(); - // if (IsNaN(*qvec) || IsNaN(*tvec)) { - // return false; - // } - return true; } + } // namespace xrsfm diff --git a/src/geometry/pnp.h b/src/geometry/pnp.h index 1837d7d..294b1d4 100644 --- a/src/geometry/pnp.h +++ b/src/geometry/pnp.h @@ -11,23 +11,10 @@ #include "base/map.h" namespace xrsfm { -bool ComputeRegisterInlierLoop(const int next_frame_id, LoopInfo &loop_info, - Map &map); - -bool RegisterNextImageLoop(const int next_frame_id, LoopInfo &loop_info, - Map &map); - bool RegisterImage(const int next_frame_id, Map &map); -bool RegisterNextImage(const int next_frame_id, Map &map, Pose &tcw, - std::vector> &cor_2d_3d_ids); - -bool RegisterNextImageLocal(const int next_frame_id, - const std::set cor_set, Map &map, Pose &tcw, - std::vector> &cor_2d_3d_ids); - -bool RegisterNextImageLocal(const int next_frame_id, - const std::set cor_set, Map &map); +bool RegisterImageLocal(const int next_frame_id, const std::set cor_set, + Map &map); bool SolvePnP_colmap(const std::vector &cor_points2ds, const std::vector &cor_points3ds, diff --git a/src/utility/random.h b/src/geometry/random.h similarity index 99% rename from src/utility/random.h rename to src/geometry/random.h index 0546e99..39a753f 100644 --- a/src/utility/random.h +++ b/src/geometry/random.h @@ -11,7 +11,7 @@ #include #include -#include "global.h" +#include "utility/global.h" namespace xrsfm { diff --git a/src/geometry/track_processor.cc b/src/geometry/track_processor.cc index 23293d9..ab13d4b 100644 --- a/src/geometry/track_processor.cc +++ b/src/geometry/track_processor.cc @@ -4,20 +4,19 @@ #include "track_processor.h" -#include "base/camera.h" -#include "geometry/colmap/estimators/triangulation.h" -#include "geometry/triangluate_svd.h" +#include "geometry/triangulate_light.h" namespace xrsfm { -double ReprojectionError(const Pose &pose, const vector2 &point2d, - const vector3 &point3d) { - vector3 p_c = pose.q * point3d + pose.t; - vector2 residual = p_c.hnormalized() - point2d; - return residual.norm(); + +size_t NChooseK(const size_t n, const size_t k) { + if (k == 0) { + return 1; + } + return (n * NChooseK(n - 1, k - 1)) / k; } -double Reprojection_Error(const Pose &pose, const Camera &camera, - const vector2 &point2d, const vector3 &point3d) { +double ReprojectionError(const Pose &pose, const Camera &camera, + const vector2 &point2d, const vector3 &point3d) { vector3 p_c = pose.q * point3d + pose.t; vector2 estimate; NormalizedToImage(camera, p_c.hnormalized(), estimate); @@ -35,9 +34,9 @@ void AddTrack(const std::vector> &observations, int min_level1 = -1, min_level2 = -1; for (const auto &[frame_id, p2d_id] : observations) { track.observations_[frame_id] = p2d_id; - map.frames_[frame_id].track_ids_[p2d_id] = track_id; + map.frame(frame_id).track_ids_[p2d_id] = track_id; - int level = map.frames_[frame_id].hierarchical_level; + int level = map.frame(frame_id).hierarchical_level; if (level == -1) continue; @@ -64,10 +63,10 @@ void AddTrack(const std::vector> &observations, void ContinueTrackUpdate(const int track_id, const int frame_id, const int p2d_id, Map &map) { - auto &track = map.tracks_[track_id]; + auto &track = map.track(track_id); track.observations_[frame_id] = p2d_id; - auto &frame = map.frames_[frame_id]; + auto &frame = map.frame(frame_id); frame.track_ids_[p2d_id] = track_id; map.AddNumCorHavePoint3D(frame_id, p2d_id); @@ -76,78 +75,46 @@ void ContinueTrackUpdate(const int track_id, const int frame_id, void SetTrackOutlier(Map &map, Track &track) { track.outlier = true; for (const auto &[t_frame_id, t_p2d_id] : track.observations_) { - map.frames_[t_frame_id].track_ids_[t_p2d_id] = -1; + map.frame(t_frame_id).track_ids_[t_p2d_id] = -1; map.DeleteNumCorHavePoint3D(t_frame_id, t_p2d_id); } } bool CreatePoint3d(const std::vector> observations, Map &map) { - std::vector> Ps; - std::vector> ps; // Prepare the data - for (auto &[t_frame_id, t_p2d_id] : observations) { - Frame &t_frame = map.frames_[t_frame_id]; - matrix<3, 3> R = t_frame.Tcw.q.toRotationMatrix(); - vector<3> T = t_frame.Tcw.t; - matrix<3, 4> P; - P << R, T; - Ps.push_back(P); - vector<2> p2d_n = GetPointNormalized(map.Camera(t_frame.camera_id), - t_frame.points[t_p2d_id]); - ps.push_back(p2d_n); - } - // make new track; - vector<3> p; - if (triangulate_point_checked(Ps, ps, p)) { - AddTrack(observations, p, map); - return true; - } - return false; -} - -bool CreatePoint3d1(const std::vector> observations, - Map &map) { size_t data_size = observations.size(); - std::vector point_data( - data_size); - std::vector pose_data(data_size); - // Prepare the data + std::vector point_data(data_size); + std::vector pose_data(data_size); for (size_t i = 0; i < data_size; ++i) { const auto &[t_frame_id, t_p2d_id] = observations[i]; - Frame &t_frame = map.frames_[t_frame_id]; + Frame &t_frame = map.frame(t_frame_id); assert(t_frame.registered); + matrix<3, 3> R = t_frame.Tcw.q.toRotationMatrix(); - vector<3> T = t_frame.Tcw.t; - matrix<3, 4> P; - P << R, T; - - point_data[i].point_normalized = GetPointNormalized( - map.Camera(t_frame.camera_id), t_frame.points[t_p2d_id]); - pose_data[i].proj_matrix = P; - pose_data[i].proj_center = t_frame.center(); + vector<2> p2d_n = GetPointNormalized(map.Camera(t_frame.camera_id), + t_frame.points[t_p2d_id]); + + point_data.at(i) = R.transpose() * (p2d_n.homogeneous()).normalized(); + pose_data.at(i) = t_frame.center(); } - colmap::EstimateTriangulationOptions tri_options; - tri_options.min_tri_angle = DegToRad(1.5); - tri_options.residual_type = - colmap::TriangulationEstimator::ResidualType::ANGULAR_ERROR; - tri_options.ransac_options.max_error = DegToRad(2); - tri_options.ransac_options.confidence = 0.9999; - tri_options.ransac_options.min_inlier_ratio = 0.02; - tri_options.ransac_options.max_num_trials = 10000; - // Enforce exhaustive sampling for small track lengths. - const size_t kExhaustiveSamplingThreshold = 15; + const double min_tri_angle = DegToRad(1.5); + colmap::RANSACOptions ransac_options; + ransac_options.max_error = DegToRad(2); + ransac_options.confidence = 0.9999; + ransac_options.min_inlier_ratio = 0.02; + ransac_options.max_num_trials = 10000; + constexpr size_t kExhaustiveSamplingThreshold = 15; if (point_data.size() <= kExhaustiveSamplingThreshold) { - tri_options.ransac_options.min_num_trials = - NChooseK(point_data.size(), 2); + ransac_options.min_num_trials = NChooseK(point_data.size(), 2); } // make new track; vector<3> p; std::vector inlier_mask; - if (EstimateTriangulation(tri_options, point_data, pose_data, &inlier_mask, - &p)) { + if (EstimatePointLight(ransac_options, min_tri_angle, point_data, pose_data, + inlier_mask, p)) { std::vector> inlier_observations; for (int k = 0; k < observations.size(); ++k) { if (inlier_mask[k]) { @@ -165,9 +132,9 @@ GetMaxAngle(const Map &map, const std::set &observed_track_ids, const int frame_id, const int p2d_id) { int best_track_id = -1; double best_angle_error = 100; - const auto &frame = map.frames_.at(frame_id); + const auto &frame = map.frame(frame_id); for (const auto &track_id : observed_track_ids) { - const auto &track = map.tracks_[track_id]; + const auto &track = map.track(track_id); if (track.observations_.count(frame_id) != 0) continue; vector3 p3d = track.point3d_; @@ -191,7 +158,7 @@ int Point3dProcessor::TriangulateFramePoint(Map &map, const int frame_id, int num_create = 0, num_extend = 0; int num_zero_track = 0, num_one_track = 0, num_multi_track = 0; - Frame &frame = map.frames_[frame_id]; + Frame &frame = map.frame(frame_id); const auto &corrs_vector = map.corr_graph_.frame_node_vec_[frame_id].corrs_vector; assert(frame.track_ids_.size() == corrs_vector.size()); @@ -204,12 +171,12 @@ int Point3dProcessor::TriangulateFramePoint(Map &map, const int frame_id, std::set observed_track_ids; std::vector> observations; for (const auto &[t_frame_id, t_p2d_id] : corrs_vector.at(p2d_id)) { - const auto &cor_frame = map.frames_.at(t_frame_id); + const auto &cor_frame = map.frame(t_frame_id); if (!cor_frame.registered) continue; if (cor_frame.track_ids_.size() <= t_p2d_id) { - std::cout << "bad" << t_frame_id << " " << t_p2d_id - << std::endl; + printf("Error: frame %d index out of bounds %d %d\n", + t_frame_id, t_p2d_id, int(cor_frame.track_ids_.size())); exit(0); } observations.emplace_back(t_frame_id, t_p2d_id); @@ -224,11 +191,9 @@ int Point3dProcessor::TriangulateFramePoint(Map &map, const int frame_id, const int num_track = observed_track_ids.size(); - // std::cout<= 1) { auto [best_track_id, best_angle_error] = @@ -262,7 +227,7 @@ inline void UpdateTrackAngle(const std::vector &frames, Track &track, double max_tri_angle = 0; for (int i = 0; i < centers.size(); ++i) { for (int j = i + 1; j < centers.size(); ++j) { - const double tri_angle = CalculateTriangulationAngle( + const double tri_angle = CalculateTriangulationAngleLight( centers[i], centers[j], track.point3d_); if (tri_angle > max_tri_angle) { max_tri_angle = tri_angle; @@ -284,10 +249,10 @@ inline void FilterPoint3d(const double max_re, const double min_tri_angle_rad, std::vector> obs_to_delete; obs_to_delete.reserve(8); for (auto &[frame_id, p2d_id] : track.observations_) { - const auto &frame = map.frames_[frame_id]; + const auto &frame = map.frame(frame_id); const auto &camera = map.Camera(frame.camera_id); // CHECK(frame.registered); - const double re = Reprojection_Error( + const double re = ReprojectionError( frame.Tcw, camera, frame.points[p2d_id], track.point3d_); const vector3 p3d = frame.Tcw.q * track.point3d_ + frame.Tcw.t; if (re > max_re || p3d.z() < 1e-3 || p3d.z() > 1e3) { @@ -305,7 +270,7 @@ inline void FilterPoint3d(const double max_re, const double min_tri_angle_rad, num_filtered1 += obs_to_delete.size(); for (const auto &[o_frame_id, o_p2d_id] : obs_to_delete) { track.observations_.erase(o_frame_id); - map.frames_[o_frame_id].track_ids_[o_p2d_id] = -1; + map.frame(o_frame_id).track_ids_[o_p2d_id] = -1; map.DeleteNumCorHavePoint3D(o_frame_id, o_p2d_id); } track.error = (reproj_error_sum / track.observations_.size()); @@ -334,13 +299,13 @@ int Point3dProcessor::FilterPoints3d(Map &map, const double max_re, int Point3dProcessor::FilterPointsFrame(Map &map, const int frame_id, const double max_re, const double deg) { const double min_tri_angle_rad = DegToRad(deg); - const auto &frame = map.frames_[frame_id]; + const auto &frame = map.frame(frame_id); int num_filtered1 = 0, num_filtered2 = 0; for (const auto &track_id : frame.track_ids_) { if (track_id == -1) continue; - auto &track = map.tracks_[track_id]; + auto &track = map.track(track_id); FilterPoint3d(max_re, min_tri_angle_rad, map, track, num_filtered1, num_filtered2); } @@ -350,81 +315,26 @@ int Point3dProcessor::FilterPointsFrame(Map &map, const int frame_id, void Point3dProcessor::CheckTrackDepth(const Map &map) { for (size_t i = 0; i < map.tracks_.size(); ++i) { - auto &track = map.tracks_[i]; + auto &track = map.track(i); if (track.outlier) continue; int count = 0; - for (auto &obs_info : track.observations_) { - const auto &frame = map.frames_[obs_info.first]; + for (auto &[frame_id, p2d_id] : track.observations_) { + const auto &frame = map.frame(frame_id); vector3 p_c = frame.Tcw.q * track.point3d_ + frame.Tcw.t; auto &camera = map.Camera(frame.camera_id); - double re = Reprojection_Error(frame.Tcw, camera, - frame.points[obs_info.second], - track.point3d_); + double re = ReprojectionError(frame.Tcw, camera, + frame.points[p2d_id], track.point3d_); if (p_c.z() < 0 || p_c.z() > 100) { - printf("Error:%zu %d %lf %lf\n", i, obs_info.first, p_c.z(), - re); + printf("Error:%zu %d %lf %lf\n", i, frame_id, p_c.z(), re); } } } } -void Point3dProcessor::ReTriangulate(Map &map) { - int count0 = 0, count1 = 0, count2 = 0; - for (size_t i = 0; i < map.tracks_.size(); ++i) { - auto &track = map.tracks_[i]; - if (track.outlier) - continue; - count2++; - - bool has_negative_depth = false; - std::vector> observations(0); - for (auto &[t_frame_id, t_p2d_id] : track.observations_) { - Frame &t_frame = map.frames_[t_frame_id]; - if (!t_frame.registered) - continue; - observations.emplace_back(t_frame_id, t_p2d_id); - vector3 p_c = t_frame.Tcw.q * track.point3d_ + t_frame.Tcw.t; - if (p_c.z() < 0) { - has_negative_depth = true; - // printf("%zu %d %lf\n",i,t_frame_id,p_c.z()); - } - } - // if (!has_negative_depth) continue; - - std::vector> Ps; - std::vector> ps; - // Prepare the data - for (auto &[frame_id, p2d_id] : observations) { - Frame &t_frame = map.frames_[frame_id]; - matrix<3, 3> R = t_frame.Tcw.q.toRotationMatrix(); - vector<3> T = t_frame.Tcw.t; - matrix<3, 4> P; - P << R, T; - - vector<2> p2d_n = GetPointNormalized(map.Camera(t_frame.camera_id), - t_frame.points[p2d_id]); - - Ps.push_back(P); - ps.push_back(p2d_n); - } - - // make new track; - vector<3> p; - std::vector inlier_mask; - if (triangulate_point_checked(Ps, ps, p)) { - track.point3d_ = p; - count0++; - } - count1++; - } - - printf("ReTriangulate %d/%d/%d\n", count0, count1, count2); -} - void Point3dProcessor::ContinueTrack(Map &map, int track_id, double max_re) { - auto &track = map.tracks_[track_id]; + auto &track = map.track(track_id); const auto init_obs = track.observations_; std::set> visted_pair; @@ -434,7 +344,7 @@ void Point3dProcessor::ContinueTrack(Map &map, int track_id, double max_re) { // search connect obs{frame,p2d} if (track.observations_.count(c_frame_id) != 0) continue; - auto &c_frame = map.frames_[c_frame_id]; + auto &c_frame = map.frame(c_frame_id); if (!c_frame.registered) continue; const int track_id2 = c_frame.track_ids_[c_p2d_id]; @@ -446,8 +356,8 @@ void Point3dProcessor::ContinueTrack(Map &map, int track_id, double max_re) { // check rpe const auto &c_camera = map.Camera(c_frame.camera_id); double re = - Reprojection_Error(c_frame.Tcw, c_camera, - c_frame.points[c_p2d_id], track.point3d_); + ReprojectionError(c_frame.Tcw, c_camera, + c_frame.points[c_p2d_id], track.point3d_); if (re > max_re) continue; ContinueTrackUpdate(track_id, c_frame_id, c_p2d_id, map); @@ -456,7 +366,7 @@ void Point3dProcessor::ContinueTrack(Map &map, int track_id, double max_re) { } void Point3dProcessor::MergeTrack(Map &map, int track_id, double max_re) { - auto &track = map.tracks_[track_id]; + auto &track = map.track(track_id); const auto init_obs = track.observations_; // merge track std::set visted_id; @@ -466,14 +376,14 @@ void Point3dProcessor::MergeTrack(Map &map, int track_id, double max_re) { // search connected track if (track.observations_.count(c_frame_id) != 0) continue; - auto &frame = map.frames_[c_frame_id]; + auto &frame = map.frame(c_frame_id); if (!frame.registered) continue; const int track_id2 = frame.track_ids_[c_p2d_id]; if (track_id2 == -1 || visted_id.count(track_id2) > 0) continue; visted_id.insert(track_id2); - auto &track2 = map.tracks_[track_id2]; + auto &track2 = map.track(track_id2); // judge whether to merge bool merge_success = true; @@ -484,11 +394,11 @@ void Point3dProcessor::MergeTrack(Map &map, int track_id, double max_re) { for (const Track *track_ptr : {&track, &track2}) { for (const auto &[t_frame_id, t_p2d_id] : track_ptr->observations_) { - const auto &t_frame = map.frames_[t_frame_id]; + const auto &t_frame = map.frame(t_frame_id); const auto &t_camera = map.Camera(t_frame.camera_id); - double re = Reprojection_Error(t_frame.Tcw, t_camera, - t_frame.points[t_p2d_id], - merged_p3d); + double re = + ReprojectionError(t_frame.Tcw, t_camera, + t_frame.points[t_p2d_id], merged_p3d); if (re > max_re) { merge_success = false; break; @@ -505,9 +415,9 @@ void Point3dProcessor::MergeTrack(Map &map, int track_id, double max_re) { track2.observations_) { if (track.observations_.count(t_frame_id) == 0) { track.observations_[t_frame_id] = t_p2d_id; - map.frames_[t_frame_id].track_ids_[t_p2d_id] = track_id; + map.frame(t_frame_id).track_ids_[t_p2d_id] = track_id; } else { - map.frames_[t_frame_id].track_ids_[t_p2d_id] = -1; + map.frame(t_frame_id).track_ids_[t_p2d_id] = -1; map.DeleteNumCorHavePoint3D(t_frame_id, t_p2d_id); } } @@ -521,10 +431,10 @@ void Point3dProcessor::MergeTracks(Map &map, const int frame_id, double max_re) { int num_merged_track = 0, num_connected_track = 0; int num_continue_track = 0, num_connected_meas = 0; - for (const auto &track_id : map.frames_[frame_id].track_ids_) { + for (const auto &track_id : map.frame(frame_id).track_ids_) { if (track_id == -1) continue; - auto &track = map.tracks_[track_id]; + auto &track = map.track(track_id); // merge track std::set visted_id; for (const auto &[_frame_id, p2d_id] : track.observations_) { @@ -534,7 +444,7 @@ void Point3dProcessor::MergeTracks(Map &map, const int frame_id, // search connected track if (track.observations_.count(c_frame_id) != 0) continue; - auto &frame = map.frames_[c_frame_id]; + auto &frame = map.frame(c_frame_id); if (!frame.registered) continue; const int track_id2 = frame.track_ids_[c_p2d_id]; @@ -542,7 +452,7 @@ void Point3dProcessor::MergeTracks(Map &map, const int frame_id, continue; num_connected_track++; visted_id.insert(track_id2); - auto &track2 = map.tracks_[track_id2]; + auto &track2 = map.track(track_id2); // judge whether to merge const double w1 = track.observations_.size(), @@ -553,11 +463,11 @@ void Point3dProcessor::MergeTracks(Map &map, const int frame_id, for (const Track *track_ptr : {&track, &track2}) { for (const auto &[t_frame_id, t_p2d_id] : track_ptr->observations_) { - const auto &t_frame = map.frames_[t_frame_id]; + const auto &t_frame = map.frame(t_frame_id); const auto &t_camera = map.Camera(t_frame.camera_id); - double re = Reprojection_Error(t_frame.Tcw, t_camera, - t_frame.points[t_p2d_id], - merged_p3d); + double re = ReprojectionError(t_frame.Tcw, t_camera, + t_frame.points[t_p2d_id], + merged_p3d); if (re > max_re) { merge_success = false; break; @@ -572,10 +482,10 @@ void Point3dProcessor::MergeTracks(Map &map, const int frame_id, track2.observations_) { if (track.observations_.count(t_frame_id) == 0) { track.observations_[t_frame_id] = t_p2d_id; - map.frames_[t_frame_id].track_ids_[t_p2d_id] = + map.frame(t_frame_id).track_ids_[t_p2d_id] = track_id; } else { - map.frames_[t_frame_id].track_ids_[t_p2d_id] = -1; + map.frame(t_frame_id).track_ids_[t_p2d_id] = -1; map.DeleteNumCorHavePoint3D(t_frame_id, t_p2d_id); } } @@ -591,7 +501,7 @@ void Point3dProcessor::MergeTracks(Map &map, const int frame_id, .corrs_vector[p2d_id]) { if (track.observations_.count(c_frame_id) != 0) continue; - auto &frame = map.frames_[c_frame_id]; + auto &frame = map.frame(c_frame_id); if (!frame.registered) continue; const int track_id2 = frame.track_ids_[c_p2d_id]; @@ -601,11 +511,11 @@ void Point3dProcessor::MergeTracks(Map &map, const int frame_id, num_connected_meas++; visted_pair.insert(pair); - const auto &t_frame = map.frames_[c_frame_id]; + const auto &t_frame = map.frame(c_frame_id); const auto &t_camera = map.Camera(t_frame.camera_id); - double re = Reprojection_Error(t_frame.Tcw, t_camera, - t_frame.points[c_p2d_id], - track.point3d_); + double re = + ReprojectionError(t_frame.Tcw, t_camera, + t_frame.points[c_p2d_id], track.point3d_); if (re < max_re) { num_continue_track++; ContinueTrackUpdate(track_id, c_frame_id, c_p2d_id, map); @@ -619,7 +529,7 @@ void Point3dProcessor::MergeTracks(Map &map, const int frame_id, bool Point3dProcessor::CheckFrameMeasurement(Map &map, int frame_id) { bool no_mea = true; - auto &frame = map.frames_[frame_id]; + auto &frame = map.frame(frame_id); for (int i = 0; i < frame.track_ids_.size(); ++i) { if (frame.track_ids_[i] == -1) continue; @@ -643,38 +553,7 @@ void Point3dProcessor::CheckFramesMeasurement(Map &map, double th_rpe_lba, FilterPointsFrame(map, frame.id, th_rpe_lba, th_angle_lba); } if (CheckFrameMeasurement(map, frame.id)) { - map.frames_[frame.id].registered = false; - } - } -} - -void Point3dProcessor::ContinueFrameTracks( - const int frame_id, const std::vector> &cor_2d_3d_ids, - Map &map) { - auto &frame = map.frames_[frame_id]; - const auto &camera = map.Camera(frame.camera_id); - for (const auto &[p2d_id, track_id] : cor_2d_3d_ids) { - bool skip = false; - auto &track = map.tracks_[track_id]; - if (track.observations_.count(frame_id) != 0) { - // two 2D points in an image are associated with the same 3D point - const int p2d_id1 = track.observations_[frame_id]; - const double re0 = Reprojection_Error( - frame.Tcw, camera, frame.points[p2d_id], track.point3d_); - const double re1 = Reprojection_Error( - frame.Tcw, camera, frame.points[p2d_id1], track.point3d_); - if (re0 < re1) { - frame.track_ids_[p2d_id1] = -1; - map.DeleteNumCorHavePoint3D(frame_id, p2d_id1); - } else { - skip = true; - } - } - - if (!skip) { - frame.track_ids_[p2d_id] = track_id; - track.observations_[frame_id] = p2d_id; - map.AddNumCorHavePoint3D(frame_id, p2d_id); + map.frame(frame.id).registered = false; } } } @@ -682,41 +561,35 @@ void Point3dProcessor::ContinueFrameTracks( bool CreatePoint3dRAW(const std::vector> &observations, vector3 &p) { size_t data_size = observations.size(); - std::vector point_data( - data_size); - std::vector pose_data(data_size); + + std::vector point_data; + std::vector pose_data; + // Prepare the data for (size_t i = 0; i < data_size; ++i) { const auto &[Tcw, p2d] = observations[i]; matrix<3, 3> R = Tcw.q.toRotationMatrix(); - vector<3> T = Tcw.t; - matrix<3, 4> P; - P << R, T; - - point_data[i].point_normalized = p2d; - pose_data[i].proj_matrix = P; - pose_data[i].proj_center = Tcw.center(); + Eigen::Vector3d ray = R.transpose() * (p2d.homogeneous()).normalized(); + point_data.push_back(ray); + pose_data.push_back(Tcw.center()); } - colmap::EstimateTriangulationOptions tri_options; - tri_options.min_tri_angle = DegToRad(1.5); - tri_options.residual_type = - colmap::TriangulationEstimator::ResidualType::ANGULAR_ERROR; - tri_options.ransac_options.max_error = DegToRad(2); - tri_options.ransac_options.confidence = 0.9999; - tri_options.ransac_options.min_inlier_ratio = 0.02; - tri_options.ransac_options.max_num_trials = 10000; + const double min_tri_angle = DegToRad(1.5); + colmap::RANSACOptions ransac_options; + ransac_options.max_error = DegToRad(2); + ransac_options.confidence = 0.9999; + ransac_options.min_inlier_ratio = 0.02; + ransac_options.max_num_trials = 10000; // Enforce exhaustive sampling for small track lengths. - const size_t kExhaustiveSamplingThreshold = 15; + constexpr size_t kExhaustiveSamplingThreshold = 15; if (point_data.size() <= kExhaustiveSamplingThreshold) { - tri_options.ransac_options.min_num_trials = - NChooseK(point_data.size(), 2); + ransac_options.min_num_trials = NChooseK(point_data.size(), 2); } // make new track; std::vector inlier_mask; - if (EstimateTriangulation(tri_options, point_data, pose_data, &inlier_mask, - &p)) { + if (EstimatePointLight(ransac_options, min_tri_angle, point_data, pose_data, + inlier_mask, p)) { int count = 0; for (int k = 0; k < observations.size(); ++k) { if (inlier_mask[k]) { @@ -728,4 +601,5 @@ bool CreatePoint3dRAW(const std::vector> &observations, } return false; } + } // namespace xrsfm diff --git a/src/geometry/track_processor.h b/src/geometry/track_processor.h index 4ce7d8f..e4a6c14 100644 --- a/src/geometry/track_processor.h +++ b/src/geometry/track_processor.h @@ -8,6 +8,7 @@ #include "base/map.h" namespace xrsfm { + bool CreatePoint3dRAW( const std::vector> &observations, Eigen::Vector3d &p); @@ -23,9 +24,6 @@ class Point3dProcessor { double th_rpe_lba_ = 8, th_angle_lba_ = 2.0; double th_rpe_gba_ = 8, th_angle_gba_ = 2.0; - void ReTriangulate(Map &map); - void UpdateTrackInfo(Map &map); - void CheckTrackDepth(const Map &map); bool CheckFrameMeasurement(Map &map, int frame_id); void CheckFramesMeasurement(Map &map, double th_rpe_lba, @@ -34,10 +32,6 @@ class Point3dProcessor { void MergeTrack(Map &map, int track_id, double max_re); void MergeTracks(Map &map, const int frame_id, double max_re); void ContinueTrack(Map &map, int track_id, double max_re); - void - ContinueFrameTracks(const int frame_id, - const std::vector> &cor_2d_3d_ids, - Map &map); int TriangulateFramePoint(Map &map, const int frame_id, const double deg_th); @@ -46,6 +40,7 @@ class Point3dProcessor { int FilterPointsFrame(Map &map, const int frame_id, const double max_re, const double deg); }; + } // namespace xrsfm #endif // XRSFM_SRC_GEOMETRY_TRACK_PROCESSOR_H diff --git a/src/geometry/triangluate_svd.cc b/src/geometry/triangluate_svd.cc deleted file mode 100644 index af4bf91..0000000 --- a/src/geometry/triangluate_svd.cc +++ /dev/null @@ -1,74 +0,0 @@ -// -// Created by SENSETIME\yezhichao1 on 2020/12/30. -// - -#include "triangluate_svd.h" - -namespace xrsfm { -double CalculateTriangulationAngle(const Eigen::Vector3d ¢er1, - const Eigen::Vector3d ¢er2, - const Eigen::Vector3d &point3D) { - const double baseline_length_squared = (center1 - center2).squaredNorm(); - - const double ray_length_squared1 = (point3D - center1).squaredNorm(); - const double ray_length_squared2 = (point3D - center2).squaredNorm(); - - // Using "law of cosines" to compute the enclosing angle between rays. - const double denominator = - 2.0 * std::sqrt(ray_length_squared1 * ray_length_squared2); - if (denominator == 0.0) { - return 0.0; - } - const double nominator = - ray_length_squared1 + ray_length_squared2 - baseline_length_squared; - const double angle = std::abs(std::acos(nominator / denominator)); - - // Triangulation is unstable for acute angles (far away points) and - // obtuse angles (close points), so always compute the minimum angle - // between the two intersecting rays. - return std::min(angle, M_PI - angle); -} - -xrsfm::vector<4> -triangulate_point(const std::vector> &Ps, - const std::vector> &points) { - xrsfm::matrix A(points.size() * 2, 4); - for (size_t i = 0; i < points.size(); ++i) { - A.row(i * 2 + 0) = points[i](0) * Ps[i].row(2) - Ps[i].row(0); - A.row(i * 2 + 1) = points[i](1) * Ps[i].row(2) - Ps[i].row(1); - } - return A.jacobiSvd(Eigen::ComputeFullV).matrixV().col(3); -} - -bool triangulate_point_scored(const std::vector> &Ps, - const std::vector> &points, - xrsfm::vector<3> &p, double &score) { - if (Ps.size() < 2) - return false; - xrsfm::vector<4> q = triangulate_point(Ps, points); - score = 0; - for (size_t i = 0; i < points.size(); ++i) { - xrsfm::vector<3> qi = Ps[i] * q; - if (!(qi[2] * q[3] > 0)) { // z<0 - return false; - } - if (!(qi[2] / q[3] < 100)) { // z>100 - return false; - } - score += (qi.hnormalized() - points[i]).squaredNorm(); - // std::cout << (qi.hnormalized() - points[i]).norm() * 700 << " - // "; - } - // std::cout << std::endl; - score /= points.size(); - p = q.hnormalized(); - return true; -} - -bool triangulate_point_checked(const std::vector> &Ps, - const std::vector> &points, - xrsfm::vector<3> &p) { - double score; - return triangulate_point_scored(Ps, points, p, score); -} -} // namespace xrsfm diff --git a/src/geometry/triangluate_svd.h b/src/geometry/triangluate_svd.h deleted file mode 100644 index 5434e5f..0000000 --- a/src/geometry/triangluate_svd.h +++ /dev/null @@ -1,31 +0,0 @@ -// -// Created by SENSETIME\yezhichao1 on 2020/12/30. -// - -#ifndef XRSFM_SRC_GEOMETRY_TRIANGLUATE_SVD_H -#define XRSFM_SRC_GEOMETRY_TRIANGLUATE_SVD_H - -#include "utility/global.h" - -namespace xrsfm { -inline double DegToRad(const double deg) { - return deg * 0.0174532925199432954743716805978692718781530857086181640625; -} - -inline size_t NChooseK(const size_t n, const size_t k) { - if (k == 0) { - return 1; - } - return (n * NChooseK(n - 1, k - 1)) / k; -} - -bool triangulate_point_checked(const std::vector> &Ps, - const std::vector> &points, - xrsfm::vector<3> &p); - -double CalculateTriangulationAngle(const Eigen::Vector3d ¢er1, - const Eigen::Vector3d ¢er2, - const Eigen::Vector3d &point3D); -} // namespace xrsfm - -#endif // XRSFM_SRC_GEOMETRY_TRIANGLUATE_SVD_H diff --git a/src/geometry/triangulate_light.cc b/src/geometry/triangulate_light.cc new file mode 100755 index 0000000..825ab39 --- /dev/null +++ b/src/geometry/triangulate_light.cc @@ -0,0 +1,137 @@ +// Copyright (c) 2019, SenseTime Group. +// All rights reserved. + +#include "triangulate_light.h" + +#include +#include + +#include "ransac/combination_sampler.h" +#include "ransac/loransac.h" + +namespace xrsfm { + +inline Eigen::Matrix3d hat(const Eigen::Vector3d &vector) { + Eigen::Matrix3d matrix; + matrix << 0, -vector(2), vector(1), vector(2), 0, -vector(0), -vector(1), + vector(0), 0; + return matrix; +} + +inline Eigen::Vector3d +MultiPointEstimate(const std::vector &points, + const std::vector &poses) { + Eigen::Matrix3d A = Eigen::Matrix3d::Zero(); + Eigen::Vector3d b = Eigen::Vector3d::Zero(); + + for (int i = 0; i < points.size(); ++i) { + Eigen::Matrix3d J = hat(points.at(i)); + Eigen::Vector3d r = (points.at(i)).cross(poses.at(i)); + A += J.transpose() * J; + b += J.transpose() * r; + } + + Eigen::LLT llt = A.llt(); + return llt.solve(b); +} + +std::vector +TriLightEstimator::Estimate(const std::vector &points, + const std::vector &poses) const { + CHECK_GE(points.size(), 2); + CHECK_EQ(points.size(), poses.size()); + const double max_cos_theta = std::cos(min_tri_angle_); + + const M_t xyz = MultiPointEstimate(points, poses); + + for (int i = 0; i < points.size(); ++i) { + if (points.at(i).dot(xyz - poses.at(i)) < 0) + return std::vector(); + } + + for (size_t i = 0; i < points.size() - 1; ++i) { + const Eigen::Vector3d ray_i = (xyz - poses.at(i)).normalized(); + for (size_t j = i + 1; j < points.size(); ++j) { + const Eigen::Vector3d ray_j = (xyz - poses.at(j)).normalized(); + const double cos_theta = ray_i.dot(ray_j); + if (cos_theta < max_cos_theta) { + return std::vector{xyz}; + } + } + } + + return std::vector(); +} + +inline double caculate_ray_error(const Eigen::Vector3d &ray1, + const Eigen::Vector3d &ray2) { + double cos_theta = ray1.dot(ray2); + cos_theta = std::acos(std::max(-1.0, std::min(1.0, cos_theta))); + return cos_theta; +} + +void TriLightEstimator::Residuals(const std::vector &points, + const std::vector &poses, + const Eigen::Vector3d &xyz, + std::vector *residuals) const { + residuals->resize(points.size()); + + for (size_t i = 0; i < points.size(); ++i) { + const Eigen::Vector3d ray1 = points.at(i); + const Eigen::Vector3d ray2 = (xyz - poses.at(i)).normalized(); + const double angular_error = caculate_ray_error(ray1, ray2); + residuals->at(i) = angular_error * angular_error; + } +} + +bool EstimatePointLight(const colmap::RANSACOptions &ransac_options, + const double min_tri_angle, + const std::vector &point_data, + const std::vector &pose_data, + std::vector &inlier_mask, Eigen::Vector3d &xyz) { + CHECK_GE(point_data.size(), 2); + CHECK_EQ(point_data.size(), pose_data.size()); + + colmap::LORANSAC + ransac(ransac_options); + ransac.estimator.min_tri_angle_ = ransac.local_estimator.min_tri_angle_ = + min_tri_angle; + const auto report = ransac.Estimate(point_data, pose_data); + + if (!report.success) { + return false; + } + + inlier_mask = report.inlier_mask; + xyz = report.model; + + return report.success; +} + +double CalculateTriangulationAngleLight(const Eigen::Vector3d &view_point0, + const Eigen::Vector3d &view_point1, + const Eigen::Vector3d &point3d) { + Eigen::Vector3d ray0 = point3d - view_point0; + Eigen::Vector3d ray1 = point3d - view_point1; + if (ray0.norm() == 0 || ray1.norm() == 0) + return 0; + ray0 = ray0.normalized(); + ray1 = ray1.normalized(); + const double cos_theta = ray0.dot(ray1); + return std::acos(cos_theta); +} + +std::vector CalculateTriangulationAnglesLight( + const Eigen::Vector3d &view_point0, const Eigen::Vector3d &view_point1, + const std::vector &points3d) { + const int num_points = points3d.size(); + std::vector angles(num_points); + for (int i = 0; i < num_points; ++i) { + angles.at(i) = CalculateTriangulationAngleLight( + view_point0, view_point1, points3d.at(i)); + } + return angles; +} + +} // namespace xrsfm diff --git a/src/geometry/triangulate_light.h b/src/geometry/triangulate_light.h new file mode 100755 index 0000000..2ff1a94 --- /dev/null +++ b/src/geometry/triangulate_light.h @@ -0,0 +1,53 @@ +// Copyright (c) 2019, SenseTime Group. +// All rights reserved. + +#ifndef SENSEMAP_ESTIMATORS_TRIANGULATION_LIGHT_H_ +#define SENSEMAP_ESTIMATORS_TRIANGULATION_LIGHT_H_ + +#include +#include + +#include "ransac/ransac.h" + +namespace xrsfm { + +inline double DegToRad(const double deg) { + return deg * 0.0174532925199432954743716805978692718781530857086181640625; +} + +class TriLightEstimator { + public: + typedef Eigen::Vector3d X_t; // ray + typedef Eigen::Vector3d Y_t; // camera position + typedef Eigen::Vector3d M_t; // point position + + // The minimum number of samples needed to estimate a model. + static constexpr int kMinNumSamples = 2; + + std::vector Estimate(const std::vector &point_data, + const std::vector &pose_data) const; + void Residuals(const std::vector &point_data, + const std::vector &pose_data, const M_t &xyz, + std::vector *residuals) const; + + double min_tri_angle_ = 0.0; +}; + +bool EstimatePointLight(const colmap::RANSACOptions &ransac_options, + const double min_tri_angle, + const std::vector &point_data, + const std::vector &pose_data, + std::vector &inlier_mask, Eigen::Vector3d &xyz); + +double CalculateTriangulationAngleLight(const Eigen::Vector3d &view_point0, + const Eigen::Vector3d &view_point1, + const Eigen::Vector3d &point3d); + +std::vector +CalculateTriangulationAnglesLight(const Eigen::Vector3d &view_point0, + const Eigen::Vector3d &view_point1, + const std::vector &points3d); + +} // namespace xrsfm + +#endif // SENSEMAP_ESTIMATORS_TRIANGULATION_LIGHT_H_ diff --git a/src/mapper/incremental_mapper.cc b/src/mapper/incremental_mapper.cc index d5194d8..7863b94 100644 --- a/src/mapper/incremental_mapper.cc +++ b/src/mapper/incremental_mapper.cc @@ -14,13 +14,13 @@ void IncrementalMapper::Reconstruct(Map &map) { // 1. Map Initialization FramePair init_frame_pair; if (options.init_id1 != -1 && options.init_id2 != -1) { - std::cout << "Init with Given Frames " << options.init_id1 << " " - << options.init_id2 << std::endl; + printf("Init with Given Frames %d %d\n", options.init_id1, + options.init_id2); init_frame_pair = FindPair(map.frame_pairs_, options.init_id1, options.init_id2); } else { - std::cout << "Found Init Frame Pair " << options.init_id1 << " " - << options.init_id2 << std::endl; + printf("Found Init Frame Pair %d %d\n", options.init_id1, + options.init_id2); init_frame_pair.id1 = options.init_id1; init_frame_pair.id2 = options.init_id2; if (!FindInitFramePair(map, init_frame_pair)) { @@ -34,7 +34,7 @@ void IncrementalMapper::Reconstruct(Map &map) { // 2. Map Iterative Extension int num_image_reg = 2, num_image_reg_pre = 2; - for (int iter = 0; iter < map.frames_.size(); iter++) { + for (int iter = 0; iter < map.NumFrames(); iter++) { printf("-----------------------------------------------\n"); // 1) Pose Estimation timer.reg.resume(); @@ -42,14 +42,13 @@ void IncrementalMapper::Reconstruct(Map &map) { if (frame_id == -1) break; printf("Iter %d %d %s\n", iter, frame_id, - map.frames_[frame_id].name.c_str()); + map.frame(frame_id).name.c_str()); if (!RegisterImage(frame_id, map)) { if (options.stop_when_register_fail) break; - map.frames_[frame_id].registered_fail = true; + map.frame(frame_id).registered_fail = true; continue; } - map.current_frame_id_ = frame_id; timer.reg.stop(); // 2) Check & Correct Frame Pose diff --git a/src/optimization/ba_solver.cc b/src/optimization/ba_solver.cc index 0af2f56..5bfaf8d 100644 --- a/src/optimization/ba_solver.cc +++ b/src/optimization/ba_solver.cc @@ -5,12 +5,29 @@ #include "ba_solver.h" #include "cost_factor_ceres.h" -#include "geometry/colmap/base/triangulation.h" -#include "geometry/colmap/util/math.h" #include "utility/timer.h" +#include "geometry/triangulate_light.h" namespace xrsfm { +template +T Percentile(const std::vector &elems, const double p) { + CHECK(!elems.empty()); + CHECK_GE(p, 0); + CHECK_LE(p, 100); + + const int idx = static_cast(std::round(p / 100 * (elems.size() - 1))); + const size_t percentile_idx = + std::max(0, std::min(static_cast(elems.size() - 1), idx)); + + std::vector ordered_elems = elems; + std::nth_element(ordered_elems.begin(), + ordered_elems.begin() + percentile_idx, + ordered_elems.end()); + + return ordered_elems.at(percentile_idx); +} + void PrintSolverSummary(const ceres::Solver::Summary &summary) { std::cout << std::right << std::setw(16) << "Residuals : "; std::cout << std::left << summary.num_residuals_reduced << std::endl; @@ -80,7 +97,7 @@ void AddCovisibilityEdge(ceres::Problem &problem, Map &map, std::vector &s_vec, std::vector &twc_vec, std::vector &num_cov, double weight_o, bool use_key) { - num_cov.assign(map.frames_.size(), 0); + num_cov.assign(map.NumFrames(), 0); for (auto &frame : map.frames_) { if (!frame.registered) continue; @@ -95,7 +112,7 @@ void AddCovisibilityEdge(ceres::Problem &problem, Map &map, // assert every key frame have at least a key frame neighbor if (frame.id <= cor_id) continue; - if (use_key && !map.frames_[cor_id].is_keyframe) + if (use_key && !map.frame(cor_id).is_keyframe) continue; auto &s2 = s_vec[cor_id]; @@ -124,7 +141,7 @@ void AddLoopEdge(ceres::Problem &problem, Map &map, const LoopInfo &loop_info, auto &pose1_mea = loop_info.twc_vec[i]; int count = 0; for (const auto &cor_id : loop_info.cor_frame_ids_vec[i]) { - if (use_key && !map.frames_[cor_id].is_keyframe) + if (use_key && !map.frame(cor_id).is_keyframe) continue; auto &s2 = s_vec[cor_id]; auto &pose2 = twc_vec[cor_id]; @@ -156,7 +173,7 @@ void BASolver::ScalePoseGraphUnorder(const LoopInfo &loop_info, Map &map, for (const auto &[frame_id, p2d_id] : track.observations_) { if (frame_id == loop_info.frame_id) continue; - const auto &frame = map.frames_[frame_id]; + const auto &frame = map.frame(frame_id); const double depth = (frame.Tcw.q * track.point3d_ + frame.Tcw.t).z(); if (best_frame_id == -1) { @@ -180,10 +197,10 @@ void BASolver::ScalePoseGraphUnorder(const LoopInfo &loop_info, Map &map, if (track.depth < 0) { std::cout << "!!! negative depth\n"; const auto &it = track.observations_.begin(); - const int track_id = map.frames_[it->first].track_ids_[it->second]; + const int track_id = map.frame(it->first).track_ids_[it->second]; printf("-%d %d %lf\n", track_id, track.ref_id, track.depth); for (const auto &[frame_id, p2d_id] : track.observations_) { - const auto &frame = map.frames_[frame_id]; + const auto &frame = map.frame(frame_id); const double depth = (frame.Tcw.q * track.point3d_ + frame.Tcw.t).z(); printf("%d %d %lf\n", track_id, frame_id, depth); @@ -194,7 +211,7 @@ void BASolver::ScalePoseGraphUnorder(const LoopInfo &loop_info, Map &map, } // prepare data - size_t num_frames = map.frames_.size(); + size_t num_frames = map.NumFrames(); std::vector twc_vec(num_frames); std::vector s_vec(twc_vec.size(), 1); std::vector s_vec_loop(loop_info.cor_frame_ids_vec.size(), 1); @@ -272,7 +289,7 @@ void BASolver::ScalePoseGraphUnorder(const LoopInfo &loop_info, Map &map, std::cout << i << " " << s_vec[i] << std::endl; } for (size_t i = 0; i < num_frames; ++i) { - map.frames_[i].Tcw = twc_vec[i].inverse(); + map.frame(i).Tcw = twc_vec[i].inverse(); } } else { int num_keyframe = 0; @@ -282,7 +299,7 @@ void BASolver::ScalePoseGraphUnorder(const LoopInfo &loop_info, Map &map, } int count = 0; for (size_t i = 0; i < num_frames; ++i) { - auto &frame = map.frames_[i]; + auto &frame = map.frame(i); if (frame.registered && frame.is_keyframe) { if (count % (num_keyframe / 10) == 0 || s_vec[i] < 0) std::cout << i << " " << s_vec[i] << std::endl; @@ -291,16 +308,16 @@ void BASolver::ScalePoseGraphUnorder(const LoopInfo &loop_info, Map &map, } for (size_t i = 0; i < num_frames; ++i) { - auto &frame = map.frames_[i]; + auto &frame = map.frame(i); if (frame.registered && frame.is_keyframe) { frame.tcw_old = frame.Tcw; frame.Tcw = twc_vec[i].inverse(); } } for (size_t i = 0; i < num_frames; ++i) { - auto &frame = map.frames_[i]; + auto &frame = map.frame(i); if (frame.registered && !frame.is_keyframe) { - const auto &ref_frame = map.frames_[frame.ref_id]; + const auto &ref_frame = map.frame(frame.ref_id); CHECK(ref_frame.is_keyframe); CHECK(ref_frame.id != loop_info.frame_id); s_vec[i] = s_vec[frame.ref_id]; @@ -318,7 +335,7 @@ void BASolver::ScalePoseGraphUnorder(const LoopInfo &loop_info, Map &map, continue; int frame_id = track.ref_id; int p2d_id = track.observations_[frame_id]; - Pose tcw = map.frames_[frame_id].Tcw; + Pose tcw = map.frame(frame_id).Tcw; vector2 p2d = map.GetNormalizedPoint(frame_id, p2d_id); track.point3d_ = @@ -336,7 +353,7 @@ inline void BASolver::SetUp(ceres::Problem &problem, Map &map, Frame &frame) { for (int i = 0; i < frame.track_ids_.size(); ++i) { if (frame.track_ids_[i] == -1) continue; - Track &track = map.tracks_[frame.track_ids_[i]]; + Track &track = map.track(frame.track_ids_[i]); ceres::CostFunction *cost_function = ReProjectionCostCreate(camera_model_id, frame.points[i]); @@ -367,7 +384,7 @@ inline void BASolver::SetUpLBA(ceres::Problem &problem, Map &map, Frame &frame, continue; num_mea++; - Track &track = map.tracks_[frame.track_ids_[i]]; + Track &track = map.track(frame.track_ids_[i]); ceres::CostFunction *cost_function = ReProjectionCostCreate(camera_model_id, frame.points[i]); @@ -392,13 +409,13 @@ inline void BASolver::SetUpLBA(ceres::Problem &problem, Map &map, Frame &frame, std::vector FindLocalBundle(const int frame_id, Map &map, const size_t num_images = 4) { - Frame &frame = map.frames_[frame_id]; + Frame &frame = map.frame(frame_id); int num_p3d = 0; std::unordered_map covisiblity; for (auto &track_id : frame.track_ids_) { if (track_id == -1) continue; - const auto &track = map.tracks_[track_id]; + const auto &track = map.track(track_id); num_p3d++; for (const auto &[t_frame_id, t_p2d_id] : track.observations_) { if (t_frame_id != frame.id) { @@ -448,7 +465,7 @@ std::vector FindLocalBundle(const int frame_id, Map &map, if (used_overlapping_images[frame_id]) continue; - const auto &frame_overlap = map.frames_[cov_vec[frame_id].first]; + const auto &frame_overlap = map.frame(cov_vec[frame_id].first); const vector3 proj_center_overlap = frame_overlap.Tcw.center(); // In the first iteration, compute the triangulation angle. In later @@ -460,14 +477,14 @@ std::vector FindLocalBundle(const int frame_id, Map &map, for (auto &track_id : frame.track_ids_) { if (track_id == -1) continue; - const auto &track = map.tracks_[track_id]; + const auto &track = map.track(track_id); shared_points3D.push_back(track.point3d_); } // Calculate the triangulation angle at a certain percentile. const double kTriangulationAnglePercentile = 75; - tri_angle = colmap::Percentile( - colmap::CalculateTriangulationAngles( + tri_angle = Percentile( + CalculateTriangulationAnglesLight( proj_center, proj_center_overlap, shared_points3D), kTriangulationAnglePercentile); } @@ -494,12 +511,12 @@ std::vector FindLocalBundle(const int frame_id, Map &map, std::vector CovisibilityNeibors(const int frame_id, Map &map, const size_t num_images = 4) { - Frame &frame = map.frames_[frame_id]; + Frame &frame = map.frame(frame_id); std::unordered_map covisiblity; for (auto &track_id : frame.track_ids_) { if (track_id == -1) continue; - const auto &track = map.tracks_[track_id]; + const auto &track = map.track(track_id); for (const auto &[t_frame_id, t_p2d_id] : track.observations_) { covisiblity[t_frame_id] += 1; } @@ -538,7 +555,7 @@ void BASolver::LBA(int frame_id, Map &map) { std::set fixed_camera_ids; for (const int &id : local_frame_ids) { printf(" %d", id); - auto &frame = map.frames_.at(id); + auto &frame = map.frame(id); SetUpLBA(problem, map, frame, frame_id); if (fixed_camera_ids.count(frame.camera_id) == 0) { fixed_camera_ids.insert(frame.camera_id); @@ -551,35 +568,32 @@ void BASolver::LBA(int frame_id, Map &map) { // fix poses int fix_num = 0; if (local_frame_ids.count(map.init_id1) != 0) { - problem.SetParameterBlockConstant( - map.frames_[map.init_id1].Tcw.t.data()); + problem.SetParameterBlockConstant(map.frame(map.init_id1).Tcw.t.data()); fix_num++; } if (local_frame_ids.count(map.init_id2) != 0) { - problem.SetParameterBlockConstant( - map.frames_[map.init_id2].Tcw.t.data()); + problem.SetParameterBlockConstant(map.frame(map.init_id2).Tcw.t.data()); fix_num++; } if (fix_num == 0) { if (local_frame_ids2.size() >= 2) { problem.SetParameterBlockConstant( - map.frames_[local_frame_ids2[local_frame_ids2.size() - 1]] + map.frame(local_frame_ids2[local_frame_ids2.size() - 1]) .Tcw.t.data()); problem.SetParameterBlockConstant( - map.frames_[local_frame_ids2[local_frame_ids2.size() - 2]] + map.frame(local_frame_ids2[local_frame_ids2.size() - 2]) .Tcw.t.data()); } else if (local_frame_ids1.size() >= 2) { problem.SetParameterBlockConstant( - map.frames_[local_frame_ids1[local_frame_ids1.size() - 1]] + map.frame(local_frame_ids1[local_frame_ids1.size() - 1]) .Tcw.t.data()); problem.SetParameterBlockConstant( - map.frames_[local_frame_ids1[local_frame_ids1.size() - 2]] + map.frame(local_frame_ids1[local_frame_ids1.size() - 2]) .Tcw.t.data()); } else { printf("!!!LBA only one frame\n"); - problem.SetParameterBlockConstant( - map.frames_[frame_id].Tcw.t.data()); + problem.SetParameterBlockConstant(map.frame(frame_id).Tcw.t.data()); } } @@ -608,10 +622,8 @@ void BASolver::GBA(Map &map, bool accurate, bool fix_all_frames) { // fix poses if (!fix_all_frames) { - problem.SetParameterBlockConstant( - map.frames_[map.init_id1].Tcw.t.data()); - problem.SetParameterBlockConstant( - map.frames_[map.init_id2].Tcw.t.data()); + problem.SetParameterBlockConstant(map.frame(map.init_id1).Tcw.t.data()); + problem.SetParameterBlockConstant(map.frame(map.init_id2).Tcw.t.data()); } else { for (auto &frame : map.frames_) { if (!frame.registered) @@ -659,8 +671,8 @@ void BASolver::KGBA(Map &map, const std::vector fix_key_frame_ids, } } - problem.SetParameterBlockConstant(map.frames_[map.init_id1].Tcw.t.data()); - problem.SetParameterBlockConstant(map.frames_[map.init_id2].Tcw.t.data()); + problem.SetParameterBlockConstant(map.frame(map.init_id1).Tcw.t.data()); + problem.SetParameterBlockConstant(map.frame(map.init_id2).Tcw.t.data()); ceres::Solver::Options solver_options = InitSolverOptions(); solver_options.minimizer_progress_to_stdout = true; @@ -670,7 +682,6 @@ void BASolver::KGBA(Map &map, const std::vector fix_key_frame_ids, solver_options.parameter_tolerance = 1e-5; ceres::Solver::Summary summary; ceres::Solve(solver_options, &problem, &summary); - // std::cout << summary.BriefReport() << "\n"; PrintSolverSummary(summary); printf("kf: %d/%d\n", num_kf, num_rf); diff --git a/src/optimization/cost_factor_ceres.h b/src/optimization/cost_factor_ceres.h index 9b58085..48cac46 100644 --- a/src/optimization/cost_factor_ceres.h +++ b/src/optimization/cost_factor_ceres.h @@ -127,7 +127,8 @@ class PoseGraphCost : public ceres::SizedCostFunction<8, 4, 3, 4, 3, 1, 1> { vector3 p2(parameters[3]); double s1 = parameters[4][0]; double s2 = parameters[5][0]; - map> residual(residuals); + map> res_q(residuals); + map> res_p(residuals + 5); quaternion q1_inverse = q1.inverse(); quaternion q12_estimated = q1_inverse * q2; @@ -135,24 +136,23 @@ class PoseGraphCost : public ceres::SizedCostFunction<8, 4, 3, 4, 3, 1, 1> { double weight_q = 1.0; double weight_p = 1.0; - residual.head<3>() = weight_q * logmap(q_mea * q12_estimated.inverse()); - residual.tail<3>() = weight_p * (p12_estimated - s1 * p_mea); + res_q = weight_q * logmap(q_mea * q12_estimated.inverse()); + res_p = weight_p * (p12_estimated - s1 * p_mea); // 1.0 00 seq be better // 0.5 02 seq be better double weight_s = 1.0; - residual(3) = weight_s * (s1 / s2 - 1); + residuals[3] = weight_s * (s1 / s2 - 1); - // double weight_o = 0.1; // 0.1 bad in seq_4761_478 if (s1 < 1) { - residual(4) = weight_o * (s1 - 1); + residuals[4] = weight_o * (s1 - 1); } else { - residual(4) = weight_o * (1.0 / s1 - 1); + residuals[4] = weight_o * (1.0 / s1 - 1); } if (jacobians) { matrix3 R1 = q1.toRotationMatrix(); - matrix3 JRI = jri(residual.head<3>()); + matrix3 JRI = jri(res_q); if (jacobians[0]) { map> j_q1(jacobians[0]); j_q1.setZero(); diff --git a/src/rec_1dsfm.cc b/src/rec_1dsfm.cc index 79261c9..de6f19a 100644 --- a/src/rec_1dsfm.cc +++ b/src/rec_1dsfm.cc @@ -7,7 +7,6 @@ #include "utility/io_ecim.hpp" #include "utility/io_feature.hpp" #include "utility/timer.h" -// #include "utility/viewer.h" using namespace xrsfm; @@ -31,7 +30,6 @@ void PreProcess(const std::string bin_path, Map &map) { const auto &camera = cameras.at(frame.camera_id); const int num_points = frame.keypoints_.size(); frame.points.clear(); - // frame.points_normalized.clear(); frame.uint_descs_.resize(0, 0); frame.track_ids_.assign(num_points, -1); for (const auto &kpt : frame.keypoints_) { @@ -39,12 +37,10 @@ void PreProcess(const std::string bin_path, Map &map) { Eigen::Vector2d ept(pt.x, pt.y), eptn; ImageToNormalized(camera, ept, eptn); frame.points.emplace_back(ept); - // frame.points_normalized.emplace_back(eptn); } } for (int i = 0; i < cameras.size(); ++i) { - std::cout << i << " " << cameras.size() << std::endl; auto &camera = cameras[i]; // camera.log(); // if distortion parameters of the camera are not estimated, the camera @@ -59,7 +55,7 @@ void PreProcess(const std::string bin_path, Map &map) { map.frame_pairs_ = frame_pairs; map.RemoveRedundancyPoints(); map.Init(); - printf("Num Frames: %d Num Pairs %d\n", map.frames_.size(), + printf("Num Frames: %d Num Pairs %d\n", map.NumFrames(), map.frame_pairs_.size()); } diff --git a/src/rec_kitti.cc b/src/rec_kitti.cc index de5f730..b3f7bc9 100644 --- a/src/rec_kitti.cc +++ b/src/rec_kitti.cc @@ -7,7 +7,6 @@ #include "utility/io_ecim.hpp" #include "utility/io_feature.hpp" #include "utility/timer.h" -// #include "utility/viewer.h" using namespace xrsfm; @@ -22,11 +21,11 @@ void PreProcess(const std::string dir_path, const int camera_param_id, // set cameras Camera seq; if (camera_param_id == 0) { - seq = Camera(0, 718.856, 718.856, 607.1928, 185.27157, 0.0); // 00-02 + seq = Camera(0, 718.856, 607.1928, 185.27157); // 00-02 } else if (camera_param_id == 1) { - seq = Camera(0, 721.5377, 721.5377, 609.5593, 172.854, 0.0); // 03 + seq = Camera(0, 721.5377, 609.5593, 172.854); // 03 } else if (camera_param_id == 2) { - seq = Camera(0, 707.0912, 707.0912, 601.8873, 183.1104, 0.0); // 04-12 + seq = Camera(0, 707.0912, 601.8873, 183.1104); // 04-12 } cameras[seq.id_] = seq; for (auto &frame : frames) { @@ -37,14 +36,10 @@ void PreProcess(const std::string dir_path, const int camera_param_id, for (auto &frame : frames) { const int num_points = frame.keypoints_.size(); frame.points.clear(); - // frame.points_normalized.clear(); frame.track_ids_.assign(num_points, -1); for (const auto &kpt : frame.keypoints_) { const auto &pt = kpt.pt; - Eigen::Vector2d ept(pt.x, pt.y), eptn; - // ImageToNormalized(cameras[0], ept, eptn); - frame.points.emplace_back(ept); - // frame.points_normalized.emplace_back(eptn); + frame.points.emplace_back(vector2(pt.x, pt.y)); } } diff --git a/src/run_matching.cc b/src/run_matching.cc index 86494ef..53c2e95 100644 --- a/src/run_matching.cc +++ b/src/run_matching.cc @@ -6,153 +6,14 @@ #include #include "base/map.h" -#include "feature/feature_processing.h" +#include "feature/feature_processor.h" #include "utility/io_ecim.hpp" #include "utility/timer.h" -using namespace xrsfm; - -void GetFeatures(const std::string &images_path, - const std::vector &image_names, - const std::string &ftr_path, std::vector &frames) { - const int num_image = image_names.size(); - frames.resize(num_image); - for (int i = 0; i < num_image; ++i) { - frames[i].id = i; - frames[i].name = image_names[i]; - } - - std::ifstream ftr_bin(ftr_path); - if (ftr_bin.good()) { - ReadFeatures(ftr_path, frames); - SetUpFramePoints(frames); - } else { - FeatureExtract(images_path, frames); - SaveFeatures(ftr_path, frames, true); - } -} - -void GetImageSizeVec(const std::string &images_path, - const std::vector &image_names, - const std::string &path, - std::vector &image_size) { - std::ifstream bin(path); - if (bin.good()) { - LoadImageSize(path, image_size); - } else { - for (const auto &image_name : image_names) { - const cv::Mat image = cv::imread(images_path + image_name, - cv::IMREAD_IGNORE_ORIENTATION); - image_size.push_back(ImageSize(image.cols, image.rows)); - } - SaveImageSize(path, image_size); - } -} - -void GetInitFramePairs(const std::string &path, - const std::vector &frames, - const std::vector> id_pairs, - std::vector &frame_pairs) { - std::ifstream bin(path); - if (bin.good()) { - ReadFramePairs(path, frame_pairs); - } else { - FeatureMatching(frames, id_pairs, frame_pairs, true); - SaveFramePairs(path, frame_pairs); - } -} - -inline void -ExtractNearestImagePairs(const std::map> &id2rank, - const int num_candidates, - std::vector> &image_id_pairs) { - std::set> image_pair_set; - // search nv5 in candidates except for nearest images - for (const auto &[id, retrieval_results] : id2rank) { - int count = 0; - for (const auto &id2 : retrieval_results) { - auto ret = image_pair_set.insert( - std::make_pair(std::min(id, id2), std::max(id, id2))); - count++; - if (count >= num_candidates) - break; - } - } - image_id_pairs.insert(image_id_pairs.end(), image_pair_set.begin(), - image_pair_set.end()); - std::sort(image_id_pairs.begin(), image_id_pairs.end(), - [](auto &a, auto &b) { - if (a.first < b.first || - (a.first == b.first && a.second < b.second)) - return true; - return false; - }); -} - -std::tuple GetInitId(const int num_image, - std::vector &frame_pairs) { - std::vector> id2cor_num_vec(num_image); - std::vector> connect_number_vec(num_image); - std::map> connect_id_vec; - for (int i = 0; i < num_image; ++i) { - connect_number_vec[i] = {i, 0}; - } - - for (auto &fp : frame_pairs) { - if (fp.inlier_num < 100) - continue; // for Trafalgar - connect_number_vec[fp.id1].second++; - connect_number_vec[fp.id2].second++; - id2cor_num_vec[fp.id1][fp.id2] = fp.matches.size(); - id2cor_num_vec[fp.id2][fp.id1] = fp.matches.size(); - } - std::sort(connect_number_vec.begin(), connect_number_vec.end(), - [](const std::pair &a, const std::pair &b) { - return a.second > b.second; - }); - const int init_id1 = connect_number_vec[0].first; - int init_id2 = -1; - for (auto &[id, number] : connect_number_vec) { - if (id2cor_num_vec[init_id1].count(id) != 0 && - id2cor_num_vec[init_id1][id] >= 100) { - init_id2 = id; - break; - } - } - return std::tuple(init_id1, init_id2); -} - -void MatchingSeq(std::vector &frames, const std::string &fp_path, - std::map> &id2rank) { - const int num_frame = frames.size(); - std::set> set_pairs; - for (int i = 0; i < num_frame; ++i) { - for (int k = 1; k < 20 && i + k < num_frame; ++k) - set_pairs.insert(std::pair(i, i + k)); - } - - for (auto &[id1, vec] : id2rank) { - if (id1 % 5 != 0) - continue; - for (auto &id2 : vec) { - if (id1 < id2) - set_pairs.insert(std::pair(id1, id2)); - else if (id2 < id1) - set_pairs.insert(std::pair(id2, id1)); - } - } - - std::vector> id_pairs; - id_pairs.assign(set_pairs.begin(), set_pairs.end()); - - std::vector frame_pairs; - FeatureMatching(frames, id_pairs, frame_pairs, true); - SaveFramePairs(fp_path, frame_pairs); -} - int main(int argc, const char *argv[]) { google::InitGoogleLogging(argv[0]); - // 1.Read Config + + using namespace xrsfm; std::string images_path, retrieval_path, matching_type, output_path; if (argc <= 2) { std::string config_path = "./config_mat.json"; @@ -177,82 +38,10 @@ int main(int argc, const char *argv[]) { } else { exit(-1); } - const std::string ftr_path = output_path + "ftr.bin"; - const std::string size_path = output_path + "size.bin"; - const std::string fp_init_path = output_path + "fp_init.bin"; - const std::string fp_path = output_path + "fp.bin"; - // 1.read images - if (!std::experimental::filesystem::exists(images_path)) { - std::cout << "image path not exists :" << images_path << "\n"; - exit(-1); - } - std::vector image_names; - LoadImageNames(images_path, image_names); - std::map name2id; - const int num_image = image_names.size(); - for (int i = 0; i < num_image; ++i) { - name2id[image_names[i]] = i; - } - std::cout << "Load Image Info Done.\n"; - - // 2.feature extraction - std::vector frames; - std::vector image_size_vec; - GetFeatures(images_path, image_names, ftr_path, frames); - GetImageSizeVec(images_path, image_names, size_path, image_size_vec); - std::cout << "Extract Features Done.\n"; - - // 5.image matching - std::map> id2rank; - bool have_retrieval_info = - LoadRetrievalRank(retrieval_path, name2id, id2rank); - std::cout << "Load Retrieval Info Done.\n"; - - Timer timer("%lf s\n"); - timer.start(); - if (matching_type == "sequential") { - if (!have_retrieval_info) { - std::cout << "Without the retrieval file, sequential matching " - "method only matches adjacent frames.\n"; - } - MatchingSeq(frames, fp_path, id2rank); - } else if (matching_type == "retrieval") { - if (!have_retrieval_info) { - std::cout << "The retrieval file is required when using " - "retrieval-based matching!!!\n"; - return 0; - } - std::vector> id_pairs; - ExtractNearestImagePairs(id2rank, 25, id_pairs); - std::vector frame_pairs; - FeatureMatching(frames, id_pairs, frame_pairs, true); - SaveFramePairs(fp_path, frame_pairs); - } else if (matching_type == "covisibility") { - if (!have_retrieval_info) { - std::cout << "The retrieval file is required when using " - "covisibility-based matching!!!\n"; - return 0; - } - std::vector> id_pairs; - ExtractNearestImagePairs(id2rank, 5, id_pairs); - std::vector frame_pairs; - GetInitFramePairs(fp_init_path, frames, id_pairs, frame_pairs); - std::cout << "Init Matching Done.\n"; - - constexpr int num_iteration = 5; - constexpr bool use_fundamental = true; - const auto [init_id1, init_id2] = GetInitId(num_image, frame_pairs); - - Map map; - map.frames_ = std::move(frames); - map.frame_pairs_ = std::move(frame_pairs); - ExpansionAndMatching(map, id2rank, num_iteration, image_size_vec, - init_id1, init_id2, use_fundamental, id_pairs); - SaveFramePairs(fp_path, map.frame_pairs_); - } - timer.stop(); - timer.print(); + FeatureProcessor feature_processor(images_path, output_path, matching_type, + retrieval_path); + feature_processor.Run(); return 0; } diff --git a/src/run_triangulation.cc b/src/run_triangulation.cc index e9ce077..7fb5a64 100644 --- a/src/run_triangulation.cc +++ b/src/run_triangulation.cc @@ -12,8 +12,6 @@ #include "optimization/ba_solver.h" #include "utility/io_ecim.hpp" #include "utility/timer.h" -// #include "utility/view.h" -// #include "utility/viewer.h" using namespace xrsfm; @@ -80,7 +78,6 @@ void PreProcess(const std::string bin_path, const std::string feature_path, count++; if (count % 100 == 0) std::cout << 1.0 * count / num_fp << std::endl; - // std::cout< points1, points2; @@ -89,7 +86,6 @@ void PreProcess(const std::string bin_path, const std::string feature_path, points2.push_back(frame2.points[match.id2]); } SolveFundamnetalCOLMAP(points1, points2, frame_pair); - // std::cout< new_matches; @@ -151,14 +147,14 @@ int main(int argc, const char *argv[]) { // Complete Tracks const int num_track = map.tracks_.size(); for (int track_id = 0; track_id < num_track; ++track_id) { - if (map.tracks_[track_id].outlier) + if (map.track(track_id).outlier) continue; p3d_processor.ContinueTrack(map, track_id, 8.0); } // Merge Tracks for (int track_id = 0; track_id < num_track; ++track_id) { - if (map.tracks_[track_id].outlier) + if (map.track(track_id).outlier) continue; p3d_processor.MergeTrack(map, track_id, 8.0); } diff --git a/src/tag/tag_extract.hpp b/src/tag/tag_extract.hpp index 818da4b..e706a38 100644 --- a/src/tag/tag_extract.hpp +++ b/src/tag/tag_extract.hpp @@ -20,7 +20,7 @@ extern "C" { } #include "base/map.h" -#include "base/camera.h" +#include "base/camera.hpp" #include "geometry/track_processor.h" #include "optimization/ba_solver.h" #include "optimization/cost_factor_ceres.h" @@ -136,15 +136,6 @@ void tag_refine(std::string image_dir, std::string map_dir, Map map; ReadColMapDataBinary(map_dir, map); const Camera &cam_seq = map.Camera(0); - for (auto &[id, frame] : map.frame_map_) { - if (!frame.registered) - continue; - frame.points_normalized.resize(frame.points.size()); - for (size_t j = 0; j < frame.points.size(); j++) { - ImageToNormalized(cam_seq, frame.points[j], - frame.points_normalized[j]); - } - } std::map name2id; for (auto &[id, frame] : map.frame_map_) { name2id[frame.name] = id; @@ -243,8 +234,11 @@ void tag_refine(std::string image_dir, std::string map_dir, Track &track = map.track_map_[frame.track_ids_[i]]; if (track.outlier) continue; + + Eigen::Vector2d points_normalized; + ImageToNormalized(cam_seq, frame.points[i], points_normalized); ceres::CostFunction *cost_function = - new ProjectionCost(frame.points_normalized[i]); + new ProjectionCost(points_normalized); problem.AddResidualBlock(cost_function, nullptr, frame.Tcw.q.coeffs().data(), frame.Tcw.t.data(), track.point3d_.data()); @@ -263,7 +257,7 @@ void tag_refine(std::string image_dir, std::string map_dir, ceres::Solve(solver_options, &problem, &summary); std::cout << summary.BriefReport() << "\n"; - std::cout << scale << std::endl; + std::cout << "scale :" << scale << std::endl; // resize map for (auto &[id, frame] : map.frame_map_) { diff --git a/src/utility/io_ecim.cc b/src/utility/io_ecim.cc index 77e6377..2dca8b9 100644 --- a/src/utility/io_ecim.cc +++ b/src/utility/io_ecim.cc @@ -3,6 +3,7 @@ // #include "io_ecim.hpp" +#include namespace xrsfm { @@ -12,17 +13,12 @@ void ReadCamerasBinary(const std::string &path, CHECK(file.is_open()) << path; const uint64_t num_camera = read_data2(file); - for (int i = 0; i < num_camera; ++i) { - uint32_t camera_id = -1, camera_model = -1; - read_data(file, camera_id); - read_data(file, camera_model); + const uint32_t camera_id = read_data2(file); + const uint32_t camera_model = read_data2(file); Camera camera(camera_id, camera_model); - - uint64_t w = 0, h = 0; // TODO set w,h in camera - read_data(file, w); - read_data(file, h); - + camera.width_ = read_data2(file); + camera.height_ = read_data2(file); read_data_vec(file, camera.params_.data(), camera.params_.size()); cameras[camera_id] = camera; } @@ -79,14 +75,22 @@ void ReadPoints3DBinary(const std::string &path, std::map &tracks) { read_data(file, p2d_id); track.observations_[frame_id] = p2d_id; } + track.outlier = false; tracks[id] = track; } } -void ReadColMapDataBinary(const std::string &output_path, Map &map) { +bool ReadColMapDataBinary(const std::string &output_path, Map &map) { + namespace fs = std::filesystem; + if (!(fs::exists(fs::path(output_path + "cameras.bin")) && + fs::exists(fs::path(output_path + "images.bin")) && + fs::exists(fs::path(output_path + "points3D.bin")))) { + return false; + } ReadCamerasBinary(output_path + "cameras.bin", map.camera_map_); ReadImagesBinary(output_path + "images.bin", map.frame_map_); ReadPoints3DBinary(output_path + "points3D.bin", map.track_map_); + return true; } void ReadImagesBinaryForTriangulation(const std::string &path, @@ -110,13 +114,10 @@ void ReadImagesBinaryForTriangulation(const std::string &path, pt.y = read_data2(file); } for (size_t i = 0; i < num_p2d; ++i) { - // Eigen::Vector desc; std::vector desc(feature_dim); read_data_vec(file, desc.data(), feature_dim); } frames[frame.id] = frame; - // std::cout<(file); frame_pair.matches.resize(num_matches); read_data_vec(file, &(frame_pair.matches[0]), num_matches); diff --git a/src/utility/view.cc b/src/utility/view.cc deleted file mode 100644 index 3166172..0000000 --- a/src/utility/view.cc +++ /dev/null @@ -1,259 +0,0 @@ -// -// Created by SENSETIME\yezhichao1 on 2020/10/19. -// -#include "view.h" - -#include -#include - -#include - -#include "utility/global.h" -#include "viewer_handle.h" - -#define OPEN_VIEW - -namespace xrsfm { -constexpr int width = 1024, height = 768, focal = 500; -const cv::Scalar cv_green(0, 255, 0), cv_gray(80, 80, 80), cv_blue(255, 0, 0), - cv_yellow(0, 255, 255), cv_red(0, 0, 255), cv_brown(40, 140, 140); - -void DrawMatchesColmap(const std::string dir_path, const FramePair &fp, - const Map &map) { - auto &f1 = map.frames_[fp.id1]; - auto &f2 = map.frames_[fp.id2]; - cv::Mat image1 = - cv::imread(dir_path + f1.name, cv::IMREAD_IGNORE_ORIENTATION); - cv::Mat image2 = - cv::imread(dir_path + f2.name, cv::IMREAD_IGNORE_ORIENTATION); - DrawFeatureMatches(image1, image2, f1.points, f2.points, fp.matches); - cv::waitKey(); -} - -void DrawFrameMatches(const Map &map, const std::vector &images, - int id) { - for (auto &t_frame_pair : map.frame_pairs_) { - if (t_frame_pair.id1 == id || t_frame_pair.id2 == id) { - int id1 = t_frame_pair.id1; - int id2 = t_frame_pair.id2; - printf("Find Pair: %d %d %d\n", id1, id2, t_frame_pair.inlier_num); - DrawFeatureMatches(images[id1], images[id2], - map.frames_[id1].points, map.frames_[id2].points, - t_frame_pair.matches, t_frame_pair.inlier_mask); - DrawFeatureMatches1(images[id1], map.frames_[id1].points, - map.frames_[id2].points, t_frame_pair.matches, - t_frame_pair.inlier_mask); - } - } -} - -void DrawFeature(const cv::Mat &image, - const std::vector &keypoints) { - for (const auto &kpt : keypoints) { - cv::circle(image, kpt.pt, 1, cv::Scalar(0, 255, 0), -1); - } - cv::imshow("", image); - cv::waitKey(0); -} - -void DrawFeatureMatches(const cv::Mat &img1, const cv::Mat &img2, - const std::vector &kpts1, - const std::vector &kpts2, - const std::vector &matches, - const std::vector mask) { - int rows = img1.rows > img2.rows ? img1.rows : img2.rows; - cv::Mat img(rows, img1.cols + img2.cols, img1.type(), cv::Scalar(0, 0, 0)); - cv::Mat img1p = img(cv::Rect(0, 0, img1.cols, img1.rows)); - cv::Mat img2p = img(cv::Rect(img1.cols, 0, img2.cols, img2.rows)); - img1.copyTo(img1p); - img2.copyTo(img2p); - cv::Scalar color; - for (size_t i = 0; i < matches.size(); ++i) { - auto match = matches[i]; - // std::cout< &kpts1, - const std::vector &kpts2, - const std::vector &matches, - const std::vector mask) { - std::vector kpts1_; - for (auto &pt : kpts1) { - cv::KeyPoint kpt; - kpt.pt = cv::Point2f(pt.x(), pt.y()); - kpts1_.emplace_back(kpt); - } - std::vector kpts2_; - for (auto &pt : kpts2) { - cv::KeyPoint kpt; - kpt.pt = cv::Point2f(pt.x(), pt.y()); - kpts2_.emplace_back(kpt); - } - DrawFeatureMatches(img1, img2, kpts1_, kpts2_, matches, mask); -} - -void DrawFeatureFlow(const cv::Mat &img1, const std::vector &pts1, - const std::vector &pts2, - const std::vector &matches, - const std::vector &states, - const std::string window_name) { - std::vector kpts1; - for (auto &pt : pts1) { - kpts1.emplace_back(cv::KeyPoint(pt.x(), pt.y(), -1)); - } - std::vector kpts2; - for (auto &pt : pts2) { - ; - kpts2.emplace_back(cv::KeyPoint(pt.x(), pt.y(), -1)); - } - - cv::Mat img = img1; - - for (size_t i = 0; i < matches.size(); ++i) { - auto match = matches[i]; - cv::Scalar color, dst_color, src_color = cv_yellow; - if (states[i] == 1) - color = dst_color = cv_green; - else if (states[i] == 0) - color = dst_color = cv_red; - else if (states[i] == -1) { - color = dst_color = cv_brown; - } else { - std::cerr << "unexpected state"; - } - cv::circle(img, kpts1[match.id1].pt, 2, src_color); - line(img, kpts1[match.id1].pt, kpts2[match.id2].pt, color, 1); - cv::circle(img, kpts2[match.id2].pt, 2, dst_color); - } - - constexpr double scale = 1.0; - resize(img, img, cv::Size(int(img.cols * scale), int(img.rows * scale))); - imshow(window_name, img); -} - -void DrawFeatureMatches1(const cv::Mat &img1, - const std::vector &kpts1, - const std::vector &kpts2, - const std::vector &matches, - const std::vector mask) { - cv::Mat img = img1.clone(); - cv::Scalar color; - - for (size_t i = 0; i < matches.size(); ++i) { - auto match = matches[i]; - color = mask.empty() || mask[i] ? cv_green : cv_red; - cv::circle(img, kpts1[match.id1].pt, 2, cv_blue); - line(img, kpts1[match.id1].pt, kpts2[match.id2].pt, color, 1); - cv::circle(img, kpts2[match.id2].pt, 2, cv_yellow); - } - - constexpr double scale = 1.0; - resize(img, img, cv::Size(int(img.cols * scale), int(img.rows * scale))); - imshow("1", img); -} - -void DrawFeatureMatches1(const cv::Mat &img1, const std::vector &kpts1, - const std::vector &kpts2, - const std::vector &matches, - const std::vector mask) { - std::vector kpts1_; - for (auto &pt : kpts1) { - cv::KeyPoint kpt; - kpt.pt = cv::Point2f(pt.x(), pt.y()); - kpts1_.emplace_back(kpt); - } - std::vector kpts2_; - for (auto &pt : kpts2) { - cv::KeyPoint kpt; - kpt.pt = cv::Point2f(pt.x(), pt.y()); - kpts2_.emplace_back(kpt); - } - DrawFeatureMatches1(img1, kpts1_, kpts2_, matches, mask); -} - -void DrawCameras(const std::vector &cameras, - const std::vector colors, double camera_size) { - const float w = camera_size; - const float h = w * 0.5f; - const float z = w * 0.6f; - const int num_camera = cameras.size(); - for (int i = 0; i < num_camera; ++i) { - const auto &camera = cameras[i]; - glPushMatrix(); - const Eigen::Matrix3d R = camera.q.toRotationMatrix(); - const vector3 t = camera.t; - Eigen::Matrix4d Tcw; - Tcw << R(0, 0), R(0, 1), R(0, 2), t(0), R(1, 0), R(1, 1), R(1, 2), t(1), - R(2, 0), R(2, 1), R(2, 2), t(2), 0, 0, 0, 1; - Eigen::Matrix4d Twc = Tcw.inverse(); - glMultMatrixd(Twc.data()); - glColor3d(colors[i](0), colors[i](1), colors[i](2)); - - glLineWidth(2.0f); - glBegin(GL_LINES); - glVertex3f(0, 0, 0); - glVertex3f(w, h, z); - glVertex3f(0, 0, 0); - glVertex3f(w, -h, z); - glVertex3f(0, 0, 0); - glVertex3f(-w, -h, z); - glVertex3f(0, 0, 0); - glVertex3f(-w, h, z); - glVertex3f(w, h, z); - glVertex3f(w, -h, z); - glVertex3f(-w, h, z); - glVertex3f(-w, -h, z); - glVertex3f(-w, h, z); - glVertex3f(w, h, z); - glVertex3f(-w, -h, z); - glVertex3f(w, -h, z); - - // glColor3f(1,0,0); - // glVertex3f(0,0,0); - // glVertex3f(2*w,0,0); - // - // glColor3f(0,1,0); - // glVertex3f(0,0,0); - // glVertex3f(0,2*w,0); - // - // glColor3f(0,0,1); - // glVertex3f(0,0,0); - // glVertex3f(0,0,2*w); - - glEnd(); - glPopMatrix(); - } -} - -void DrawPoints(const std::vector &points) { - glPointSize(1.0f); - glBegin(GL_POINTS); - for (const auto &pt : points) { - glColor3f(0.0f, 0.0f, 0.0f); - glVertex3d(pt(0), pt(1), pt(2)); - } - glEnd(); -} - -void DrawPoints(const std::vector &points, - const std::vector colors) { - glPointSize(1.0f); - glBegin(GL_POINTS); - for (int i = 0; i < points.size(); ++i) { - glColor3d(colors[i](0), colors[i](1), colors[i](2)); - glVertex3d(points[i](0), points[i](1), points[i](2)); - } - glEnd(); -} - -} // namespace xrsfm diff --git a/src/utility/view.h b/src/utility/view.h deleted file mode 100644 index 7378265..0000000 --- a/src/utility/view.h +++ /dev/null @@ -1,47 +0,0 @@ -// -// Created by SENSETIME\yezhichao1 on 2020/10/18. -// - -#pragma once - -#include - -#include "base/map.h" -#include "viewer_handle.h" - -namespace xrsfm { - -void DrawFeature(const cv::Mat &image, - const std::vector &keypoints); - -void DrawFeatureMatches(const cv::Mat &img1, const cv::Mat &img2, - const std::vector &kpts1, - const std::vector &kpts2, - const std::vector &matches, - const std::vector mask = std::vector(0)); - -void DrawFeatureMatches(const cv::Mat &img1, const cv::Mat &img2, - const std::vector &kpts1, - const std::vector &kpts2, - const std::vector &matches, - const std::vector mask = std::vector(0)); - -void DrawFeatureMatches1(const cv::Mat &img1, - const std::vector &kpts1, - const std::vector &kpts2, - const std::vector &matches, - const std::vector mask = std::vector(0)); - -void DrawFeatureFlow(const cv::Mat &img1, - const std::vector &pts1, - const std::vector &pts2, - const std::vector &matches, - const std::vector &states, - const std::string window_name = "feature flow"); - -void DrawCameras(const std::vector &cameras, - const std::vector colors, double camera_size); - -void DrawPoints(const std::vector &points); - -} // namespace xrsfm diff --git a/src/utility/viewer.cc b/src/utility/viewer.cc deleted file mode 100644 index 477e4bc..0000000 --- a/src/utility/viewer.cc +++ /dev/null @@ -1,163 +0,0 @@ -// -// Created by SENSETIME\yezhichao1 on 2020/10/19. -// -// #include "viewer.h" - -#include - -#include "view.h" -#include "viewer_handle.h" - -#define OPEN_VIEW - -namespace xrsfm { -constexpr int width = 1024, height = 768, focal = 500; -const vector3 red(1.0, 0, 0), green(0, 1.0, 0), blue(0, 0, 1.0), - yellow(1, 1, 0), grey(0.5, 0.5, 0.5), black(0, 0, 0); - -void ViewerThread::start() { - worker_running = true; - worker_has_stop = false; - worker_thread = std::thread(&ViewerThread::worker_loop, this); -} - -void ViewerThread::stop() { - if (worker_running) { - worker_running = false; - } - while (!worker_has_stop) { - usleep(500); - } - worker_thread.join(); -} - -void ViewerThread::worker_loop() { -#ifdef OPEN_VIEW - pangolin::CreateWindowAndBind("ECI-SfM", width, height); - glEnable(GL_DEPTH_TEST); - glEnable(GL_BLEND); - glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); - // Define Projection and initial ModelView matrix - pangolin::OpenGlRenderState s_cam( - pangolin::ProjectionMatrix(width, height, focal, focal, width / 2, - height / 2, 0.1, 1000), - pangolin::ModelViewLookAt(0, 0, -10, 0, 0, 0, pangolin::AxisNegY)); - - // Create Interactive View in window - pangolin::MyHandler3D handler(s_cam); - pangolin::View &d_cam = pangolin::CreateDisplay() - .SetBounds(0.0, 1.0, 0.0, 1.0, -640.0f / 480.0f) - .SetHandler(&handler); - - while (!pangolin::ShouldQuit() & worker_running) { - if (handler.init_viewpoint) { - s_cam.SetModelViewMatrix(pangolin::ModelViewLookAt( - 0, 0, -10, 0, 0, 0, pangolin::AxisNegY)); - handler.init_viewpoint = false; - } - - d_cam.Activate(s_cam); - glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); - glClearColor(1.0f, 1.0f, 1.0f, 1.0f); - - std::unique_lock lk(map_mutex); - DrawCameras(cameras, colors_cam, camera_size_); - if (colors_pt.empty()) - DrawPoints(points); - else { - for (size_t i = 0; i < points.size(); i++) { - const vector3 pt = points.at(i); - const vector3 color = colors_pt.at(i); - if (color == red) { - glPointSize(5.0f); - } else { - glPointSize(1.0f); - } - glBegin(GL_POINTS); - glColor3f(color(0), color(1), color(2)); - glVertex3d(pt(0), pt(1), pt(2)); - glEnd(); - } - } - - pangolin::FinishFrame(); - usleep(2000); - } - - pangolin::DestroyWindow("ECI-SfM"); - pangolin::QuitAll(); - worker_has_stop = true; -#endif -} - -void ViewerThread::update_map(const Map &map) { - std::unique_lock lk(map_mutex); - cameras.clear(); - colors_cam.clear(); - points.clear(); - colors_pt.clear(); - - for (const auto &frame : map.frames_) { - if (!frame.registered) - continue; - cameras.emplace_back(frame.Tcw); - if (frame.flag_for_view) { - colors_cam.emplace_back(red); - } else if (frame.is_keyframe) { - colors_cam.emplace_back(green); - } else { - colors_cam.emplace_back(grey); - } - } - - if (map.tmp_frame.registered) { - cameras.emplace_back(map.tmp_frame.Tcw); - colors_cam.emplace_back(yellow); - } - - for (const auto &track : map.tracks_) { - if (!track.outlier) //&& track.is_keypoint) { - points.emplace_back(track.point3d_); - } -} - -void ViewerThread::update_map_colmap(const Map &map) { - std::unique_lock lk(map_mutex); - cameras.clear(); - colors_cam.clear(); - points.clear(); - colors_pt.clear(); - - for (const auto &[id, frame] : map.frame_map_) { - if (!frame.registered) - continue; - cameras.emplace_back(frame.Tcw); - if (frame.flag_for_view) { - colors_cam.emplace_back(red); - } else if (frame.is_keyframe) { - colors_cam.emplace_back(green); - } else { - colors_cam.emplace_back(grey); - } - } - - for (const auto &[id, track] : map.track_map_) { - if (track.outlier) - continue; - points.emplace_back(track.point3d_); - colors_pt.emplace_back(black); - } -} - -void ViewerThread::add_tag_points( - const std::map> &pt_world_vec) { - for (const auto &[id, pt_world] : pt_world_vec) { - if (pt_world.empty()) - continue; - for (const auto &pt : pt_world) { - points.emplace_back(pt); - colors_pt.emplace_back(red); - } - } -} -} // namespace xrsfm diff --git a/src/utility/viewer.h b/src/utility/viewer.h deleted file mode 100644 index 03ecca5..0000000 --- a/src/utility/viewer.h +++ /dev/null @@ -1,37 +0,0 @@ -// -// Created by SENSETIME\yezhichao1 on 2020/10/18. -// - -#pragma once - -#include - -#include - -#include "base/map.h" -#include "utility/global.h" - -namespace xrsfm { -class ViewerThread { - public: - void start(); - void worker_loop(); - void stop(); - - void update_map(const Map &map); - void update_map_colmap(const Map &map); - void - add_tag_points(const std::map> &pt_world_vec); - - double camera_size_ = 0.1; - std::vector cameras; - std::vector colors_cam; - std::vector points; - std::vector colors_pt; - - mutable std::mutex map_mutex; - std::atomic worker_running; - std::atomic worker_has_stop; - std::thread worker_thread; -}; -} // namespace xrsfm diff --git a/src/utility/viewer_handle.h b/src/utility/viewer_handle.h deleted file mode 100644 index bf9def4..0000000 --- a/src/utility/viewer_handle.h +++ /dev/null @@ -1,26 +0,0 @@ -// -// Created by yzc on 19-5-14. -// - -#pragma once - -#include - -namespace pangolin { -struct MyHandler3D : Handler3D { - MyHandler3D(OpenGlRenderState &cam_state, - AxisDirection enforce_up = AxisNone, float trans_scale = 1.0f, - float zoom_fraction = 3 * PANGO_DFLT_HANDLER3D_ZF) - : Handler3D(cam_state, enforce_up, trans_scale, zoom_fraction){}; - - MyHandler3D::~MyHandler3D(); - - // void Mouse(View &d, MouseButton button, int x, int y, bool - // pressed, int button_state); - void Keyboard(View &d, unsigned char key, int x, int y, bool pressed); - - public: - bool draw_current; - std::atomic init_viewpoint; -}; -} // namespace pangolin diff --git a/src/utility/viewer_handler.cc b/src/utility/viewer_handler.cc deleted file mode 100644 index 5332626..0000000 --- a/src/utility/viewer_handler.cc +++ /dev/null @@ -1,18 +0,0 @@ -// -// Created by yzc on 19-5-14. -// - -#include "viewer_handle.h" - -namespace pangolin { -MyHandler3D::~MyHandler3D() { init_viewpoint = false; } -void MyHandler3D::Keyboard(View &d, unsigned char key, int x, int y, - bool pressed) { - if (key == 'n' && pressed) { - draw_current = false; - } else if (key == 'o' && pressed) { - init_viewpoint = true; - } -} - -} // namespace pangolin diff --git a/src/xrsfm_gui.cc b/src/xrsfm_gui.cc new file mode 100644 index 0000000..599531d --- /dev/null +++ b/src/xrsfm_gui.cc @@ -0,0 +1,102 @@ +#include +#include +#include +#include +#include +#include "feature/feature_processor.h" +#include "ui/model_viewer_widget.h" +#include "ui/project_widget.h" + +namespace xrsfm { +class MainWindow : public QMainWindow { + public: + MainWindow() { + model_viewer_widget_ = new ModelViewerWidget(this); + setCentralWidget(model_viewer_widget_); + model_viewer_widget_->map = new Map(); + + std::setlocale(LC_NUMERIC, "C"); + resize(1024, 600); + setWindowTitle(QString::fromStdString("XRSfM")); + + project_widget_ = new ProjectWidget(this); + + action_view_model_ = new QAction("Open Sparse Model", this); + connect(action_view_model_, &QAction::triggered, this, + &MainWindow::selectModelPath); + action_set_project_ = new QAction("Set Project", this); + connect(action_set_project_, &QAction::triggered, this, + &MainWindow::showMenu); + action_start_reconstruction_ = + new QAction("Start Reconstruction", this); + connect(action_start_reconstruction_, &QAction::triggered, this, + &MainWindow::mapping); + + file_toolbar_ = addToolBar(tr("File")); + file_toolbar_->addAction(action_view_model_); + // TODO the reconstruction part + // file_toolbar_->addAction(action_set_project_); + // file_toolbar_->addAction(action_start_reconstruction_); + file_toolbar_->setIconSize(QSize(16, 16)); + } + + void selectModelPath() { + const auto model_path = QFileDialog::getExistingDirectory( + this, tr("Select model path..."), "", QFileDialog::ShowDirsOnly); + std::string model_path_str = model_path.toUtf8().constData(); + model_path_str += "/"; + if (ReadColMapDataBinary(model_path_str, *model_viewer_widget_->map)) { + model_viewer_widget_->Upload(); + std::cout << model_path_str << " load success\n"; + } else { + std::cout << model_path_str << " load fail\n"; + } + } + + void showMenu() { + project_widget_->Reset(); + project_widget_->show(); + project_widget_->raise(); + } + + void mapping() { + const std::string image_path = project_widget_->GetImagePath(); + const std::string work_path = project_widget_->GetWorkspacePath(); + + FeatureProcessor feature_processor(image_path, work_path); + feature_processor.Run(); + + // Map map; + // PreProcess(work_path, camera_path, map); + + // IncrementalMapper imapper; + // imapper.options.init_id1 = -1; + // imapper.options.init_id2 = -1; + // imapper.options.correct_pose = false; + // imapper.options.stop_when_register_fail = true; + // imapper.Reconstruct(map); + } + + QToolBar *file_toolbar_; + QAction *action_view_model_; + QAction *action_set_project_; + QAction *action_start_reconstruction_; + ProjectWidget *project_widget_; + ModelViewerWidget *model_viewer_widget_; +}; +} // namespace xrsfm + +int main(int argc, char **argv) { +#if (QT_VERSION >= QT_VERSION_CHECK(5, 6, 0)) + Q_INIT_RESOURCE(resources); + QApplication::setAttribute(Qt::AA_EnableHighDpiScaling); + QApplication::setAttribute(Qt::AA_UseHighDpiPixmaps); +#endif + + QApplication app(argc, argv); + QMainWindow window; + xrsfm::MainWindow main_window; + main_window.show(); + + return app.exec(); +}