diff --git a/.gitattributes b/.gitattributes index c7d9f3332a950355d5a77d85000f05e6f45435ea..b0b0e9a9fbf077231f2a1e9a271c7491987b62a6 100644 --- a/.gitattributes +++ b/.gitattributes @@ -32,3 +32,12 @@ saved_model/**/* filter=lfs diff=lfs merge=lfs -text *.zip filter=lfs diff=lfs merge=lfs -text *.zst filter=lfs diff=lfs merge=lfs -text *tfevents* filter=lfs diff=lfs merge=lfs -text +bin/colmap filter=lfs diff=lfs merge=lfs -text +lib/libceres.a filter=lfs diff=lfs merge=lfs -text +lib/colmap/libcolmap_cuda.a filter=lfs diff=lfs merge=lfs -text +lib/colmap/libflann.a filter=lfs diff=lfs merge=lfs -text +lib/colmap/libpoisson_recon.a filter=lfs diff=lfs merge=lfs -text +lib/colmap/libcolmap.a filter=lfs diff=lfs merge=lfs -text +lib/colmap/libpba.a filter=lfs diff=lfs merge=lfs -text +lib/colmap/libsqlite3.a filter=lfs diff=lfs merge=lfs -text +lib/colmap/libsift_gpu.a filter=lfs diff=lfs merge=lfs -text diff --git a/bin/colmap b/bin/colmap new file mode 100644 index 0000000000000000000000000000000000000000..92634041eec43a78d47445246d34920ade4fd512 --- /dev/null +++ b/bin/colmap @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:5256130472c6f6e7e82c04b2bcd08ca11628cc8da83f65e011f5a5c3ff260796 +size 30940856 diff --git a/cmake/Ceres/CeresConfig.cmake b/cmake/Ceres/CeresConfig.cmake new file mode 100644 index 0000000000000000000000000000000000000000..49c7db8488605fa2720e33f3c352e05f6877c199 --- /dev/null +++ b/cmake/Ceres/CeresConfig.cmake @@ -0,0 +1,340 @@ +# Ceres Solver - A fast non-linear least squares minimizer +# Copyright 2015 Google Inc. All rights reserved. +# http://ceres-solver.org/ +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# * Neither the name of Google Inc. nor the names of its contributors may be +# used to endorse or promote products derived from this software without +# specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# Authors: pablo.speciale@gmail.com (Pablo Speciale) +# alexs.mac@gmail.com (Alex Stewart) +# + +# Config file for Ceres Solver - Find Ceres & dependencies. +# +# This file is used by CMake when find_package(Ceres) is invoked and either +# the directory containing this file either is present in CMAKE_MODULE_PATH +# (if Ceres was installed), or exists in the local CMake package registry if +# the Ceres build directory was exported. +# +# This module defines the following variables: +# +# Ceres_FOUND / CERES_FOUND: True if Ceres has been successfully +# found. Both variables are set as although +# FindPackage() only references Ceres_FOUND +# in Config mode, given the conventions for +# _FOUND when FindPackage() is +# called in Module mode, users could +# reasonably expect to use CERES_FOUND +# instead. +# +# CERES_VERSION: Version of Ceres found. +# +# CERES_LIBRARIES: Libraries for Ceres and all +# dependencies against which Ceres was +# compiled. This will not include any optional +# dependencies that were disabled when Ceres was +# compiled. +# +# NOTE: There is no equivalent of CERES_INCLUDE_DIRS as the exported +# CMake target already includes the definition of its public +# include directories. + +include(CMakeFindDependencyMacro) + +# Called if we failed to find Ceres or any of its required dependencies, +# unsets all public (designed to be used externally) variables and reports +# error message at priority depending upon [REQUIRED/QUIET/] argument. +macro(CERES_REPORT_NOT_FOUND REASON_MSG) + # FindPackage() only references Ceres_FOUND, and requires it to be + # explicitly set FALSE to denote not found (not merely undefined). + set(Ceres_FOUND FALSE) + set(CERES_FOUND FALSE) + unset(CERES_INCLUDE_DIR) + unset(CERES_INCLUDE_DIRS) + unset(CERES_LIBRARIES) + + # Reset the CMake module path to its state when this script was called. + set(CMAKE_MODULE_PATH ${CALLERS_CMAKE_MODULE_PATH}) + + # Note _FIND_[REQUIRED/QUIETLY] variables defined by + # FindPackage() use the camelcase library name, not uppercase. + if (Ceres_FIND_QUIETLY) + message(STATUS "Failed to find Ceres - " ${REASON_MSG} ${ARGN}) + elseif (Ceres_FIND_REQUIRED) + message(FATAL_ERROR "Failed to find Ceres - " ${REASON_MSG} ${ARGN}) + else() + # Neither QUIETLY nor REQUIRED, use SEND_ERROR which emits an error + # that prevents generation, but continues configuration. + message(SEND_ERROR "Failed to find Ceres - " ${REASON_MSG} ${ARGN}) + endif () + return() +endmacro(CERES_REPORT_NOT_FOUND) + + +# ceres_message([mode] "message text") +# +# Wraps the standard cmake 'message' command, but suppresses output +# if the QUIET flag was passed to the find_package(Ceres ...) call. +function(ceres_message) + if (NOT Ceres_FIND_QUIETLY) + message(${ARGN}) + endif() +endfunction() + + +# ceres_pretty_print_cmake_list( OUTPUT_VAR [item1 [item2 ... ]] ) +# +# Sets ${OUTPUT_VAR} in the caller's scope to a human-readable string +# representation of the list passed as the remaining arguments formed +# as: "[item1, item2, ..., itemN]". +function(ceres_pretty_print_cmake_list OUTPUT_VAR) + string(REPLACE ";" ", " PRETTY_LIST_STRING "[${ARGN}]") + set(${OUTPUT_VAR} "${PRETTY_LIST_STRING}" PARENT_SCOPE) +endfunction() + +# The list of (optional) components this version of Ceres was compiled with. +set(CERES_COMPILED_COMPONENTS "EigenSparse;SparseLinearAlgebraLibrary;LAPACK;SuiteSparse;CXSparse;SchurSpecializations;Multithreading") + +# If Ceres was not installed, then by definition it was exported +# from a build directory. +set(CERES_WAS_INSTALLED TRUE) + +# Record the state of the CMake module path when this script was +# called so that we can ensure that we leave it in the same state on +# exit as it was on entry, but modify it locally. +set(CALLERS_CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH}) + +# Get the (current, i.e. installed) directory containing this file. +get_filename_component(CERES_CURRENT_CONFIG_DIR + "${CMAKE_CURRENT_LIST_FILE}" PATH) + +if (CERES_WAS_INSTALLED) + # Reset CMake module path to the installation directory of this + # script, thus we will use the FindPackage() scripts shipped with + # Ceres to find Ceres' dependencies, even if the user has equivalently + # named FindPackage() scripts in their project. + set(CMAKE_MODULE_PATH ${CERES_CURRENT_CONFIG_DIR}) + + # Build the absolute root install directory as a relative path + # (determined when Ceres was configured & built) from the current + # install directory for this this file. This allows for the install + # tree to be relocated, after Ceres was built, outside of CMake. + get_filename_component(CURRENT_ROOT_INSTALL_DIR + "${CERES_CURRENT_CONFIG_DIR}/../../../" + ABSOLUTE) + if (NOT EXISTS ${CURRENT_ROOT_INSTALL_DIR}) + ceres_report_not_found( + "Ceres install root: ${CURRENT_ROOT_INSTALL_DIR}, " + "determined from relative path from CeresConfig.cmake install location: " + "${CERES_CURRENT_CONFIG_DIR}, does not exist. Either the install " + "directory was deleted, or the install tree was only partially relocated " + "outside of CMake after Ceres was built.") + endif (NOT EXISTS ${CURRENT_ROOT_INSTALL_DIR}) + +else(CERES_WAS_INSTALLED) + # Ceres was exported from the build tree. + set(CERES_EXPORTED_BUILD_DIR ${CERES_CURRENT_CONFIG_DIR}) + get_filename_component(CERES_EXPORTED_SOURCE_DIR + "${CERES_EXPORTED_BUILD_DIR}/../../../" + ABSOLUTE) + if (NOT EXISTS ${CERES_EXPORTED_SOURCE_DIR}) + ceres_report_not_found( + "Ceres exported source directory: ${CERES_EXPORTED_SOURCE_DIR}, " + "determined from relative path from CeresConfig.cmake exported build " + "directory: ${CERES_EXPORTED_BUILD_DIR} does not exist.") + endif() + + # Reset CMake module path to the cmake directory in the Ceres source + # tree which was exported, thus we will use the FindPackage() scripts shipped + # with Ceres to find Ceres' dependencies, even if the user has equivalently + # named FindPackage() scripts in their project. + set(CMAKE_MODULE_PATH ${CERES_EXPORTED_SOURCE_DIR}/cmake) +endif(CERES_WAS_INSTALLED) + +# Set the version. +set(CERES_VERSION 2.1.0) + +include(CMakeFindDependencyMacro) +find_dependency(Threads) + +# Optional dependencies +find_dependency(CXSparse 3.1.9) +find_dependency(SuiteSparse 5.1.2) + +# As imported CMake targets are not re-exported when a dependent target is +# exported, we must invoke find_package(XXX) here to reload the definition +# of their targets. Without this, the dependency target names (e.g. +# 'gflags-shared') which will be present in the ceres target would not be +# defined, and so CMake will assume that they refer to a library name and +# fail to link correctly. + +# Eigen. +# Flag set during configuration and build of Ceres. +set(CERES_EIGEN_VERSION 3.3.4) +# Search quietly to control the timing of the error message if not found. The +# search should be for an exact match, but for usability reasons do a soft +# match and reject with an explanation below. +find_package(Eigen3 ${CERES_EIGEN_VERSION} QUIET) +if (Eigen3_FOUND) + if (NOT Eigen3_VERSION VERSION_EQUAL CERES_EIGEN_VERSION) + # CMake's VERSION check in FIND_PACKAGE() will accept any version >= the + # specified version. However, only version = is supported. Improve + # usability by explaining why we don't accept non-exact version matching. + ceres_report_not_found("Found Eigen dependency, but the version of Eigen " + "found (${Eigen3_VERSION}) does not exactly match the version of Eigen " + "Ceres was compiled with (${CERES_EIGEN_VERSION}). This can cause subtle " + "bugs by triggering violations of the One Definition Rule. See the " + "Wikipedia article http://en.wikipedia.org/wiki/One_Definition_Rule " + "for more details") + endif () + ceres_message(STATUS "Found required Ceres dependency: " + "Eigen version ${CERES_EIGEN_VERSION} in ${Eigen3_DIR}") +else (Eigen3_FOUND) + ceres_report_not_found("Missing required Ceres " + "dependency: Eigen version ${CERES_EIGEN_VERSION}, please set " + "Eigen3_DIR.") +endif (Eigen3_FOUND) + +# glog (and maybe gflags). +# +# Flags set during configuration and build of Ceres. +set(CERES_USES_MINIGLOG OFF) +set(CERES_GLOG_VERSION ) +set(CERES_GLOG_WAS_BUILT_WITH_CMAKE 0) + +set(CERES_USES_GFLAGS ON) +set(CERES_GFLAGS_VERSION 2.2.1) + +if (CERES_USES_MINIGLOG) + # Output message at standard log level (not the lower STATUS) so that + # the message is output in GUI during configuration to warn user. + ceres_message("-- Found Ceres compiled with miniglog substitute " + "for glog, beware this will likely cause problems if glog is later linked.") +else(CERES_USES_MINIGLOG) + if (CERES_GLOG_WAS_BUILT_WITH_CMAKE) + find_package(glog ${CERES_GLOG_VERSION} CONFIG QUIET) + set(GLOG_FOUND ${glog_FOUND}) + else() + # Version of glog against which Ceres was built was not built with CMake, + # use the exported glog find_package() module from Ceres to find it again. + # Append the locations of glog when Ceres was built to the search path hints. + list(APPEND GLOG_INCLUDE_DIR_HINTS "/usr/include") + get_filename_component(CERES_BUILD_GLOG_LIBRARY_DIR "/usr/lib/x86_64-linux-gnu/libglog.so" PATH) + list(APPEND GLOG_LIBRARY_DIR_HINTS ${CERES_BUILD_GLOG_LIBRARY_DIR}) + + # Search quietly s/t we control the timing of the error message if not found. + find_package(Glog QUIET) + endif() + + if (GLOG_FOUND) + ceres_message(STATUS "Found required Ceres dependency: glog") + else() + ceres_report_not_found("Missing required Ceres dependency: glog.") + endif() + + # gflags is only a public dependency of Ceres via glog, thus is not required + # if Ceres was built with MINIGLOG. + if (CERES_USES_GFLAGS) + # Search quietly s/t we control the timing of the error message if not found. + find_package(gflags ${CERES_GFLAGS_VERSION} QUIET) + if (gflags_FOUND AND TARGET gflags) + ceres_message(STATUS "Found required Ceres dependency: gflags") + else() + ceres_report_not_found("Missing required Ceres " + "dependency: gflags (not found, or not found as exported CMake target).") + endif() + endif() +endif(CERES_USES_MINIGLOG) + +# Import exported Ceres targets, if they have not already been imported. +if (NOT TARGET ceres AND NOT Ceres_BINARY_DIR) + include(${CERES_CURRENT_CONFIG_DIR}/CeresTargets.cmake) +endif (NOT TARGET ceres AND NOT Ceres_BINARY_DIR) +# Set the expected XX_LIBRARIES variable for FindPackage(). +set(CERES_LIBRARIES Ceres::ceres) + +# Reset CMake module path to its state when this script was called. +set(CMAKE_MODULE_PATH ${CALLERS_CMAKE_MODULE_PATH}) + +# Build the detected Ceres version string to correctly capture whether it +# was installed, or exported. +ceres_pretty_print_cmake_list(CERES_COMPILED_COMPONENTS_STRING + ${CERES_COMPILED_COMPONENTS}) +if (CERES_WAS_INSTALLED) + set(CERES_DETECTED_VERSION_STRING "Ceres version: ${CERES_VERSION} " + "installed in: ${CURRENT_ROOT_INSTALL_DIR} with components: " + "${CERES_COMPILED_COMPONENTS_STRING}") +else (CERES_WAS_INSTALLED) + set(CERES_DETECTED_VERSION_STRING "Ceres version: ${CERES_VERSION} " + "exported from build directory: ${CERES_EXPORTED_BUILD_DIR} with " + "components: ${CERES_COMPILED_COMPONENTS_STRING}") +endif() + +# If the user called this script through find_package() whilst specifying +# particular Ceres components that should be found via: +# find_package(Ceres COMPONENTS XXX YYY), check the requested components against +# those with which Ceres was compiled. In this case, we should only report +# Ceres as found if all the requested components have been found. +if (Ceres_FIND_COMPONENTS) + foreach (REQUESTED_COMPONENT ${Ceres_FIND_COMPONENTS}) + list(FIND CERES_COMPILED_COMPONENTS ${REQUESTED_COMPONENT} HAVE_REQUESTED_COMPONENT) + # list(FIND ..) returns -1 if the element was not in the list, but CMake + # interprets if (VAR) to be true if VAR is any non-zero number, even + # negative ones, hence we have to explicitly check for >= 0. + if (HAVE_REQUESTED_COMPONENT EQUAL -1) + # Check for the presence of all requested components before reporting + # not found, such that we report all of the missing components rather + # than just the first. + list(APPEND MISSING_CERES_COMPONENTS ${REQUESTED_COMPONENT}) + endif() + endforeach() + if (MISSING_CERES_COMPONENTS) + ceres_pretty_print_cmake_list(REQUESTED_CERES_COMPONENTS_STRING + ${Ceres_FIND_COMPONENTS}) + ceres_pretty_print_cmake_list(MISSING_CERES_COMPONENTS_STRING + ${MISSING_CERES_COMPONENTS}) + ceres_report_not_found("Missing requested Ceres components: " + "${MISSING_CERES_COMPONENTS_STRING} (components requested: " + "${REQUESTED_CERES_COMPONENTS_STRING}). Detected " + "${CERES_DETECTED_VERSION_STRING}.") + endif() +endif() + +# As we use CERES_REPORT_NOT_FOUND() to abort, if we reach this point we have +# found Ceres and all required dependencies. +ceres_message(STATUS "Found " ${CERES_DETECTED_VERSION_STRING}) + +# Set CERES_FOUND to be equivalent to Ceres_FOUND, which is set to +# TRUE by FindPackage() if this file is found and run, and after which +# Ceres_FOUND is not (explicitly, i.e. undefined does not count) set +# to FALSE. +set(CERES_FOUND TRUE) + +if (NOT TARGET ceres) + # For backwards compatibility, create a local 'alias' target with the + # non-namespace-qualified Ceres target name. Note that this is not a + # true ALIAS library in CMake terms as they cannot point to imported targets. + add_library(ceres INTERFACE IMPORTED) + set_target_properties(ceres PROPERTIES INTERFACE_LINK_LIBRARIES Ceres::ceres) +endif() diff --git a/cmake/Ceres/CeresConfigVersion.cmake b/cmake/Ceres/CeresConfigVersion.cmake new file mode 100644 index 0000000000000000000000000000000000000000..f0004769aee3a15efc6248d2bd6d404f79c8c388 --- /dev/null +++ b/cmake/Ceres/CeresConfigVersion.cmake @@ -0,0 +1,70 @@ +# This is a basic version file for the Config-mode of find_package(). +# It is used by write_basic_package_version_file() as input file for configure_file() +# to create a version-file which can be installed along a config.cmake file. +# +# The created file sets PACKAGE_VERSION_EXACT if the current version string and +# the requested version string are exactly the same and it sets +# PACKAGE_VERSION_COMPATIBLE if the current version is >= requested version, +# but only if the requested major version is the same as the current one. +# The variable CVF_VERSION must be set before calling configure_file(). + + +set(PACKAGE_VERSION "2.1.0") + +if(PACKAGE_VERSION VERSION_LESS PACKAGE_FIND_VERSION) + set(PACKAGE_VERSION_COMPATIBLE FALSE) +else() + + if("2.1.0" MATCHES "^([0-9]+)\\.") + set(CVF_VERSION_MAJOR "${CMAKE_MATCH_1}") + if(NOT CVF_VERSION_MAJOR VERSION_EQUAL 0) + string(REGEX REPLACE "^0+" "" CVF_VERSION_MAJOR "${CVF_VERSION_MAJOR}") + endif() + else() + set(CVF_VERSION_MAJOR "2.1.0") + endif() + + if(PACKAGE_FIND_VERSION_RANGE) + # both endpoints of the range must have the expected major version + math (EXPR CVF_VERSION_MAJOR_NEXT "${CVF_VERSION_MAJOR} + 1") + if (NOT PACKAGE_FIND_VERSION_MIN_MAJOR STREQUAL CVF_VERSION_MAJOR + OR ((PACKAGE_FIND_VERSION_RANGE_MAX STREQUAL "INCLUDE" AND NOT PACKAGE_FIND_VERSION_MAX_MAJOR STREQUAL CVF_VERSION_MAJOR) + OR (PACKAGE_FIND_VERSION_RANGE_MAX STREQUAL "EXCLUDE" AND NOT PACKAGE_FIND_VERSION_MAX VERSION_LESS_EQUAL CVF_VERSION_MAJOR_NEXT))) + set(PACKAGE_VERSION_COMPATIBLE FALSE) + elseif(PACKAGE_FIND_VERSION_MIN_MAJOR STREQUAL CVF_VERSION_MAJOR + AND ((PACKAGE_FIND_VERSION_RANGE_MAX STREQUAL "INCLUDE" AND PACKAGE_VERSION VERSION_LESS_EQUAL PACKAGE_FIND_VERSION_MAX) + OR (PACKAGE_FIND_VERSION_RANGE_MAX STREQUAL "EXCLUDE" AND PACKAGE_VERSION VERSION_LESS PACKAGE_FIND_VERSION_MAX))) + set(PACKAGE_VERSION_COMPATIBLE TRUE) + else() + set(PACKAGE_VERSION_COMPATIBLE FALSE) + endif() + else() + if(PACKAGE_FIND_VERSION_MAJOR STREQUAL CVF_VERSION_MAJOR) + set(PACKAGE_VERSION_COMPATIBLE TRUE) + else() + set(PACKAGE_VERSION_COMPATIBLE FALSE) + endif() + + if(PACKAGE_FIND_VERSION STREQUAL PACKAGE_VERSION) + set(PACKAGE_VERSION_EXACT TRUE) + endif() + endif() +endif() + + +# if the installed project requested no architecture check, don't perform the check +if("FALSE") + return() +endif() + +# if the installed or the using project don't have CMAKE_SIZEOF_VOID_P set, ignore it: +if("${CMAKE_SIZEOF_VOID_P}" STREQUAL "" OR "8" STREQUAL "") + return() +endif() + +# check that the installed version has the same 32/64bit-ness as the one which is currently searching: +if(NOT CMAKE_SIZEOF_VOID_P STREQUAL "8") + math(EXPR installedBits "8 * 8") + set(PACKAGE_VERSION "${PACKAGE_VERSION} (${installedBits}bit)") + set(PACKAGE_VERSION_UNSUITABLE TRUE) +endif() diff --git a/cmake/Ceres/CeresTargets-release.cmake b/cmake/Ceres/CeresTargets-release.cmake new file mode 100644 index 0000000000000000000000000000000000000000..cdef82c94ab24a45a26592f98d52984c8ae5270e --- /dev/null +++ b/cmake/Ceres/CeresTargets-release.cmake @@ -0,0 +1,19 @@ +#---------------------------------------------------------------- +# Generated CMake target import file for configuration "Release". +#---------------------------------------------------------------- + +# Commands may need to know the format version. +set(CMAKE_IMPORT_FILE_VERSION 1) + +# Import target "Ceres::ceres" for configuration "Release" +set_property(TARGET Ceres::ceres APPEND PROPERTY IMPORTED_CONFIGURATIONS RELEASE) +set_target_properties(Ceres::ceres PROPERTIES + IMPORTED_LINK_INTERFACE_LANGUAGES_RELEASE "CXX" + IMPORTED_LOCATION_RELEASE "${_IMPORT_PREFIX}/lib/libceres.a" + ) + +list(APPEND _IMPORT_CHECK_TARGETS Ceres::ceres ) +list(APPEND _IMPORT_CHECK_FILES_FOR_Ceres::ceres "${_IMPORT_PREFIX}/lib/libceres.a" ) + +# Commands beyond this point should not need to know the version. +set(CMAKE_IMPORT_FILE_VERSION) diff --git a/cmake/Ceres/CeresTargets.cmake b/cmake/Ceres/CeresTargets.cmake new file mode 100644 index 0000000000000000000000000000000000000000..273190bb044ba6f200c2df05d400be636ad90b69 --- /dev/null +++ b/cmake/Ceres/CeresTargets.cmake @@ -0,0 +1,100 @@ +# Generated by CMake + +if("${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION}" LESS 2.6) + message(FATAL_ERROR "CMake >= 2.6.0 required") +endif() +cmake_policy(PUSH) +cmake_policy(VERSION 2.6...3.20) +#---------------------------------------------------------------- +# Generated CMake target import file. +#---------------------------------------------------------------- + +# Commands may need to know the format version. +set(CMAKE_IMPORT_FILE_VERSION 1) + +# Protect against multiple inclusion, which would fail when already imported targets are added once more. +set(_targetsDefined) +set(_targetsNotDefined) +set(_expectedTargets) +foreach(_expectedTarget Ceres::ceres) + list(APPEND _expectedTargets ${_expectedTarget}) + if(NOT TARGET ${_expectedTarget}) + list(APPEND _targetsNotDefined ${_expectedTarget}) + endif() + if(TARGET ${_expectedTarget}) + list(APPEND _targetsDefined ${_expectedTarget}) + endif() +endforeach() +if("${_targetsDefined}" STREQUAL "${_expectedTargets}") + unset(_targetsDefined) + unset(_targetsNotDefined) + unset(_expectedTargets) + set(CMAKE_IMPORT_FILE_VERSION) + cmake_policy(POP) + return() +endif() +if(NOT "${_targetsDefined}" STREQUAL "") + message(FATAL_ERROR "Some (but not all) targets in this export set were already defined.\nTargets Defined: ${_targetsDefined}\nTargets not yet defined: ${_targetsNotDefined}\n") +endif() +unset(_targetsDefined) +unset(_targetsNotDefined) +unset(_expectedTargets) + + +# Compute the installation prefix relative to this file. +get_filename_component(_IMPORT_PREFIX "${CMAKE_CURRENT_LIST_FILE}" PATH) +get_filename_component(_IMPORT_PREFIX "${_IMPORT_PREFIX}" PATH) +get_filename_component(_IMPORT_PREFIX "${_IMPORT_PREFIX}" PATH) +get_filename_component(_IMPORT_PREFIX "${_IMPORT_PREFIX}" PATH) +if(_IMPORT_PREFIX STREQUAL "/") + set(_IMPORT_PREFIX "") +endif() + +# Create imported target Ceres::ceres +add_library(Ceres::ceres STATIC IMPORTED) + +set_target_properties(Ceres::ceres PROPERTIES + INTERFACE_COMPILE_FEATURES "cxx_std_14" + INTERFACE_INCLUDE_DIRECTORIES "${_IMPORT_PREFIX}/include;/usr/include" + INTERFACE_LINK_LIBRARIES "Threads::Threads;/usr/lib/x86_64-linux-gnu/libglog.so;gflags;\$;\$;\$;/usr/local/cuda/lib64/libcudart_static.a;\$;\$;/usr/lib/x86_64-linux-gnu/librt.so;/usr/local/cuda/lib64/libcublas.so;/usr/local/cuda/lib64/libcusolver.so;/usr/local/cuda/lib64/libcusparse.so;/usr/local/lib/libmkl_intel_lp64.so;/usr/local/lib/libmkl_intel_thread.so;/usr/local/lib/libmkl_core.so;/usr/local/lib/libiomp5.so;\$;\$;\$;\$;\$;\$;Eigen3::Eigen" +) + +if(CMAKE_VERSION VERSION_LESS 2.8.12) + message(FATAL_ERROR "This file relies on consumers using CMake 2.8.12 or greater.") +endif() + +# Load information for each installed configuration. +get_filename_component(_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH) +file(GLOB CONFIG_FILES "${_DIR}/CeresTargets-*.cmake") +foreach(f ${CONFIG_FILES}) + include(${f}) +endforeach() + +# Cleanup temporary variables. +set(_IMPORT_PREFIX) + +# Loop over all imported files and verify that they actually exist +foreach(target ${_IMPORT_CHECK_TARGETS} ) + foreach(file ${_IMPORT_CHECK_FILES_FOR_${target}} ) + if(NOT EXISTS "${file}" ) + message(FATAL_ERROR "The imported target \"${target}\" references the file + \"${file}\" +but this file does not exist. Possible reasons include: +* The file was deleted, renamed, or moved to another location. +* An install or uninstall procedure did not complete successfully. +* The installation package was faulty and contained + \"${CMAKE_CURRENT_LIST_FILE}\" +but not all the files it references. +") + endif() + endforeach() + unset(_IMPORT_CHECK_FILES_FOR_${target}) +endforeach() +unset(_IMPORT_CHECK_TARGETS) + +# This file does not depend on other imported targets which have +# been exported from the same project but in a separate export set. + +# Commands beyond this point should not need to know the version. +set(CMAKE_IMPORT_FILE_VERSION) +cmake_policy(POP) diff --git a/cmake/Ceres/FindCXSparse.cmake b/cmake/Ceres/FindCXSparse.cmake new file mode 100644 index 0000000000000000000000000000000000000000..afd1ebf152bcdadb35c8b5e84f9a4f1e74589c67 --- /dev/null +++ b/cmake/Ceres/FindCXSparse.cmake @@ -0,0 +1,240 @@ +# Ceres Solver - A fast non-linear least squares minimizer +# Copyright 2022 Google Inc. All rights reserved. +# http://ceres-solver.org/ +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# * Neither the name of Google Inc. nor the names of its contributors may be +# used to endorse or promote products derived from this software without +# specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# Author: alexs.mac@gmail.com (Alex Stewart) +# + +#[=======================================================================[.rst: +FindCXSparse +============ + +Find CXSparse and its dependencies. + +This module defines the following variables which should be referenced by the +caller to use the library. + +``CXSparse_FOUND`` + ``TRUE`` iff CXSparse and all dependencies have been found. + +``CXSparse_VERSION`` + Extracted from ``cs.h``. + +``CXSparse_VERSION_MAJOR`` + Equal to 3 if ``CXSparse_VERSION`` = 3.1.2 + +``CXSparse_VERSION_MINOR`` + Equal to 1 if ``CXSparse_VERSION`` = 3.1.2 + +``CXSparse_VERSION_PATCH`` + Equal to 2 if ``CXSparse_VERSION`` = 3.1.2 + +The following variables control the behaviour of this module: + +``CXSparse_NO_CMAKE`` + Do not attempt to use the native CXSparse CMake package configuration. + +Targets +------- + +The following target defines CXSparse. + +``CXSparse::CXSparse`` + The main CXSparse to be linked against. + +The following variables are also defined by this module, but in line with CMake +recommended ``find_package`` module style should NOT be referenced directly by +callers (use the plural variables detailed above instead). These variables do +however affect the behaviour of the module via ``find_[path/library]()`` which +are NOT re-called (i.e., search for library is not repeated) if these variables +are set with valid values *in the CMake cache*. This means that if these +variables are set directly in the cache, either by the user in the CMake GUI, or +by the user passing ``-DVAR=VALUE`` directives to CMake when called (which +explicitly defines a cache variable), then they will be used verbatim, bypassing +the ``HINTS`` variables and other hard-coded search locations. + +``CXSparse_INCLUDE_DIR`` + Include directory for CXSparse, not including the include directory of any + dependencies. + +``CXSparse_LIBRARY`` + CXSparse library, not including the libraries of any dependencies. +]=======================================================================] + +if (NOT CXSparse_NO_CMAKE) + find_package (CXSparse NO_MODULE QUIET) +endif (NOT CXSparse_NO_CMAKE) + +if (CXSparse_FOUND) + return () +endif (CXSparse_FOUND) + +# Reset CALLERS_CMAKE_FIND_LIBRARY_PREFIXES to its value when +# FindCXSparse was invoked. +macro(CXSparse_RESET_FIND_LIBRARY_PREFIX) + if (MSVC) + set(CMAKE_FIND_LIBRARY_PREFIXES "${CALLERS_CMAKE_FIND_LIBRARY_PREFIXES}") + endif (MSVC) +endmacro(CXSparse_RESET_FIND_LIBRARY_PREFIX) + +# Called if we failed to find CXSparse or any of it's required dependencies, +# unsets all public (designed to be used externally) variables and reports +# error message at priority depending upon [REQUIRED/QUIET/] argument. +macro(CXSparse_REPORT_NOT_FOUND REASON_MSG) + # Make results of search visible in the CMake GUI if CXSparse has not + # been found so that user does not have to toggle to advanced view. + mark_as_advanced(CLEAR CXSparse_INCLUDE_DIR + CXSparse_LIBRARY) + + cxsparse_reset_find_library_prefix() + + # Note _FIND_[REQUIRED/QUIETLY] variables defined by FindPackage() + # use the camelcase library name, not uppercase. + if (CXSparse_FIND_QUIETLY) + message(STATUS "Failed to find CXSparse - " ${REASON_MSG} ${ARGN}) + elseif (CXSparse_FIND_REQUIRED) + message(FATAL_ERROR "Failed to find CXSparse - " ${REASON_MSG} ${ARGN}) + else() + # Neither QUIETLY nor REQUIRED, use no priority which emits a message + # but continues configuration and allows generation. + message("-- Failed to find CXSparse - " ${REASON_MSG} ${ARGN}) + endif () + return() +endmacro(CXSparse_REPORT_NOT_FOUND) + +# Handle possible presence of lib prefix for libraries on MSVC, see +# also CXSparse_RESET_FIND_LIBRARY_PREFIX(). +if (MSVC) + # Preserve the caller's original values for CMAKE_FIND_LIBRARY_PREFIXES + # s/t we can set it back before returning. + set(CALLERS_CMAKE_FIND_LIBRARY_PREFIXES "${CMAKE_FIND_LIBRARY_PREFIXES}") + # The empty string in this list is important, it represents the case when + # the libraries have no prefix (shared libraries / DLLs). + set(CMAKE_FIND_LIBRARY_PREFIXES "lib" "" "${CMAKE_FIND_LIBRARY_PREFIXES}") +endif (MSVC) + +# Additional suffixes to try appending to each search path. +list(APPEND CXSparse_CHECK_PATH_SUFFIXES + suitesparse) # Linux/Windows + +# Search supplied hint directories first if supplied. +find_path(CXSparse_INCLUDE_DIR + NAMES cs.h + PATH_SUFFIXES ${CXSparse_CHECK_PATH_SUFFIXES}) +if (NOT CXSparse_INCLUDE_DIR OR + NOT EXISTS ${CXSparse_INCLUDE_DIR}) + cxsparse_report_not_found( + "Could not find CXSparse include directory, set CXSparse_INCLUDE_DIR " + "to directory containing cs.h") +endif (NOT CXSparse_INCLUDE_DIR OR + NOT EXISTS ${CXSparse_INCLUDE_DIR}) + +find_library(CXSparse_LIBRARY NAMES cxsparse + PATH_SUFFIXES ${CXSparse_CHECK_PATH_SUFFIXES}) + +if (NOT CXSparse_LIBRARY OR + NOT EXISTS ${CXSparse_LIBRARY}) + cxsparse_report_not_found( + "Could not find CXSparse library, set CXSparse_LIBRARY " + "to full path to libcxsparse.") +endif (NOT CXSparse_LIBRARY OR + NOT EXISTS ${CXSparse_LIBRARY}) + +# Mark internally as found, then verify. CXSparse_REPORT_NOT_FOUND() unsets +# if called. +set(CXSparse_FOUND TRUE) + +# Extract CXSparse version from cs.h +if (CXSparse_INCLUDE_DIR) + set(CXSparse_VERSION_FILE ${CXSparse_INCLUDE_DIR}/cs.h) + if (NOT EXISTS ${CXSparse_VERSION_FILE}) + cxsparse_report_not_found( + "Could not find file: ${CXSparse_VERSION_FILE} " + "containing version information in CXSparse install located at: " + "${CXSparse_INCLUDE_DIR}.") + else (NOT EXISTS ${CXSparse_VERSION_FILE}) + file(READ ${CXSparse_INCLUDE_DIR}/cs.h CXSparse_VERSION_FILE_CONTENTS) + + string(REGEX MATCH "#define CS_VER [0-9]+" + CXSparse_VERSION_MAJOR "${CXSparse_VERSION_FILE_CONTENTS}") + string(REGEX REPLACE "#define CS_VER ([0-9]+)" "\\1" + CXSparse_VERSION_MAJOR "${CXSparse_VERSION_MAJOR}") + + string(REGEX MATCH "#define CS_SUBVER [0-9]+" + CXSparse_VERSION_MINOR "${CXSparse_VERSION_FILE_CONTENTS}") + string(REGEX REPLACE "#define CS_SUBVER ([0-9]+)" "\\1" + CXSparse_VERSION_MINOR "${CXSparse_VERSION_MINOR}") + + string(REGEX MATCH "#define CS_SUBSUB [0-9]+" + CXSparse_VERSION_PATCH "${CXSparse_VERSION_FILE_CONTENTS}") + string(REGEX REPLACE "#define CS_SUBSUB ([0-9]+)" "\\1" + CXSparse_VERSION_PATCH "${CXSparse_VERSION_PATCH}") + + # This is on a single line s/t CMake does not interpret it as a list of + # elements and insert ';' separators which would result in 3.;1.;2 nonsense. + set(CXSparse_VERSION "${CXSparse_VERSION_MAJOR}.${CXSparse_VERSION_MINOR}.${CXSparse_VERSION_PATCH}") + set(CXSparse_VERSION_COMPONENTS 3) + endif (NOT EXISTS ${CXSparse_VERSION_FILE}) +endif (CXSparse_INCLUDE_DIR) + +# Catch the case when the caller has set CXSparse_LIBRARY in the cache / GUI and +# thus FIND_LIBRARY was not called, but specified library is invalid, otherwise +# we would report CXSparse as found. +# TODO: This regex for CXSparse library is pretty primitive, we use lowercase +# for comparison to handle Windows using CamelCase library names, could +# this check be better? +string(TOLOWER "${CXSparse_LIBRARY}" LOWERCASE_CXSparse_LIBRARY) +if (CXSparse_LIBRARY AND + EXISTS ${CXSparse_LIBRARY} AND + NOT "${LOWERCASE_CXSparse_LIBRARY}" MATCHES ".*cxsparse[^/]*") + cxsparse_report_not_found( + "Caller defined CXSparse_LIBRARY: " + "${CXSparse_LIBRARY} does not match CXSparse.") +endif (CXSparse_LIBRARY AND + EXISTS ${CXSparse_LIBRARY} AND + NOT "${LOWERCASE_CXSparse_LIBRARY}" MATCHES ".*cxsparse[^/]*") + +cxsparse_reset_find_library_prefix() + +mark_as_advanced(CXSparse_INCLUDE_DIR CXSparse_LIBRARY) + +# Handle REQUIRED / QUIET optional arguments and version. +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(CXSparse + REQUIRED_VARS CXSparse_INCLUDE_DIR CXSparse_LIBRARY + VERSION_VAR CXSparse_VERSION) + +if (CXSparse_INCLUDE_DIR AND CXSparse_LIBRARY) + if (NOT TARGET CXSparse::CXSparse) + add_library (CXSparse::CXSparse IMPORTED UNKNOWN) + endif (NOT TARGET CXSparse::CXSparse) + + set_property (TARGET CXSparse::CXSparse PROPERTY + IMPORTED_LOCATION ${CXSparse_LIBRARY}) + set_property (TARGET CXSparse::CXSparse PROPERTY + INTERFACE_INCLUDE_DIRECTORIES ${CXSparse_INCLUDE_DIR}) +endif (CXSparse_INCLUDE_DIR AND CXSparse_LIBRARY) diff --git a/cmake/Ceres/FindGlog.cmake b/cmake/Ceres/FindGlog.cmake new file mode 100644 index 0000000000000000000000000000000000000000..1a7b6c092ab6f36fd3d41c68c99518db624d0cb0 --- /dev/null +++ b/cmake/Ceres/FindGlog.cmake @@ -0,0 +1,379 @@ +# Ceres Solver - A fast non-linear least squares minimizer +# Copyright 2015 Google Inc. All rights reserved. +# http://ceres-solver.org/ +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# * Neither the name of Google Inc. nor the names of its contributors may be +# used to endorse or promote products derived from this software without +# specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# Author: alexs.mac@gmail.com (Alex Stewart) +# + +# FindGlog.cmake - Find Google glog logging library. +# +# This module defines the following variables: +# +# GLOG_FOUND: TRUE iff glog is found. +# GLOG_INCLUDE_DIRS: Include directories for glog. +# GLOG_LIBRARIES: Libraries required to link glog. +# FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION: True iff the version of glog found +# was built & installed / exported +# as a CMake package. +# +# The following variables control the behaviour of this module: +# +# GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION: TRUE/FALSE, iff TRUE then +# then prefer using an exported CMake configuration +# generated by glog > 0.3.4 over searching for the +# glog components manually. Otherwise (FALSE) +# ignore any exported glog CMake configurations and +# always perform a manual search for the components. +# Default: TRUE iff user does not define this variable +# before we are called, and does NOT specify either +# GLOG_INCLUDE_DIR_HINTS or GLOG_LIBRARY_DIR_HINTS +# otherwise FALSE. +# GLOG_INCLUDE_DIR_HINTS: List of additional directories in which to +# search for glog includes, e.g: /timbuktu/include. +# GLOG_LIBRARY_DIR_HINTS: List of additional directories in which to +# search for glog libraries, e.g: /timbuktu/lib. +# +# The following variables are also defined by this module, but in line with +# CMake recommended FindPackage() module style should NOT be referenced directly +# by callers (use the plural variables detailed above instead). These variables +# do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which +# are NOT re-called (i.e. search for library is not repeated) if these variables +# are set with valid values _in the CMake cache_. This means that if these +# variables are set directly in the cache, either by the user in the CMake GUI, +# or by the user passing -DVAR=VALUE directives to CMake when called (which +# explicitly defines a cache variable), then they will be used verbatim, +# bypassing the HINTS variables and other hard-coded search locations. +# +# GLOG_INCLUDE_DIR: Include directory for glog, not including the +# include directory of any dependencies. +# GLOG_LIBRARY: glog library, not including the libraries of any +# dependencies. + +# Reset CALLERS_CMAKE_FIND_LIBRARY_PREFIXES to its value when +# FindGlog was invoked. +macro(GLOG_RESET_FIND_LIBRARY_PREFIX) + if (MSVC AND CALLERS_CMAKE_FIND_LIBRARY_PREFIXES) + set(CMAKE_FIND_LIBRARY_PREFIXES "${CALLERS_CMAKE_FIND_LIBRARY_PREFIXES}") + endif() +endmacro(GLOG_RESET_FIND_LIBRARY_PREFIX) + +# Called if we failed to find glog or any of it's required dependencies, +# unsets all public (designed to be used externally) variables and reports +# error message at priority depending upon [REQUIRED/QUIET/] argument. +macro(GLOG_REPORT_NOT_FOUND REASON_MSG) + unset(GLOG_FOUND) + unset(GLOG_INCLUDE_DIRS) + unset(GLOG_LIBRARIES) + # Make results of search visible in the CMake GUI if glog has not + # been found so that user does not have to toggle to advanced view. + mark_as_advanced(CLEAR GLOG_INCLUDE_DIR + GLOG_LIBRARY) + + glog_reset_find_library_prefix() + + # Note _FIND_[REQUIRED/QUIETLY] variables defined by FindPackage() + # use the camelcase library name, not uppercase. + if (Glog_FIND_QUIETLY) + message(STATUS "Failed to find glog - " ${REASON_MSG} ${ARGN}) + elseif (Glog_FIND_REQUIRED) + message(FATAL_ERROR "Failed to find glog - " ${REASON_MSG} ${ARGN}) + else() + # Neither QUIETLY nor REQUIRED, use no priority which emits a message + # but continues configuration and allows generation. + message("-- Failed to find glog - " ${REASON_MSG} ${ARGN}) + endif () + return() +endmacro(GLOG_REPORT_NOT_FOUND) + +# glog_message([mode] "message text") +# +# Wraps the standard cmake 'message' command, but suppresses output +# if the QUIET flag was passed to the find_package(Glog ...) call. +function(GLOG_MESSAGE) + if (NOT Glog_FIND_QUIETLY) + message(${ARGN}) + endif() +endfunction() + +# Protect against any alternative find_package scripts for this library having +# been called previously (in a client project) which set GLOG_FOUND, but not +# the other variables we require / set here which could cause the search logic +# here to fail. +unset(GLOG_FOUND) + +# ----------------------------------------------------------------- +# By default, if the user has expressed no preference for using an exported +# glog CMake configuration over performing a search for the installed +# components, and has not specified any hints for the search locations, then +# prefer a glog exported configuration if available. +if (NOT DEFINED GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION + AND NOT GLOG_INCLUDE_DIR_HINTS + AND NOT GLOG_LIBRARY_DIR_HINTS) + glog_message(STATUS "No preference for use of exported glog CMake " + "configuration set, and no hints for include/library directories provided. " + "Defaulting to preferring an installed/exported glog CMake configuration " + "if available.") + set(GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION TRUE) +endif() + +# On macOS, add the Homebrew prefix (with appropriate suffixes) to the +# respective HINTS directories (after any user-specified locations). This +# handles Homebrew installations into non-standard locations (not /usr/local). +# We do not use CMAKE_PREFIX_PATH for this as given the search ordering of +# find_xxx(), doing so would override any user-specified HINTS locations with +# the Homebrew version if it exists. +if (CMAKE_SYSTEM_NAME MATCHES "Darwin") + find_program(HOMEBREW_EXECUTABLE brew) + mark_as_advanced(FORCE HOMEBREW_EXECUTABLE) + if (HOMEBREW_EXECUTABLE) + # Detected a Homebrew install, query for its install prefix. + execute_process(COMMAND ${HOMEBREW_EXECUTABLE} --prefix + OUTPUT_VARIABLE HOMEBREW_INSTALL_PREFIX + OUTPUT_STRIP_TRAILING_WHITESPACE) + glog_message(STATUS "Detected Homebrew with install prefix: " + "${HOMEBREW_INSTALL_PREFIX}, adding to CMake search paths.") + list(APPEND GLOG_INCLUDE_DIR_HINTS "${HOMEBREW_INSTALL_PREFIX}/include") + list(APPEND GLOG_LIBRARY_DIR_HINTS "${HOMEBREW_INSTALL_PREFIX}/lib") + endif() +endif() + +if (GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION) + # Try to find an exported CMake configuration for glog, as generated by + # glog versions > 0.3.4 + # + # We search twice, s/t we can invert the ordering of precedence used by + # find_package() for exported package build directories, and installed + # packages (found via CMAKE_SYSTEM_PREFIX_PATH), listed as items 6) and 7) + # respectively in [1]. + # + # By default, exported build directories are (in theory) detected first, and + # this is usually the case on Windows. However, on OS X & Linux, the install + # path (/usr/local) is typically present in the PATH environment variable + # which is checked in item 4) in [1] (i.e. before both of the above, unless + # NO_SYSTEM_ENVIRONMENT_PATH is passed). As such on those OSs installed + # packages are usually detected in preference to exported package build + # directories. + # + # To ensure a more consistent response across all OSs, and as users usually + # want to prefer an installed version of a package over a locally built one + # where both exist (esp. as the exported build directory might be removed + # after installation), we first search with NO_CMAKE_PACKAGE_REGISTRY which + # means any build directories exported by the user are ignored, and thus + # installed directories are preferred. If this fails to find the package + # we then research again, but without NO_CMAKE_PACKAGE_REGISTRY, so any + # exported build directories will now be detected. + # + # To prevent confusion on Windows, we also pass NO_CMAKE_BUILDS_PATH (which + # is item 5) in [1]), to not preferentially use projects that were built + # recently with the CMake GUI to ensure that we always prefer an installed + # version if available. + # + # NOTE: We use the NAMES option as glog erroneously uses 'google-glog' as its + # project name when built with CMake, but exports itself as just 'glog'. + # On Linux/OS X this does not break detection as the project name is + # not used as part of the install path for the CMake package files, + # e.g. /usr/local/lib/cmake/glog, where the suffix is hardcoded + # in glog's CMakeLists. However, on Windows the project name *is* + # part of the install prefix: C:/Program Files/google-glog/[include,lib]. + # However, by default CMake checks: + # C:/Program Files/ which does not + # exist and thus detection fails. Thus we use the NAMES to force the + # search to use both google-glog & glog. + # + # [1] http://www.cmake.org/cmake/help/v2.8.11/cmake.html#command:find_package + find_package(glog QUIET + NAMES google-glog glog + HINTS ${glog_DIR} ${HOMEBREW_INSTALL_PREFIX} + NO_MODULE + NO_CMAKE_PACKAGE_REGISTRY + NO_CMAKE_BUILDS_PATH) + if (glog_FOUND) + glog_message(STATUS "Found installed version of glog: ${glog_DIR}") + else() + # Failed to find an installed version of glog, repeat search allowing + # exported build directories. + glog_message(STATUS "Failed to find installed glog CMake configuration, " + "searching for glog build directories exported with CMake.") + # Again pass NO_CMAKE_BUILDS_PATH, as we know that glog is exported and + # do not want to treat projects built with the CMake GUI preferentially. + find_package(glog QUIET + NAMES google-glog glog + NO_MODULE + NO_CMAKE_BUILDS_PATH) + if (glog_FOUND) + glog_message(STATUS "Found exported glog build directory: ${glog_DIR}") + endif(glog_FOUND) + endif(glog_FOUND) + + set(FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION ${glog_FOUND}) + + if (FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION) + glog_message(STATUS "Detected glog version: ${glog_VERSION}") + set(GLOG_FOUND ${glog_FOUND}) + # glog wraps the include directories into the exported glog::glog target. + set(GLOG_INCLUDE_DIR "") + set(GLOG_LIBRARY glog::glog) + else (FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION) + glog_message(STATUS "Failed to find an installed/exported CMake " + "configuration for glog, will perform search for installed glog " + "components.") + endif (FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION) +endif(GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION) + +if (NOT GLOG_FOUND) + # Either failed to find an exported glog CMake configuration, or user + # told us not to use one. Perform a manual search for all glog components. + + # Handle possible presence of lib prefix for libraries on MSVC, see + # also GLOG_RESET_FIND_LIBRARY_PREFIX(). + if (MSVC) + # Preserve the caller's original values for CMAKE_FIND_LIBRARY_PREFIXES + # s/t we can set it back before returning. + set(CALLERS_CMAKE_FIND_LIBRARY_PREFIXES "${CMAKE_FIND_LIBRARY_PREFIXES}") + # The empty string in this list is important, it represents the case when + # the libraries have no prefix (shared libraries / DLLs). + set(CMAKE_FIND_LIBRARY_PREFIXES "lib" "" "${CMAKE_FIND_LIBRARY_PREFIXES}") + endif (MSVC) + + # Search user-installed locations first, so that we prefer user installs + # to system installs where both exist. + list(APPEND GLOG_CHECK_INCLUDE_DIRS + /usr/local/include + /usr/local/homebrew/include # Mac OS X + /opt/local/var/macports/software # Mac OS X. + /opt/local/include + /usr/include) + # Windows (for C:/Program Files prefix). + list(APPEND GLOG_CHECK_PATH_SUFFIXES + glog/include + glog/Include + Glog/include + Glog/Include + google-glog/include # CMake installs with project name prefix. + google-glog/Include) + + list(APPEND GLOG_CHECK_LIBRARY_DIRS + /usr/local/lib + /usr/local/homebrew/lib # Mac OS X. + /opt/local/lib + /usr/lib) + # Windows (for C:/Program Files prefix). + list(APPEND GLOG_CHECK_LIBRARY_SUFFIXES + glog/lib + glog/Lib + Glog/lib + Glog/Lib + google-glog/lib # CMake installs with project name prefix. + google-glog/Lib) + + # Search supplied hint directories first if supplied. + find_path(GLOG_INCLUDE_DIR + NAMES glog/logging.h + HINTS ${GLOG_INCLUDE_DIR_HINTS} + PATHS ${GLOG_CHECK_INCLUDE_DIRS} + PATH_SUFFIXES ${GLOG_CHECK_PATH_SUFFIXES}) + if (NOT GLOG_INCLUDE_DIR OR + NOT EXISTS ${GLOG_INCLUDE_DIR}) + glog_report_not_found( + "Could not find glog include directory, set GLOG_INCLUDE_DIR " + "to directory containing glog/logging.h") + endif (NOT GLOG_INCLUDE_DIR OR + NOT EXISTS ${GLOG_INCLUDE_DIR}) + + find_library(GLOG_LIBRARY NAMES glog + HINTS ${GLOG_LIBRARY_DIR_HINTS} + PATHS ${GLOG_CHECK_LIBRARY_DIRS} + PATH_SUFFIXES ${GLOG_CHECK_LIBRARY_SUFFIXES}) + if (NOT GLOG_LIBRARY OR + NOT EXISTS ${GLOG_LIBRARY}) + glog_report_not_found( + "Could not find glog library, set GLOG_LIBRARY " + "to full path to libglog.") + endif (NOT GLOG_LIBRARY OR + NOT EXISTS ${GLOG_LIBRARY}) + + # Mark internally as found, then verify. GLOG_REPORT_NOT_FOUND() unsets + # if called. + set(GLOG_FOUND TRUE) + + # Glog does not seem to provide any record of the version in its + # source tree, thus cannot extract version. + + # Catch case when caller has set GLOG_INCLUDE_DIR in the cache / GUI and + # thus FIND_[PATH/LIBRARY] are not called, but specified locations are + # invalid, otherwise we would report the library as found. + if (GLOG_INCLUDE_DIR AND + NOT EXISTS ${GLOG_INCLUDE_DIR}/glog/logging.h) + glog_report_not_found( + "Caller defined GLOG_INCLUDE_DIR:" + " ${GLOG_INCLUDE_DIR} does not contain glog/logging.h header.") + endif (GLOG_INCLUDE_DIR AND + NOT EXISTS ${GLOG_INCLUDE_DIR}/glog/logging.h) + # TODO: This regex for glog library is pretty primitive, we use lowercase + # for comparison to handle Windows using CamelCase library names, could + # this check be better? + string(TOLOWER "${GLOG_LIBRARY}" LOWERCASE_GLOG_LIBRARY) + if (GLOG_LIBRARY AND + NOT "${LOWERCASE_GLOG_LIBRARY}" MATCHES ".*glog[^/]*") + glog_report_not_found( + "Caller defined GLOG_LIBRARY: " + "${GLOG_LIBRARY} does not match glog.") + endif (GLOG_LIBRARY AND + NOT "${LOWERCASE_GLOG_LIBRARY}" MATCHES ".*glog[^/]*") + + glog_reset_find_library_prefix() + +endif(NOT GLOG_FOUND) + +# Set standard CMake FindPackage variables if found. +if (GLOG_FOUND) + set(GLOG_INCLUDE_DIRS ${GLOG_INCLUDE_DIR}) + set(GLOG_LIBRARIES ${GLOG_LIBRARY}) +endif (GLOG_FOUND) + +# If we are using an exported CMake glog target, the include directories are +# wrapped into the target itself, and do not have to be (and are not) +# separately specified. In which case, we should not add GLOG_INCLUDE_DIRS +# to the list of required variables in order that glog be reported as found. +if (FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION) + set(GLOG_REQUIRED_VARIABLES GLOG_LIBRARIES) +else() + set(GLOG_REQUIRED_VARIABLES GLOG_INCLUDE_DIRS GLOG_LIBRARIES) +endif() + +# Handle REQUIRED / QUIET optional arguments. +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(Glog DEFAULT_MSG + ${GLOG_REQUIRED_VARIABLES}) + +# Only mark internal variables as advanced if we found glog, otherwise +# leave them visible in the standard GUI for the user to set manually. +if (GLOG_FOUND) + mark_as_advanced(FORCE GLOG_INCLUDE_DIR + GLOG_LIBRARY + glog_DIR) # Autogenerated by find_package(glog) +endif (GLOG_FOUND) diff --git a/cmake/Ceres/FindMETIS.cmake b/cmake/Ceres/FindMETIS.cmake new file mode 100644 index 0000000000000000000000000000000000000000..5f41792d78ddda35d9ac793b7459c1cdf00aa003 --- /dev/null +++ b/cmake/Ceres/FindMETIS.cmake @@ -0,0 +1,110 @@ +# +# Copyright (c) 2022 Sergiu Deitsch +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTMETISLAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. +# +#[=======================================================================[.rst: +Module for locating METIS +========================= + +Read-only variables: + +``METIS_FOUND`` + Indicates whether the library has been found. + +``METIS_VERSION`` + Indicates library version. + +Targets +------- + +``METIS::METIS`` + Specifies targets that should be passed to target_link_libararies. +]=======================================================================] + +include (FindPackageHandleStandardArgs) + +find_path (METIS_INCLUDE_DIR NAMES metis.h + PATH_SUFFIXES include + DOC "METIS include directory") +find_library (METIS_LIBRARY_DEBUG NAMES metis + PATH_SUFFIXES Debug + DOC "METIS debug library") +find_library (METIS_LIBRARY_RELEASE NAMES metis + PATH_SUFFIXES Release + DOC "METIS release library") + +if (METIS_LIBRARY_RELEASE) + if (METIS_LIBRARY_DEBUG) + set (METIS_LIBRARY debug ${METIS_LIBRARY_DEBUG} optimized + ${METIS_LIBRARY_RELEASE} CACHE STRING "METIS library") + else (METIS_LIBRARY_DEBUG) + set (METIS_LIBRARY ${METIS_LIBRARY_RELEASE} CACHE FILEPATH "METIS library") + endif (METIS_LIBRARY_DEBUG) +elseif (METIS_LIBRARY_DEBUG) + set (METIS_LIBRARY ${METIS_LIBRARY_DEBUG} CACHE FILEPATH "METIS library") +endif (METIS_LIBRARY_RELEASE) + +set (_METIS_VERSION_HEADER ${METIS_INCLUDE_DIR}/metis.h) + +if (EXISTS ${_METIS_VERSION_HEADER}) + file (READ ${_METIS_VERSION_HEADER} _METIS_VERSION_CONTENTS) + + string (REGEX REPLACE ".*#define METIS_VER_MAJOR[ \t]+([0-9]+).*" "\\1" + METIS_VERSION_MAJOR "${_METIS_VERSION_CONTENTS}") + string (REGEX REPLACE ".*#define METIS_VER_MINOR[ \t]+([0-9]+).*" "\\1" + METIS_VERSION_MINOR "${_METIS_VERSION_CONTENTS}") + string (REGEX REPLACE ".*#define METIS_VER_SUBMINOR[ \t]+([0-9]+).*" "\\1" + METIS_VERSION_PATCH "${_METIS_VERSION_CONTENTS}") + + set (METIS_VERSION + ${METIS_VERSION_MAJOR}.${METIS_VERSION_MINOR}.${METIS_VERSION_PATCH}) + set (METIS_VERSION_COMPONENTS 3) +endif (EXISTS ${_METIS_VERSION_HEADER}) + +mark_as_advanced (METIS_INCLUDE_DIR METIS_LIBRARY_DEBUG METIS_LIBRARY_RELEASE + METIS_LIBRARY) + +if (NOT TARGET METIS::METIS) + if (METIS_INCLUDE_DIR OR METIS_LIBRARY) + add_library (METIS::METIS IMPORTED UNKNOWN) + endif (METIS_INCLUDE_DIR OR METIS_LIBRARY) +endif (NOT TARGET METIS::METIS) + +if (METIS_INCLUDE_DIR) + set_property (TARGET METIS::METIS PROPERTY INTERFACE_INCLUDE_DIRECTORIES + ${METIS_INCLUDE_DIR}) +endif (METIS_INCLUDE_DIR) + +if (METIS_LIBRARY_RELEASE) + set_property (TARGET METIS::METIS PROPERTY IMPORTED_LOCATION_RELEASE + ${METIS_LIBRARY_RELEASE}) + set_property (TARGET METIS::METIS APPEND PROPERTY IMPORTED_CONFIGURATIONS + RELEASE) +endif (METIS_LIBRARY_RELEASE) + +if (METIS_LIBRARY_DEBUG) + set_property (TARGET METIS::METIS PROPERTY IMPORTED_LOCATION_DEBUG + ${METIS_LIBRARY_DEBUG}) + set_property (TARGET METIS::METIS APPEND PROPERTY IMPORTED_CONFIGURATIONS + DEBUG) +endif (METIS_LIBRARY_DEBUG) + +find_package_handle_standard_args (METIS REQUIRED_VARS + METIS_INCLUDE_DIR METIS_LIBRARY VERSION_VAR METIS_VERSION) diff --git a/cmake/Ceres/FindSuiteSparse.cmake b/cmake/Ceres/FindSuiteSparse.cmake new file mode 100644 index 0000000000000000000000000000000000000000..768b5df3a5f9bb23e6ed721b0f9b3cf575c3ad5a --- /dev/null +++ b/cmake/Ceres/FindSuiteSparse.cmake @@ -0,0 +1,488 @@ +# Ceres Solver - A fast non-linear least squares minimizer +# Copyright 2022 Google Inc. All rights reserved. +# http://ceres-solver.org/ +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# * Neither the name of Google Inc. nor the names of its contributors may be +# used to endorse or promote products derived from this software without +# specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# Author: alexs.mac@gmail.com (Alex Stewart) +# + +#[=======================================================================[.rst: +FindSuiteSparse +=============== + +Module for locating SuiteSparse libraries and its dependencies. + +This module defines the following variables: + +``SuiteSparse_FOUND`` + ``TRUE`` iff SuiteSparse and all dependencies have been found. + +``SuiteSparse_VERSION`` + Extracted from ``SuiteSparse_config.h`` (>= v4). + +``SuiteSparse_VERSION_MAJOR`` + Equal to 4 if ``SuiteSparse_VERSION`` = 4.2.1 + +``SuiteSparse_VERSION_MINOR`` + Equal to 2 if ``SuiteSparse_VERSION`` = 4.2.1 + +``SuiteSparse_VERSION_PATCH`` + Equal to 1 if ``SuiteSparse_VERSION`` = 4.2.1 + +The following variables control the behaviour of this module: + +``SuiteSparse_NO_CMAKE`` + Do not attempt to use the native SuiteSparse CMake package configuration. + + +Targets +------- + +The following targets define the SuiteSparse components searched for. + +``SuiteSparse::AMD`` + Symmetric Approximate Minimum Degree (AMD) + +``SuiteSparse::CAMD`` + Constrained Approximate Minimum Degree (CAMD) + +``SuiteSparse::COLAMD`` + Column Approximate Minimum Degree (COLAMD) + +``SuiteSparse::CCOLAMD`` + Constrained Column Approximate Minimum Degree (CCOLAMD) + +``SuiteSparse::CHOLMOD`` + Sparse Supernodal Cholesky Factorization and Update/Downdate (CHOLMOD) + +``SuiteSparse::SPQR`` + Multifrontal Sparse QR (SuiteSparseQR) + +``SuiteSparse::Config`` + Common configuration for all but CSparse (SuiteSparse version >= 4). + +Optional SuiteSparse dependencies: + +``METIS::METIS`` + Serial Graph Partitioning and Fill-reducing Matrix Ordering (METIS) +]=======================================================================] + +if (NOT SuiteSparse_NO_CMAKE) + find_package (SuiteSparse NO_MODULE QUIET) +endif (NOT SuiteSparse_NO_CMAKE) + +if (SuiteSparse_FOUND) + return () +endif (SuiteSparse_FOUND) + +# Push CMP0057 to enable support for IN_LIST, when cmake_minimum_required is +# set to <3.3. +cmake_policy (PUSH) +cmake_policy (SET CMP0057 NEW) + +if (NOT SuiteSparse_FIND_COMPONENTS) + set (SuiteSparse_FIND_COMPONENTS + AMD + CAMD + CCOLAMD + CHOLMOD + COLAMD + SPQR + ) + + foreach (component IN LISTS SuiteSparse_FIND_COMPONENTS) + set (SuiteSparse_FIND_REQUIRED_${component} TRUE) + endforeach (component IN LISTS SuiteSparse_FIND_COMPONENTS) +endif (NOT SuiteSparse_FIND_COMPONENTS) + +# Assume SuiteSparse was found and set it to false only if third-party +# dependencies could not be located. SuiteSparse components are handled by +# FindPackageHandleStandardArgs HANDLE_COMPONENTS option. +set (SuiteSparse_FOUND TRUE) + +include (CheckLibraryExists) + +# Config is a base component and thus always required +set (SuiteSparse_IMPLICIT_COMPONENTS Config) + +# CHOLMOD depends on AMD, CAMD, CCOLAMD, and COLAMD. +if (CHOLMOD IN_LIST SuiteSparse_FIND_COMPONENTS) + list (APPEND SuiteSparse_IMPLICIT_COMPONENTS AMD CAMD CCOLAMD COLAMD) +endif (CHOLMOD IN_LIST SuiteSparse_FIND_COMPONENTS) + +# SPQR depends on CHOLMOD. +if (SPQR IN_LIST SuiteSparse_FIND_COMPONENTS) + list (APPEND SuiteSparse_IMPLICIT_COMPONENTS CHOLMOD) +endif (SPQR IN_LIST SuiteSparse_FIND_COMPONENTS) + +# Implicit components are always required +foreach (component IN LISTS SuiteSparse_IMPLICIT_COMPONENTS) + set (SuiteSparse_FIND_REQUIRED_${component} TRUE) +endforeach (component IN LISTS SuiteSparse_IMPLICIT_COMPONENTS) + +list (APPEND SuiteSparse_FIND_COMPONENTS ${SuiteSparse_IMPLICIT_COMPONENTS}) + +# Do not list components multiple times. +list (REMOVE_DUPLICATES SuiteSparse_FIND_COMPONENTS) + +# Reset CALLERS_CMAKE_FIND_LIBRARY_PREFIXES to its value when +# FindSuiteSparse was invoked. +macro(SuiteSparse_RESET_FIND_LIBRARY_PREFIX) + if (MSVC) + set(CMAKE_FIND_LIBRARY_PREFIXES "${CALLERS_CMAKE_FIND_LIBRARY_PREFIXES}") + endif (MSVC) +endmacro(SuiteSparse_RESET_FIND_LIBRARY_PREFIX) + +# Called if we failed to find SuiteSparse or any of it's required dependencies, +# unsets all public (designed to be used externally) variables and reports +# error message at priority depending upon [REQUIRED/QUIET/] argument. +macro(SuiteSparse_REPORT_NOT_FOUND REASON_MSG) + # Will be set to FALSE by find_package_handle_standard_args + unset (SuiteSparse_FOUND) + + # Do NOT unset SuiteSparse_REQUIRED_VARS here, as it is used by + # FindPackageHandleStandardArgs() to generate the automatic error message on + # failure which highlights which components are missing. + + suitesparse_reset_find_library_prefix() + + # Note _FIND_[REQUIRED/QUIETLY] variables defined by FindPackage() + # use the camelcase library name, not uppercase. + if (SuiteSparse_FIND_QUIETLY) + message(STATUS "Failed to find SuiteSparse - " ${REASON_MSG} ${ARGN}) + elseif (SuiteSparse_FIND_REQUIRED) + message(FATAL_ERROR "Failed to find SuiteSparse - " ${REASON_MSG} ${ARGN}) + else() + # Neither QUIETLY nor REQUIRED, use no priority which emits a message + # but continues configuration and allows generation. + message("-- Failed to find SuiteSparse - " ${REASON_MSG} ${ARGN}) + endif (SuiteSparse_FIND_QUIETLY) + + # Do not call return(), s/t we keep processing if not called with REQUIRED + # and report all missing components, rather than bailing after failing to find + # the first. +endmacro(SuiteSparse_REPORT_NOT_FOUND) + +# Handle possible presence of lib prefix for libraries on MSVC, see +# also SuiteSparse_RESET_FIND_LIBRARY_PREFIX(). +if (MSVC) + # Preserve the caller's original values for CMAKE_FIND_LIBRARY_PREFIXES + # s/t we can set it back before returning. + set(CALLERS_CMAKE_FIND_LIBRARY_PREFIXES "${CMAKE_FIND_LIBRARY_PREFIXES}") + # The empty string in this list is important, it represents the case when + # the libraries have no prefix (shared libraries / DLLs). + set(CMAKE_FIND_LIBRARY_PREFIXES "lib" "" "${CMAKE_FIND_LIBRARY_PREFIXES}") +endif (MSVC) + +# Additional suffixes to try appending to each search path. +list(APPEND SuiteSparse_CHECK_PATH_SUFFIXES + suitesparse) # Windows/Ubuntu + +# Wrappers to find_path/library that pass the SuiteSparse search hints/paths. +# +# suitesparse_find_component( [FILES name1 [name2 ...]] +# [LIBRARIES name1 [name2 ...]]) +macro(suitesparse_find_component COMPONENT) + include(CMakeParseArguments) + set(MULTI_VALUE_ARGS FILES LIBRARIES) + cmake_parse_arguments(SuiteSparse_FIND_COMPONENT_${COMPONENT} + "" "" "${MULTI_VALUE_ARGS}" ${ARGN}) + + set(SuiteSparse_${COMPONENT}_FOUND TRUE) + if (SuiteSparse_FIND_COMPONENT_${COMPONENT}_FILES) + find_path(SuiteSparse_${COMPONENT}_INCLUDE_DIR + NAMES ${SuiteSparse_FIND_COMPONENT_${COMPONENT}_FILES} + PATH_SUFFIXES ${SuiteSparse_CHECK_PATH_SUFFIXES}) + if (SuiteSparse_${COMPONENT}_INCLUDE_DIR) + message(STATUS "Found ${COMPONENT} headers in: " + "${SuiteSparse_${COMPONENT}_INCLUDE_DIR}") + mark_as_advanced(SuiteSparse_${COMPONENT}_INCLUDE_DIR) + else() + # Specified headers not found. + set(SuiteSparse_${COMPONENT}_FOUND FALSE) + if (SuiteSparse_FIND_REQUIRED_${COMPONENT}) + suitesparse_report_not_found( + "Did not find ${COMPONENT} header (required SuiteSparse component).") + else() + message(STATUS "Did not find ${COMPONENT} header (optional " + "SuiteSparse component).") + # Hide optional vars from CMake GUI even if not found. + mark_as_advanced(SuiteSparse_${COMPONENT}_INCLUDE_DIR) + endif() + endif() + endif() + + if (SuiteSparse_FIND_COMPONENT_${COMPONENT}_LIBRARIES) + find_library(SuiteSparse_${COMPONENT}_LIBRARY + NAMES ${SuiteSparse_FIND_COMPONENT_${COMPONENT}_LIBRARIES} + PATH_SUFFIXES ${SuiteSparse_CHECK_PATH_SUFFIXES}) + if (SuiteSparse_${COMPONENT}_LIBRARY) + message(STATUS "Found ${COMPONENT} library: ${SuiteSparse_${COMPONENT}_LIBRARY}") + mark_as_advanced(SuiteSparse_${COMPONENT}_LIBRARY) + else () + # Specified libraries not found. + set(SuiteSparse_${COMPONENT}_FOUND FALSE) + if (SuiteSparse_FIND_REQUIRED_${COMPONENT}) + suitesparse_report_not_found( + "Did not find ${COMPONENT} library (required SuiteSparse component).") + else() + message(STATUS "Did not find ${COMPONENT} library (optional SuiteSparse " + "dependency)") + # Hide optional vars from CMake GUI even if not found. + mark_as_advanced(SuiteSparse_${COMPONENT}_LIBRARY) + endif() + endif() + endif() + + # A component can be optional (given to OPTIONAL_COMPONENTS). However, if the + # component is implicit (must be always present, such as the Config component) + # assume it be required as well. + if (SuiteSparse_FIND_REQUIRED_${COMPONENT}) + list (APPEND SuiteSparse_REQUIRED_VARS SuiteSparse_${COMPONENT}_INCLUDE_DIR) + list (APPEND SuiteSparse_REQUIRED_VARS SuiteSparse_${COMPONENT}_LIBRARY) + endif (SuiteSparse_FIND_REQUIRED_${COMPONENT}) + + # Define the target only if the include directory and the library were found + if (SuiteSparse_${COMPONENT}_INCLUDE_DIR AND SuiteSparse_${COMPONENT}_LIBRARY) + if (NOT TARGET SuiteSparse::${COMPONENT}) + add_library(SuiteSparse::${COMPONENT} IMPORTED UNKNOWN) + endif (NOT TARGET SuiteSparse::${COMPONENT}) + + set_property(TARGET SuiteSparse::${COMPONENT} PROPERTY + INTERFACE_INCLUDE_DIRECTORIES ${SuiteSparse_${COMPONENT}_INCLUDE_DIR}) + set_property(TARGET SuiteSparse::${COMPONENT} PROPERTY + IMPORTED_LOCATION ${SuiteSparse_${COMPONENT}_LIBRARY}) + endif (SuiteSparse_${COMPONENT}_INCLUDE_DIR AND SuiteSparse_${COMPONENT}_LIBRARY) +endmacro() + +# Given the number of components of SuiteSparse, and to ensure that the +# automatic failure message generated by FindPackageHandleStandardArgs() +# when not all required components are found is helpful, we maintain a list +# of all variables that must be defined for SuiteSparse to be considered found. +unset(SuiteSparse_REQUIRED_VARS) + +# BLAS. +find_package(BLAS QUIET) +if (NOT BLAS_FOUND) + suitesparse_report_not_found( + "Did not find BLAS library (required for SuiteSparse).") +endif (NOT BLAS_FOUND) + +# LAPACK. +find_package(LAPACK QUIET) +if (NOT LAPACK_FOUND) + suitesparse_report_not_found( + "Did not find LAPACK library (required for SuiteSparse).") +endif (NOT LAPACK_FOUND) + +foreach (component IN LISTS SuiteSparse_FIND_COMPONENTS) + string (TOLOWER ${component} component_library) + + if (component STREQUAL "Config") + set (component_header SuiteSparse_config.h) + set (component_library suitesparseconfig) + elseif (component STREQUAL "SPQR") + set (component_header SuiteSparseQR.hpp) + else (component STREQUAL "SPQR") + set (component_header ${component_library}.h) + endif (component STREQUAL "Config") + + suitesparse_find_component(${component} + FILES ${component_header} + LIBRARIES ${component_library}) +endforeach (component IN LISTS SuiteSparse_FIND_COMPONENTS) + +if (TARGET SuiteSparse::SPQR) + # SuiteSparseQR may be compiled with Intel Threading Building Blocks, + # we assume that if TBB is installed, SuiteSparseQR was compiled with + # support for it, this will do no harm if it wasn't. + find_package(TBB QUIET) + if (TBB_FOUND) + message(STATUS "Found Intel Thread Building Blocks (TBB) library " + "(${TBB_VERSION_MAJOR}.${TBB_VERSION_MINOR} / ${TBB_INTERFACE_VERSION}) " + "include location: ${TBB_INCLUDE_DIRS}. Assuming SuiteSparseQR was " + "compiled with TBB.") + # Add the TBB libraries to the SuiteSparseQR libraries (the only + # libraries to optionally depend on TBB). + if (TARGET TBB::tbb) + # Native TBB package configuration provides an imported target. Use it if + # available. + set_property (TARGET SuiteSparse::SPQR APPEND PROPERTY + INTERFACE_LINK_LIBRARIES TBB::tbb) + else (TARGET TBB::tbb) + set_property (TARGET SuiteSparse::SPQR APPEND PROPERTY + INTERFACE_INCLUDE_DIRECTORIES ${TBB_INCLUDE_DIRS}) + set_property (TARGET SuiteSparse::SPQR APPEND PROPERTY + INTERFACE_LINK_LIBRARIES ${TBB_LIBRARIES}) + endif (TARGET TBB::tbb) + else (TBB_FOUND) + message(STATUS "Did not find Intel TBB library, assuming SuiteSparseQR was " + "not compiled with TBB.") + endif (TBB_FOUND) +endif (TARGET SuiteSparse::SPQR) + +check_library_exists(rt shm_open "" HAVE_LIBRT) + +if (TARGET SuiteSparse::Config) + # SuiteSparse_config (SuiteSparse version >= 4) requires librt library for + # timing by default when compiled on Linux or Unix, but not on OSX (which + # does not have librt). + if (HAVE_LIBRT) + message(STATUS "Adding librt to " + "SuiteSparse_config libraries (required on Linux & Unix [not OSX] if " + "SuiteSparse is compiled with timing).") + set_property (TARGET SuiteSparse::Config APPEND PROPERTY + INTERFACE_LINK_LIBRARIES $) + else (HAVE_LIBRT) + message(STATUS "Could not find librt, but found SuiteSparse_config, " + "assuming that SuiteSparse was compiled without timing.") + endif (HAVE_LIBRT) + + # Add BLAS and LAPACK as dependencies of SuiteSparse::Config for convenience + # given that all components depend on it. + if (BLAS_FOUND) + if (TARGET BLAS::BLAS) + set_property (TARGET SuiteSparse::Config APPEND PROPERTY + INTERFACE_LINK_LIBRARIES $) + else (TARGET BLAS::BLAS) + set_property (TARGET SuiteSparse::Config APPEND PROPERTY + INTERFACE_LINK_LIBRARIES ${BLAS_LIBRARIES}) + endif (TARGET BLAS::BLAS) + endif (BLAS_FOUND) + + if (LAPACK_FOUND) + if (TARGET LAPACK::LAPACK) + set_property (TARGET SuiteSparse::Config APPEND PROPERTY + INTERFACE_LINK_LIBRARIES $) + else (TARGET LAPACK::LAPACK) + set_property (TARGET SuiteSparse::Config APPEND PROPERTY + INTERFACE_LINK_LIBRARIES ${LAPACK_LIBRARIES}) + endif (TARGET LAPACK::LAPACK) + endif (LAPACK_FOUND) + + # SuiteSparse version >= 4. + set(SuiteSparse_VERSION_FILE + ${SuiteSparse_Config_INCLUDE_DIR}/SuiteSparse_config.h) + if (NOT EXISTS ${SuiteSparse_VERSION_FILE}) + suitesparse_report_not_found( + "Could not find file: ${SuiteSparse_VERSION_FILE} containing version " + "information for >= v4 SuiteSparse installs, but SuiteSparse_config was " + "found (only present in >= v4 installs).") + else (NOT EXISTS ${SuiteSparse_VERSION_FILE}) + file(READ ${SuiteSparse_VERSION_FILE} Config_CONTENTS) + + string(REGEX MATCH "#define SUITESPARSE_MAIN_VERSION [0-9]+" + SuiteSparse_VERSION_MAJOR "${Config_CONTENTS}") + string(REGEX REPLACE "#define SUITESPARSE_MAIN_VERSION ([0-9]+)" "\\1" + SuiteSparse_VERSION_MAJOR "${SuiteSparse_VERSION_MAJOR}") + + string(REGEX MATCH "#define SUITESPARSE_SUB_VERSION [0-9]+" + SuiteSparse_VERSION_MINOR "${Config_CONTENTS}") + string(REGEX REPLACE "#define SUITESPARSE_SUB_VERSION ([0-9]+)" "\\1" + SuiteSparse_VERSION_MINOR "${SuiteSparse_VERSION_MINOR}") + + string(REGEX MATCH "#define SUITESPARSE_SUBSUB_VERSION [0-9]+" + SuiteSparse_VERSION_PATCH "${Config_CONTENTS}") + string(REGEX REPLACE "#define SUITESPARSE_SUBSUB_VERSION ([0-9]+)" "\\1" + SuiteSparse_VERSION_PATCH "${SuiteSparse_VERSION_PATCH}") + + # This is on a single line s/t CMake does not interpret it as a list of + # elements and insert ';' separators which would result in 4.;2.;1 nonsense. + set(SuiteSparse_VERSION + "${SuiteSparse_VERSION_MAJOR}.${SuiteSparse_VERSION_MINOR}.${SuiteSparse_VERSION_PATCH}") + set(SuiteSparse_VERSION_COMPONENTS 3) + endif (NOT EXISTS ${SuiteSparse_VERSION_FILE}) +endif (TARGET SuiteSparse::Config) + +# METIS (Optional dependency). +find_package (METIS) + +# CHOLMOD requires AMD CAMD CCOLAMD COLAMD +if (TARGET SuiteSparse::CHOLMOD) + # METIS is optional + if (TARGET METIS::METIS) + set_property (TARGET SuiteSparse::CHOLMOD APPEND PROPERTY + INTERFACE_LINK_LIBRARIES METIS::METIS) + endif (TARGET METIS::METIS) + + foreach (component IN ITEMS AMD CAMD CCOLAMD COLAMD) + if (TARGET SuiteSparse::${component}) + set_property (TARGET SuiteSparse::CHOLMOD APPEND PROPERTY + INTERFACE_LINK_LIBRARIES SuiteSparse::${component}) + else (TARGET SuiteSparse::${component}) + # Consider CHOLMOD not found if COLAMD cannot be found + set (SuiteSparse_CHOLMOD_FOUND FALSE) + endif (TARGET SuiteSparse::${component}) + endforeach (component IN ITEMS AMD CAMD CCOLAMD COLAMD) +endif (TARGET SuiteSparse::CHOLMOD) + +# SPQR requires CHOLMOD +if (TARGET SuiteSparse::SPQR) + if (TARGET SuiteSparse::CHOLMOD) + set_property (TARGET SuiteSparse::SPQR APPEND PROPERTY + INTERFACE_LINK_LIBRARIES SuiteSparse::CHOLMOD) + else (TARGET SuiteSparse::CHOLMOD) + # Consider SPQR not found if CHOLMOD cannot be found + set (SuiteSparse_SQPR_FOUND FALSE) + endif (TARGET SuiteSparse::CHOLMOD) +endif (TARGET SuiteSparse::SPQR) + +# Add SuiteSparse::Config as dependency to all components +if (TARGET SuiteSparse::Config) + foreach (component IN LISTS SuiteSparse_FIND_COMPONENTS) + if (component STREQUAL Config) + continue () + endif (component STREQUAL Config) + + if (TARGET SuiteSparse::${component}) + set_property (TARGET SuiteSparse::${component} APPEND PROPERTY + INTERFACE_LINK_LIBRARIES SuiteSparse::Config) + endif (TARGET SuiteSparse::${component}) + endforeach (component IN LISTS SuiteSparse_FIND_COMPONENTS) +endif (TARGET SuiteSparse::Config) + +suitesparse_reset_find_library_prefix() + +# Handle REQUIRED and QUIET arguments to FIND_PACKAGE +include(FindPackageHandleStandardArgs) +if (SuiteSparse_FOUND) + find_package_handle_standard_args(SuiteSparse + REQUIRED_VARS ${SuiteSparse_REQUIRED_VARS} + VERSION_VAR SuiteSparse_VERSION + FAIL_MESSAGE "Failed to find some/all required components of SuiteSparse." + HANDLE_COMPONENTS) +else (SuiteSparse_FOUND) + # Do not pass VERSION_VAR to FindPackageHandleStandardArgs() if we failed to + # find SuiteSparse to avoid a confusing autogenerated failure message + # that states 'not found (missing: FOO) (found version: x.y.z)'. + find_package_handle_standard_args(SuiteSparse + REQUIRED_VARS ${SuiteSparse_REQUIRED_VARS} + FAIL_MESSAGE "Failed to find some/all required components of SuiteSparse." + HANDLE_COMPONENTS) +endif (SuiteSparse_FOUND) + +# Pop CMP0057. +cmake_policy (POP) diff --git a/include/ceres/autodiff_cost_function.h b/include/ceres/autodiff_cost_function.h new file mode 100644 index 0000000000000000000000000000000000000000..cd256432a9889a3d02787cfbded016624e467127 --- /dev/null +++ b/include/ceres/autodiff_cost_function.h @@ -0,0 +1,228 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2019 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Create CostFunctions as needed by the least squares framework, with +// Jacobians computed via automatic differentiation. For more +// information on automatic differentiation, see the wikipedia article +// at http://en.wikipedia.org/wiki/Automatic_differentiation +// +// To get an auto differentiated cost function, you must define a class with a +// templated operator() (a functor) that computes the cost function in terms of +// the template parameter T. The autodiff framework substitutes appropriate +// "jet" objects for T in order to compute the derivative when necessary, but +// this is hidden, and you should write the function as if T were a scalar type +// (e.g. a double-precision floating point number). +// +// The function must write the computed value in the last argument +// (the only non-const one) and return true to indicate +// success. Please see cost_function.h for details on how the return +// value maybe used to impose simple constraints on the parameter +// block. +// +// For example, consider a scalar error e = k - x'y, where both x and y are +// two-dimensional column vector parameters, the prime sign indicates +// transposition, and k is a constant. The form of this error, which is the +// difference between a constant and an expression, is a common pattern in least +// squares problems. For example, the value x'y might be the model expectation +// for a series of measurements, where there is an instance of the cost function +// for each measurement k. +// +// The actual cost added to the total problem is e^2, or (k - x'y)^2; however, +// the squaring is implicitly done by the optimization framework. +// +// To write an auto-differentiable cost function for the above model, first +// define the object +// +// class MyScalarCostFunctor { +// MyScalarCostFunctor(double k): k_(k) {} +// +// template +// bool operator()(const T* const x , const T* const y, T* e) const { +// e[0] = T(k_) - x[0] * y[0] + x[1] * y[1]; +// return true; +// } +// +// private: +// double k_; +// }; +// +// Note that in the declaration of operator() the input parameters x and y come +// first, and are passed as const pointers to arrays of T. If there were three +// input parameters, then the third input parameter would come after y. The +// output is always the last parameter, and is also a pointer to an array. In +// the example above, e is a scalar, so only e[0] is set. +// +// Then given this class definition, the auto differentiated cost function for +// it can be constructed as follows. +// +// CostFunction* cost_function +// = new AutoDiffCostFunction( +// new MyScalarCostFunctor(1.0)); ^ ^ ^ +// | | | +// Dimension of residual -----+ | | +// Dimension of x ---------------+ | +// Dimension of y ------------------+ +// +// In this example, there is usually an instance for each measurement of k. +// +// In the instantiation above, the template parameters following +// "MyScalarCostFunctor", "1, 2, 2", describe the functor as computing a +// 1-dimensional output from two arguments, both 2-dimensional. +// +// AutoDiffCostFunction also supports cost functions with a +// runtime-determined number of residuals. For example: +// +// CostFunction* cost_function +// = new AutoDiffCostFunction( +// new CostFunctorWithDynamicNumResiduals(1.0), ^ ^ ^ +// runtime_number_of_residuals); <----+ | | | +// | | | | +// | | | | +// Actual number of residuals ------+ | | | +// Indicate dynamic number of residuals --------+ | | +// Dimension of x ------------------------------------+ | +// Dimension of y ---------------------------------------+ +// +// WARNING #1: Since the functor will get instantiated with different types for +// T, you must convert from other numeric types to T before mixing +// computations with other variables of type T. In the example above, this is +// seen where instead of using k_ directly, k_ is wrapped with T(k_). +// +// WARNING #2: A common beginner's error when first using autodiff cost +// functions is to get the sizing wrong. In particular, there is a tendency to +// set the template parameters to (dimension of residual, number of parameters) +// instead of passing a dimension parameter for *every parameter*. In the +// example above, that would be , which is missing +// the last '2' argument. Please be careful when setting the size parameters. + +#ifndef CERES_PUBLIC_AUTODIFF_COST_FUNCTION_H_ +#define CERES_PUBLIC_AUTODIFF_COST_FUNCTION_H_ + +#include + +#include "ceres/internal/autodiff.h" +#include "ceres/sized_cost_function.h" +#include "ceres/types.h" +#include "glog/logging.h" + +namespace ceres { + +// A cost function which computes the derivative of the cost with respect to +// the parameters (a.k.a. the jacobian) using an auto differentiation framework. +// The first template argument is the functor object, described in the header +// comment. The second argument is the dimension of the residual (or +// ceres::DYNAMIC to indicate it will be set at runtime), and subsequent +// arguments describe the size of the Nth parameter, one per parameter. +// +// The constructors take ownership of the cost functor. +// +// If the number of residuals (argument kNumResiduals below) is +// ceres::DYNAMIC, then the two-argument constructor must be used. The +// second constructor takes a number of residuals (in addition to the +// templated number of residuals). This allows for varying the number +// of residuals for a single autodiff cost function at runtime. +template // Number of parameters in each parameter block. +class AutoDiffCostFunction final + : public SizedCostFunction { + public: + // Takes ownership of functor by default. Uses the template-provided + // value for the number of residuals ("kNumResiduals"). + explicit AutoDiffCostFunction(CostFunctor* functor, + Ownership ownership = TAKE_OWNERSHIP) + : functor_(functor), ownership_(ownership) { + static_assert(kNumResiduals != DYNAMIC, + "Can't run the fixed-size constructor if the number of " + "residuals is set to ceres::DYNAMIC."); + } + + // Takes ownership of functor by default. Ignores the template-provided + // kNumResiduals in favor of the "num_residuals" argument provided. + // + // This allows for having autodiff cost functions which return varying + // numbers of residuals at runtime. + AutoDiffCostFunction(CostFunctor* functor, + int num_residuals, + Ownership ownership = TAKE_OWNERSHIP) + : functor_(functor), ownership_(ownership) { + static_assert(kNumResiduals == DYNAMIC, + "Can't run the dynamic-size constructor if the number of " + "residuals is not ceres::DYNAMIC."); + SizedCostFunction::set_num_residuals(num_residuals); + } + + AutoDiffCostFunction(AutoDiffCostFunction&& other) + : functor_(std::move(other.functor_)), ownership_(other.ownership_) {} + + virtual ~AutoDiffCostFunction() { + // Manually release pointer if configured to not take ownership rather than + // deleting only if ownership is taken. + // This is to stay maximally compatible to old user code which may have + // forgotten to implement a virtual destructor, from when the + // AutoDiffCostFunction always took ownership. + if (ownership_ == DO_NOT_TAKE_OWNERSHIP) { + functor_.release(); + } + } + + // Implementation details follow; clients of the autodiff cost function should + // not have to examine below here. + // + // To handle variadic cost functions, some template magic is needed. It's + // mostly hidden inside autodiff.h. + bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const override { + using ParameterDims = + typename SizedCostFunction::ParameterDims; + + if (!jacobians) { + return internal::VariadicEvaluate( + *functor_, parameters, residuals); + } + return internal::AutoDifferentiate( + *functor_, + parameters, + SizedCostFunction::num_residuals(), + residuals, + jacobians); + }; + + const CostFunctor& functor() const { return *functor_; } + + private: + std::unique_ptr functor_; + Ownership ownership_; +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_AUTODIFF_COST_FUNCTION_H_ diff --git a/include/ceres/autodiff_first_order_function.h b/include/ceres/autodiff_first_order_function.h new file mode 100644 index 0000000000000000000000000000000000000000..7c13f4239a6b8322be221b064983f06289e78e54 --- /dev/null +++ b/include/ceres/autodiff_first_order_function.h @@ -0,0 +1,151 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2019 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_PUBLIC_AUTODIFF_FIRST_ORDER_FUNCTION_H_ +#define CERES_PUBLIC_AUTODIFF_FIRST_ORDER_FUNCTION_H_ + +#include + +#include "ceres/first_order_function.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/fixed_array.h" +#include "ceres/jet.h" +#include "ceres/types.h" + +namespace ceres { + +// Create FirstOrderFunctions as needed by the GradientProblem +// framework, with gradients computed via automatic +// differentiation. For more information on automatic differentiation, +// see the wikipedia article at +// http://en.wikipedia.org/wiki/Automatic_differentiation +// +// To get an auto differentiated function, you must define a class +// with a templated operator() (a functor) that computes the cost +// function in terms of the template parameter T. The autodiff +// framework substitutes appropriate "jet" objects for T in order to +// compute the derivative when necessary, but this is hidden, and you +// should write the function as if T were a scalar type (e.g. a +// double-precision floating point number). +// +// The function must write the computed value in the last argument +// (the only non-const one) and return true to indicate +// success. +// +// For example, consider a scalar error e = x'y - a, where both x and y are +// two-dimensional column vector parameters, the prime sign indicates +// transposition, and a is a constant. +// +// To write an auto-differentiable FirstOrderFunction for the above model, first +// define the object +// +// class QuadraticCostFunctor { +// public: +// explicit QuadraticCostFunctor(double a) : a_(a) {} +// template +// bool operator()(const T* const xy, T* cost) const { +// const T* const x = xy; +// const T* const y = xy + 2; +// *cost = x[0] * y[0] + x[1] * y[1] - T(a_); +// return true; +// } +// +// private: +// double a_; +// }; +// +// Note that in the declaration of operator() the input parameters xy come +// first, and are passed as const pointers to arrays of T. The +// output is the last parameter. +// +// Then given this class definition, the auto differentiated FirstOrderFunction +// for it can be constructed as follows. +// +// FirstOrderFunction* function = +// new AutoDiffFirstOrderFunction( +// new QuadraticCostFunctor(1.0))); +// +// In the instantiation above, the template parameters following +// "QuadraticCostFunctor", "4", describe the functor as computing a +// 1-dimensional output from a four dimensional vector. +// +// WARNING: Since the functor will get instantiated with different types for +// T, you must convert from other numeric types to T before mixing +// computations with other variables of type T. In the example above, this is +// seen where instead of using a_ directly, a_ is wrapped with T(a_). + +template +class AutoDiffFirstOrderFunction final : public FirstOrderFunction { + public: + // Takes ownership of functor. + explicit AutoDiffFirstOrderFunction(FirstOrderFunctor* functor) + : functor_(functor) { + static_assert(kNumParameters > 0, "kNumParameters must be positive"); + } + + bool Evaluate(const double* const parameters, + double* cost, + double* gradient) const override { + if (gradient == nullptr) { + return (*functor_)(parameters, cost); + } + + using JetT = Jet; + internal::FixedArray x(kNumParameters); + for (int i = 0; i < kNumParameters; ++i) { + x[i].a = parameters[i]; + x[i].v.setZero(); + x[i].v[i] = 1.0; + } + + JetT output; + output.a = kImpossibleValue; + output.v.setConstant(kImpossibleValue); + + if (!(*functor_)(x.data(), &output)) { + return false; + } + + *cost = output.a; + VectorRef(gradient, kNumParameters) = output.v; + return true; + } + + int NumParameters() const override { return kNumParameters; } + + const FirstOrderFunctor& functor() const { return *functor_; } + + private: + std::unique_ptr functor_; +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_AUTODIFF_FIRST_ORDER_FUNCTION_H_ diff --git a/include/ceres/autodiff_local_parameterization.h b/include/ceres/autodiff_local_parameterization.h new file mode 100644 index 0000000000000000000000000000000000000000..5f9b04d06707ca08553aa2a523e8ee6b65b91417 --- /dev/null +++ b/include/ceres/autodiff_local_parameterization.h @@ -0,0 +1,158 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2019 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sergey.vfx@gmail.com (Sergey Sharybin) +// mierle@gmail.com (Keir Mierle) +// sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_PUBLIC_AUTODIFF_LOCAL_PARAMETERIZATION_H_ +#define CERES_PUBLIC_AUTODIFF_LOCAL_PARAMETERIZATION_H_ + +#include + +#include "ceres/internal/autodiff.h" +#include "ceres/local_parameterization.h" + +namespace ceres { + +// WARNING: LocalParameterizations are deprecated, so is +// AutoDiffLocalParameterization. They will be removed from Ceres Solver in +// version 2.2.0. Please use Manifolds and AutoDiffManifold instead. + +// Create local parameterization with Jacobians computed via automatic +// differentiation. For more information on local parameterizations, +// see include/ceres/local_parameterization.h +// +// To get an auto differentiated local parameterization, you must define +// a class with a templated operator() (a functor) that computes +// +// x_plus_delta = Plus(x, delta); +// +// the template parameter T. The autodiff framework substitutes appropriate +// "Jet" objects for T in order to compute the derivative when necessary, but +// this is hidden, and you should write the function as if T were a scalar type +// (e.g. a double-precision floating point number). +// +// The function must write the computed value in the last argument (the only +// non-const one) and return true to indicate success. +// +// For example, Quaternions have a three dimensional local +// parameterization. It's plus operation can be implemented as (taken +// from internal/ceres/auto_diff_local_parameterization_test.cc) +// +// struct QuaternionPlus { +// template +// bool operator()(const T* x, const T* delta, T* x_plus_delta) const { +// const T squared_norm_delta = +// delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]; +// +// T q_delta[4]; +// if (squared_norm_delta > T(0.0)) { +// T norm_delta = sqrt(squared_norm_delta); +// const T sin_delta_by_delta = sin(norm_delta) / norm_delta; +// q_delta[0] = cos(norm_delta); +// q_delta[1] = sin_delta_by_delta * delta[0]; +// q_delta[2] = sin_delta_by_delta * delta[1]; +// q_delta[3] = sin_delta_by_delta * delta[2]; +// } else { +// // We do not just use q_delta = [1,0,0,0] here because that is a +// // constant and when used for automatic differentiation will +// // lead to a zero derivative. Instead we take a first order +// // approximation and evaluate it at zero. +// q_delta[0] = T(1.0); +// q_delta[1] = delta[0]; +// q_delta[2] = delta[1]; +// q_delta[3] = delta[2]; +// } +// +// QuaternionProduct(q_delta, x, x_plus_delta); +// return true; +// } +// }; +// +// Then given this struct, the auto differentiated local +// parameterization can now be constructed as +// +// LocalParameterization* local_parameterization = +// new AutoDiffLocalParameterization; +// | | +// Global Size ---------------+ | +// Local Size -------------------+ +// +// WARNING: Since the functor will get instantiated with different types for +// T, you must to convert from other numeric types to T before mixing +// computations with other variables of type T. In the example above, this is +// seen where instead of using k_ directly, k_ is wrapped with T(k_). + +template +class CERES_DEPRECATED_WITH_MSG("Use AutoDiffManifold instead.") + AutoDiffLocalParameterization : public LocalParameterization { + public: + AutoDiffLocalParameterization() : functor_(new Functor()) {} + + // Takes ownership of functor. + explicit AutoDiffLocalParameterization(Functor* functor) + : functor_(functor) {} + + bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const override { + return (*functor_)(x, delta, x_plus_delta); + } + + bool ComputeJacobian(const double* x, double* jacobian) const override { + double zero_delta[kLocalSize]; + for (int i = 0; i < kLocalSize; ++i) { + zero_delta[i] = 0.0; + } + + double x_plus_delta[kGlobalSize]; + for (int i = 0; i < kGlobalSize; ++i) { + x_plus_delta[i] = 0.0; + } + + const double* parameter_ptrs[2] = {x, zero_delta}; + double* jacobian_ptrs[2] = {nullptr, jacobian}; + return internal::AutoDifferentiate< + kGlobalSize, + internal::StaticParameterDims>( + *functor_, parameter_ptrs, kGlobalSize, x_plus_delta, jacobian_ptrs); + } + + int GlobalSize() const override { return kGlobalSize; } + int LocalSize() const override { return kLocalSize; } + + const Functor& functor() const { return *functor_; } + + private: + std::unique_ptr functor_; +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_AUTODIFF_LOCAL_PARAMETERIZATION_H_ diff --git a/include/ceres/autodiff_manifold.h b/include/ceres/autodiff_manifold.h new file mode 100644 index 0000000000000000000000000000000000000000..3063e19e8023c8944b9424b25e2b2aa453439c59 --- /dev/null +++ b/include/ceres/autodiff_manifold.h @@ -0,0 +1,259 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2022 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_PUBLIC_AUTODIFF_MANIFOLD_H_ +#define CERES_PUBLIC_AUTODIFF_MANIFOLD_H_ + +#include + +#include "ceres/internal/autodiff.h" +#include "ceres/manifold.h" + +namespace ceres { + +// Create a Manifold with Jacobians computed via automatic differentiation. For +// more information on manifolds, see include/ceres/manifold.h +// +// To get an auto differentiated manifold, you must define a class/struct with +// templated Plus and Minus functions that compute +// +// x_plus_delta = Plus(x, delta); +// y_minus_x = Minus(y, x); +// +// Where, x, y and x_plus_y are vectors on the manifold in the ambient space (so +// they are kAmbientSize vectors) and delta, y_minus_x are vectors in the +// tangent space (so they are kTangentSize vectors). +// +// The Functor should have the signature: +// +// struct Functor { +// template +// bool Plus(const T* x, const T* delta, T* x_plus_delta) const; +// +// template +// bool Minus(const T* y, const T* x, T* y_minus_x) const; +// }; +// +// Observe that the Plus and Minus operations are templated on the parameter T. +// The autodiff framework substitutes appropriate "Jet" objects for T in order +// to compute the derivative when necessary. This is the same mechanism that is +// used to compute derivatives when using AutoDiffCostFunction. +// +// Plus and Minus should return true if the computation is successful and false +// otherwise, in which case the result will not be used. +// +// Given this Functor, the corresponding Manifold can be constructed as: +// +// AutoDiffManifold manifold; +// +// As a concrete example consider the case of Quaternions. Quaternions form a +// three dimensional manifold embedded in R^4, i.e. they have an ambient +// dimension of 4 and their tangent space has dimension 3. The following Functor +// (taken from autodiff_manifold_test.cc) defines the Plus and Minus operations +// on the Quaternion manifold: +// +// NOTE: The following is only used for illustration purposes. Ceres Solver +// ships with optimized production grade QuaternionManifold implementation. See +// manifold.h. +// +// This functor assumes that the quaternions are laid out as [w,x,y,z] in +// memory, i.e. the real or scalar part is the first coordinate. +// +// struct QuaternionFunctor { +// template +// bool Plus(const T* x, const T* delta, T* x_plus_delta) const { +// const T squared_norm_delta = +// delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]; +// +// T q_delta[4]; +// if (squared_norm_delta > T(0.0)) { +// T norm_delta = sqrt(squared_norm_delta); +// const T sin_delta_by_delta = sin(norm_delta) / norm_delta; +// q_delta[0] = cos(norm_delta); +// q_delta[1] = sin_delta_by_delta * delta[0]; +// q_delta[2] = sin_delta_by_delta * delta[1]; +// q_delta[3] = sin_delta_by_delta * delta[2]; +// } else { +// // We do not just use q_delta = [1,0,0,0] here because that is a +// // constant and when used for automatic differentiation will +// // lead to a zero derivative. Instead we take a first order +// // approximation and evaluate it at zero. +// q_delta[0] = T(1.0); +// q_delta[1] = delta[0]; +// q_delta[2] = delta[1]; +// q_delta[3] = delta[2]; +// } +// +// QuaternionProduct(q_delta, x, x_plus_delta); +// return true; +// } +// +// template +// bool Minus(const T* y, const T* x, T* y_minus_x) const { +// T minus_x[4] = {x[0], -x[1], -x[2], -x[3]}; +// T ambient_y_minus_x[4]; +// QuaternionProduct(y, minus_x, ambient_y_minus_x); +// T u_norm = sqrt(ambient_y_minus_x[1] * ambient_y_minus_x[1] + +// ambient_y_minus_x[2] * ambient_y_minus_x[2] + +// ambient_y_minus_x[3] * ambient_y_minus_x[3]); +// if (u_norm > 0.0) { +// T theta = atan2(u_norm, ambient_y_minus_x[0]); +// y_minus_x[0] = theta * ambient_y_minus_x[1] / u_norm; +// y_minus_x[1] = theta * ambient_y_minus_x[2] / u_norm; +// y_minus_x[2] = theta * ambient_y_minus_x[3] / u_norm; +// } else { +// // We do not use [0,0,0] here because even though the value part is +// // a constant, the derivative part is not. +// y_minus_x[0] = ambient_y_minus_x[1]; +// y_minus_x[1] = ambient_y_minus_x[2]; +// y_minus_x[2] = ambient_y_minus_x[3]; +// } +// return true; +// } +// }; +// +// Then given this struct, the auto differentiated Quaternion Manifold can now +// be constructed as +// +// Manifold* manifold = new AutoDiffManifold; + +template +class AutoDiffManifold final : public Manifold { + public: + AutoDiffManifold() : functor_(std::make_unique()) {} + + // Takes ownership of functor. + explicit AutoDiffManifold(Functor* functor) : functor_(functor) {} + + int AmbientSize() const override { return kAmbientSize; } + int TangentSize() const override { return kTangentSize; } + + bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const override { + return functor_->Plus(x, delta, x_plus_delta); + } + + bool PlusJacobian(const double* x, double* jacobian) const override; + + bool Minus(const double* y, + const double* x, + double* y_minus_x) const override { + return functor_->Minus(y, x, y_minus_x); + } + + bool MinusJacobian(const double* x, double* jacobian) const override; + + const Functor& functor() const { return *functor_; } + + private: + std::unique_ptr functor_; +}; + +namespace internal { + +// The following two helper structs are needed to interface the Plus and Minus +// methods of the ManifoldFunctor with the automatic differentiation which +// expects a Functor with operator(). +template +struct PlusWrapper { + explicit PlusWrapper(const Functor& functor) : functor(functor) {} + template + bool operator()(const T* x, const T* delta, T* x_plus_delta) const { + return functor.Plus(x, delta, x_plus_delta); + } + const Functor& functor; +}; + +template +struct MinusWrapper { + explicit MinusWrapper(const Functor& functor) : functor(functor) {} + template + bool operator()(const T* y, const T* x, T* y_minus_x) const { + return functor.Minus(y, x, y_minus_x); + } + const Functor& functor; +}; +} // namespace internal + +template +bool AutoDiffManifold::PlusJacobian( + const double* x, double* jacobian) const { + double zero_delta[kTangentSize]; + for (int i = 0; i < kTangentSize; ++i) { + zero_delta[i] = 0.0; + } + + double x_plus_delta[kAmbientSize]; + for (int i = 0; i < kAmbientSize; ++i) { + x_plus_delta[i] = 0.0; + } + + const double* parameter_ptrs[2] = {x, zero_delta}; + + // PlusJacobian is D_2 Plus(x,0) so we only need to compute the Jacobian + // w.r.t. the second argument. + double* jacobian_ptrs[2] = {nullptr, jacobian}; + return internal::AutoDifferentiate< + kAmbientSize, + internal::StaticParameterDims>( + internal::PlusWrapper(*functor_), + parameter_ptrs, + kAmbientSize, + x_plus_delta, + jacobian_ptrs); +} + +template +bool AutoDiffManifold::MinusJacobian( + const double* x, double* jacobian) const { + double y_minus_x[kTangentSize]; + for (int i = 0; i < kTangentSize; ++i) { + y_minus_x[i] = 0.0; + } + + const double* parameter_ptrs[2] = {x, x}; + + // MinusJacobian is D_1 Minus(x,x), so we only need to compute the Jacobian + // w.r.t. the first argument. + double* jacobian_ptrs[2] = {jacobian, nullptr}; + return internal::AutoDifferentiate< + kTangentSize, + internal::StaticParameterDims>( + internal::MinusWrapper(*functor_), + parameter_ptrs, + kTangentSize, + y_minus_x, + jacobian_ptrs); +} + +} // namespace ceres + +#endif // CERES_PUBLIC_AUTODIFF_MANIFOLD_H_ diff --git a/include/ceres/c_api.h b/include/ceres/c_api.h new file mode 100644 index 0000000000000000000000000000000000000000..1be8ca2e0773e424e0137468377f43469216f445 --- /dev/null +++ b/include/ceres/c_api.h @@ -0,0 +1,148 @@ +/* Ceres Solver - A fast non-linear least squares minimizer + * Copyright 2019 Google Inc. All rights reserved. + * http://ceres-solver.org/ + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * - Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * - Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * - Neither the name of Google Inc. nor the names of its contributors may be + * used to endorse or promote products derived from this software without + * specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: mierle@gmail.com (Keir Mierle) + * + * A minimal C API for Ceres. Not all functionality is included. This API is + * not intended for clients of Ceres, but is instead intended for easing the + * process of binding Ceres to other languages. + * + * Currently this is a work in progress. + */ + +#ifndef CERES_PUBLIC_C_API_H_ +#define CERES_PUBLIC_C_API_H_ + +// clang-format off +#include "ceres/internal/export.h" +#include "ceres/internal/disable_warnings.h" +// clang-format on + +#ifdef __cplusplus +extern "C" { +#endif + +/* Init the Ceres private data. Must be called before anything else. */ +CERES_EXPORT void ceres_init(); + +/* Equivalent to CostFunction::Evaluate() in the C++ API. + * + * The user may keep private information inside the opaque user_data object. + * The pointer here is the same one passed in the ceres_add_residual_block(). + */ +typedef int (*ceres_cost_function_t)(void* user_data, + double** parameters, + double* residuals, + double** jacobians); + +/* Equivalent to LossFunction::Evaluate() from the C++ API. */ +typedef void (*ceres_loss_function_t)(void* user_data, + double squared_norm, + double out[3]); + +/* Create callback data for Ceres' stock loss functions. + * + * Ceres has several loss functions available by default, and these functions + * expose those to the C API. To use the stock loss functions, call + * ceres_create_*_loss_data(), which internally creates an instance of one of + * the stock loss functions (for example ceres::CauchyLoss), and pass the + * returned "loss_function_data" along with the ceres_stock_loss_function to + * ceres_add_residual_block(). + * + * For example: + * + * void* cauchy_loss_function_data = + * ceres_create_cauchy_loss_function_data(1.2, 0.0); + * ceres_problem_add_residual_block( + * problem, + * my_cost_function, + * my_cost_function_data, + * ceres_stock_loss_function, + * cauchy_loss_function_data, + * 1, + * 2, + * parameter_sizes, + * parameter_pointers); + * ... + * ceres_free_stock_loss_function_data(cauchy_loss_function_data); + * + * See loss_function.h for the details of each loss function. + */ +CERES_EXPORT void* ceres_create_huber_loss_function_data(double a); +CERES_EXPORT void* ceres_create_softl1_loss_function_data(double a); +CERES_EXPORT void* ceres_create_cauchy_loss_function_data(double a); +CERES_EXPORT void* ceres_create_arctan_loss_function_data(double a); +CERES_EXPORT void* ceres_create_tolerant_loss_function_data(double a, double b); + +/* Free the given stock loss function data. */ +CERES_EXPORT void ceres_free_stock_loss_function_data(void* loss_function_data); + +/* This is an implementation of ceres_loss_function_t contained within Ceres + * itself, intended as a way to access the various stock Ceres loss functions + * from the C API. This should be passed to ceres_add_residual() below, in + * combination with a user_data pointer generated by + * ceres_create_stock_loss_function() above. */ +CERES_EXPORT void ceres_stock_loss_function(void* user_data, + double squared_norm, + double out[3]); + +/* Equivalent to Problem from the C++ API. */ +struct ceres_problem_s; +typedef struct ceres_problem_s ceres_problem_t; + +struct ceres_residual_block_id_s; +typedef struct ceres_residual_block_id_s ceres_residual_block_id_t; + +/* Create and destroy a problem */ +/* TODO(keir): Add options for the problem. */ +CERES_EXPORT ceres_problem_t* ceres_create_problem(); +CERES_EXPORT void ceres_free_problem(ceres_problem_t* problem); + +/* Add a residual block. */ +CERES_EXPORT ceres_residual_block_id_t* ceres_problem_add_residual_block( + ceres_problem_t* problem, + ceres_cost_function_t cost_function, + void* cost_function_data, + ceres_loss_function_t loss_function, + void* loss_function_data, + int num_residuals, + int num_parameter_blocks, + int* parameter_block_sizes, + double** parameters); + +CERES_EXPORT void ceres_solve(ceres_problem_t* problem); + +/* TODO(keir): Figure out a way to pass a config in. */ + +#ifdef __cplusplus +} +#endif + +#include "ceres/internal/reenable_warnings.h" + +#endif /* CERES_PUBLIC_C_API_H_ */ diff --git a/include/ceres/ceres.h b/include/ceres/ceres.h new file mode 100644 index 0000000000000000000000000000000000000000..c32477d42543bb054f174ec6d784734aaf7f36b4 --- /dev/null +++ b/include/ceres/ceres.h @@ -0,0 +1,74 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2022 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// This is a forwarding header containing the public symbols exported from +// Ceres. Anything in the "ceres" namespace is available for use. + +#ifndef CERES_PUBLIC_CERES_H_ +#define CERES_PUBLIC_CERES_H_ + +#include "ceres/autodiff_cost_function.h" +#include "ceres/autodiff_first_order_function.h" +#include "ceres/autodiff_local_parameterization.h" +#include "ceres/autodiff_manifold.h" +#include "ceres/conditioned_cost_function.h" +#include "ceres/context.h" +#include "ceres/cost_function.h" +#include "ceres/cost_function_to_functor.h" +#include "ceres/covariance.h" +#include "ceres/crs_matrix.h" +#include "ceres/dynamic_autodiff_cost_function.h" +#include "ceres/dynamic_cost_function.h" +#include "ceres/dynamic_cost_function_to_functor.h" +#include "ceres/dynamic_numeric_diff_cost_function.h" +#include "ceres/evaluation_callback.h" +#include "ceres/first_order_function.h" +#include "ceres/gradient_checker.h" +#include "ceres/gradient_problem.h" +#include "ceres/gradient_problem_solver.h" +#include "ceres/iteration_callback.h" +#include "ceres/jet.h" +#include "ceres/line_manifold.h" +#include "ceres/local_parameterization.h" +#include "ceres/loss_function.h" +#include "ceres/manifold.h" +#include "ceres/numeric_diff_cost_function.h" +#include "ceres/numeric_diff_first_order_function.h" +#include "ceres/numeric_diff_options.h" +#include "ceres/ordered_groups.h" +#include "ceres/problem.h" +#include "ceres/product_manifold.h" +#include "ceres/sized_cost_function.h" +#include "ceres/solver.h" +#include "ceres/sphere_manifold.h" +#include "ceres/types.h" +#include "ceres/version.h" + +#endif // CERES_PUBLIC_CERES_H_ diff --git a/include/ceres/conditioned_cost_function.h b/include/ceres/conditioned_cost_function.h new file mode 100644 index 0000000000000000000000000000000000000000..e4c3decbfd5b32af4ee3353bba76b43f010f4f87 --- /dev/null +++ b/include/ceres/conditioned_cost_function.h @@ -0,0 +1,101 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2019 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: wjr@google.com (William Rucklidge) +// +// This file contains a cost function that can apply a transformation to +// each residual value before they are square-summed. + +#ifndef CERES_PUBLIC_CONDITIONED_COST_FUNCTION_H_ +#define CERES_PUBLIC_CONDITIONED_COST_FUNCTION_H_ + +#include +#include + +#include "ceres/cost_function.h" +#include "ceres/internal/disable_warnings.h" +#include "ceres/types.h" + +namespace ceres { + +// This class allows you to apply different conditioning to the residual +// values of a wrapped cost function. An example where this is useful is +// where you have an existing cost function that produces N values, but you +// want the total cost to be something other than just the sum of these +// squared values - maybe you want to apply a different scaling to some +// values, to change their contribution to the cost. +// +// Usage: +// +// // my_cost_function produces N residuals +// CostFunction* my_cost_function = ... +// CHECK_EQ(N, my_cost_function->num_residuals()); +// vector conditioners; +// +// // Make N 1x1 cost functions (1 parameter, 1 residual) +// CostFunction* f_1 = ... +// conditioners.push_back(f_1); +// ... +// CostFunction* f_N = ... +// conditioners.push_back(f_N); +// ConditionedCostFunction* ccf = +// new ConditionedCostFunction(my_cost_function, conditioners); +// +// Now ccf's residual i (i=0..N-1) will be passed though the i'th conditioner. +// +// ccf_residual[i] = f_i(my_cost_function_residual[i]) +// +// and the Jacobian will be affected appropriately. +class CERES_EXPORT ConditionedCostFunction final : public CostFunction { + public: + // Builds a cost function based on a wrapped cost function, and a + // per-residual conditioner. Takes ownership of all of the wrapped cost + // functions, or not, depending on the ownership parameter. Conditioners + // may be nullptr, in which case the corresponding residual is not modified. + // + // The conditioners can repeat. + ConditionedCostFunction(CostFunction* wrapped_cost_function, + const std::vector& conditioners, + Ownership ownership); + ~ConditionedCostFunction() override; + + bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const override; + + private: + std::unique_ptr wrapped_cost_function_; + std::vector conditioners_; + Ownership ownership_; +}; + +} // namespace ceres + +#include "ceres/internal/reenable_warnings.h" + +#endif // CERES_PUBLIC_CONDITIONED_COST_FUNCTION_H_ diff --git a/include/ceres/context.h b/include/ceres/context.h new file mode 100644 index 0000000000000000000000000000000000000000..6c6e8f4c95359e5b905c4afc284cb88e9bdfb3a6 --- /dev/null +++ b/include/ceres/context.h @@ -0,0 +1,58 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2019 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: vitus@google.com (Michael Vitus) + +#ifndef CERES_PUBLIC_CONTEXT_H_ +#define CERES_PUBLIC_CONTEXT_H_ + +#include "ceres/internal/export.h" + +namespace ceres { + +// A global context for processing data in Ceres. This provides a mechanism to +// allow Ceres to reuse items that are expensive to create between multiple +// calls; for example, thread pools. The same Context can be used on multiple +// Problems, either serially or in parallel. When using it with multiple +// Problems at the same time, they may end up contending for resources +// (e.g. threads) managed by the Context. +class CERES_EXPORT Context { + public: + Context(); + Context(const Context&) = delete; + void operator=(const Context&) = delete; + + virtual ~Context(); + + // Creates a context object and the caller takes ownership. + static Context* Create(); +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_CONTEXT_H_ diff --git a/include/ceres/cost_function.h b/include/ceres/cost_function.h new file mode 100644 index 0000000000000000000000000000000000000000..fef972b75af43b615241697d891e67fb8832a7eb --- /dev/null +++ b/include/ceres/cost_function.h @@ -0,0 +1,144 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2019 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// keir@google.m (Keir Mierle) +// +// This is the interface through which the least squares solver accesses the +// residual and Jacobian of the least squares problem. Users are expected to +// subclass CostFunction to define their own terms in the least squares problem. +// +// It is recommended that users define templated residual functors for use as +// arguments for AutoDiffCostFunction (see autodiff_cost_function.h), instead of +// directly implementing the CostFunction interface. This often results in both +// shorter code and faster execution than hand-coded derivatives. However, +// specialized cases may demand direct implementation of the lower-level +// CostFunction interface; for example, this is true when calling legacy code +// which is not templated on numeric types. + +#ifndef CERES_PUBLIC_COST_FUNCTION_H_ +#define CERES_PUBLIC_COST_FUNCTION_H_ + +#include +#include + +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" + +namespace ceres { + +// This class implements the computation of the cost (a.k.a. residual) terms as +// a function of the input (control) variables, and is the interface for users +// to describe their least squares problem to Ceres. In other words, this is the +// modeling layer between users and the Ceres optimizer. The signature of the +// function (number and sizes of input parameter blocks and number of outputs) +// is stored in parameter_block_sizes_ and num_residuals_ respectively. User +// code inheriting from this class is expected to set these two members with the +// corresponding accessors. This information will be verified by the Problem +// when added with AddResidualBlock(). +class CERES_EXPORT CostFunction { + public: + CostFunction(); + CostFunction(const CostFunction&) = delete; + void operator=(const CostFunction&) = delete; + + virtual ~CostFunction(); + + // Inputs: + // + // parameters is an array of pointers to arrays containing the + // various parameter blocks. parameters has the same number of + // elements as parameter_block_sizes_. Parameter blocks are in the + // same order as parameter_block_sizes_.i.e., + // + // parameters_[i] = double[parameter_block_sizes_[i]] + // + // Outputs: + // + // residuals is an array of size num_residuals_. + // + // jacobians is an array of size parameter_block_sizes_ containing + // pointers to storage for jacobian blocks corresponding to each + // parameter block. Jacobian blocks are in the same order as + // parameter_block_sizes, i.e. jacobians[i], is an + // array that contains num_residuals_* parameter_block_sizes_[i] + // elements. Each jacobian block is stored in row-major order, i.e., + // + // jacobians[i][r*parameter_block_size_[i] + c] = + // d residual[r] / d parameters[i][c] + // + // If jacobians is nullptr, then no derivatives are returned; this is + // the case when computing cost only. If jacobians[i] is nullptr, then + // the jacobian block corresponding to the i'th parameter block must + // not to be returned. + // + // The return value indicates whether the computation of the + // residuals and/or jacobians was successful or not. + // + // This can be used to communicate numerical failures in jacobian + // computations for instance. + // + // A more interesting and common use is to impose constraints on the + // parameters. If the initial values of the parameter blocks satisfy + // the constraints, then returning false whenever the constraints + // are not satisfied will prevent the solver from moving into the + // infeasible region. This is not a very sophisticated mechanism for + // enforcing constraints, but is often good enough. + // + // Note that it is important that the initial values of the + // parameter block must be feasible, otherwise the solver will + // declare a numerical problem at iteration 0. + virtual bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const = 0; + + const std::vector& parameter_block_sizes() const { + return parameter_block_sizes_; + } + + int num_residuals() const { return num_residuals_; } + + protected: + std::vector* mutable_parameter_block_sizes() { + return ¶meter_block_sizes_; + } + + void set_num_residuals(int num_residuals) { num_residuals_ = num_residuals; } + + private: + // Cost function signature metadata: number of inputs & their sizes, + // number of outputs (residuals). + std::vector parameter_block_sizes_; + int num_residuals_; +}; + +} // namespace ceres + +#include "ceres/internal/reenable_warnings.h" + +#endif // CERES_PUBLIC_COST_FUNCTION_H_ diff --git a/include/ceres/cost_function_to_functor.h b/include/ceres/cost_function_to_functor.h new file mode 100644 index 0000000000000000000000000000000000000000..08a8050c5f8701085b3087f957d1b445c8c9bea2 --- /dev/null +++ b/include/ceres/cost_function_to_functor.h @@ -0,0 +1,171 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2019 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// CostFunctionToFunctor is an adapter class that allows users to use +// SizedCostFunction objects in templated functors which are to be used for +// automatic differentiation. This allows the user to seamlessly mix +// analytic, numeric and automatic differentiation. +// +// For example, let us assume that +// +// class IntrinsicProjection : public SizedCostFunction<2, 5, 3> { +// public: +// IntrinsicProjection(const double* observation); +// bool Evaluate(double const* const* parameters, +// double* residuals, +// double** jacobians) const override; +// }; +// +// is a cost function that implements the projection of a point in its +// local coordinate system onto its image plane and subtracts it from +// the observed point projection. It can compute its residual and +// jacobians either via analytic or numerical differentiation. +// +// Now we would like to compose the action of this CostFunction with +// the action of camera extrinsics, i.e., rotation and +// translation. Say we have a templated function +// +// template +// void RotateAndTranslatePoint(const T* rotation, +// const T* translation, +// const T* point, +// T* result); +// +// Then we can now do the following, +// +// struct CameraProjection { +// CameraProjection(const double* observation) +// : intrinsic_projection_(new IntrinsicProjection(observation)) { +// } +// template +// bool operator()(const T* rotation, +// const T* translation, +// const T* intrinsics, +// const T* point, +// T* residual) const { +// T transformed_point[3]; +// RotateAndTranslatePoint(rotation, translation, point, transformed_point); +// +// // Note that we call intrinsic_projection_, just like it was +// // any other templated functor. +// +// return intrinsic_projection_(intrinsics, transformed_point, residual); +// } +// +// private: +// CostFunctionToFunctor<2,5,3> intrinsic_projection_; +// }; + +#ifndef CERES_PUBLIC_COST_FUNCTION_TO_FUNCTOR_H_ +#define CERES_PUBLIC_COST_FUNCTION_TO_FUNCTOR_H_ + +#include +#include +#include +#include +#include + +#include "ceres/cost_function.h" +#include "ceres/dynamic_cost_function_to_functor.h" +#include "ceres/internal/export.h" +#include "ceres/internal/fixed_array.h" +#include "ceres/internal/parameter_dims.h" +#include "ceres/types.h" +#include "glog/logging.h" + +namespace ceres { + +template +class CostFunctionToFunctor { + public: + // Takes ownership of cost_function. + explicit CostFunctionToFunctor(CostFunction* cost_function) + : cost_functor_(cost_function) { + CHECK(cost_function != nullptr); + CHECK(kNumResiduals > 0 || kNumResiduals == DYNAMIC); + + const std::vector& parameter_block_sizes = + cost_function->parameter_block_sizes(); + const int num_parameter_blocks = ParameterDims::kNumParameterBlocks; + CHECK_EQ(static_cast(parameter_block_sizes.size()), + num_parameter_blocks); + + if (parameter_block_sizes.size() == num_parameter_blocks) { + for (int block = 0; block < num_parameter_blocks; ++block) { + CHECK_EQ(ParameterDims::GetDim(block), parameter_block_sizes[block]) + << "Parameter block size missmatch. The specified static parameter " + "block dimension does not match the one from the cost function."; + } + } + + CHECK_EQ(accumulate( + parameter_block_sizes.begin(), parameter_block_sizes.end(), 0), + ParameterDims::kNumParameters); + } + + template + bool operator()(const T* p1, Ts*... ps) const { + // Add one because of residual block. + static_assert(sizeof...(Ts) + 1 == ParameterDims::kNumParameterBlocks + 1, + "Invalid number of parameter blocks specified."); + + auto params = std::make_tuple(p1, ps...); + + // Extract residual pointer from params. The residual pointer is the + // last pointer. + constexpr int kResidualIndex = ParameterDims::kNumParameterBlocks; + T* residuals = std::get(params); + + // Extract parameter block pointers from params. + using Indices = + std::make_integer_sequence; + std::array parameter_blocks = + GetParameterPointers(params, Indices()); + + return cost_functor_(parameter_blocks.data(), residuals); + } + + private: + using ParameterDims = internal::StaticParameterDims; + + template + static std::array + GetParameterPointers(const Tuple& paramPointers, + std::integer_sequence) { + return std::array{ + {std::get(paramPointers)...}}; + } + + DynamicCostFunctionToFunctor cost_functor_; +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_COST_FUNCTION_TO_FUNCTOR_H_ diff --git a/include/ceres/covariance.h b/include/ceres/covariance.h new file mode 100644 index 0000000000000000000000000000000000000000..60bcc80b80f282a878176a1c2f92783c50902573 --- /dev/null +++ b/include/ceres/covariance.h @@ -0,0 +1,459 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2019 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_PUBLIC_COVARIANCE_H_ +#define CERES_PUBLIC_COVARIANCE_H_ + +#include +#include +#include + +#include "ceres/internal/config.h" +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" +#include "ceres/types.h" + +namespace ceres { + +class Problem; + +namespace internal { +class CovarianceImpl; +} // namespace internal + +// WARNING +// ======= +// It is very easy to use this class incorrectly without understanding +// the underlying mathematics. Please read and understand the +// documentation completely before attempting to use it. +// +// +// This class allows the user to evaluate the covariance for a +// non-linear least squares problem and provides random access to its +// blocks +// +// Background +// ========== +// One way to assess the quality of the solution returned by a +// non-linear least squares solver is to analyze the covariance of the +// solution. +// +// Let us consider the non-linear regression problem +// +// y = f(x) + N(0, I) +// +// i.e., the observation y is a random non-linear function of the +// independent variable x with mean f(x) and identity covariance. Then +// the maximum likelihood estimate of x given observations y is the +// solution to the non-linear least squares problem: +// +// x* = arg min_x |f(x) - y|^2 +// +// And the covariance of x* is given by +// +// C(x*) = inverse[J'(x*)J(x*)] +// +// Here J(x*) is the Jacobian of f at x*. The above formula assumes +// that J(x*) has full column rank. +// +// If J(x*) is rank deficient, then the covariance matrix C(x*) is +// also rank deficient and is given by +// +// C(x*) = pseudoinverse[J'(x*)J(x*)] +// +// Note that in the above, we assumed that the covariance +// matrix for y was identity. This is an important assumption. If this +// is not the case and we have +// +// y = f(x) + N(0, S) +// +// Where S is a positive semi-definite matrix denoting the covariance +// of y, then the maximum likelihood problem to be solved is +// +// x* = arg min_x f'(x) inverse[S] f(x) +// +// and the corresponding covariance estimate of x* is given by +// +// C(x*) = inverse[J'(x*) inverse[S] J(x*)] +// +// So, if it is the case that the observations being fitted to have a +// covariance matrix not equal to identity, then it is the user's +// responsibility that the corresponding cost functions are correctly +// scaled, e.g. in the above case the cost function for this problem +// should evaluate S^{-1/2} f(x) instead of just f(x), where S^{-1/2} +// is the inverse square root of the covariance matrix S. +// +// This class allows the user to evaluate the covariance for a +// non-linear least squares problem and provides random access to its +// blocks. The computation assumes that the CostFunctions compute +// residuals such that their covariance is identity. +// +// Since the computation of the covariance matrix requires computing +// the inverse of a potentially large matrix, this can involve a +// rather large amount of time and memory. However, it is usually the +// case that the user is only interested in a small part of the +// covariance matrix. Quite often just the block diagonal. This class +// allows the user to specify the parts of the covariance matrix that +// she is interested in and then uses this information to only compute +// and store those parts of the covariance matrix. +// +// Rank of the Jacobian +// -------------------- +// As we noted above, if the jacobian is rank deficient, then the +// inverse of J'J is not defined and instead a pseudo inverse needs to +// be computed. +// +// The rank deficiency in J can be structural -- columns which are +// always known to be zero or numerical -- depending on the exact +// values in the Jacobian. +// +// Structural rank deficiency occurs when the problem contains +// parameter blocks that are constant. This class correctly handles +// structural rank deficiency like that. +// +// Numerical rank deficiency, where the rank of the matrix cannot be +// predicted by its sparsity structure and requires looking at its +// numerical values is more complicated. Here again there are two +// cases. +// +// a. The rank deficiency arises from overparameterization. e.g., a +// four dimensional quaternion used to parameterize SO(3), which is +// a three dimensional manifold. In cases like this, the user should +// use an appropriate LocalParameterization/Manifold. Not only will this lead +// to better numerical behaviour of the Solver, it will also expose +// the rank deficiency to the Covariance object so that it can +// handle it correctly. +// +// b. More general numerical rank deficiency in the Jacobian +// requires the computation of the so called Singular Value +// Decomposition (SVD) of J'J. We do not know how to do this for +// large sparse matrices efficiently. For small and moderate sized +// problems this is done using dense linear algebra. +// +// Gauge Invariance +// ---------------- +// In structure from motion (3D reconstruction) problems, the +// reconstruction is ambiguous up to a similarity transform. This is +// known as a Gauge Ambiguity. Handling Gauges correctly requires the +// use of SVD or custom inversion algorithms. For small problems the +// user can use the dense algorithm. For more details see +// +// Ken-ichi Kanatani, Daniel D. Morris: Gauges and gauge +// transformations for uncertainty description of geometric structure +// with indeterminacy. IEEE Transactions on Information Theory 47(5): +// 2017-2028 (2001) +// +// Example Usage +// ============= +// +// double x[3]; +// double y[2]; +// +// Problem problem; +// problem.AddParameterBlock(x, 3); +// problem.AddParameterBlock(y, 2); +// +// +// +// Covariance::Options options; +// Covariance covariance(options); +// +// std::vector> covariance_blocks; +// covariance_blocks.push_back(make_pair(x, x)); +// covariance_blocks.push_back(make_pair(y, y)); +// covariance_blocks.push_back(make_pair(x, y)); +// +// CHECK(covariance.Compute(covariance_blocks, &problem)); +// +// double covariance_xx[3 * 3]; +// double covariance_yy[2 * 2]; +// double covariance_xy[3 * 2]; +// covariance.GetCovarianceBlock(x, x, covariance_xx) +// covariance.GetCovarianceBlock(y, y, covariance_yy) +// covariance.GetCovarianceBlock(x, y, covariance_xy) +// +class CERES_EXPORT Covariance { + public: + struct CERES_EXPORT Options { + // Sparse linear algebra library to use when a sparse matrix + // factorization is being used to compute the covariance matrix. + // + // Currently this only applies to SPARSE_QR. + SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type = +#if !defined(CERES_NO_SUITESPARSE) + SUITE_SPARSE; +#else + // Eigen's QR factorization is always available. + EIGEN_SPARSE; +#endif + + // Ceres supports two different algorithms for covariance + // estimation, which represent different tradeoffs in speed, + // accuracy and reliability. + // + // 1. DENSE_SVD uses Eigen's JacobiSVD to perform the + // computations. It computes the singular value decomposition + // + // U * D * V' = J + // + // and then uses it to compute the pseudo inverse of J'J as + // + // pseudoinverse[J'J] = V * pseudoinverse[D^2] * V' + // + // It is an accurate but slow method and should only be used + // for small to moderate sized problems. It can handle + // full-rank as well as rank deficient Jacobians. + // + // 2. SPARSE_QR uses the sparse QR factorization algorithm + // to compute the decomposition + // + // Q * R = J + // + // [J'J]^-1 = [R'*R]^-1 + // + // SPARSE_QR is not capable of computing the covariance if the + // Jacobian is rank deficient. Depending on the value of + // Covariance::Options::sparse_linear_algebra_library_type, either + // Eigen's Sparse QR factorization algorithm will be used or + // SuiteSparse's high performance SuiteSparseQR algorithm will be + // used. + CovarianceAlgorithmType algorithm_type = SPARSE_QR; + + // If the Jacobian matrix is near singular, then inverting J'J + // will result in unreliable results, e.g, if + // + // J = [1.0 1.0 ] + // [1.0 1.0000001 ] + // + // which is essentially a rank deficient matrix, we have + // + // inv(J'J) = [ 2.0471e+14 -2.0471e+14] + // [-2.0471e+14 2.0471e+14] + // + // This is not a useful result. Therefore, by default + // Covariance::Compute will return false if a rank deficient + // Jacobian is encountered. How rank deficiency is detected + // depends on the algorithm being used. + // + // 1. DENSE_SVD + // + // min_sigma / max_sigma < sqrt(min_reciprocal_condition_number) + // + // where min_sigma and max_sigma are the minimum and maxiumum + // singular values of J respectively. + // + // 2. SPARSE_QR + // + // rank(J) < num_col(J) + // + // Here rank(J) is the estimate of the rank of J returned by the + // sparse QR factorization algorithm. It is a fairly reliable + // indication of rank deficiency. + // + double min_reciprocal_condition_number = 1e-14; + + // When using DENSE_SVD, the user has more control in dealing with + // singular and near singular covariance matrices. + // + // As mentioned above, when the covariance matrix is near + // singular, instead of computing the inverse of J'J, the + // Moore-Penrose pseudoinverse of J'J should be computed. + // + // If J'J has the eigen decomposition (lambda_i, e_i), where + // lambda_i is the i^th eigenvalue and e_i is the corresponding + // eigenvector, then the inverse of J'J is + // + // inverse[J'J] = sum_i e_i e_i' / lambda_i + // + // and computing the pseudo inverse involves dropping terms from + // this sum that correspond to small eigenvalues. + // + // How terms are dropped is controlled by + // min_reciprocal_condition_number and null_space_rank. + // + // If null_space_rank is non-negative, then the smallest + // null_space_rank eigenvalue/eigenvectors are dropped + // irrespective of the magnitude of lambda_i. If the ratio of the + // smallest non-zero eigenvalue to the largest eigenvalue in the + // truncated matrix is still below + // min_reciprocal_condition_number, then the Covariance::Compute() + // will fail and return false. + // + // Setting null_space_rank = -1 drops all terms for which + // + // lambda_i / lambda_max < min_reciprocal_condition_number. + // + // This option has no effect on the SUITE_SPARSE_QR and + // EIGEN_SPARSE_QR algorithms. + int null_space_rank = 0; + + int num_threads = 1; + + // Even though the residual blocks in the problem may contain loss + // functions, setting apply_loss_function to false will turn off + // the application of the loss function to the output of the cost + // function and in turn its effect on the covariance. + // + // TODO(sameergaarwal): Expand this based on Jim's experiments. + bool apply_loss_function = true; + }; + + explicit Covariance(const Options& options); + ~Covariance(); + + // Compute a part of the covariance matrix. + // + // The vector covariance_blocks, indexes into the covariance matrix + // block-wise using pairs of parameter blocks. This allows the + // covariance estimation algorithm to only compute and store these + // blocks. + // + // Since the covariance matrix is symmetric, if the user passes + // (block1, block2), then GetCovarianceBlock can be called with + // block1, block2 as well as block2, block1. + // + // covariance_blocks cannot contain duplicates. Bad things will + // happen if they do. + // + // Note that the list of covariance_blocks is only used to determine + // what parts of the covariance matrix are computed. The full + // Jacobian is used to do the computation, i.e. they do not have an + // impact on what part of the Jacobian is used for computation. + // + // The return value indicates the success or failure of the + // covariance computation. Please see the documentation for + // Covariance::Options for more on the conditions under which this + // function returns false. + bool Compute(const std::vector>& + covariance_blocks, + Problem* problem); + + // Compute a part of the covariance matrix. + // + // The vector parameter_blocks contains the parameter blocks that + // are used for computing the covariance matrix. From this vector + // all covariance pairs are generated. This allows the covariance + // estimation algorithm to only compute and store these blocks. + // + // parameter_blocks cannot contain duplicates. Bad things will + // happen if they do. + // + // Note that the list of covariance_blocks is only used to determine + // what parts of the covariance matrix are computed. The full + // Jacobian is used to do the computation, i.e. they do not have an + // impact on what part of the Jacobian is used for computation. + // + // The return value indicates the success or failure of the + // covariance computation. Please see the documentation for + // Covariance::Options for more on the conditions under which this + // function returns false. + bool Compute(const std::vector& parameter_blocks, + Problem* problem); + + // Return the block of the cross-covariance matrix corresponding to + // parameter_block1 and parameter_block2. + // + // Compute must be called before the first call to + // GetCovarianceBlock and the pair OR the pair must have been present in the vector + // covariance_blocks when Compute was called. Otherwise + // GetCovarianceBlock will return false. + // + // covariance_block must point to a memory location that can store a + // parameter_block1_size x parameter_block2_size matrix. The + // returned covariance will be a row-major matrix. + bool GetCovarianceBlock(const double* parameter_block1, + const double* parameter_block2, + double* covariance_block) const; + + // Return the block of the cross-covariance matrix corresponding to + // parameter_block1 and parameter_block2. + // Returns cross-covariance in the tangent space if a local + // parameterization is associated with either parameter block; + // else returns cross-covariance in the ambient space. + // + // Compute must be called before the first call to + // GetCovarianceBlock and the pair OR the pair must have been present in the vector + // covariance_blocks when Compute was called. Otherwise + // GetCovarianceBlock will return false. + // + // covariance_block must point to a memory location that can store a + // parameter_block1_local_size x parameter_block2_local_size matrix. The + // returned covariance will be a row-major matrix. + bool GetCovarianceBlockInTangentSpace(const double* parameter_block1, + const double* parameter_block2, + double* covariance_block) const; + + // Return the covariance matrix corresponding to all parameter_blocks. + // + // Compute must be called before calling GetCovarianceMatrix and all + // parameter_blocks must have been present in the vector + // parameter_blocks when Compute was called. Otherwise + // GetCovarianceMatrix returns false. + // + // covariance_matrix must point to a memory location that can store + // the size of the covariance matrix. The covariance matrix will be + // a square matrix whose row and column count is equal to the sum of + // the sizes of the individual parameter blocks. The covariance + // matrix will be a row-major matrix. + bool GetCovarianceMatrix(const std::vector& parameter_blocks, + double* covariance_matrix) const; + + // Return the covariance matrix corresponding to parameter_blocks + // in the tangent space if a local parameterization is associated + // with one of the parameter blocks else returns the covariance + // matrix in the ambient space. + // + // Compute must be called before calling GetCovarianceMatrix and all + // parameter_blocks must have been present in the vector + // parameters_blocks when Compute was called. Otherwise + // GetCovarianceMatrix returns false. + // + // covariance_matrix must point to a memory location that can store + // the size of the covariance matrix. The covariance matrix will be + // a square matrix whose row and column count is equal to the sum of + // the sizes of the tangent spaces of the individual parameter + // blocks. The covariance matrix will be a row-major matrix. + bool GetCovarianceMatrixInTangentSpace( + const std::vector& parameter_blocks, + double* covariance_matrix) const; + + private: + std::unique_ptr impl_; +}; + +} // namespace ceres + +#include "ceres/internal/reenable_warnings.h" + +#endif // CERES_PUBLIC_COVARIANCE_H_ diff --git a/include/ceres/crs_matrix.h b/include/ceres/crs_matrix.h new file mode 100644 index 0000000000000000000000000000000000000000..faa0f9885289a2419bc4f2e985810a98d97bef4f --- /dev/null +++ b/include/ceres/crs_matrix.h @@ -0,0 +1,87 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2019 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_PUBLIC_CRS_MATRIX_H_ +#define CERES_PUBLIC_CRS_MATRIX_H_ + +#include + +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" + +namespace ceres { + +// A compressed row sparse matrix used primarily for communicating the +// Jacobian matrix to the user. +struct CERES_EXPORT CRSMatrix { + CRSMatrix() = default; + + int num_rows{0}; + int num_cols{0}; + + // A compressed row matrix stores its contents in three arrays, + // rows, cols and values. + // + // rows is a num_rows + 1 sized array that points into the cols and + // values array. For each row i: + // + // cols[rows[i]] ... cols[rows[i + 1] - 1] are the indices of the + // non-zero columns of row i. + // + // values[rows[i]] .. values[rows[i + 1] - 1] are the values of the + // corresponding entries. + // + // cols and values contain as many entries as there are non-zeros in + // the matrix. + // + // e.g, consider the 3x4 sparse matrix + // + // [ 0 10 0 4 ] + // [ 0 2 -3 2 ] + // [ 1 2 0 0 ] + // + // The three arrays will be: + // + // + // -row0- ---row1--- -row2- + // rows = [ 0, 2, 5, 7] + // cols = [ 1, 3, 1, 2, 3, 0, 1] + // values = [10, 4, 2, -3, 2, 1, 2] + + std::vector cols; + std::vector rows; + std::vector values; +}; + +} // namespace ceres + +#include "ceres/internal/reenable_warnings.h" + +#endif // CERES_PUBLIC_CRS_MATRIX_H_ diff --git a/include/ceres/cubic_interpolation.h b/include/ceres/cubic_interpolation.h new file mode 100644 index 0000000000000000000000000000000000000000..3ca6b11b407471bc362207ab71df34d294d3a2ca --- /dev/null +++ b/include/ceres/cubic_interpolation.h @@ -0,0 +1,436 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2019 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_PUBLIC_CUBIC_INTERPOLATION_H_ +#define CERES_PUBLIC_CUBIC_INTERPOLATION_H_ + +#include "Eigen/Core" +#include "ceres/internal/export.h" +#include "glog/logging.h" + +namespace ceres { + +// Given samples from a function sampled at four equally spaced points, +// +// p0 = f(-1) +// p1 = f(0) +// p2 = f(1) +// p3 = f(2) +// +// Evaluate the cubic Hermite spline (also known as the Catmull-Rom +// spline) at a point x that lies in the interval [0, 1]. +// +// This is also the interpolation kernel (for the case of a = 0.5) as +// proposed by R. Keys, in: +// +// "Cubic convolution interpolation for digital image processing". +// IEEE Transactions on Acoustics, Speech, and Signal Processing +// 29 (6): 1153-1160. +// +// For more details see +// +// http://en.wikipedia.org/wiki/Cubic_Hermite_spline +// http://en.wikipedia.org/wiki/Bicubic_interpolation +// +// f if not nullptr will contain the interpolated function values. +// dfdx if not nullptr will contain the interpolated derivative values. +template +void CubicHermiteSpline(const Eigen::Matrix& p0, + const Eigen::Matrix& p1, + const Eigen::Matrix& p2, + const Eigen::Matrix& p3, + const double x, + double* f, + double* dfdx) { + using VType = Eigen::Matrix; + const VType a = 0.5 * (-p0 + 3.0 * p1 - 3.0 * p2 + p3); + const VType b = 0.5 * (2.0 * p0 - 5.0 * p1 + 4.0 * p2 - p3); + const VType c = 0.5 * (-p0 + p2); + const VType d = p1; + + // Use Horner's rule to evaluate the function value and its + // derivative. + + // f = ax^3 + bx^2 + cx + d + if (f != nullptr) { + Eigen::Map(f, kDataDimension) = d + x * (c + x * (b + x * a)); + } + + // dfdx = 3ax^2 + 2bx + c + if (dfdx != nullptr) { + Eigen::Map(dfdx, kDataDimension) = c + x * (2.0 * b + 3.0 * a * x); + } +} + +// Given as input an infinite one dimensional grid, which provides the +// following interface. +// +// class Grid { +// public: +// enum { DATA_DIMENSION = 2; }; +// void GetValue(int n, double* f) const; +// }; +// +// Here, GetValue gives the value of a function f (possibly vector +// valued) for any integer n. +// +// The enum DATA_DIMENSION indicates the dimensionality of the +// function being interpolated. For example if you are interpolating +// rotations in axis-angle format over time, then DATA_DIMENSION = 3. +// +// CubicInterpolator uses cubic Hermite splines to produce a smooth +// approximation to it that can be used to evaluate the f(x) and f'(x) +// at any point on the real number line. +// +// For more details on cubic interpolation see +// +// http://en.wikipedia.org/wiki/Cubic_Hermite_spline +// +// Example usage: +// +// const double data[] = {1.0, 2.0, 5.0, 6.0}; +// Grid1D grid(data, 0, 4); +// CubicInterpolator> interpolator(grid); +// double f, dfdx; +// interpolator.Evaluator(1.5, &f, &dfdx); +template +class CubicInterpolator { + public: + explicit CubicInterpolator(const Grid& grid) : grid_(grid) { + // The + casts the enum into an int before doing the + // comparison. It is needed to prevent + // "-Wunnamed-type-template-args" related errors. + CHECK_GE(+Grid::DATA_DIMENSION, 1); + } + + void Evaluate(double x, double* f, double* dfdx) const { + const int n = std::floor(x); + Eigen::Matrix p0, p1, p2, p3; + grid_.GetValue(n - 1, p0.data()); + grid_.GetValue(n, p1.data()); + grid_.GetValue(n + 1, p2.data()); + grid_.GetValue(n + 2, p3.data()); + CubicHermiteSpline(p0, p1, p2, p3, x - n, f, dfdx); + } + + // The following two Evaluate overloads are needed for interfacing + // with automatic differentiation. The first is for when a scalar + // evaluation is done, and the second one is for when Jets are used. + void Evaluate(const double& x, double* f) const { Evaluate(x, f, nullptr); } + + template + void Evaluate(const JetT& x, JetT* f) const { + double fx[Grid::DATA_DIMENSION], dfdx[Grid::DATA_DIMENSION]; + Evaluate(x.a, fx, dfdx); + for (int i = 0; i < Grid::DATA_DIMENSION; ++i) { + f[i].a = fx[i]; + f[i].v = dfdx[i] * x.v; + } + } + + private: + const Grid& grid_; +}; + +// An object that implements an infinite one dimensional grid needed +// by the CubicInterpolator where the source of the function values is +// an array of type T on the interval +// +// [begin, ..., end - 1] +// +// Since the input array is finite and the grid is infinite, values +// outside this interval needs to be computed. Grid1D uses the value +// from the nearest edge. +// +// The function being provided can be vector valued, in which case +// kDataDimension > 1. The dimensional slices of the function maybe +// interleaved, or they maybe stacked, i.e, if the function has +// kDataDimension = 2, if kInterleaved = true, then it is stored as +// +// f01, f02, f11, f12 .... +// +// and if kInterleaved = false, then it is stored as +// +// f01, f11, .. fn1, f02, f12, .. , fn2 +// +template +struct Grid1D { + public: + enum { DATA_DIMENSION = kDataDimension }; + + Grid1D(const T* data, const int begin, const int end) + : data_(data), begin_(begin), end_(end), num_values_(end - begin) { + CHECK_LT(begin, end); + } + + EIGEN_STRONG_INLINE void GetValue(const int n, double* f) const { + const int idx = (std::min)((std::max)(begin_, n), end_ - 1) - begin_; + if (kInterleaved) { + for (int i = 0; i < kDataDimension; ++i) { + f[i] = static_cast(data_[kDataDimension * idx + i]); + } + } else { + for (int i = 0; i < kDataDimension; ++i) { + f[i] = static_cast(data_[i * num_values_ + idx]); + } + } + } + + private: + const T* data_; + const int begin_; + const int end_; + const int num_values_; +}; + +// Given as input an infinite two dimensional grid like object, which +// provides the following interface: +// +// struct Grid { +// enum { DATA_DIMENSION = 1 }; +// void GetValue(int row, int col, double* f) const; +// }; +// +// Where, GetValue gives us the value of a function f (possibly vector +// valued) for any pairs of integers (row, col), and the enum +// DATA_DIMENSION indicates the dimensionality of the function being +// interpolated. For example if you are interpolating a color image +// with three channels (Red, Green & Blue), then DATA_DIMENSION = 3. +// +// BiCubicInterpolator uses the cubic convolution interpolation +// algorithm of R. Keys, to produce a smooth approximation to it that +// can be used to evaluate the f(r,c), df(r, c)/dr and df(r,c)/dc at +// any point in the real plane. +// +// For more details on the algorithm used here see: +// +// "Cubic convolution interpolation for digital image processing". +// Robert G. Keys, IEEE Trans. on Acoustics, Speech, and Signal +// Processing 29 (6): 1153-1160, 1981. +// +// http://en.wikipedia.org/wiki/Cubic_Hermite_spline +// http://en.wikipedia.org/wiki/Bicubic_interpolation +// +// Example usage: +// +// const double data[] = {1.0, 3.0, -1.0, 4.0, +// 3.6, 2.1, 4.2, 2.0, +// 2.0, 1.0, 3.1, 5.2}; +// Grid2D grid(data, 3, 4); +// BiCubicInterpolator> interpolator(grid); +// double f, dfdr, dfdc; +// interpolator.Evaluate(1.2, 2.5, &f, &dfdr, &dfdc); + +template +class BiCubicInterpolator { + public: + explicit BiCubicInterpolator(const Grid& grid) : grid_(grid) { + // The + casts the enum into an int before doing the + // comparison. It is needed to prevent + // "-Wunnamed-type-template-args" related errors. + CHECK_GE(+Grid::DATA_DIMENSION, 1); + } + + // Evaluate the interpolated function value and/or its + // derivative. Uses the nearest point on the grid boundary if r or + // c is out of bounds. + void Evaluate( + double r, double c, double* f, double* dfdr, double* dfdc) const { + // BiCubic interpolation requires 16 values around the point being + // evaluated. We will use pij, to indicate the elements of the + // 4x4 grid of values. + // + // col + // p00 p01 p02 p03 + // row p10 p11 p12 p13 + // p20 p21 p22 p23 + // p30 p31 p32 p33 + // + // The point (r,c) being evaluated is assumed to lie in the square + // defined by p11, p12, p22 and p21. + + const int row = std::floor(r); + const int col = std::floor(c); + + Eigen::Matrix p0, p1, p2, p3; + + // Interpolate along each of the four rows, evaluating the function + // value and the horizontal derivative in each row. + Eigen::Matrix f0, f1, f2, f3; + Eigen::Matrix df0dc, df1dc, df2dc, df3dc; + + grid_.GetValue(row - 1, col - 1, p0.data()); + grid_.GetValue(row - 1, col, p1.data()); + grid_.GetValue(row - 1, col + 1, p2.data()); + grid_.GetValue(row - 1, col + 2, p3.data()); + CubicHermiteSpline( + p0, p1, p2, p3, c - col, f0.data(), df0dc.data()); + + grid_.GetValue(row, col - 1, p0.data()); + grid_.GetValue(row, col, p1.data()); + grid_.GetValue(row, col + 1, p2.data()); + grid_.GetValue(row, col + 2, p3.data()); + CubicHermiteSpline( + p0, p1, p2, p3, c - col, f1.data(), df1dc.data()); + + grid_.GetValue(row + 1, col - 1, p0.data()); + grid_.GetValue(row + 1, col, p1.data()); + grid_.GetValue(row + 1, col + 1, p2.data()); + grid_.GetValue(row + 1, col + 2, p3.data()); + CubicHermiteSpline( + p0, p1, p2, p3, c - col, f2.data(), df2dc.data()); + + grid_.GetValue(row + 2, col - 1, p0.data()); + grid_.GetValue(row + 2, col, p1.data()); + grid_.GetValue(row + 2, col + 1, p2.data()); + grid_.GetValue(row + 2, col + 2, p3.data()); + CubicHermiteSpline( + p0, p1, p2, p3, c - col, f3.data(), df3dc.data()); + + // Interpolate vertically the interpolated value from each row and + // compute the derivative along the columns. + CubicHermiteSpline(f0, f1, f2, f3, r - row, f, dfdr); + if (dfdc != nullptr) { + // Interpolate vertically the derivative along the columns. + CubicHermiteSpline( + df0dc, df1dc, df2dc, df3dc, r - row, dfdc, nullptr); + } + } + + // The following two Evaluate overloads are needed for interfacing + // with automatic differentiation. The first is for when a scalar + // evaluation is done, and the second one is for when Jets are used. + void Evaluate(const double& r, const double& c, double* f) const { + Evaluate(r, c, f, nullptr, nullptr); + } + + template + void Evaluate(const JetT& r, const JetT& c, JetT* f) const { + double frc[Grid::DATA_DIMENSION]; + double dfdr[Grid::DATA_DIMENSION]; + double dfdc[Grid::DATA_DIMENSION]; + Evaluate(r.a, c.a, frc, dfdr, dfdc); + for (int i = 0; i < Grid::DATA_DIMENSION; ++i) { + f[i].a = frc[i]; + f[i].v = dfdr[i] * r.v + dfdc[i] * c.v; + } + } + + private: + const Grid& grid_; +}; + +// An object that implements an infinite two dimensional grid needed +// by the BiCubicInterpolator where the source of the function values +// is an grid of type T on the grid +// +// [(row_start, col_start), ..., (row_start, col_end - 1)] +// [ ... ] +// [(row_end - 1, col_start), ..., (row_end - 1, col_end - 1)] +// +// Since the input grid is finite and the grid is infinite, values +// outside this interval needs to be computed. Grid2D uses the value +// from the nearest edge. +// +// The function being provided can be vector valued, in which case +// kDataDimension > 1. The data maybe stored in row or column major +// format and the various dimensional slices of the function maybe +// interleaved, or they maybe stacked, i.e, if the function has +// kDataDimension = 2, is stored in row-major format and if +// kInterleaved = true, then it is stored as +// +// f001, f002, f011, f012, ... +// +// A commonly occuring example are color images (RGB) where the three +// channels are stored interleaved. +// +// If kInterleaved = false, then it is stored as +// +// f001, f011, ..., fnm1, f002, f012, ... +template +struct Grid2D { + public: + enum { DATA_DIMENSION = kDataDimension }; + + Grid2D(const T* data, + const int row_begin, + const int row_end, + const int col_begin, + const int col_end) + : data_(data), + row_begin_(row_begin), + row_end_(row_end), + col_begin_(col_begin), + col_end_(col_end), + num_rows_(row_end - row_begin), + num_cols_(col_end - col_begin), + num_values_(num_rows_ * num_cols_) { + CHECK_GE(kDataDimension, 1); + CHECK_LT(row_begin, row_end); + CHECK_LT(col_begin, col_end); + } + + EIGEN_STRONG_INLINE void GetValue(const int r, const int c, double* f) const { + const int row_idx = + (std::min)((std::max)(row_begin_, r), row_end_ - 1) - row_begin_; + const int col_idx = + (std::min)((std::max)(col_begin_, c), col_end_ - 1) - col_begin_; + + const int n = (kRowMajor) ? num_cols_ * row_idx + col_idx + : num_rows_ * col_idx + row_idx; + + if (kInterleaved) { + for (int i = 0; i < kDataDimension; ++i) { + f[i] = static_cast(data_[kDataDimension * n + i]); + } + } else { + for (int i = 0; i < kDataDimension; ++i) { + f[i] = static_cast(data_[i * num_values_ + n]); + } + } + } + + private: + const T* data_; + const int row_begin_; + const int row_end_; + const int col_begin_; + const int col_end_; + const int num_rows_; + const int num_cols_; + const int num_values_; +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_CUBIC_INTERPOLATOR_H_ diff --git a/include/ceres/dynamic_autodiff_cost_function.h b/include/ceres/dynamic_autodiff_cost_function.h new file mode 100644 index 0000000000000000000000000000000000000000..c21d0517f27f5cfa1e9d37d9aa12b61cb9e23a70 --- /dev/null +++ b/include/ceres/dynamic_autodiff_cost_function.h @@ -0,0 +1,274 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2019 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// mierle@gmail.com (Keir Mierle) + +#ifndef CERES_PUBLIC_DYNAMIC_AUTODIFF_COST_FUNCTION_H_ +#define CERES_PUBLIC_DYNAMIC_AUTODIFF_COST_FUNCTION_H_ + +#include +#include +#include +#include + +#include "ceres/dynamic_cost_function.h" +#include "ceres/internal/fixed_array.h" +#include "ceres/jet.h" +#include "ceres/types.h" +#include "glog/logging.h" + +namespace ceres { + +// This autodiff implementation differs from the one found in +// autodiff_cost_function.h by supporting autodiff on cost functions +// with variable numbers of parameters with variable sizes. With the +// other implementation, all the sizes (both the number of parameter +// blocks and the size of each block) must be fixed at compile time. +// +// The functor API differs slightly from the API for fixed size +// autodiff; the expected interface for the cost functors is: +// +// struct MyCostFunctor { +// template +// bool operator()(T const* const* parameters, T* residuals) const { +// // Use parameters[i] to access the i'th parameter block. +// } +// }; +// +// Since the sizing of the parameters is done at runtime, you must +// also specify the sizes after creating the dynamic autodiff cost +// function. For example: +// +// DynamicAutoDiffCostFunction cost_function( +// new MyCostFunctor()); +// cost_function.AddParameterBlock(5); +// cost_function.AddParameterBlock(10); +// cost_function.SetNumResiduals(21); +// +// Under the hood, the implementation evaluates the cost function +// multiple times, computing a small set of the derivatives (four by +// default, controlled by the Stride template parameter) with each +// pass. There is a tradeoff with the size of the passes; you may want +// to experiment with the stride. +template +class DynamicAutoDiffCostFunction final : public DynamicCostFunction { + public: + // Takes ownership by default. + explicit DynamicAutoDiffCostFunction(CostFunctor* functor, + Ownership ownership = TAKE_OWNERSHIP) + : functor_(functor), ownership_(ownership) {} + + DynamicAutoDiffCostFunction(DynamicAutoDiffCostFunction&& other) + : functor_(std::move(other.functor_)), ownership_(other.ownership_) {} + + ~DynamicAutoDiffCostFunction() override { + // Manually release pointer if configured to not take ownership + // rather than deleting only if ownership is taken. This is to + // stay maximally compatible to old user code which may have + // forgotten to implement a virtual destructor, from when the + // AutoDiffCostFunction always took ownership. + if (ownership_ == DO_NOT_TAKE_OWNERSHIP) { + functor_.release(); + } + } + + bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const override { + CHECK_GT(num_residuals(), 0) + << "You must call DynamicAutoDiffCostFunction::SetNumResiduals() " + << "before DynamicAutoDiffCostFunction::Evaluate()."; + + if (jacobians == nullptr) { + return (*functor_)(parameters, residuals); + } + + // The difficulty with Jets, as implemented in Ceres, is that they were + // originally designed for strictly compile-sized use. At this point, there + // is a large body of code that assumes inside a cost functor it is + // acceptable to do e.g. T(1.5) and get an appropriately sized jet back. + // + // Unfortunately, it is impossible to communicate the expected size of a + // dynamically sized jet to the static instantiations that existing code + // depends on. + // + // To work around this issue, the solution here is to evaluate the + // jacobians in a series of passes, each one computing Stride * + // num_residuals() derivatives. This is done with small, fixed-size jets. + const int num_parameter_blocks = + static_cast(parameter_block_sizes().size()); + const int num_parameters = std::accumulate( + parameter_block_sizes().begin(), parameter_block_sizes().end(), 0); + + // Allocate scratch space for the strided evaluation. + using JetT = Jet; + internal::FixedArray input_jets( + num_parameters); + internal::FixedArray output_jets( + num_residuals()); + + // Make the parameter pack that is sent to the functor (reused). + internal::FixedArray*> jet_parameters( + num_parameter_blocks, nullptr); + int num_active_parameters = 0; + + // To handle constant parameters between non-constant parameter blocks, the + // start position --- a raw parameter index --- of each contiguous block of + // non-constant parameters is recorded in start_derivative_section. + std::vector start_derivative_section; + bool in_derivative_section = false; + int parameter_cursor = 0; + + // Discover the derivative sections and set the parameter values. + for (int i = 0; i < num_parameter_blocks; ++i) { + jet_parameters[i] = &input_jets[parameter_cursor]; + + const int parameter_block_size = parameter_block_sizes()[i]; + if (jacobians[i] != nullptr) { + if (!in_derivative_section) { + start_derivative_section.push_back(parameter_cursor); + in_derivative_section = true; + } + + num_active_parameters += parameter_block_size; + } else { + in_derivative_section = false; + } + + for (int j = 0; j < parameter_block_size; ++j, parameter_cursor++) { + input_jets[parameter_cursor].a = parameters[i][j]; + } + } + + if (num_active_parameters == 0) { + return (*functor_)(parameters, residuals); + } + // When `num_active_parameters % Stride != 0` then it can be the case + // that `active_parameter_count < Stride` while parameter_cursor is less + // than the total number of parameters and with no remaining non-constant + // parameter blocks. Pushing parameter_cursor (the total number of + // parameters) as a final entry to start_derivative_section is required + // because if a constant parameter block is encountered after the + // last non-constant block then current_derivative_section is incremented + // and would otherwise index an invalid position in + // start_derivative_section. Setting the final element to the total number + // of parameters means that this can only happen at most once in the loop + // below. + start_derivative_section.push_back(parameter_cursor); + + // Evaluate all of the strides. Each stride is a chunk of the derivative to + // evaluate, typically some size proportional to the size of the SIMD + // registers of the CPU. + int num_strides = static_cast( + ceil(num_active_parameters / static_cast(Stride))); + + int current_derivative_section = 0; + int current_derivative_section_cursor = 0; + + for (int pass = 0; pass < num_strides; ++pass) { + // Set most of the jet components to zero, except for + // non-constant #Stride parameters. + const int initial_derivative_section = current_derivative_section; + const int initial_derivative_section_cursor = + current_derivative_section_cursor; + + int active_parameter_count = 0; + parameter_cursor = 0; + + for (int i = 0; i < num_parameter_blocks; ++i) { + for (int j = 0; j < parameter_block_sizes()[i]; + ++j, parameter_cursor++) { + input_jets[parameter_cursor].v.setZero(); + if (active_parameter_count < Stride && + parameter_cursor >= + (start_derivative_section[current_derivative_section] + + current_derivative_section_cursor)) { + if (jacobians[i] != nullptr) { + input_jets[parameter_cursor].v[active_parameter_count] = 1.0; + ++active_parameter_count; + ++current_derivative_section_cursor; + } else { + ++current_derivative_section; + current_derivative_section_cursor = 0; + } + } + } + } + + if (!(*functor_)(&jet_parameters[0], &output_jets[0])) { + return false; + } + + // Copy the pieces of the jacobians into their final place. + active_parameter_count = 0; + + current_derivative_section = initial_derivative_section; + current_derivative_section_cursor = initial_derivative_section_cursor; + + for (int i = 0, parameter_cursor = 0; i < num_parameter_blocks; ++i) { + for (int j = 0; j < parameter_block_sizes()[i]; + ++j, parameter_cursor++) { + if (active_parameter_count < Stride && + parameter_cursor >= + (start_derivative_section[current_derivative_section] + + current_derivative_section_cursor)) { + if (jacobians[i] != nullptr) { + for (int k = 0; k < num_residuals(); ++k) { + jacobians[i][k * parameter_block_sizes()[i] + j] = + output_jets[k].v[active_parameter_count]; + } + ++active_parameter_count; + ++current_derivative_section_cursor; + } else { + ++current_derivative_section; + current_derivative_section_cursor = 0; + } + } + } + } + + // Only copy the residuals over once (even though we compute them on + // every loop). + if (pass == num_strides - 1) { + for (int k = 0; k < num_residuals(); ++k) { + residuals[k] = output_jets[k].a; + } + } + } + return true; + } + + private: + std::unique_ptr functor_; + Ownership ownership_; +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_DYNAMIC_AUTODIFF_COST_FUNCTION_H_ diff --git a/include/ceres/dynamic_cost_function.h b/include/ceres/dynamic_cost_function.h new file mode 100644 index 0000000000000000000000000000000000000000..c84a366dafbe6318c8caa925342ec6cba61046d3 --- /dev/null +++ b/include/ceres/dynamic_cost_function.h @@ -0,0 +1,57 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2019 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_PUBLIC_DYNAMIC_COST_FUNCTION_H_ +#define CERES_PUBLIC_DYNAMIC_COST_FUNCTION_H_ + +#include "ceres/cost_function.h" +#include "ceres/internal/disable_warnings.h" + +namespace ceres { + +// A common base class for DynamicAutoDiffCostFunction and +// DynamicNumericDiffCostFunction which depend on methods that can add +// parameter blocks and set the number of residuals at run time. +class CERES_EXPORT DynamicCostFunction : public CostFunction { + public: + virtual void AddParameterBlock(int size) { + mutable_parameter_block_sizes()->push_back(size); + } + + virtual void SetNumResiduals(int num_residuals) { + set_num_residuals(num_residuals); + } +}; + +} // namespace ceres + +#include "ceres/internal/reenable_warnings.h" + +#endif // CERES_PUBLIC_DYNAMIC_COST_FUNCTION_H_ diff --git a/include/ceres/dynamic_cost_function_to_functor.h b/include/ceres/dynamic_cost_function_to_functor.h new file mode 100644 index 0000000000000000000000000000000000000000..5b5feaaf58e6327a5d6f4d66ccd624b53672fb43 --- /dev/null +++ b/include/ceres/dynamic_cost_function_to_functor.h @@ -0,0 +1,194 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2019 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// dgossow@google.com (David Gossow) + +#ifndef CERES_PUBLIC_DYNAMIC_COST_FUNCTION_TO_FUNCTOR_H_ +#define CERES_PUBLIC_DYNAMIC_COST_FUNCTION_TO_FUNCTOR_H_ + +#include +#include +#include + +#include "ceres/dynamic_cost_function.h" +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" +#include "ceres/internal/fixed_array.h" +#include "glog/logging.h" + +namespace ceres { + +// DynamicCostFunctionToFunctor allows users to use CostFunction +// objects in templated functors which are to be used for automatic +// differentiation. It works similar to CostFunctionToFunctor, with the +// difference that it allows you to wrap a cost function with dynamic numbers +// of parameters and residuals. +// +// For example, let us assume that +// +// class IntrinsicProjection : public CostFunction { +// public: +// IntrinsicProjection(const double* observation); +// bool Evaluate(double const* const* parameters, +// double* residuals, +// double** jacobians) const override; +// }; +// +// is a cost function that implements the projection of a point in its +// local coordinate system onto its image plane and subtracts it from +// the observed point projection. It can compute its residual and +// either via analytic or numerical differentiation can compute its +// jacobians. The intrinsics are passed in as parameters[0] and the point as +// parameters[1]. +// +// Now we would like to compose the action of this CostFunction with +// the action of camera extrinsics, i.e., rotation and +// translation. Say we have a templated function +// +// template +// void RotateAndTranslatePoint(double const* const* parameters, +// double* residuals); +// +// Then we can now do the following, +// +// struct CameraProjection { +// CameraProjection(const double* observation) +// : intrinsic_projection_.(new IntrinsicProjection(observation)) { +// } +// template +// bool operator()(T const* const* parameters, +// T* residual) const { +// const T* rotation = parameters[0]; +// const T* translation = parameters[1]; +// const T* intrinsics = parameters[2]; +// const T* point = parameters[3]; +// T transformed_point[3]; +// RotateAndTranslatePoint(rotation, translation, point, transformed_point); +// +// // Note that we call intrinsic_projection_, just like it was +// // any other templated functor. +// const T* projection_parameters[2]; +// projection_parameters[0] = intrinsics; +// projection_parameters[1] = transformed_point; +// return intrinsic_projection_(projection_parameters, residual); +// } +// +// private: +// DynamicCostFunctionToFunctor intrinsic_projection_; +// }; +class CERES_EXPORT DynamicCostFunctionToFunctor { + public: + // Takes ownership of cost_function. + explicit DynamicCostFunctionToFunctor(CostFunction* cost_function) + : cost_function_(cost_function) { + CHECK(cost_function != nullptr); + } + + bool operator()(double const* const* parameters, double* residuals) const { + return cost_function_->Evaluate(parameters, residuals, nullptr); + } + + template + bool operator()(JetT const* const* inputs, JetT* output) const { + const std::vector& parameter_block_sizes = + cost_function_->parameter_block_sizes(); + const int num_parameter_blocks = + static_cast(parameter_block_sizes.size()); + const int num_residuals = cost_function_->num_residuals(); + const int num_parameters = std::accumulate( + parameter_block_sizes.begin(), parameter_block_sizes.end(), 0); + + internal::FixedArray parameters(num_parameters); + internal::FixedArray parameter_blocks(num_parameter_blocks); + internal::FixedArray jacobians(num_residuals * num_parameters); + internal::FixedArray jacobian_blocks(num_parameter_blocks); + internal::FixedArray residuals(num_residuals); + + // Build a set of arrays to get the residuals and jacobians from + // the CostFunction wrapped by this functor. + double* parameter_ptr = parameters.data(); + double* jacobian_ptr = jacobians.data(); + for (int i = 0; i < num_parameter_blocks; ++i) { + parameter_blocks[i] = parameter_ptr; + jacobian_blocks[i] = jacobian_ptr; + for (int j = 0; j < parameter_block_sizes[i]; ++j) { + *parameter_ptr++ = inputs[i][j].a; + } + jacobian_ptr += num_residuals * parameter_block_sizes[i]; + } + + if (!cost_function_->Evaluate(parameter_blocks.data(), + residuals.data(), + jacobian_blocks.data())) { + return false; + } + + // Now that we have the incoming Jets, which are carrying the + // partial derivatives of each of the inputs w.r.t to some other + // underlying parameters. The derivative of the outputs of the + // cost function w.r.t to the same underlying parameters can now + // be computed by applying the chain rule. + // + // d output[i] d output[i] d input[j] + // -------------- = sum_j ----------- * ------------ + // d parameter[k] d input[j] d parameter[k] + // + // d input[j] + // -------------- = inputs[j], so + // d parameter[k] + // + // outputJet[i] = sum_k jacobian[i][k] * inputJet[k] + // + // The following loop, iterates over the residuals, computing one + // output jet at a time. + for (int i = 0; i < num_residuals; ++i) { + output[i].a = residuals[i]; + output[i].v.setZero(); + + for (int j = 0; j < num_parameter_blocks; ++j) { + const int32_t block_size = parameter_block_sizes[j]; + for (int k = 0; k < parameter_block_sizes[j]; ++k) { + output[i].v += + jacobian_blocks[j][i * block_size + k] * inputs[j][k].v; + } + } + } + + return true; + } + + private: + std::unique_ptr cost_function_; +}; + +} // namespace ceres + +#include "ceres/internal/reenable_warnings.h" + +#endif // CERES_PUBLIC_DYNAMIC_COST_FUNCTION_TO_FUNCTOR_H_ diff --git a/include/ceres/dynamic_numeric_diff_cost_function.h b/include/ceres/dynamic_numeric_diff_cost_function.h new file mode 100644 index 0000000000000000000000000000000000000000..e1892e8ba4a63d496af45f323c28ec3bd55234a1 --- /dev/null +++ b/include/ceres/dynamic_numeric_diff_cost_function.h @@ -0,0 +1,164 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2019 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: mierle@gmail.com (Keir Mierle) +// sameeragarwal@google.com (Sameer Agarwal) +// thadh@gmail.com (Thad Hughes) +// tbennun@gmail.com (Tal Ben-Nun) + +#ifndef CERES_PUBLIC_DYNAMIC_NUMERIC_DIFF_COST_FUNCTION_H_ +#define CERES_PUBLIC_DYNAMIC_NUMERIC_DIFF_COST_FUNCTION_H_ + +#include +#include +#include +#include + +#include "ceres/dynamic_cost_function.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/numeric_diff.h" +#include "ceres/internal/parameter_dims.h" +#include "ceres/numeric_diff_options.h" +#include "ceres/types.h" +#include "glog/logging.h" + +namespace ceres { + +// This numeric diff implementation differs from the one found in +// numeric_diff_cost_function.h by supporting numericdiff on cost +// functions with variable numbers of parameters with variable +// sizes. With the other implementation, all the sizes (both the +// number of parameter blocks and the size of each block) must be +// fixed at compile time. +// +// The functor API differs slightly from the API for fixed size +// numeric diff; the expected interface for the cost functors is: +// +// struct MyCostFunctor { +// bool operator()(double const* +// const* parameters, +// double* residuals) const { +// // Use parameters[i] to access the i'th parameter block. +// } +// } +// +// Since the sizing of the parameters is done at runtime, you must +// also specify the sizes after creating the +// DynamicNumericDiffCostFunction. For example: +// +// DynamicAutoDiffCostFunction cost_function( +// new MyCostFunctor()); +// cost_function.AddParameterBlock(5); +// cost_function.AddParameterBlock(10); +// cost_function.SetNumResiduals(21); +template +class DynamicNumericDiffCostFunction final : public DynamicCostFunction { + public: + explicit DynamicNumericDiffCostFunction( + const CostFunctor* functor, + Ownership ownership = TAKE_OWNERSHIP, + const NumericDiffOptions& options = NumericDiffOptions()) + : functor_(functor), ownership_(ownership), options_(options) {} + + DynamicNumericDiffCostFunction(DynamicNumericDiffCostFunction&& other) + : functor_(std::move(other.functor_)), ownership_(other.ownership_) {} + + ~DynamicNumericDiffCostFunction() override { + if (ownership_ != TAKE_OWNERSHIP) { + functor_.release(); + } + } + + bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const override { + using internal::NumericDiff; + CHECK_GT(num_residuals(), 0) + << "You must call DynamicNumericDiffCostFunction::SetNumResiduals() " + << "before DynamicNumericDiffCostFunction::Evaluate()."; + + const std::vector& block_sizes = parameter_block_sizes(); + CHECK(!block_sizes.empty()) + << "You must call DynamicNumericDiffCostFunction::AddParameterBlock() " + << "before DynamicNumericDiffCostFunction::Evaluate()."; + + const bool status = + internal::VariadicEvaluate( + *functor_.get(), parameters, residuals); + if (jacobians == nullptr || !status) { + return status; + } + + // Create local space for a copy of the parameters which will get mutated. + int parameters_size = accumulate(block_sizes.begin(), block_sizes.end(), 0); + std::vector parameters_copy(parameters_size); + std::vector parameters_references_copy(block_sizes.size()); + parameters_references_copy[0] = ¶meters_copy[0]; + for (size_t block = 1; block < block_sizes.size(); ++block) { + parameters_references_copy[block] = + parameters_references_copy[block - 1] + block_sizes[block - 1]; + } + + // Copy the parameters into the local temp space. + for (size_t block = 0; block < block_sizes.size(); ++block) { + memcpy(parameters_references_copy[block], + parameters[block], + block_sizes[block] * sizeof(*parameters[block])); + } + + for (size_t block = 0; block < block_sizes.size(); ++block) { + if (jacobians[block] != nullptr && + !NumericDiff:: + EvaluateJacobianForParameterBlock(functor_.get(), + residuals, + options_, + this->num_residuals(), + block, + block_sizes[block], + ¶meters_references_copy[0], + jacobians[block])) { + return false; + } + } + return true; + } + + private: + std::unique_ptr functor_; + Ownership ownership_; + NumericDiffOptions options_; +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_DYNAMIC_AUTODIFF_COST_FUNCTION_H_ diff --git a/include/ceres/evaluation_callback.h b/include/ceres/evaluation_callback.h new file mode 100644 index 0000000000000000000000000000000000000000..495d565047a5fe014ffb8e8a00e7850ddea2377c --- /dev/null +++ b/include/ceres/evaluation_callback.h @@ -0,0 +1,80 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2019 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: mierle@gmail.com (Keir Mierle) + +#ifndef CERES_PUBLIC_EVALUATION_CALLBACK_H_ +#define CERES_PUBLIC_EVALUATION_CALLBACK_H_ + +#include "ceres/internal/export.h" + +namespace ceres { + +// Using this callback interface, Ceres can notify you when it is +// about to evaluate the residuals or jacobians. With the callback, +// you can share computation between residual blocks by doing the +// shared computation in PrepareForEvaluation() before Ceres calls +// CostFunction::Evaluate(). It also enables caching results between a +// pure residual evaluation and a residual & jacobian evaluation, via +// the new_evaluation_point argument. +// +// One use case for this callback is if the cost function compute is +// moved to the GPU. In that case, the prepare call does the actual +// cost function evaluation, and subsequent calls from Ceres to the +// actual cost functions merely copy the results from the GPU onto the +// corresponding blocks for Ceres to plug into the solver. +// +// NOTE: Ceres provides no mechanism to share data other than the +// notification from the callback. Users must provide access to +// pre-computed shared data to their cost functions behind the scenes; +// this all happens without Ceres knowing. +// +// One approach is to put a pointer to the shared data in each cost +// function (recommended) or to use a global shared variable +// (discouraged; bug-prone). As far as Ceres is concerned, it is +// evaluating cost functions like any other; it just so happens that +// behind the scenes the cost functions reuse pre-computed data to +// execute faster. +class CERES_EXPORT EvaluationCallback { + public: + virtual ~EvaluationCallback(); + + // Called before Ceres requests residuals or jacobians for a given setting of + // the parameters. User parameters (the double* values provided to the cost + // functions) are fixed until the next call to PrepareForEvaluation(). If + // new_evaluation_point == true, then this is a new point that is different + // from the last evaluated point. Otherwise, it is the same point that was + // evaluated previously (either jacobian or residual) and the user can use + // cached results from previous evaluations. + virtual void PrepareForEvaluation(bool evaluate_jacobians, + bool new_evaluation_point) = 0; +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_EVALUATION_CALLBACK_H_ diff --git a/include/ceres/first_order_function.h b/include/ceres/first_order_function.h new file mode 100644 index 0000000000000000000000000000000000000000..d718b6679cead0c7f9da6f800108c5a15b449761 --- /dev/null +++ b/include/ceres/first_order_function.h @@ -0,0 +1,54 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2019 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_PUBLIC_FIRST_ORDER_FUNCTION_H_ +#define CERES_PUBLIC_FIRST_ORDER_FUNCTION_H_ + +#include "ceres/internal/export.h" + +namespace ceres { + +// A FirstOrderFunction object implements the evaluation of a function +// and its gradient. +class CERES_EXPORT FirstOrderFunction { + public: + virtual ~FirstOrderFunction(); + + // cost is never null. gradient may be null. The return value + // indicates whether the evaluation was successful or not. + virtual bool Evaluate(const double* const parameters, + double* cost, + double* gradient) const = 0; + virtual int NumParameters() const = 0; +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_FIRST_ORDER_FUNCTION_H_ diff --git a/include/ceres/gradient_checker.h b/include/ceres/gradient_checker.h new file mode 100644 index 0000000000000000000000000000000000000000..178fa2b0dd2c073d9a11ace23dda6cfd4ff3b56f --- /dev/null +++ b/include/ceres/gradient_checker.h @@ -0,0 +1,189 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2019 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// Copyright 2007 Google Inc. All Rights Reserved. +// +// Authors: wjr@google.com (William Rucklidge), +// keir@google.com (Keir Mierle), +// dgossow@google.com (David Gossow) + +#ifndef CERES_PUBLIC_GRADIENT_CHECKER_H_ +#define CERES_PUBLIC_GRADIENT_CHECKER_H_ + +#include +#include +#include + +#include "ceres/cost_function.h" +#include "ceres/dynamic_numeric_diff_cost_function.h" +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/export.h" +#include "ceres/internal/fixed_array.h" +#include "ceres/local_parameterization.h" +#include "ceres/manifold.h" +#include "glog/logging.h" + +namespace ceres { + +// GradientChecker compares the Jacobians returned by a cost function against +// derivatives estimated using finite differencing. +// +// The condition enforced is that +// +// (J_actual(i, j) - J_numeric(i, j)) +// ------------------------------------ < relative_precision +// max(J_actual(i, j), J_numeric(i, j)) +// +// where J_actual(i, j) is the jacobian as computed by the supplied cost +// function (by the user) multiplied by the local parameterization Jacobian +// and J_numeric is the jacobian as computed by finite differences, multiplied +// by the local parameterization Jacobian as well. +// +// How to use: Fill in an array of pointers to parameter blocks for your +// CostFunction, and then call Probe(). Check that the return value is 'true'. +class CERES_EXPORT GradientChecker { + public: + // This constructor will not take ownership of the cost function or local + // parameterizations. + // + // function: The cost function to probe. + // + // local_parameterizations: A vector of local parameterizations, one for each + // parameter block. May be nullptr or contain nullptrs to indicate that the + // respective parameter does not have a local parameterization. + // + // options: Options to use for numerical differentiation. + // + // NOTE: This constructor is deprecated and will be removed in the next public + // release of Ceres Solver. Please transition to using the Manifold based + // version. + CERES_DEPRECATED_WITH_MSG( + "Local Parameterizations are deprecated. Use the constructor that uses " + "Manifolds instead.") + GradientChecker( + const CostFunction* function, + const std::vector* local_parameterizations, + const NumericDiffOptions& options); + + // This will not take ownership of the cost function or manifolds. + // + // function: The cost function to probe. + // + // manifolds: A vector of manifolds for each parameter. May be nullptr or + // contain nullptrs to indicate that the respective parameter blocks are + // Euclidean. + // + // options: Options to use for numerical differentiation. + GradientChecker(const CostFunction* function, + const std::vector* manifolds, + const NumericDiffOptions& options); + ~GradientChecker(); + + // Contains results from a call to Probe for later inspection. + struct CERES_EXPORT ProbeResults { + // The return value of the cost function. + bool return_value; + + // Computed residual vector. + Vector residuals; + + // The sizes of the Jacobians below are dictated by the cost function's + // parameter block size and residual block sizes. If a parameter block has a + // manifold associated with it, the size of the "local" Jacobian will be + // determined by the dimension of the manifold (which is the same as the + // dimension of the tangent space) and residual block size, otherwise it + // will be identical to the regular Jacobian. + + // Derivatives as computed by the cost function. + std::vector jacobians; + + // Derivatives as computed by the cost function in local space. + std::vector local_jacobians; + + // Derivatives as computed by numerical differentiation in local space. + std::vector numeric_jacobians; + + // Derivatives as computed by numerical differentiation in local space. + std::vector local_numeric_jacobians; + + // Contains the maximum relative error found in the local Jacobians. + double maximum_relative_error; + + // If an error was detected, this will contain a detailed description of + // that error. + std::string error_log; + }; + + // Call the cost function, compute alternative Jacobians using finite + // differencing and compare results. If manifolds are given, the Jacobians + // will be multiplied by the manifold Jacobians before performing the check, + // which effectively means that all errors along the null space of the + // manifold will be ignored. Returns false if the Jacobians don't match, the + // cost function return false, or if a cost function returns a different + // residual when called with a Jacobian output argument vs. calling it + // without. Otherwise returns true. + // + // parameters: The parameter values at which to probe. + // relative_precision: A threshold for the relative difference between the + // Jacobians. If the Jacobians differ by more than this amount, then the + // probe fails. + // results: On return, the Jacobians (and other information) will be stored + // here. May be nullptr. + // + // Returns true if no problems are detected and the difference between the + // Jacobians is less than error_tolerance. + bool Probe(double const* const* parameters, + double relative_precision, + ProbeResults* results) const; + + private: + GradientChecker() = delete; + GradientChecker(const GradientChecker&) = delete; + void operator=(const GradientChecker&) = delete; + + // This bool is used to determine whether the constructor with the + // LocalParameterizations is called or the one with Manifolds is called. If + // the former, then the vector of manifolds is a vector of ManifoldAdapter + // objects which we own and should be deleted. If the latter then they are + // real Manifold objects owned by the caller and will not be deleted. + // + // This bool is only needed during the LocalParameterization to Manifold + // transition, once this transition is complete the LocalParameterization + // based constructor and this bool will be removed. + const bool delete_manifolds_ = false; + + std::vector manifolds_; + const CostFunction* function_; + std::unique_ptr finite_diff_cost_function_; +}; + +} // namespace ceres + +#include "ceres/internal/reenable_warnings.h" + +#endif // CERES_PUBLIC_GRADIENT_CHECKER_H_ diff --git a/include/ceres/gradient_problem.h b/include/ceres/gradient_problem.h new file mode 100644 index 0000000000000000000000000000000000000000..b6a8b86742145c7f13fabc389c7f6bf49f677db5 --- /dev/null +++ b/include/ceres/gradient_problem.h @@ -0,0 +1,185 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2019 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_PUBLIC_GRADIENT_PROBLEM_H_ +#define CERES_PUBLIC_GRADIENT_PROBLEM_H_ + +#include + +#include "ceres/first_order_function.h" +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" +#include "ceres/local_parameterization.h" +#include "ceres/manifold.h" + +namespace ceres { + +class FirstOrderFunction; + +// Instances of GradientProblem represent general non-linear +// optimization problems that must be solved using just the value of +// the objective function and its gradient. + +// Unlike the Problem class, which can only be used to model non-linear least +// squares problems, instances of GradientProblem are not restricted in the form +// of the objective function. +// +// Structurally GradientProblem is a composition of a FirstOrderFunction and +// optionally a Manifold. +// +// The FirstOrderFunction is responsible for evaluating the cost and gradient of +// the objective function. +// +// The Manifold is responsible for going back and forth between the ambient +// space and the local tangent space. (See manifold.h for more details). When a +// Manifold is not provided, then the tangent space is assumed to coincide with +// the ambient Euclidean space that the gradient vector lives in. +// +// Example usage: +// +// The following demonstrate the problem construction for Rosenbrock's function +// +// f(x,y) = (1-x)^2 + 100(y - x^2)^2; +// +// class Rosenbrock : public ceres::FirstOrderFunction { +// public: +// virtual ~Rosenbrock() {} +// +// virtual bool Evaluate(const double* parameters, +// double* cost, +// double* gradient) const { +// const double x = parameters[0]; +// const double y = parameters[1]; +// +// cost[0] = (1.0 - x) * (1.0 - x) + 100.0 * (y - x * x) * (y - x * x); +// if (gradient != nullptr) { +// gradient[0] = -2.0 * (1.0 - x) - 200.0 * (y - x * x) * 2.0 * x; +// gradient[1] = 200.0 * (y - x * x); +// } +// return true; +// }; +// +// virtual int NumParameters() const { return 2; }; +// }; +// +// ceres::GradientProblem problem(new Rosenbrock()); +// +// NOTE: We are currently in the process of transitioning from +// LocalParameterization to Manifolds in the Ceres API. During this period, +// GradientProblem will support using both Manifold and LocalParameterization +// objects interchangably. For methods in the API affected by this change, see +// their documentation below. +class CERES_EXPORT GradientProblem { + public: + // Takes ownership of the function. + explicit GradientProblem(FirstOrderFunction* function); + + // Takes ownership of the function and the parameterization. + // + // NOTE: This constructor is deprecated and will be removed in the next public + // release of Ceres Solver. Please move to using the Manifold based + // constructor. + CERES_DEPRECATED_WITH_MSG( + "LocalParameterizations are deprecated. Please use the constructor that " + "uses Manifold instead.") + GradientProblem(FirstOrderFunction* function, + LocalParameterization* parameterization); + + // Takes ownership of the function and the manifold. + GradientProblem(FirstOrderFunction* function, Manifold* manifold); + + int NumParameters() const; + + // Dimension of the manifold (and its tangent space). + // + // During the transition from LocalParameterization to Manifold, this method + // reports the LocalSize of the LocalParameterization or the TangentSize of + // the Manifold object associated with this problem. + int NumTangentParameters() const; + + // Dimension of the manifold (and its tangent space). + // + // NOTE: This method is deprecated and will be removed in the next public + // release of Ceres Solver. Please move to using NumTangentParameters() + // instead. + int NumLocalParameters() const { return NumTangentParameters(); } + + // This call is not thread safe. + bool Evaluate(const double* parameters, double* cost, double* gradient) const; + bool Plus(const double* x, const double* delta, double* x_plus_delta) const; + + const FirstOrderFunction* function() const { return function_.get(); } + FirstOrderFunction* mutable_function() { return function_.get(); } + + // NOTE: During the transition from LocalParameterization to Manifold we need + // to support both The LocalParameterization and Manifold based constructors. + // + // When the user uses the LocalParameterization, internally the solver will + // wrap it in a ManifoldAdapter object and return it when manifold or + // mutable_manifold are called. + // + // As a result this method will return a non-nullptr result if a Manifold or a + // LocalParameterization was used when constructing the GradientProblem. + const Manifold* manifold() const { return manifold_.get(); } + Manifold* mutable_manifold() { return manifold_.get(); } + + // If the problem is constructed without a LocalParameterization or with a + // Manifold this method will return a nullptr. + // + // NOTE: This method is deprecated and will be removed in the next public + // release of Ceres Solver. + CERES_DEPRECATED_WITH_MSG("Use Manifolds instead.") + const LocalParameterization* parameterization() const { + return parameterization_.get(); + } + + // If the problem is constructed without a LocalParameterization or with a + // Manifold this method will return a nullptr. + // + // NOTE: This method is deprecated and will be removed in the next public + // release of Ceres Solver. + CERES_DEPRECATED_WITH_MSG("Use Manifolds instead.") + LocalParameterization* mutable_parameterization() { + return parameterization_.get(); + } + + private: + std::unique_ptr function_; + CERES_DEPRECATED_WITH_MSG("") + std::unique_ptr parameterization_; + std::unique_ptr manifold_; + std::unique_ptr scratch_; +}; + +} // namespace ceres + +#include "ceres/internal/reenable_warnings.h" + +#endif // CERES_PUBLIC_GRADIENT_PROBLEM_H_ diff --git a/include/ceres/gradient_problem_solver.h b/include/ceres/gradient_problem_solver.h new file mode 100644 index 0000000000000000000000000000000000000000..b6290c80c28f4bd81bb6556e468ecba0b7d3ed64 --- /dev/null +++ b/include/ceres/gradient_problem_solver.h @@ -0,0 +1,357 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2019 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_PUBLIC_GRADIENT_PROBLEM_SOLVER_H_ +#define CERES_PUBLIC_GRADIENT_PROBLEM_SOLVER_H_ + +#include +#include +#include + +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" +#include "ceres/internal/port.h" +#include "ceres/iteration_callback.h" +#include "ceres/types.h" + +namespace ceres { + +class GradientProblem; + +class CERES_EXPORT GradientProblemSolver { + public: + virtual ~GradientProblemSolver(); + + // The options structure contains, not surprisingly, options that control how + // the solver operates. The defaults should be suitable for a wide range of + // problems; however, better performance is often obtainable with tweaking. + // + // The constants are defined inside types.h + struct CERES_EXPORT Options { + // Returns true if the options struct has a valid + // configuration. Returns false otherwise, and fills in *error + // with a message describing the problem. + bool IsValid(std::string* error) const; + + // Minimizer options ---------------------------------------- + LineSearchDirectionType line_search_direction_type = LBFGS; + LineSearchType line_search_type = WOLFE; + NonlinearConjugateGradientType nonlinear_conjugate_gradient_type = + FLETCHER_REEVES; + + // The LBFGS hessian approximation is a low rank approximation to + // the inverse of the Hessian matrix. The rank of the + // approximation determines (linearly) the space and time + // complexity of using the approximation. Higher the rank, the + // better is the quality of the approximation. The increase in + // quality is however is bounded for a number of reasons. + // + // 1. The method only uses secant information and not actual + // derivatives. + // + // 2. The Hessian approximation is constrained to be positive + // definite. + // + // So increasing this rank to a large number will cost time and + // space complexity without the corresponding increase in solution + // quality. There are no hard and fast rules for choosing the + // maximum rank. The best choice usually requires some problem + // specific experimentation. + // + // For more theoretical and implementation details of the LBFGS + // method, please see: + // + // Nocedal, J. (1980). "Updating Quasi-Newton Matrices with + // Limited Storage". Mathematics of Computation 35 (151): 773-782. + int max_lbfgs_rank = 20; + + // As part of the (L)BFGS update step (BFGS) / right-multiply step (L-BFGS), + // the initial inverse Hessian approximation is taken to be the Identity. + // However, Oren showed that using instead I * \gamma, where \gamma is + // chosen to approximate an eigenvalue of the true inverse Hessian can + // result in improved convergence in a wide variety of cases. Setting + // use_approximate_eigenvalue_bfgs_scaling to true enables this scaling. + // + // It is important to note that approximate eigenvalue scaling does not + // always improve convergence, and that it can in fact significantly degrade + // performance for certain classes of problem, which is why it is disabled + // by default. In particular it can degrade performance when the + // sensitivity of the problem to different parameters varies significantly, + // as in this case a single scalar factor fails to capture this variation + // and detrimentally downscales parts of the jacobian approximation which + // correspond to low-sensitivity parameters. It can also reduce the + // robustness of the solution to errors in the jacobians. + // + // Oren S.S., Self-scaling variable metric (SSVM) algorithms + // Part II: Implementation and experiments, Management Science, + // 20(5), 863-874, 1974. + bool use_approximate_eigenvalue_bfgs_scaling = false; + + // Degree of the polynomial used to approximate the objective + // function. Valid values are BISECTION, QUADRATIC and CUBIC. + // + // BISECTION corresponds to pure backtracking search with no + // interpolation. + LineSearchInterpolationType line_search_interpolation_type = CUBIC; + + // If during the line search, the step_size falls below this + // value, it is truncated to zero. + double min_line_search_step_size = 1e-9; + + // Line search parameters. + + // Solving the line search problem exactly is computationally + // prohibitive. Fortunately, line search based optimization + // algorithms can still guarantee convergence if instead of an + // exact solution, the line search algorithm returns a solution + // which decreases the value of the objective function + // sufficiently. More precisely, we are looking for a step_size + // s.t. + // + // f(step_size) <= f(0) + sufficient_decrease * f'(0) * step_size + // + double line_search_sufficient_function_decrease = 1e-4; + + // In each iteration of the line search, + // + // new_step_size >= max_line_search_step_contraction * step_size + // + // Note that by definition, for contraction: + // + // 0 < max_step_contraction < min_step_contraction < 1 + // + double max_line_search_step_contraction = 1e-3; + + // In each iteration of the line search, + // + // new_step_size <= min_line_search_step_contraction * step_size + // + // Note that by definition, for contraction: + // + // 0 < max_step_contraction < min_step_contraction < 1 + // + double min_line_search_step_contraction = 0.6; + + // Maximum number of trial step size iterations during each line search, + // if a step size satisfying the search conditions cannot be found within + // this number of trials, the line search will terminate. + int max_num_line_search_step_size_iterations = 20; + + // Maximum number of restarts of the line search direction algorithm before + // terminating the optimization. Restarts of the line search direction + // algorithm occur when the current algorithm fails to produce a new descent + // direction. This typically indicates a numerical failure, or a breakdown + // in the validity of the approximations used. + int max_num_line_search_direction_restarts = 5; + + // The strong Wolfe conditions consist of the Armijo sufficient + // decrease condition, and an additional requirement that the + // step-size be chosen s.t. the _magnitude_ ('strong' Wolfe + // conditions) of the gradient along the search direction + // decreases sufficiently. Precisely, this second condition + // is that we seek a step_size s.t. + // + // |f'(step_size)| <= sufficient_curvature_decrease * |f'(0)| + // + // Where f() is the line search objective and f'() is the derivative + // of f w.r.t step_size (d f / d step_size). + double line_search_sufficient_curvature_decrease = 0.9; + + // During the bracketing phase of the Wolfe search, the step size is + // increased until either a point satisfying the Wolfe conditions is + // found, or an upper bound for a bracket containing a point satisfying + // the conditions is found. Precisely, at each iteration of the + // expansion: + // + // new_step_size <= max_step_expansion * step_size. + // + // By definition for expansion, max_step_expansion > 1.0. + double max_line_search_step_expansion = 10.0; + + // Maximum number of iterations for the minimizer to run for. + int max_num_iterations = 50; + + // Maximum time for which the minimizer should run for. + double max_solver_time_in_seconds = 1e9; + + // Minimizer terminates when + // + // (new_cost - old_cost) < function_tolerance * old_cost; + // + double function_tolerance = 1e-6; + + // Minimizer terminates when + // + // max_i |x - Project(Plus(x, -g(x))| < gradient_tolerance + // + // This value should typically be 1e-4 * function_tolerance. + double gradient_tolerance = 1e-10; + + // Minimizer terminates when + // + // |step|_2 <= parameter_tolerance * ( |x|_2 + parameter_tolerance) + // + double parameter_tolerance = 1e-8; + + // Logging options --------------------------------------------------------- + + LoggingType logging_type = PER_MINIMIZER_ITERATION; + + // By default the Minimizer progress is logged to VLOG(1), which + // is sent to STDERR depending on the vlog level. If this flag is + // set to true, and logging_type is not SILENT, the logging output + // is sent to STDOUT. + bool minimizer_progress_to_stdout = false; + + // If true, the user's parameter blocks are updated at the end of + // every Minimizer iteration, otherwise they are updated when the + // Minimizer terminates. This is useful if, for example, the user + // wishes to visualize the state of the optimization every + // iteration. + bool update_state_every_iteration = false; + + // Callbacks that are executed at the end of each iteration of the + // Minimizer. An iteration may terminate midway, either due to + // numerical failures or because one of the convergence tests has + // been satisfied. In this case none of the callbacks are + // executed. + + // Callbacks are executed in the order that they are specified in + // this vector. By default, parameter blocks are updated only at + // the end of the optimization, i.e when the Minimizer + // terminates. This behaviour is controlled by + // update_state_every_variable. If the user wishes to have access + // to the update parameter blocks when his/her callbacks are + // executed, then set update_state_every_iteration to true. + // + // The solver does NOT take ownership of these pointers. + std::vector callbacks; + }; + + struct CERES_EXPORT Summary { + // A brief one line description of the state of the solver after + // termination. + std::string BriefReport() const; + + // A full multiline description of the state of the solver after + // termination. + std::string FullReport() const; + + bool IsSolutionUsable() const; + + // Minimizer summary ------------------------------------------------- + TerminationType termination_type = FAILURE; + + // Reason why the solver terminated. + std::string message = "ceres::GradientProblemSolve was not called."; + + // Cost of the problem (value of the objective function) before + // the optimization. + double initial_cost = -1.0; + + // Cost of the problem (value of the objective function) after the + // optimization. + double final_cost = -1.0; + + // IterationSummary for each minimizer iteration in order. + std::vector iterations; + + // Number of times the cost (and not the gradient) was evaluated. + int num_cost_evaluations = -1; + + // Number of times the gradient (and the cost) were evaluated. + int num_gradient_evaluations = -1; + + // Sum total of all time spent inside Ceres when Solve is called. + double total_time_in_seconds = -1.0; + + // Time (in seconds) spent evaluating the cost. + double cost_evaluation_time_in_seconds = -1.0; + + // Time (in seconds) spent evaluating the gradient. + double gradient_evaluation_time_in_seconds = -1.0; + + // Time (in seconds) spent minimizing the interpolating polynomial + // to compute the next candidate step size as part of a line search. + double line_search_polynomial_minimization_time_in_seconds = -1.0; + + // Number of parameters in the problem. + int num_parameters = -1; + + // Dimension of the tangent space of the problem. + CERES_DEPRECATED_WITH_MSG("Use num_tangent_parameters.") + int num_local_parameters = -1; + + // Dimension of the tangent space of the problem. + int num_tangent_parameters = -1; + + // Type of line search direction used. + LineSearchDirectionType line_search_direction_type = LBFGS; + + // Type of the line search algorithm used. + LineSearchType line_search_type = WOLFE; + + // When performing line search, the degree of the polynomial used + // to approximate the objective function. + LineSearchInterpolationType line_search_interpolation_type = CUBIC; + + // If the line search direction is NONLINEAR_CONJUGATE_GRADIENT, + // then this indicates the particular variant of non-linear + // conjugate gradient used. + NonlinearConjugateGradientType nonlinear_conjugate_gradient_type = + FLETCHER_REEVES; + + // If the type of the line search direction is LBFGS, then this + // indicates the rank of the Hessian approximation. + int max_lbfgs_rank = -1; + }; + + // Once a least squares problem has been built, this function takes + // the problem and optimizes it based on the values of the options + // parameters. Upon return, a detailed summary of the work performed + // by the preprocessor, the non-linear minimizer and the linear + // solver are reported in the summary object. + virtual void Solve(const GradientProblemSolver::Options& options, + const GradientProblem& problem, + double* parameters, + GradientProblemSolver::Summary* summary); +}; + +// Helper function which avoids going through the interface. +CERES_EXPORT void Solve(const GradientProblemSolver::Options& options, + const GradientProblem& problem, + double* parameters, + GradientProblemSolver::Summary* summary); + +} // namespace ceres + +#include "ceres/internal/reenable_warnings.h" + +#endif // CERES_PUBLIC_GRADIENT_PROBLEM_SOLVER_H_ diff --git a/include/ceres/internal/array_selector.h b/include/ceres/internal/array_selector.h new file mode 100644 index 0000000000000000000000000000000000000000..b4db012f00bb1df516129edbfdd3bd59dd12e3b0 --- /dev/null +++ b/include/ceres/internal/array_selector.h @@ -0,0 +1,97 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2020 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: darius.rueckert@fau.de (Darius Rueckert) +// + +#ifndef CERES_PUBLIC_INTERNAL_ARRAY_SELECTOR_H_ +#define CERES_PUBLIC_INTERNAL_ARRAY_SELECTOR_H_ + +#include +#include + +#include "ceres/internal/fixed_array.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +// StaticFixedArray selects the best array implementation based on template +// arguments. If the size is not known at compile-time, pass +// ceres::DYNAMIC as a size-template argument. +// +// Three different containers are selected in different scenarios: +// +// num_elements == DYNAMIC: +// -> ceres::internal::FixedArray(size) + +// num_elements != DYNAMIC && num_elements <= max_stack_size +// -> std::array + +// num_elements != DYNAMIC && num_elements > max_stack_size +// -> std::vector(num_elements) +// +template +struct ArraySelector {}; + +template +struct ArraySelector + : ceres::internal::FixedArray { + explicit ArraySelector(int s) + : ceres::internal::FixedArray(s) {} +}; + +template +struct ArraySelector + : std::array { + explicit ArraySelector(int s) { CHECK_EQ(s, num_elements); } +}; + +template +struct ArraySelector + : std::vector { + explicit ArraySelector(int s) : std::vector(s) { + CHECK_EQ(s, num_elements); + } +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_PUBLIC_INTERNAL_ARRAY_SELECTOR_H_ diff --git a/include/ceres/internal/autodiff.h b/include/ceres/internal/autodiff.h new file mode 100644 index 0000000000000000000000000000000000000000..c796618cd2db078960a2a9e52ce5ab6e4100e3b5 --- /dev/null +++ b/include/ceres/internal/autodiff.h @@ -0,0 +1,365 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2019 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// Computation of the Jacobian matrix for vector-valued functions of multiple +// variables, using automatic differentiation based on the implementation of +// dual numbers in jet.h. Before reading the rest of this file, it is advisable +// to read jet.h's header comment in detail. +// +// The helper wrapper AutoDifferentiate() computes the jacobian of +// functors with templated operator() taking this form: +// +// struct F { +// template +// bool operator()(const T *x, const T *y, ..., T *z) { +// // Compute z[] based on x[], y[], ... +// // return true if computation succeeded, false otherwise. +// } +// }; +// +// All inputs and outputs may be vector-valued. +// +// To understand how jets are used to compute the jacobian, a +// picture may help. Consider a vector-valued function, F, returning 3 +// dimensions and taking a vector-valued parameter of 4 dimensions: +// +// y x +// [ * ] F [ * ] +// [ * ] <--- [ * ] +// [ * ] [ * ] +// [ * ] +// +// Similar to the 2-parameter example for f described in jet.h, computing the +// jacobian dy/dx is done by substituting a suitable jet object for x and all +// intermediate steps of the computation of F. Since x is has 4 dimensions, use +// a Jet. +// +// Before substituting a jet object for x, the dual components are set +// appropriately for each dimension of x: +// +// y x +// [ * | * * * * ] f [ * | 1 0 0 0 ] x0 +// [ * | * * * * ] <--- [ * | 0 1 0 0 ] x1 +// [ * | * * * * ] [ * | 0 0 1 0 ] x2 +// ---+--- [ * | 0 0 0 1 ] x3 +// | ^ ^ ^ ^ +// dy/dx | | | +----- infinitesimal for x3 +// | | +------- infinitesimal for x2 +// | +--------- infinitesimal for x1 +// +----------- infinitesimal for x0 +// +// The reason to set the internal 4x4 submatrix to the identity is that we wish +// to take the derivative of y separately with respect to each dimension of x. +// Each column of the 4x4 identity is therefore for a single component of the +// independent variable x. +// +// Then the jacobian of the mapping, dy/dx, is the 3x4 sub-matrix of the +// extended y vector, indicated in the above diagram. +// +// Functors with multiple parameters +// --------------------------------- +// In practice, it is often convenient to use a function f of two or more +// vector-valued parameters, for example, x[3] and z[6]. Unfortunately, the jet +// framework is designed for a single-parameter vector-valued input. The wrapper +// in this file addresses this issue adding support for functions with one or +// more parameter vectors. +// +// To support multiple parameters, all the parameter vectors are concatenated +// into one and treated as a single parameter vector, except that since the +// functor expects different inputs, we need to construct the jets as if they +// were part of a single parameter vector. The extended jets are passed +// separately for each parameter. +// +// For example, consider a functor F taking two vector parameters, p[2] and +// q[3], and producing an output y[4]: +// +// struct F { +// template +// bool operator()(const T *p, const T *q, T *z) { +// // ... +// } +// }; +// +// In this case, the necessary jet type is Jet. Here is a +// visualization of the jet objects in this case: +// +// Dual components for p ----+ +// | +// -+- +// y [ * | 1 0 | 0 0 0 ] --- p[0] +// [ * | 0 1 | 0 0 0 ] --- p[1] +// [ * | . . | + + + ] | +// [ * | . . | + + + ] v +// [ * | . . | + + + ] <--- F(p, q) +// [ * | . . | + + + ] ^ +// ^^^ ^^^^^ | +// dy/dp dy/dq [ * | 0 0 | 1 0 0 ] --- q[0] +// [ * | 0 0 | 0 1 0 ] --- q[1] +// [ * | 0 0 | 0 0 1 ] --- q[2] +// --+-- +// | +// Dual components for q --------------+ +// +// where the 4x2 submatrix (marked with ".") and 4x3 submatrix (marked with "+" +// of y in the above diagram are the derivatives of y with respect to p and q +// respectively. This is how autodiff works for functors taking multiple vector +// valued arguments (up to 6). +// +// Jacobian null pointers (nullptr) +// -------------------------------- +// In general, the functions below will accept nullptr for all or some of the +// Jacobian parameters, meaning that those Jacobians will not be computed. + +#ifndef CERES_PUBLIC_INTERNAL_AUTODIFF_H_ +#define CERES_PUBLIC_INTERNAL_AUTODIFF_H_ + +#include +#include +#include + +#include "ceres/internal/array_selector.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/fixed_array.h" +#include "ceres/internal/parameter_dims.h" +#include "ceres/internal/variadic_evaluate.h" +#include "ceres/jet.h" +#include "ceres/types.h" +#include "glog/logging.h" + +// If the number of parameters exceeds this values, the corresponding jets are +// placed on the heap. This will reduce performance by a factor of 2-5 on +// current compilers. +#ifndef CERES_AUTODIFF_MAX_PARAMETERS_ON_STACK +#define CERES_AUTODIFF_MAX_PARAMETERS_ON_STACK 50 +#endif + +#ifndef CERES_AUTODIFF_MAX_RESIDUALS_ON_STACK +#define CERES_AUTODIFF_MAX_RESIDUALS_ON_STACK 20 +#endif + +namespace ceres { +namespace internal { + +// Extends src by a 1st order perturbation for every dimension and puts it in +// dst. The size of src is N. Since this is also used for perturbations in +// blocked arrays, offset is used to shift which part of the jet the +// perturbation occurs. This is used to set up the extended x augmented by an +// identity matrix. The JetT type should be a Jet type, and T should be a +// numeric type (e.g. double). For example, +// +// 0 1 2 3 4 5 6 7 8 +// dst[0] [ * | . . | 1 0 0 | . . . ] +// dst[1] [ * | . . | 0 1 0 | . . . ] +// dst[2] [ * | . . | 0 0 1 | . . . ] +// +// is what would get put in dst if N was 3, offset was 3, and the jet type JetT +// was 8-dimensional. +template +struct Make1stOrderPerturbation { + public: + inline static void Apply(const T* src, JetT* dst) { + if (j == 0) { + DCHECK(src); + DCHECK(dst); + } + dst[j] = JetT(src[j], j + Offset); + Make1stOrderPerturbation::Apply(src, dst); + } +}; + +template +struct Make1stOrderPerturbation { + public: + static void Apply(const T* /* NOT USED */, JetT* /* NOT USED */) {} +}; + +// Calls Make1stOrderPerturbation for every parameter block. +// +// Example: +// If one having three parameter blocks with dimensions (3, 2, 4), the call +// Make1stOrderPerturbations::Apply(params, x); +// will result in the following calls to Make1stOrderPerturbation: +// Make1stOrderPerturbation<0, 3, 0>::Apply(params[0], x + 0); +// Make1stOrderPerturbation<0, 2, 3>::Apply(params[1], x + 3); +// Make1stOrderPerturbation<0, 4, 5>::Apply(params[2], x + 5); +template +struct Make1stOrderPerturbations; + +template +struct Make1stOrderPerturbations, + ParameterIdx, + Offset> { + template + inline static void Apply(T const* const* parameters, JetT* x) { + Make1stOrderPerturbation<0, N, Offset, T, JetT>::Apply( + parameters[ParameterIdx], x + Offset); + Make1stOrderPerturbations, + ParameterIdx + 1, + Offset + N>::Apply(parameters, x); + } +}; + +// End of 'recursion'. Nothing more to do. +template +struct Make1stOrderPerturbations, + ParameterIdx, + Total> { + template + static void Apply(T const* const* /* NOT USED */, JetT* /* NOT USED */) {} +}; + +// Takes the 0th order part of src, assumed to be a Jet type, and puts it in +// dst. This is used to pick out the "vector" part of the extended y. +template +inline void Take0thOrderPart(int M, const JetT* src, T dst) { + DCHECK(src); + for (int i = 0; i < M; ++i) { + dst[i] = src[i].a; + } +} + +// Takes N 1st order parts, starting at index N0, and puts them in the M x N +// matrix 'dst'. This is used to pick out the "matrix" parts of the extended y. +template +inline void Take1stOrderPart(const int M, const JetT* src, T* dst) { + DCHECK(src); + DCHECK(dst); + for (int i = 0; i < M; ++i) { + Eigen::Map>(dst + N * i, N) = + src[i].v.template segment(N0); + } +} + +// Calls Take1stOrderPart for every parameter block. +// +// Example: +// If one having three parameter blocks with dimensions (3, 2, 4), the call +// Take1stOrderParts::Apply(num_outputs, +// output, +// jacobians); +// will result in the following calls to Take1stOrderPart: +// if (jacobians[0]) { +// Take1stOrderPart<0, 3>(num_outputs, output, jacobians[0]); +// } +// if (jacobians[1]) { +// Take1stOrderPart<3, 2>(num_outputs, output, jacobians[1]); +// } +// if (jacobians[2]) { +// Take1stOrderPart<5, 4>(num_outputs, output, jacobians[2]); +// } +template +struct Take1stOrderParts; + +template +struct Take1stOrderParts, + ParameterIdx, + Offset> { + template + inline static void Apply(int num_outputs, JetT* output, T** jacobians) { + if (jacobians[ParameterIdx]) { + Take1stOrderPart(num_outputs, output, jacobians[ParameterIdx]); + } + Take1stOrderParts, + ParameterIdx + 1, + Offset + N>::Apply(num_outputs, output, jacobians); + } +}; + +// End of 'recursion'. Nothing more to do. +template +struct Take1stOrderParts, ParameterIdx, Offset> { + template + static void Apply(int /* NOT USED*/, + JetT* /* NOT USED*/, + T** /* NOT USED */) {} +}; + +template +inline bool AutoDifferentiate(const Functor& functor, + T const* const* parameters, + int dynamic_num_outputs, + T* function_value, + T** jacobians) { + using JetT = Jet; + using Parameters = typename ParameterDims::Parameters; + + if (kNumResiduals != DYNAMIC) { + DCHECK_EQ(kNumResiduals, dynamic_num_outputs); + } + + ArraySelector + parameters_as_jets(ParameterDims::kNumParameters); + + // Pointers to the beginning of each parameter block + std::array unpacked_parameters = + ParameterDims::GetUnpackedParameters(parameters_as_jets.data()); + + // If the number of residuals is fixed, we use the template argument as the + // number of outputs. Otherwise we use the num_outputs parameter. Note: The + // ?-operator here is compile-time evaluated, therefore num_outputs is also + // a compile-time constant for functors with fixed residuals. + const int num_outputs = + kNumResiduals == DYNAMIC ? dynamic_num_outputs : kNumResiduals; + DCHECK_GT(num_outputs, 0); + + ArraySelector + residuals_as_jets(num_outputs); + + // Invalidate the output Jets, so that we can detect if the user + // did not assign values to all of them. + for (int i = 0; i < num_outputs; ++i) { + residuals_as_jets[i].a = kImpossibleValue; + residuals_as_jets[i].v.setConstant(kImpossibleValue); + } + + Make1stOrderPerturbations::Apply(parameters, + parameters_as_jets.data()); + + if (!VariadicEvaluate( + functor, unpacked_parameters.data(), residuals_as_jets.data())) { + return false; + } + + Take0thOrderPart(num_outputs, residuals_as_jets.data(), function_value); + Take1stOrderParts::Apply( + num_outputs, residuals_as_jets.data(), jacobians); + + return true; +} + +} // namespace internal +} // namespace ceres + +#endif // CERES_PUBLIC_INTERNAL_AUTODIFF_H_ diff --git a/include/ceres/internal/config.h b/include/ceres/internal/config.h new file mode 100644 index 0000000000000000000000000000000000000000..a3aa08fdd7f2a248e67e2498199e24f47c2918fa --- /dev/null +++ b/include/ceres/internal/config.h @@ -0,0 +1,123 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2022 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: alexs.mac@gmail.com (Alex Stewart) + +// Configuration options for Ceres. +// +// Do not edit this file, it was automatically configured by CMake when +// Ceres was compiled with the relevant configuration for the machine +// on which Ceres was compiled. +// +// Ceres Developers: All options should have the same name as their mapped +// CMake options, in the preconfigured version of this file +// all options should be enclosed in '@'. + +#ifndef CERES_PUBLIC_INTERNAL_CONFIG_H_ +#define CERES_PUBLIC_INTERNAL_CONFIG_H_ + +// If defined, use the LGPL code in Eigen. +#define CERES_USE_EIGEN_SPARSE + +// If defined, Ceres was compiled without LAPACK. +// #define CERES_NO_LAPACK + +// If defined, Ceres was compiled without SuiteSparse. +// #define CERES_NO_SUITESPARSE + +// If defined, Ceres was compiled without CXSparse. +// #define CERES_NO_CXSPARSE + +// If defined, Ceres was compiled without CUDA. +// #define CERES_NO_CUDA + +// If defined, Ceres was compiled without Apple's Accelerate framework solvers. +#define CERES_NO_ACCELERATE_SPARSE + +#if defined(CERES_NO_SUITESPARSE) && \ + defined(CERES_NO_ACCELERATE_SPARSE) && \ + defined(CERES_NO_CXSPARSE) && \ + !defined(CERES_USE_EIGEN_SPARSE) // NOLINT +// If defined Ceres was compiled without any sparse linear algebra support. +#define CERES_NO_SPARSE +#endif + +// If defined, Ceres was compiled without Schur specializations. +// #define CERES_RESTRICT_SCHUR_SPECIALIZATION + +// If defined, Ceres was compiled to use Eigen instead of hardcoded BLAS +// routines. +// #define CERES_NO_CUSTOM_BLAS + +// If defined, Ceres was compiled without multithreading support. +// #define CERES_NO_THREADS +// If defined Ceres was compiled with OpenMP multithreading. +// #define CERES_USE_OPENMP +// If defined Ceres was compiled with modern C++ multithreading. +#define CERES_USE_CXX_THREADS + +// If defined, Ceres was compiled with a version MSVC >= 2005 which +// deprecated the standard POSIX names for bessel functions, replacing them +// with underscore prefixed versions (e.g. j0() -> _j0()). +// #define CERES_MSVC_USE_UNDERSCORE_PREFIXED_BESSEL_FUNCTIONS + +#if defined(CERES_USE_OPENMP) +#if defined(CERES_USE_CXX_THREADS) || defined(CERES_NO_THREADS) +#error CERES_USE_OPENMP is mutually exclusive to CERES_USE_CXX_THREADS and CERES_NO_THREADS +#endif +#elif defined(CERES_USE_CXX_THREADS) +#if defined(CERES_USE_OPENMP) || defined(CERES_NO_THREADS) +#error CERES_USE_CXX_THREADS is mutually exclusive to CERES_USE_OPENMP, CERES_USE_CXX_THREADS and CERES_NO_THREADS +#endif +#elif defined(CERES_NO_THREADS) +#if defined(CERES_USE_OPENMP) || defined(CERES_USE_CXX_THREADS) +#error CERES_NO_THREADS is mutually exclusive to CERES_USE_OPENMP and CERES_USE_CXX_THREADS +#endif +#else +# error One of CERES_USE_OPENMP, CERES_USE_CXX_THREADS or CERES_NO_THREADS must be defined. +#endif + +// CERES_NO_SPARSE should be automatically defined by config.h if Ceres was +// compiled without any sparse back-end. Verify that it has not subsequently +// been inconsistently redefined. +#if defined(CERES_NO_SPARSE) +#if !defined(CERES_NO_SUITESPARSE) +#error CERES_NO_SPARSE requires CERES_NO_SUITESPARSE. +#endif +#if !defined(CERES_NO_CXSPARSE) +#error CERES_NO_SPARSE requires CERES_NO_CXSPARSE +#endif +#if !defined(CERES_NO_ACCELERATE_SPARSE) +#error CERES_NO_SPARSE requires CERES_NO_ACCELERATE_SPARSE +#endif +#if defined(CERES_USE_EIGEN_SPARSE) +#error CERES_NO_SPARSE requires !CERES_USE_EIGEN_SPARSE +#endif +#endif + +#endif // CERES_PUBLIC_INTERNAL_CONFIG_H_ diff --git a/include/ceres/internal/disable_warnings.h b/include/ceres/internal/disable_warnings.h new file mode 100644 index 0000000000000000000000000000000000000000..d7766a0a08fdbec186f389dfe84e7ca952e62fa5 --- /dev/null +++ b/include/ceres/internal/disable_warnings.h @@ -0,0 +1,44 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// This file has the sole purpose to silence warnings when including Ceres. + +// This is not your usual header guard. The macro CERES_WARNINGS_DISABLED +// shows up again in reenable_warnings.h. +#ifndef CERES_WARNINGS_DISABLED +#define CERES_WARNINGS_DISABLED + +#ifdef _MSC_VER +#pragma warning(push) +// Disable the warning C4251 which is triggered by stl classes in +// Ceres' public interface. To quote MSDN: "C4251 can be ignored " +// "if you are deriving from a type in the Standard C++ Library" +#pragma warning(disable : 4251) +#endif + +#endif // CERES_WARNINGS_DISABLED diff --git a/include/ceres/internal/eigen.h b/include/ceres/internal/eigen.h new file mode 100644 index 0000000000000000000000000000000000000000..111cc7a07bb4735768cbe1fe760df5e667cc7a08 --- /dev/null +++ b/include/ceres/internal/eigen.h @@ -0,0 +1,75 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_EIGEN_H_ +#define CERES_INTERNAL_EIGEN_H_ + +#include "Eigen/Core" + +namespace ceres { + +using Vector = Eigen::Matrix; +using Matrix = + Eigen::Matrix; +using VectorRef = Eigen::Map; +using MatrixRef = Eigen::Map; +using ConstVectorRef = Eigen::Map; +using ConstMatrixRef = Eigen::Map; + +// Column major matrices for DenseSparseMatrix/DenseQRSolver +using ColMajorMatrix = + Eigen::Matrix; + +using ColMajorMatrixRef = + Eigen::Map>; + +using ConstColMajorMatrixRef = + Eigen::Map>; + +// C++ does not support templated typdefs, thus the need for this +// struct so that we can support statically sized Matrix and Maps. +template +struct EigenTypes { + using Matrix = + Eigen::Matrix; + + using MatrixRef = Eigen::Map; + using ConstMatrixRef = Eigen::Map; + using Vector = Eigen::Matrix; + using VectorRef = Eigen::Map>; + using ConstVectorRef = Eigen::Map>; +}; + +} // namespace ceres + +#endif // CERES_INTERNAL_EIGEN_H_ diff --git a/include/ceres/internal/export.h b/include/ceres/internal/export.h new file mode 100644 index 0000000000000000000000000000000000000000..c85bc5ca65dc763637ed31fa9b2ca6fb25fb54f8 --- /dev/null +++ b/include/ceres/internal/export.h @@ -0,0 +1,42 @@ + +#ifndef CERES_EXPORT_H +#define CERES_EXPORT_H + +#ifdef CERES_STATIC_DEFINE +# define CERES_EXPORT +# define CERES_NO_EXPORT +#else +# ifndef CERES_EXPORT +# ifdef ceres_EXPORTS + /* We are building this library */ +# define CERES_EXPORT +# else + /* We are using this library */ +# define CERES_EXPORT +# endif +# endif + +# ifndef CERES_NO_EXPORT +# define CERES_NO_EXPORT +# endif +#endif + +#ifndef CERES_DEPRECATED +# define CERES_DEPRECATED __attribute__ ((__deprecated__)) +#endif + +#ifndef CERES_DEPRECATED_EXPORT +# define CERES_DEPRECATED_EXPORT CERES_EXPORT CERES_DEPRECATED +#endif + +#ifndef CERES_DEPRECATED_NO_EXPORT +# define CERES_DEPRECATED_NO_EXPORT CERES_NO_EXPORT CERES_DEPRECATED +#endif + +#if 0 /* DEFINE_NO_DEPRECATED */ +# ifndef CERES_NO_DEPRECATED +# define CERES_NO_DEPRECATED +# endif +#endif + +#endif /* CERES_EXPORT_H */ diff --git a/include/ceres/internal/fixed_array.h b/include/ceres/internal/fixed_array.h new file mode 100644 index 0000000000000000000000000000000000000000..dcbddcd3a1d14698dc2d07799b842aa9c9f111d6 --- /dev/null +++ b/include/ceres/internal/fixed_array.h @@ -0,0 +1,467 @@ +// Copyright 2018 The Abseil Authors. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// https://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// ----------------------------------------------------------------------------- +// File: fixed_array.h +// ----------------------------------------------------------------------------- +// +// A `FixedArray` represents a non-resizable array of `T` where the length of +// the array can be determined at run-time. It is a good replacement for +// non-standard and deprecated uses of `alloca()` and variable length arrays +// within the GCC extension. (See +// https://gcc.gnu.org/onlinedocs/gcc/Variable-Length.html). +// +// `FixedArray` allocates small arrays inline, keeping performance fast by +// avoiding heap operations. It also helps reduce the chances of +// accidentally overflowing your stack if large input is passed to +// your function. + +#ifndef CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_ +#define CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_ + +#include // For Eigen::aligned_allocator +#include +#include +#include +#include +#include +#include + +#include "ceres/internal/memory.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +constexpr static auto kFixedArrayUseDefault = static_cast(-1); + +// The default fixed array allocator. +// +// As one can not easily detect if a struct contains or inherits from a fixed +// size Eigen type, to be safe the Eigen::aligned_allocator is used by default. +// But trivial types can never contain Eigen types, so std::allocator is used to +// safe some heap memory. +template +using FixedArrayDefaultAllocator = + typename std::conditional::value, + std::allocator, + Eigen::aligned_allocator>::type; + +// ----------------------------------------------------------------------------- +// FixedArray +// ----------------------------------------------------------------------------- +// +// A `FixedArray` provides a run-time fixed-size array, allocating a small array +// inline for efficiency. +// +// Most users should not specify an `inline_elements` argument and let +// `FixedArray` automatically determine the number of elements +// to store inline based on `sizeof(T)`. If `inline_elements` is specified, the +// `FixedArray` implementation will use inline storage for arrays with a +// length <= `inline_elements`. +// +// Note that a `FixedArray` constructed with a `size_type` argument will +// default-initialize its values by leaving trivially constructible types +// uninitialized (e.g. int, int[4], double), and others default-constructed. +// This matches the behavior of c-style arrays and `std::array`, but not +// `std::vector`. +// +// Note that `FixedArray` does not provide a public allocator; if it requires a +// heap allocation, it will do so with global `::operator new[]()` and +// `::operator delete[]()`, even if T provides class-scope overrides for these +// operators. +template > +class FixedArray { + static_assert(!std::is_array::value || std::extent::value > 0, + "Arrays with unknown bounds cannot be used with FixedArray."); + + static constexpr size_t kInlineBytesDefault = 256; + + using AllocatorTraits = std::allocator_traits; + // std::iterator_traits isn't guaranteed to be SFINAE-friendly until C++17, + // but this seems to be mostly pedantic. + template + using EnableIfForwardIterator = typename std::enable_if::iterator_category, + std::forward_iterator_tag>::value>::type; + static constexpr bool DefaultConstructorIsNonTrivial() { + return !std::is_trivially_default_constructible::value; + } + + public: + using allocator_type = typename AllocatorTraits::allocator_type; + using value_type = typename AllocatorTraits::value_type; + using pointer = typename AllocatorTraits::pointer; + using const_pointer = typename AllocatorTraits::const_pointer; + using reference = value_type&; + using const_reference = const value_type&; + using size_type = typename AllocatorTraits::size_type; + using difference_type = typename AllocatorTraits::difference_type; + using iterator = pointer; + using const_iterator = const_pointer; + using reverse_iterator = std::reverse_iterator; + using const_reverse_iterator = std::reverse_iterator; + + static constexpr size_type inline_elements = + (N == kFixedArrayUseDefault ? kInlineBytesDefault / sizeof(value_type) + : static_cast(N)); + + FixedArray(const FixedArray& other, + const allocator_type& a = allocator_type()) + : FixedArray(other.begin(), other.end(), a) {} + + FixedArray(FixedArray&& other, const allocator_type& a = allocator_type()) + : FixedArray(std::make_move_iterator(other.begin()), + std::make_move_iterator(other.end()), + a) {} + + // Creates an array object that can store `n` elements. + // Note that trivially constructible elements will be uninitialized. + explicit FixedArray(size_type n, const allocator_type& a = allocator_type()) + : storage_(n, a) { + if (DefaultConstructorIsNonTrivial()) { + ConstructRange(storage_.alloc(), storage_.begin(), storage_.end()); + } + } + + // Creates an array initialized with `n` copies of `val`. + FixedArray(size_type n, + const value_type& val, + const allocator_type& a = allocator_type()) + : storage_(n, a) { + ConstructRange(storage_.alloc(), storage_.begin(), storage_.end(), val); + } + + // Creates an array initialized with the size and contents of `init_list`. + FixedArray(std::initializer_list init_list, + const allocator_type& a = allocator_type()) + : FixedArray(init_list.begin(), init_list.end(), a) {} + + // Creates an array initialized with the elements from the input + // range. The array's size will always be `std::distance(first, last)`. + // REQUIRES: Iterator must be a forward_iterator or better. + template * = nullptr> + FixedArray(Iterator first, + Iterator last, + const allocator_type& a = allocator_type()) + : storage_(std::distance(first, last), a) { + CopyRange(storage_.alloc(), storage_.begin(), first, last); + } + + ~FixedArray() noexcept { + for (auto* cur = storage_.begin(); cur != storage_.end(); ++cur) { + AllocatorTraits::destroy(storage_.alloc(), cur); + } + } + + // Assignments are deleted because they break the invariant that the size of a + // `FixedArray` never changes. + void operator=(FixedArray&&) = delete; + void operator=(const FixedArray&) = delete; + + // FixedArray::size() + // + // Returns the length of the fixed array. + size_type size() const { return storage_.size(); } + + // FixedArray::max_size() + // + // Returns the largest possible value of `std::distance(begin(), end())` for a + // `FixedArray`. This is equivalent to the most possible addressable bytes + // over the number of bytes taken by T. + constexpr size_type max_size() const { + return (std::numeric_limits::max)() / sizeof(value_type); + } + + // FixedArray::empty() + // + // Returns whether or not the fixed array is empty. + bool empty() const { return size() == 0; } + + // FixedArray::memsize() + // + // Returns the memory size of the fixed array in bytes. + size_t memsize() const { return size() * sizeof(value_type); } + + // FixedArray::data() + // + // Returns a const T* pointer to elements of the `FixedArray`. This pointer + // can be used to access (but not modify) the contained elements. + const_pointer data() const { return AsValueType(storage_.begin()); } + + // Overload of FixedArray::data() to return a T* pointer to elements of the + // fixed array. This pointer can be used to access and modify the contained + // elements. + pointer data() { return AsValueType(storage_.begin()); } + + // FixedArray::operator[] + // + // Returns a reference the ith element of the fixed array. + // REQUIRES: 0 <= i < size() + reference operator[](size_type i) { + DCHECK_LT(i, size()); + return data()[i]; + } + + // Overload of FixedArray::operator()[] to return a const reference to the + // ith element of the fixed array. + // REQUIRES: 0 <= i < size() + const_reference operator[](size_type i) const { + DCHECK_LT(i, size()); + return data()[i]; + } + + // FixedArray::front() + // + // Returns a reference to the first element of the fixed array. + reference front() { return *begin(); } + + // Overload of FixedArray::front() to return a reference to the first element + // of a fixed array of const values. + const_reference front() const { return *begin(); } + + // FixedArray::back() + // + // Returns a reference to the last element of the fixed array. + reference back() { return *(end() - 1); } + + // Overload of FixedArray::back() to return a reference to the last element + // of a fixed array of const values. + const_reference back() const { return *(end() - 1); } + + // FixedArray::begin() + // + // Returns an iterator to the beginning of the fixed array. + iterator begin() { return data(); } + + // Overload of FixedArray::begin() to return a const iterator to the + // beginning of the fixed array. + const_iterator begin() const { return data(); } + + // FixedArray::cbegin() + // + // Returns a const iterator to the beginning of the fixed array. + const_iterator cbegin() const { return begin(); } + + // FixedArray::end() + // + // Returns an iterator to the end of the fixed array. + iterator end() { return data() + size(); } + + // Overload of FixedArray::end() to return a const iterator to the end of the + // fixed array. + const_iterator end() const { return data() + size(); } + + // FixedArray::cend() + // + // Returns a const iterator to the end of the fixed array. + const_iterator cend() const { return end(); } + + // FixedArray::rbegin() + // + // Returns a reverse iterator from the end of the fixed array. + reverse_iterator rbegin() { return reverse_iterator(end()); } + + // Overload of FixedArray::rbegin() to return a const reverse iterator from + // the end of the fixed array. + const_reverse_iterator rbegin() const { + return const_reverse_iterator(end()); + } + + // FixedArray::crbegin() + // + // Returns a const reverse iterator from the end of the fixed array. + const_reverse_iterator crbegin() const { return rbegin(); } + + // FixedArray::rend() + // + // Returns a reverse iterator from the beginning of the fixed array. + reverse_iterator rend() { return reverse_iterator(begin()); } + + // Overload of FixedArray::rend() for returning a const reverse iterator + // from the beginning of the fixed array. + const_reverse_iterator rend() const { + return const_reverse_iterator(begin()); + } + + // FixedArray::crend() + // + // Returns a reverse iterator from the beginning of the fixed array. + const_reverse_iterator crend() const { return rend(); } + + // FixedArray::fill() + // + // Assigns the given `value` to all elements in the fixed array. + void fill(const value_type& val) { std::fill(begin(), end(), val); } + + // Relational operators. Equality operators are elementwise using + // `operator==`, while order operators order FixedArrays lexicographically. + friend bool operator==(const FixedArray& lhs, const FixedArray& rhs) { + return std::equal(lhs.begin(), lhs.end(), rhs.begin(), rhs.end()); + } + + friend bool operator!=(const FixedArray& lhs, const FixedArray& rhs) { + return !(lhs == rhs); + } + + friend bool operator<(const FixedArray& lhs, const FixedArray& rhs) { + return std::lexicographical_compare( + lhs.begin(), lhs.end(), rhs.begin(), rhs.end()); + } + + friend bool operator>(const FixedArray& lhs, const FixedArray& rhs) { + return rhs < lhs; + } + + friend bool operator<=(const FixedArray& lhs, const FixedArray& rhs) { + return !(rhs < lhs); + } + + friend bool operator>=(const FixedArray& lhs, const FixedArray& rhs) { + return !(lhs < rhs); + } + + private: + // StorageElement + // + // For FixedArrays with a C-style-array value_type, StorageElement is a POD + // wrapper struct called StorageElementWrapper that holds the value_type + // instance inside. This is needed for construction and destruction of the + // entire array regardless of how many dimensions it has. For all other cases, + // StorageElement is just an alias of value_type. + // + // Maintainer's Note: The simpler solution would be to simply wrap value_type + // in a struct whether it's an array or not. That causes some paranoid + // diagnostics to misfire, believing that 'data()' returns a pointer to a + // single element, rather than the packed array that it really is. + // e.g.: + // + // FixedArray buf(1); + // sprintf(buf.data(), "foo"); + // + // error: call to int __builtin___sprintf_chk(etc...) + // will always overflow destination buffer [-Werror] + // + template ::type, + size_t InnerN = std::extent::value> + struct StorageElementWrapper { + InnerT array[InnerN]; + }; + + using StorageElement = + typename std::conditional::value, + StorageElementWrapper, + value_type>::type; + + static pointer AsValueType(pointer ptr) { return ptr; } + static pointer AsValueType(StorageElementWrapper* ptr) { + return std::addressof(ptr->array); + } + + static_assert(sizeof(StorageElement) == sizeof(value_type), ""); + static_assert(alignof(StorageElement) == alignof(value_type), ""); + + class NonEmptyInlinedStorage { + public: + StorageElement* data() { return reinterpret_cast(buff_); } + void AnnotateConstruct(size_type) {} + void AnnotateDestruct(size_type) {} + + // #ifdef ADDRESS_SANITIZER + // void* RedzoneBegin() { return &redzone_begin_; } + // void* RedzoneEnd() { return &redzone_end_ + 1; } + // #endif // ADDRESS_SANITIZER + + private: + // ADDRESS_SANITIZER_REDZONE(redzone_begin_); + alignas(StorageElement) char buff_[sizeof(StorageElement[inline_elements])]; + // ADDRESS_SANITIZER_REDZONE(redzone_end_); + }; + + class EmptyInlinedStorage { + public: + StorageElement* data() { return nullptr; } + void AnnotateConstruct(size_type) {} + void AnnotateDestruct(size_type) {} + }; + + using InlinedStorage = + typename std::conditional::type; + + // Storage + // + // An instance of Storage manages the inline and out-of-line memory for + // instances of FixedArray. This guarantees that even when construction of + // individual elements fails in the FixedArray constructor body, the + // destructor for Storage will still be called and out-of-line memory will be + // properly deallocated. + // + class Storage : public InlinedStorage { + public: + Storage(size_type n, const allocator_type& a) + : size_alloc_(n, a), data_(InitializeData()) {} + + ~Storage() noexcept { + if (UsingInlinedStorage(size())) { + InlinedStorage::AnnotateDestruct(size()); + } else { + AllocatorTraits::deallocate(alloc(), AsValueType(begin()), size()); + } + } + + size_type size() const { return std::get<0>(size_alloc_); } + StorageElement* begin() const { return data_; } + StorageElement* end() const { return begin() + size(); } + allocator_type& alloc() { return std::get<1>(size_alloc_); } + + private: + static bool UsingInlinedStorage(size_type n) { + return n <= inline_elements; + } + + StorageElement* InitializeData() { + if (UsingInlinedStorage(size())) { + InlinedStorage::AnnotateConstruct(size()); + return InlinedStorage::data(); + } else { + return reinterpret_cast( + AllocatorTraits::allocate(alloc(), size())); + } + } + + // Using std::tuple and not absl::CompressedTuple, as it has a lot of + // dependencies to other absl headers. + std::tuple size_alloc_; + StorageElement* data_; + }; + + Storage storage_; +}; + +template +constexpr size_t FixedArray::kInlineBytesDefault; + +template +constexpr typename FixedArray::size_type + FixedArray::inline_elements; + +} // namespace internal +} // namespace ceres + +#endif // CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_ diff --git a/include/ceres/internal/householder_vector.h b/include/ceres/internal/householder_vector.h new file mode 100644 index 0000000000000000000000000000000000000000..7700208be222e0e14742a635d6405fa753a411a3 --- /dev/null +++ b/include/ceres/internal/householder_vector.h @@ -0,0 +1,96 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: vitus@google.com (Michael Vitus) + +#ifndef CERES_PUBLIC_INTERNAL_HOUSEHOLDER_VECTOR_H_ +#define CERES_PUBLIC_INTERNAL_HOUSEHOLDER_VECTOR_H_ + +#include "Eigen/Core" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +// Algorithm 5.1.1 from 'Matrix Computations' by Golub et al. (Johns Hopkins +// Studies in Mathematical Sciences) but using the nth element of the input +// vector as pivot instead of first. This computes the vector v with v(n) = 1 +// and beta such that H = I - beta * v * v^T is orthogonal and +// H * x = ||x||_2 * e_n. +// +// NOTE: Some versions of MSVC have trouble deducing the type of v if +// you do not specify all the template arguments explicitly. +template +void ComputeHouseholderVector(const XVectorType& x, + Eigen::Matrix* v, + Scalar* beta) { + CHECK(beta != nullptr); + CHECK(v != nullptr); + CHECK_GT(x.rows(), 1); + CHECK_EQ(x.rows(), v->rows()); + + Scalar sigma = x.head(x.rows() - 1).squaredNorm(); + *v = x; + (*v)(v->rows() - 1) = Scalar(1.0); + + *beta = Scalar(0.0); + const Scalar& x_pivot = x(x.rows() - 1); + + if (sigma <= Scalar(std::numeric_limits::epsilon())) { + if (x_pivot < Scalar(0.0)) { + *beta = Scalar(2.0); + } + return; + } + + const Scalar mu = sqrt(x_pivot * x_pivot + sigma); + Scalar v_pivot = Scalar(1.0); + + if (x_pivot <= Scalar(0.0)) { + v_pivot = x_pivot - mu; + } else { + v_pivot = -sigma / (x_pivot + mu); + } + + *beta = Scalar(2.0) * v_pivot * v_pivot / (sigma + v_pivot * v_pivot); + + v->head(v->rows() - 1) /= v_pivot; +} + +template +typename Derived::PlainObject ApplyHouseholderVector( + const XVectorType& y, + const Eigen::MatrixBase& v, + const typename Derived::Scalar& beta) { + return (y - v * (beta * (v.transpose() * y))); +} + +} // namespace internal +} // namespace ceres + +#endif // CERES_PUBLIC_INTERNAL_HOUSEHOLDER_VECTOR_H_ diff --git a/include/ceres/internal/integer_sequence_algorithm.h b/include/ceres/internal/integer_sequence_algorithm.h new file mode 100644 index 0000000000000000000000000000000000000000..777c119a77fd195c6d5c1566b96b79b224f48451 --- /dev/null +++ b/include/ceres/internal/integer_sequence_algorithm.h @@ -0,0 +1,291 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2022 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: jodebo_beck@gmx.de (Johannes Beck) +// sergiu.deitsch@gmail.com (Sergiu Deitsch) +// +// Algorithms to be used together with integer_sequence, like computing the sum +// or the exclusive scan (sometimes called exclusive prefix sum) at compile +// time. + +#ifndef CERES_PUBLIC_INTERNAL_INTEGER_SEQUENCE_ALGORITHM_H_ +#define CERES_PUBLIC_INTERNAL_INTEGER_SEQUENCE_ALGORITHM_H_ + +#include + +#include "ceres/jet_fwd.h" + +namespace ceres { +namespace internal { + +// Implementation of calculating the sum of an integer sequence. +// Recursively instantiate SumImpl and calculate the sum of the N first +// numbers. This reduces the number of instantiations and speeds up +// compilation. +// +// Examples: +// 1) integer_sequence: +// Value = 5 +// +// 2) integer_sequence: +// Value = 4 + 2 + SumImpl>::Value +// Value = 4 + 2 + 0 +// +// 3) integer_sequence: +// Value = 2 + 1 + SumImpl>::Value +// Value = 2 + 1 + 4 +template +struct SumImpl; + +// Strip of and sum the first number. +template +struct SumImpl> { + static constexpr T Value = + N + SumImpl>::Value; +}; + +// Strip of and sum the first two numbers. +template +struct SumImpl> { + static constexpr T Value = + N1 + N2 + SumImpl>::Value; +}; + +// Strip of and sum the first four numbers. +template +struct SumImpl> { + static constexpr T Value = + N1 + N2 + N3 + N4 + SumImpl>::Value; +}; + +// Only one number is left. 'Value' is just that number ('recursion' ends). +template +struct SumImpl> { + static constexpr T Value = N; +}; + +// No number is left. 'Value' is the identity element (for sum this is zero). +template +struct SumImpl> { + static constexpr T Value = T(0); +}; + +// Calculate the sum of an integer sequence. The resulting sum will be stored in +// 'Value'. +template +class Sum { + using T = typename Seq::value_type; + + public: + static constexpr T Value = SumImpl::Value; +}; + +// Implementation of calculating an exclusive scan (exclusive prefix sum) of an +// integer sequence. Exclusive means that the i-th input element is not included +// in the i-th sum. Calculating the exclusive scan for an input array I results +// in the following output R: +// +// R[0] = 0 +// R[1] = I[0]; +// R[2] = I[0] + I[1]; +// R[3] = I[0] + I[1] + I[2]; +// ... +// +// In C++17 std::exclusive_scan does the same operation at runtime (but +// cannot be used to calculate the prefix sum at compile time). See +// https://en.cppreference.com/w/cpp/algorithm/exclusive_scan for a more +// detailed description. +// +// Example for integer_sequence (seq := integer_sequence): +// T , Sum, Ns... , Rs... +// ExclusiveScanImpl, seq> +// ExclusiveScanImpl, seq> +// ExclusiveScanImpl, seq> +// ExclusiveScanImpl, seq> +// ^^^^^^^^^^^^^^^^^ +// resulting sequence +template +struct ExclusiveScanImpl; + +template +struct ExclusiveScanImpl, + std::integer_sequence> { + using Type = + typename ExclusiveScanImpl, + std::integer_sequence>::Type; +}; + +// End of 'recursion'. The resulting type is SeqOut. +template +struct ExclusiveScanImpl, SeqOut> { + using Type = SeqOut; +}; + +// Calculates the exclusive scan of the specified integer sequence. The last +// element (the total) is not included in the resulting sequence so they have +// same length. This means the exclusive scan of integer_sequence +// will be integer_sequence. +template +class ExclusiveScanT { + using T = typename Seq::value_type; + + public: + using Type = + typename ExclusiveScanImpl>::Type; +}; + +// Helper to use exclusive scan without typename. +template +using ExclusiveScan = typename ExclusiveScanT::Type; + +// Removes all elements from a integer sequence corresponding to specified +// ValueToRemove. +// +// This type should not be used directly but instead RemoveValue. +template +struct RemoveValueImpl; + +// Final filtered sequence +template +struct RemoveValueImpl, + std::integer_sequence> { + using type = std::integer_sequence; +}; + +// Found a matching value +template +struct RemoveValueImpl, + std::integer_sequence> + : RemoveValueImpl, + std::integer_sequence> {}; + +// Move one element from the tail to the head +template +struct RemoveValueImpl, + std::integer_sequence> + : RemoveValueImpl, + std::integer_sequence> {}; + +// Start recursion by splitting the integer sequence into two separate ones +template +struct RemoveValueImpl> + : RemoveValueImpl, + std::integer_sequence> {}; + +// RemoveValue takes an integer Sequence of arbitrary type and removes all +// elements matching ValueToRemove. +// +// In contrast to RemoveValueImpl, this implementation deduces the value type +// eliminating the need to specify it explicitly. +// +// As an example, RemoveValue, 4>::type will +// not transform the type of the original sequence. However, +// RemoveValue, 2>::type will generate a new +// sequence of type std::integer_sequence by removing the value 2. +template +struct RemoveValue + : RemoveValueImpl { +}; + +// Convenience template alias for RemoveValue. +template +using RemoveValue_t = typename RemoveValue::type; + +// Determines whether the values of an integer sequence are all the same. +// +// The integer sequence must contain at least one value. The predicate is +// undefined for empty sequences. The evaluation result of the predicate for a +// sequence containing only one value is defined to be true. +template +struct AreAllEqual; + +// The predicate result for a sequence containing one element is defined to be +// true. +template +struct AreAllEqual> : std::true_type {}; + +// Recursion end. +template +struct AreAllEqual> + : std::integral_constant {}; + +// Recursion for sequences containing at least two elements. +template +// clang-format off +struct AreAllEqual > + : std::integral_constant +< + bool, + AreAllEqual >::value && + AreAllEqual >::value +> +// clang-format on +{}; + +// Convenience variable template for AreAllEqual. +template +constexpr bool AreAllEqual_v = AreAllEqual::value; + +// Predicate determining whether an integer sequence is either empty or all +// values are equal. +template +struct IsEmptyOrAreAllEqual; + +// Empty case. +template +struct IsEmptyOrAreAllEqual> : std::true_type {}; + +// General case for sequences containing at least one value. +template +struct IsEmptyOrAreAllEqual> + : AreAllEqual> {}; + +// Convenience variable template for IsEmptyOrAreAllEqual. +template +constexpr bool IsEmptyOrAreAllEqual_v = IsEmptyOrAreAllEqual::value; + +} // namespace internal +} // namespace ceres + +#endif // CERES_PUBLIC_INTERNAL_INTEGER_SEQUENCE_ALGORITHM_H_ diff --git a/include/ceres/internal/jet_traits.h b/include/ceres/internal/jet_traits.h new file mode 100644 index 0000000000000000000000000000000000000000..2a38c05b7dab672b9807ee9a7502fb9c795f9ac5 --- /dev/null +++ b/include/ceres/internal/jet_traits.h @@ -0,0 +1,223 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2022 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sergiu.deitsch@gmail.com (Sergiu Deitsch) +// + +#ifndef CERES_PUBLIC_INTERNAL_JET_TRAITS_H_ +#define CERES_PUBLIC_INTERNAL_JET_TRAITS_H_ + +#include +#include +#include + +#include "ceres/internal/integer_sequence_algorithm.h" +#include "ceres/jet_fwd.h" + +namespace ceres { +namespace internal { + +// Predicate that determines whether T is a Jet. +template +struct IsJet : std::false_type {}; + +template +struct IsJet> : std::true_type {}; + +// Convenience variable template for IsJet. +template +constexpr bool IsJet_v = IsJet::value; + +// Predicate that determines whether any of the Types is a Jet. +template +struct AreAnyJet : std::false_type {}; + +template +struct AreAnyJet : AreAnyJet {}; + +template +struct AreAnyJet, Types...> : std::true_type {}; + +// Convenience variable template for AreAnyJet. +template +constexpr bool AreAnyJet_v = AreAnyJet::value; + +// Extracts the underlying floating-point from a type T. +template +struct UnderlyingScalar { + using type = T; +}; + +template +struct UnderlyingScalar> : UnderlyingScalar {}; + +// Convenience template alias for UnderlyingScalar type trait. +template +using UnderlyingScalar_t = typename UnderlyingScalar::type; + +// Predicate determining whether all Types in the pack are the same. +// +// Specifically, the predicate applies std::is_same recursively to pairs of +// Types in the pack. +// +// The predicate is defined only for template packs containing at least two +// types. +template +// clang-format off +struct AreAllSame : std::integral_constant +< + bool, + AreAllSame::value && + AreAllSame::value +> +// clang-format on +{}; + +// AreAllSame pairwise test. +template +struct AreAllSame : std::is_same {}; + +// Convenience variable template for AreAllSame. +template +constexpr bool AreAllSame_v = AreAllSame::value; + +// Determines the rank of a type. This allows to ensure that types passed as +// arguments are compatible to each other. The rank of Jet is determined by the +// dimensions of the dual part. The rank of a scalar is always 0. +// Non-specialized types default to a rank of -1. +template +struct Rank : std::integral_constant {}; + +// The rank of a scalar is 0. +template +struct Rank::value>> + : std::integral_constant {}; + +// The rank of a Jet is given by its dimensionality. +template +struct Rank> : std::integral_constant {}; + +// Convenience variable template for Rank. +template +constexpr int Rank_v = Rank::value; + +// Constructs an integer sequence of ranks for each of the Types in the pack. +template +using Ranks_t = std::integer_sequence...>; + +// Returns the scalar part of a type. This overload acts as an identity. +template +constexpr decltype(auto) AsScalar(T&& value) noexcept { + return std::forward(value); +} + +// Recursively unwraps the scalar part of a Jet until a non-Jet scalar type is +// encountered. +template +constexpr decltype(auto) AsScalar(const Jet& value) noexcept( + noexcept(AsScalar(value.a))) { + return AsScalar(value.a); +} + +} // namespace internal + +// Type trait ensuring at least one of the types is a Jet, +// the underlying scalar types are the same and Jet dimensions match. +// +// The type trait can be further specialized if necessary. +// +// This trait is a candidate for a concept definition once C++20 features can +// be used. +template +// clang-format off +struct CompatibleJetOperands : std::integral_constant +< + bool, + // At least one of the types is a Jet + internal::AreAnyJet_v && + // The underlying floating-point types are exactly the same + internal::AreAllSame_v...> && + // Non-zero ranks of types are equal + internal::IsEmptyOrAreAllEqual_v, 0>> +> +// clang-format on +{}; + +// Single Jet operand is always compatible. +template +struct CompatibleJetOperands> : std::true_type {}; + +// Single non-Jet operand is always incompatible. +template +struct CompatibleJetOperands : std::false_type {}; + +// Empty operands are always incompatible. +template <> +struct CompatibleJetOperands<> : std::false_type {}; + +// Convenience variable template ensuring at least one of the types is a Jet, +// the underlying scalar types are the same and Jet dimensions match. +// +// This trait is a candidate for a concept definition once C++20 features can +// be used. +template +constexpr bool CompatibleJetOperands_v = CompatibleJetOperands::value; + +// Type trait ensuring at least one of the types is a Jet, +// the underlying scalar types are compatible among each other and Jet +// dimensions match. +// +// The type trait can be further specialized if necessary. +// +// This trait is a candidate for a concept definition once C++20 features can +// be used. +template +// clang-format off +struct PromotableJetOperands : std::integral_constant +< + bool, + // Types can be compatible among each other + internal::AreAnyJet_v && + // Non-zero ranks of types are equal + internal::IsEmptyOrAreAllEqual_v, 0>> +> +// clang-format on +{}; + +// Convenience variable template ensuring at least one of the types is a Jet, +// the underlying scalar types are compatible among each other and Jet +// dimensions match. +// +// This trait is a candidate for a concept definition once C++20 features can +// be used. +template +constexpr bool PromotableJetOperands_v = PromotableJetOperands::value; + +} // namespace ceres + +#endif // CERES_PUBLIC_INTERNAL_JET_TRAITS_H_ diff --git a/include/ceres/internal/line_parameterization.h b/include/ceres/internal/line_parameterization.h new file mode 100644 index 0000000000000000000000000000000000000000..eda390148df9b5de170f2d2c2b867941c865399d --- /dev/null +++ b/include/ceres/internal/line_parameterization.h @@ -0,0 +1,183 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2020 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: jodebo_beck@gmx.de (Johannes Beck) +// + +#ifndef CERES_PUBLIC_INTERNAL_LINE_PARAMETERIZATION_H_ +#define CERES_PUBLIC_INTERNAL_LINE_PARAMETERIZATION_H_ + +#include "householder_vector.h" + +namespace ceres { + +template +bool LineParameterization::Plus( + const double* x_ptr, + const double* delta_ptr, + double* x_plus_delta_ptr) const { + // We seek a box plus operator of the form + // + // [o*, d*] = Plus([o, d], [delta_o, delta_d]) + // + // where o is the origin point, d is the direction vector, delta_o is + // the delta of the origin point and delta_d the delta of the direction and + // o* and d* is the updated origin point and direction. + // + // We separate the Plus operator into the origin point and directional part + // d* = Plus_d(d, delta_d) + // o* = Plus_o(o, d, delta_o) + // + // The direction update function Plus_d is the same as for the homogeneous + // vector parameterization: + // + // d* = H_{v(d)} [0.5 sinc(0.5 |delta_d|) delta_d, cos(0.5 |delta_d|)]^T + // + // where H is the householder matrix + // H_{v} = I - (2 / |v|^2) v v^T + // and + // v(d) = d - sign(d_n) |d| e_n. + // + // The origin point update function Plus_o is defined as + // + // o* = o + H_{v(d)} [0.5 delta_o, 0]^T. + + static constexpr int kDim = AmbientSpaceDimension; + using AmbientVector = Eigen::Matrix; + using AmbientVectorRef = Eigen::Map>; + using ConstAmbientVectorRef = + Eigen::Map>; + using ConstTangentVectorRef = + Eigen::Map>; + + ConstAmbientVectorRef o(x_ptr); + ConstAmbientVectorRef d(x_ptr + kDim); + + ConstTangentVectorRef delta_o(delta_ptr); + ConstTangentVectorRef delta_d(delta_ptr + kDim - 1); + AmbientVectorRef o_plus_delta(x_plus_delta_ptr); + AmbientVectorRef d_plus_delta(x_plus_delta_ptr + kDim); + + const double norm_delta_d = delta_d.norm(); + + o_plus_delta = o; + + // Shortcut for zero delta direction. + if (norm_delta_d == 0.0) { + d_plus_delta = d; + + if (delta_o.isZero(0.0)) { + return true; + } + } + + // Calculate the householder transformation which is needed for f_d and f_o. + AmbientVector v; + double beta; + + // NOTE: The explicit template arguments are needed here because + // ComputeHouseholderVector is templated and some versions of MSVC + // have trouble deducing the type of v automatically. + internal::ComputeHouseholderVector( + d, &v, &beta); + + if (norm_delta_d != 0.0) { + // Map the delta from the minimum representation to the over parameterized + // homogeneous vector. See section A6.9.2 on page 624 of Hartley & Zisserman + // (2nd Edition) for a detailed description. Note there is a typo on Page + // 625, line 4 so check the book errata. + const double norm_delta_div_2 = 0.5 * norm_delta_d; + const double sin_delta_by_delta = + std::sin(norm_delta_div_2) / norm_delta_div_2; + + // Apply the delta update to remain on the unit sphere. See section A6.9.3 + // on page 625 of Hartley & Zisserman (2nd Edition) for a detailed + // description. + AmbientVector y; + y.template head() = 0.5 * sin_delta_by_delta * delta_d; + y[kDim - 1] = std::cos(norm_delta_div_2); + + d_plus_delta = d.norm() * (y - v * (beta * (v.transpose() * y))); + } + + // The null space is in the direction of the line, so the tangent space is + // perpendicular to the line direction. This is achieved by using the + // householder matrix of the direction and allow only movements + // perpendicular to e_n. + // + // The factor of 0.5 is used to be consistent with the line direction + // update. + AmbientVector y; + y << 0.5 * delta_o, 0; + o_plus_delta += y - v * (beta * (v.transpose() * y)); + + return true; +} + +template +bool LineParameterization::ComputeJacobian( + const double* x_ptr, double* jacobian_ptr) const { + static constexpr int kDim = AmbientSpaceDimension; + using AmbientVector = Eigen::Matrix; + using ConstAmbientVectorRef = + Eigen::Map>; + using MatrixRef = Eigen::Map< + Eigen::Matrix>; + + ConstAmbientVectorRef d(x_ptr + kDim); + MatrixRef jacobian(jacobian_ptr); + + // Clear the Jacobian as only half of the matrix is not zero. + jacobian.setZero(); + + AmbientVector v; + double beta; + + // NOTE: The explicit template arguments are needed here because + // ComputeHouseholderVector is templated and some versions of MSVC + // have trouble deducing the type of v automatically. + internal::ComputeHouseholderVector( + d, &v, &beta); + + // The Jacobian is equal to J = 0.5 * H.leftCols(kDim - 1) where H is + // the Householder matrix (H = I - beta * v * v') for the origin point. For + // the line direction part the Jacobian is scaled by the norm of the + // direction. + for (int i = 0; i < kDim - 1; ++i) { + jacobian.block(0, i, kDim, 1) = -0.5 * beta * v(i) * v; + jacobian.col(i)(i) += 0.5; + } + + jacobian.template block(kDim, kDim - 1) = + jacobian.template block(0, 0) * d.norm(); + return true; +} + +} // namespace ceres + +#endif // CERES_PUBLIC_INTERNAL_LINE_PARAMETERIZATION_H_ diff --git a/include/ceres/internal/memory.h b/include/ceres/internal/memory.h new file mode 100644 index 0000000000000000000000000000000000000000..45c5b67c353db52a29345289470fee563c404754 --- /dev/null +++ b/include/ceres/internal/memory.h @@ -0,0 +1,90 @@ +// Copyright 2017 The Abseil Authors. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// https://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// ----------------------------------------------------------------------------- +// File: memory.h +// ----------------------------------------------------------------------------- +// +// This header file contains utility functions for managing the creation and +// conversion of smart pointers. This file is an extension to the C++ +// standard library header file. + +#ifndef CERES_PUBLIC_INTERNAL_MEMORY_H_ +#define CERES_PUBLIC_INTERNAL_MEMORY_H_ + +#include + +#ifdef CERES_HAVE_EXCEPTIONS +#define CERES_INTERNAL_TRY try +#define CERES_INTERNAL_CATCH_ANY catch (...) +#define CERES_INTERNAL_RETHROW \ + do { \ + throw; \ + } while (false) +#else // CERES_HAVE_EXCEPTIONS +#define CERES_INTERNAL_TRY if (true) +#define CERES_INTERNAL_CATCH_ANY else if (false) +#define CERES_INTERNAL_RETHROW \ + do { \ + } while (false) +#endif // CERES_HAVE_EXCEPTIONS + +namespace ceres { +namespace internal { + +template +void ConstructRange(Allocator& alloc, + Iterator first, + Iterator last, + const Args&... args) { + for (Iterator cur = first; cur != last; ++cur) { + CERES_INTERNAL_TRY { + std::allocator_traits::construct( + alloc, std::addressof(*cur), args...); + } + CERES_INTERNAL_CATCH_ANY { + while (cur != first) { + --cur; + std::allocator_traits::destroy(alloc, std::addressof(*cur)); + } + CERES_INTERNAL_RETHROW; + } + } +} + +template +void CopyRange(Allocator& alloc, + Iterator destination, + InputIterator first, + InputIterator last) { + for (Iterator cur = destination; first != last; + static_cast(++cur), static_cast(++first)) { + CERES_INTERNAL_TRY { + std::allocator_traits::construct( + alloc, std::addressof(*cur), *first); + } + CERES_INTERNAL_CATCH_ANY { + while (cur != destination) { + --cur; + std::allocator_traits::destroy(alloc, std::addressof(*cur)); + } + CERES_INTERNAL_RETHROW; + } + } +} + +} // namespace internal +} // namespace ceres + +#endif // CERES_PUBLIC_INTERNAL_MEMORY_H_ diff --git a/include/ceres/internal/numeric_diff.h b/include/ceres/internal/numeric_diff.h new file mode 100644 index 0000000000000000000000000000000000000000..351845c05fbd1a2cdb0350a74a51062976e9a792 --- /dev/null +++ b/include/ceres/internal/numeric_diff.h @@ -0,0 +1,508 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// mierle@gmail.com (Keir Mierle) +// tbennun@gmail.com (Tal Ben-Nun) +// +// Finite differencing routines used by NumericDiffCostFunction. + +#ifndef CERES_PUBLIC_INTERNAL_NUMERIC_DIFF_H_ +#define CERES_PUBLIC_INTERNAL_NUMERIC_DIFF_H_ + +#include +#include + +#include "Eigen/Dense" +#include "Eigen/StdVector" +#include "ceres/cost_function.h" +#include "ceres/internal/fixed_array.h" +#include "ceres/internal/variadic_evaluate.h" +#include "ceres/numeric_diff_options.h" +#include "ceres/types.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +// This is split from the main class because C++ doesn't allow partial template +// specializations for member functions. The alternative is to repeat the main +// class for differing numbers of parameters, which is also unfortunate. +template +struct NumericDiff { + // Mutates parameters but must restore them before return. + static bool EvaluateJacobianForParameterBlock( + const CostFunctor* functor, + const double* residuals_at_eval_point, + const NumericDiffOptions& options, + int num_residuals, + int parameter_block_index, + int parameter_block_size, + double** parameters, + double* jacobian) { + using Eigen::ColMajor; + using Eigen::Map; + using Eigen::Matrix; + using Eigen::RowMajor; + + DCHECK(jacobian); + + const int num_residuals_internal = + (kNumResiduals != ceres::DYNAMIC ? kNumResiduals : num_residuals); + const int parameter_block_index_internal = + (kParameterBlock != ceres::DYNAMIC ? kParameterBlock + : parameter_block_index); + const int parameter_block_size_internal = + (kParameterBlockSize != ceres::DYNAMIC ? kParameterBlockSize + : parameter_block_size); + + using ResidualVector = Matrix; + using ParameterVector = Matrix; + + // The convoluted reasoning for choosing the Row/Column major + // ordering of the matrix is an artifact of the restrictions in + // Eigen that prevent it from creating RowMajor matrices with a + // single column. In these cases, we ask for a ColMajor matrix. + using JacobianMatrix = + Matrix; + + Map parameter_jacobian( + jacobian, num_residuals_internal, parameter_block_size_internal); + + Map x_plus_delta( + parameters[parameter_block_index_internal], + parameter_block_size_internal); + ParameterVector x(x_plus_delta); + ParameterVector step_size = + x.array().abs() * ((kMethod == RIDDERS) + ? options.ridders_relative_initial_step_size + : options.relative_step_size); + + // It is not a good idea to make the step size arbitrarily + // small. This will lead to problems with round off and numerical + // instability when dividing by the step size. The general + // recommendation is to not go down below sqrt(epsilon). + double min_step_size = std::sqrt(std::numeric_limits::epsilon()); + + // For Ridders' method, the initial step size is required to be large, + // thus ridders_relative_initial_step_size is used. + if (kMethod == RIDDERS) { + min_step_size = + (std::max)(min_step_size, options.ridders_relative_initial_step_size); + } + + // For each parameter in the parameter block, use finite differences to + // compute the derivative for that parameter. + FixedArray temp_residual_array(num_residuals_internal); + FixedArray residual_array(num_residuals_internal); + Map residuals(residual_array.data(), + num_residuals_internal); + + for (int j = 0; j < parameter_block_size_internal; ++j) { + const double delta = (std::max)(min_step_size, step_size(j)); + + if (kMethod == RIDDERS) { + if (!EvaluateRiddersJacobianColumn(functor, + j, + delta, + options, + num_residuals_internal, + parameter_block_size_internal, + x.data(), + residuals_at_eval_point, + parameters, + x_plus_delta.data(), + temp_residual_array.data(), + residual_array.data())) { + return false; + } + } else { + if (!EvaluateJacobianColumn(functor, + j, + delta, + num_residuals_internal, + parameter_block_size_internal, + x.data(), + residuals_at_eval_point, + parameters, + x_plus_delta.data(), + temp_residual_array.data(), + residual_array.data())) { + return false; + } + } + + parameter_jacobian.col(j).matrix() = residuals; + } + return true; + } + + static bool EvaluateJacobianColumn(const CostFunctor* functor, + int parameter_index, + double delta, + int num_residuals, + int parameter_block_size, + const double* x_ptr, + const double* residuals_at_eval_point, + double** parameters, + double* x_plus_delta_ptr, + double* temp_residuals_ptr, + double* residuals_ptr) { + using Eigen::Map; + using Eigen::Matrix; + + using ResidualVector = Matrix; + using ParameterVector = Matrix; + + Map x(x_ptr, parameter_block_size); + Map x_plus_delta(x_plus_delta_ptr, parameter_block_size); + + Map residuals(residuals_ptr, num_residuals); + Map temp_residuals(temp_residuals_ptr, num_residuals); + + // Mutate 1 element at a time and then restore. + x_plus_delta(parameter_index) = x(parameter_index) + delta; + + if (!VariadicEvaluate( + *functor, parameters, residuals.data())) { + return false; + } + + // Compute this column of the jacobian in 3 steps: + // 1. Store residuals for the forward part. + // 2. Subtract residuals for the backward (or 0) part. + // 3. Divide out the run. + double one_over_delta = 1.0 / delta; + if (kMethod == CENTRAL || kMethod == RIDDERS) { + // Compute the function on the other side of x(parameter_index). + x_plus_delta(parameter_index) = x(parameter_index) - delta; + + if (!VariadicEvaluate( + *functor, parameters, temp_residuals.data())) { + return false; + } + + residuals -= temp_residuals; + one_over_delta /= 2; + } else { + // Forward difference only; reuse existing residuals evaluation. + residuals -= + Map(residuals_at_eval_point, num_residuals); + } + + // Restore x_plus_delta. + x_plus_delta(parameter_index) = x(parameter_index); + + // Divide out the run to get slope. + residuals *= one_over_delta; + + return true; + } + + // This numeric difference implementation uses adaptive differentiation + // on the parameters to obtain the Jacobian matrix. The adaptive algorithm + // is based on Ridders' method for adaptive differentiation, which creates + // a Romberg tableau from varying step sizes and extrapolates the + // intermediate results to obtain the current computational error. + // + // References: + // C.J.F. Ridders, Accurate computation of F'(x) and F'(x) F"(x), Advances + // in Engineering Software (1978), Volume 4, Issue 2, April 1982, + // Pages 75-76, ISSN 0141-1195, + // http://dx.doi.org/10.1016/S0141-1195(82)80057-0. + static bool EvaluateRiddersJacobianColumn( + const CostFunctor* functor, + int parameter_index, + double delta, + const NumericDiffOptions& options, + int num_residuals, + int parameter_block_size, + const double* x_ptr, + const double* residuals_at_eval_point, + double** parameters, + double* x_plus_delta_ptr, + double* temp_residuals_ptr, + double* residuals_ptr) { + using Eigen::aligned_allocator; + using Eigen::Map; + using Eigen::Matrix; + + using ResidualVector = Matrix; + using ResidualCandidateMatrix = + Matrix; + using ParameterVector = Matrix; + + Map x(x_ptr, parameter_block_size); + Map x_plus_delta(x_plus_delta_ptr, parameter_block_size); + + Map residuals(residuals_ptr, num_residuals); + Map temp_residuals(temp_residuals_ptr, num_residuals); + + // In order for the algorithm to converge, the step size should be + // initialized to a value that is large enough to produce a significant + // change in the function. + // As the derivative is estimated, the step size decreases. + // By default, the step sizes are chosen so that the middle column + // of the Romberg tableau uses the input delta. + double current_step_size = + delta * pow(options.ridders_step_shrink_factor, + options.max_num_ridders_extrapolations / 2); + + // Double-buffering temporary differential candidate vectors + // from previous step size. + ResidualCandidateMatrix stepsize_candidates_a( + num_residuals, options.max_num_ridders_extrapolations); + ResidualCandidateMatrix stepsize_candidates_b( + num_residuals, options.max_num_ridders_extrapolations); + ResidualCandidateMatrix* current_candidates = &stepsize_candidates_a; + ResidualCandidateMatrix* previous_candidates = &stepsize_candidates_b; + + // Represents the computational error of the derivative. This variable is + // initially set to a large value, and is set to the difference between + // current and previous finite difference extrapolations. + // norm_error is supposed to decrease as the finite difference tableau + // generation progresses, serving both as an estimate for differentiation + // error and as a measure of differentiation numerical stability. + double norm_error = (std::numeric_limits::max)(); + + // Loop over decreasing step sizes until: + // 1. Error is smaller than a given value (ridders_epsilon), + // 2. Maximal order of extrapolation reached, or + // 3. Extrapolation becomes numerically unstable. + for (int i = 0; i < options.max_num_ridders_extrapolations; ++i) { + // Compute the numerical derivative at this step size. + if (!EvaluateJacobianColumn(functor, + parameter_index, + current_step_size, + num_residuals, + parameter_block_size, + x.data(), + residuals_at_eval_point, + parameters, + x_plus_delta.data(), + temp_residuals.data(), + current_candidates->col(0).data())) { + // Something went wrong; bail. + return false; + } + + // Store initial results. + if (i == 0) { + residuals = current_candidates->col(0); + } + + // Shrink differentiation step size. + current_step_size /= options.ridders_step_shrink_factor; + + // Extrapolation factor for Richardson acceleration method (see below). + double richardson_factor = options.ridders_step_shrink_factor * + options.ridders_step_shrink_factor; + for (int k = 1; k <= i; ++k) { + // Extrapolate the various orders of finite differences using + // the Richardson acceleration method. + current_candidates->col(k) = + (richardson_factor * current_candidates->col(k - 1) - + previous_candidates->col(k - 1)) / + (richardson_factor - 1.0); + + richardson_factor *= options.ridders_step_shrink_factor * + options.ridders_step_shrink_factor; + + // Compute the difference between the previous value and the current. + double candidate_error = (std::max)( + (current_candidates->col(k) - current_candidates->col(k - 1)) + .norm(), + (current_candidates->col(k) - previous_candidates->col(k - 1)) + .norm()); + + // If the error has decreased, update results. + if (candidate_error <= norm_error) { + norm_error = candidate_error; + residuals = current_candidates->col(k); + + // If the error is small enough, stop. + if (norm_error < options.ridders_epsilon) { + break; + } + } + } + + // After breaking out of the inner loop, declare convergence. + if (norm_error < options.ridders_epsilon) { + break; + } + + // Check to see if the current gradient estimate is numerically unstable. + // If so, bail out and return the last stable result. + if (i > 0) { + double tableau_error = + (current_candidates->col(i) - previous_candidates->col(i - 1)) + .norm(); + + // Compare current error to the chosen candidate's error. + if (tableau_error >= 2 * norm_error) { + break; + } + } + + std::swap(current_candidates, previous_candidates); + } + return true; + } +}; + +// This function calls NumericDiff<...>::EvaluateJacobianForParameterBlock for +// each parameter block. +// +// Example: +// A call to +// EvaluateJacobianForParameterBlocks>( +// functor, +// residuals_at_eval_point, +// options, +// num_residuals, +// parameters, +// jacobians); +// will result in the following calls to +// NumericDiff<...>::EvaluateJacobianForParameterBlock: +// +// if (jacobians[0] != nullptr) { +// if (!NumericDiff< +// CostFunctor, +// method, +// kNumResiduals, +// StaticParameterDims<2, 3>, +// 0, +// 2>::EvaluateJacobianForParameterBlock(functor, +// residuals_at_eval_point, +// options, +// num_residuals, +// 0, +// 2, +// parameters, +// jacobians[0])) { +// return false; +// } +// } +// if (jacobians[1] != nullptr) { +// if (!NumericDiff< +// CostFunctor, +// method, +// kNumResiduals, +// StaticParameterDims<2, 3>, +// 1, +// 3>::EvaluateJacobianForParameterBlock(functor, +// residuals_at_eval_point, +// options, +// num_residuals, +// 1, +// 3, +// parameters, +// jacobians[1])) { +// return false; +// } +// } +template +struct EvaluateJacobianForParameterBlocks; + +template +struct EvaluateJacobianForParameterBlocks, + ParameterIdx> { + template + static bool Apply(const CostFunctor* functor, + const double* residuals_at_eval_point, + const NumericDiffOptions& options, + int num_residuals, + double** parameters, + double** jacobians) { + if (jacobians[ParameterIdx] != nullptr) { + if (!NumericDiff< + CostFunctor, + method, + kNumResiduals, + ParameterDims, + ParameterIdx, + N>::EvaluateJacobianForParameterBlock(functor, + residuals_at_eval_point, + options, + num_residuals, + ParameterIdx, + N, + parameters, + jacobians[ParameterIdx])) { + return false; + } + } + + return EvaluateJacobianForParameterBlocks, + ParameterIdx + 1>:: + template Apply(functor, + residuals_at_eval_point, + options, + num_residuals, + parameters, + jacobians); + } +}; + +// End of 'recursion'. Nothing more to do. +template +struct EvaluateJacobianForParameterBlocks, + ParameterIdx> { + template + static bool Apply(const CostFunctor* /* NOT USED*/, + const double* /* NOT USED*/, + const NumericDiffOptions& /* NOT USED*/, + int /* NOT USED*/, + double** /* NOT USED*/, + double** /* NOT USED*/) { + return true; + } +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_PUBLIC_INTERNAL_NUMERIC_DIFF_H_ diff --git a/include/ceres/internal/parameter_dims.h b/include/ceres/internal/parameter_dims.h new file mode 100644 index 0000000000000000000000000000000000000000..240210614162beceea4466f2ea30fdf92a5ced0e --- /dev/null +++ b/include/ceres/internal/parameter_dims.h @@ -0,0 +1,124 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2018 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: jodebo_beck@gmx.de (Johannes Beck) + +#ifndef CERES_PUBLIC_INTERNAL_PARAMETER_DIMS_H_ +#define CERES_PUBLIC_INTERNAL_PARAMETER_DIMS_H_ + +#include +#include + +#include "ceres/internal/integer_sequence_algorithm.h" + +namespace ceres { +namespace internal { + +// Checks, whether the given parameter block sizes are valid. Valid means every +// dimension is bigger than zero. +constexpr bool IsValidParameterDimensionSequence(std::integer_sequence) { + return true; +} + +template +constexpr bool IsValidParameterDimensionSequence( + std::integer_sequence) { + return (N <= 0) ? false + : IsValidParameterDimensionSequence( + std::integer_sequence()); +} + +// Helper class that represents the parameter dimensions. The parameter +// dimensions are either dynamic or the sizes are known at compile time. It is +// used to pass parameter block dimensions around (e.g. between functions or +// classes). +// +// As an example if one have three parameter blocks with dimensions (2, 4, 1), +// one would use 'StaticParameterDims<2, 4, 1>' which is a synonym for +// 'ParameterDims'. +// For dynamic parameter dims, one would just use 'DynamicParameterDims', which +// is a synonym for 'ParameterDims'. +template +class ParameterDims { + public: + using Parameters = std::integer_sequence; + + // The parameter dimensions are only valid if all parameter block dimensions + // are greater than zero. + static constexpr bool kIsValid = + IsValidParameterDimensionSequence(Parameters()); + static_assert(kIsValid, + "Invalid parameter block dimension detected. Each parameter " + "block dimension must be bigger than zero."); + + static constexpr bool kIsDynamic = IsDynamic; + static constexpr int kNumParameterBlocks = sizeof...(Ns); + static_assert(kIsDynamic || kNumParameterBlocks > 0, + "At least one parameter block must be specified."); + + static constexpr int kNumParameters = + Sum>::Value; + + static constexpr int GetDim(int dim) { return params_[dim]; } + + // If one has all parameters packed into a single array this function unpacks + // the parameters. + template + static inline std::array GetUnpackedParameters( + T* ptr) { + using Offsets = ExclusiveScan; + return GetUnpackedParameters(ptr, Offsets()); + } + + private: + template + static inline std::array GetUnpackedParameters( + T* ptr, std::integer_sequence) { + return std::array{{ptr + Indices...}}; + } + + static constexpr std::array params_{Ns...}; +}; + +// Even static constexpr member variables needs to be defined (not only +// declared). As the ParameterDims class is tempalted this definition must +// be in the header file. +template +constexpr std::array::kNumParameterBlocks> + ParameterDims::params_; + +// Using declarations for static and dynamic parameter dims. This makes client +// code easier to read. +template +using StaticParameterDims = ParameterDims; +using DynamicParameterDims = ParameterDims; + +} // namespace internal +} // namespace ceres + +#endif // CERES_PUBLIC_INTERNAL_PARAMETER_DIMS_H_ diff --git a/include/ceres/internal/port.h b/include/ceres/internal/port.h new file mode 100644 index 0000000000000000000000000000000000000000..4275b0e15c361c2a741e71fe95c2640c83f58921 --- /dev/null +++ b/include/ceres/internal/port.h @@ -0,0 +1,88 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2022 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#ifndef CERES_PUBLIC_INTERNAL_PORT_H_ +#define CERES_PUBLIC_INTERNAL_PORT_H_ + +// A macro to mark a function/variable/class as deprecated. +// We use compiler specific attributes rather than the c++ +// attribute because they do not mix well with each other. +#if defined(_MSC_VER) +#define CERES_DEPRECATED_WITH_MSG(message) __declspec(deprecated(message)) +#elif defined(__GNUC__) +#define CERES_DEPRECATED_WITH_MSG(message) __attribute__((deprecated(message))) +#else +// In the worst case fall back to c++ attribute. +#define CERES_DEPRECATED_WITH_MSG(message) [[deprecated(message)]] +#endif + +#ifndef CERES_GET_FLAG +#define CERES_GET_FLAG(X) X +#endif + +// Indicates whether C++17 is currently active +#ifndef CERES_HAS_CPP17 +#if __cplusplus >= 201703L || (defined(_MSVC_LANG) && _MSVC_LANG >= 201703L) +#define CERES_HAS_CPP17 +#endif // __cplusplus >= 201703L || (defined(_MSVC_LANG) && _MSVC_LANG >= + // 201703L) +#endif // !defined(CERES_HAS_CPP17) + +// Indicates whether C++20 is currently active +#ifndef CERES_HAS_CPP20 +#if __cplusplus >= 202002L || (defined(_MSVC_LANG) && _MSVC_LANG >= 202002L) +#define CERES_HAS_CPP20 +#endif // __cplusplus >= 202002L || (defined(_MSVC_LANG) && _MSVC_LANG >= + // 202002L) +#endif // !defined(CERES_HAS_CPP20) + +// Prevents symbols from being substituted by the corresponding macro definition +// under the same name. For instance, min and max are defined as macros on +// Windows (unless NOMINMAX is defined) which causes compilation errors when +// defining or referencing symbols under the same name. +// +// To be robust in all cases particularly when NOMINMAX cannot be used, use this +// macro to annotate min/max declarations/definitions. Examples: +// +// int max CERES_PREVENT_MACRO_SUBSTITUTION(); +// min CERES_PREVENT_MACRO_SUBSTITUTION(a, b); +// max CERES_PREVENT_MACRO_SUBSTITUTION(a, b); +// +// NOTE: In case the symbols for which the substitution must be prevented are +// used within another macro, the substitution must be inhibited using parens as +// +// (std::numerical_limits::max)() +// +// since the helper macro will not work here. Do not use this technique in +// general case, because it will prevent argument-dependent lookup (ADL). +// +#define CERES_PREVENT_MACRO_SUBSTITUTION // Yes, it's empty + +#endif // CERES_PUBLIC_INTERNAL_PORT_H_ diff --git a/include/ceres/internal/reenable_warnings.h b/include/ceres/internal/reenable_warnings.h new file mode 100644 index 0000000000000000000000000000000000000000..2c5db061fd7a8d2be0c86c571ee4cf0ef4f128da --- /dev/null +++ b/include/ceres/internal/reenable_warnings.h @@ -0,0 +1,38 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// + +// This is not your usual header guard. See disable_warnings.h +#ifdef CERES_WARNINGS_DISABLED +#undef CERES_WARNINGS_DISABLED + +#ifdef _MSC_VER +#pragma warning(pop) +#endif + +#endif // CERES_WARNINGS_DISABLED diff --git a/include/ceres/internal/sphere_manifold_functions.h b/include/ceres/internal/sphere_manifold_functions.h new file mode 100644 index 0000000000000000000000000000000000000000..5be3321a5792933be1860a9b542dfa1fc6083fe3 --- /dev/null +++ b/include/ceres/internal/sphere_manifold_functions.h @@ -0,0 +1,162 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2022 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: vitus@google.com (Mike Vitus) +// jodebo_beck@gmx.de (Johannes Beck) + +#ifndef CERES_PUBLIC_INTERNAL_SPHERE_MANIFOLD_HELPERS_H_ +#define CERES_PUBLIC_INTERNAL_SPHERE_MANIFOLD_HELPERS_H_ + +#include "ceres/internal/householder_vector.h" + +// This module contains functions to compute the SphereManifold plus and minus +// operator and their Jacobians. +// +// As the parameters to these functions are shared between them, they are +// described here: The following variable names are used: +// Plus(x, delta) = x + delta = x_plus_delta, +// Minus(y, x) = y - x = y_minus_x. +// +// The remaining ones are v and beta which describe the Householder +// transformation of x, and norm_delta which is the norm of delta. +// +// The types of x, y, x_plus_delta and y_minus_x need to be equivalent to +// Eigen::Matrix and the type of delta needs +// to be equivalent to Eigen::Matrix. +// +// The type of Jacobian plus needs to be equivalent to Eigen::Matrix and for +// Jacobian minus Eigen::Matrix. +// +// For all vector / matrix inputs and outputs, template parameters are +// used in order to allow also Eigen::Ref and Eigen block expressions to +// be passed to the function. + +namespace ceres { +namespace internal { + +template +inline void ComputeSphereManifoldPlus(const VT& v, + double beta, + const XT& x, + const DeltaT& delta, + double norm_delta, + XPlusDeltaT* x_plus_delta) { + constexpr int AmbientDim = VT::RowsAtCompileTime; + + // Map the delta from the minimum representation to the over parameterized + // homogeneous vector. See B.2 p.25 equation (106) - (107) for more details. + const double norm_delta_div_2 = 0.5 * norm_delta; + const double sin_delta_by_delta = + std::sin(norm_delta_div_2) / norm_delta_div_2; + + Eigen::Matrix y(v.size()); + y << 0.5 * sin_delta_by_delta * delta, std::cos(norm_delta_div_2); + + // Apply the delta update to remain on the sphere. + *x_plus_delta = x.norm() * ApplyHouseholderVector(y, v, beta); +} + +template +inline void ComputeSphereManifoldPlusJacobian(const VT& x, + JacobianT* jacobian) { + constexpr int AmbientSpaceDim = VT::RowsAtCompileTime; + using AmbientVector = Eigen::Matrix; + const int ambient_size = x.size(); + const int tangent_size = x.size() - 1; + + AmbientVector v(ambient_size); + double beta; + + // NOTE: The explicit template arguments are needed here because + // ComputeHouseholderVector is templated and some versions of MSVC + // have trouble deducing the type of v automatically. + ComputeHouseholderVector(x, &v, &beta); + + // The Jacobian is equal to J = 0.5 * H.leftCols(size_ - 1) where H is the + // Householder matrix (H = I - beta * v * v'). + for (int i = 0; i < tangent_size; ++i) { + (*jacobian).col(i) = -0.5 * beta * v(i) * v; + (*jacobian)(i, i) += 0.5; + } + (*jacobian) *= x.norm(); +} + +template +inline void ComputeSphereManifoldMinus( + const VT& v, double beta, const XT& x, const YT& y, YMinusXT* y_minus_x) { + constexpr int AmbientSpaceDim = VT::RowsAtCompileTime; + constexpr int TangentSpaceDim = + AmbientSpaceDim == Eigen::Dynamic ? Eigen::Dynamic : AmbientSpaceDim - 1; + using AmbientVector = Eigen::Matrix; + + const int tanget_size = v.size() - 1; + + const AmbientVector hy = ApplyHouseholderVector(y, v, beta) / x.norm(); + + // Calculate y - x. See B.2 p.25 equation (108). + double y_last = hy[tanget_size]; + double hy_norm = hy.template head(tanget_size).norm(); + if (hy_norm == 0.0) { + y_minus_x->setZero(); + } else { + *y_minus_x = 2.0 * std::atan2(hy_norm, y_last) / hy_norm * + hy.template head(tanget_size); + } +} + +template +inline void ComputeSphereManifoldMinusJacobian(const VT& x, + JacobianT* jacobian) { + constexpr int AmbientSpaceDim = VT::RowsAtCompileTime; + using AmbientVector = Eigen::Matrix; + const int ambient_size = x.size(); + const int tangent_size = x.size() - 1; + + AmbientVector v(ambient_size); + double beta; + + // NOTE: The explicit template arguments are needed here because + // ComputeHouseholderVector is templated and some versions of MSVC + // have trouble deducing the type of v automatically. + ComputeHouseholderVector(x, &v, &beta); + + // The Jacobian is equal to J = 2.0 * H.leftCols(size_ - 1) where H is the + // Householder matrix (H = I - beta * v * v'). + for (int i = 0; i < tangent_size; ++i) { + (*jacobian).row(i) = -2.0 * beta * v(i) * v; + (*jacobian)(i, i) += 2.0; + } + (*jacobian) /= x.norm(); +} + +} // namespace internal +} // namespace ceres + +#endif diff --git a/include/ceres/internal/variadic_evaluate.h b/include/ceres/internal/variadic_evaluate.h new file mode 100644 index 0000000000000000000000000000000000000000..b8408237cc335dfd270c4a2634e8af57b18bbd4a --- /dev/null +++ b/include/ceres/internal/variadic_evaluate.h @@ -0,0 +1,113 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// mierle@gmail.com (Keir Mierle) +// jodebo_beck@gmx.de (Johannes Beck) + +#ifndef CERES_PUBLIC_INTERNAL_VARIADIC_EVALUATE_H_ +#define CERES_PUBLIC_INTERNAL_VARIADIC_EVALUATE_H_ + +#include +#include +#include + +#include "ceres/cost_function.h" +#include "ceres/internal/parameter_dims.h" + +namespace ceres { +namespace internal { + +// For fixed size cost functors +template +inline bool VariadicEvaluateImpl(const Functor& functor, + T const* const* input, + T* output, + std::false_type /*is_dynamic*/, + std::integer_sequence) { + static_assert(sizeof...(Indices), + "Invalid number of parameter blocks. At least one parameter " + "block must be specified."); + return functor(input[Indices]..., output); +} + +// For dynamic sized cost functors +template +inline bool VariadicEvaluateImpl(const Functor& functor, + T const* const* input, + T* output, + std::true_type /*is_dynamic*/, + std::integer_sequence) { + return functor(input, output); +} + +// For ceres cost functors (not ceres::CostFunction) +template +inline bool VariadicEvaluateImpl(const Functor& functor, + T const* const* input, + T* output, + const void* /* NOT USED */) { + using ParameterBlockIndices = + std::make_integer_sequence; + using IsDynamic = std::integral_constant; + return VariadicEvaluateImpl( + functor, input, output, IsDynamic(), ParameterBlockIndices()); +} + +// For ceres::CostFunction +template +inline bool VariadicEvaluateImpl(const Functor& functor, + T const* const* input, + T* output, + const CostFunction* /* NOT USED */) { + return functor.Evaluate(input, output, nullptr); +} + +// Variadic evaluate is a helper function to evaluate ceres cost function or +// functors using an input, output and the parameter dimensions. There are +// several ways different possibilities: +// 1) If the passed functor is a 'ceres::CostFunction' its evaluate method is +// called. +// 2) If the functor is not a 'ceres::CostFunction' and the specified parameter +// dims is dynamic, the functor must have the following signature +// 'bool(T const* const* input, T* output)'. +// 3) If the functor is not a 'ceres::CostFunction' and the specified parameter +// dims is not dynamic, the input is expanded by using the number of parameter +// blocks. The signature of the functor must have the following signature +// 'bool()(const T* i_1, const T* i_2, ... const T* i_n, T* output)'. +template +inline bool VariadicEvaluate(const Functor& functor, + T const* const* input, + T* output) { + return VariadicEvaluateImpl(functor, input, output, &functor); +} + +} // namespace internal +} // namespace ceres + +#endif // CERES_PUBLIC_INTERNAL_VARIADIC_EVALUATE_H_ diff --git a/include/ceres/iteration_callback.h b/include/ceres/iteration_callback.h new file mode 100644 index 0000000000000000000000000000000000000000..3d7e8e94f3049ff79535d10aafffdead837a3ec5 --- /dev/null +++ b/include/ceres/iteration_callback.h @@ -0,0 +1,204 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2019 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// When an iteration callback is specified, Ceres calls the callback +// after each minimizer step (if the minimizer has not converged) and +// passes it an IterationSummary object, defined below. + +#ifndef CERES_PUBLIC_ITERATION_CALLBACK_H_ +#define CERES_PUBLIC_ITERATION_CALLBACK_H_ + +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" +#include "ceres/types.h" + +namespace ceres { + +// This struct describes the state of the optimizer after each +// iteration of the minimization. +struct CERES_EXPORT IterationSummary { + // Current iteration number. + int iteration = 0; + + // Step was numerically valid, i.e., all values are finite and the + // step reduces the value of the linearized model. + // + // Note: step_is_valid is always true when iteration = 0. + bool step_is_valid = false; + + // Step did not reduce the value of the objective function + // sufficiently, but it was accepted because of the relaxed + // acceptance criterion used by the non-monotonic trust region + // algorithm. + // + // Note: step_is_nonmonotonic is always false when iteration = 0; + bool step_is_nonmonotonic = false; + + // Whether or not the minimizer accepted this step or not. If the + // ordinary trust region algorithm is used, this means that the + // relative reduction in the objective function value was greater + // than Solver::Options::min_relative_decrease. However, if the + // non-monotonic trust region algorithm is used + // (Solver::Options:use_nonmonotonic_steps = true), then even if the + // relative decrease is not sufficient, the algorithm may accept the + // step and the step is declared successful. + // + // Note: step_is_successful is always true when iteration = 0. + bool step_is_successful = false; + + // Value of the objective function. + double cost = 0.0; + + // Change in the value of the objective function in this + // iteration. This can be positive or negative. + double cost_change = 0.0; + + // Infinity norm of the gradient vector. + double gradient_max_norm = 0.0; + + // 2-norm of the gradient vector. + double gradient_norm = 0.0; + + // 2-norm of the size of the step computed by the optimization + // algorithm. + double step_norm = 0.0; + + // For trust region algorithms, the ratio of the actual change in + // cost and the change in the cost of the linearized approximation. + double relative_decrease = 0.0; + + // Size of the trust region at the end of the current iteration. For + // the Levenberg-Marquardt algorithm, the regularization parameter + // mu = 1.0 / trust_region_radius. + double trust_region_radius = 0.0; + + // For the inexact step Levenberg-Marquardt algorithm, this is the + // relative accuracy with which the Newton(LM) step is solved. This + // number affects only the iterative solvers capable of solving + // linear systems inexactly. Factorization-based exact solvers + // ignore it. + double eta = 0.0; + + // Step sized computed by the line search algorithm. + double step_size = 0.0; + + // Number of function value evaluations used by the line search algorithm. + int line_search_function_evaluations = 0; + + // Number of function gradient evaluations used by the line search algorithm. + int line_search_gradient_evaluations = 0; + + // Number of iterations taken by the line search algorithm. + int line_search_iterations = 0; + + // Number of iterations taken by the linear solver to solve for the + // Newton step. + int linear_solver_iterations = 0; + + // All times reported below are wall times. + + // Time (in seconds) spent inside the minimizer loop in the current + // iteration. + double iteration_time_in_seconds = 0.0; + + // Time (in seconds) spent inside the trust region step solver. + double step_solver_time_in_seconds = 0.0; + + // Time (in seconds) since the user called Solve(). + double cumulative_time_in_seconds = 0.0; +}; + +// Interface for specifying callbacks that are executed at the end of +// each iteration of the Minimizer. The solver uses the return value +// of operator() to decide whether to continue solving or to +// terminate. The user can return three values. +// +// SOLVER_ABORT indicates that the callback detected an abnormal +// situation. The solver returns without updating the parameter blocks +// (unless Solver::Options::update_state_every_iteration is set +// true). Solver returns with Solver::Summary::termination_type set to +// USER_ABORT. +// +// SOLVER_TERMINATE_SUCCESSFULLY indicates that there is no need to +// optimize anymore (some user specified termination criterion has +// been met). Solver returns with Solver::Summary::termination_type +// set to USER_SUCCESS. +// +// SOLVER_CONTINUE indicates that the solver should continue +// optimizing. +// +// For example, the following Callback is used internally by Ceres to +// log the progress of the optimization. +// +// Callback for logging the state of the minimizer to STDERR or STDOUT +// depending on the user's preferences and logging level. +// +// class LoggingCallback : public IterationCallback { +// public: +// explicit LoggingCallback(bool log_to_stdout) +// : log_to_stdout_(log_to_stdout) {} +// +// CallbackReturnType operator()(const IterationSummary& summary) { +// const char* kReportRowFormat = +// "% 4d: f:% 8e d:% 3.2e g:% 3.2e h:% 3.2e " +// "rho:% 3.2e mu:% 3.2e eta:% 3.2e li:% 3d"; +// string output = StringPrintf(kReportRowFormat, +// summary.iteration, +// summary.cost, +// summary.cost_change, +// summary.gradient_max_norm, +// summary.step_norm, +// summary.relative_decrease, +// summary.trust_region_radius, +// summary.eta, +// summary.linear_solver_iterations); +// if (log_to_stdout_) { +// cout << output << endl; +// } else { +// VLOG(1) << output; +// } +// return SOLVER_CONTINUE; +// } +// +// private: +// const bool log_to_stdout_; +// }; +// +class CERES_EXPORT IterationCallback { + public: + virtual ~IterationCallback(); + virtual CallbackReturnType operator()(const IterationSummary& summary) = 0; +}; + +} // namespace ceres + +#include "ceres/internal/reenable_warnings.h" + +#endif // CERES_PUBLIC_ITERATION_CALLBACK_H_ diff --git a/include/ceres/jet.h b/include/ceres/jet.h new file mode 100644 index 0000000000000000000000000000000000000000..fba1e2ab6e0541c84b77effde2124f69665edc03 --- /dev/null +++ b/include/ceres/jet.h @@ -0,0 +1,1387 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2022 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// A simple implementation of N-dimensional dual numbers, for automatically +// computing exact derivatives of functions. +// +// While a complete treatment of the mechanics of automatic differentiation is +// beyond the scope of this header (see +// http://en.wikipedia.org/wiki/Automatic_differentiation for details), the +// basic idea is to extend normal arithmetic with an extra element, "e," often +// denoted with the greek symbol epsilon, such that e != 0 but e^2 = 0. Dual +// numbers are extensions of the real numbers analogous to complex numbers: +// whereas complex numbers augment the reals by introducing an imaginary unit i +// such that i^2 = -1, dual numbers introduce an "infinitesimal" unit e such +// that e^2 = 0. Dual numbers have two components: the "real" component and the +// "infinitesimal" component, generally written as x + y*e. Surprisingly, this +// leads to a convenient method for computing exact derivatives without needing +// to manipulate complicated symbolic expressions. +// +// For example, consider the function +// +// f(x) = x^2 , +// +// evaluated at 10. Using normal arithmetic, f(10) = 100, and df/dx(10) = 20. +// Next, argument 10 with an infinitesimal to get: +// +// f(10 + e) = (10 + e)^2 +// = 100 + 2 * 10 * e + e^2 +// = 100 + 20 * e -+- +// -- | +// | +--- This is zero, since e^2 = 0 +// | +// +----------------- This is df/dx! +// +// Note that the derivative of f with respect to x is simply the infinitesimal +// component of the value of f(x + e). So, in order to take the derivative of +// any function, it is only necessary to replace the numeric "object" used in +// the function with one extended with infinitesimals. The class Jet, defined in +// this header, is one such example of this, where substitution is done with +// templates. +// +// To handle derivatives of functions taking multiple arguments, different +// infinitesimals are used, one for each variable to take the derivative of. For +// example, consider a scalar function of two scalar parameters x and y: +// +// f(x, y) = x^2 + x * y +// +// Following the technique above, to compute the derivatives df/dx and df/dy for +// f(1, 3) involves doing two evaluations of f, the first time replacing x with +// x + e, the second time replacing y with y + e. +// +// For df/dx: +// +// f(1 + e, y) = (1 + e)^2 + (1 + e) * 3 +// = 1 + 2 * e + 3 + 3 * e +// = 4 + 5 * e +// +// --> df/dx = 5 +// +// For df/dy: +// +// f(1, 3 + e) = 1^2 + 1 * (3 + e) +// = 1 + 3 + e +// = 4 + e +// +// --> df/dy = 1 +// +// To take the gradient of f with the implementation of dual numbers ("jets") in +// this file, it is necessary to create a single jet type which has components +// for the derivative in x and y, and passing them to a templated version of f: +// +// template +// T f(const T &x, const T &y) { +// return x * x + x * y; +// } +// +// // The "2" means there should be 2 dual number components. +// // It computes the partial derivative at x=10, y=20. +// Jet x(10, 0); // Pick the 0th dual number for x. +// Jet y(20, 1); // Pick the 1st dual number for y. +// Jet z = f(x, y); +// +// LOG(INFO) << "df/dx = " << z.v[0] +// << "df/dy = " << z.v[1]; +// +// Most users should not use Jet objects directly; a wrapper around Jet objects, +// which makes computing the derivative, gradient, or jacobian of templated +// functors simple, is in autodiff.h. Even autodiff.h should not be used +// directly; instead autodiff_cost_function.h is typically the file of interest. +// +// For the more mathematically inclined, this file implements first-order +// "jets". A 1st order jet is an element of the ring +// +// T[N] = T[t_1, ..., t_N] / (t_1, ..., t_N)^2 +// +// which essentially means that each jet consists of a "scalar" value 'a' from T +// and a 1st order perturbation vector 'v' of length N: +// +// x = a + \sum_i v[i] t_i +// +// A shorthand is to write an element as x = a + u, where u is the perturbation. +// Then, the main point about the arithmetic of jets is that the product of +// perturbations is zero: +// +// (a + u) * (b + v) = ab + av + bu + uv +// = ab + (av + bu) + 0 +// +// which is what operator* implements below. Addition is simpler: +// +// (a + u) + (b + v) = (a + b) + (u + v). +// +// The only remaining question is how to evaluate the function of a jet, for +// which we use the chain rule: +// +// f(a + u) = f(a) + f'(a) u +// +// where f'(a) is the (scalar) derivative of f at a. +// +// By pushing these things through sufficiently and suitably templated +// functions, we can do automatic differentiation. Just be sure to turn on +// function inlining and common-subexpression elimination, or it will be very +// slow! +// +// WARNING: Most Ceres users should not directly include this file or know the +// details of how jets work. Instead the suggested method for automatic +// derivatives is to use autodiff_cost_function.h, which is a wrapper around +// both jets.h and autodiff.h to make taking derivatives of cost functions for +// use in Ceres easier. + +#ifndef CERES_PUBLIC_JET_H_ +#define CERES_PUBLIC_JET_H_ + +#include +#include +#include +#include // NOLINT +#include +#include +#include +#include + +#include "Eigen/Core" +#include "ceres/internal/jet_traits.h" +#include "ceres/internal/port.h" +#include "ceres/jet_fwd.h" + +// Here we provide partial specializations of std::common_type for the Jet class +// to allow determining a Jet type with a common underlying arithmetic type. +// Such an arithmetic type can be either a scalar or an another Jet. An example +// for a common type, say, between a float and a Jet is a Jet (i.e., std::common_type_t> and +// ceres::Jet refer to the same type.) +// +// The partial specialization are also used for determining compatible types by +// means of SFINAE and thus allow such types to be expressed as operands of +// logical comparison operators. Missing (partial) specialization of +// std::common_type for a particular (custom) type will therefore disable the +// use of comparison operators defined by Ceres. +// +// Since these partial specializations are used as SFINAE constraints, they +// enable standard promotion rules between various scalar types and consequently +// their use in comparison against a Jet without providing implicit +// conversions from a scalar, such as an int, to a Jet (see the implementation +// of logical comparison operators below). + +template +struct std::common_type> { + using type = ceres::Jet, N>; +}; + +template +struct std::common_type, U> { + using type = ceres::Jet, N>; +}; + +template +struct std::common_type, ceres::Jet> { + using type = ceres::Jet, N>; +}; + +namespace ceres { + +template +struct Jet { + enum { DIMENSION = N }; + using Scalar = T; + + // Default-construct "a" because otherwise this can lead to false errors about + // uninitialized uses when other classes relying on default constructed T + // (where T is a Jet). This usually only happens in opt mode. Note that + // the C++ standard mandates that e.g. default constructed doubles are + // initialized to 0.0; see sections 8.5 of the C++03 standard. + Jet() : a() { v.setConstant(Scalar()); } + + // Constructor from scalar: a + 0. + explicit Jet(const T& value) { + a = value; + v.setConstant(Scalar()); + } + + // Constructor from scalar plus variable: a + t_i. + Jet(const T& value, int k) { + a = value; + v.setConstant(Scalar()); + v[k] = T(1.0); + } + + // Constructor from scalar and vector part + // The use of Eigen::DenseBase allows Eigen expressions + // to be passed in without being fully evaluated until + // they are assigned to v + template + EIGEN_STRONG_INLINE Jet(const T& a, const Eigen::DenseBase& v) + : a(a), v(v) {} + + // Compound operators + Jet& operator+=(const Jet& y) { + *this = *this + y; + return *this; + } + + Jet& operator-=(const Jet& y) { + *this = *this - y; + return *this; + } + + Jet& operator*=(const Jet& y) { + *this = *this * y; + return *this; + } + + Jet& operator/=(const Jet& y) { + *this = *this / y; + return *this; + } + + // Compound with scalar operators. + Jet& operator+=(const T& s) { + *this = *this + s; + return *this; + } + + Jet& operator-=(const T& s) { + *this = *this - s; + return *this; + } + + Jet& operator*=(const T& s) { + *this = *this * s; + return *this; + } + + Jet& operator/=(const T& s) { + *this = *this / s; + return *this; + } + + // The scalar part. + T a; + + // The infinitesimal part. + Eigen::Matrix v; + + // This struct needs to have an Eigen aligned operator new as it contains + // fixed-size Eigen types. + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +// Unary + +template +inline Jet const& operator+(const Jet& f) { + return f; +} + +// TODO(keir): Try adding __attribute__((always_inline)) to these functions to +// see if it causes a performance increase. + +// Unary - +template +inline Jet operator-(const Jet& f) { + return Jet(-f.a, -f.v); +} + +// Binary + +template +inline Jet operator+(const Jet& f, const Jet& g) { + return Jet(f.a + g.a, f.v + g.v); +} + +// Binary + with a scalar: x + s +template +inline Jet operator+(const Jet& f, T s) { + return Jet(f.a + s, f.v); +} + +// Binary + with a scalar: s + x +template +inline Jet operator+(T s, const Jet& f) { + return Jet(f.a + s, f.v); +} + +// Binary - +template +inline Jet operator-(const Jet& f, const Jet& g) { + return Jet(f.a - g.a, f.v - g.v); +} + +// Binary - with a scalar: x - s +template +inline Jet operator-(const Jet& f, T s) { + return Jet(f.a - s, f.v); +} + +// Binary - with a scalar: s - x +template +inline Jet operator-(T s, const Jet& f) { + return Jet(s - f.a, -f.v); +} + +// Binary * +template +inline Jet operator*(const Jet& f, const Jet& g) { + return Jet(f.a * g.a, f.a * g.v + f.v * g.a); +} + +// Binary * with a scalar: x * s +template +inline Jet operator*(const Jet& f, T s) { + return Jet(f.a * s, f.v * s); +} + +// Binary * with a scalar: s * x +template +inline Jet operator*(T s, const Jet& f) { + return Jet(f.a * s, f.v * s); +} + +// Binary / +template +inline Jet operator/(const Jet& f, const Jet& g) { + // This uses: + // + // a + u (a + u)(b - v) (a + u)(b - v) + // ----- = -------------- = -------------- + // b + v (b + v)(b - v) b^2 + // + // which holds because v*v = 0. + const T g_a_inverse = T(1.0) / g.a; + const T f_a_by_g_a = f.a * g_a_inverse; + return Jet(f_a_by_g_a, (f.v - f_a_by_g_a * g.v) * g_a_inverse); +} + +// Binary / with a scalar: s / x +template +inline Jet operator/(T s, const Jet& g) { + const T minus_s_g_a_inverse2 = -s / (g.a * g.a); + return Jet(s / g.a, g.v * minus_s_g_a_inverse2); +} + +// Binary / with a scalar: x / s +template +inline Jet operator/(const Jet& f, T s) { + const T s_inverse = T(1.0) / s; + return Jet(f.a * s_inverse, f.v * s_inverse); +} + +// Binary comparison operators for both scalars and jets. At least one of the +// operands must be a Jet. Promotable scalars (e.g., int, float, double etc.) +// can appear on either side of the operator. std::common_type_t is used as an +// SFINAE constraint to selectively enable compatible operand types. This allows +// comparison, for instance, against int literals without implicit conversion. +// In case the Jet arithmetic type is a Jet itself, a recursive expansion of Jet +// value is performed. +#define CERES_DEFINE_JET_COMPARISON_OPERATOR(op) \ + template >* = nullptr> \ + constexpr bool operator op(const Lhs& f, const Rhs& g) noexcept( \ + noexcept(internal::AsScalar(f) op internal::AsScalar(g))) { \ + using internal::AsScalar; \ + return AsScalar(f) op AsScalar(g); \ + } +CERES_DEFINE_JET_COMPARISON_OPERATOR(<) // NOLINT +CERES_DEFINE_JET_COMPARISON_OPERATOR(<=) // NOLINT +CERES_DEFINE_JET_COMPARISON_OPERATOR(>) // NOLINT +CERES_DEFINE_JET_COMPARISON_OPERATOR(>=) // NOLINT +CERES_DEFINE_JET_COMPARISON_OPERATOR(==) // NOLINT +CERES_DEFINE_JET_COMPARISON_OPERATOR(!=) // NOLINT +#undef CERES_DEFINE_JET_COMPARISON_OPERATOR + +// Pull some functions from namespace std. +// +// This is necessary because we want to use the same name (e.g. 'sqrt') for +// double-valued and Jet-valued functions, but we are not allowed to put +// Jet-valued functions inside namespace std. +using std::abs; +using std::acos; +using std::asin; +using std::atan; +using std::atan2; +using std::cbrt; +using std::ceil; +using std::copysign; +using std::cos; +using std::cosh; +using std::erf; +using std::erfc; +using std::exp; +using std::exp2; +using std::expm1; +using std::fdim; +using std::floor; +using std::fma; +using std::fmax; +using std::fmin; +using std::fpclassify; +using std::hypot; +using std::isfinite; +using std::isinf; +using std::isnan; +using std::isnormal; +using std::log; +using std::log10; +using std::log1p; +using std::log2; +using std::norm; +using std::pow; +using std::signbit; +using std::sin; +using std::sinh; +using std::sqrt; +using std::tan; +using std::tanh; + +// MSVC (up to 1930) defines quiet comparison functions as template functions +// which causes compilation errors due to ambiguity in the template parameter +// type resolution for using declarations in the ceres namespace. Workaround the +// issue by defining specific overload and bypass MSVC standard library +// definitions. +#if defined(_MSC_VER) +inline bool isgreater(double lhs, + double rhs) noexcept(noexcept(std::isgreater(lhs, rhs))) { + return std::isgreater(lhs, rhs); +} +inline bool isless(double lhs, + double rhs) noexcept(noexcept(std::isless(lhs, rhs))) { + return std::isless(lhs, rhs); +} +inline bool islessequal(double lhs, + double rhs) noexcept(noexcept(std::islessequal(lhs, + rhs))) { + return std::islessequal(lhs, rhs); +} +inline bool isgreaterequal(double lhs, double rhs) noexcept( + noexcept(std::isgreaterequal(lhs, rhs))) { + return std::isgreaterequal(lhs, rhs); +} +inline bool islessgreater(double lhs, double rhs) noexcept( + noexcept(std::islessgreater(lhs, rhs))) { + return std::islessgreater(lhs, rhs); +} +inline bool isunordered(double lhs, + double rhs) noexcept(noexcept(std::isunordered(lhs, + rhs))) { + return std::isunordered(lhs, rhs); +} +#else +using std::isgreater; +using std::isgreaterequal; +using std::isless; +using std::islessequal; +using std::islessgreater; +using std::isunordered; +#endif + +#ifdef CERES_HAS_CPP20 +using std::lerp; +using std::midpoint; +#endif // defined(CERES_HAS_CPP20) + +// Legacy names from pre-C++11 days. +// clang-format off +CERES_DEPRECATED_WITH_MSG("ceres::IsFinite will be removed in a future Ceres Solver release. Please use ceres::isfinite.") +inline bool IsFinite(double x) { return std::isfinite(x); } +CERES_DEPRECATED_WITH_MSG("ceres::IsInfinite will be removed in a future Ceres Solver release. Please use ceres::isinf.") +inline bool IsInfinite(double x) { return std::isinf(x); } +CERES_DEPRECATED_WITH_MSG("ceres::IsNaN will be removed in a future Ceres Solver release. Please use ceres::isnan.") +inline bool IsNaN(double x) { return std::isnan(x); } +CERES_DEPRECATED_WITH_MSG("ceres::IsNormal will be removed in a future Ceres Solver release. Please use ceres::isnormal.") +inline bool IsNormal(double x) { return std::isnormal(x); } +// clang-format on + +// In general, f(a + h) ~= f(a) + f'(a) h, via the chain rule. + +// abs(x + h) ~= abs(x) + sgn(x)h +template +inline Jet abs(const Jet& f) { + return Jet(abs(f.a), copysign(T(1), f.a) * f.v); +} + +// copysign(a, b) composes a float with the magnitude of a and the sign of b. +// Therefore, the function can be formally defined as +// +// copysign(a, b) = sgn(b)|a| +// +// where +// +// d/dx |x| = sgn(x) +// d/dx sgn(x) = 2δ(x) +// +// sgn(x) being the signum function. Differentiating copysign(a, b) with respect +// to a and b gives: +// +// d/da sgn(b)|a| = sgn(a) sgn(b) +// d/db sgn(b)|a| = 2|a|δ(b) +// +// with the dual representation given by +// +// copysign(a + da, b + db) ~= sgn(b)|a| + (sgn(a)sgn(b) da + 2|a|δ(b) db) +// +// where δ(b) is the Dirac delta function. +template +inline Jet copysign(const Jet& f, const Jet g) { + // The Dirac delta function δ(b) is undefined at b=0 (here it's + // infinite) and 0 everywhere else. + T d = fpclassify(g) == FP_ZERO ? std::numeric_limits::infinity() : T(0); + T sa = copysign(T(1), f.a); // sgn(a) + T sb = copysign(T(1), g.a); // sgn(b) + // The second part of the infinitesimal is 2|a|δ(b) which is either infinity + // or 0 unless a or any of the values of the b infinitesimal are 0. In the + // latter case, the corresponding values become NaNs (multiplying 0 by + // infinity gives NaN). We drop the constant factor 2 since it does not change + // the result (its values will still be either 0, infinity or NaN). + return Jet(copysign(f.a, g.a), sa * sb * f.v + abs(f.a) * d * g.v); +} + +// log(a + h) ~= log(a) + h / a +template +inline Jet log(const Jet& f) { + const T a_inverse = T(1.0) / f.a; + return Jet(log(f.a), f.v * a_inverse); +} + +// log10(a + h) ~= log10(a) + h / (a log(10)) +template +inline Jet log10(const Jet& f) { + // Most compilers will expand log(10) to a constant. + const T a_inverse = T(1.0) / (f.a * log(T(10.0))); + return Jet(log10(f.a), f.v * a_inverse); +} + +// log1p(a + h) ~= log1p(a) + h / (1 + a) +template +inline Jet log1p(const Jet& f) { + const T a_inverse = T(1.0) / (T(1.0) + f.a); + return Jet(log1p(f.a), f.v * a_inverse); +} + +// exp(a + h) ~= exp(a) + exp(a) h +template +inline Jet exp(const Jet& f) { + const T tmp = exp(f.a); + return Jet(tmp, tmp * f.v); +} + +// expm1(a + h) ~= expm1(a) + exp(a) h +template +inline Jet expm1(const Jet& f) { + const T tmp = expm1(f.a); + const T expa = tmp + T(1.0); // exp(a) = expm1(a) + 1 + return Jet(tmp, expa * f.v); +} + +// sqrt(a + h) ~= sqrt(a) + h / (2 sqrt(a)) +template +inline Jet sqrt(const Jet& f) { + const T tmp = sqrt(f.a); + const T two_a_inverse = T(1.0) / (T(2.0) * tmp); + return Jet(tmp, f.v * two_a_inverse); +} + +// cos(a + h) ~= cos(a) - sin(a) h +template +inline Jet cos(const Jet& f) { + return Jet(cos(f.a), -sin(f.a) * f.v); +} + +// acos(a + h) ~= acos(a) - 1 / sqrt(1 - a^2) h +template +inline Jet acos(const Jet& f) { + const T tmp = -T(1.0) / sqrt(T(1.0) - f.a * f.a); + return Jet(acos(f.a), tmp * f.v); +} + +// sin(a + h) ~= sin(a) + cos(a) h +template +inline Jet sin(const Jet& f) { + return Jet(sin(f.a), cos(f.a) * f.v); +} + +// asin(a + h) ~= asin(a) + 1 / sqrt(1 - a^2) h +template +inline Jet asin(const Jet& f) { + const T tmp = T(1.0) / sqrt(T(1.0) - f.a * f.a); + return Jet(asin(f.a), tmp * f.v); +} + +// tan(a + h) ~= tan(a) + (1 + tan(a)^2) h +template +inline Jet tan(const Jet& f) { + const T tan_a = tan(f.a); + const T tmp = T(1.0) + tan_a * tan_a; + return Jet(tan_a, tmp * f.v); +} + +// atan(a + h) ~= atan(a) + 1 / (1 + a^2) h +template +inline Jet atan(const Jet& f) { + const T tmp = T(1.0) / (T(1.0) + f.a * f.a); + return Jet(atan(f.a), tmp * f.v); +} + +// sinh(a + h) ~= sinh(a) + cosh(a) h +template +inline Jet sinh(const Jet& f) { + return Jet(sinh(f.a), cosh(f.a) * f.v); +} + +// cosh(a + h) ~= cosh(a) + sinh(a) h +template +inline Jet cosh(const Jet& f) { + return Jet(cosh(f.a), sinh(f.a) * f.v); +} + +// tanh(a + h) ~= tanh(a) + (1 - tanh(a)^2) h +template +inline Jet tanh(const Jet& f) { + const T tanh_a = tanh(f.a); + const T tmp = T(1.0) - tanh_a * tanh_a; + return Jet(tanh_a, tmp * f.v); +} + +// The floor function should be used with extreme care as this operation will +// result in a zero derivative which provides no information to the solver. +// +// floor(a + h) ~= floor(a) + 0 +template +inline Jet floor(const Jet& f) { + return Jet(floor(f.a)); +} + +// The ceil function should be used with extreme care as this operation will +// result in a zero derivative which provides no information to the solver. +// +// ceil(a + h) ~= ceil(a) + 0 +template +inline Jet ceil(const Jet& f) { + return Jet(ceil(f.a)); +} + +// Some new additions to C++11: + +// cbrt(a + h) ~= cbrt(a) + h / (3 a ^ (2/3)) +template +inline Jet cbrt(const Jet& f) { + const T derivative = T(1.0) / (T(3.0) * cbrt(f.a * f.a)); + return Jet(cbrt(f.a), f.v * derivative); +} + +// exp2(x + h) = 2^(x+h) ~= 2^x + h*2^x*log(2) +template +inline Jet exp2(const Jet& f) { + const T tmp = exp2(f.a); + const T derivative = tmp * log(T(2)); + return Jet(tmp, f.v * derivative); +} + +// log2(x + h) ~= log2(x) + h / (x * log(2)) +template +inline Jet log2(const Jet& f) { + const T derivative = T(1.0) / (f.a * log(T(2))); + return Jet(log2(f.a), f.v * derivative); +} + +// Like sqrt(x^2 + y^2), +// but acts to prevent underflow/overflow for small/large x/y. +// Note that the function is non-smooth at x=y=0, +// so the derivative is undefined there. +template +inline Jet hypot(const Jet& x, const Jet& y) { + // d/da sqrt(a) = 0.5 / sqrt(a) + // d/dx x^2 + y^2 = 2x + // So by the chain rule: + // d/dx sqrt(x^2 + y^2) = 0.5 / sqrt(x^2 + y^2) * 2x = x / sqrt(x^2 + y^2) + // d/dy sqrt(x^2 + y^2) = y / sqrt(x^2 + y^2) + const T tmp = hypot(x.a, y.a); + return Jet(tmp, x.a / tmp * x.v + y.a / tmp * y.v); +} + +#ifdef CERES_HAS_CPP17 +// Like sqrt(x^2 + y^2 + z^2), +// but acts to prevent underflow/overflow for small/large x/y/z. +// Note that the function is non-smooth at x=y=z=0, +// so the derivative is undefined there. +template +inline Jet hypot(const Jet& x, + const Jet& y, + const Jet& z) { + // d/da sqrt(a) = 0.5 / sqrt(a) + // d/dx x^2 + y^2 + z^2 = 2x + // So by the chain rule: + // d/dx sqrt(x^2 + y^2 + z^2) + // = 0.5 / sqrt(x^2 + y^2 + z^2) * 2x + // = x / sqrt(x^2 + y^2 + z^2) + // d/dy sqrt(x^2 + y^2 + z^2) = y / sqrt(x^2 + y^2 + z^2) + // d/dz sqrt(x^2 + y^2 + z^2) = z / sqrt(x^2 + y^2 + z^2) + const T tmp = hypot(x.a, y.a, z.a); + return Jet(tmp, x.a / tmp * x.v + y.a / tmp * y.v + z.a / tmp * z.v); +} +#endif // defined(CERES_HAS_CPP17) + +// Like x * y + z but rounded only once. +template +inline Jet fma(const Jet& x, + const Jet& y, + const Jet& z) { + // d/dx fma(x, y, z) = y + // d/dy fma(x, y, z) = x + // d/dz fma(x, y, z) = 1 + return Jet(fma(x.a, y.a, z.a), y.a * x.v + x.a * y.v + z.v); +} + +// Returns the larger of the two arguments. NaNs are treated as missing data. +// +// NOTE: This function is NOT subject to any of the error conditions specified +// in `math_errhandling`. +template >* = nullptr> +inline decltype(auto) fmax(const Lhs& f, const Rhs& g) { + using J = std::common_type_t; + return (isnan(g) || isgreater(f, g)) ? J{f} : J{g}; +} + +// Returns the smaller of the two arguments. NaNs are treated as missing data. +// +// NOTE: This function is NOT subject to any of the error conditions specified +// in `math_errhandling`. +template >* = nullptr> +inline decltype(auto) fmin(const Lhs& f, const Rhs& g) { + using J = std::common_type_t; + return (isnan(f) || isless(g, f)) ? J{g} : J{f}; +} + +// Returns the positive difference (f - g) of two arguments and zero if f <= g. +// If at least one argument is NaN, a NaN is return. +// +// NOTE At least one of the argument types must be a Jet, the other one can be a +// scalar. In case both arguments are Jets, their dimensionality must match. +template >* = nullptr> +inline decltype(auto) fdim(const Lhs& f, const Rhs& g) { + using J = std::common_type_t; + if (isnan(f) || isnan(g)) { + return std::numeric_limits::quiet_NaN(); + } + return isgreater(f, g) ? J{f - g} : J{}; +} + +// erf is defined as an integral that cannot be expressed analytically +// however, the derivative is trivial to compute +// erf(x + h) = erf(x) + h * 2*exp(-x^2)/sqrt(pi) +template +inline Jet erf(const Jet& x) { + // We evaluate the constant as follows: + // 2 / sqrt(pi) = 1 / sqrt(atan(1.)) + // On POSIX sytems it is defined as M_2_SQRTPI, but this is not + // portable and the type may not be T. The above expression + // evaluates to full precision with IEEE arithmetic and, since it's + // constant, the compiler can generate exactly the same code. gcc + // does so even at -O0. + return Jet(erf(x.a), x.v * exp(-x.a * x.a) * (T(1) / sqrt(atan(T(1))))); +} + +// erfc(x) = 1-erf(x) +// erfc(x + h) = erfc(x) + h * (-2*exp(-x^2)/sqrt(pi)) +template +inline Jet erfc(const Jet& x) { + // See in erf() above for the evaluation of the constant in the derivative. + return Jet(erfc(x.a), + -x.v * exp(-x.a * x.a) * (T(1) / sqrt(atan(T(1))))); +} + +// Bessel functions of the first kind with integer order equal to 0, 1, n. +// +// Microsoft has deprecated the j[0,1,n]() POSIX Bessel functions in favour of +// _j[0,1,n](). Where available on MSVC, use _j[0,1,n]() to avoid deprecated +// function errors in client code (the specific warning is suppressed when +// Ceres itself is built). +inline double BesselJ0(double x) { +#if defined(CERES_MSVC_USE_UNDERSCORE_PREFIXED_BESSEL_FUNCTIONS) + return _j0(x); +#else + return j0(x); +#endif +} +inline double BesselJ1(double x) { +#if defined(CERES_MSVC_USE_UNDERSCORE_PREFIXED_BESSEL_FUNCTIONS) + return _j1(x); +#else + return j1(x); +#endif +} +inline double BesselJn(int n, double x) { +#if defined(CERES_MSVC_USE_UNDERSCORE_PREFIXED_BESSEL_FUNCTIONS) + return _jn(n, x); +#else + return jn(n, x); +#endif +} + +// For the formulae of the derivatives of the Bessel functions see the book: +// Olver, Lozier, Boisvert, Clark, NIST Handbook of Mathematical Functions, +// Cambridge University Press 2010. +// +// Formulae are also available at http://dlmf.nist.gov + +// See formula http://dlmf.nist.gov/10.6#E3 +// j0(a + h) ~= j0(a) - j1(a) h +template +inline Jet BesselJ0(const Jet& f) { + return Jet(BesselJ0(f.a), -BesselJ1(f.a) * f.v); +} + +// See formula http://dlmf.nist.gov/10.6#E1 +// j1(a + h) ~= j1(a) + 0.5 ( j0(a) - j2(a) ) h +template +inline Jet BesselJ1(const Jet& f) { + return Jet(BesselJ1(f.a), + T(0.5) * (BesselJ0(f.a) - BesselJn(2, f.a)) * f.v); +} + +// See formula http://dlmf.nist.gov/10.6#E1 +// j_n(a + h) ~= j_n(a) + 0.5 ( j_{n-1}(a) - j_{n+1}(a) ) h +template +inline Jet BesselJn(int n, const Jet& f) { + return Jet( + BesselJn(n, f.a), + T(0.5) * (BesselJn(n - 1, f.a) - BesselJn(n + 1, f.a)) * f.v); +} + +// Classification and comparison functionality referencing only the scalar part +// of a Jet. To classify the derivatives (e.g., for sanity checks), the dual +// part should be referenced explicitly. For instance, to check whether the +// derivatives of a Jet 'f' are reasonable, one can use +// +// isfinite(f.v.array()).all() +// !isnan(f.v.array()).any() +// +// etc., depending on the desired semantics. +// +// NOTE: Floating-point classification and comparison functions and operators +// should be used with care as no derivatives can be propagated by such +// functions directly but only by expressions resulting from corresponding +// conditional statements. At the same time, conditional statements can possibly +// introduce a discontinuity in the cost function making it impossible to +// evaluate its derivative and thus the optimization problem intractable. + +// Determines whether the scalar part of the Jet is finite. +template +inline bool isfinite(const Jet& f) { + return isfinite(f.a); +} + +// Determines whether the scalar part of the Jet is infinite. +template +inline bool isinf(const Jet& f) { + return isinf(f.a); +} + +// Determines whether the scalar part of the Jet is NaN. +template +inline bool isnan(const Jet& f) { + return isnan(f.a); +} + +// Determines whether the scalar part of the Jet is neither zero, subnormal, +// infinite, nor NaN. +template +inline bool isnormal(const Jet& f) { + return isnormal(f.a); +} + +// Determines whether the scalar part of the Jet f is less than the scalar +// part of g. +// +// NOTE: This function does NOT set any floating-point exceptions. +template >* = nullptr> +inline bool isless(const Lhs& f, const Rhs& g) { + using internal::AsScalar; + return isless(AsScalar(f), AsScalar(g)); +} + +// Determines whether the scalar part of the Jet f is greater than the scalar +// part of g. +// +// NOTE: This function does NOT set any floating-point exceptions. +template >* = nullptr> +inline bool isgreater(const Lhs& f, const Rhs& g) { + using internal::AsScalar; + return isgreater(AsScalar(f), AsScalar(g)); +} + +// Determines whether the scalar part of the Jet f is less than or equal to the +// scalar part of g. +// +// NOTE: This function does NOT set any floating-point exceptions. +template >* = nullptr> +inline bool islessequal(const Lhs& f, const Rhs& g) { + using internal::AsScalar; + return islessequal(AsScalar(f), AsScalar(g)); +} + +// Determines whether the scalar part of the Jet f is less than or greater than +// (f < g || f > g) the scalar part of g. +// +// NOTE: This function does NOT set any floating-point exceptions. +template >* = nullptr> +inline bool islessgreater(const Lhs& f, const Rhs& g) { + using internal::AsScalar; + return islessgreater(AsScalar(f), AsScalar(g)); +} + +// Determines whether the scalar part of the Jet f is greater than or equal to +// the scalar part of g. +// +// NOTE: This function does NOT set any floating-point exceptions. +template >* = nullptr> +inline bool isgreaterequal(const Lhs& f, const Rhs& g) { + using internal::AsScalar; + return isgreaterequal(AsScalar(f), AsScalar(g)); +} + +// Determines if either of the scalar parts of the arguments are NaN and +// thus cannot be ordered with respect to each other. +template >* = nullptr> +inline bool isunordered(const Lhs& f, const Rhs& g) { + using internal::AsScalar; + return isunordered(AsScalar(f), AsScalar(g)); +} + +// Categorize scalar part as zero, subnormal, normal, infinite, NaN, or +// implementation-defined. +template +inline int fpclassify(const Jet& f) { + return fpclassify(f.a); +} + +// Determines whether the scalar part of the argument is negative. +template +inline bool signbit(const Jet& f) { + return signbit(f.a); +} + +// Legacy functions from the pre-C++11 days. +template +CERES_DEPRECATED_WITH_MSG( + "ceres::IsFinite will be removed in a future Ceres Solver release. Please " + "use ceres::isfinite.") +inline bool IsFinite(const Jet& f) { + return isfinite(f); +} + +template +CERES_DEPRECATED_WITH_MSG( + "ceres::IsNaN will be removed in a future Ceres Solver release. Please use " + "ceres::isnan.") +inline bool IsNaN(const Jet& f) { + return isnan(f); +} + +template +CERES_DEPRECATED_WITH_MSG( + "ceres::IsNormal will be removed in a future Ceres Solver release. Please " + "use ceres::isnormal.") +inline bool IsNormal(const Jet& f) { + return isnormal(f); +} + +// The jet is infinite if any part of the jet is infinite. +template +CERES_DEPRECATED_WITH_MSG( + "ceres::IsInfinite will be removed in a future Ceres Solver release. " + "Please use ceres::isinf.") +inline bool IsInfinite(const Jet& f) { + return isinf(f); +} + +#ifdef CERES_HAS_CPP20 +// Computes the linear interpolation a + t(b - a) between a and b at the value +// t. For arguments outside of the range 0 <= t <= 1, the values are +// extrapolated. +// +// Differentiating lerp(a, b, t) with respect to a, b, and t gives: +// +// d/da lerp(a, b, t) = 1 - t +// d/db lerp(a, b, t) = t +// d/dt lerp(a, b, t) = b - a +// +// with the dual representation given by +// +// lerp(a + da, b + db, t + dt) +// ~= lerp(a, b, t) + (1 - t) da + t db + (b - a) dt . +template +inline Jet lerp(const Jet& a, + const Jet& b, + const Jet& t) { + return Jet{lerp(a.a, b.a, t.a), + (T(1) - t.a) * a.v + t.a * b.v + (b.a - a.a) * t.v}; +} + +// Computes the midpoint a + (b - a) / 2. +// +// Differentiating midpoint(a, b) with respect to a and b gives: +// +// d/da midpoint(a, b) = 1/2 +// d/db midpoint(a, b) = 1/2 +// +// with the dual representation given by +// +// midpoint(a + da, b + db) ~= midpoint(a, b) + (da + db) / 2 . +template +inline Jet midpoint(const Jet& a, const Jet& b) { + Jet result{midpoint(a.a, b.a)}; + // To avoid overflow in the differential, compute + // (da + db) / 2 using midpoint. + for (int i = 0; i < N; ++i) { + result.v[i] = midpoint(a.v[i], b.v[i]); + } + return result; +} +#endif // defined(CERES_HAS_CPP20) + +// atan2(b + db, a + da) ~= atan2(b, a) + (- b da + a db) / (a^2 + b^2) +// +// In words: the rate of change of theta is 1/r times the rate of +// change of (x, y) in the positive angular direction. +template +inline Jet atan2(const Jet& g, const Jet& f) { + // Note order of arguments: + // + // f = a + da + // g = b + db + + T const tmp = T(1.0) / (f.a * f.a + g.a * g.a); + return Jet(atan2(g.a, f.a), tmp * (-g.a * f.v + f.a * g.v)); +} + +// Computes the square x^2 of a real number x (not the Euclidean L^2 norm as +// the name might suggest). +// +// NOTE: While std::norm is primarily intended for computing the squared +// magnitude of a std::complex<> number, the current Jet implementation does not +// support mixing a scalar T in its real part and std::complex and in the +// infinitesimal. Mixed Jet support is necessary for the type decay from +// std::complex to T (the squared magnitude of a complex number is always +// real) performed by std::norm. +// +// norm(x + h) ~= norm(x) + 2x h +template +inline Jet norm(const Jet& f) { + return Jet(norm(f.a), T(2) * f.a * f.v); +} + +// pow -- base is a differentiable function, exponent is a constant. +// (a+da)^p ~= a^p + p*a^(p-1) da +template +inline Jet pow(const Jet& f, double g) { + T const tmp = g * pow(f.a, g - T(1.0)); + return Jet(pow(f.a, g), tmp * f.v); +} + +// pow -- base is a constant, exponent is a differentiable function. +// We have various special cases, see the comment for pow(Jet, Jet) for +// analysis: +// +// 1. For f > 0 we have: (f)^(g + dg) ~= f^g + f^g log(f) dg +// +// 2. For f == 0 and g > 0 we have: (f)^(g + dg) ~= f^g +// +// 3. For f < 0 and integer g we have: (f)^(g + dg) ~= f^g but if dg +// != 0, the derivatives are not defined and we return NaN. + +template +inline Jet pow(T f, const Jet& g) { + Jet result; + + if (fpclassify(f) == FP_ZERO && g > 0) { + // Handle case 2. + result = Jet(T(0.0)); + } else { + if (f < 0 && g == floor(g.a)) { // Handle case 3. + result = Jet(pow(f, g.a)); + for (int i = 0; i < N; i++) { + if (fpclassify(g.v[i]) != FP_ZERO) { + // Return a NaN when g.v != 0. + result.v[i] = std::numeric_limits::quiet_NaN(); + } + } + } else { + // Handle case 1. + T const tmp = pow(f, g.a); + result = Jet(tmp, log(f) * tmp * g.v); + } + } + + return result; +} + +// pow -- both base and exponent are differentiable functions. This has a +// variety of special cases that require careful handling. +// +// 1. For f > 0: +// (f + df)^(g + dg) ~= f^g + f^(g - 1) * (g * df + f * log(f) * dg) +// The numerical evaluation of f * log(f) for f > 0 is well behaved, even for +// extremely small values (e.g. 1e-99). +// +// 2. For f == 0 and g > 1: (f + df)^(g + dg) ~= 0 +// This cases is needed because log(0) can not be evaluated in the f > 0 +// expression. However the function f*log(f) is well behaved around f == 0 +// and its limit as f-->0 is zero. +// +// 3. For f == 0 and g == 1: (f + df)^(g + dg) ~= 0 + df +// +// 4. For f == 0 and 0 < g < 1: The value is finite but the derivatives are not. +// +// 5. For f == 0 and g < 0: The value and derivatives of f^g are not finite. +// +// 6. For f == 0 and g == 0: The C standard incorrectly defines 0^0 to be 1 +// "because there are applications that can exploit this definition". We +// (arbitrarily) decree that derivatives here will be nonfinite, since that +// is consistent with the behavior for f == 0, g < 0 and 0 < g < 1. +// Practically any definition could have been justified because mathematical +// consistency has been lost at this point. +// +// 7. For f < 0, g integer, dg == 0: (f + df)^(g + dg) ~= f^g + g * f^(g - 1) df +// This is equivalent to the case where f is a differentiable function and g +// is a constant (to first order). +// +// 8. For f < 0, g integer, dg != 0: The value is finite but the derivatives are +// not, because any change in the value of g moves us away from the point +// with a real-valued answer into the region with complex-valued answers. +// +// 9. For f < 0, g noninteger: The value and derivatives of f^g are not finite. + +template +inline Jet pow(const Jet& f, const Jet& g) { + Jet result; + + if (fpclassify(f) == FP_ZERO && g >= 1) { + // Handle cases 2 and 3. + if (g > 1) { + result = Jet(T(0.0)); + } else { + result = f; + } + + } else { + if (f < 0 && g == floor(g.a)) { + // Handle cases 7 and 8. + T const tmp = g.a * pow(f.a, g.a - T(1.0)); + result = Jet(pow(f.a, g.a), tmp * f.v); + for (int i = 0; i < N; i++) { + if (fpclassify(g.v[i]) != FP_ZERO) { + // Return a NaN when g.v != 0. + result.v[i] = T(std::numeric_limits::quiet_NaN()); + } + } + } else { + // Handle the remaining cases. For cases 4,5,6,9 we allow the log() + // function to generate -HUGE_VAL or NaN, since those cases result in a + // nonfinite derivative. + T const tmp1 = pow(f.a, g.a); + T const tmp2 = g.a * pow(f.a, g.a - T(1.0)); + T const tmp3 = tmp1 * log(f.a); + result = Jet(tmp1, tmp2 * f.v + tmp3 * g.v); + } + } + + return result; +} + +// Note: This has to be in the ceres namespace for argument dependent lookup to +// function correctly. Otherwise statements like CHECK_LE(x, 2.0) fail with +// strange compile errors. +template +inline std::ostream& operator<<(std::ostream& s, const Jet& z) { + s << "[" << z.a << " ; "; + for (int i = 0; i < N; ++i) { + s << z.v[i]; + if (i != N - 1) { + s << ", "; + } + } + s << "]"; + return s; +} +} // namespace ceres + +namespace std { +template +struct numeric_limits> { + static constexpr bool is_specialized = true; + static constexpr bool is_signed = std::numeric_limits::is_signed; + static constexpr bool is_integer = std::numeric_limits::is_integer; + static constexpr bool is_exact = std::numeric_limits::is_exact; + static constexpr bool has_infinity = std::numeric_limits::has_infinity; + static constexpr bool has_quiet_NaN = std::numeric_limits::has_quiet_NaN; + static constexpr bool has_signaling_NaN = + std::numeric_limits::has_signaling_NaN; + static constexpr bool is_iec559 = std::numeric_limits::is_iec559; + static constexpr bool is_bounded = std::numeric_limits::is_bounded; + static constexpr bool is_modulo = std::numeric_limits::is_modulo; + + static constexpr std::float_denorm_style has_denorm = + std::numeric_limits::has_denorm; + static constexpr std::float_round_style round_style = + std::numeric_limits::round_style; + + static constexpr int digits = std::numeric_limits::digits; + static constexpr int digits10 = std::numeric_limits::digits10; + static constexpr int max_digits10 = std::numeric_limits::max_digits10; + static constexpr int radix = std::numeric_limits::radix; + static constexpr int min_exponent = std::numeric_limits::min_exponent; + static constexpr int min_exponent10 = std::numeric_limits::max_exponent10; + static constexpr int max_exponent = std::numeric_limits::max_exponent; + static constexpr int max_exponent10 = std::numeric_limits::max_exponent10; + static constexpr bool traps = std::numeric_limits::traps; + static constexpr bool tinyness_before = + std::numeric_limits::tinyness_before; + + static constexpr ceres::Jet min + CERES_PREVENT_MACRO_SUBSTITUTION() noexcept { + return ceres::Jet((std::numeric_limits::min)()); + } + static constexpr ceres::Jet lowest() noexcept { + return ceres::Jet(std::numeric_limits::lowest()); + } + static constexpr ceres::Jet epsilon() noexcept { + return ceres::Jet(std::numeric_limits::epsilon()); + } + static constexpr ceres::Jet round_error() noexcept { + return ceres::Jet(std::numeric_limits::round_error()); + } + static constexpr ceres::Jet infinity() noexcept { + return ceres::Jet(std::numeric_limits::infinity()); + } + static constexpr ceres::Jet quiet_NaN() noexcept { + return ceres::Jet(std::numeric_limits::quiet_NaN()); + } + static constexpr ceres::Jet signaling_NaN() noexcept { + return ceres::Jet(std::numeric_limits::signaling_NaN()); + } + static constexpr ceres::Jet denorm_min() noexcept { + return ceres::Jet(std::numeric_limits::denorm_min()); + } + + static constexpr ceres::Jet max + CERES_PREVENT_MACRO_SUBSTITUTION() noexcept { + return ceres::Jet((std::numeric_limits::max)()); + } +}; + +} // namespace std + +namespace Eigen { + +// Creating a specialization of NumTraits enables placing Jet objects inside +// Eigen arrays, getting all the goodness of Eigen combined with autodiff. +template +struct NumTraits> { + using Real = ceres::Jet; + using NonInteger = ceres::Jet; + using Nested = ceres::Jet; + using Literal = ceres::Jet; + + static typename ceres::Jet dummy_precision() { + return ceres::Jet(1e-12); + } + + static inline Real epsilon() { + return Real(std::numeric_limits::epsilon()); + } + + static inline int digits10() { return NumTraits::digits10(); } + + enum { + IsComplex = 0, + IsInteger = 0, + IsSigned, + ReadCost = 1, + AddCost = 1, + // For Jet types, multiplication is more expensive than addition. + MulCost = 3, + HasFloatingPoint = 1, + RequireInitialization = 1 + }; + + template + struct Div { + enum { +#if defined(EIGEN_VECTORIZE_AVX) + AVX = true, +#else + AVX = false, +#endif + + // Assuming that for Jets, division is as expensive as + // multiplication. + Cost = 3 + }; + }; + + static inline Real highest() { return Real((std::numeric_limits::max)()); } + static inline Real lowest() { return Real(-(std::numeric_limits::max)()); } +}; + +// Specifying the return type of binary operations between Jets and scalar types +// allows you to perform matrix/array operations with Eigen matrices and arrays +// such as addition, subtraction, multiplication, and division where one Eigen +// matrix/array is of type Jet and the other is a scalar type. This improves +// performance by using the optimized scalar-to-Jet binary operations but +// is only available on Eigen versions >= 3.3 +template +struct ScalarBinaryOpTraits, T, BinaryOp> { + using ReturnType = ceres::Jet; +}; +template +struct ScalarBinaryOpTraits, BinaryOp> { + using ReturnType = ceres::Jet; +}; + +} // namespace Eigen + +#endif // CERES_PUBLIC_JET_H_ diff --git a/include/ceres/jet_fwd.h b/include/ceres/jet_fwd.h new file mode 100644 index 0000000000000000000000000000000000000000..fbb6286958cf48e006d98c5ec2797a15b52eaaed --- /dev/null +++ b/include/ceres/jet_fwd.h @@ -0,0 +1,44 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2022 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sergiu.deitsch@gmail.com (Sergiu Deitsch) +// + +#ifndef CERES_PUBLIC_JET_FWD_H_ +#define CERES_PUBLIC_JET_FWD_H_ + +namespace ceres { + +// Jet forward declaration necessary for the following partial specialization of +// std::common_type and type traits. +template +struct Jet; + +} // namespace ceres + +#endif // CERES_PUBLIC_JET_FWD_H_ diff --git a/include/ceres/line_manifold.h b/include/ceres/line_manifold.h new file mode 100644 index 0000000000000000000000000000000000000000..f8f1b235220c74ba863360c29fd95f63988c6575 --- /dev/null +++ b/include/ceres/line_manifold.h @@ -0,0 +1,304 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2022 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: jodebo_beck@gmx.de (Johannes Beck) +// + +#ifndef CERES_PUBLIC_LINE_MANIFOLD_H_ +#define CERES_PUBLIC_LINE_MANIFOLD_H_ + +#include +#include +#include +#include +#include + +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" +#include "ceres/internal/householder_vector.h" +#include "ceres/internal/sphere_manifold_functions.h" +#include "ceres/manifold.h" +#include "ceres/types.h" +#include "glog/logging.h" + +namespace ceres { +// This provides a manifold for lines, where the line is +// over-parameterized by an origin point and a direction vector. So the +// parameter vector size needs to be two times the ambient space dimension, +// where the first half is interpreted as the origin point and the second half +// as the direction. +// +// The plus operator for the line direction is the same as for the +// SphereManifold. The update of the origin point is +// perpendicular to the line direction before the update. +// +// This manifold is a special case of the affine Grassmannian +// manifold (see https://en.wikipedia.org/wiki/Affine_Grassmannian_(manifold)) +// for the case Graff_1(R^n). +// +// The class works with dynamic and static ambient space dimensions. If the +// ambient space dimensions is known at compile time use +// +// LineManifold<3> manifold; +// +// If the ambient space dimensions is not known at compile time the template +// parameter needs to be set to ceres::DYNAMIC and the actual dimension needs +// to be provided as a constructor argument: +// +// LineManifold manifold(ambient_dim); +// +template +class LineManifold final : public Manifold { + public: + static_assert(AmbientSpaceDimension == DYNAMIC || AmbientSpaceDimension >= 2, + "The ambient space must be at least 2."); + static_assert(ceres::DYNAMIC == Eigen::Dynamic, + "ceres::DYNAMIC needs to be the same as Eigen::Dynamic."); + + LineManifold(); + explicit LineManifold(int size); + + int AmbientSize() const override { return 2 * size_; } + int TangentSize() const override { return 2 * (size_ - 1); } + bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const override; + bool PlusJacobian(const double* x, double* jacobian) const override; + bool Minus(const double* y, + const double* x, + double* y_minus_x) const override; + bool MinusJacobian(const double* x, double* jacobian) const override; + + private: + static constexpr bool IsDynamic = (AmbientSpaceDimension == ceres::DYNAMIC); + static constexpr int TangentSpaceDimension = + IsDynamic ? ceres::DYNAMIC : AmbientSpaceDimension - 1; + + static constexpr int DAmbientSpaceDimension = + IsDynamic ? ceres::DYNAMIC : 2 * AmbientSpaceDimension; + static constexpr int DTangentSpaceDimension = + IsDynamic ? ceres::DYNAMIC : 2 * TangentSpaceDimension; + + using AmbientVector = Eigen::Matrix; + using TangentVector = Eigen::Matrix; + using MatrixPlusJacobian = Eigen::Matrix; + using MatrixMinusJacobian = Eigen::Matrix; + + const int size_{AmbientSpaceDimension}; +}; + +template +LineManifold::LineManifold() + : size_{AmbientSpaceDimension} { + static_assert( + AmbientSpaceDimension != Eigen::Dynamic, + "The size is set to dynamic. Please call the constructor with a size."); +} + +template +LineManifold::LineManifold(int size) : size_{size} { + if (AmbientSpaceDimension != Eigen::Dynamic) { + CHECK_EQ(AmbientSpaceDimension, size) + << "Specified size by template parameter differs from the supplied " + "one."; + } else { + CHECK_GT(size_, 1) + << "The size of the manifold needs to be greater than 1."; + } +} + +template +bool LineManifold::Plus(const double* x_ptr, + const double* delta_ptr, + double* x_plus_delta_ptr) const { + // We seek a box plus operator of the form + // + // [o*, d*] = Plus([o, d], [delta_o, delta_d]) + // + // where o is the origin point, d is the direction vector, delta_o is + // the delta of the origin point and delta_d the delta of the direction and + // o* and d* is the updated origin point and direction. + // + // We separate the Plus operator into the origin point and directional part + // d* = Plus_d(d, delta_d) + // o* = Plus_o(o, d, delta_o) + // + // The direction update function Plus_d is the same as as the SphereManifold: + // + // d* = H_{v(d)} [0.5 sinc(0.5 |delta_d|) delta_d, cos(0.5 |delta_d|)]^T + // + // where H is the householder matrix + // H_{v} = I - (2 / |v|^2) v v^T + // and + // v(d) = d - sign(d_n) |d| e_n. + // + // The origin point update function Plus_o is defined as + // + // o* = o + H_{v(d)} [0.5 delta_o, 0]^T. + + Eigen::Map o(x_ptr, size_); + Eigen::Map d(x_ptr + size_, size_); + + Eigen::Map delta_o(delta_ptr, size_ - 1); + Eigen::Map delta_d(delta_ptr + size_ - 1, size_ - 1); + Eigen::Map o_plus_delta(x_plus_delta_ptr, size_); + Eigen::Map d_plus_delta(x_plus_delta_ptr + size_, size_); + + const double norm_delta_d = delta_d.norm(); + + o_plus_delta = o; + + // Shortcut for zero delta direction. + if (norm_delta_d == 0.0) { + d_plus_delta = d; + + if (delta_o.isZero(0.0)) { + return true; + } + } + + // Calculate the householder transformation which is needed for f_d and f_o. + AmbientVector v(size_); + double beta; + + // NOTE: The explicit template arguments are needed here because + // ComputeHouseholderVector is templated and some versions of MSVC + // have trouble deducing the type of v automatically. + internal::ComputeHouseholderVector, + double, + AmbientSpaceDimension>(d, &v, &beta); + + if (norm_delta_d != 0.0) { + internal::ComputeSphereManifoldPlus( + v, beta, d, delta_d, norm_delta_d, &d_plus_delta); + } + + // The null space is in the direction of the line, so the tangent space is + // perpendicular to the line direction. This is achieved by using the + // householder matrix of the direction and allow only movements + // perpendicular to e_n. + // + // The factor of 0.5 is used to be consistent with the line direction + // update. + AmbientVector y(size_); + y << 0.5 * delta_o, 0; + o_plus_delta += internal::ApplyHouseholderVector(y, v, beta); + + return true; +} + +template +bool LineManifold::PlusJacobian( + const double* x_ptr, double* jacobian_ptr) const { + Eigen::Map d(x_ptr + size_, size_); + Eigen::Map jacobian( + jacobian_ptr, 2 * size_, 2 * (size_ - 1)); + + // Clear the Jacobian as only half of the matrix is not zero. + jacobian.setZero(); + + auto jacobian_d = + jacobian + .template topLeftCorner( + size_, size_ - 1); + auto jacobian_o = jacobian.template bottomRightCorner( + size_, size_ - 1); + internal::ComputeSphereManifoldPlusJacobian(d, &jacobian_d); + jacobian_o = jacobian_d; + return true; +} + +template +bool LineManifold::Minus(const double* y_ptr, + const double* x_ptr, + double* y_minus_x) const { + Eigen::Map y_o(y_ptr, size_); + Eigen::Map y_d(y_ptr + size_, size_); + Eigen::Map x_o(x_ptr, size_); + Eigen::Map x_d(x_ptr + size_, size_); + + Eigen::Map y_minus_x_o(y_minus_x, size_ - 1); + Eigen::Map y_minus_x_d(y_minus_x + size_ - 1, size_ - 1); + + AmbientVector v(size_); + double beta; + + // NOTE: The explicit template arguments are needed here because + // ComputeHouseholderVector is templated and some versions of MSVC + // have trouble deducing the type of v automatically. + internal::ComputeHouseholderVector, + double, + AmbientSpaceDimension>(x_d, &v, &beta); + + internal::ComputeSphereManifoldMinus(v, beta, x_d, y_d, &y_minus_x_d); + + AmbientVector delta_o = y_o - x_o; + const AmbientVector h_delta_o = + 2.0 * internal::ApplyHouseholderVector(delta_o, v, beta); + y_minus_x_o = h_delta_o.template head(size_ - 1); + + return true; +} + +template +bool LineManifold::MinusJacobian( + const double* x_ptr, double* jacobian_ptr) const { + Eigen::Map d(x_ptr + size_, size_); + Eigen::Map jacobian( + jacobian_ptr, 2 * (size_ - 1), 2 * size_); + + // Clear the Jacobian as only half of the matrix is not zero. + jacobian.setZero(); + + auto jacobian_d = + jacobian + .template topLeftCorner( + size_ - 1, size_); + auto jacobian_o = jacobian.template bottomRightCorner( + size_ - 1, size_); + internal::ComputeSphereManifoldMinusJacobian(d, &jacobian_d); + jacobian_o = jacobian_d; + + return true; +} + +} // namespace ceres + +// clang-format off +#include "ceres/internal/reenable_warnings.h" +// clang-format on + +#endif // CERES_PUBLIC_LINE_MANIFOLD_H_ diff --git a/include/ceres/local_parameterization.h b/include/ceres/local_parameterization.h new file mode 100644 index 0000000000000000000000000000000000000000..5815dd17d15915812aaa61fe69f773201b21afa7 --- /dev/null +++ b/include/ceres/local_parameterization.h @@ -0,0 +1,371 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2019 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_PUBLIC_LOCAL_PARAMETERIZATION_H_ +#define CERES_PUBLIC_LOCAL_PARAMETERIZATION_H_ + +#include +#include +#include + +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" +#include "ceres/internal/port.h" + +namespace ceres { + +// WARNING: LocalParameterizations are deprecated. They will be removed from +// Ceres Solver in version 2.2.0. Please use Manifolds instead. + +// Purpose: Sometimes parameter blocks x can overparameterize a problem +// +// min f(x) +// x +// +// In that case it is desirable to choose a parameterization for the +// block itself to remove the null directions of the cost. More +// generally, if x lies on a manifold of a smaller dimension than the +// ambient space that it is embedded in, then it is numerically and +// computationally more effective to optimize it using a +// parameterization that lives in the tangent space of that manifold +// at each point. +// +// For example, a sphere in three dimensions is a 2 dimensional +// manifold, embedded in a three dimensional space. At each point on +// the sphere, the plane tangent to it defines a two dimensional +// tangent space. For a cost function defined on this sphere, given a +// point x, moving in the direction normal to the sphere at that point +// is not useful. Thus a better way to do a local optimization is to +// optimize over two dimensional vector delta in the tangent space at +// that point and then "move" to the point x + delta, where the move +// operation involves projecting back onto the sphere. Doing so +// removes a redundant dimension from the optimization, making it +// numerically more robust and efficient. +// +// More generally we can define a function +// +// x_plus_delta = Plus(x, delta), +// +// where x_plus_delta has the same size as x, and delta is of size +// less than or equal to x. The function Plus, generalizes the +// definition of vector addition. Thus it satisfies the identify +// +// Plus(x, 0) = x, for all x. +// +// A trivial version of Plus is when delta is of the same size as x +// and +// +// Plus(x, delta) = x + delta +// +// A more interesting case if x is two dimensional vector, and the +// user wishes to hold the first coordinate constant. Then, delta is a +// scalar and Plus is defined as +// +// Plus(x, delta) = x + [0] * delta +// [1] +// +// An example that occurs commonly in Structure from Motion problems +// is when camera rotations are parameterized using Quaternion. There, +// it is useful to only make updates orthogonal to that 4-vector +// defining the quaternion. One way to do this is to let delta be a 3 +// dimensional vector and define Plus to be +// +// Plus(x, delta) = [cos(|delta|), sin(|delta|) delta / |delta|] * x +// +// The multiplication between the two 4-vectors on the RHS is the +// standard quaternion product. +// +// Given f and a point x, optimizing f can now be restated as +// +// min f(Plus(x, delta)) +// delta +// +// Given a solution delta to this problem, the optimal value is then +// given by +// +// x* = Plus(x, delta) +// +// The class LocalParameterization defines the function Plus and its +// Jacobian which is needed to compute the Jacobian of f w.r.t delta. +class CERES_DEPRECATED_WITH_MSG( + "LocalParameterizations will be removed from the Ceres Solver API in " + "version 2.2.0. Use Manifolds instead.") + CERES_EXPORT LocalParameterization { + public: + virtual ~LocalParameterization(); + + // Generalization of the addition operation, + // + // x_plus_delta = Plus(x, delta) + // + // with the condition that Plus(x, 0) = x. + // + virtual bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const = 0; + + // The jacobian of Plus(x, delta) w.r.t delta at delta = 0. + // + // jacobian is a row-major GlobalSize() x LocalSize() matrix. + virtual bool ComputeJacobian(const double* x, double* jacobian) const = 0; + + // local_matrix = global_matrix * jacobian + // + // global_matrix is a num_rows x GlobalSize row major matrix. + // local_matrix is a num_rows x LocalSize row major matrix. + // jacobian(x) is the matrix returned by ComputeJacobian at x. + // + // This is only used by GradientProblem. For most normal uses, it is + // okay to use the default implementation. + virtual bool MultiplyByJacobian(const double* x, + const int num_rows, + const double* global_matrix, + double* local_matrix) const; + + // Size of x. + virtual int GlobalSize() const = 0; + + // Size of delta. + virtual int LocalSize() const = 0; +}; + +// Some basic parameterizations + +// Identity Parameterization: Plus(x, delta) = x + delta +class CERES_DEPRECATED_WITH_MSG("Use EuclideanManifold instead.") + CERES_EXPORT IdentityParameterization : public LocalParameterization { + public: + explicit IdentityParameterization(int size); + bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const override; + bool ComputeJacobian(const double* x, double* jacobian) const override; + bool MultiplyByJacobian(const double* x, + const int num_cols, + const double* global_matrix, + double* local_matrix) const override; + int GlobalSize() const override { return size_; } + int LocalSize() const override { return size_; } + + private: + const int size_; +}; + +// Hold a subset of the parameters inside a parameter block constant. +class CERES_DEPRECATED_WITH_MSG("Use SubsetManifold instead.") + CERES_EXPORT SubsetParameterization : public LocalParameterization { + public: + explicit SubsetParameterization(int size, + const std::vector& constant_parameters); + bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const override; + bool ComputeJacobian(const double* x, double* jacobian) const override; + bool MultiplyByJacobian(const double* x, + const int num_cols, + const double* global_matrix, + double* local_matrix) const override; + int GlobalSize() const override { + return static_cast(constancy_mask_.size()); + } + int LocalSize() const override { return local_size_; } + + private: + const int local_size_; + std::vector constancy_mask_; +}; + +// Plus(x, delta) = [cos(|delta|), sin(|delta|) delta / |delta|] * x +// with * being the quaternion multiplication operator. Here we assume +// that the first element of the quaternion vector is the real (cos +// theta) part. +class CERES_DEPRECATED_WITH_MSG("Use QuaternionManifold instead.") + CERES_EXPORT QuaternionParameterization : public LocalParameterization { + public: + bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const override; + bool ComputeJacobian(const double* x, double* jacobian) const override; + int GlobalSize() const override { return 4; } + int LocalSize() const override { return 3; } +}; + +// Implements the quaternion local parameterization for Eigen's representation +// of the quaternion. Eigen uses a different internal memory layout for the +// elements of the quaternion than what is commonly used. Specifically, Eigen +// stores the elements in memory as [x, y, z, w] where the real part is last +// whereas it is typically stored first. Note, when creating an Eigen quaternion +// through the constructor the elements are accepted in w, x, y, z order. Since +// Ceres operates on parameter blocks which are raw double pointers this +// difference is important and requires a different parameterization. +// +// Plus(x, delta) = [sin(|delta|) delta / |delta|, cos(|delta|)] * x +// with * being the quaternion multiplication operator. +class CERES_DEPRECATED_WITH_MSG("Use EigenQuaternionManifold instead.") + CERES_EXPORT EigenQuaternionParameterization + : public ceres::LocalParameterization { + public: + bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const override; + bool ComputeJacobian(const double* x, double* jacobian) const override; + int GlobalSize() const override { return 4; } + int LocalSize() const override { return 3; } +}; + +// This provides a parameterization for homogeneous vectors which are commonly +// used in Structure from Motion problems. One example where they are used is +// in representing points whose triangulation is ill-conditioned. Here it is +// advantageous to use an over-parameterization since homogeneous vectors can +// represent points at infinity. +// +// The plus operator is defined as +// Plus(x, delta) = +// [sin(0.5 * |delta|) * delta / |delta|, cos(0.5 * |delta|)] * x +// +// with * defined as an operator which applies the update orthogonal to x to +// remain on the sphere. We assume that the last element of x is the scalar +// component. The size of the homogeneous vector is required to be greater than +// 1. +class CERES_DEPRECATED_WITH_MSG("Use SphereManifold instead.") CERES_EXPORT + HomogeneousVectorParameterization : public LocalParameterization { + public: + explicit HomogeneousVectorParameterization(int size); + bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const override; + bool ComputeJacobian(const double* x, double* jacobian) const override; + int GlobalSize() const override { return size_; } + int LocalSize() const override { return size_ - 1; } + + private: + const int size_; +}; + +// This provides a parameterization for lines, where the line is +// over-parameterized by an origin point and a direction vector. So the +// parameter vector size needs to be two times the ambient space dimension, +// where the first half is interpreted as the origin point and the second half +// as the direction. +// +// The plus operator for the line direction is the same as for the +// HomogeneousVectorParameterization. The update of the origin point is +// perpendicular to the line direction before the update. +// +// This local parameterization is a special case of the affine Grassmannian +// manifold (see https://en.wikipedia.org/wiki/Affine_Grassmannian_(manifold)) +// for the case Graff_1(R^n). +template +class CERES_DEPRECATED_WITH_MSG("Use LineManifold instead.") + LineParameterization : public LocalParameterization { + public: + static_assert(AmbientSpaceDimension >= 2, + "The ambient space must be at least 2"); + + bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const override; + bool ComputeJacobian(const double* x, double* jacobian) const override; + int GlobalSize() const override { return 2 * AmbientSpaceDimension; } + int LocalSize() const override { return 2 * (AmbientSpaceDimension - 1); } +}; + +// Construct a local parameterization by taking the Cartesian product +// of a number of other local parameterizations. This is useful, when +// a parameter block is the cartesian product of two or more +// manifolds. For example the parameters of a camera consist of a +// rotation and a translation, i.e., SO(3) x R^3. +// +// Example usage: +// +// ProductParameterization product_param(new QuaterionionParameterization(), +// new IdentityParameterization(3)); +// +// is the local parameterization for a rigid transformation, where the +// rotation is represented using a quaternion. +// +class CERES_DEPRECATED_WITH_MSG("Use ProductManifold instead.") + CERES_EXPORT ProductParameterization : public LocalParameterization { + public: + ProductParameterization(const ProductParameterization&) = delete; + ProductParameterization& operator=(const ProductParameterization&) = delete; + // + // NOTE: The constructor takes ownership of the input local + // parameterizations. + // + template + explicit ProductParameterization(LocalParams*... local_params) + : local_params_(sizeof...(LocalParams)) { + constexpr int kNumLocalParams = sizeof...(LocalParams); + static_assert(kNumLocalParams >= 2, + "At least two local parameterizations must be specified."); + + using LocalParameterizationPtr = std::unique_ptr; + + // Wrap all raw pointers into std::unique_ptr for exception safety. + std::array local_params_array{ + LocalParameterizationPtr(local_params)...}; + + // Initialize internal state. + for (int i = 0; i < kNumLocalParams; ++i) { + LocalParameterizationPtr& param = local_params_[i]; + param = std::move(local_params_array[i]); + + buffer_size_ = + std::max(buffer_size_, param->LocalSize() * param->GlobalSize()); + global_size_ += param->GlobalSize(); + local_size_ += param->LocalSize(); + } + } + + bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const override; + bool ComputeJacobian(const double* x, double* jacobian) const override; + int GlobalSize() const override { return global_size_; } + int LocalSize() const override { return local_size_; } + + private: + std::vector> local_params_; + int local_size_{0}; + int global_size_{0}; + int buffer_size_{0}; +}; + +} // namespace ceres + +// clang-format off +#include "ceres/internal/reenable_warnings.h" +// clang-format on + +#include "ceres/internal/line_parameterization.h" + +#endif // CERES_PUBLIC_LOCAL_PARAMETERIZATION_H_ diff --git a/include/ceres/loss_function.h b/include/ceres/loss_function.h new file mode 100644 index 0000000000000000000000000000000000000000..8a5a37ff665d83d23ab76bb724fec3e7627a5aec --- /dev/null +++ b/include/ceres/loss_function.h @@ -0,0 +1,433 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2019 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// The LossFunction interface is the way users describe how residuals +// are converted to cost terms for the overall problem cost function. +// For the exact manner in which loss functions are converted to the +// overall cost for a problem, see problem.h. +// +// For least squares problem where there are no outliers and standard +// squared loss is expected, it is not necessary to create a loss +// function; instead passing a nullptr to the problem when adding +// residuals implies a standard squared loss. +// +// For least squares problems where the minimization may encounter +// input terms that contain outliers, that is, completely bogus +// measurements, it is important to use a loss function that reduces +// their associated penalty. +// +// Consider a structure from motion problem. The unknowns are 3D +// points and camera parameters, and the measurements are image +// coordinates describing the expected reprojected position for a +// point in a camera. For example, we want to model the geometry of a +// street scene with fire hydrants and cars, observed by a moving +// camera with unknown parameters, and the only 3D points we care +// about are the pointy tippy-tops of the fire hydrants. Our magic +// image processing algorithm, which is responsible for producing the +// measurements that are input to Ceres, has found and matched all +// such tippy-tops in all image frames, except that in one of the +// frame it mistook a car's headlight for a hydrant. If we didn't do +// anything special (i.e. if we used a basic quadratic loss), the +// residual for the erroneous measurement will result in extreme error +// due to the quadratic nature of squared loss. This results in the +// entire solution getting pulled away from the optimum to reduce +// the large error that would otherwise be attributed to the wrong +// measurement. +// +// Using a robust loss function, the cost for large residuals is +// reduced. In the example above, this leads to outlier terms getting +// downweighted so they do not overly influence the final solution. +// +// What cost function is best? +// +// In general, there isn't a principled way to select a robust loss +// function. The authors suggest starting with a non-robust cost, then +// only experimenting with robust loss functions if standard squared +// loss doesn't work. + +#ifndef CERES_PUBLIC_LOSS_FUNCTION_H_ +#define CERES_PUBLIC_LOSS_FUNCTION_H_ + +#include + +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" +#include "ceres/types.h" +#include "glog/logging.h" + +namespace ceres { + +class CERES_EXPORT LossFunction { + public: + virtual ~LossFunction(); + + // For a residual vector with squared 2-norm 'sq_norm', this method + // is required to fill in the value and derivatives of the loss + // function (rho in this example): + // + // out[0] = rho(sq_norm), + // out[1] = rho'(sq_norm), + // out[2] = rho''(sq_norm), + // + // Here the convention is that the contribution of a term to the + // cost function is given by 1/2 rho(s), where + // + // s = ||residuals||^2. + // + // Calling the method with a negative value of 's' is an error and + // the implementations are not required to handle that case. + // + // Most sane choices of rho() satisfy: + // + // rho(0) = 0, + // rho'(0) = 1, + // rho'(s) < 1 in outlier region, + // rho''(s) < 0 in outlier region, + // + // so that they mimic the least squares cost for small residuals. + virtual void Evaluate(double sq_norm, double out[3]) const = 0; +}; + +// Some common implementations follow below. +// +// Note: in the region of interest (i.e. s < 3) we have: +// TrivialLoss >= HuberLoss >= SoftLOneLoss >= CauchyLoss + +// This corresponds to no robustification. +// +// rho(s) = s +// +// At s = 0: rho = [0, 1, 0]. +// +// It is not normally necessary to use this, as passing nullptr for the +// loss function when building the problem accomplishes the same +// thing. +class CERES_EXPORT TrivialLoss final : public LossFunction { + public: + void Evaluate(double, double*) const override; +}; + +// Scaling +// ------- +// Given one robustifier +// s -> rho(s) +// one can change the length scale at which robustification takes +// place, by adding a scale factor 'a' as follows: +// +// s -> a^2 rho(s / a^2). +// +// The first and second derivatives are: +// +// s -> rho'(s / a^2), +// s -> (1 / a^2) rho''(s / a^2), +// +// but the behaviour near s = 0 is the same as the original function, +// i.e. +// +// rho(s) = s + higher order terms, +// a^2 rho(s / a^2) = s + higher order terms. +// +// The scalar 'a' should be positive. +// +// The reason for the appearance of squaring is that 'a' is in the +// units of the residual vector norm whereas 's' is a squared +// norm. For applications it is more convenient to specify 'a' than +// its square. The commonly used robustifiers below are described in +// un-scaled format (a = 1) but their implementations work for any +// non-zero value of 'a'. + +// Huber. +// +// rho(s) = s for s <= 1, +// rho(s) = 2 sqrt(s) - 1 for s >= 1. +// +// At s = 0: rho = [0, 1, 0]. +// +// The scaling parameter 'a' corresponds to 'delta' on this page: +// http://en.wikipedia.org/wiki/Huber_Loss_Function +class CERES_EXPORT HuberLoss final : public LossFunction { + public: + explicit HuberLoss(double a) : a_(a), b_(a * a) {} + void Evaluate(double, double*) const override; + + private: + const double a_; + // b = a^2. + const double b_; +}; + +// Soft L1, similar to Huber but smooth. +// +// rho(s) = 2 (sqrt(1 + s) - 1). +// +// At s = 0: rho = [0, 1, -1 / (2 * a^2)]. +class CERES_EXPORT SoftLOneLoss final : public LossFunction { + public: + explicit SoftLOneLoss(double a) : b_(a * a), c_(1 / b_) {} + void Evaluate(double, double*) const override; + + private: + // b = a^2. + const double b_; + // c = 1 / a^2. + const double c_; +}; + +// Inspired by the Cauchy distribution +// +// rho(s) = log(1 + s). +// +// At s = 0: rho = [0, 1, -1 / a^2]. +class CERES_EXPORT CauchyLoss final : public LossFunction { + public: + explicit CauchyLoss(double a) : b_(a * a), c_(1 / b_) {} + void Evaluate(double, double*) const override; + + private: + // b = a^2. + const double b_; + // c = 1 / a^2. + const double c_; +}; + +// Loss that is capped beyond a certain level using the arc-tangent function. +// The scaling parameter 'a' determines the level where falloff occurs. +// For costs much smaller than 'a', the loss function is linear and behaves like +// TrivialLoss, and for values much larger than 'a' the value asymptotically +// approaches the constant value of a * PI / 2. +// +// rho(s) = a atan(s / a). +// +// At s = 0: rho = [0, 1, 0]. +class CERES_EXPORT ArctanLoss final : public LossFunction { + public: + explicit ArctanLoss(double a) : a_(a), b_(1 / (a * a)) {} + void Evaluate(double, double*) const override; + + private: + const double a_; + // b = 1 / a^2. + const double b_; +}; + +// Loss function that maps to approximately zero cost in a range around the +// origin, and reverts to linear in error (quadratic in cost) beyond this range. +// The tolerance parameter 'a' sets the nominal point at which the +// transition occurs, and the transition size parameter 'b' sets the nominal +// distance over which most of the transition occurs. Both a and b must be +// greater than zero, and typically b will be set to a fraction of a. +// The slope rho'[s] varies smoothly from about 0 at s <= a - b to +// about 1 at s >= a + b. +// +// The term is computed as: +// +// rho(s) = b log(1 + exp((s - a) / b)) - c0. +// +// where c0 is chosen so that rho(0) == 0 +// +// c0 = b log(1 + exp(-a / b) +// +// This has the following useful properties: +// +// rho(s) == 0 for s = 0 +// rho'(s) ~= 0 for s << a - b +// rho'(s) ~= 1 for s >> a + b +// rho''(s) > 0 for all s +// +// In addition, all derivatives are continuous, and the curvature is +// concentrated in the range a - b to a + b. +// +// At s = 0: rho = [0, ~0, ~0]. +class CERES_EXPORT TolerantLoss final : public LossFunction { + public: + explicit TolerantLoss(double a, double b); + void Evaluate(double, double*) const override; + + private: + const double a_, b_, c_; +}; + +// This is the Tukey biweight loss function which aggressively +// attempts to suppress large errors. +// +// The term is computed as follows where the equations are scaled by a +// factor of 2 because the cost function is given by 1/2 rho(s): +// +// rho(s) = a^2 / 3 * (1 - (1 - s / a^2)^3 ) for s <= a^2, +// rho(s) = a^2 / 3 for s > a^2. +// +// At s = 0: rho = [0, 1, -2 / a^2] +class CERES_EXPORT TukeyLoss final : public ceres::LossFunction { + public: + explicit TukeyLoss(double a) : a_squared_(a * a) {} + void Evaluate(double, double*) const override; + + private: + const double a_squared_; +}; + +// Composition of two loss functions. The error is the result of first +// evaluating g followed by f to yield the composition f(g(s)). +// The loss functions must not be nullptr. +class CERES_EXPORT ComposedLoss final : public LossFunction { + public: + explicit ComposedLoss(const LossFunction* f, + Ownership ownership_f, + const LossFunction* g, + Ownership ownership_g); + ~ComposedLoss() override; + void Evaluate(double, double*) const override; + + private: + std::unique_ptr f_, g_; + const Ownership ownership_f_, ownership_g_; +}; + +// The discussion above has to do with length scaling: it affects the space +// in which s is measured. Sometimes you want to simply scale the output +// value of the robustifier. For example, you might want to weight +// different error terms differently (e.g., weight pixel reprojection +// errors differently from terrain errors). +// +// If rho is the wrapped robustifier, then this simply outputs +// s -> a * rho(s) +// +// The first and second derivatives are, not surprisingly +// s -> a * rho'(s) +// s -> a * rho''(s) +// +// Since we treat the a nullptr Loss function as the Identity loss +// function, rho = nullptr is a valid input and will result in the input +// being scaled by a. This provides a simple way of implementing a +// scaled ResidualBlock. +class CERES_EXPORT ScaledLoss final : public LossFunction { + public: + // Constructs a ScaledLoss wrapping another loss function. Takes + // ownership of the wrapped loss function or not depending on the + // ownership parameter. + ScaledLoss(const LossFunction* rho, double a, Ownership ownership) + : rho_(rho), a_(a), ownership_(ownership) {} + ScaledLoss(const ScaledLoss&) = delete; + void operator=(const ScaledLoss&) = delete; + + ~ScaledLoss() override { + if (ownership_ == DO_NOT_TAKE_OWNERSHIP) { + rho_.release(); + } + } + void Evaluate(double, double*) const override; + + private: + std::unique_ptr rho_; + const double a_; + const Ownership ownership_; +}; + +// Sometimes after the optimization problem has been constructed, we +// wish to mutate the scale of the loss function. For example, when +// performing estimation from data which has substantial outliers, +// convergence can be improved by starting out with a large scale, +// optimizing the problem and then reducing the scale. This can have +// better convergence behaviour than just using a loss function with a +// small scale. +// +// This templated class allows the user to implement a loss function +// whose scale can be mutated after an optimization problem has been +// constructed. +// +// Since we treat the a nullptr Loss function as the Identity loss +// function, rho = nullptr is a valid input. +// +// Example usage +// +// Problem problem; +// +// // Add parameter blocks +// +// CostFunction* cost_function = +// new AutoDiffCostFunction < UW_Camera_Mapper, 2, 9, 3>( +// new UW_Camera_Mapper(feature_x, feature_y)); +// +// LossFunctionWrapper* loss_function = new LossFunctionWrapper( +// new HuberLoss(1.0), TAKE_OWNERSHIP); +// +// problem.AddResidualBlock(cost_function, loss_function, parameters); +// +// Solver::Options options; +// Solger::Summary summary; +// +// Solve(options, &problem, &summary) +// +// loss_function->Reset(new HuberLoss(1.0), TAKE_OWNERSHIP); +// +// Solve(options, &problem, &summary) +// +class CERES_EXPORT LossFunctionWrapper final : public LossFunction { + public: + LossFunctionWrapper(LossFunction* rho, Ownership ownership) + : rho_(rho), ownership_(ownership) {} + + LossFunctionWrapper(const LossFunctionWrapper&) = delete; + void operator=(const LossFunctionWrapper&) = delete; + + ~LossFunctionWrapper() override { + if (ownership_ == DO_NOT_TAKE_OWNERSHIP) { + rho_.release(); + } + } + + void Evaluate(double sq_norm, double out[3]) const override { + if (rho_.get() == nullptr) { + out[0] = sq_norm; + out[1] = 1.0; + out[2] = 0.0; + } else { + rho_->Evaluate(sq_norm, out); + } + } + + void Reset(LossFunction* rho, Ownership ownership) { + if (ownership_ == DO_NOT_TAKE_OWNERSHIP) { + rho_.release(); + } + rho_.reset(rho); + ownership_ = ownership; + } + + private: + std::unique_ptr rho_; + Ownership ownership_; +}; + +} // namespace ceres + +#include "ceres/internal/reenable_warnings.h" + +#endif // CERES_PUBLIC_LOSS_FUNCTION_H_ diff --git a/include/ceres/manifold.h b/include/ceres/manifold.h new file mode 100644 index 0000000000000000000000000000000000000000..4d6e9fa0f5933c1dbf567f7bfb2d07d50d9cb8c1 --- /dev/null +++ b/include/ceres/manifold.h @@ -0,0 +1,411 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2022 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_PUBLIC_MANIFOLD_H_ +#define CERES_PUBLIC_MANIFOLD_H_ + +#include +#include +#include +#include +#include +#include + +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" +#include "ceres/types.h" +#include "glog/logging.h" + +namespace ceres { + +// In sensor fusion problems, often we have to model quantities that live in +// spaces known as Manifolds, for example the rotation/orientation of a sensor +// that is represented by a quaternion. +// +// Manifolds are spaces which locally look like Euclidean spaces. More +// precisely, at each point on the manifold there is a linear space that is +// tangent to the manifold. It has dimension equal to the intrinsic dimension of +// the manifold itself, which is less than or equal to the ambient space in +// which the manifold is embedded. +// +// For example, the tangent space to a point on a sphere in three dimensions is +// the two dimensional plane that is tangent to the sphere at that point. There +// are two reasons tangent spaces are interesting: +// +// 1. They are Eucliean spaces so the usual vector space operations apply there, +// which makes numerical operations easy. +// 2. Movement in the tangent space translate into movements along the manifold. +// Movements perpendicular to the tangent space do not translate into +// movements on the manifold. +// +// Returning to our sphere example, moving in the 2 dimensional plane +// tangent to the sphere and projecting back onto the sphere will move you away +// from the point you started from but moving along the normal at the same point +// and the projecting back onto the sphere brings you back to the point. +// +// The Manifold interface defines two operations (and their derivatives) +// involving the tangent space, allowing filtering and optimization to be +// performed on said manifold: +// +// 1. x_plus_delta = Plus(x, delta) +// 2. delta = Minus(x_plus_delta, x) +// +// "Plus" computes the result of moving along delta in the tangent space at x, +// and then projecting back onto the manifold that x belongs to. In Differential +// Geometry this is known as a "Retraction". It is a generalization of vector +// addition in Euclidean spaces. +// +// Given two points on the manifold, "Minus" computes the change delta to x in +// the tangent space at x, that will take it to x_plus_delta. +// +// Let us now consider two examples. +// +// The Euclidean space R^n is the simplest example of a manifold. It has +// dimension n (and so does its tangent space) and Plus and Minus are the +// familiar vector sum and difference operations. +// +// Plus(x, delta) = x + delta = y, +// Minus(y, x) = y - x = delta. +// +// A more interesting case is SO(3), the special orthogonal group in three +// dimensions - the space of 3x3 rotation matrices. SO(3) is a three dimensional +// manifold embedded in R^9 or R^(3x3). So points on SO(3) are represented using +// 9 dimensional vectors or 3x3 matrices, and points in its tangent spaces are +// represented by 3 dimensional vectors. +// +// Defining Plus and Minus are defined in terms of the matrix Exp and Log +// operations as follows: +// +// Let Exp(p, q, r) = [cos(theta) + cp^2, -sr + cpq , sq + cpr ] +// [sr + cpq , cos(theta) + cq^2, -sp + cqr ] +// [-sq + cpr , sp + cqr , cos(theta) + cr^2] +// +// where: theta = sqrt(p^2 + q^2 + r^2) +// s = sinc(theta) +// c = (1 - cos(theta))/theta^2 +// +// and Log(x) = 1/(2 sinc(theta))[x_32 - x_23, x_13 - x_31, x_21 - x_12] +// +// where: theta = acos((Trace(x) - 1)/2) +// +// Then, +// +// Plus(x, delta) = x Exp(delta) +// Minus(y, x) = Log(x^T y) +// +// For Plus and Minus to be mathematically consistent, the following identities +// must be satisfied at all points x on the manifold: +// +// 1. Plus(x, 0) = x. +// 2. For all y, Plus(x, Minus(y, x)) = y. +// 3. For all delta, Minus(Plus(x, delta), x) = delta. +// 4. For all delta_1, delta_2 +// |Minus(Plus(x, delta_1), Plus(x, delta_2)) <= |delta_1 - delta_2| +// +// Briefly: +// (1) Ensures that the tangent space is "centered" at x, and the zero vector is +// the identity element. +// (2) Ensures that any y can be reached from x. +// (3) Ensures that Plus is an injective (one-to-one) map. +// (4) Allows us to define a metric on the manifold. +// +// Additionally we require that Plus and Minus be sufficiently smooth. In +// particular they need to be differentiable everywhere on the manifold. +// +// For more details, please see +// +// "Integrating Generic Sensor Fusion Algorithms with Sound State +// Representations through Encapsulation of Manifolds" +// By C. Hertzberg, R. Wagner, U. Frese and L. Schroder +// https://arxiv.org/pdf/1107.1119.pdf +class CERES_EXPORT Manifold { + public: + virtual ~Manifold(); + + // Dimension of the ambient space in which the manifold is embedded. + virtual int AmbientSize() const = 0; + + // Dimension of the manifold/tangent space. + virtual int TangentSize() const = 0; + + // x_plus_delta = Plus(x, delta), + // + // A generalization of vector addition in Euclidean space, Plus computes the + // result of moving along delta in the tangent space at x, and then projecting + // back onto the manifold that x belongs to. + // + // x and x_plus_delta are AmbientSize() vectors. + // delta is a TangentSize() vector. + // + // Return value indicates if the operation was successful or not. + virtual bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const = 0; + + // Compute the derivative of Plus(x, delta) w.r.t delta at delta = 0, i.e. + // + // (D_2 Plus)(x, 0) + // + // jacobian is a row-major AmbientSize() x TangentSize() matrix. + // + // Return value indicates whether the operation was successful or not. + virtual bool PlusJacobian(const double* x, double* jacobian) const = 0; + + // tangent_matrix = ambient_matrix * (D_2 Plus)(x, 0) + // + // ambient_matrix is a row-major num_rows x AmbientSize() matrix. + // tangent_matrix is a row-major num_rows x TangentSize() matrix. + // + // Return value indicates whether the operation was successful or not. + // + // This function is only used by the GradientProblemSolver, where the + // dimension of the parameter block can be large and it may be more efficient + // to compute this product directly rather than first evaluating the Jacobian + // into a matrix and then doing a matrix vector product. + // + // Because this is not an often used function, we provide a default + // implementation for convenience. If performance becomes an issue then the + // user should consider implementing a specialization. + virtual bool RightMultiplyByPlusJacobian(const double* x, + const int num_rows, + const double* ambient_matrix, + double* tangent_matrix) const; + + // y_minus_x = Minus(y, x) + // + // Given two points on the manifold, Minus computes the change to x in the + // tangent space at x, that will take it to y. + // + // x and y are AmbientSize() vectors. + // y_minus_x is a TangentSize() vector. + // + // Return value indicates if the operation was successful or not. + virtual bool Minus(const double* y, + const double* x, + double* y_minus_x) const = 0; + + // Compute the derivative of Minus(y, x) w.r.t y at y = x, i.e + // + // (D_1 Minus) (x, x) + // + // Jacobian is a row-major TangentSize() x AmbientSize() matrix. + // + // Return value indicates whether the operation was successful or not. + virtual bool MinusJacobian(const double* x, double* jacobian) const = 0; +}; + +// The Euclidean manifold is another name for the ordinary vector space R^size, +// where the plus and minus operations are the usual vector addition and +// subtraction: +// Plus(x, delta) = x + delta +// Minus(y, x) = y - x. +// +// The class works with dynamic and static ambient space dimensions. If the +// ambient space dimensions is know at compile time use +// +// EuclideanManifold<3> manifold; +// +// If the ambient space dimensions is not known at compile time the template +// parameter needs to be set to ceres::DYNAMIC and the actual dimension needs +// to be provided as a constructor argument: +// +// EuclideanManifold manifold(ambient_dim); +template +class EuclideanManifold final : public Manifold { + public: + static_assert(Size == ceres::DYNAMIC || Size >= 0, + "The size of the manifold needs to be non-negative."); + static_assert(ceres::DYNAMIC == Eigen::Dynamic, + "ceres::DYNAMIC needs to be the same as Eigen::Dynamic."); + + EuclideanManifold() : size_{Size} { + static_assert( + Size != ceres::DYNAMIC, + "The size is set to dynamic. Please call the constructor with a size."); + } + + explicit EuclideanManifold(int size) : size_(size) { + if (Size != ceres::DYNAMIC) { + CHECK_EQ(Size, size) + << "Specified size by template parameter differs from the supplied " + "one."; + } else { + CHECK_GE(size_, 0) + << "The size of the manifold needs to be non-negative."; + } + } + + int AmbientSize() const override { return size_; } + int TangentSize() const override { return size_; } + + bool Plus(const double* x_ptr, + const double* delta_ptr, + double* x_plus_delta_ptr) const override { + Eigen::Map x(x_ptr, size_); + Eigen::Map delta(delta_ptr, size_); + Eigen::Map x_plus_delta(x_plus_delta_ptr, size_); + x_plus_delta = x + delta; + return true; + } + + bool PlusJacobian(const double* x_ptr, double* jacobian_ptr) const override { + Eigen::Map jacobian(jacobian_ptr, size_, size_); + jacobian.setIdentity(); + return true; + } + + bool RightMultiplyByPlusJacobian(const double* x, + const int num_rows, + const double* ambient_matrix, + double* tangent_matrix) const override { + std::copy_n(ambient_matrix, num_rows * size_, tangent_matrix); + return true; + } + + bool Minus(const double* y_ptr, + const double* x_ptr, + double* y_minus_x_ptr) const override { + Eigen::Map x(x_ptr, size_); + Eigen::Map y(y_ptr, size_); + Eigen::Map y_minus_x(y_minus_x_ptr, size_); + y_minus_x = y - x; + return true; + } + + bool MinusJacobian(const double* x_ptr, double* jacobian_ptr) const override { + Eigen::Map jacobian(jacobian_ptr, size_, size_); + jacobian.setIdentity(); + return true; + } + + private: + static constexpr bool IsDynamic = (Size == ceres::DYNAMIC); + using AmbientVector = Eigen::Matrix; + using MatrixJacobian = Eigen::Matrix; + + int size_{}; +}; + +// Hold a subset of the parameters inside a parameter block constant. +class CERES_EXPORT SubsetManifold final : public Manifold { + public: + SubsetManifold(int size, const std::vector& constant_parameters); + int AmbientSize() const override; + int TangentSize() const override; + + bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const override; + bool PlusJacobian(const double* x, double* jacobian) const override; + bool RightMultiplyByPlusJacobian(const double* x, + const int num_rows, + const double* ambient_matrix, + double* tangent_matrix) const override; + bool Minus(const double* y, + const double* x, + double* y_minus_x) const override; + bool MinusJacobian(const double* x, double* jacobian) const override; + + private: + const int tangent_size_ = 0; + std::vector constancy_mask_; +}; + +// Implements the manifold for a Hamilton quaternion as defined in +// https://en.wikipedia.org/wiki/Quaternion. Quaternions are represented as +// unit norm 4-vectors, i.e. +// +// q = [q0; q1; q2; q3], |q| = 1 +// +// is the ambient space representation. +// +// q0 scalar part. +// q1 coefficient of i. +// q2 coefficient of j. +// q3 coefficient of k. +// +// where: i*i = j*j = k*k = -1 and i*j = k, j*k = i, k*i = j. +// +// The tangent space is R^3, which relates to the ambient space through the +// Plus and Minus operations defined as: +// +// Plus(x, delta) = [cos(|delta|); sin(|delta|) * delta / |delta|] * x +// Minus(y, x) = to_delta(y * x^{-1}) +// +// where "*" is the quaternion product and because q is a unit quaternion +// (|q|=1), q^-1 = [q0; -q1; -q2; -q3] +// +// and to_delta( [q0; u_{3x1}] ) = u / |u| * atan2(|u|, q0) +class CERES_EXPORT QuaternionManifold final : public Manifold { + public: + int AmbientSize() const override { return 4; } + int TangentSize() const override { return 3; } + + bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const override; + bool PlusJacobian(const double* x, double* jacobian) const override; + bool Minus(const double* y, + const double* x, + double* y_minus_x) const override; + bool MinusJacobian(const double* x, double* jacobian) const override; +}; + +// Implements the quaternion manifold for Eigen's representation of the +// Hamilton quaternion. Geometrically it is exactly the same as the +// QuaternionManifold defined above. However, Eigen uses a different internal +// memory layout for the elements of the quaternion than what is commonly +// used. It stores the quaternion in memory as [q1, q2, q3, q0] or +// [x, y, z, w] where the real (scalar) part is last. +// +// Since Ceres operates on parameter blocks which are raw double pointers this +// difference is important and requires a different manifold. +class CERES_EXPORT EigenQuaternionManifold final : public Manifold { + public: + int AmbientSize() const override { return 4; } + int TangentSize() const override { return 3; } + + bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const override; + bool PlusJacobian(const double* x, double* jacobian) const override; + bool Minus(const double* y, + const double* x, + double* y_minus_x) const override; + bool MinusJacobian(const double* x, double* jacobian) const override; +}; + +} // namespace ceres + +// clang-format off +#include "ceres/internal/reenable_warnings.h" +// clang-format on + +#endif // CERES_PUBLIC_MANIFOLD_H_ diff --git a/include/ceres/manifold_test_utils.h b/include/ceres/manifold_test_utils.h new file mode 100644 index 0000000000000000000000000000000000000000..3f9fb21e8f34747f23948aab3359f683f41f59b4 --- /dev/null +++ b/include/ceres/manifold_test_utils.h @@ -0,0 +1,328 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2022 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include +#include +#include + +#include "ceres/dynamic_numeric_diff_cost_function.h" +#include "ceres/internal/eigen.h" +#include "ceres/manifold.h" +#include "ceres/numeric_diff_options.h" +#include "ceres/types.h" +#include "gmock/gmock.h" +#include "gtest/gtest.h" + +namespace ceres { + +// Matchers and macros for help with testing Manifold objects. +// +// Testing a Manifold has two parts. +// +// 1. Checking that Manifold::Plus is correctly defined. This requires per +// manifold tests. +// +// 2. The other methods of the manifold have mathematical properties that make +// it compatible with Plus, as described in: +// +// "Integrating Generic Sensor Fusion Algorithms with Sound State +// Representations through Encapsulation of Manifolds" +// By C. Hertzberg, R. Wagner, U. Frese and L. Schroder +// https://arxiv.org/pdf/1107.1119.pdf +// +// These tests are implemented using generic matchers defined below which can +// all be called by the macro EXPECT_THAT_MANIFOLD_INVARIANTS_HOLD(manifold, x, +// delta, y, tolerance). See manifold_test.cc for example usage. + +// Checks that the invariant Plus(x, 0) == x holds. +MATCHER_P2(XPlusZeroIsXAt, x, tolerance, "") { + const int ambient_size = arg.AmbientSize(); + const int tangent_size = arg.TangentSize(); + + Vector actual = Vector::Zero(ambient_size); + Vector zero = Vector::Zero(tangent_size); + EXPECT_TRUE(arg.Plus(x.data(), zero.data(), actual.data())); + const double n = (actual - x).norm(); + const double d = x.norm(); + const double diffnorm = (d == 0.0) ? n : (n / d); + if (diffnorm > tolerance) { + *result_listener << "\nexpected (x): " << x.transpose() + << "\nactual: " << actual.transpose() + << "\ndiffnorm: " << diffnorm; + return false; + } + return true; +} + +// Checks that the invariant Minus(x, x) == 0 holds. +MATCHER_P2(XMinusXIsZeroAt, x, tolerance, "") { + const int tangent_size = arg.TangentSize(); + Vector actual = Vector::Zero(tangent_size); + EXPECT_TRUE(arg.Minus(x.data(), x.data(), actual.data())); + const double diffnorm = actual.norm(); + if (diffnorm > tolerance) { + *result_listener << "\nx: " << x.transpose() // + << "\nexpected: 0 0 0" + << "\nactual: " << actual.transpose() + << "\ndiffnorm: " << diffnorm; + return false; + } + return true; +} + +// Helper struct to curry Plus(x, .) so that it can be numerically +// differentiated. +struct PlusFunctor { + PlusFunctor(const Manifold& manifold, const double* x) + : manifold(manifold), x(x) {} + bool operator()(double const* const* parameters, double* x_plus_delta) const { + return manifold.Plus(x, parameters[0], x_plus_delta); + } + + const Manifold& manifold; + const double* x; +}; + +// Checks that the output of PlusJacobian matches the one obtained by +// numerically evaluating D_2 Plus(x,0). +MATCHER_P2(HasCorrectPlusJacobianAt, x, tolerance, "") { + const int ambient_size = arg.AmbientSize(); + const int tangent_size = arg.TangentSize(); + + NumericDiffOptions options; + options.ridders_relative_initial_step_size = 1e-4; + + DynamicNumericDiffCostFunction cost_function( + new PlusFunctor(arg, x.data()), TAKE_OWNERSHIP, options); + cost_function.AddParameterBlock(tangent_size); + cost_function.SetNumResiduals(ambient_size); + + Vector zero = Vector::Zero(tangent_size); + double* parameters[1] = {zero.data()}; + + Vector x_plus_zero = Vector::Zero(ambient_size); + Matrix expected = Matrix::Zero(ambient_size, tangent_size); + double* jacobians[1] = {expected.data()}; + + EXPECT_TRUE( + cost_function.Evaluate(parameters, x_plus_zero.data(), jacobians)); + + Matrix actual = Matrix::Random(ambient_size, tangent_size); + EXPECT_TRUE(arg.PlusJacobian(x.data(), actual.data())); + + const double n = (actual - expected).norm(); + const double d = expected.norm(); + const double diffnorm = (d == 0.0) ? n : n / d; + if (diffnorm > tolerance) { + *result_listener << "\nx: " << x.transpose() << "\nexpected: \n" + << expected << "\nactual:\n" + << actual << "\ndiff:\n" + << expected - actual << "\ndiffnorm : " << diffnorm; + return false; + } + return true; +} + +// Checks that the invariant Minus(Plus(x, delta), x) == delta holds. +MATCHER_P3(MinusPlusIsIdentityAt, x, delta, tolerance, "") { + const int ambient_size = arg.AmbientSize(); + const int tangent_size = arg.TangentSize(); + Vector x_plus_delta = Vector::Zero(ambient_size); + EXPECT_TRUE(arg.Plus(x.data(), delta.data(), x_plus_delta.data())); + Vector actual = Vector::Zero(tangent_size); + EXPECT_TRUE(arg.Minus(x_plus_delta.data(), x.data(), actual.data())); + + const double n = (actual - delta).norm(); + const double d = delta.norm(); + const double diffnorm = (d == 0.0) ? n : (n / d); + if (diffnorm > tolerance) { + *result_listener << "\nx: " << x.transpose() + << "\nexpected: " << delta.transpose() + << "\nactual:" << actual.transpose() + << "\ndiff:" << (delta - actual).transpose() + << "\ndiffnorm: " << diffnorm; + return false; + } + return true; +} + +// Checks that the invariant Plus(Minus(y, x), x) == y holds. +MATCHER_P3(PlusMinusIsIdentityAt, x, y, tolerance, "") { + const int ambient_size = arg.AmbientSize(); + const int tangent_size = arg.TangentSize(); + + Vector y_minus_x = Vector::Zero(tangent_size); + EXPECT_TRUE(arg.Minus(y.data(), x.data(), y_minus_x.data())); + + Vector actual = Vector::Zero(ambient_size); + EXPECT_TRUE(arg.Plus(x.data(), y_minus_x.data(), actual.data())); + + const double n = (actual - y).norm(); + const double d = y.norm(); + const double diffnorm = (d == 0.0) ? n : (n / d); + if (diffnorm > tolerance) { + *result_listener << "\nx: " << x.transpose() + << "\nexpected: " << y.transpose() + << "\nactual:" << actual.transpose() + << "\ndiff:" << (y - actual).transpose() + << "\ndiffnorm: " << diffnorm; + return false; + } + return true; +} + +// Helper struct to curry Minus(., x) so that it can be numerically +// differentiated. +struct MinusFunctor { + MinusFunctor(const Manifold& manifold, const double* x) + : manifold(manifold), x(x) {} + bool operator()(double const* const* parameters, double* y_minus_x) const { + return manifold.Minus(parameters[0], x, y_minus_x); + } + + const Manifold& manifold; + const double* x; +}; + +// Checks that the output of MinusJacobian matches the one obtained by +// numerically evaluating D_1 Minus(x,x). +MATCHER_P2(HasCorrectMinusJacobianAt, x, tolerance, "") { + const int ambient_size = arg.AmbientSize(); + const int tangent_size = arg.TangentSize(); + + Vector y = x; + Vector y_minus_x = Vector::Zero(tangent_size); + + NumericDiffOptions options; + options.ridders_relative_initial_step_size = 1e-4; + DynamicNumericDiffCostFunction cost_function( + new MinusFunctor(arg, x.data()), TAKE_OWNERSHIP, options); + cost_function.AddParameterBlock(ambient_size); + cost_function.SetNumResiduals(tangent_size); + + double* parameters[1] = {y.data()}; + + Matrix expected = Matrix::Zero(tangent_size, ambient_size); + double* jacobians[1] = {expected.data()}; + + EXPECT_TRUE(cost_function.Evaluate(parameters, y_minus_x.data(), jacobians)); + + Matrix actual = Matrix::Random(tangent_size, ambient_size); + EXPECT_TRUE(arg.MinusJacobian(x.data(), actual.data())); + + const double n = (actual - expected).norm(); + const double d = expected.norm(); + const double diffnorm = (d == 0.0) ? n : (n / d); + if (diffnorm > tolerance) { + *result_listener << "\nx: " << x.transpose() << "\nexpected: \n" + << expected << "\nactual:\n" + << actual << "\ndiff:\n" + << expected - actual << "\ndiffnorm: " << diffnorm; + return false; + } + return true; +} + +// Checks that D_delta Minus(Plus(x, delta), x) at delta = 0 is an identity +// matrix. +MATCHER_P2(MinusPlusJacobianIsIdentityAt, x, tolerance, "") { + const int ambient_size = arg.AmbientSize(); + const int tangent_size = arg.TangentSize(); + + Matrix plus_jacobian(ambient_size, tangent_size); + EXPECT_TRUE(arg.PlusJacobian(x.data(), plus_jacobian.data())); + Matrix minus_jacobian(tangent_size, ambient_size); + EXPECT_TRUE(arg.MinusJacobian(x.data(), minus_jacobian.data())); + + const Matrix actual = minus_jacobian * plus_jacobian; + const Matrix expected = Matrix::Identity(tangent_size, tangent_size); + + const double n = (actual - expected).norm(); + const double d = expected.norm(); + const double diffnorm = n / d; + if (diffnorm > tolerance) { + *result_listener << "\nx: " << x.transpose() << "\nexpected: \n" + << expected << "\nactual:\n" + << actual << "\ndiff:\n" + << expected - actual << "\ndiffnorm: " << diffnorm; + + return false; + } + return true; +} + +// Verify that the output of RightMultiplyByPlusJacobian is ambient_matrix * +// plus_jacobian. +MATCHER_P2(HasCorrectRightMultiplyByPlusJacobianAt, x, tolerance, "") { + const int ambient_size = arg.AmbientSize(); + const int tangent_size = arg.TangentSize(); + + constexpr int kMinNumRows = 0; + constexpr int kMaxNumRows = 3; + for (int num_rows = kMinNumRows; num_rows <= kMaxNumRows; ++num_rows) { + Matrix plus_jacobian = Matrix::Random(ambient_size, tangent_size); + EXPECT_TRUE(arg.PlusJacobian(x.data(), plus_jacobian.data())); + + Matrix ambient_matrix = Matrix::Random(num_rows, ambient_size); + Matrix expected = ambient_matrix * plus_jacobian; + + Matrix actual = Matrix::Random(num_rows, tangent_size); + EXPECT_TRUE(arg.RightMultiplyByPlusJacobian( + x.data(), num_rows, ambient_matrix.data(), actual.data())); + const double n = (actual - expected).norm(); + const double d = expected.norm(); + const double diffnorm = (d == 0.0) ? n : (n / d); + if (diffnorm > tolerance) { + *result_listener << "\nx: " << x.transpose() << "\nambient_matrix : \n" + << ambient_matrix << "\nplus_jacobian : \n" + << plus_jacobian << "\nexpected: \n" + << expected << "\nactual:\n" + << actual << "\ndiff:\n" + << expected - actual << "\ndiffnorm : " << diffnorm; + return false; + } + } + return true; +} + +#define EXPECT_THAT_MANIFOLD_INVARIANTS_HOLD(manifold, x, delta, y, tolerance) \ + Vector zero_tangent = Vector::Zero(manifold.TangentSize()); \ + EXPECT_THAT(manifold, XPlusZeroIsXAt(x, tolerance)); \ + EXPECT_THAT(manifold, XMinusXIsZeroAt(x, tolerance)); \ + EXPECT_THAT(manifold, MinusPlusIsIdentityAt(x, delta, tolerance)); \ + EXPECT_THAT(manifold, MinusPlusIsIdentityAt(x, zero_tangent, tolerance)); \ + EXPECT_THAT(manifold, PlusMinusIsIdentityAt(x, x, tolerance)); \ + EXPECT_THAT(manifold, PlusMinusIsIdentityAt(x, y, tolerance)); \ + EXPECT_THAT(manifold, HasCorrectPlusJacobianAt(x, tolerance)); \ + EXPECT_THAT(manifold, HasCorrectMinusJacobianAt(x, tolerance)); \ + EXPECT_THAT(manifold, MinusPlusJacobianIsIdentityAt(x, tolerance)); \ + EXPECT_THAT(manifold, HasCorrectRightMultiplyByPlusJacobianAt(x, tolerance)); + +} // namespace ceres diff --git a/include/ceres/normal_prior.h b/include/ceres/normal_prior.h new file mode 100644 index 0000000000000000000000000000000000000000..c5c7f3e623efaaa247b3e3e67c9cf32cb11725bf --- /dev/null +++ b/include/ceres/normal_prior.h @@ -0,0 +1,78 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2019 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Cost term that implements a prior on a parameter block using a +// normal distribution. + +#ifndef CERES_PUBLIC_NORMAL_PRIOR_H_ +#define CERES_PUBLIC_NORMAL_PRIOR_H_ + +#include "ceres/cost_function.h" +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/eigen.h" + +namespace ceres { + +// Implements a cost function of the form +// +// cost(x) = ||A(x - b)||^2 +// +// where, the matrix A and the vector b are fixed and x is the +// variable. In case the user is interested in implementing a cost +// function of the form +// +// cost(x) = (x - mu)^T S^{-1} (x - mu) +// +// where, mu is a vector and S is a covariance matrix, then, A = +// S^{-1/2}, i.e the matrix A is the square root of the inverse of the +// covariance, also known as the stiffness matrix. There are however +// no restrictions on the shape of A. It is free to be rectangular, +// which would be the case if the covariance matrix S is rank +// deficient. + +class CERES_EXPORT NormalPrior final : public CostFunction { + public: + // Check that the number of rows in the vector b are the same as the + // number of columns in the matrix A, crash otherwise. + NormalPrior(const Matrix& A, const Vector& b); + bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const override; + + private: + Matrix A_; + Vector b_; +}; + +} // namespace ceres + +#include "ceres/internal/reenable_warnings.h" + +#endif // CERES_PUBLIC_NORMAL_PRIOR_H_ diff --git a/include/ceres/numeric_diff_cost_function.h b/include/ceres/numeric_diff_cost_function.h new file mode 100644 index 0000000000000000000000000000000000000000..6ec53175030c1618c88deb19a3acf5a6665480e1 --- /dev/null +++ b/include/ceres/numeric_diff_cost_function.h @@ -0,0 +1,260 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2019 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// sameeragarwal@google.com (Sameer Agarwal) +// +// Create CostFunctions as needed by the least squares framework with jacobians +// computed via numeric (a.k.a. finite) differentiation. For more details see +// http://en.wikipedia.org/wiki/Numerical_differentiation. +// +// To get an numerically differentiated cost function, you must define +// a class with a operator() (a functor) that computes the residuals. +// +// The function must write the computed value in the last argument +// (the only non-const one) and return true to indicate success. +// Please see cost_function.h for details on how the return value +// maybe used to impose simple constraints on the parameter block. +// +// For example, consider a scalar error e = k - x'y, where both x and y are +// two-dimensional column vector parameters, the prime sign indicates +// transposition, and k is a constant. The form of this error, which is the +// difference between a constant and an expression, is a common pattern in least +// squares problems. For example, the value x'y might be the model expectation +// for a series of measurements, where there is an instance of the cost function +// for each measurement k. +// +// The actual cost added to the total problem is e^2, or (k - x'k)^2; however, +// the squaring is implicitly done by the optimization framework. +// +// To write an numerically-differentiable cost function for the above model, +// first define the object +// +// class MyScalarCostFunctor { +// explicit MyScalarCostFunctor(double k): k_(k) {} +// +// bool operator()(const double* const x, +// const double* const y, +// double* residuals) const { +// residuals[0] = k_ - x[0] * y[0] - x[1] * y[1]; +// return true; +// } +// +// private: +// double k_; +// }; +// +// Note that in the declaration of operator() the input parameters x +// and y come first, and are passed as const pointers to arrays of +// doubles. If there were three input parameters, then the third input +// parameter would come after y. The output is always the last +// parameter, and is also a pointer to an array. In the example above, +// the residual is a scalar, so only residuals[0] is set. +// +// Then given this class definition, the numerically differentiated +// cost function with central differences used for computing the +// derivative can be constructed as follows. +// +// CostFunction* cost_function +// = new NumericDiffCostFunction( +// new MyScalarCostFunctor(1.0)); ^ ^ ^ ^ +// | | | | +// Finite Differencing Scheme -+ | | | +// Dimension of residual ------------+ | | +// Dimension of x ----------------------+ | +// Dimension of y -------------------------+ +// +// In this example, there is usually an instance for each measurement of k. +// +// In the instantiation above, the template parameters following +// "MyScalarCostFunctor", "1, 2, 2", describe the functor as computing +// a 1-dimensional output from two arguments, both 2-dimensional. +// +// NumericDiffCostFunction also supports cost functions with a +// runtime-determined number of residuals. For example: +// +// clang-format off +// +// CostFunction* cost_function +// = new NumericDiffCostFunction( +// new CostFunctorWithDynamicNumResiduals(1.0), ^ ^ ^ +// TAKE_OWNERSHIP, | | | +// runtime_number_of_residuals); <----+ | | | +// | | | | +// | | | | +// Actual number of residuals ------+ | | | +// Indicate dynamic number of residuals --------------------+ | | +// Dimension of x ------------------------------------------------+ | +// Dimension of y ---------------------------------------------------+ +// clang-format on +// +// +// The central difference method is considerably more accurate at the cost of +// twice as many function evaluations than forward difference. Consider using +// central differences begin with, and only after that works, trying forward +// difference to improve performance. +// +// WARNING #1: A common beginner's error when first using +// NumericDiffCostFunction is to get the sizing wrong. In particular, +// there is a tendency to set the template parameters to (dimension of +// residual, number of parameters) instead of passing a dimension +// parameter for *every parameter*. In the example above, that would +// be , which is missing the last '2' +// argument. Please be careful when setting the size parameters. +// +//////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////// +// +// ALTERNATE INTERFACE +// +// For a variety of reasons, including compatibility with legacy code, +// NumericDiffCostFunction can also take CostFunction objects as +// input. The following describes how. +// +// To get a numerically differentiated cost function, define a +// subclass of CostFunction such that the Evaluate() function ignores +// the jacobian parameter. The numeric differentiation wrapper will +// fill in the jacobian parameter if necessary by repeatedly calling +// the Evaluate() function with small changes to the appropriate +// parameters, and computing the slope. For performance, the numeric +// differentiation wrapper class is templated on the concrete cost +// function, even though it could be implemented only in terms of the +// virtual CostFunction interface. +// +// The numerically differentiated version of a cost function for a cost function +// can be constructed as follows: +// +// CostFunction* cost_function +// = new NumericDiffCostFunction( +// new MyCostFunction(...), TAKE_OWNERSHIP); +// +// where MyCostFunction has 1 residual and 2 parameter blocks with sizes 4 and 8 +// respectively. Look at the tests for a more detailed example. +// +// TODO(keir): Characterize accuracy; mention pitfalls; provide alternatives. + +#ifndef CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_ +#define CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_ + +#include +#include + +#include "Eigen/Dense" +#include "ceres/cost_function.h" +#include "ceres/internal/numeric_diff.h" +#include "ceres/internal/parameter_dims.h" +#include "ceres/numeric_diff_options.h" +#include "ceres/sized_cost_function.h" +#include "ceres/types.h" +#include "glog/logging.h" + +namespace ceres { + +template // Parameters dimensions for each block. +class NumericDiffCostFunction final + : public SizedCostFunction { + public: + explicit NumericDiffCostFunction( + CostFunctor* functor, + Ownership ownership = TAKE_OWNERSHIP, + int num_residuals = kNumResiduals, + const NumericDiffOptions& options = NumericDiffOptions()) + : functor_(functor), ownership_(ownership), options_(options) { + if (kNumResiduals == DYNAMIC) { + SizedCostFunction::set_num_residuals(num_residuals); + } + } + + NumericDiffCostFunction(NumericDiffCostFunction&& other) + : functor_(std::move(other.functor_)), ownership_(other.ownership_) {} + + virtual ~NumericDiffCostFunction() { + if (ownership_ != TAKE_OWNERSHIP) { + functor_.release(); + } + } + + bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const override { + using internal::FixedArray; + using internal::NumericDiff; + + using ParameterDims = + typename SizedCostFunction::ParameterDims; + + constexpr int kNumParameters = ParameterDims::kNumParameters; + constexpr int kNumParameterBlocks = ParameterDims::kNumParameterBlocks; + + // Get the function value (residuals) at the the point to evaluate. + if (!internal::VariadicEvaluate( + *functor_, parameters, residuals)) { + return false; + } + + if (jacobians == nullptr) { + return true; + } + + // Create a copy of the parameters which will get mutated. + FixedArray parameters_copy(kNumParameters); + std::array parameters_reference_copy = + ParameterDims::GetUnpackedParameters(parameters_copy.data()); + + for (int block = 0; block < kNumParameterBlocks; ++block) { + memcpy(parameters_reference_copy[block], + parameters[block], + sizeof(double) * ParameterDims::GetDim(block)); + } + + internal::EvaluateJacobianForParameterBlocks:: + template Apply( + functor_.get(), + residuals, + options_, + SizedCostFunction::num_residuals(), + parameters_reference_copy.data(), + jacobians); + + return true; + } + + const CostFunctor& functor() const { return *functor_; } + + private: + std::unique_ptr functor_; + Ownership ownership_; + NumericDiffOptions options_; +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_ diff --git a/include/ceres/numeric_diff_first_order_function.h b/include/ceres/numeric_diff_first_order_function.h new file mode 100644 index 0000000000000000000000000000000000000000..f5bb005be582a23328bb0b397bfbff27a2136998 --- /dev/null +++ b/include/ceres/numeric_diff_first_order_function.h @@ -0,0 +1,163 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2019 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_PUBLIC_NUMERIC_DIFF_FIRST_ORDER_FUNCTION_H_ +#define CERES_PUBLIC_NUMERIC_DIFF_FIRST_ORDER_FUNCTION_H_ + +#include +#include + +#include "ceres/first_order_function.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/fixed_array.h" +#include "ceres/internal/numeric_diff.h" +#include "ceres/internal/parameter_dims.h" +#include "ceres/internal/variadic_evaluate.h" +#include "ceres/numeric_diff_options.h" +#include "ceres/types.h" + +namespace ceres { + +// Creates FirstOrderFunctions as needed by the GradientProblem +// framework, with gradients computed via numeric differentiation. For +// more information on numeric differentiation, see the wikipedia +// article at https://en.wikipedia.org/wiki/Numerical_differentiation +// +// To get an numerically differentiated cost function, you must define +// a class with an operator() (a functor) that computes the cost. +// +// The function must write the computed value in the last argument +// (the only non-const one) and return true to indicate success. +// +// For example, consider a scalar error e = x'y - a, where both x and y are +// two-dimensional column vector parameters, the prime sign indicates +// transposition, and a is a constant. +// +// To write an numerically-differentiable cost function for the above model, +// first define the object +// +// class QuadraticCostFunctor { +// public: +// explicit QuadraticCostFunctor(double a) : a_(a) {} +// bool operator()(const double* const xy, double* cost) const { +// constexpr int kInputVectorLength = 2; +// const double* const x = xy; +// const double* const y = xy + kInputVectorLength; +// *cost = x[0] * y[0] + x[1] * y[1] - a_; +// return true; +// } +// +// private: +// double a_; +// }; +// +// +// Note that in the declaration of operator() the input parameters xy +// come first, and are passed as const pointers to array of +// doubles. The output cost is the last parameter. +// +// Then given this class definition, the numerically differentiated +// first order function with central differences used for computing the +// derivative can be constructed as follows. +// +// FirstOrderFunction* function +// = new NumericDiffFirstOrderFunction( +// new QuadraticCostFunctor(1.0)); ^ ^ ^ +// | | | +// Finite Differencing Scheme -+ | | +// Dimension of xy ------------------------+ +// +// +// In the instantiation above, the template parameters following +// "QuadraticCostFunctor", "CENTRAL, 4", describe the finite +// differencing scheme as "central differencing" and the functor as +// computing its cost from a 4 dimensional input. +template +class NumericDiffFirstOrderFunction final : public FirstOrderFunction { + public: + explicit NumericDiffFirstOrderFunction( + FirstOrderFunctor* functor, + Ownership ownership = TAKE_OWNERSHIP, + const NumericDiffOptions& options = NumericDiffOptions()) + : functor_(functor), ownership_(ownership), options_(options) { + static_assert(kNumParameters > 0, "kNumParameters must be positive"); + } + + ~NumericDiffFirstOrderFunction() override { + if (ownership_ != TAKE_OWNERSHIP) { + functor_.release(); + } + } + + bool Evaluate(const double* const parameters, + double* cost, + double* gradient) const override { + using ParameterDims = internal::StaticParameterDims; + constexpr int kNumResiduals = 1; + + // Get the function value (cost) at the the point to evaluate. + if (!internal::VariadicEvaluate( + *functor_, ¶meters, cost)) { + return false; + } + + if (gradient == nullptr) { + return true; + } + + // Create a copy of the parameters which will get mutated. + internal::FixedArray parameters_copy(kNumParameters); + std::copy_n(parameters, kNumParameters, parameters_copy.data()); + double* parameters_ptr = parameters_copy.data(); + internal::EvaluateJacobianForParameterBlocks< + ParameterDims>::template Apply(functor_.get(), + cost, + options_, + kNumResiduals, + ¶meters_ptr, + &gradient); + return true; + } + + int NumParameters() const override { return kNumParameters; } + + const FirstOrderFunctor& functor() const { return *functor_; } + + private: + std::unique_ptr functor_; + Ownership ownership_; + NumericDiffOptions options_; +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_NUMERIC_DIFF_FIRST_ORDER_FUNCTION_H_ diff --git a/include/ceres/numeric_diff_options.h b/include/ceres/numeric_diff_options.h new file mode 100644 index 0000000000000000000000000000000000000000..b025b51d938d6220e5f44c5725d2acc0f7dc9c4b --- /dev/null +++ b/include/ceres/numeric_diff_options.h @@ -0,0 +1,76 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2019 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: tbennun@gmail.com (Tal Ben-Nun) +// + +#ifndef CERES_PUBLIC_NUMERIC_DIFF_OPTIONS_H_ +#define CERES_PUBLIC_NUMERIC_DIFF_OPTIONS_H_ + +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" + +namespace ceres { + +// Options pertaining to numeric differentiation (e.g., convergence criteria, +// step sizes). +struct CERES_EXPORT NumericDiffOptions { + // Numeric differentiation step size (multiplied by parameter block's + // order of magnitude). If parameters are close to zero, the step size + // is set to sqrt(machine_epsilon). + double relative_step_size = 1e-6; + + // Initial step size for Ridders adaptive numeric differentiation (multiplied + // by parameter block's order of magnitude). + // If parameters are close to zero, Ridders' method sets the step size + // directly to this value. This parameter is separate from + // "relative_step_size" in order to set a different default value. + // + // Note: For Ridders' method to converge, the step size should be initialized + // to a value that is large enough to produce a significant change in the + // function. As the derivative is estimated, the step size decreases. + double ridders_relative_initial_step_size = 1e-2; + + // Maximal number of adaptive extrapolations (sampling) in Ridders' method. + int max_num_ridders_extrapolations = 10; + + // Convergence criterion on extrapolation error for Ridders adaptive + // differentiation. The available error estimation methods are defined in + // NumericDiffErrorType and set in the "ridders_error_method" field. + double ridders_epsilon = 1e-12; + + // The factor in which to shrink the step size with each extrapolation in + // Ridders' method. + double ridders_step_shrink_factor = 2.0; +}; + +} // namespace ceres + +#include "ceres/internal/reenable_warnings.h" + +#endif // CERES_PUBLIC_NUMERIC_DIFF_OPTIONS_H_ diff --git a/include/ceres/ordered_groups.h b/include/ceres/ordered_groups.h new file mode 100644 index 0000000000000000000000000000000000000000..c1531cce65f79dc9a84b874f664ea37292c285f1 --- /dev/null +++ b/include/ceres/ordered_groups.h @@ -0,0 +1,197 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2019 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_PUBLIC_ORDERED_GROUPS_H_ +#define CERES_PUBLIC_ORDERED_GROUPS_H_ + +#include +#include +#include +#include + +#include "ceres/internal/export.h" +#include "glog/logging.h" + +namespace ceres { + +// A class for storing and manipulating an ordered collection of +// groups/sets with the following semantics: +// +// Group ids are non-negative integer values. Elements are any type +// that can serve as a key in a map or an element of a set. +// +// An element can only belong to one group at a time. A group may +// contain an arbitrary number of elements. +// +// Groups are ordered by their group id. +template +class OrderedGroups { + public: + // Add an element to a group. If a group with this id does not + // exist, one is created. This method can be called any number of + // times for the same element. Group ids should be non-negative + // numbers. + // + // Return value indicates if adding the element was a success. + bool AddElementToGroup(const T element, const int group) { + if (group < 0) { + return false; + } + + auto it = element_to_group_.find(element); + if (it != element_to_group_.end()) { + if (it->second == group) { + // Element is already in the right group, nothing to do. + return true; + } + + group_to_elements_[it->second].erase(element); + if (group_to_elements_[it->second].size() == 0) { + group_to_elements_.erase(it->second); + } + } + + element_to_group_[element] = group; + group_to_elements_[group].insert(element); + return true; + } + + void Clear() { + group_to_elements_.clear(); + element_to_group_.clear(); + } + + // Remove the element, no matter what group it is in. Return value + // indicates if the element was actually removed. + bool Remove(const T element) { + const int current_group = GroupId(element); + if (current_group < 0) { + return false; + } + + group_to_elements_[current_group].erase(element); + + if (group_to_elements_[current_group].size() == 0) { + // If the group is empty, then get rid of it. + group_to_elements_.erase(current_group); + } + + element_to_group_.erase(element); + return true; + } + + // Bulk remove elements. The return value indicates the number of + // elements successfully removed. + int Remove(const std::vector& elements) { + if (NumElements() == 0 || elements.size() == 0) { + return 0; + } + + int num_removed = 0; + for (int i = 0; i < elements.size(); ++i) { + num_removed += Remove(elements[i]); + } + return num_removed; + } + + // Reverse the order of the groups in place. + void Reverse() { + if (NumGroups() == 0) { + return; + } + + auto it = group_to_elements_.rbegin(); + std::map> new_group_to_elements; + new_group_to_elements[it->first] = it->second; + + int new_group_id = it->first + 1; + for (++it; it != group_to_elements_.rend(); ++it) { + for (const auto& element : it->second) { + element_to_group_[element] = new_group_id; + } + new_group_to_elements[new_group_id] = it->second; + new_group_id++; + } + + group_to_elements_.swap(new_group_to_elements); + } + + // Return the group id for the element. If the element is not a + // member of any group, return -1. + int GroupId(const T element) const { + auto it = element_to_group_.find(element); + if (it == element_to_group_.end()) { + return -1; + } + return it->second; + } + + bool IsMember(const T element) const { + auto it = element_to_group_.find(element); + return (it != element_to_group_.end()); + } + + // This function always succeeds, i.e., implicitly there exists a + // group for every integer. + int GroupSize(const int group) const { + auto it = group_to_elements_.find(group); + return (it == group_to_elements_.end()) ? 0 : it->second.size(); + } + + int NumElements() const { return element_to_group_.size(); } + + // Number of groups with one or more elements. + int NumGroups() const { return group_to_elements_.size(); } + + // The first group with one or more elements. Calling this when + // there are no groups with non-zero elements will result in a + // crash. + int MinNonZeroGroup() const { + CHECK_NE(NumGroups(), 0); + return group_to_elements_.begin()->first; + } + + const std::map>& group_to_elements() const { + return group_to_elements_; + } + + const std::map& element_to_group() const { return element_to_group_; } + + private: + std::map> group_to_elements_; + std::unordered_map element_to_group_; +}; + +// Typedef for the most commonly used version of OrderedGroups. +using ParameterBlockOrdering = OrderedGroups; + +} // namespace ceres + +#endif // CERES_PUBLIC_ORDERED_GROUP_H_ diff --git a/include/ceres/problem.h b/include/ceres/problem.h new file mode 100644 index 0000000000000000000000000000000000000000..819fa454b212a336fd02d843e336751b1f3f1133 --- /dev/null +++ b/include/ceres/problem.h @@ -0,0 +1,685 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2021 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// keir@google.com (Keir Mierle) +// +// The Problem object is used to build and hold least squares problems. + +#ifndef CERES_PUBLIC_PROBLEM_H_ +#define CERES_PUBLIC_PROBLEM_H_ + +#include +#include +#include +#include +#include +#include + +#include "ceres/context.h" +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" +#include "ceres/internal/port.h" +#include "ceres/types.h" +#include "glog/logging.h" + +namespace ceres { + +class CostFunction; +class EvaluationCallback; +class LossFunction; +class LocalParameterization; +class Manifold; +class Solver; +struct CRSMatrix; + +namespace internal { +class Preprocessor; +class ProblemImpl; +class ParameterBlock; +class ResidualBlock; +} // namespace internal + +// A ResidualBlockId is an opaque handle clients can use to remove residual +// blocks from a Problem after adding them. +using ResidualBlockId = internal::ResidualBlock*; + +// A class to represent non-linear least squares problems. Such +// problems have a cost function that is a sum of error terms (known +// as "residuals"), where each residual is a function of some subset +// of the parameters. The cost function takes the form +// +// N 1 +// SUM --- loss( || r_i1, r_i2,..., r_ik ||^2 ), +// i=1 2 +// +// where +// +// r_ij is residual number i, component j; the residual is a function of some +// subset of the parameters x1...xk. For example, in a structure from +// motion problem a residual might be the difference between a measured +// point in an image and the reprojected position for the matching +// camera, point pair. The residual would have two components, error in x +// and error in y. +// +// loss(y) is the loss function; for example, squared error or Huber L1 +// loss. If loss(y) = y, then the cost function is non-robustified +// least squares. +// +// This class is specifically designed to address the important subset of +// "sparse" least squares problems, where each component of the residual depends +// only on a small number number of parameters, even though the total number of +// residuals and parameters may be very large. This property affords tremendous +// gains in scale, allowing efficient solving of large problems that are +// otherwise inaccessible. +// +// The canonical example of a sparse least squares problem is +// "structure-from-motion" (SFM), where the parameters are points and cameras, +// and residuals are reprojection errors. Typically a single residual will +// depend only on 9 parameters (3 for the point, 6 for the camera). +// +// To create a least squares problem, use the AddResidualBlock() and +// AddParameterBlock() methods, documented below. Here is an example least +// squares problem containing 3 parameter blocks of sizes 3, 4 and 5 +// respectively and two residual terms of size 2 and 6: +// +// double x1[] = { 1.0, 2.0, 3.0 }; +// double x2[] = { 1.0, 2.0, 3.0, 5.0 }; +// double x3[] = { 1.0, 2.0, 3.0, 6.0, 7.0 }; +// +// Problem problem; +// +// problem.AddResidualBlock(new MyUnaryCostFunction(...), nullptr, x1); +// problem.AddResidualBlock(new MyBinaryCostFunction(...), nullptr, x2, x3); +// +// Please see cost_function.h for details of the CostFunction object. +// +// NOTE: We are currently in the process of transitioning from +// LocalParameterization to Manifolds in the Ceres API. During this period, +// Problem will support using both Manifold and LocalParameterization objects +// interchangably. In particular, adding a LocalParameterization to a parameter +// block is the same as adding a Manifold to that parameter block. For methods +// in the API affected by this change, see their documentation below. +class CERES_EXPORT Problem { + public: + struct CERES_EXPORT Options { + // These flags control whether the Problem object owns the CostFunctions, + // LossFunctions, LocalParameterizations, and Manifolds passed into the + // Problem. + // + // If set to TAKE_OWNERSHIP, then the problem object will delete the + // corresponding object on destruction. The destructor is careful to delete + // the pointers only once, since sharing objects is allowed. + Ownership cost_function_ownership = TAKE_OWNERSHIP; + Ownership loss_function_ownership = TAKE_OWNERSHIP; + CERES_DEPRECATED_WITH_MSG( + "Local Parameterizations are deprecated. Use Manifold and " + "manifold_ownership instead.") + Ownership local_parameterization_ownership = TAKE_OWNERSHIP; + Ownership manifold_ownership = TAKE_OWNERSHIP; + + // If true, trades memory for faster RemoveResidualBlock() and + // RemoveParameterBlock() operations. + // + // By default, RemoveParameterBlock() and RemoveResidualBlock() take time + // proportional to the size of the entire problem. If you only ever remove + // parameters or residuals from the problem occasionally, this might be + // acceptable. However, if you have memory to spare, enable this option to + // make RemoveParameterBlock() take time proportional to the number of + // residual blocks that depend on it, and RemoveResidualBlock() take (on + // average) constant time. + // + // The increase in memory usage is two-fold: an additional hash set per + // parameter block containing all the residuals that depend on the parameter + // block; and a hash set in the problem containing all residuals. + bool enable_fast_removal = false; + + // By default, Ceres performs a variety of safety checks when constructing + // the problem. There is a small but measurable performance penalty to these + // checks, typically around 5% of construction time. If you are sure your + // problem construction is correct, and 5% of the problem construction time + // is truly an overhead you want to avoid, then you can set + // disable_all_safety_checks to true. + // + // WARNING: Do not set this to true, unless you are absolutely sure of what + // you are doing. + bool disable_all_safety_checks = false; + + // A Ceres global context to use for solving this problem. This may help to + // reduce computation time as Ceres can reuse expensive objects to create. + // The context object can be nullptr, in which case Ceres may create one. + // + // Ceres does NOT take ownership of the pointer. + Context* context = nullptr; + + // Using this callback interface, Ceres can notify you when it is about to + // evaluate the residuals or jacobians. With the callback, you can share + // computation between residual blocks by doing the shared computation in + // EvaluationCallback::PrepareForEvaluation() before Ceres calls + // CostFunction::Evaluate(). It also enables caching results between a pure + // residual evaluation and a residual & jacobian evaluation. + // + // Problem DOES NOT take ownership of the callback. + // + // NOTE: Evaluation callbacks are incompatible with inner iterations. So + // calling Solve with Solver::Options::use_inner_iterations = true on a + // Problem with a non-null evaluation callback is an error. + EvaluationCallback* evaluation_callback = nullptr; + }; + + // The default constructor is equivalent to the invocation + // Problem(Problem::Options()). + Problem(); + explicit Problem(const Options& options); + Problem(Problem&&); + Problem& operator=(Problem&&); + + Problem(const Problem&) = delete; + Problem& operator=(const Problem&) = delete; + + ~Problem(); + + // Add a residual block to the overall cost function. The cost function + // carries with its information about the sizes of the parameter blocks it + // expects. The function checks that these match the sizes of the parameter + // blocks listed in parameter_blocks. The program aborts if a mismatch is + // detected. loss_function can be nullptr, in which case the cost of the term + // is just the squared norm of the residuals. + // + // The user has the option of explicitly adding the parameter blocks using + // AddParameterBlock. This causes additional correctness checking; however, + // AddResidualBlock implicitly adds the parameter blocks if they are not + // present, so calling AddParameterBlock explicitly is not required. + // + // The Problem object by default takes ownership of the cost_function and + // loss_function pointers (See Problem::Options to override this behaviour). + // These objects remain live for the life of the Problem object. If the user + // wishes to keep control over the destruction of these objects, then they can + // do this by setting the corresponding enums in the Options struct. + // + // Note: Even though the Problem takes ownership of cost_function and + // loss_function, it does not preclude the user from re-using them in another + // residual block. The destructor takes care to call delete on each + // cost_function or loss_function pointer only once, regardless of how many + // residual blocks refer to them. + // + // Example usage: + // + // double x1[] = {1.0, 2.0, 3.0}; + // double x2[] = {1.0, 2.0, 5.0, 6.0}; + // double x3[] = {3.0, 6.0, 2.0, 5.0, 1.0}; + // + // Problem problem; + // + // problem.AddResidualBlock(new MyUnaryCostFunction(...), nullptr, x1); + // problem.AddResidualBlock(new MyBinaryCostFunction(...), nullptr, x2, x1); + // + // Add a residual block by listing the parameter block pointers directly + // instead of wapping them in a container. + template + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0, + Ts*... xs) { + const std::array parameter_blocks{{x0, xs...}}; + return AddResidualBlock(cost_function, + loss_function, + parameter_blocks.data(), + static_cast(parameter_blocks.size())); + } + + // Add a residual block by providing a vector of parameter blocks. + ResidualBlockId AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + const std::vector& parameter_blocks); + + // Add a residual block by providing a pointer to the parameter block array + // and the number of parameter blocks. + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* const* const parameter_blocks, + int num_parameter_blocks); + + // Add a parameter block with appropriate size to the problem. Repeated calls + // with the same arguments are ignored. Repeated calls with the same double + // pointer but a different size will result in a crash. + void AddParameterBlock(double* values, int size); + + // Add a parameter block with appropriate size and parameterization to the + // problem. It is okay for local_parameterization to be nullptr. + // + // Repeated calls with the same arguments are ignored. Repeated calls + // with the same double pointer but a different size results in a crash + // (unless Solver::Options::diable_all_safety_checks is set to true). + // + // Repeated calls with the same double pointer and size but different + // LocalParameterization is equivalent to calling + // SetParameterization(local_parameterization), i.e., any previously + // associated LocalParameterization or Manifold object will be replaced with + // the local_parameterization. + // + // NOTE: + // ---- + // + // This method is deprecated and will be removed in the next public + // release of Ceres Solver. Please move to using the Manifold based version of + // AddParameterBlock. + // + // During the transition from LocalParameterization to Manifold, internally + // the LocalParameterization is treated as a Manifold by wrapping it using a + // ManifoldAdapter object. So HasManifold() will return true, GetManifold() + // will return the wrapped object and ParameterBlockTangentSize() will return + // the LocalSize of the LocalParameterization. + CERES_DEPRECATED_WITH_MSG( + "LocalParameterizations are deprecated. Use the version with Manifolds " + "instead.") + void AddParameterBlock(double* values, + int size, + LocalParameterization* local_parameterization); + + // Add a parameter block with appropriate size and Manifold to the + // problem. It is okay for manifold to be nullptr. + // + // Repeated calls with the same arguments are ignored. Repeated calls + // with the same double pointer but a different size results in a crash + // (unless Solver::Options::diable_all_safety_checks is set to true). + // + // Repeated calls with the same double pointer and size but different Manifold + // is equivalent to calling SetManifold(manifold), i.e., any previously + // associated LocalParameterization or Manifold object will be replaced with + // the manifold. + // + // Note: + // ---- + // + // During the transition from LocalParameterization to Manifold, calling + // AddParameterBlock with a Manifold when a LocalParameterization is already + // associated with the parameter block is okay. It is equivalent to calling + // SetManifold(manifold), i.e., any previously associated + // LocalParameterization or Manifold object will be replaced with the + // manifold. + void AddParameterBlock(double* values, int size, Manifold* manifold); + + // Remove a parameter block from the problem. The LocalParameterization or + // Manifold of the parameter block, if it exists, will persist until the + // deletion of the problem (similar to cost/loss functions in residual block + // removal). Any residual blocks that depend on the parameter are also + // removed, as described above in RemoveResidualBlock(). + // + // If Problem::Options::enable_fast_removal is true, then the removal is fast + // (almost constant time). Otherwise, removing a parameter block will incur a + // scan of the entire Problem object. + // + // WARNING: Removing a residual or parameter block will destroy the implicit + // ordering, rendering the jacobian or residuals returned from the solver + // uninterpretable. If you depend on the evaluated jacobian, do not use + // remove! This may change in a future release. + void RemoveParameterBlock(const double* values); + + // Remove a residual block from the problem. Any parameters that the residual + // block depends on are not removed. The cost and loss functions for the + // residual block will not get deleted immediately; won't happen until the + // problem itself is deleted. + // + // WARNING: Removing a residual or parameter block will destroy the implicit + // ordering, rendering the jacobian or residuals returned from the solver + // uninterpretable. If you depend on the evaluated jacobian, do not use + // remove! This may change in a future release. + void RemoveResidualBlock(ResidualBlockId residual_block); + + // Hold the indicated parameter block constant during optimization. + void SetParameterBlockConstant(const double* values); + + // Allow the indicated parameter block to vary during optimization. + void SetParameterBlockVariable(double* values); + + // Returns true if a parameter block is set constant, and false otherwise. A + // parameter block may be set constant in two ways: either by calling + // SetParameterBlockConstant or by associating a LocalParameterization or + // Manifold with a zero dimensional tangent space with it. + bool IsParameterBlockConstant(const double* values) const; + + // Set the LocalParameterization for the parameter block. Calling + // SetParameterization with nullptr will clear any previously set + // LocalParameterization or Manifold for the parameter block. + // + // Repeated calls will cause any previously associated LocalParameterization + // or Manifold object to be replaced with the local_parameterization. + // + // The local_parameterization is owned by the Problem by default (See + // Problem::Options to override this behaviour). + // + // It is acceptable to set the same LocalParameterization for multiple + // parameter blocks; the destructor is careful to delete + // LocalParamaterizations only once. + // + // NOTE: + // ---- + // + // This method is deprecated and will be removed in the next public + // release of Ceres Solver. Please move to using the SetManifold instead. + // + // During the transition from LocalParameterization to Manifold, internally + // the LocalParameterization is treated as a Manifold by wrapping it using a + // ManifoldAdapter object. So HasManifold() will return true, GetManifold() + // will return the wrapped object and ParameterBlockTangentSize will return + // the same value of ParameterBlockLocalSize. + CERES_DEPRECATED_WITH_MSG( + "LocalParameterizations are deprecated. Use SetManifold instead.") + void SetParameterization(double* values, + LocalParameterization* local_parameterization); + + // Get the LocalParameterization object associated with this parameter block. + // If there is no LocalParameterization associated then nullptr is returned. + // + // NOTE: This method is deprecated and will be removed in the next public + // release of Ceres Solver. Use GetManifold instead. + // + // Note also that if a LocalParameterization is associated with a parameter + // block, HasManifold will return true and GetManifold will return the + // LocalParameterization wrapped in a ManifoldAdapter. + // + // The converse is NOT true, i.e., if a Manifold is associated with a + // parameter block, HasParameterization will return false and + // GetParameterization will return a nullptr. + CERES_DEPRECATED_WITH_MSG( + "LocalParameterizations are deprecated. Use GetManifold " + "instead.") + const LocalParameterization* GetParameterization(const double* values) const; + + // Returns true if a LocalParameterization is associated with this parameter + // block, false otherwise. + // + // NOTE: This method is deprecated and will be removed in the next public + // release of Ceres Solver. Use HasManifold instead. + // + // Note also that if a Manifold is associated with the parameter block, this + // method will return false. + CERES_DEPRECATED_WITH_MSG( + "LocalParameterizations are deprecated. Use HasManifold instead.") + bool HasParameterization(const double* values) const; + + // Set the Manifold for the parameter block. Calling SetManifold with nullptr + // will clear any previously set LocalParameterization or Manifold for the + // parameter block. + // + // Repeated calls will result in any previously associated + // LocalParameterization or Manifold object to be replaced with the manifold. + // + // The manifold is owned by the Problem by default (See Problem::Options to + // override this behaviour). + // + // It is acceptable to set the same Manifold for multiple parameter blocks. + void SetManifold(double* values, Manifold* manifold); + + // Get the Manifold object associated with this parameter block. + // + // If there is no Manifold Or LocalParameterization object associated then + // nullptr is returned. + // + // NOTE: During the transition from LocalParameterization to Manifold, + // internally the LocalParameterization is treated as a Manifold by wrapping + // it using a ManifoldAdapter object. So calling GetManifold on a parameter + // block with a LocalParameterization associated with it will return the + // LocalParameterization wrapped in a ManifoldAdapter + const Manifold* GetManifold(const double* values) const; + + // Returns true if a Manifold or a LocalParameterization is associated with + // this parameter block, false otherwise. + bool HasManifold(const double* values) const; + + // Set the lower/upper bound for the parameter at position "index". + void SetParameterLowerBound(double* values, int index, double lower_bound); + void SetParameterUpperBound(double* values, int index, double upper_bound); + + // Get the lower/upper bound for the parameter at position "index". If the + // parameter is not bounded by the user, then its lower bound is + // -std::numeric_limits::max() and upper bound is + // std::numeric_limits::max(). + double GetParameterLowerBound(const double* values, int index) const; + double GetParameterUpperBound(const double* values, int index) const; + + // Number of parameter blocks in the problem. Always equals + // parameter_blocks().size() and parameter_block_sizes().size(). + int NumParameterBlocks() const; + + // The size of the parameter vector obtained by summing over the sizes of all + // the parameter blocks. + int NumParameters() const; + + // Number of residual blocks in the problem. Always equals + // residual_blocks().size(). + int NumResidualBlocks() const; + + // The size of the residual vector obtained by summing over the sizes of all + // of the residual blocks. + int NumResiduals() const; + + // The size of the parameter block. + int ParameterBlockSize(const double* values) const; + + // The dimension of the tangent space of the LocalParameterization or Manifold + // for the parameter block. If there is no LocalParameterization or Manifold + // associated with this parameter block, then ParameterBlockLocalSize = + // ParameterBlockSize. + CERES_DEPRECATED_WITH_MSG( + "LocalParameterizations are deprecated. Use ParameterBlockTangentSize " + "instead.") + int ParameterBlockLocalSize(const double* values) const; + + // The dimenion of the tangent space of the LocalParameterization or Manifold + // for the parameter block. If there is no LocalParameterization or Manifold + // associated with this parameter block, then ParameterBlockTangentSize = + // ParameterBlockSize. + int ParameterBlockTangentSize(const double* values) const; + + // Is the given parameter block present in this problem or not? + bool HasParameterBlock(const double* values) const; + + // Fills the passed parameter_blocks vector with pointers to the parameter + // blocks currently in the problem. After this call, parameter_block.size() == + // NumParameterBlocks. + void GetParameterBlocks(std::vector* parameter_blocks) const; + + // Fills the passed residual_blocks vector with pointers to the residual + // blocks currently in the problem. After this call, residual_blocks.size() == + // NumResidualBlocks. + void GetResidualBlocks(std::vector* residual_blocks) const; + + // Get all the parameter blocks that depend on the given residual block. + void GetParameterBlocksForResidualBlock( + const ResidualBlockId residual_block, + std::vector* parameter_blocks) const; + + // Get the CostFunction for the given residual block. + const CostFunction* GetCostFunctionForResidualBlock( + const ResidualBlockId residual_block) const; + + // Get the LossFunction for the given residual block. Returns nullptr + // if no loss function is associated with this residual block. + const LossFunction* GetLossFunctionForResidualBlock( + const ResidualBlockId residual_block) const; + + // Get all the residual blocks that depend on the given parameter block. + // + // If Problem::Options::enable_fast_removal is true, then getting the residual + // blocks is fast and depends only on the number of residual + // blocks. Otherwise, getting the residual blocks for a parameter block will + // incur a scan of the entire Problem object. + void GetResidualBlocksForParameterBlock( + const double* values, + std::vector* residual_blocks) const; + + // Options struct to control Problem::Evaluate. + struct EvaluateOptions { + // The set of parameter blocks for which evaluation should be + // performed. This vector determines the order that parameter blocks occur + // in the gradient vector and in the columns of the jacobian matrix. If + // parameter_blocks is empty, then it is assumed to be equal to vector + // containing ALL the parameter blocks. Generally speaking the parameter + // blocks will occur in the order in which they were added to the + // problem. But, this may change if the user removes any parameter blocks + // from the problem. + // + // NOTE: This vector should contain the same pointers as the ones used to + // add parameter blocks to the Problem. These parameter block should NOT + // point to new memory locations. Bad things will happen otherwise. + std::vector parameter_blocks; + + // The set of residual blocks to evaluate. This vector determines the order + // in which the residuals occur, and how the rows of the jacobian are + // ordered. If residual_blocks is empty, then it is assumed to be equal to + // the vector containing ALL the residual blocks. Generally speaking the + // residual blocks will occur in the order in which they were added to the + // problem. But, this may change if the user removes any residual blocks + // from the problem. + std::vector residual_blocks; + + // Even though the residual blocks in the problem may contain loss + // functions, setting apply_loss_function to false will turn off the + // application of the loss function to the output of the cost function. This + // is of use for example if the user wishes to analyse the solution quality + // by studying the distribution of residuals before and after the solve. + bool apply_loss_function = true; + + int num_threads = 1; + }; + + // Evaluate Problem. Any of the output pointers can be nullptr. Which residual + // blocks and parameter blocks are used is controlled by the EvaluateOptions + // struct above. + // + // Note 1: The evaluation will use the values stored in the memory locations + // pointed to by the parameter block pointers used at the time of the + // construction of the problem. i.e., + // + // Problem problem; + // double x = 1; + // problem.AddResidualBlock(new MyCostFunction, nullptr, &x); + // + // double cost = 0.0; + // problem.Evaluate(Problem::EvaluateOptions(), &cost, + // nullptr, nullptr, nullptr); + // + // The cost is evaluated at x = 1. If you wish to evaluate the problem at x = + // 2, then + // + // x = 2; + // problem.Evaluate(Problem::EvaluateOptions(), &cost, + // nullptr, nullptr, nullptr); + // + // is the way to do so. + // + // Note 2: If no LocalParameterizations or Manifolds are used, then the size + // of the gradient vector (and the number of columns in the jacobian) is the + // sum of the sizes of all the parameter blocks. If a parameter block has a + // LocalParameterization or Manifold, then it contributes "TangentSize" + // entries to the gradient vector (and the number of columns in the jacobian). + // + // Note 3: This function cannot be called while the problem is being solved, + // for example it cannot be called from an IterationCallback at the end of an + // iteration during a solve. + // + // Note 4: If an EvaluationCallback is associated with the problem, then its + // PrepareForEvaluation method will be called every time this method is called + // with new_point = true. + bool Evaluate(const EvaluateOptions& options, + double* cost, + std::vector* residuals, + std::vector* gradient, + CRSMatrix* jacobian); + + // Evaluates the residual block, storing the scalar cost in *cost, the + // residual components in *residuals, and the jacobians between the parameters + // and residuals in jacobians[i], in row-major order. + // + // If residuals is nullptr, the residuals are not computed. + // + // If jacobians is nullptr, no Jacobians are computed. If jacobians[i] is + // nullptr, then the Jacobian for that parameter block is not computed. + // + // It is not okay to request the Jacobian w.r.t a parameter block that is + // constant. + // + // The return value indicates the success or failure. Even if the function + // returns false, the caller should expect the output memory locations to have + // been modified. + // + // The returned cost and jacobians have had robustification and + // LocalParameterization/Manifold applied already; for example, the jacobian + // for a 4-dimensional quaternion parameter using the + // "QuaternionParameterization" is num_residuals by 3 instead of num_residuals + // by 4. + // + // apply_loss_function as the name implies allows the user to switch the + // application of the loss function on and off. + // + // If an EvaluationCallback is associated with the problem, then its + // PrepareForEvaluation method will be called every time this method is called + // with new_point = true. This conservatively assumes that the user may have + // changed the parameter values since the previous call to evaluate / solve. + // For improved efficiency, and only if you know that the parameter values + // have not changed between calls, see + // EvaluateResidualBlockAssumingParametersUnchanged(). + bool EvaluateResidualBlock(ResidualBlockId residual_block_id, + bool apply_loss_function, + double* cost, + double* residuals, + double** jacobians) const; + + // Same as EvaluateResidualBlock except that if an EvaluationCallback is + // associated with the problem, then its PrepareForEvaluation method will be + // called every time this method is called with new_point = false. + // + // This means, if an EvaluationCallback is associated with the problem then it + // is the user's responsibility to call PrepareForEvaluation before calling + // this method if necessary, i.e. iff the parameter values have been changed + // since the last call to evaluate / solve.' + // + // This is because, as the name implies, we assume that the parameter blocks + // did not change since the last time PrepareForEvaluation was called (via + // Solve, Evaluate or EvaluateResidualBlock). + bool EvaluateResidualBlockAssumingParametersUnchanged( + ResidualBlockId residual_block_id, + bool apply_loss_function, + double* cost, + double* residuals, + double** jacobians) const; + + private: + friend class Solver; + friend class Covariance; + std::unique_ptr impl_; +}; + +} // namespace ceres + +#include "ceres/internal/reenable_warnings.h" + +#endif // CERES_PUBLIC_PROBLEM_H_ diff --git a/include/ceres/product_manifold.h b/include/ceres/product_manifold.h new file mode 100644 index 0000000000000000000000000000000000000000..33f046da24e685e3cf50233250469a0cd141a388 --- /dev/null +++ b/include/ceres/product_manifold.h @@ -0,0 +1,328 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2022 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// sergiu.deitsch@gmail.com (Sergiu Deitsch) +// + +#ifndef CERES_PUBLIC_PRODUCT_MANIFOLD_H_ +#define CERES_PUBLIC_PRODUCT_MANIFOLD_H_ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ceres/internal/eigen.h" +#include "ceres/internal/fixed_array.h" +#include "ceres/internal/port.h" +#include "ceres/manifold.h" + +namespace ceres { + +// Construct a manifold by taking the Cartesian product of a number of other +// manifolds. This is useful, when a parameter block is the Cartesian product +// of two or more manifolds. For example the parameters of a camera consist of +// a rotation and a translation, i.e., SO(3) x R^3. +// +// Example usage: +// +// ProductManifold> se3; +// +// is the manifold for a rigid transformation, where the rotation is +// represented using a quaternion. +// +// Manifolds can be copied and moved to ProductManifold: +// +// SubsetManifold manifold1(5, {2}); +// SubsetManifold manifold2(3, {0, 1}); +// ProductManifold manifold(manifold1, +// manifold2); +// +// In advanced use cases, manifolds can be dynamically allocated and passed as +// (smart) pointers: +// +// ProductManifold, EuclideanManifold<3>> +// se3{std::make_unique(), EuclideanManifold<3>{}}; +// +// In C++17, the template parameters can be left out as they are automatically +// deduced making the initialization much simpler: +// +// ProductManifold se3{QuaternionManifold{}, EuclideanManifold<3>{}}; +// +// The manifold implementations must be either default constructible, copyable +// or moveable to be usable in a ProductManifold. +template +class ProductManifold final : public Manifold { + public: + // ProductManifold constructor perfect forwards arguments to store manifolds. + // + // Either use default construction or if you need to copy or move-construct a + // manifold instance, you need to pass an instance as an argument for all + // types given as class template parameters. + template , + Args...>::value>* = nullptr> + explicit ProductManifold(Args&&... manifolds) + : ProductManifold{std::make_index_sequence{}, + std::forward(manifolds)...} {} + + int AmbientSize() const override { return ambient_size_; } + int TangentSize() const override { return tangent_size_; } + + bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const override { + return PlusImpl( + x, delta, x_plus_delta, std::make_index_sequence{}); + } + + bool Minus(const double* y, + const double* x, + double* y_minus_x) const override { + return MinusImpl( + y, x, y_minus_x, std::make_index_sequence{}); + } + + bool PlusJacobian(const double* x, double* jacobian_ptr) const override { + MatrixRef jacobian(jacobian_ptr, AmbientSize(), TangentSize()); + jacobian.setZero(); + internal::FixedArray buffer(buffer_size_); + + return PlusJacobianImpl( + x, jacobian, buffer, std::make_index_sequence{}); + } + + bool MinusJacobian(const double* x, double* jacobian_ptr) const override { + MatrixRef jacobian(jacobian_ptr, TangentSize(), AmbientSize()); + jacobian.setZero(); + internal::FixedArray buffer(buffer_size_); + + return MinusJacobianImpl( + x, jacobian, buffer, std::make_index_sequence{}); + } + + private: + static constexpr std::size_t kNumManifolds = 2 + sizeof...(ManifoldN); + + template + explicit ProductManifold(std::index_sequence, Args&&... manifolds) + : manifolds_{std::forward(manifolds)...}, + buffer_size_{(std::max)( + {(Dereference(std::get(manifolds_)).TangentSize() * + Dereference(std::get(manifolds_)).AmbientSize())...})}, + ambient_sizes_{ + Dereference(std::get(manifolds_)).AmbientSize()...}, + tangent_sizes_{ + Dereference(std::get(manifolds_)).TangentSize()...}, + ambient_offsets_{ExclusiveScan(ambient_sizes_)}, + tangent_offsets_{ExclusiveScan(tangent_sizes_)}, + ambient_size_{ + std::accumulate(ambient_sizes_.begin(), ambient_sizes_.end(), 0)}, + tangent_size_{ + std::accumulate(tangent_sizes_.begin(), tangent_sizes_.end(), 0)} {} + + template + bool PlusImpl(const double* x, + const double* delta, + double* x_plus_delta, + std::index_sequence) const { + if (!Dereference(std::get(manifolds_)) + .Plus(x + ambient_offsets_[Index0], + delta + tangent_offsets_[Index0], + x_plus_delta + ambient_offsets_[Index0])) { + return false; + } + + return PlusImpl(x, delta, x_plus_delta, std::index_sequence{}); + } + + static constexpr bool PlusImpl(const double* /*x*/, + const double* /*delta*/, + double* /*x_plus_delta*/, + std::index_sequence<>) noexcept { + return true; + } + + template + bool MinusImpl(const double* y, + const double* x, + double* y_minus_x, + std::index_sequence) const { + if (!Dereference(std::get(manifolds_)) + .Minus(y + ambient_offsets_[Index0], + x + ambient_offsets_[Index0], + y_minus_x + tangent_offsets_[Index0])) { + return false; + } + + return MinusImpl(y, x, y_minus_x, std::index_sequence{}); + } + + static constexpr bool MinusImpl(const double* /*y*/, + const double* /*x*/, + double* /*y_minus_x*/, + std::index_sequence<>) noexcept { + return true; + } + + template + bool PlusJacobianImpl(const double* x, + MatrixRef& jacobian, + internal::FixedArray& buffer, + std::index_sequence) const { + if (!Dereference(std::get(manifolds_)) + .PlusJacobian(x + ambient_offsets_[Index0], buffer.data())) { + return false; + } + + jacobian.block(ambient_offsets_[Index0], + tangent_offsets_[Index0], + ambient_sizes_[Index0], + tangent_sizes_[Index0]) = + MatrixRef( + buffer.data(), ambient_sizes_[Index0], tangent_sizes_[Index0]); + + return PlusJacobianImpl( + x, jacobian, buffer, std::index_sequence{}); + } + + static constexpr bool PlusJacobianImpl( + const double* /*x*/, + MatrixRef& /*jacobian*/, + internal::FixedArray& /*buffer*/, + std::index_sequence<>) noexcept { + return true; + } + + template + bool MinusJacobianImpl(const double* x, + MatrixRef& jacobian, + internal::FixedArray& buffer, + std::index_sequence) const { + if (!Dereference(std::get(manifolds_)) + .MinusJacobian(x + ambient_offsets_[Index0], buffer.data())) { + return false; + } + + jacobian.block(tangent_offsets_[Index0], + ambient_offsets_[Index0], + tangent_sizes_[Index0], + ambient_sizes_[Index0]) = + MatrixRef( + buffer.data(), tangent_sizes_[Index0], ambient_sizes_[Index0]); + + return MinusJacobianImpl( + x, jacobian, buffer, std::index_sequence{}); + } + + static constexpr bool MinusJacobianImpl( + const double* /*x*/, + MatrixRef& /*jacobian*/, + internal::FixedArray& /*buffer*/, + std::index_sequence<>) noexcept { + return true; + } + + template + static std::array ExclusiveScan(const std::array& values) { + std::array result; + T init = 0; + + // TODO Replace by std::exclusive_scan once C++17 is available + for (std::size_t i = 0; i != N; ++i) { + result[i] = init; + init += values[i]; + } + + return result; + } + + // TODO Replace by std::void_t once C++17 is available + template + struct Void { + using type = void; + }; + + template + struct IsDereferenceable : std::false_type {}; + + template + struct IsDereferenceable())>::type> + : std::true_type {}; + + template ::value>* = nullptr> + static constexpr decltype(auto) Dereference(T& value) { + return value; + } + + // Support dereferenceable types such as std::unique_ptr, std::shared_ptr, raw + // pointers etc. + template ::value>* = nullptr> + static constexpr decltype(auto) Dereference(T& value) { + return *value; + } + + template + static constexpr decltype(auto) Dereference(T* p) { + assert(p != nullptr); + return *p; + } + + std::tuple manifolds_; + int buffer_size_; + std::array ambient_sizes_; + std::array tangent_sizes_; + std::array ambient_offsets_; + std::array tangent_offsets_; + int ambient_size_; + int tangent_size_; +}; + +#ifdef CERES_HAS_CPP17 +// C++17 deduction guide that allows the user to avoid explicitly specifying +// the template parameters of ProductManifold. The class can instead be +// instantiated as follows: +// +// ProductManifold manifold{QuaternionManifold{}, EuclideanManifold<3>{}}; +// +template +ProductManifold(Manifold0&&, Manifold1&&, Manifolds&&...) + -> ProductManifold; +#endif + +} // namespace ceres + +#endif // CERES_PUBLIC_PRODUCT_MANIFOLD_H_ diff --git a/include/ceres/rotation.h b/include/ceres/rotation.h new file mode 100644 index 0000000000000000000000000000000000000000..51079901aaf9774eafd4ea7dd404a5aef37e4c7f --- /dev/null +++ b/include/ceres/rotation.h @@ -0,0 +1,655 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2019 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// sameeragarwal@google.com (Sameer Agarwal) +// +// Templated functions for manipulating rotations. The templated +// functions are useful when implementing functors for automatic +// differentiation. +// +// In the following, the Quaternions are laid out as 4-vectors, thus: +// +// q[0] scalar part. +// q[1] coefficient of i. +// q[2] coefficient of j. +// q[3] coefficient of k. +// +// where: i*i = j*j = k*k = -1 and i*j = k, j*k = i, k*i = j. + +#ifndef CERES_PUBLIC_ROTATION_H_ +#define CERES_PUBLIC_ROTATION_H_ + +#include +#include +#include + +#include "glog/logging.h" + +namespace ceres { + +// Trivial wrapper to index linear arrays as matrices, given a fixed +// column and row stride. When an array "T* array" is wrapped by a +// +// (const) MatrixAdapter M" +// +// the expression M(i, j) is equivalent to +// +// arrary[i * row_stride + j * col_stride] +// +// Conversion functions to and from rotation matrices accept +// MatrixAdapters to permit using row-major and column-major layouts, +// and rotation matrices embedded in larger matrices (such as a 3x4 +// projection matrix). +template +struct MatrixAdapter; + +// Convenience functions to create a MatrixAdapter that treats the +// array pointed to by "pointer" as a 3x3 (contiguous) column-major or +// row-major matrix. +template +MatrixAdapter ColumnMajorAdapter3x3(T* pointer); + +template +MatrixAdapter RowMajorAdapter3x3(T* pointer); + +// Convert a value in combined axis-angle representation to a quaternion. +// The value angle_axis is a triple whose norm is an angle in radians, +// and whose direction is aligned with the axis of rotation, +// and quaternion is a 4-tuple that will contain the resulting quaternion. +// The implementation may be used with auto-differentiation up to the first +// derivative, higher derivatives may have unexpected results near the origin. +template +void AngleAxisToQuaternion(const T* angle_axis, T* quaternion); + +// Convert a quaternion to the equivalent combined axis-angle representation. +// The value quaternion must be a unit quaternion - it is not normalized first, +// and angle_axis will be filled with a value whose norm is the angle of +// rotation in radians, and whose direction is the axis of rotation. +// The implementation may be used with auto-differentiation up to the first +// derivative, higher derivatives may have unexpected results near the origin. +template +void QuaternionToAngleAxis(const T* quaternion, T* angle_axis); + +// Conversions between 3x3 rotation matrix (in column major order) and +// quaternion rotation representations. Templated for use with +// autodifferentiation. +template +void RotationMatrixToQuaternion(const T* R, T* quaternion); + +template +void RotationMatrixToQuaternion( + const MatrixAdapter& R, T* quaternion); + +// Conversions between 3x3 rotation matrix (in column major order) and +// axis-angle rotation representations. Templated for use with +// autodifferentiation. +template +void RotationMatrixToAngleAxis(const T* R, T* angle_axis); + +template +void RotationMatrixToAngleAxis( + const MatrixAdapter& R, T* angle_axis); + +template +void AngleAxisToRotationMatrix(const T* angle_axis, T* R); + +template +void AngleAxisToRotationMatrix( + const T* angle_axis, const MatrixAdapter& R); + +// Conversions between 3x3 rotation matrix (in row major order) and +// Euler angle (in degrees) rotation representations. +// +// The {pitch,roll,yaw} Euler angles are rotations around the {x,y,z} +// axes, respectively. They are applied in that same order, so the +// total rotation R is Rz * Ry * Rx. +template +void EulerAnglesToRotationMatrix(const T* euler, int row_stride, T* R); + +template +void EulerAnglesToRotationMatrix( + const T* euler, const MatrixAdapter& R); + +// Convert a 4-vector to a 3x3 scaled rotation matrix. +// +// The choice of rotation is such that the quaternion [1 0 0 0] goes to an +// identity matrix and for small a, b, c the quaternion [1 a b c] goes to +// the matrix +// +// [ 0 -c b ] +// I + 2 [ c 0 -a ] + higher order terms +// [ -b a 0 ] +// +// which corresponds to a Rodrigues approximation, the last matrix being +// the cross-product matrix of [a b c]. Together with the property that +// R(q1 * q2) = R(q1) * R(q2) this uniquely defines the mapping from q to R. +// +// No normalization of the quaternion is performed, i.e. +// R = ||q||^2 * Q, where Q is an orthonormal matrix +// such that det(Q) = 1 and Q*Q' = I +// +// WARNING: The rotation matrix is ROW MAJOR +template +inline void QuaternionToScaledRotation(const T q[4], T R[3 * 3]); + +template +inline void QuaternionToScaledRotation( + const T q[4], const MatrixAdapter& R); + +// Same as above except that the rotation matrix is normalized by the +// Frobenius norm, so that R * R' = I (and det(R) = 1). +// +// WARNING: The rotation matrix is ROW MAJOR +template +inline void QuaternionToRotation(const T q[4], T R[3 * 3]); + +template +inline void QuaternionToRotation( + const T q[4], const MatrixAdapter& R); + +// Rotates a point pt by a quaternion q: +// +// result = R(q) * pt +// +// Assumes the quaternion is unit norm. This assumption allows us to +// write the transform as (something)*pt + pt, as is clear from the +// formula below. If you pass in a quaternion with |q|^2 = 2 then you +// WILL NOT get back 2 times the result you get for a unit quaternion. +// +// Inplace rotation is not supported. pt and result must point to different +// memory locations, otherwise the result will be undefined. +template +inline void UnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]); + +// With this function you do not need to assume that q has unit norm. +// It does assume that the norm is non-zero. +// +// Inplace rotation is not supported. pt and result must point to different +// memory locations, otherwise the result will be undefined. +template +inline void QuaternionRotatePoint(const T q[4], const T pt[3], T result[3]); + +// zw = z * w, where * is the Quaternion product between 4 vectors. +// +// Inplace quaternion product is not supported. The resulting quaternion zw must +// not share the memory with the input quaternion z and w, otherwise the result +// will be undefined. +template +inline void QuaternionProduct(const T z[4], const T w[4], T zw[4]); + +// xy = x cross y; +// +// Inplace cross product is not supported. The resulting vector x_cross_y must +// not share the memory with the input vectors x and y, otherwise the result +// will be undefined. +template +inline void CrossProduct(const T x[3], const T y[3], T x_cross_y[3]); + +template +inline T DotProduct(const T x[3], const T y[3]); + +// y = R(angle_axis) * x; +// +// Inplace rotation is not supported. pt and result must point to different +// memory locations, otherwise the result will be undefined. +template +inline void AngleAxisRotatePoint(const T angle_axis[3], + const T pt[3], + T result[3]); + +// --- IMPLEMENTATION + +template +struct MatrixAdapter { + T* pointer_; + explicit MatrixAdapter(T* pointer) : pointer_(pointer) {} + + T& operator()(int r, int c) const { + return pointer_[r * row_stride + c * col_stride]; + } +}; + +template +MatrixAdapter ColumnMajorAdapter3x3(T* pointer) { + return MatrixAdapter(pointer); +} + +template +MatrixAdapter RowMajorAdapter3x3(T* pointer) { + return MatrixAdapter(pointer); +} + +template +inline void AngleAxisToQuaternion(const T* angle_axis, T* quaternion) { + const T& a0 = angle_axis[0]; + const T& a1 = angle_axis[1]; + const T& a2 = angle_axis[2]; + const T theta_squared = a0 * a0 + a1 * a1 + a2 * a2; + + // For points not at the origin, the full conversion is numerically stable. + if (theta_squared > T(0.0)) { + const T theta = sqrt(theta_squared); + const T half_theta = theta * T(0.5); + const T k = sin(half_theta) / theta; + quaternion[0] = cos(half_theta); + quaternion[1] = a0 * k; + quaternion[2] = a1 * k; + quaternion[3] = a2 * k; + } else { + // At the origin, sqrt() will produce NaN in the derivative since + // the argument is zero. By approximating with a Taylor series, + // and truncating at one term, the value and first derivatives will be + // computed correctly when Jets are used. + const T k(0.5); + quaternion[0] = T(1.0); + quaternion[1] = a0 * k; + quaternion[2] = a1 * k; + quaternion[3] = a2 * k; + } +} + +template +inline void QuaternionToAngleAxis(const T* quaternion, T* angle_axis) { + const T& q1 = quaternion[1]; + const T& q2 = quaternion[2]; + const T& q3 = quaternion[3]; + const T sin_squared_theta = q1 * q1 + q2 * q2 + q3 * q3; + + // For quaternions representing non-zero rotation, the conversion + // is numerically stable. + if (sin_squared_theta > T(0.0)) { + const T sin_theta = sqrt(sin_squared_theta); + const T& cos_theta = quaternion[0]; + + // If cos_theta is negative, theta is greater than pi/2, which + // means that angle for the angle_axis vector which is 2 * theta + // would be greater than pi. + // + // While this will result in the correct rotation, it does not + // result in a normalized angle-axis vector. + // + // In that case we observe that 2 * theta ~ 2 * theta - 2 * pi, + // which is equivalent saying + // + // theta - pi = atan(sin(theta - pi), cos(theta - pi)) + // = atan(-sin(theta), -cos(theta)) + // + const T two_theta = + T(2.0) * ((cos_theta < T(0.0)) ? atan2(-sin_theta, -cos_theta) + : atan2(sin_theta, cos_theta)); + const T k = two_theta / sin_theta; + angle_axis[0] = q1 * k; + angle_axis[1] = q2 * k; + angle_axis[2] = q3 * k; + } else { + // For zero rotation, sqrt() will produce NaN in the derivative since + // the argument is zero. By approximating with a Taylor series, + // and truncating at one term, the value and first derivatives will be + // computed correctly when Jets are used. + const T k(2.0); + angle_axis[0] = q1 * k; + angle_axis[1] = q2 * k; + angle_axis[2] = q3 * k; + } +} + +template +void RotationMatrixToQuaternion(const T* R, T* quaternion) { + RotationMatrixToQuaternion(ColumnMajorAdapter3x3(R), quaternion); +} + +// This algorithm comes from "Quaternion Calculus and Fast Animation", +// Ken Shoemake, 1987 SIGGRAPH course notes +template +void RotationMatrixToQuaternion( + const MatrixAdapter& R, T* quaternion) { + const T trace = R(0, 0) + R(1, 1) + R(2, 2); + if (trace >= 0.0) { + T t = sqrt(trace + T(1.0)); + quaternion[0] = T(0.5) * t; + t = T(0.5) / t; + quaternion[1] = (R(2, 1) - R(1, 2)) * t; + quaternion[2] = (R(0, 2) - R(2, 0)) * t; + quaternion[3] = (R(1, 0) - R(0, 1)) * t; + } else { + int i = 0; + if (R(1, 1) > R(0, 0)) { + i = 1; + } + + if (R(2, 2) > R(i, i)) { + i = 2; + } + + const int j = (i + 1) % 3; + const int k = (j + 1) % 3; + T t = sqrt(R(i, i) - R(j, j) - R(k, k) + T(1.0)); + quaternion[i + 1] = T(0.5) * t; + t = T(0.5) / t; + quaternion[0] = (R(k, j) - R(j, k)) * t; + quaternion[j + 1] = (R(j, i) + R(i, j)) * t; + quaternion[k + 1] = (R(k, i) + R(i, k)) * t; + } +} + +// The conversion of a rotation matrix to the angle-axis form is +// numerically problematic when then rotation angle is close to zero +// or to Pi. The following implementation detects when these two cases +// occurs and deals with them by taking code paths that are guaranteed +// to not perform division by a small number. +template +inline void RotationMatrixToAngleAxis(const T* R, T* angle_axis) { + RotationMatrixToAngleAxis(ColumnMajorAdapter3x3(R), angle_axis); +} + +template +void RotationMatrixToAngleAxis( + const MatrixAdapter& R, T* angle_axis) { + T quaternion[4]; + RotationMatrixToQuaternion(R, quaternion); + QuaternionToAngleAxis(quaternion, angle_axis); + return; +} + +template +inline void AngleAxisToRotationMatrix(const T* angle_axis, T* R) { + AngleAxisToRotationMatrix(angle_axis, ColumnMajorAdapter3x3(R)); +} + +template +void AngleAxisToRotationMatrix( + const T* angle_axis, const MatrixAdapter& R) { + static const T kOne = T(1.0); + const T theta2 = DotProduct(angle_axis, angle_axis); + if (theta2 > T(std::numeric_limits::epsilon())) { + // We want to be careful to only evaluate the square root if the + // norm of the angle_axis vector is greater than zero. Otherwise + // we get a division by zero. + const T theta = sqrt(theta2); + const T wx = angle_axis[0] / theta; + const T wy = angle_axis[1] / theta; + const T wz = angle_axis[2] / theta; + + const T costheta = cos(theta); + const T sintheta = sin(theta); + + // clang-format off + R(0, 0) = costheta + wx*wx*(kOne - costheta); + R(1, 0) = wz*sintheta + wx*wy*(kOne - costheta); + R(2, 0) = -wy*sintheta + wx*wz*(kOne - costheta); + R(0, 1) = wx*wy*(kOne - costheta) - wz*sintheta; + R(1, 1) = costheta + wy*wy*(kOne - costheta); + R(2, 1) = wx*sintheta + wy*wz*(kOne - costheta); + R(0, 2) = wy*sintheta + wx*wz*(kOne - costheta); + R(1, 2) = -wx*sintheta + wy*wz*(kOne - costheta); + R(2, 2) = costheta + wz*wz*(kOne - costheta); + // clang-format on + } else { + // Near zero, we switch to using the first order Taylor expansion. + R(0, 0) = kOne; + R(1, 0) = angle_axis[2]; + R(2, 0) = -angle_axis[1]; + R(0, 1) = -angle_axis[2]; + R(1, 1) = kOne; + R(2, 1) = angle_axis[0]; + R(0, 2) = angle_axis[1]; + R(1, 2) = -angle_axis[0]; + R(2, 2) = kOne; + } +} + +template +inline void EulerAnglesToRotationMatrix(const T* euler, + const int row_stride_parameter, + T* R) { + EulerAnglesToRotationMatrix(euler, RowMajorAdapter3x3(R)); +} + +template +void EulerAnglesToRotationMatrix( + const T* euler, const MatrixAdapter& R) { + const double kPi = 3.14159265358979323846; + const T degrees_to_radians(kPi / 180.0); + + const T pitch(euler[0] * degrees_to_radians); + const T roll(euler[1] * degrees_to_radians); + const T yaw(euler[2] * degrees_to_radians); + + const T c1 = cos(yaw); + const T s1 = sin(yaw); + const T c2 = cos(roll); + const T s2 = sin(roll); + const T c3 = cos(pitch); + const T s3 = sin(pitch); + + R(0, 0) = c1 * c2; + R(0, 1) = -s1 * c3 + c1 * s2 * s3; + R(0, 2) = s1 * s3 + c1 * s2 * c3; + + R(1, 0) = s1 * c2; + R(1, 1) = c1 * c3 + s1 * s2 * s3; + R(1, 2) = -c1 * s3 + s1 * s2 * c3; + + R(2, 0) = -s2; + R(2, 1) = c2 * s3; + R(2, 2) = c2 * c3; +} + +template +inline void QuaternionToScaledRotation(const T q[4], T R[3 * 3]) { + QuaternionToScaledRotation(q, RowMajorAdapter3x3(R)); +} + +template +inline void QuaternionToScaledRotation( + const T q[4], const MatrixAdapter& R) { + // Make convenient names for elements of q. + T a = q[0]; + T b = q[1]; + T c = q[2]; + T d = q[3]; + // This is not to eliminate common sub-expression, but to + // make the lines shorter so that they fit in 80 columns! + T aa = a * a; + T ab = a * b; + T ac = a * c; + T ad = a * d; + T bb = b * b; + T bc = b * c; + T bd = b * d; + T cc = c * c; + T cd = c * d; + T dd = d * d; + + // clang-format off + R(0, 0) = aa + bb - cc - dd; R(0, 1) = T(2) * (bc - ad); R(0, 2) = T(2) * (ac + bd); + R(1, 0) = T(2) * (ad + bc); R(1, 1) = aa - bb + cc - dd; R(1, 2) = T(2) * (cd - ab); + R(2, 0) = T(2) * (bd - ac); R(2, 1) = T(2) * (ab + cd); R(2, 2) = aa - bb - cc + dd; + // clang-format on +} + +template +inline void QuaternionToRotation(const T q[4], T R[3 * 3]) { + QuaternionToRotation(q, RowMajorAdapter3x3(R)); +} + +template +inline void QuaternionToRotation( + const T q[4], const MatrixAdapter& R) { + QuaternionToScaledRotation(q, R); + + T normalizer = q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]; + normalizer = T(1) / normalizer; + + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + R(i, j) *= normalizer; + } + } +} + +template +inline void UnitQuaternionRotatePoint(const T q[4], + const T pt[3], + T result[3]) { + DCHECK_NE(pt, result) << "Inplace rotation is not supported."; + + // clang-format off + T uv0 = q[2] * pt[2] - q[3] * pt[1]; + T uv1 = q[3] * pt[0] - q[1] * pt[2]; + T uv2 = q[1] * pt[1] - q[2] * pt[0]; + uv0 += uv0; + uv1 += uv1; + uv2 += uv2; + result[0] = pt[0] + q[0] * uv0; + result[1] = pt[1] + q[0] * uv1; + result[2] = pt[2] + q[0] * uv2; + result[0] += q[2] * uv2 - q[3] * uv1; + result[1] += q[3] * uv0 - q[1] * uv2; + result[2] += q[1] * uv1 - q[2] * uv0; + // clang-format on +} + +template +inline void QuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) { + DCHECK_NE(pt, result) << "Inplace rotation is not supported."; + + // 'scale' is 1 / norm(q). + const T scale = + T(1) / sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); + + // Make unit-norm version of q. + const T unit[4] = { + scale * q[0], + scale * q[1], + scale * q[2], + scale * q[3], + }; + + UnitQuaternionRotatePoint(unit, pt, result); +} + +template +inline void QuaternionProduct(const T z[4], const T w[4], T zw[4]) { + DCHECK_NE(z, zw) << "Inplace quaternion product is not supported."; + DCHECK_NE(w, zw) << "Inplace quaternion product is not supported."; + + // clang-format off + zw[0] = z[0] * w[0] - z[1] * w[1] - z[2] * w[2] - z[3] * w[3]; + zw[1] = z[0] * w[1] + z[1] * w[0] + z[2] * w[3] - z[3] * w[2]; + zw[2] = z[0] * w[2] - z[1] * w[3] + z[2] * w[0] + z[3] * w[1]; + zw[3] = z[0] * w[3] + z[1] * w[2] - z[2] * w[1] + z[3] * w[0]; + // clang-format on +} + +// xy = x cross y; +template +inline void CrossProduct(const T x[3], const T y[3], T x_cross_y[3]) { + DCHECK_NE(x, x_cross_y) << "Inplace cross product is not supported."; + DCHECK_NE(y, x_cross_y) << "Inplace cross product is not supported."; + + x_cross_y[0] = x[1] * y[2] - x[2] * y[1]; + x_cross_y[1] = x[2] * y[0] - x[0] * y[2]; + x_cross_y[2] = x[0] * y[1] - x[1] * y[0]; +} + +template +inline T DotProduct(const T x[3], const T y[3]) { + return (x[0] * y[0] + x[1] * y[1] + x[2] * y[2]); +} + +template +inline void AngleAxisRotatePoint(const T angle_axis[3], + const T pt[3], + T result[3]) { + DCHECK_NE(pt, result) << "Inplace rotation is not supported."; + + const T theta2 = DotProduct(angle_axis, angle_axis); + if (theta2 > T(std::numeric_limits::epsilon())) { + // Away from zero, use the rodriguez formula + // + // result = pt costheta + + // (w x pt) * sintheta + + // w (w . pt) (1 - costheta) + // + // We want to be careful to only evaluate the square root if the + // norm of the angle_axis vector is greater than zero. Otherwise + // we get a division by zero. + // + const T theta = sqrt(theta2); + const T costheta = cos(theta); + const T sintheta = sin(theta); + const T theta_inverse = T(1.0) / theta; + + const T w[3] = {angle_axis[0] * theta_inverse, + angle_axis[1] * theta_inverse, + angle_axis[2] * theta_inverse}; + + // Explicitly inlined evaluation of the cross product for + // performance reasons. + const T w_cross_pt[3] = {w[1] * pt[2] - w[2] * pt[1], + w[2] * pt[0] - w[0] * pt[2], + w[0] * pt[1] - w[1] * pt[0]}; + const T tmp = + (w[0] * pt[0] + w[1] * pt[1] + w[2] * pt[2]) * (T(1.0) - costheta); + + result[0] = pt[0] * costheta + w_cross_pt[0] * sintheta + w[0] * tmp; + result[1] = pt[1] * costheta + w_cross_pt[1] * sintheta + w[1] * tmp; + result[2] = pt[2] * costheta + w_cross_pt[2] * sintheta + w[2] * tmp; + } else { + // Near zero, the first order Taylor approximation of the rotation + // matrix R corresponding to a vector w and angle theta is + // + // R = I + hat(w) * sin(theta) + // + // But sintheta ~ theta and theta * w = angle_axis, which gives us + // + // R = I + hat(angle_axis) + // + // and actually performing multiplication with the point pt, gives us + // R * pt = pt + angle_axis x pt. + // + // Switching to the Taylor expansion near zero provides meaningful + // derivatives when evaluated using Jets. + // + // Explicitly inlined evaluation of the cross product for + // performance reasons. + const T w_cross_pt[3] = {angle_axis[1] * pt[2] - angle_axis[2] * pt[1], + angle_axis[2] * pt[0] - angle_axis[0] * pt[2], + angle_axis[0] * pt[1] - angle_axis[1] * pt[0]}; + + result[0] = pt[0] + w_cross_pt[0]; + result[1] = pt[1] + w_cross_pt[1]; + result[2] = pt[2] + w_cross_pt[2]; + } +} + +} // namespace ceres + +#endif // CERES_PUBLIC_ROTATION_H_ diff --git a/include/ceres/sized_cost_function.h b/include/ceres/sized_cost_function.h new file mode 100644 index 0000000000000000000000000000000000000000..d76b5c26b4cf45260db662e76ab41c52716f5e8c --- /dev/null +++ b/include/ceres/sized_cost_function.h @@ -0,0 +1,69 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2019 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// A convenience class for cost functions which are statically sized. +// Compared to the dynamically-sized base class, this reduces boilerplate. +// +// The kNumResiduals template parameter can be a constant such as 2 or 5, or it +// can be ceres::DYNAMIC. If kNumResiduals is ceres::DYNAMIC, then subclasses +// are responsible for calling set_num_residuals() at runtime. + +#ifndef CERES_PUBLIC_SIZED_COST_FUNCTION_H_ +#define CERES_PUBLIC_SIZED_COST_FUNCTION_H_ + +#include "ceres/cost_function.h" +#include "ceres/types.h" +#include "glog/logging.h" +#include "internal/parameter_dims.h" + +namespace ceres { + +template +class SizedCostFunction : public CostFunction { + public: + static_assert(kNumResiduals > 0 || kNumResiduals == DYNAMIC, + "Cost functions must have at least one residual block."); + static_assert(internal::StaticParameterDims::kIsValid, + "Invalid parameter block dimension detected. Each parameter " + "block dimension must be bigger than zero."); + + using ParameterDims = internal::StaticParameterDims; + + SizedCostFunction() { + set_num_residuals(kNumResiduals); + *mutable_parameter_block_sizes() = std::vector{Ns...}; + } + + // Subclasses must implement Evaluate(). +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_SIZED_COST_FUNCTION_H_ diff --git a/include/ceres/solver.h b/include/ceres/solver.h new file mode 100644 index 0000000000000000000000000000000000000000..026fc1c08306888a01870fdcc3eaddfb00cf2277 --- /dev/null +++ b/include/ceres/solver.h @@ -0,0 +1,1066 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2019 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_PUBLIC_SOLVER_H_ +#define CERES_PUBLIC_SOLVER_H_ + +#include +#include +#include +#include +#include + +#include "ceres/crs_matrix.h" +#include "ceres/internal/config.h" +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" +#include "ceres/iteration_callback.h" +#include "ceres/ordered_groups.h" +#include "ceres/problem.h" +#include "ceres/types.h" + +namespace ceres { + +// Interface for non-linear least squares solvers. +class CERES_EXPORT Solver { + public: + virtual ~Solver(); + + // The options structure contains, not surprisingly, options that control how + // the solver operates. The defaults should be suitable for a wide range of + // problems; however, better performance is often obtainable with tweaking. + // + // The constants are defined inside types.h + struct CERES_EXPORT Options { + // Returns true if the options struct has a valid + // configuration. Returns false otherwise, and fills in *error + // with a message describing the problem. + bool IsValid(std::string* error) const; + + // Minimizer options ---------------------------------------- + + // Ceres supports the two major families of optimization strategies - + // Trust Region and Line Search. + // + // 1. The line search approach first finds a descent direction + // along which the objective function will be reduced and then + // computes a step size that decides how far should move along + // that direction. The descent direction can be computed by + // various methods, such as gradient descent, Newton's method and + // Quasi-Newton method. The step size can be determined either + // exactly or inexactly. + // + // 2. The trust region approach approximates the objective + // function using a model function (often a quadratic) over + // a subset of the search space known as the trust region. If the + // model function succeeds in minimizing the true objective + // function the trust region is expanded; conversely, otherwise it + // is contracted and the model optimization problem is solved + // again. + // + // Trust region methods are in some sense dual to line search methods: + // trust region methods first choose a step size (the size of the + // trust region) and then a step direction while line search methods + // first choose a step direction and then a step size. + MinimizerType minimizer_type = TRUST_REGION; + + LineSearchDirectionType line_search_direction_type = LBFGS; + LineSearchType line_search_type = WOLFE; + NonlinearConjugateGradientType nonlinear_conjugate_gradient_type = + FLETCHER_REEVES; + + // The LBFGS hessian approximation is a low rank approximation to + // the inverse of the Hessian matrix. The rank of the + // approximation determines (linearly) the space and time + // complexity of using the approximation. Higher the rank, the + // better is the quality of the approximation. The increase in + // quality is however is bounded for a number of reasons. + // + // 1. The method only uses secant information and not actual + // derivatives. + // + // 2. The Hessian approximation is constrained to be positive + // definite. + // + // So increasing this rank to a large number will cost time and + // space complexity without the corresponding increase in solution + // quality. There are no hard and fast rules for choosing the + // maximum rank. The best choice usually requires some problem + // specific experimentation. + // + // For more theoretical and implementation details of the LBFGS + // method, please see: + // + // Nocedal, J. (1980). "Updating Quasi-Newton Matrices with + // Limited Storage". Mathematics of Computation 35 (151): 773-782. + int max_lbfgs_rank = 20; + + // As part of the (L)BFGS update step (BFGS) / right-multiply step (L-BFGS), + // the initial inverse Hessian approximation is taken to be the Identity. + // However, Oren showed that using instead I * \gamma, where \gamma is + // chosen to approximate an eigenvalue of the true inverse Hessian can + // result in improved convergence in a wide variety of cases. Setting + // use_approximate_eigenvalue_bfgs_scaling to true enables this scaling. + // + // It is important to note that approximate eigenvalue scaling does not + // always improve convergence, and that it can in fact significantly degrade + // performance for certain classes of problem, which is why it is disabled + // by default. In particular it can degrade performance when the + // sensitivity of the problem to different parameters varies significantly, + // as in this case a single scalar factor fails to capture this variation + // and detrimentally downscales parts of the jacobian approximation which + // correspond to low-sensitivity parameters. It can also reduce the + // robustness of the solution to errors in the jacobians. + // + // Oren S.S., Self-scaling variable metric (SSVM) algorithms + // Part II: Implementation and experiments, Management Science, + // 20(5), 863-874, 1974. + bool use_approximate_eigenvalue_bfgs_scaling = false; + + // Degree of the polynomial used to approximate the objective + // function. Valid values are BISECTION, QUADRATIC and CUBIC. + // + // BISECTION corresponds to pure backtracking search with no + // interpolation. + LineSearchInterpolationType line_search_interpolation_type = CUBIC; + + // If during the line search, the step_size falls below this + // value, it is truncated to zero. + double min_line_search_step_size = 1e-9; + + // Line search parameters. + + // Solving the line search problem exactly is computationally + // prohibitive. Fortunately, line search based optimization + // algorithms can still guarantee convergence if instead of an + // exact solution, the line search algorithm returns a solution + // which decreases the value of the objective function + // sufficiently. More precisely, we are looking for a step_size + // s.t. + // + // f(step_size) <= f(0) + sufficient_decrease * f'(0) * step_size + // + double line_search_sufficient_function_decrease = 1e-4; + + // In each iteration of the line search, + // + // new_step_size >= max_line_search_step_contraction * step_size + // + // Note that by definition, for contraction: + // + // 0 < max_step_contraction < min_step_contraction < 1 + // + double max_line_search_step_contraction = 1e-3; + + // In each iteration of the line search, + // + // new_step_size <= min_line_search_step_contraction * step_size + // + // Note that by definition, for contraction: + // + // 0 < max_step_contraction < min_step_contraction < 1 + // + double min_line_search_step_contraction = 0.6; + + // Maximum number of trial step size iterations during each line + // search, if a step size satisfying the search conditions cannot + // be found within this number of trials, the line search will + // terminate. + + // The minimum allowed value is 0 for trust region minimizer and 1 + // otherwise. If 0 is specified for the trust region minimizer, + // then line search will not be used when solving constrained + // optimization problems. + int max_num_line_search_step_size_iterations = 20; + + // Maximum number of restarts of the line search direction algorithm before + // terminating the optimization. Restarts of the line search direction + // algorithm occur when the current algorithm fails to produce a new descent + // direction. This typically indicates a numerical failure, or a breakdown + // in the validity of the approximations used. + int max_num_line_search_direction_restarts = 5; + + // The strong Wolfe conditions consist of the Armijo sufficient + // decrease condition, and an additional requirement that the + // step-size be chosen s.t. the _magnitude_ ('strong' Wolfe + // conditions) of the gradient along the search direction + // decreases sufficiently. Precisely, this second condition + // is that we seek a step_size s.t. + // + // |f'(step_size)| <= sufficient_curvature_decrease * |f'(0)| + // + // Where f() is the line search objective and f'() is the derivative + // of f w.r.t step_size (d f / d step_size). + double line_search_sufficient_curvature_decrease = 0.9; + + // During the bracketing phase of the Wolfe search, the step size is + // increased until either a point satisfying the Wolfe conditions is + // found, or an upper bound for a bracket containing a point satisfying + // the conditions is found. Precisely, at each iteration of the + // expansion: + // + // new_step_size <= max_step_expansion * step_size. + // + // By definition for expansion, max_step_expansion > 1.0. + double max_line_search_step_expansion = 10.0; + + TrustRegionStrategyType trust_region_strategy_type = LEVENBERG_MARQUARDT; + + // Type of dogleg strategy to use. + DoglegType dogleg_type = TRADITIONAL_DOGLEG; + + // The classical trust region methods are descent methods, in that + // they only accept a point if it strictly reduces the value of + // the objective function. + // + // Relaxing this requirement allows the algorithm to be more + // efficient in the long term at the cost of some local increase + // in the value of the objective function. + // + // This is because allowing for non-decreasing objective function + // values in a principled manner allows the algorithm to "jump over + // boulders" as the method is not restricted to move into narrow + // valleys while preserving its convergence properties. + // + // Setting use_nonmonotonic_steps to true enables the + // non-monotonic trust region algorithm as described by Conn, + // Gould & Toint in "Trust Region Methods", Section 10.1. + // + // The parameter max_consecutive_nonmonotonic_steps controls the + // window size used by the step selection algorithm to accept + // non-monotonic steps. + // + // Even though the value of the objective function may be larger + // than the minimum value encountered over the course of the + // optimization, the final parameters returned to the user are the + // ones corresponding to the minimum cost over all iterations. + bool use_nonmonotonic_steps = false; + int max_consecutive_nonmonotonic_steps = 5; + + // Maximum number of iterations for the minimizer to run for. + int max_num_iterations = 50; + + // Maximum time for which the minimizer should run for. + double max_solver_time_in_seconds = 1e9; + + // Number of threads used by Ceres for evaluating the cost and + // jacobians. + int num_threads = 1; + + // Trust region minimizer settings. + double initial_trust_region_radius = 1e4; + double max_trust_region_radius = 1e16; + + // Minimizer terminates when the trust region radius becomes + // smaller than this value. + double min_trust_region_radius = 1e-32; + + // Lower bound for the relative decrease before a step is + // accepted. + double min_relative_decrease = 1e-3; + + // For the Levenberg-Marquadt algorithm, the scaled diagonal of + // the normal equations J'J is used to control the size of the + // trust region. Extremely small and large values along the + // diagonal can make this regularization scheme + // fail. max_lm_diagonal and min_lm_diagonal, clamp the values of + // diag(J'J) from above and below. In the normal course of + // operation, the user should not have to modify these parameters. + double min_lm_diagonal = 1e-6; + double max_lm_diagonal = 1e32; + + // Sometimes due to numerical conditioning problems or linear + // solver flakiness, the trust region strategy may return a + // numerically invalid step that can be fixed by reducing the + // trust region size. So the TrustRegionMinimizer allows for a few + // successive invalid steps before it declares NUMERICAL_FAILURE. + int max_num_consecutive_invalid_steps = 5; + + // Minimizer terminates when + // + // (new_cost - old_cost) < function_tolerance * old_cost; + // + double function_tolerance = 1e-6; + + // Minimizer terminates when + // + // max_i |x - Project(Plus(x, -g(x))| < gradient_tolerance + // + // This value should typically be 1e-4 * function_tolerance. + double gradient_tolerance = 1e-10; + + // Minimizer terminates when + // + // |step|_2 <= parameter_tolerance * ( |x|_2 + parameter_tolerance) + // + double parameter_tolerance = 1e-8; + + // Linear least squares solver options ------------------------------------- + + LinearSolverType linear_solver_type = +#if defined(CERES_NO_SPARSE) + DENSE_QR; +#else + SPARSE_NORMAL_CHOLESKY; +#endif + + // Type of preconditioner to use with the iterative linear solvers. + PreconditionerType preconditioner_type = JACOBI; + + // Type of clustering algorithm to use for visibility based + // preconditioning. This option is used only when the + // preconditioner_type is CLUSTER_JACOBI or CLUSTER_TRIDIAGONAL. + VisibilityClusteringType visibility_clustering_type = CANONICAL_VIEWS; + + // Subset preconditioner is a preconditioner for problems with + // general sparsity. Given a subset of residual blocks of a + // problem, it uses the corresponding subset of the rows of the + // Jacobian to construct a preconditioner. + // + // Suppose the Jacobian J has been horizontally partitioned as + // + // J = [P] + // [Q] + // + // Where, Q is the set of rows corresponding to the residual + // blocks in residual_blocks_for_subset_preconditioner. + // + // The preconditioner is the inverse of the matrix Q'Q. + // + // Obviously, the efficacy of the preconditioner depends on how + // well the matrix Q approximates J'J, or how well the chosen + // residual blocks approximate the non-linear least squares + // problem. + // + // If Solver::Options::preconditioner_type == SUBSET, then + // residual_blocks_for_subset_preconditioner must be non-empty. + std::unordered_set + residual_blocks_for_subset_preconditioner; + + // Ceres supports using multiple dense linear algebra libraries for dense + // matrix factorizations. Currently EIGEN, LAPACK and CUDA are the valid + // choices. EIGEN is always available, LAPACK refers to the system BLAS + + // LAPACK library which may or may not be available. CUDA refers to Nvidia's + // GPU based dense linear algebra library, which may or may not be + // available. + // + // This setting affects the DENSE_QR, DENSE_NORMAL_CHOLESKY and DENSE_SCHUR + // solvers. For small to moderate sized problem EIGEN is a fine choice but + // for large problems, an optimized LAPACK + BLAS or CUDA implementation can + // make a substantial difference in performance. + DenseLinearAlgebraLibraryType dense_linear_algebra_library_type = EIGEN; + + // Ceres supports using multiple sparse linear algebra libraries for sparse + // matrix ordering and factorizations. Currently, SUITE_SPARSE and CX_SPARSE + // are the valid choices, depending on whether they are linked into Ceres at + // build time. + SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type = +#if !defined(CERES_NO_SUITESPARSE) + SUITE_SPARSE; +#elif defined(CERES_USE_EIGEN_SPARSE) + EIGEN_SPARSE; +#elif !defined(CERES_NO_CXSPARSE) + CX_SPARSE; +#elif !defined(CERES_NO_ACCELERATE_SPARSE) + ACCELERATE_SPARSE; +#else + NO_SPARSE; +#endif + + // The order in which variables are eliminated in a linear solver + // can have a significant of impact on the efficiency and accuracy + // of the method. e.g., when doing sparse Cholesky factorization, + // there are matrices for which a good ordering will give a + // Cholesky factor with O(n) storage, where as a bad ordering will + // result in an completely dense factor. + // + // Ceres allows the user to provide varying amounts of hints to + // the solver about the variable elimination ordering to use. This + // can range from no hints, where the solver is free to decide the + // best possible ordering based on the user's choices like the + // linear solver being used, to an exact order in which the + // variables should be eliminated, and a variety of possibilities + // in between. + // + // Instances of the ParameterBlockOrdering class are used to + // communicate this information to Ceres. + // + // Formally an ordering is an ordered partitioning of the + // parameter blocks, i.e, each parameter block belongs to exactly + // one group, and each group has a unique non-negative integer + // associated with it, that determines its order in the set of + // groups. + // + // Given such an ordering, Ceres ensures that the parameter blocks in + // the lowest numbered group are eliminated first, and then the + // parameter blocks in the next lowest numbered group and so on. Within + // each group, Ceres is free to order the parameter blocks as it + // chooses. + // + // If nullptr, then all parameter blocks are assumed to be in the + // same group and the solver is free to decide the best + // ordering. + // + // e.g. Consider the linear system + // + // x + y = 3 + // 2x + 3y = 7 + // + // There are two ways in which it can be solved. First eliminating x + // from the two equations, solving for y and then back substituting + // for x, or first eliminating y, solving for x and back substituting + // for y. The user can construct three orderings here. + // + // {0: x}, {1: y} - eliminate x first. + // {0: y}, {1: x} - eliminate y first. + // {0: x, y} - Solver gets to decide the elimination order. + // + // Thus, to have Ceres determine the ordering automatically using + // heuristics, put all the variables in group 0 and to control the + // ordering for every variable, create groups 0..N-1, one per + // variable, in the desired order. + // + // Bundle Adjustment + // ----------------- + // + // A particular case of interest is bundle adjustment, where the user + // has two options. The default is to not specify an ordering at all, + // the solver will see that the user wants to use a Schur type solver + // and figure out the right elimination ordering. + // + // But if the user already knows what parameter blocks are points and + // what are cameras, they can save preprocessing time by partitioning + // the parameter blocks into two groups, one for the points and one + // for the cameras, where the group containing the points has an id + // smaller than the group containing cameras. + std::shared_ptr linear_solver_ordering; + + // Use an explicitly computed Schur complement matrix with + // ITERATIVE_SCHUR. + // + // By default this option is disabled and ITERATIVE_SCHUR + // evaluates matrix-vector products between the Schur + // complement and a vector implicitly by exploiting the algebraic + // expression for the Schur complement. + // + // The cost of this evaluation scales with the number of non-zeros + // in the Jacobian. + // + // For small to medium sized problems there is a sweet spot where + // computing the Schur complement is cheap enough that it is much + // more efficient to explicitly compute it and use it for evaluating + // the matrix-vector products. + // + // Enabling this option tells ITERATIVE_SCHUR to use an explicitly + // computed Schur complement. + // + // NOTE: This option can only be used with the SCHUR_JACOBI + // preconditioner. + bool use_explicit_schur_complement = false; + + // Sparse Cholesky factorization algorithms use a fill-reducing + // ordering to permute the columns of the Jacobian matrix. There + // are two ways of doing this. + + // 1. Compute the Jacobian matrix in some order and then have the + // factorization algorithm permute the columns of the Jacobian. + + // 2. Compute the Jacobian with its columns already permuted. + + // The first option incurs a significant memory penalty. The + // factorization algorithm has to make a copy of the permuted + // Jacobian matrix, thus Ceres pre-permutes the columns of the + // Jacobian matrix and generally speaking, there is no performance + // penalty for doing so. + + // In some rare cases, it is worth using a more complicated + // reordering algorithm which has slightly better runtime + // performance at the expense of an extra copy of the Jacobian + // matrix. Setting use_postordering to true enables this tradeoff. + bool use_postordering = false; + + // Some non-linear least squares problems are symbolically dense but + // numerically sparse. i.e. at any given state only a small number + // of jacobian entries are non-zero, but the position and number of + // non-zeros is different depending on the state. For these problems + // it can be useful to factorize the sparse jacobian at each solver + // iteration instead of including all of the zero entries in a single + // general factorization. + // + // If your problem does not have this property (or you do not know), + // then it is probably best to keep this false, otherwise it will + // likely lead to worse performance. + + // This settings only affects the SPARSE_NORMAL_CHOLESKY solver. + bool dynamic_sparsity = false; + + // TODO(sameeragarwal): Further expand the documentation for the + // following two options. + + // NOTE1: EXPERIMENTAL FEATURE, UNDER DEVELOPMENT, USE AT YOUR OWN RISK. + // + // If use_mixed_precision_solves is true, the Gauss-Newton matrix + // is computed in double precision, but its factorization is + // computed in single precision. This can result in significant + // time and memory savings at the cost of some accuracy in the + // Gauss-Newton step. Iterative refinement is used to recover some + // of this accuracy back. + // + // If use_mixed_precision_solves is true, we recommend setting + // max_num_refinement_iterations to 2-3. + // + // NOTE2: The following two options are currently only applicable + // if sparse_linear_algebra_library_type is EIGEN_SPARSE or + // ACCELERATE_SPARSE, and linear_solver_type is SPARSE_NORMAL_CHOLESKY + // or SPARSE_SCHUR. + bool use_mixed_precision_solves = false; + + // Number steps of the iterative refinement process to run when + // computing the Gauss-Newton step. + int max_num_refinement_iterations = 0; + + // Some non-linear least squares problems have additional + // structure in the way the parameter blocks interact that it is + // beneficial to modify the way the trust region step is computed. + // + // e.g., consider the following regression problem + // + // y = a_1 exp(b_1 x) + a_2 exp(b_3 x^2 + c_1) + // + // Given a set of pairs{(x_i, y_i)}, the user wishes to estimate + // a_1, a_2, b_1, b_2, and c_1. + // + // Notice here that the expression on the left is linear in a_1 + // and a_2, and given any value for b_1, b_2 and c_1, it is + // possible to use linear regression to estimate the optimal + // values of a_1 and a_2. Indeed, its possible to analytically + // eliminate the variables a_1 and a_2 from the problem all + // together. Problems like these are known as separable least + // squares problem and the most famous algorithm for solving them + // is the Variable Projection algorithm invented by Golub & + // Pereyra. + // + // Similar structure can be found in the matrix factorization with + // missing data problem. There the corresponding algorithm is + // known as Wiberg's algorithm. + // + // Ruhe & Wedin (Algorithms for Separable Nonlinear Least Squares + // Problems, SIAM Reviews, 22(3), 1980) present an analysis of + // various algorithms for solving separable non-linear least + // squares problems and refer to "Variable Projection" as + // Algorithm I in their paper. + // + // Implementing Variable Projection is tedious and expensive, and + // they present a simpler algorithm, which they refer to as + // Algorithm II, where once the Newton/Trust Region step has been + // computed for the whole problem (a_1, a_2, b_1, b_2, c_1) and + // additional optimization step is performed to estimate a_1 and + // a_2 exactly. + // + // This idea can be generalized to cases where the residual is not + // linear in a_1 and a_2, i.e., Solve for the trust region step + // for the full problem, and then use it as the starting point to + // further optimize just a_1 and a_2. For the linear case, this + // amounts to doing a single linear least squares solve. For + // non-linear problems, any method for solving the a_1 and a_2 + // optimization problems will do. The only constraint on a_1 and + // a_2 is that they do not co-occur in any residual block. + // + // This idea can be further generalized, by not just optimizing + // (a_1, a_2), but decomposing the graph corresponding to the + // Hessian matrix's sparsity structure in a collection of + // non-overlapping independent sets and optimizing each of them. + // + // Setting "use_inner_iterations" to true enables the use of this + // non-linear generalization of Ruhe & Wedin's Algorithm II. This + // version of Ceres has a higher iteration complexity, but also + // displays better convergence behaviour per iteration. Setting + // Solver::Options::num_threads to the maximum number possible is + // highly recommended. + bool use_inner_iterations = false; + + // If inner_iterations is true, then the user has two choices. + // + // 1. Let the solver heuristically decide which parameter blocks + // to optimize in each inner iteration. To do this leave + // Solver::Options::inner_iteration_ordering untouched. + // + // 2. Specify a collection of of ordered independent sets. Where + // the lower numbered groups are optimized before the higher + // number groups. Each group must be an independent set. Not + // all parameter blocks need to be present in the ordering. + std::shared_ptr inner_iteration_ordering; + + // Generally speaking, inner iterations make significant progress + // in the early stages of the solve and then their contribution + // drops down sharply, at which point the time spent doing inner + // iterations is not worth it. + // + // Once the relative decrease in the objective function due to + // inner iterations drops below inner_iteration_tolerance, the use + // of inner iterations in subsequent trust region minimizer + // iterations is disabled. + double inner_iteration_tolerance = 1e-3; + + // Minimum number of iterations for which the linear solver should + // run, even if the convergence criterion is satisfied. + int min_linear_solver_iterations = 0; + + // Maximum number of iterations for which the linear solver should + // run. If the solver does not converge in less than + // max_linear_solver_iterations, then it returns MAX_ITERATIONS, + // as its termination type. + int max_linear_solver_iterations = 500; + + // Forcing sequence parameter. The truncated Newton solver uses + // this number to control the relative accuracy with which the + // Newton step is computed. + // + // This constant is passed to ConjugateGradientsSolver which uses + // it to terminate the iterations when + // + // (Q_i - Q_{i-1})/Q_i < eta/i + double eta = 1e-1; + + // Normalize the jacobian using Jacobi scaling before calling + // the linear least squares solver. + bool jacobi_scaling = true; + + // Logging options --------------------------------------------------------- + + LoggingType logging_type = PER_MINIMIZER_ITERATION; + + // By default the Minimizer progress is logged to VLOG(1), which + // is sent to STDERR depending on the vlog level. If this flag is + // set to true, and logging_type is not SILENT, the logging output + // is sent to STDOUT. + bool minimizer_progress_to_stdout = false; + + // List of iterations at which the minimizer should dump the trust + // region problem. Useful for testing and benchmarking. If empty + // (default), no problems are dumped. + std::vector trust_region_minimizer_iterations_to_dump; + + // Directory to which the problems should be written to. Should be + // non-empty if trust_region_minimizer_iterations_to_dump is + // non-empty and trust_region_problem_dump_format_type is not + // CONSOLE. + std::string trust_region_problem_dump_directory = "/tmp"; + DumpFormatType trust_region_problem_dump_format_type = TEXTFILE; + + // Finite differences options ---------------------------------------------- + + // Check all jacobians computed by each residual block with finite + // differences. This is expensive since it involves computing the + // derivative by normal means (e.g. user specified, autodiff, + // etc), then also computing it using finite differences. The + // results are compared, and if they differ substantially, details + // are printed to the log. + bool check_gradients = false; + + // Relative precision to check for in the gradient checker. If the + // relative difference between an element in a jacobian exceeds + // this number, then the jacobian for that cost term is dumped. + double gradient_check_relative_precision = 1e-8; + + // WARNING: This option only applies to the to the numeric + // differentiation used for checking the user provided derivatives + // when when Solver::Options::check_gradients is true. If you are + // using NumericDiffCostFunction and are interested in changing + // the step size for numeric differentiation in your cost + // function, please have a look at + // include/ceres/numeric_diff_options.h. + // + // Relative shift used for taking numeric derivatives when + // Solver::Options::check_gradients is true. + // + // For finite differencing, each dimension is evaluated at + // slightly shifted values; for the case of central difference, + // this is what gets evaluated: + // + // delta = gradient_check_numeric_derivative_relative_step_size; + // f_initial = f(x) + // f_forward = f((1 + delta) * x) + // f_backward = f((1 - delta) * x) + // + // The finite differencing is done along each dimension. The + // reason to use a relative (rather than absolute) step size is + // that this way, numeric differentiation works for functions where + // the arguments are typically large (e.g. 1e9) and when the + // values are small (e.g. 1e-5). It is possible to construct + // "torture cases" which break this finite difference heuristic, + // but they do not come up often in practice. + // + // TODO(keir): Pick a smarter number than the default above! In + // theory a good choice is sqrt(eps) * x, which for doubles means + // about 1e-8 * x. However, I have found this number too + // optimistic. This number should be exposed for users to change. + double gradient_check_numeric_derivative_relative_step_size = 1e-6; + + // If update_state_every_iteration is true, then Ceres Solver will + // guarantee that at the end of every iteration and before any + // user provided IterationCallback is called, the parameter blocks + // are updated to the current best solution found by the + // solver. Thus the IterationCallback can inspect the values of + // the parameter blocks for purposes of computation, visualization + // or termination. + + // If update_state_every_iteration is false then there is no such + // guarantee, and user provided IterationCallbacks should not + // expect to look at the parameter blocks and interpret their + // values. + bool update_state_every_iteration = false; + + // Callbacks that are executed at the end of each iteration of the + // Minimizer. An iteration may terminate midway, either due to + // numerical failures or because one of the convergence tests has + // been satisfied. In this case none of the callbacks are + // executed. + + // Callbacks are executed in the order that they are specified in + // this vector. By default, parameter blocks are updated only at the + // end of the optimization, i.e when the Minimizer terminates. This + // behaviour is controlled by update_state_every_iteration. If the + // user wishes to have access to the updated parameter blocks when + // his/her callbacks are executed, then set + // update_state_every_iteration to true. + // + // The solver does NOT take ownership of these pointers. + std::vector callbacks; + }; + + struct CERES_EXPORT Summary { + // A brief one line description of the state of the solver after + // termination. + std::string BriefReport() const; + + // A full multiline description of the state of the solver after + // termination. + std::string FullReport() const; + + bool IsSolutionUsable() const; + + // Minimizer summary ------------------------------------------------- + MinimizerType minimizer_type = TRUST_REGION; + + TerminationType termination_type = FAILURE; + + // Reason why the solver terminated. + std::string message = "ceres::Solve was not called."; + + // Cost of the problem (value of the objective function) before + // the optimization. + double initial_cost = -1.0; + + // Cost of the problem (value of the objective function) after the + // optimization. + double final_cost = -1.0; + + // The part of the total cost that comes from residual blocks that + // were held fixed by the preprocessor because all the parameter + // blocks that they depend on were fixed. + double fixed_cost = -1.0; + + // IterationSummary for each minimizer iteration in order. + std::vector iterations; + + // Number of minimizer iterations in which the step was + // accepted. Unless use_non_monotonic_steps is true this is also + // the number of steps in which the objective function value/cost + // went down. + int num_successful_steps = -1; + + // Number of minimizer iterations in which the step was rejected + // either because it did not reduce the cost enough or the step + // was not numerically valid. + int num_unsuccessful_steps = -1; + + // Number of times inner iterations were performed. + int num_inner_iteration_steps = -1; + + // Total number of iterations inside the line search algorithm + // across all invocations. We call these iterations "steps" to + // distinguish them from the outer iterations of the line search + // and trust region minimizer algorithms which call the line + // search algorithm as a subroutine. + int num_line_search_steps = -1; + + // All times reported below are wall times. + + // When the user calls Solve, before the actual optimization + // occurs, Ceres performs a number of preprocessing steps. These + // include error checks, memory allocations, and reorderings. This + // time is accounted for as preprocessing time. + double preprocessor_time_in_seconds = -1.0; + + // Time spent in the TrustRegionMinimizer. + double minimizer_time_in_seconds = -1.0; + + // After the Minimizer is finished, some time is spent in + // re-evaluating residuals etc. This time is accounted for in the + // postprocessor time. + double postprocessor_time_in_seconds = -1.0; + + // Some total of all time spent inside Ceres when Solve is called. + double total_time_in_seconds = -1.0; + + // Time (in seconds) spent in the linear solver computing the + // trust region step. + double linear_solver_time_in_seconds = -1.0; + + // Number of times the Newton step was computed by solving a + // linear system. This does not include linear solves used by + // inner iterations. + int num_linear_solves = -1; + + // Time (in seconds) spent evaluating the residual vector. + double residual_evaluation_time_in_seconds = -1.0; + + // Number of residual only evaluations. + int num_residual_evaluations = -1; + + // Time (in seconds) spent evaluating the jacobian matrix. + double jacobian_evaluation_time_in_seconds = -1.0; + + // Number of Jacobian (and residual) evaluations. + int num_jacobian_evaluations = -1; + + // Time (in seconds) spent doing inner iterations. + double inner_iteration_time_in_seconds = -1.0; + + // Cumulative timing information for line searches performed as part of the + // solve. Note that in addition to the case when the Line Search minimizer + // is used, the Trust Region minimizer also uses a line search when + // solving a constrained problem. + + // Time (in seconds) spent evaluating the univariate cost function as part + // of a line search. + double line_search_cost_evaluation_time_in_seconds = -1.0; + + // Time (in seconds) spent evaluating the gradient of the univariate cost + // function as part of a line search. + double line_search_gradient_evaluation_time_in_seconds = -1.0; + + // Time (in seconds) spent minimizing the interpolating polynomial + // to compute the next candidate step size as part of a line search. + double line_search_polynomial_minimization_time_in_seconds = -1.0; + + // Total time (in seconds) spent performing line searches. + double line_search_total_time_in_seconds = -1.0; + + // Number of parameter blocks in the problem. + int num_parameter_blocks = -1; + + // Number of parameters in the problem. + int num_parameters = -1; + + // Dimension of the tangent space of the problem (or the number of + // columns in the Jacobian for the problem). This is different + // from num_parameters if a parameter block is associated with a + // LocalParameterization/Manifold. + int num_effective_parameters = -1; + + // Number of residual blocks in the problem. + int num_residual_blocks = -1; + + // Number of residuals in the problem. + int num_residuals = -1; + + // Number of parameter blocks in the problem after the inactive + // and constant parameter blocks have been removed. A parameter + // block is inactive if no residual block refers to it. + int num_parameter_blocks_reduced = -1; + + // Number of parameters in the reduced problem. + int num_parameters_reduced = -1; + + // Dimension of the tangent space of the reduced problem (or the + // number of columns in the Jacobian for the reduced + // problem). This is different from num_parameters_reduced if a + // parameter block in the reduced problem is associated with a + // LocalParameterization/Manifold. + int num_effective_parameters_reduced = -1; + + // Number of residual blocks in the reduced problem. + int num_residual_blocks_reduced = -1; + + // Number of residuals in the reduced problem. + int num_residuals_reduced = -1; + + // Is the reduced problem bounds constrained. + bool is_constrained = false; + + // Number of threads specified by the user for Jacobian and + // residual evaluation. + int num_threads_given = -1; + + // Number of threads actually used by the solver for Jacobian and + // residual evaluation. This number is not equal to + // num_threads_given if OpenMP is not available. + int num_threads_used = -1; + + // Type of the linear solver requested by the user. + LinearSolverType linear_solver_type_given = +#if defined(CERES_NO_SPARSE) + DENSE_QR; +#else + SPARSE_NORMAL_CHOLESKY; +#endif + // Type of the linear solver actually used. This may be different + // from linear_solver_type_given if Ceres determines that the + // problem structure is not compatible with the linear solver + // requested or if the linear solver requested by the user is not + // available, e.g. The user requested SPARSE_NORMAL_CHOLESKY but + // no sparse linear algebra library was available. + LinearSolverType linear_solver_type_used = +#if defined(CERES_NO_SPARSE) + DENSE_QR; +#else + SPARSE_NORMAL_CHOLESKY; +#endif + + // Size of the elimination groups given by the user as hints to + // the linear solver. + std::vector linear_solver_ordering_given; + + // Size of the parameter groups used by the solver when ordering + // the columns of the Jacobian. This maybe different from + // linear_solver_ordering_given if the user left + // linear_solver_ordering_given blank and asked for an automatic + // ordering, or if the problem contains some constant or inactive + // parameter blocks. + std::vector linear_solver_ordering_used; + + // For Schur type linear solvers, this string describes the + // template specialization which was detected in the problem and + // should be used. + std::string schur_structure_given; + + // This is the Schur template specialization that was actually + // instantiated and used. The reason this will be different from + // schur_structure_given is because the corresponding template + // specialization does not exist. + // + // Template specializations can be added to ceres by editing + // internal/ceres/generate_template_specializations.py + std::string schur_structure_used; + + // True if the user asked for inner iterations to be used as part + // of the optimization. + bool inner_iterations_given = false; + + // True if the user asked for inner iterations to be used as part + // of the optimization and the problem structure was such that + // they were actually performed. e.g., in a problem with just one + // parameter block, inner iterations are not performed. + bool inner_iterations_used = false; + + // Size of the parameter groups given by the user for performing + // inner iterations. + std::vector inner_iteration_ordering_given; + + // Size of the parameter groups given used by the solver for + // performing inner iterations. This maybe different from + // inner_iteration_ordering_given if the user left + // inner_iteration_ordering_given blank and asked for an automatic + // ordering, or if the problem contains some constant or inactive + // parameter blocks. + std::vector inner_iteration_ordering_used; + + // Type of the preconditioner requested by the user. + PreconditionerType preconditioner_type_given = IDENTITY; + + // Type of the preconditioner actually used. This may be different + // from linear_solver_type_given if Ceres determines that the + // problem structure is not compatible with the linear solver + // requested or if the linear solver requested by the user is not + // available. + PreconditionerType preconditioner_type_used = IDENTITY; + + // Type of clustering algorithm used for visibility based + // preconditioning. Only meaningful when the preconditioner_type + // is CLUSTER_JACOBI or CLUSTER_TRIDIAGONAL. + VisibilityClusteringType visibility_clustering_type = CANONICAL_VIEWS; + + // Type of trust region strategy. + TrustRegionStrategyType trust_region_strategy_type = LEVENBERG_MARQUARDT; + + // Type of dogleg strategy used for solving the trust region + // problem. + DoglegType dogleg_type = TRADITIONAL_DOGLEG; + + // Type of the dense linear algebra library used. + DenseLinearAlgebraLibraryType dense_linear_algebra_library_type = EIGEN; + + // Type of the sparse linear algebra library used. + SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type = + NO_SPARSE; + + // Type of line search direction used. + LineSearchDirectionType line_search_direction_type = LBFGS; + + // Type of the line search algorithm used. + LineSearchType line_search_type = WOLFE; + + // When performing line search, the degree of the polynomial used + // to approximate the objective function. + LineSearchInterpolationType line_search_interpolation_type = CUBIC; + + // If the line search direction is NONLINEAR_CONJUGATE_GRADIENT, + // then this indicates the particular variant of non-linear + // conjugate gradient used. + NonlinearConjugateGradientType nonlinear_conjugate_gradient_type = + FLETCHER_REEVES; + + // If the type of the line search direction is LBFGS, then this + // indicates the rank of the Hessian approximation. + int max_lbfgs_rank = -1; + }; + + // Once a least squares problem has been built, this function takes + // the problem and optimizes it based on the values of the options + // parameters. Upon return, a detailed summary of the work performed + // by the preprocessor, the non-linear minimizer and the linear + // solver are reported in the summary object. + virtual void Solve(const Options& options, + Problem* problem, + Solver::Summary* summary); +}; + +// Helper function which avoids going through the interface. +CERES_EXPORT void Solve(const Solver::Options& options, + Problem* problem, + Solver::Summary* summary); + +} // namespace ceres + +#include "ceres/internal/reenable_warnings.h" + +#endif // CERES_PUBLIC_SOLVER_H_ diff --git a/include/ceres/sphere_manifold.h b/include/ceres/sphere_manifold.h new file mode 100644 index 0000000000000000000000000000000000000000..5d71cbbca9a6d4dffdcaf4c16f642ae7a5e8bea9 --- /dev/null +++ b/include/ceres/sphere_manifold.h @@ -0,0 +1,231 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2022 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: vitus@google.com (Mike Vitus) +// jodebo_beck@gmx.de (Johannes Beck) + +#ifndef CERES_PUBLIC_SPHERE_MANIFOLD_H_ +#define CERES_PUBLIC_SPHERE_MANIFOLD_H_ + +#include +#include +#include +#include +#include + +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" +#include "ceres/internal/householder_vector.h" +#include "ceres/internal/sphere_manifold_functions.h" +#include "ceres/manifold.h" +#include "ceres/types.h" +#include "glog/logging.h" + +namespace ceres { + +// This provides a manifold on a sphere meaning that the norm of the vector +// stays the same. Such cases often arises in Structure for Motion +// problems. One example where they are used is in representing points whose +// triangulation is ill-conditioned. Here it is advantageous to use an +// over-parameterization since homogeneous vectors can represent points at +// infinity. +// +// The plus operator is defined as +// Plus(x, delta) = +// [sin(0.5 * |delta|) * delta / |delta|, cos(0.5 * |delta|)] * x +// +// The minus operator is defined as +// Minus(x, y) = 2 atan2(nhy, y[-1]) / nhy * hy[0 : size_ - 1] +// with nhy = norm(hy[0 : size_ - 1]) +// +// with * defined as an operator which applies the update orthogonal to x to +// remain on the sphere. The ambient space dimension is required to be greater +// than 1. +// +// The class works with dynamic and static ambient space dimensions. If the +// ambient space dimensions is known at compile time use +// +// SphereManifold<3> manifold; +// +// If the ambient space dimensions is not known at compile time the template +// parameter needs to be set to ceres::DYNAMIC and the actual dimension needs +// to be provided as a constructor argument: +// +// SphereManifold manifold(ambient_dim); +// +// See section B.2 (p.25) in "Integrating Generic Sensor Fusion Algorithms +// with Sound State Representations through Encapsulation of Manifolds" by C. +// Hertzberg, R. Wagner, U. Frese and L. Schroder for more details +// (https://arxiv.org/pdf/1107.1119.pdf) +template +class SphereManifold final : public Manifold { + public: + static_assert( + AmbientSpaceDimension == ceres::DYNAMIC || AmbientSpaceDimension > 1, + "The size of the homogeneous vector needs to be greater than 1."); + static_assert(ceres::DYNAMIC == Eigen::Dynamic, + "ceres::DYNAMIC needs to be the same as Eigen::Dynamic."); + + SphereManifold(); + explicit SphereManifold(int size); + + int AmbientSize() const override { + return AmbientSpaceDimension == ceres::DYNAMIC ? size_ + : AmbientSpaceDimension; + } + int TangentSize() const override { return AmbientSize() - 1; } + + bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const override; + bool PlusJacobian(const double* x, double* jacobian) const override; + + bool Minus(const double* y, + const double* x, + double* y_minus_x) const override; + bool MinusJacobian(const double* x, double* jacobian) const override; + + private: + static constexpr int TangentSpaceDimension = + AmbientSpaceDimension > 0 ? AmbientSpaceDimension - 1 : Eigen::Dynamic; + + using AmbientVector = Eigen::Matrix; + using TangentVector = Eigen::Matrix; + using MatrixPlusJacobian = Eigen::Matrix; + using MatrixMinusJacobian = Eigen::Matrix; + + const int size_{}; +}; + +template +SphereManifold::SphereManifold() + : size_{AmbientSpaceDimension} { + static_assert( + AmbientSpaceDimension != Eigen::Dynamic, + "The size is set to dynamic. Please call the constructor with a size."); +} + +template +SphereManifold::SphereManifold(int size) : size_{size} { + if (AmbientSpaceDimension != Eigen::Dynamic) { + CHECK_EQ(AmbientSpaceDimension, size) + << "Specified size by template parameter differs from the supplied " + "one."; + } else { + CHECK_GT(size_, 1) + << "The size of the manifold needs to be greater than 1."; + } +} + +template +bool SphereManifold::Plus( + const double* x_ptr, + const double* delta_ptr, + double* x_plus_delta_ptr) const { + Eigen::Map x(x_ptr, size_); + Eigen::Map delta(delta_ptr, size_ - 1); + Eigen::Map x_plus_delta(x_plus_delta_ptr, size_); + + const double norm_delta = delta.norm(); + + if (norm_delta == 0.0) { + x_plus_delta = x; + return true; + } + + AmbientVector v(size_); + double beta; + + // NOTE: The explicit template arguments are needed here because + // ComputeHouseholderVector is templated and some versions of MSVC + // have trouble deducing the type of v automatically. + internal::ComputeHouseholderVector, + double, + AmbientSpaceDimension>(x, &v, &beta); + + internal::ComputeSphereManifoldPlus( + v, beta, x, delta, norm_delta, &x_plus_delta); + + return true; +} + +template +bool SphereManifold::PlusJacobian( + const double* x_ptr, double* jacobian_ptr) const { + Eigen::Map x(x_ptr, size_); + Eigen::Map jacobian(jacobian_ptr, size_, size_ - 1); + internal::ComputeSphereManifoldPlusJacobian(x, &jacobian); + + return true; +} + +template +bool SphereManifold::Minus(const double* y_ptr, + const double* x_ptr, + double* y_minus_x_ptr) const { + AmbientVector y = Eigen::Map(y_ptr, size_); + Eigen::Map x(x_ptr, size_); + Eigen::Map y_minus_x(y_minus_x_ptr, size_ - 1); + + // Apply hoseholder transformation. + AmbientVector v(size_); + double beta; + + // NOTE: The explicit template arguments are needed here because + // ComputeHouseholderVector is templated and some versions of MSVC + // have trouble deducing the type of v automatically. + internal::ComputeHouseholderVector, + double, + AmbientSpaceDimension>(x, &v, &beta); + internal::ComputeSphereManifoldMinus(v, beta, x, y, &y_minus_x); + return true; +} + +template +bool SphereManifold::MinusJacobian( + const double* x_ptr, double* jacobian_ptr) const { + Eigen::Map x(x_ptr, size_); + Eigen::Map jacobian(jacobian_ptr, size_ - 1, size_); + + internal::ComputeSphereManifoldMinusJacobian(x, &jacobian); + return true; +} + +} // namespace ceres + +// clang-format off +#include "ceres/internal/reenable_warnings.h" +// clang-format on + +#endif // CERES_PUBLIC_SPHERE_MANIFOLD_H_ diff --git a/include/ceres/tiny_solver.h b/include/ceres/tiny_solver.h new file mode 100644 index 0000000000000000000000000000000000000000..86a655dc07d25bb60899505d5378c4db76a3f022 --- /dev/null +++ b/include/ceres/tiny_solver.h @@ -0,0 +1,400 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2021 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: mierle@gmail.com (Keir Mierle) +// +// WARNING WARNING WARNING +// WARNING WARNING WARNING Tiny solver is experimental and will change. +// WARNING WARNING WARNING +// +// A tiny least squares solver using Levenberg-Marquardt, intended for solving +// small dense problems with low latency and low overhead. The implementation +// takes care to do all allocation up front, so that no memory is allocated +// during solving. This is especially useful when solving many similar problems; +// for example, inverse pixel distortion for every pixel on a grid. +// +// Note: This code has no dependencies beyond Eigen, including on other parts of +// Ceres, so it is possible to take this file alone and put it in another +// project without the rest of Ceres. +// +// Algorithm based off of: +// +// [1] K. Madsen, H. Nielsen, O. Tingleoff. +// Methods for Non-linear Least Squares Problems. +// http://www2.imm.dtu.dk/pubdb/views/edoc_download.php/3215/pdf/imm3215.pdf + +#ifndef CERES_PUBLIC_TINY_SOLVER_H_ +#define CERES_PUBLIC_TINY_SOLVER_H_ + +#include +#include + +#include "Eigen/Dense" + +namespace ceres { + +// To use tiny solver, create a class or struct that allows computing the cost +// function (described below). This is similar to a ceres::CostFunction, but is +// different to enable statically allocating all memory for the solver +// (specifically, enum sizes). Key parts are the Scalar typedef, the enums to +// describe problem sizes (needed to remove all heap allocations), and the +// operator() overload to evaluate the cost and (optionally) jacobians. +// +// struct TinySolverCostFunctionTraits { +// typedef double Scalar; +// enum { +// NUM_RESIDUALS = OR Eigen::Dynamic, +// NUM_PARAMETERS = OR Eigen::Dynamic, +// }; +// bool operator()(const double* parameters, +// double* residuals, +// double* jacobian) const; +// +// int NumResiduals() const; -- Needed if NUM_RESIDUALS == Eigen::Dynamic. +// int NumParameters() const; -- Needed if NUM_PARAMETERS == Eigen::Dynamic. +// }; +// +// For operator(), the size of the objects is: +// +// double* parameters -- NUM_PARAMETERS or NumParameters() +// double* residuals -- NUM_RESIDUALS or NumResiduals() +// double* jacobian -- NUM_RESIDUALS * NUM_PARAMETERS in column-major format +// (Eigen's default); or nullptr if no jacobian +// requested. +// +// An example (fully statically sized): +// +// struct MyCostFunctionExample { +// typedef double Scalar; +// enum { +// NUM_RESIDUALS = 2, +// NUM_PARAMETERS = 3, +// }; +// bool operator()(const double* parameters, +// double* residuals, +// double* jacobian) const { +// residuals[0] = x + 2*y + 4*z; +// residuals[1] = y * z; +// if (jacobian) { +// jacobian[0 * 2 + 0] = 1; // First column (x). +// jacobian[0 * 2 + 1] = 0; +// +// jacobian[1 * 2 + 0] = 2; // Second column (y). +// jacobian[1 * 2 + 1] = z; +// +// jacobian[2 * 2 + 0] = 4; // Third column (z). +// jacobian[2 * 2 + 1] = y; +// } +// return true; +// } +// }; +// +// The solver supports either statically or dynamically sized cost +// functions. If the number of residuals is dynamic then the Function +// must define: +// +// int NumResiduals() const; +// +// If the number of parameters is dynamic then the Function must +// define: +// +// int NumParameters() const; +// +template >> +class TinySolver { + public: + // This class needs to have an Eigen aligned operator new as it contains + // fixed-size Eigen types. + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + enum { + NUM_RESIDUALS = Function::NUM_RESIDUALS, + NUM_PARAMETERS = Function::NUM_PARAMETERS + }; + using Scalar = typename Function::Scalar; + using Parameters = typename Eigen::Matrix; + + enum Status { + // max_norm |J'(x) * f(x)| < gradient_tolerance + GRADIENT_TOO_SMALL, + // ||dx|| <= parameter_tolerance * (||x|| + parameter_tolerance) + RELATIVE_STEP_SIZE_TOO_SMALL, + // cost_threshold > ||f(x)||^2 / 2 + COST_TOO_SMALL, + // num_iterations >= max_num_iterations + HIT_MAX_ITERATIONS, + // (new_cost - old_cost) < function_tolerance * old_cost + COST_CHANGE_TOO_SMALL, + + // TODO(sameeragarwal): Deal with numerical failures. + }; + + struct Options { + int max_num_iterations = 50; + + // max_norm |J'(x) * f(x)| < gradient_tolerance + Scalar gradient_tolerance = 1e-10; + + // ||dx|| <= parameter_tolerance * (||x|| + parameter_tolerance) + Scalar parameter_tolerance = 1e-8; + + // (new_cost - old_cost) < function_tolerance * old_cost + Scalar function_tolerance = 1e-6; + + // cost_threshold > ||f(x)||^2 / 2 + Scalar cost_threshold = std::numeric_limits::epsilon(); + + Scalar initial_trust_region_radius = 1e4; + }; + + struct Summary { + // 1/2 ||f(x_0)||^2 + Scalar initial_cost = -1; + // 1/2 ||f(x)||^2 + Scalar final_cost = -1; + // max_norm(J'f(x)) + Scalar gradient_max_norm = -1; + int iterations = -1; + Status status = HIT_MAX_ITERATIONS; + }; + + bool Update(const Function& function, const Parameters& x) { + if (!function(x.data(), residuals_.data(), jacobian_.data())) { + return false; + } + + residuals_ = -residuals_; + + // On the first iteration, compute a diagonal (Jacobi) scaling + // matrix, which we store as a vector. + if (summary.iterations == 0) { + // jacobi_scaling = 1 / (1 + diagonal(J'J)) + // + // 1 is added to the denominator to regularize small diagonal + // entries. + jacobi_scaling_ = 1.0 / (1.0 + jacobian_.colwise().norm().array()); + } + + // This explicitly computes the normal equations, which is numerically + // unstable. Nevertheless, it is often good enough and is fast. + // + // TODO(sameeragarwal): Refactor this to allow for DenseQR + // factorization. + jacobian_ = jacobian_ * jacobi_scaling_.asDiagonal(); + jtj_ = jacobian_.transpose() * jacobian_; + g_ = jacobian_.transpose() * residuals_; + summary.gradient_max_norm = g_.array().abs().maxCoeff(); + cost_ = residuals_.squaredNorm() / 2; + return true; + } + + const Summary& Solve(const Function& function, Parameters* x_and_min) { + Initialize(function); + assert(x_and_min); + Parameters& x = *x_and_min; + summary = Summary(); + summary.iterations = 0; + + // TODO(sameeragarwal): Deal with failure here. + Update(function, x); + summary.initial_cost = cost_; + summary.final_cost = cost_; + + if (summary.gradient_max_norm < options.gradient_tolerance) { + summary.status = GRADIENT_TOO_SMALL; + return summary; + } + + if (cost_ < options.cost_threshold) { + summary.status = COST_TOO_SMALL; + return summary; + } + + Scalar u = 1.0 / options.initial_trust_region_radius; + Scalar v = 2; + + for (summary.iterations = 1; + summary.iterations < options.max_num_iterations; + summary.iterations++) { + jtj_regularized_ = jtj_; + const Scalar min_diagonal = 1e-6; + const Scalar max_diagonal = 1e32; + for (int i = 0; i < lm_diagonal_.rows(); ++i) { + lm_diagonal_[i] = std::sqrt( + u * (std::min)((std::max)(jtj_(i, i), min_diagonal), max_diagonal)); + jtj_regularized_(i, i) += lm_diagonal_[i] * lm_diagonal_[i]; + } + + // TODO(sameeragarwal): Check for failure and deal with it. + linear_solver_.compute(jtj_regularized_); + lm_step_ = linear_solver_.solve(g_); + dx_ = jacobi_scaling_.asDiagonal() * lm_step_; + + // Adding parameter_tolerance to x.norm() ensures that this + // works if x is near zero. + const Scalar parameter_tolerance = + options.parameter_tolerance * + (x.norm() + options.parameter_tolerance); + if (dx_.norm() < parameter_tolerance) { + summary.status = RELATIVE_STEP_SIZE_TOO_SMALL; + break; + } + x_new_ = x + dx_; + + // TODO(keir): Add proper handling of errors from user eval of cost + // functions. + function(&x_new_[0], &f_x_new_[0], nullptr); + + const Scalar cost_change = (2 * cost_ - f_x_new_.squaredNorm()); + // TODO(sameeragarwal): Better more numerically stable evaluation. + const Scalar model_cost_change = lm_step_.dot(2 * g_ - jtj_ * lm_step_); + + // rho is the ratio of the actual reduction in error to the reduction + // in error that would be obtained if the problem was linear. See [1] + // for details. + Scalar rho(cost_change / model_cost_change); + if (rho > 0) { + // Accept the Levenberg-Marquardt step because the linear + // model fits well. + x = x_new_; + + if (std::abs(cost_change) < options.function_tolerance) { + cost_ = f_x_new_.squaredNorm() / 2; + summary.status = COST_CHANGE_TOO_SMALL; + break; + } + + // TODO(sameeragarwal): Deal with failure. + Update(function, x); + if (summary.gradient_max_norm < options.gradient_tolerance) { + summary.status = GRADIENT_TOO_SMALL; + break; + } + + if (cost_ < options.cost_threshold) { + summary.status = COST_TOO_SMALL; + break; + } + + Scalar tmp = Scalar(2 * rho - 1); + u = u * (std::max)(Scalar(1 / 3.), Scalar(1) - tmp * tmp * tmp); + v = 2; + + } else { + // Reject the update because either the normal equations failed to solve + // or the local linear model was not good (rho < 0). + + // Additionally if the cost change is too small, then terminate. + if (std::abs(cost_change) < options.function_tolerance) { + // Terminate + summary.status = COST_CHANGE_TOO_SMALL; + break; + } + + // Reduce the size of the trust region. + u *= v; + v *= 2; + } + } + + summary.final_cost = cost_; + return summary; + } + + Options options; + Summary summary; + + private: + // Preallocate everything, including temporary storage needed for solving the + // linear system. This allows reusing the intermediate storage across solves. + LinearSolver linear_solver_; + Scalar cost_; + Parameters dx_, x_new_, g_, jacobi_scaling_, lm_diagonal_, lm_step_; + Eigen::Matrix residuals_, f_x_new_; + Eigen::Matrix jacobian_; + Eigen::Matrix jtj_, jtj_regularized_; + + // The following definitions are needed for template metaprogramming. + template + struct enable_if; + + template + struct enable_if { + using type = T; + }; + + // The number of parameters and residuals are dynamically sized. + template + typename enable_if<(R == Eigen::Dynamic && P == Eigen::Dynamic), void>::type + Initialize(const Function& function) { + Initialize(function.NumResiduals(), function.NumParameters()); + } + + // The number of parameters is dynamically sized and the number of + // residuals is statically sized. + template + typename enable_if<(R == Eigen::Dynamic && P != Eigen::Dynamic), void>::type + Initialize(const Function& function) { + Initialize(function.NumResiduals(), P); + } + + // The number of parameters is statically sized and the number of + // residuals is dynamically sized. + template + typename enable_if<(R != Eigen::Dynamic && P == Eigen::Dynamic), void>::type + Initialize(const Function& function) { + Initialize(R, function.NumParameters()); + } + + // The number of parameters and residuals are statically sized. + template + typename enable_if<(R != Eigen::Dynamic && P != Eigen::Dynamic), void>::type + Initialize(const Function& /* function */) {} + + void Initialize(int num_residuals, int num_parameters) { + dx_.resize(num_parameters); + x_new_.resize(num_parameters); + g_.resize(num_parameters); + jacobi_scaling_.resize(num_parameters); + lm_diagonal_.resize(num_parameters); + lm_step_.resize(num_parameters); + residuals_.resize(num_residuals); + f_x_new_.resize(num_residuals); + jacobian_.resize(num_residuals, num_parameters); + jtj_.resize(num_parameters, num_parameters); + jtj_regularized_.resize(num_parameters, num_parameters); + } +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_TINY_SOLVER_H_ diff --git a/include/ceres/tiny_solver_autodiff_function.h b/include/ceres/tiny_solver_autodiff_function.h new file mode 100644 index 0000000000000000000000000000000000000000..3e3675ff07068be175af1c35de464b8c6c297821 --- /dev/null +++ b/include/ceres/tiny_solver_autodiff_function.h @@ -0,0 +1,206 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2019 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: mierle@gmail.com (Keir Mierle) +// +// WARNING WARNING WARNING +// WARNING WARNING WARNING Tiny solver is experimental and will change. +// WARNING WARNING WARNING + +#ifndef CERES_PUBLIC_TINY_SOLVER_AUTODIFF_FUNCTION_H_ +#define CERES_PUBLIC_TINY_SOLVER_AUTODIFF_FUNCTION_H_ + +#include +#include + +#include "Eigen/Core" +#include "ceres/jet.h" +#include "ceres/types.h" // For kImpossibleValue. + +namespace ceres { + +// An adapter around autodiff-style CostFunctors to enable easier use of +// TinySolver. See the example below showing how to use it: +// +// // Example for cost functor with static residual size. +// // Same as an autodiff cost functor, but taking only 1 parameter. +// struct MyFunctor { +// template +// bool operator()(const T* const parameters, T* residuals) const { +// const T& x = parameters[0]; +// const T& y = parameters[1]; +// const T& z = parameters[2]; +// residuals[0] = x + 2.*y + 4.*z; +// residuals[1] = y * z; +// return true; +// } +// }; +// +// typedef TinySolverAutoDiffFunction +// AutoDiffFunction; +// +// MyFunctor my_functor; +// AutoDiffFunction f(my_functor); +// +// Vec3 x = ...; +// TinySolver solver; +// solver.Solve(f, &x); +// +// // Example for cost functor with dynamic residual size. +// // NumResiduals() supplies dynamic size of residuals. +// // Same functionality as in tiny_solver.h but with autodiff. +// struct MyFunctorWithDynamicResiduals { +// int NumResiduals() const { +// return 2; +// } +// +// template +// bool operator()(const T* const parameters, T* residuals) const { +// const T& x = parameters[0]; +// const T& y = parameters[1]; +// const T& z = parameters[2]; +// residuals[0] = x + static_cast(2.)*y + static_cast(4.)*z; +// residuals[1] = y * z; +// return true; +// } +// }; +// +// typedef TinySolverAutoDiffFunction +// AutoDiffFunctionWithDynamicResiduals; +// +// MyFunctorWithDynamicResiduals my_functor_dyn; +// AutoDiffFunctionWithDynamicResiduals f(my_functor_dyn); +// +// Vec3 x = ...; +// TinySolver solver; +// solver.Solve(f, &x); +// +// WARNING: The cost function adapter is not thread safe. +template +class TinySolverAutoDiffFunction { + public: + // This class needs to have an Eigen aligned operator new as it contains + // as a member a Jet type, which itself has a fixed-size Eigen type as member. + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + explicit TinySolverAutoDiffFunction(const CostFunctor& cost_functor) + : cost_functor_(cost_functor) { + Initialize(cost_functor); + } + + using Scalar = T; + enum { + NUM_PARAMETERS = kNumParameters, + NUM_RESIDUALS = kNumResiduals, + }; + + // This is similar to AutoDifferentiate(), but since there is only one + // parameter block it is easier to inline to avoid overhead. + bool operator()(const T* parameters, T* residuals, T* jacobian) const { + if (jacobian == nullptr) { + // No jacobian requested, so just directly call the cost function with + // doubles, skipping jets and derivatives. + return cost_functor_(parameters, residuals); + } + // Initialize the input jets with passed parameters. + for (int i = 0; i < kNumParameters; ++i) { + jet_parameters_[i].a = parameters[i]; // Scalar part. + jet_parameters_[i].v.setZero(); // Derivative part. + jet_parameters_[i].v[i] = T(1.0); + } + + // Initialize the output jets such that we can detect user errors. + for (int i = 0; i < num_residuals_; ++i) { + jet_residuals_[i].a = kImpossibleValue; + jet_residuals_[i].v.setConstant(kImpossibleValue); + } + + // Execute the cost function, but with jets to find the derivative. + if (!cost_functor_(jet_parameters_, jet_residuals_.data())) { + return false; + } + + // Copy the jacobian out of the derivative part of the residual jets. + Eigen::Map> jacobian_matrix( + jacobian, num_residuals_, kNumParameters); + for (int r = 0; r < num_residuals_; ++r) { + residuals[r] = jet_residuals_[r].a; + // Note that while this looks like a fast vectorized write, in practice it + // unfortunately thrashes the cache since the writes to the column-major + // jacobian are strided (e.g. rows are non-contiguous). + jacobian_matrix.row(r) = jet_residuals_[r].v; + } + return true; + } + + int NumResiduals() const { + return num_residuals_; // Set by Initialize. + } + + private: + const CostFunctor& cost_functor_; + + // The number of residuals at runtime. + // This will be overriden if NUM_RESIDUALS == Eigen::Dynamic. + int num_residuals_ = kNumResiduals; + + // To evaluate the cost function with jets, temporary storage is needed. These + // are the buffers that are used during evaluation; parameters for the input, + // and jet_residuals_ are where the final cost and derivatives end up. + // + // Since this buffer is used for evaluation, the adapter is not thread safe. + using JetType = Jet; + mutable JetType jet_parameters_[kNumParameters]; + // Eigen::Matrix serves as static or dynamic container. + mutable Eigen::Matrix jet_residuals_; + + // The number of residuals is dynamically sized and the number of + // parameters is statically sized. + template + typename std::enable_if<(R == Eigen::Dynamic), void>::type Initialize( + const CostFunctor& function) { + jet_residuals_.resize(function.NumResiduals()); + num_residuals_ = function.NumResiduals(); + } + + // The number of parameters and residuals are statically sized. + template + typename std::enable_if<(R != Eigen::Dynamic), void>::type Initialize( + const CostFunctor& /* function */) { + num_residuals_ = kNumResiduals; + } +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_TINY_SOLVER_AUTODIFF_FUNCTION_H_ diff --git a/include/ceres/tiny_solver_cost_function_adapter.h b/include/ceres/tiny_solver_cost_function_adapter.h new file mode 100644 index 0000000000000000000000000000000000000000..cc5ca16af5d3ebb95d43625bea19d6da491f0c47 --- /dev/null +++ b/include/ceres/tiny_solver_cost_function_adapter.h @@ -0,0 +1,142 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2019 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_PUBLIC_TINY_SOLVER_COST_FUNCTION_ADAPTER_H_ +#define CERES_PUBLIC_TINY_SOLVER_COST_FUNCTION_ADAPTER_H_ + +#include "Eigen/Core" +#include "ceres/cost_function.h" +#include "glog/logging.h" + +namespace ceres { + +// An adapter class that lets users of TinySolver use +// ceres::CostFunction objects that have exactly one parameter block. +// +// The adapter allows for the number of residuals and the size of the +// parameter block to be specified at compile or run-time. +// +// WARNING: This object is not thread-safe. +// +// Example usage: +// +// CostFunction* cost_function = ... +// +// Number of residuals and parameter block size known at compile time: +// +// TinySolverCostFunctionAdapter +// cost_function_adapter(*cost_function); +// +// Number of residuals known at compile time and the parameter block +// size not known at compile time. +// +// TinySolverCostFunctionAdapter +// cost_function_adapter(*cost_function); +// +// Number of residuals not known at compile time and the parameter +// block size known at compile time. +// +// TinySolverCostFunctionAdapter +// cost_function_adapter(*cost_function); +// +// Number of residuals not known at compile time and the parameter +// block size not known at compile time. +// +// TinySolverCostFunctionAdapter cost_function_adapter(*cost_function); +// +template +class TinySolverCostFunctionAdapter { + public: + using Scalar = double; + enum ComponentSizeType { + NUM_PARAMETERS = kNumParameters, + NUM_RESIDUALS = kNumResiduals + }; + + // This struct needs to have an Eigen aligned operator new as it contains + // fixed-size Eigen types. + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + explicit TinySolverCostFunctionAdapter(const CostFunction& cost_function) + : cost_function_(cost_function) { + CHECK_EQ(cost_function_.parameter_block_sizes().size(), 1) + << "Only CostFunctions with exactly one parameter blocks are allowed."; + + const int parameter_block_size = cost_function_.parameter_block_sizes()[0]; + if (NUM_PARAMETERS == Eigen::Dynamic || NUM_RESIDUALS == Eigen::Dynamic) { + if (NUM_RESIDUALS != Eigen::Dynamic) { + CHECK_EQ(cost_function_.num_residuals(), NUM_RESIDUALS); + } + if (NUM_PARAMETERS != Eigen::Dynamic) { + CHECK_EQ(parameter_block_size, NUM_PARAMETERS); + } + + row_major_jacobian_.resize(cost_function_.num_residuals(), + parameter_block_size); + } + } + + bool operator()(const double* parameters, + double* residuals, + double* jacobian) const { + if (!jacobian) { + return cost_function_.Evaluate(¶meters, residuals, nullptr); + } + + double* jacobians[1] = {row_major_jacobian_.data()}; + if (!cost_function_.Evaluate(¶meters, residuals, jacobians)) { + return false; + } + + // The Function object used by TinySolver takes its Jacobian in a + // column-major layout, and the CostFunction objects use row-major + // Jacobian matrices. So the following bit of code does the + // conversion from row-major Jacobians to column-major Jacobians. + Eigen::Map> + col_major_jacobian(jacobian, NumResiduals(), NumParameters()); + col_major_jacobian = row_major_jacobian_; + return true; + } + + int NumResiduals() const { return cost_function_.num_residuals(); } + int NumParameters() const { + return cost_function_.parameter_block_sizes()[0]; + } + + private: + const CostFunction& cost_function_; + mutable Eigen::Matrix + row_major_jacobian_; +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_TINY_SOLVER_COST_FUNCTION_ADAPTER_H_ diff --git a/include/ceres/types.h b/include/ceres/types.h new file mode 100644 index 0000000000000000000000000000000000000000..e5224238129dc48dd8798d7f02d14dbc6343e0f7 --- /dev/null +++ b/include/ceres/types.h @@ -0,0 +1,535 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2019 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Enums and other top level class definitions. +// +// Note: internal/types.cc defines stringification routines for some +// of these enums. Please update those routines if you extend or +// remove enums from here. + +#ifndef CERES_PUBLIC_TYPES_H_ +#define CERES_PUBLIC_TYPES_H_ + +#include + +#include "ceres/internal/disable_warnings.h" +#include "ceres/internal/export.h" + +namespace ceres { + +// Argument type used in interfaces that can optionally take ownership +// of a passed in argument. If TAKE_OWNERSHIP is passed, the called +// object takes ownership of the pointer argument, and will call +// delete on it upon completion. +enum Ownership { + DO_NOT_TAKE_OWNERSHIP, + TAKE_OWNERSHIP, +}; + +// TODO(keir): Considerably expand the explanations of each solver type. +enum LinearSolverType { + // These solvers are for general rectangular systems formed from the + // normal equations A'A x = A'b. They are direct solvers and do not + // assume any special problem structure. + + // Solve the normal equations using a dense Cholesky solver; based + // on Eigen. + DENSE_NORMAL_CHOLESKY, + + // Solve the normal equations using a dense QR solver; based on + // Eigen. + DENSE_QR, + + // Solve the normal equations using a sparse cholesky solver; requires + // SuiteSparse or CXSparse. + SPARSE_NORMAL_CHOLESKY, + + // Specialized solvers, specific to problems with a generalized + // bi-partitite structure. + + // Solves the reduced linear system using a dense Cholesky solver; + // based on Eigen. + DENSE_SCHUR, + + // Solves the reduced linear system using a sparse Cholesky solver; + // based on CHOLMOD. + SPARSE_SCHUR, + + // Solves the reduced linear system using Conjugate Gradients, based + // on a new Ceres implementation. Suitable for large scale + // problems. + ITERATIVE_SCHUR, + + // Conjugate gradients on the normal equations. + CGNR +}; + +enum PreconditionerType { + // Trivial preconditioner - the identity matrix. + IDENTITY, + + // Block diagonal of the Gauss-Newton Hessian. + JACOBI, + + // Note: The following three preconditioners can only be used with + // the ITERATIVE_SCHUR solver. They are well suited for Structure + // from Motion problems. + + // Block diagonal of the Schur complement. This preconditioner may + // only be used with the ITERATIVE_SCHUR solver. + SCHUR_JACOBI, + + // Visibility clustering based preconditioners. + // + // The following two preconditioners use the visibility structure of + // the scene to determine the sparsity structure of the + // preconditioner. This is done using a clustering algorithm. The + // available visibility clustering algorithms are described below. + CLUSTER_JACOBI, + CLUSTER_TRIDIAGONAL, + + // Subset preconditioner is a general purpose preconditioner + // linear least squares problems. Given a set of residual blocks, + // it uses the corresponding subset of the rows of the Jacobian to + // construct a preconditioner. + // + // Suppose the Jacobian J has been horizontally partitioned as + // + // J = [P] + // [Q] + // + // Where, Q is the set of rows corresponding to the residual + // blocks in residual_blocks_for_subset_preconditioner. + // + // The preconditioner is the inverse of the matrix Q'Q. + // + // Obviously, the efficacy of the preconditioner depends on how + // well the matrix Q approximates J'J, or how well the chosen + // residual blocks approximate the non-linear least squares + // problem. + SUBSET, +}; + +enum VisibilityClusteringType { + // Canonical views algorithm as described in + // + // "Scene Summarization for Online Image Collections", Ian Simon, Noah + // Snavely, Steven M. Seitz, ICCV 2007. + // + // This clustering algorithm can be quite slow, but gives high + // quality clusters. The original visibility based clustering paper + // used this algorithm. + CANONICAL_VIEWS, + + // The classic single linkage algorithm. It is extremely fast as + // compared to CANONICAL_VIEWS, but can give slightly poorer + // results. For problems with large number of cameras though, this + // is generally a pretty good option. + // + // If you are using SCHUR_JACOBI preconditioner and have SuiteSparse + // available, CLUSTER_JACOBI and CLUSTER_TRIDIAGONAL in combination + // with the SINGLE_LINKAGE algorithm will generally give better + // results. + SINGLE_LINKAGE +}; + +enum SparseLinearAlgebraLibraryType { + // High performance sparse Cholesky factorization and approximate + // minimum degree ordering. + SUITE_SPARSE, + + // A lightweight replacement for SuiteSparse, which does not require + // a LAPACK/BLAS implementation. Consequently, its performance is + // also a bit lower than SuiteSparse. + CX_SPARSE, + + // Eigen's sparse linear algebra routines. In particular Ceres uses + // the Simplicial LDLT routines. + EIGEN_SPARSE, + + // Apple's Accelerate framework sparse linear algebra routines. + ACCELERATE_SPARSE, + + // No sparse linear solver should be used. This does not necessarily + // imply that Ceres was built without any sparse library, although that + // is the likely use case, merely that one should not be used. + NO_SPARSE +}; + +enum DenseLinearAlgebraLibraryType { + EIGEN, + LAPACK, + CUDA, +}; + +// Logging options +// The options get progressively noisier. +enum LoggingType { + SILENT, + PER_MINIMIZER_ITERATION, +}; + +enum MinimizerType { + LINE_SEARCH, + TRUST_REGION, +}; + +enum LineSearchDirectionType { + // Negative of the gradient. + STEEPEST_DESCENT, + + // A generalization of the Conjugate Gradient method to non-linear + // functions. The generalization can be performed in a number of + // different ways, resulting in a variety of search directions. The + // precise choice of the non-linear conjugate gradient algorithm + // used is determined by NonlinerConjuateGradientType. + NONLINEAR_CONJUGATE_GRADIENT, + + // BFGS, and it's limited memory approximation L-BFGS, are quasi-Newton + // algorithms that approximate the Hessian matrix by iteratively refining + // an initial estimate with rank-one updates using the gradient at each + // iteration. They are a generalisation of the Secant method and satisfy + // the Secant equation. The Secant equation has an infinium of solutions + // in multiple dimensions, as there are N*(N+1)/2 degrees of freedom in a + // symmetric matrix but only N conditions are specified by the Secant + // equation. The requirement that the Hessian approximation be positive + // definite imposes another N additional constraints, but that still leaves + // remaining degrees-of-freedom. (L)BFGS methods uniquely determine the + // approximate Hessian by imposing the additional constraints that the + // approximation at the next iteration must be the 'closest' to the current + // approximation (the nature of how this proximity is measured is actually + // the defining difference between a family of quasi-Newton methods including + // (L)BFGS & DFP). (L)BFGS is currently regarded as being the best known + // general quasi-Newton method. + // + // The principal difference between BFGS and L-BFGS is that whilst BFGS + // maintains a full, dense approximation to the (inverse) Hessian, L-BFGS + // maintains only a window of the last M observations of the parameters and + // gradients. Using this observation history, the calculation of the next + // search direction can be computed without requiring the construction of the + // full dense inverse Hessian approximation. This is particularly important + // for problems with a large number of parameters, where storage of an N-by-N + // matrix in memory would be prohibitive. + // + // For more details on BFGS see: + // + // Broyden, C.G., "The Convergence of a Class of Double-rank Minimization + // Algorithms,"; J. Inst. Maths. Applics., Vol. 6, pp 76-90, 1970. + // + // Fletcher, R., "A New Approach to Variable Metric Algorithms," + // Computer Journal, Vol. 13, pp 317-322, 1970. + // + // Goldfarb, D., "A Family of Variable Metric Updates Derived by Variational + // Means," Mathematics of Computing, Vol. 24, pp 23-26, 1970. + // + // Shanno, D.F., "Conditioning of Quasi-Newton Methods for Function + // Minimization," Mathematics of Computing, Vol. 24, pp 647-656, 1970. + // + // For more details on L-BFGS see: + // + // Nocedal, J. (1980). "Updating Quasi-Newton Matrices with Limited + // Storage". Mathematics of Computation 35 (151): 773-782. + // + // Byrd, R. H.; Nocedal, J.; Schnabel, R. B. (1994). + // "Representations of Quasi-Newton Matrices and their use in + // Limited Memory Methods". Mathematical Programming 63 (4): + // 129-156. + // + // A general reference for both methods: + // + // Nocedal J., Wright S., Numerical Optimization, 2nd Ed. Springer, 1999. + LBFGS, + BFGS, +}; + +// Nonlinear conjugate gradient methods are a generalization of the +// method of Conjugate Gradients for linear systems. The +// generalization can be carried out in a number of different ways +// leading to number of different rules for computing the search +// direction. Ceres provides a number of different variants. For more +// details see Numerical Optimization by Nocedal & Wright. +enum NonlinearConjugateGradientType { + FLETCHER_REEVES, + POLAK_RIBIERE, + HESTENES_STIEFEL, +}; + +enum LineSearchType { + // Backtracking line search with polynomial interpolation or + // bisection. + ARMIJO, + WOLFE, +}; + +// Ceres supports different strategies for computing the trust region +// step. +enum TrustRegionStrategyType { + // The default trust region strategy is to use the step computation + // used in the Levenberg-Marquardt algorithm. For more details see + // levenberg_marquardt_strategy.h + LEVENBERG_MARQUARDT, + + // Powell's dogleg algorithm interpolates between the Cauchy point + // and the Gauss-Newton step. It is particularly useful if the + // LEVENBERG_MARQUARDT algorithm is making a large number of + // unsuccessful steps. For more details see dogleg_strategy.h. + // + // NOTES: + // + // 1. This strategy has not been experimented with or tested as + // extensively as LEVENBERG_MARQUARDT, and therefore it should be + // considered EXPERIMENTAL for now. + // + // 2. For now this strategy should only be used with exact + // factorization based linear solvers, i.e., SPARSE_SCHUR, + // DENSE_SCHUR, DENSE_QR and SPARSE_NORMAL_CHOLESKY. + DOGLEG +}; + +// Ceres supports two different dogleg strategies. +// The "traditional" dogleg method by Powell and the +// "subspace" method described in +// R. H. Byrd, R. B. Schnabel, and G. A. Shultz, +// "Approximate solution of the trust region problem by minimization +// over two-dimensional subspaces", Mathematical Programming, +// 40 (1988), pp. 247--263 +enum DoglegType { + // The traditional approach constructs a dogleg path + // consisting of two line segments and finds the furthest + // point on that path that is still inside the trust region. + TRADITIONAL_DOGLEG, + + // The subspace approach finds the exact minimum of the model + // constrained to the subspace spanned by the dogleg path. + SUBSPACE_DOGLEG +}; + +enum TerminationType { + // Minimizer terminated because one of the convergence criterion set + // by the user was satisfied. + // + // 1. (new_cost - old_cost) < function_tolerance * old_cost; + // 2. max_i |gradient_i| < gradient_tolerance + // 3. |step|_2 <= parameter_tolerance * ( |x|_2 + parameter_tolerance) + // + // The user's parameter blocks will be updated with the solution. + CONVERGENCE, + + // The solver ran for maximum number of iterations or maximum amount + // of time specified by the user, but none of the convergence + // criterion specified by the user were met. The user's parameter + // blocks will be updated with the solution found so far. + NO_CONVERGENCE, + + // The minimizer terminated because of an error. The user's + // parameter blocks will not be updated. + FAILURE, + + // Using an IterationCallback object, user code can control the + // minimizer. The following enums indicate that the user code was + // responsible for termination. + // + // Minimizer terminated successfully because a user + // IterationCallback returned SOLVER_TERMINATE_SUCCESSFULLY. + // + // The user's parameter blocks will be updated with the solution. + USER_SUCCESS, + + // Minimizer terminated because because a user IterationCallback + // returned SOLVER_ABORT. + // + // The user's parameter blocks will not be updated. + USER_FAILURE +}; + +// Enums used by the IterationCallback instances to indicate to the +// solver whether it should continue solving, the user detected an +// error or the solution is good enough and the solver should +// terminate. +enum CallbackReturnType { + // Continue solving to next iteration. + SOLVER_CONTINUE, + + // Terminate solver, and do not update the parameter blocks upon + // return. Unless the user has set + // Solver:Options:::update_state_every_iteration, in which case the + // state would have been updated every iteration + // anyways. Solver::Summary::termination_type is set to USER_ABORT. + SOLVER_ABORT, + + // Terminate solver, update state and + // return. Solver::Summary::termination_type is set to USER_SUCCESS. + SOLVER_TERMINATE_SUCCESSFULLY +}; + +// The format in which linear least squares problems should be logged +// when Solver::Options::lsqp_iterations_to_dump is non-empty. +enum DumpFormatType { + // Print the linear least squares problem in a human readable format + // to stderr. The Jacobian is printed as a dense matrix. The vectors + // D, x and f are printed as dense vectors. This should only be used + // for small problems. + CONSOLE, + + // Write out the linear least squares problem to the directory + // pointed to by Solver::Options::lsqp_dump_directory as text files + // which can be read into MATLAB/Octave. The Jacobian is dumped as a + // text file containing (i,j,s) triplets, the vectors D, x and f are + // dumped as text files containing a list of their values. + // + // A MATLAB/octave script called lm_iteration_???.m is also output, + // which can be used to parse and load the problem into memory. + TEXTFILE +}; + +// For SizedCostFunction and AutoDiffCostFunction, DYNAMIC can be +// specified for the number of residuals. If specified, then the +// number of residuas for that cost function can vary at runtime. +enum DimensionType { + DYNAMIC = -1, +}; + +// The differentiation method used to compute numerical derivatives in +// NumericDiffCostFunction and DynamicNumericDiffCostFunction. +enum NumericDiffMethodType { + // Compute central finite difference: f'(x) ~ (f(x+h) - f(x-h)) / 2h. + CENTRAL, + + // Compute forward finite difference: f'(x) ~ (f(x+h) - f(x)) / h. + FORWARD, + + // Adaptive numerical differentiation using Ridders' method. Provides more + // accurate and robust derivatives at the expense of additional cost + // function evaluations. + RIDDERS +}; + +enum LineSearchInterpolationType { + BISECTION, + QUADRATIC, + CUBIC, +}; + +enum CovarianceAlgorithmType { + DENSE_SVD, + SPARSE_QR, +}; + +// It is a near impossibility that user code generates this exact +// value in normal operation, thus we will use it to fill arrays +// before passing them to user code. If on return an element of the +// array still contains this value, we will assume that the user code +// did not write to that memory location. +const double kImpossibleValue = 1e302; + +CERES_EXPORT const char* LinearSolverTypeToString(LinearSolverType type); +CERES_EXPORT bool StringToLinearSolverType(std::string value, + LinearSolverType* type); + +CERES_EXPORT const char* PreconditionerTypeToString(PreconditionerType type); +CERES_EXPORT bool StringToPreconditionerType(std::string value, + PreconditionerType* type); + +CERES_EXPORT const char* VisibilityClusteringTypeToString( + VisibilityClusteringType type); +CERES_EXPORT bool StringToVisibilityClusteringType( + std::string value, VisibilityClusteringType* type); + +CERES_EXPORT const char* SparseLinearAlgebraLibraryTypeToString( + SparseLinearAlgebraLibraryType type); +CERES_EXPORT bool StringToSparseLinearAlgebraLibraryType( + std::string value, SparseLinearAlgebraLibraryType* type); + +CERES_EXPORT const char* DenseLinearAlgebraLibraryTypeToString( + DenseLinearAlgebraLibraryType type); +CERES_EXPORT bool StringToDenseLinearAlgebraLibraryType( + std::string value, DenseLinearAlgebraLibraryType* type); + +CERES_EXPORT const char* TrustRegionStrategyTypeToString( + TrustRegionStrategyType type); +CERES_EXPORT bool StringToTrustRegionStrategyType( + std::string value, TrustRegionStrategyType* type); + +CERES_EXPORT const char* DoglegTypeToString(DoglegType type); +CERES_EXPORT bool StringToDoglegType(std::string value, DoglegType* type); + +CERES_EXPORT const char* MinimizerTypeToString(MinimizerType type); +CERES_EXPORT bool StringToMinimizerType(std::string value, MinimizerType* type); + +CERES_EXPORT const char* LineSearchDirectionTypeToString( + LineSearchDirectionType type); +CERES_EXPORT bool StringToLineSearchDirectionType( + std::string value, LineSearchDirectionType* type); + +CERES_EXPORT const char* LineSearchTypeToString(LineSearchType type); +CERES_EXPORT bool StringToLineSearchType(std::string value, + LineSearchType* type); + +CERES_EXPORT const char* NonlinearConjugateGradientTypeToString( + NonlinearConjugateGradientType type); +CERES_EXPORT bool StringToNonlinearConjugateGradientType( + std::string value, NonlinearConjugateGradientType* type); + +CERES_EXPORT const char* LineSearchInterpolationTypeToString( + LineSearchInterpolationType type); +CERES_EXPORT bool StringToLineSearchInterpolationType( + std::string value, LineSearchInterpolationType* type); + +CERES_EXPORT const char* CovarianceAlgorithmTypeToString( + CovarianceAlgorithmType type); +CERES_EXPORT bool StringToCovarianceAlgorithmType( + std::string value, CovarianceAlgorithmType* type); + +CERES_EXPORT const char* NumericDiffMethodTypeToString( + NumericDiffMethodType type); +CERES_EXPORT bool StringToNumericDiffMethodType(std::string value, + NumericDiffMethodType* type); + +CERES_EXPORT const char* LoggingTypeToString(LoggingType type); +CERES_EXPORT bool StringtoLoggingType(std::string value, LoggingType* type); + +CERES_EXPORT const char* DumpFormatTypeToString(DumpFormatType type); +CERES_EXPORT bool StringtoDumpFormatType(std::string value, + DumpFormatType* type); +CERES_EXPORT bool StringtoDumpFormatType(std::string value, LoggingType* type); + +CERES_EXPORT const char* TerminationTypeToString(TerminationType type); + +CERES_EXPORT bool IsSchurType(LinearSolverType type); +CERES_EXPORT bool IsSparseLinearAlgebraLibraryTypeAvailable( + SparseLinearAlgebraLibraryType type); +CERES_EXPORT bool IsDenseLinearAlgebraLibraryTypeAvailable( + DenseLinearAlgebraLibraryType type); + +} // namespace ceres + +#include "ceres/internal/reenable_warnings.h" + +#endif // CERES_PUBLIC_TYPES_H_ diff --git a/include/ceres/version.h b/include/ceres/version.h new file mode 100644 index 0000000000000000000000000000000000000000..e0d61972896676af940c85dec6525e6b5342246b --- /dev/null +++ b/include/ceres/version.h @@ -0,0 +1,49 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2021 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: mierle@gmail.com (Keir Mierle) + +#ifndef CERES_PUBLIC_VERSION_H_ +#define CERES_PUBLIC_VERSION_H_ + +#define CERES_VERSION_MAJOR 2 +#define CERES_VERSION_MINOR 1 +#define CERES_VERSION_REVISION 0 + +// Classic CPP stringifcation; the extra level of indirection allows the +// preprocessor to expand the macro before being converted to a string. +#define CERES_TO_STRING_HELPER(x) #x +#define CERES_TO_STRING(x) CERES_TO_STRING_HELPER(x) + +// The Ceres version as a string; for example "1.9.0". +#define CERES_VERSION_STRING \ + CERES_TO_STRING(CERES_VERSION_MAJOR) \ + "." CERES_TO_STRING(CERES_VERSION_MINOR) "." CERES_TO_STRING( \ + CERES_VERSION_REVISION) + +#endif // CERES_PUBLIC_VERSION_H_ diff --git a/include/colmap/base/camera.h b/include/colmap/base/camera.h new file mode 100644 index 0000000000000000000000000000000000000000..125033d55095fa2bbe4a4582971ebf4bcd59d727 --- /dev/null +++ b/include/colmap/base/camera.h @@ -0,0 +1,211 @@ +// Copyright (c) 2022, 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 "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& params); + + // 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 is already undistorted + bool IsUndistorted() 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& params) { params_ = params; } + +} // namespace colmap + +#endif // COLMAP_SRC_BASE_CAMERA_H_ diff --git a/include/colmap/base/camera_database.h b/include/colmap/base/camera_database.h new file mode 100644 index 0000000000000000000000000000000000000000..20dc221b282736a69bbf12f5260e7db9530b9d36 --- /dev/null +++ b/include/colmap/base/camera_database.h @@ -0,0 +1,58 @@ +// Copyright (c) 2022, 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_DATABASE_H_ +#define COLMAP_SRC_BASE_CAMERA_DATABASE_H_ + +#include + +#include "util/camera_specs.h" + +namespace colmap { + +// Database that contains sensor widths for many cameras, which is useful +// to automatically extract the focal length if EXIF information is incomplete. +class CameraDatabase { + public: + CameraDatabase(); + + size_t NumEntries() const { return specs_.size(); } + + bool QuerySensorWidth(const std::string& make, const std::string& model, + double* sensor_width); + + private: + static const camera_specs_t specs_; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_BASE_CAMERA_DATABASE_H_ diff --git a/include/colmap/base/camera_models.h b/include/colmap/base/camera_models.h new file mode 100644 index 0000000000000000000000000000000000000000..c4c50a8821278c5466a1b6c3ca48d7ecbaa015b0 --- /dev/null +++ b/include/colmap/base/camera_models.h @@ -0,0 +1,1533 @@ +// Copyright (c) 2022, 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_MODELS_H_ +#define COLMAP_SRC_BASE_CAMERA_MODELS_H_ + +#include +#include +#include + +#include +#include + +#include + +namespace colmap { + +// This file defines several different camera models and arbitrary new camera +// models can be added by the following steps: +// +// 1. Add a new struct in this file which implements all the necessary methods. +// 2. Define an unique model_name and model_id for the camera model. +// 3. Add camera model to `CAMERA_MODEL_CASES` macro in this file. +// 4. Add new template specialization of test case for camera model to +// `camera_models_test.cc`. +// +// A camera model can have three different types of camera parameters: focal +// length, principal point, extra parameters (distortion parameters). The +// parameter array is split into different groups, so that we can enable or +// disable the refinement of the individual groups during bundle adjustment. It +// is up to the camera model to access the parameters correctly (it is free to +// do so in an arbitrary manner) - the parameters are not accessed from outside. +// +// A camera model must have the following methods: +// +// - `WorldToImage`: transform normalized camera coordinates to image +// coordinates (the inverse of `ImageToWorld`). Assumes that the world +// coordinates are given as (u, v, 1). +// - `ImageToWorld`: transform image coordinates to normalized camera +// coordinates (the inverse of `WorldToImage`). Produces world coordinates +// as (u, v, 1). +// - `ImageToWorldThreshold`: transform a threshold given in pixels to +// normalized units (e.g. useful for reprojection error thresholds). +// +// Whenever you specify the camera parameters in a list, they must appear +// exactly in the order as they are accessed in the defined model struct. +// +// The camera models follow the convention that the upper left image corner has +// the coordinate (0, 0), the lower right corner (width, height), i.e. that +// the upper left pixel center has coordinate (0.5, 0.5) and the lower right +// pixel center has the coordinate (width - 0.5, height - 0.5). + +static const int kInvalidCameraModelId = -1; + +#ifndef CAMERA_MODEL_DEFINITIONS +#define CAMERA_MODEL_DEFINITIONS(model_id_value, model_name_value, \ + num_params_value) \ + static const int kModelId = model_id_value; \ + static const size_t kNumParams = num_params_value; \ + static const int model_id; \ + static const std::string model_name; \ + static const size_t num_params; \ + static const std::string params_info; \ + static const std::vector focal_length_idxs; \ + static const std::vector principal_point_idxs; \ + static const std::vector extra_params_idxs; \ + \ + static inline int InitializeModelId() { return model_id_value; }; \ + static inline std::string InitializeModelName() { \ + return model_name_value; \ + }; \ + static inline size_t InitializeNumParams() { return num_params_value; }; \ + static inline std::string InitializeParamsInfo(); \ + static inline std::vector InitializeFocalLengthIdxs(); \ + static inline std::vector InitializePrincipalPointIdxs(); \ + static inline std::vector InitializeExtraParamsIdxs(); \ + static inline std::vector InitializeParams( \ + const double focal_length, const size_t width, const size_t height); \ + \ + template \ + static void WorldToImage(const T* params, const T u, const T v, T* x, T* y); \ + template \ + static void ImageToWorld(const T* params, const T x, const T y, T* u, T* v); \ + template \ + static void Distortion(const T* extra_params, const T u, const T v, T* du, \ + T* dv); +#endif + +#ifndef CAMERA_MODEL_CASES +#define CAMERA_MODEL_CASES \ + CAMERA_MODEL_CASE(SimplePinholeCameraModel) \ + CAMERA_MODEL_CASE(PinholeCameraModel) \ + CAMERA_MODEL_CASE(SimpleRadialCameraModel) \ + CAMERA_MODEL_CASE(SimpleRadialFisheyeCameraModel) \ + CAMERA_MODEL_CASE(RadialCameraModel) \ + CAMERA_MODEL_CASE(RadialFisheyeCameraModel) \ + CAMERA_MODEL_CASE(OpenCVCameraModel) \ + CAMERA_MODEL_CASE(OpenCVFisheyeCameraModel) \ + CAMERA_MODEL_CASE(FullOpenCVCameraModel) \ + CAMERA_MODEL_CASE(FOVCameraModel) \ + CAMERA_MODEL_CASE(ThinPrismFisheyeCameraModel) +#endif + +#ifndef CAMERA_MODEL_SWITCH_CASES +#define CAMERA_MODEL_SWITCH_CASES \ + CAMERA_MODEL_CASES \ + default: \ + CAMERA_MODEL_DOES_NOT_EXIST_EXCEPTION \ + break; +#endif + +#define CAMERA_MODEL_DOES_NOT_EXIST_EXCEPTION \ + throw std::domain_error("Camera model does not exist"); + +// The "Curiously Recurring Template Pattern" (CRTP) is used here, so that we +// can reuse some shared functionality between all camera models - +// defined in the BaseCameraModel. +template +struct BaseCameraModel { + template + static inline bool HasBogusParams(const std::vector& params, + const size_t width, const size_t height, + const T min_focal_length_ratio, + const T max_focal_length_ratio, + const T max_extra_param); + + template + static inline bool HasBogusFocalLength(const std::vector& params, + const size_t width, + const size_t height, + const T min_focal_length_ratio, + const T max_focal_length_ratio); + + template + static inline bool HasBogusPrincipalPoint(const std::vector& params, + const size_t width, + const size_t height); + + template + static inline bool HasBogusExtraParams(const std::vector& params, + const T max_extra_param); + + template + static inline T ImageToWorldThreshold(const T* params, const T threshold); + + template + static inline void IterativeUndistortion(const T* params, T* u, T* v); +}; + +// Simple Pinhole camera model. +// +// No Distortion is assumed. Only focal length and principal point is modeled. +// +// Parameter list is expected in the following order: +// +// f, cx, cy +// +// See https://en.wikipedia.org/wiki/Pinhole_camera_model +struct SimplePinholeCameraModel + : public BaseCameraModel { + CAMERA_MODEL_DEFINITIONS(0, "SIMPLE_PINHOLE", 3) +}; + +// Pinhole camera model. +// +// No Distortion is assumed. Only focal length and principal point is modeled. +// +// Parameter list is expected in the following order: +// +// fx, fy, cx, cy +// +// See https://en.wikipedia.org/wiki/Pinhole_camera_model +struct PinholeCameraModel : public BaseCameraModel { + CAMERA_MODEL_DEFINITIONS(1, "PINHOLE", 4) +}; + +// Simple camera model with one focal length and one radial distortion +// parameter. +// +// This model is similar to the camera model that VisualSfM uses with the +// difference that the distortion here is applied to the projections and +// not to the measurements. +// +// Parameter list is expected in the following order: +// +// f, cx, cy, k +// +struct SimpleRadialCameraModel + : public BaseCameraModel { + CAMERA_MODEL_DEFINITIONS(2, "SIMPLE_RADIAL", 4) +}; + +// Simple camera model with one focal length and two radial distortion +// parameters. +// +// This model is equivalent to the camera model that Bundler uses +// (except for an inverse z-axis in the camera coordinate system). +// +// Parameter list is expected in the following order: +// +// f, cx, cy, k1, k2 +// +struct RadialCameraModel : public BaseCameraModel { + CAMERA_MODEL_DEFINITIONS(3, "RADIAL", 5) +}; + +// OpenCV camera model. +// +// Based on the pinhole camera model. Additionally models radial and +// tangential distortion (up to 2nd degree of coefficients). Not suitable for +// large radial distortions of fish-eye cameras. +// +// Parameter list is expected in the following order: +// +// fx, fy, cx, cy, k1, k2, p1, p2 +// +// See +// http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html +struct OpenCVCameraModel : public BaseCameraModel { + CAMERA_MODEL_DEFINITIONS(4, "OPENCV", 8) +}; + +// OpenCV fish-eye camera model. +// +// Based on the pinhole camera model. Additionally models radial and +// tangential Distortion (up to 2nd degree of coefficients). Suitable for +// large radial distortions of fish-eye cameras. +// +// Parameter list is expected in the following order: +// +// fx, fy, cx, cy, k1, k2, k3, k4 +// +// See +// http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html +struct OpenCVFisheyeCameraModel + : public BaseCameraModel { + CAMERA_MODEL_DEFINITIONS(5, "OPENCV_FISHEYE", 8) +}; + +// Full OpenCV camera model. +// +// Based on the pinhole camera model. Additionally models radial and +// tangential Distortion. +// +// Parameter list is expected in the following order: +// +// fx, fy, cx, cy, k1, k2, p1, p2, k3, k4, k5, k6 +// +// See +// http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html +struct FullOpenCVCameraModel : public BaseCameraModel { + CAMERA_MODEL_DEFINITIONS(6, "FULL_OPENCV", 12) +}; + +// FOV camera model. +// +// Based on the pinhole camera model. Additionally models radial distortion. +// This model is for example used by Project Tango for its equidistant +// calibration type. +// +// Parameter list is expected in the following order: +// +// fx, fy, cx, cy, omega +// +// See: +// Frederic Devernay, Olivier Faugeras. Straight lines have to be straight: +// Automatic calibration and removal of distortion from scenes of structured +// environments. Machine vision and applications, 2001. +struct FOVCameraModel : public BaseCameraModel { + CAMERA_MODEL_DEFINITIONS(7, "FOV", 5) + + template + static void Undistortion(const T* extra_params, const T u, const T v, T* du, + T* dv); +}; + +// Simple camera model with one focal length and one radial distortion +// parameter, suitable for fish-eye cameras. +// +// This model is equivalent to the OpenCVFisheyeCameraModel but has only one +// radial distortion coefficient. +// +// Parameter list is expected in the following order: +// +// f, cx, cy, k +// +struct SimpleRadialFisheyeCameraModel + : public BaseCameraModel { + CAMERA_MODEL_DEFINITIONS(8, "SIMPLE_RADIAL_FISHEYE", 4) +}; + +// Simple camera model with one focal length and two radial distortion +// parameters, suitable for fish-eye cameras. +// +// This model is equivalent to the OpenCVFisheyeCameraModel but has only two +// radial distortion coefficients. +// +// Parameter list is expected in the following order: +// +// f, cx, cy, k1, k2 +// +struct RadialFisheyeCameraModel + : public BaseCameraModel { + CAMERA_MODEL_DEFINITIONS(9, "RADIAL_FISHEYE", 5) +}; + +// Camera model with radial and tangential distortion coefficients and +// additional coefficients accounting for thin-prism distortion. +// +// This camera model is described in +// +// "Camera Calibration with Distortion Models and Accuracy Evaluation", +// J Weng et al., TPAMI, 1992. +// +// Parameter list is expected in the following order: +// +// fx, fy, cx, cy, k1, k2, p1, p2, k3, k4, sx1, sy1 +// +struct ThinPrismFisheyeCameraModel + : public BaseCameraModel { + CAMERA_MODEL_DEFINITIONS(10, "THIN_PRISM_FISHEYE", 12) +}; + +// Check whether camera model with given name or identifier exists. +bool ExistsCameraModelWithName(const std::string& model_name); +bool ExistsCameraModelWithId(const int model_id); + +// Convert camera name to unique camera model identifier. +// +// @param name Unique name of camera model. +// +// @return Unique identifier of camera model. +int CameraModelNameToId(const std::string& model_name); + +// Convert camera model identifier to unique camera model name. +// +// @param model_id Unique identifier of camera model. +// +// @return Unique name of camera model. +std::string CameraModelIdToName(const int model_id); + +// Initialize camera parameters using given image properties. +// +// Initializes all focal length parameters to the same given focal length and +// sets the principal point to the image center. +// +// @param model_id Unique identifier of camera model. +// @param focal_length Focal length, equal for all focal length parameters. +// @param width Sensor width of the camera. +// @param height Sensor height of the camera. +std::vector CameraModelInitializeParams(const int model_id, + const double focal_length, + const size_t width, + const size_t height); + +// Get human-readable information about the parameter vector order. +// +// @param model_id Unique identifier of camera model. +std::string CameraModelParamsInfo(const int model_id); + +// Get the indices of the parameter groups in the parameter vector. +// +// @param model_id Unique identifier of camera model. +const std::vector& CameraModelFocalLengthIdxs(const int model_id); +const std::vector& CameraModelPrincipalPointIdxs(const int model_id); +const std::vector& CameraModelExtraParamsIdxs(const int model_id); + +// Get the total number of parameters of a camera model. +size_t CameraModelNumParams(const int model_id); + +// Check whether parameters are valid, i.e. the parameter vector has +// the correct dimensions that match the specified camera model. +// +// @param model_id Unique identifier of camera model. +// @param params Array of camera parameters. +bool CameraModelVerifyParams(const int model_id, + const std::vector& params); + +// Check whether camera has bogus parameters. +// +// @param model_id Unique identifier of camera model. +// @param params Array of camera parameters. +// @param width Sensor width of the camera. +// @param height Sensor height of the camera. +// @param min_focal_length_ratio Minimum ratio of focal length over +// maximum sensor dimension. +// @param min_focal_length_ratio Maximum ratio of focal length over +// maximum sensor dimension. +// @param max_extra_param Maximum magnitude of each extra parameter. +bool CameraModelHasBogusParams(const int model_id, + const std::vector& params, + const size_t width, const size_t height, + const double min_focal_length_ratio, + const double max_focal_length_ratio, + const double max_extra_param); + +// Transform world coordinates in camera coordinate system to image coordinates. +// +// This is the inverse of `CameraModelImageToWorld`. +// +// @param model_id Unique model_id of camera model as defined in +// `CAMERA_MODEL_NAME_TO_CODE`. +// @param params Array of camera parameters. +// @param u, v Coordinates in camera system as (u, v, 1). +// @param x, y Output image coordinates in pixels. +inline void CameraModelWorldToImage(const int model_id, + const std::vector& params, + const double u, const double v, double* x, + double* y); + +// Transform image coordinates to world coordinates in camera coordinate system. +// +// This is the inverse of `CameraModelWorldToImage`. +// +// @param model_id Unique identifier of camera model. +// @param params Array of camera parameters. +// @param x, y Image coordinates in pixels. +// @param v, u Output Coordinates in camera system as (u, v, 1). +inline void CameraModelImageToWorld(const int model_id, + const std::vector& params, + const double x, const double y, double* u, + double* v); + +// Convert pixel threshold in image plane to world space by dividing +// the threshold through the mean focal length. +// +// @param model_id Unique identifier of camera model. +// @param params Array of camera parameters. +// @param threshold Image space threshold in pixels. +// +// @ return World space threshold. +inline double CameraModelImageToWorldThreshold( + const int model_id, const std::vector& params, + const double threshold); + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +//////////////////////////////////////////////////////////////////////////////// +// BaseCameraModel + +template +template +bool BaseCameraModel::HasBogusParams( + const std::vector& params, const size_t width, const size_t height, + const T min_focal_length_ratio, const T max_focal_length_ratio, + const T max_extra_param) { + if (HasBogusPrincipalPoint(params, width, height)) { + return true; + } + + if (HasBogusFocalLength(params, width, height, min_focal_length_ratio, + max_focal_length_ratio)) { + return true; + } + + if (HasBogusExtraParams(params, max_extra_param)) { + return true; + } + + return false; +} + +template +template +bool BaseCameraModel::HasBogusFocalLength( + const std::vector& params, const size_t width, const size_t height, + const T min_focal_length_ratio, const T max_focal_length_ratio) { + const size_t max_size = std::max(width, height); + + for (const auto& idx : CameraModel::focal_length_idxs) { + const T focal_length_ratio = params[idx] / max_size; + if (focal_length_ratio < min_focal_length_ratio || + focal_length_ratio > max_focal_length_ratio) { + return true; + } + } + + return false; +} + +template +template +bool BaseCameraModel::HasBogusPrincipalPoint( + const std::vector& params, const size_t width, const size_t height) { + const T cx = params[CameraModel::principal_point_idxs[0]]; + const T cy = params[CameraModel::principal_point_idxs[1]]; + return cx < 0 || cx > width || cy < 0 || cy > height; +} + +template +template +bool BaseCameraModel::HasBogusExtraParams( + const std::vector& params, const T max_extra_param) { + for (const auto& idx : CameraModel::extra_params_idxs) { + if (std::abs(params[idx]) > max_extra_param) { + return true; + } + } + + return false; +} + +template +template +T BaseCameraModel::ImageToWorldThreshold(const T* params, + const T threshold) { + T mean_focal_length = 0; + for (const auto& idx : CameraModel::focal_length_idxs) { + mean_focal_length += params[idx]; + } + mean_focal_length /= CameraModel::focal_length_idxs.size(); + return threshold / mean_focal_length; +} + +template +template +void BaseCameraModel::IterativeUndistortion(const T* params, T* u, + T* v) { + // Parameters for Newton iteration using numerical differentiation with + // central differences, 100 iterations should be enough even for complex + // camera models with higher order terms. + const size_t kNumIterations = 100; + const double kMaxStepNorm = 1e-10; + const double kRelStepSize = 1e-6; + + Eigen::Matrix2d J; + const Eigen::Vector2d x0(*u, *v); + Eigen::Vector2d x(*u, *v); + Eigen::Vector2d dx; + Eigen::Vector2d dx_0b; + Eigen::Vector2d dx_0f; + Eigen::Vector2d dx_1b; + Eigen::Vector2d dx_1f; + + for (size_t i = 0; i < kNumIterations; ++i) { + const double step0 = std::max(std::numeric_limits::epsilon(), + std::abs(kRelStepSize * x(0))); + const double step1 = std::max(std::numeric_limits::epsilon(), + std::abs(kRelStepSize * x(1))); + CameraModel::Distortion(params, x(0), x(1), &dx(0), &dx(1)); + CameraModel::Distortion(params, x(0) - step0, x(1), &dx_0b(0), &dx_0b(1)); + CameraModel::Distortion(params, x(0) + step0, x(1), &dx_0f(0), &dx_0f(1)); + CameraModel::Distortion(params, x(0), x(1) - step1, &dx_1b(0), &dx_1b(1)); + CameraModel::Distortion(params, x(0), x(1) + step1, &dx_1f(0), &dx_1f(1)); + J(0, 0) = 1 + (dx_0f(0) - dx_0b(0)) / (2 * step0); + J(0, 1) = (dx_1f(0) - dx_1b(0)) / (2 * step1); + J(1, 0) = (dx_0f(1) - dx_0b(1)) / (2 * step0); + J(1, 1) = 1 + (dx_1f(1) - dx_1b(1)) / (2 * step1); + const Eigen::Vector2d step_x = J.inverse() * (x + dx - x0); + x -= step_x; + if (step_x.squaredNorm() < kMaxStepNorm) { + break; + } + } + + *u = x(0); + *v = x(1); +} + +//////////////////////////////////////////////////////////////////////////////// +// SimplePinholeCameraModel + +std::string SimplePinholeCameraModel::InitializeParamsInfo() { + return "f, cx, cy"; +} + +std::vector SimplePinholeCameraModel::InitializeFocalLengthIdxs() { + return {0}; +} + +std::vector SimplePinholeCameraModel::InitializePrincipalPointIdxs() { + return {1, 2}; +} + +std::vector SimplePinholeCameraModel::InitializeExtraParamsIdxs() { + return {}; +} + +std::vector SimplePinholeCameraModel::InitializeParams( + const double focal_length, const size_t width, const size_t height) { + return {focal_length, width / 2.0, height / 2.0}; +} + +template +void SimplePinholeCameraModel::WorldToImage(const T* params, const T u, + const T v, T* x, T* y) { + const T f = params[0]; + const T c1 = params[1]; + const T c2 = params[2]; + + // No Distortion + + // Transform to image coordinates + *x = f * u + c1; + *y = f * v + c2; +} + +template +void SimplePinholeCameraModel::ImageToWorld(const T* params, const T x, + const T y, T* u, T* v) { + const T f = params[0]; + const T c1 = params[1]; + const T c2 = params[2]; + + *u = (x - c1) / f; + *v = (y - c2) / f; +} + +//////////////////////////////////////////////////////////////////////////////// +// PinholeCameraModel + +std::string PinholeCameraModel::InitializeParamsInfo() { + return "fx, fy, cx, cy"; +} + +std::vector PinholeCameraModel::InitializeFocalLengthIdxs() { + return {0, 1}; +} + +std::vector PinholeCameraModel::InitializePrincipalPointIdxs() { + return {2, 3}; +} + +std::vector PinholeCameraModel::InitializeExtraParamsIdxs() { + return {}; +} + +std::vector PinholeCameraModel::InitializeParams( + const double focal_length, const size_t width, const size_t height) { + return {focal_length, focal_length, width / 2.0, height / 2.0}; +} + +template +void PinholeCameraModel::WorldToImage(const T* params, const T u, const T v, + T* x, T* y) { + const T f1 = params[0]; + const T f2 = params[1]; + const T c1 = params[2]; + const T c2 = params[3]; + + // No Distortion + + // Transform to image coordinates + *x = f1 * u + c1; + *y = f2 * v + c2; +} + +template +void PinholeCameraModel::ImageToWorld(const T* params, const T x, const T y, + T* u, T* v) { + const T f1 = params[0]; + const T f2 = params[1]; + const T c1 = params[2]; + const T c2 = params[3]; + + *u = (x - c1) / f1; + *v = (y - c2) / f2; +} + +//////////////////////////////////////////////////////////////////////////////// +// SimpleRadialCameraModel + +std::string SimpleRadialCameraModel::InitializeParamsInfo() { + return "f, cx, cy, k"; +} + +std::vector SimpleRadialCameraModel::InitializeFocalLengthIdxs() { + return {0}; +} + +std::vector SimpleRadialCameraModel::InitializePrincipalPointIdxs() { + return {1, 2}; +} + +std::vector SimpleRadialCameraModel::InitializeExtraParamsIdxs() { + return {3}; +} + +std::vector SimpleRadialCameraModel::InitializeParams( + const double focal_length, const size_t width, const size_t height) { + return {focal_length, width / 2.0, height / 2.0, 0}; +} + +template +void SimpleRadialCameraModel::WorldToImage(const T* params, const T u, + const T v, T* x, T* y) { + const T f = params[0]; + const T c1 = params[1]; + const T c2 = params[2]; + + // Distortion + T du, dv; + Distortion(¶ms[3], u, v, &du, &dv); + *x = u + du; + *y = v + dv; + + // Transform to image coordinates + *x = f * *x + c1; + *y = f * *y + c2; +} + +template +void SimpleRadialCameraModel::ImageToWorld(const T* params, const T x, + const T y, T* u, T* v) { + const T f = params[0]; + const T c1 = params[1]; + const T c2 = params[2]; + + // Lift points to normalized plane + *u = (x - c1) / f; + *v = (y - c2) / f; + + IterativeUndistortion(¶ms[3], u, v); +} + +template +void SimpleRadialCameraModel::Distortion(const T* extra_params, const T u, + const T v, T* du, T* dv) { + const T k = extra_params[0]; + + const T u2 = u * u; + const T v2 = v * v; + const T r2 = u2 + v2; + const T radial = k * r2; + *du = u * radial; + *dv = v * radial; +} + +//////////////////////////////////////////////////////////////////////////////// +// RadialCameraModel + +std::string RadialCameraModel::InitializeParamsInfo() { + return "f, cx, cy, k1, k2"; +} + +std::vector RadialCameraModel::InitializeFocalLengthIdxs() { + return {0}; +} + +std::vector RadialCameraModel::InitializePrincipalPointIdxs() { + return {1, 2}; +} + +std::vector RadialCameraModel::InitializeExtraParamsIdxs() { + return {3, 4}; +} + +std::vector RadialCameraModel::InitializeParams( + const double focal_length, const size_t width, const size_t height) { + return {focal_length, width / 2.0, height / 2.0, 0, 0}; +} + +template +void RadialCameraModel::WorldToImage(const T* params, const T u, const T v, + T* x, T* y) { + const T f = params[0]; + const T c1 = params[1]; + const T c2 = params[2]; + + // Distortion + T du, dv; + Distortion(¶ms[3], u, v, &du, &dv); + *x = u + du; + *y = v + dv; + + // Transform to image coordinates + *x = f * *x + c1; + *y = f * *y + c2; +} + +template +void RadialCameraModel::ImageToWorld(const T* params, const T x, const T y, + T* u, T* v) { + const T f = params[0]; + const T c1 = params[1]; + const T c2 = params[2]; + + // Lift points to normalized plane + *u = (x - c1) / f; + *v = (y - c2) / f; + + IterativeUndistortion(¶ms[3], u, v); +} + +template +void RadialCameraModel::Distortion(const T* extra_params, const T u, const T v, + T* du, T* dv) { + const T k1 = extra_params[0]; + const T k2 = extra_params[1]; + + const T u2 = u * u; + const T v2 = v * v; + const T r2 = u2 + v2; + const T radial = k1 * r2 + k2 * r2 * r2; + *du = u * radial; + *dv = v * radial; +} + +//////////////////////////////////////////////////////////////////////////////// +// OpenCVCameraModel + +std::string OpenCVCameraModel::InitializeParamsInfo() { + return "fx, fy, cx, cy, k1, k2, p1, p2"; +} + +std::vector OpenCVCameraModel::InitializeFocalLengthIdxs() { + return {0, 1}; +} + +std::vector OpenCVCameraModel::InitializePrincipalPointIdxs() { + return {2, 3}; +} + +std::vector OpenCVCameraModel::InitializeExtraParamsIdxs() { + return {4, 5, 6, 7}; +} + +std::vector OpenCVCameraModel::InitializeParams( + const double focal_length, const size_t width, const size_t height) { + return {focal_length, focal_length, width / 2.0, height / 2.0, 0, 0, 0, 0}; +} + +template +void OpenCVCameraModel::WorldToImage(const T* params, const T u, const T v, + T* x, T* y) { + const T f1 = params[0]; + const T f2 = params[1]; + const T c1 = params[2]; + const T c2 = params[3]; + + // Distortion + T du, dv; + Distortion(¶ms[4], u, v, &du, &dv); + *x = u + du; + *y = v + dv; + + // Transform to image coordinates + *x = f1 * *x + c1; + *y = f2 * *y + c2; +} + +template +void OpenCVCameraModel::ImageToWorld(const T* params, const T x, const T y, + T* u, T* v) { + const T f1 = params[0]; + const T f2 = params[1]; + const T c1 = params[2]; + const T c2 = params[3]; + + // Lift points to normalized plane + *u = (x - c1) / f1; + *v = (y - c2) / f2; + + IterativeUndistortion(¶ms[4], u, v); +} + +template +void OpenCVCameraModel::Distortion(const T* extra_params, const T u, const T v, + T* du, T* dv) { + const T k1 = extra_params[0]; + const T k2 = extra_params[1]; + const T p1 = extra_params[2]; + const T p2 = extra_params[3]; + + const T u2 = u * u; + const T uv = u * v; + const T v2 = v * v; + const T r2 = u2 + v2; + const T radial = k1 * r2 + k2 * r2 * r2; + *du = u * radial + T(2) * p1 * uv + p2 * (r2 + T(2) * u2); + *dv = v * radial + T(2) * p2 * uv + p1 * (r2 + T(2) * v2); +} + +//////////////////////////////////////////////////////////////////////////////// +// OpenCVFisheyeCameraModel + +std::string OpenCVFisheyeCameraModel::InitializeParamsInfo() { + return "fx, fy, cx, cy, k1, k2, k3, k4"; +} + +std::vector OpenCVFisheyeCameraModel::InitializeFocalLengthIdxs() { + return {0, 1}; +} + +std::vector OpenCVFisheyeCameraModel::InitializePrincipalPointIdxs() { + return {2, 3}; +} + +std::vector OpenCVFisheyeCameraModel::InitializeExtraParamsIdxs() { + return {4, 5, 6, 7}; +} + +std::vector OpenCVFisheyeCameraModel::InitializeParams( + const double focal_length, const size_t width, const size_t height) { + return {focal_length, focal_length, width / 2.0, height / 2.0, 0, 0, 0, 0}; +} + +template +void OpenCVFisheyeCameraModel::WorldToImage(const T* params, const T u, + const T v, T* x, T* y) { + const T f1 = params[0]; + const T f2 = params[1]; + const T c1 = params[2]; + const T c2 = params[3]; + + // Distortion + T du, dv; + Distortion(¶ms[4], u, v, &du, &dv); + *x = u + du; + *y = v + dv; + + // Transform to image coordinates + *x = f1 * *x + c1; + *y = f2 * *y + c2; +} + +template +void OpenCVFisheyeCameraModel::ImageToWorld(const T* params, const T x, + const T y, T* u, T* v) { + const T f1 = params[0]; + const T f2 = params[1]; + const T c1 = params[2]; + const T c2 = params[3]; + + // Lift points to normalized plane + *u = (x - c1) / f1; + *v = (y - c2) / f2; + + IterativeUndistortion(¶ms[4], u, v); +} + +template +void OpenCVFisheyeCameraModel::Distortion(const T* extra_params, const T u, + const T v, T* du, T* dv) { + const T k1 = extra_params[0]; + const T k2 = extra_params[1]; + const T k3 = extra_params[2]; + const T k4 = extra_params[3]; + + const T r = ceres::sqrt(u * u + v * v); + + if (r > T(std::numeric_limits::epsilon())) { + const T theta = ceres::atan(r); + const T theta2 = theta * theta; + const T theta4 = theta2 * theta2; + const T theta6 = theta4 * theta2; + const T theta8 = theta4 * theta4; + const T thetad = + theta * (T(1) + k1 * theta2 + k2 * theta4 + k3 * theta6 + k4 * theta8); + *du = u * thetad / r - u; + *dv = v * thetad / r - v; + } else { + *du = T(0); + *dv = T(0); + } +} + +//////////////////////////////////////////////////////////////////////////////// +// FullOpenCVCameraModel + +std::string FullOpenCVCameraModel::InitializeParamsInfo() { + return "fx, fy, cx, cy, k1, k2, p1, p2, k3, k4, k5, k6"; +} + +std::vector FullOpenCVCameraModel::InitializeFocalLengthIdxs() { + return {0, 1}; +} + +std::vector FullOpenCVCameraModel::InitializePrincipalPointIdxs() { + return {2, 3}; +} + +std::vector FullOpenCVCameraModel::InitializeExtraParamsIdxs() { + return {4, 5, 6, 7, 8, 9, 10, 11}; +} + +std::vector FullOpenCVCameraModel::InitializeParams( + const double focal_length, const size_t width, const size_t height) { + return {focal_length, + focal_length, + width / 2.0, + height / 2.0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0}; +} + +template +void FullOpenCVCameraModel::WorldToImage(const T* params, const T u, const T v, + T* x, T* y) { + const T f1 = params[0]; + const T f2 = params[1]; + const T c1 = params[2]; + const T c2 = params[3]; + + // Distortion + T du, dv; + Distortion(¶ms[4], u, v, &du, &dv); + *x = u + du; + *y = v + dv; + + // Transform to image coordinates + *x = f1 * *x + c1; + *y = f2 * *y + c2; +} + +template +void FullOpenCVCameraModel::ImageToWorld(const T* params, const T x, const T y, + T* u, T* v) { + const T f1 = params[0]; + const T f2 = params[1]; + const T c1 = params[2]; + const T c2 = params[3]; + + // Lift points to normalized plane + *u = (x - c1) / f1; + *v = (y - c2) / f2; + + IterativeUndistortion(¶ms[4], u, v); +} + +template +void FullOpenCVCameraModel::Distortion(const T* extra_params, const T u, + const T v, T* du, T* dv) { + const T k1 = extra_params[0]; + const T k2 = extra_params[1]; + const T p1 = extra_params[2]; + const T p2 = extra_params[3]; + const T k3 = extra_params[4]; + const T k4 = extra_params[5]; + const T k5 = extra_params[6]; + const T k6 = extra_params[7]; + + const T u2 = u * u; + const T uv = u * v; + const T v2 = v * v; + const T r2 = u2 + v2; + const T r4 = r2 * r2; + const T r6 = r4 * r2; + const T radial = (T(1) + k1 * r2 + k2 * r4 + k3 * r6) / + (T(1) + k4 * r2 + k5 * r4 + k6 * r6); + *du = u * radial + T(2) * p1 * uv + p2 * (r2 + T(2) * u2) - u; + *dv = v * radial + T(2) * p2 * uv + p1 * (r2 + T(2) * v2) - v; +} + +//////////////////////////////////////////////////////////////////////////////// +// FOVCameraModel + +std::string FOVCameraModel::InitializeParamsInfo() { + return "fx, fy, cx, cy, omega"; +} + +std::vector FOVCameraModel::InitializeFocalLengthIdxs() { + return {0, 1}; +} + +std::vector FOVCameraModel::InitializePrincipalPointIdxs() { + return {2, 3}; +} + +std::vector FOVCameraModel::InitializeExtraParamsIdxs() { return {4}; } + +std::vector FOVCameraModel::InitializeParams(const double focal_length, + const size_t width, + const size_t height) { + return {focal_length, focal_length, width / 2.0, height / 2.0, 1e-2}; +} + +template +void FOVCameraModel::WorldToImage(const T* params, const T u, const T v, T* x, + T* y) { + const T f1 = params[0]; + const T f2 = params[1]; + const T c1 = params[2]; + const T c2 = params[3]; + + // Distortion + Distortion(¶ms[4], u, v, x, y); + + // Transform to image coordinates + *x = f1 * *x + c1; + *y = f2 * *y + c2; +} + +template +void FOVCameraModel::ImageToWorld(const T* params, const T x, const T y, T* u, + T* v) { + const T f1 = params[0]; + const T f2 = params[1]; + const T c1 = params[2]; + const T c2 = params[3]; + + // Lift points to normalized plane + const T uu = (x - c1) / f1; + const T vv = (y - c2) / f2; + + // Undistortion + Undistortion(¶ms[4], uu, vv, u, v); +} + +template +void FOVCameraModel::Distortion(const T* extra_params, const T u, const T v, + T* du, T* dv) { + const T omega = extra_params[0]; + + // Chosen arbitrarily. + const T kEpsilon = T(1e-4); + + const T radius2 = u * u + v * v; + const T omega2 = omega * omega; + + T factor; + if (omega2 < kEpsilon) { + // Derivation of this case with Matlab: + // syms radius omega; + // factor(radius) = atan(radius * 2 * tan(omega / 2)) / ... + // (radius * omega); + // simplify(taylor(factor, omega, 'order', 3)) + factor = (omega2 * radius2) / T(3) - omega2 / T(12) + T(1); + } else if (radius2 < kEpsilon) { + // Derivation of this case with Matlab: + // syms radius omega; + // factor(radius) = atan(radius * 2 * tan(omega / 2)) / ... + // (radius * omega); + // simplify(taylor(factor, radius, 'order', 3)) + const T tan_half_omega = ceres::tan(omega / T(2)); + factor = (T(-2) * tan_half_omega * + (T(4) * radius2 * tan_half_omega * tan_half_omega - T(3))) / + (T(3) * omega); + } else { + const T radius = ceres::sqrt(radius2); + const T numerator = ceres::atan(radius * T(2) * ceres::tan(omega / T(2))); + factor = numerator / (radius * omega); + } + + *du = u * factor; + *dv = v * factor; +} + +template +void FOVCameraModel::Undistortion(const T* extra_params, const T u, const T v, + T* du, T* dv) { + T omega = extra_params[0]; + + // Chosen arbitrarily. + const T kEpsilon = T(1e-4); + + const T radius2 = u * u + v * v; + const T omega2 = omega * omega; + + T factor; + if (omega2 < kEpsilon) { + // Derivation of this case with Matlab: + // syms radius omega; + // factor(radius) = tan(radius * omega) / ... + // (radius * 2*tan(omega/2)); + // simplify(taylor(factor, omega, 'order', 3)) + factor = (omega2 * radius2) / T(3) - omega2 / T(12) + T(1); + } else if (radius2 < kEpsilon) { + // Derivation of this case with Matlab: + // syms radius omega; + // factor(radius) = tan(radius * omega) / ... + // (radius * 2*tan(omega/2)); + // simplify(taylor(factor, radius, 'order', 3)) + factor = (omega * (omega * omega * radius2 + T(3))) / + (T(6) * ceres::tan(omega / T(2))); + } else { + const T radius = ceres::sqrt(radius2); + const T numerator = ceres::tan(radius * omega); + factor = numerator / (radius * T(2) * ceres::tan(omega / T(2))); + } + + *du = u * factor; + *dv = v * factor; +} + +//////////////////////////////////////////////////////////////////////////////// +// SimpleRadialFisheyeCameraModel + +std::string SimpleRadialFisheyeCameraModel::InitializeParamsInfo() { + return "f, cx, cy, k"; +} + +std::vector +SimpleRadialFisheyeCameraModel::InitializeFocalLengthIdxs() { + return {0}; +} + +std::vector +SimpleRadialFisheyeCameraModel::InitializePrincipalPointIdxs() { + return {1, 2}; +} + +std::vector +SimpleRadialFisheyeCameraModel::InitializeExtraParamsIdxs() { + return {3}; +} + +std::vector SimpleRadialFisheyeCameraModel::InitializeParams( + const double focal_length, const size_t width, const size_t height) { + return {focal_length, width / 2.0, height / 2.0, 0}; +} + +template +void SimpleRadialFisheyeCameraModel::WorldToImage(const T* params, const T u, + const T v, T* x, T* y) { + const T f = params[0]; + const T c1 = params[1]; + const T c2 = params[2]; + + // Distortion + T du, dv; + Distortion(¶ms[3], u, v, &du, &dv); + *x = u + du; + *y = v + dv; + + // Transform to image coordinates + *x = f * *x + c1; + *y = f * *y + c2; +} + +template +void SimpleRadialFisheyeCameraModel::ImageToWorld(const T* params, const T x, + const T y, T* u, T* v) { + const T f = params[0]; + const T c1 = params[1]; + const T c2 = params[2]; + + // Lift points to normalized plane + *u = (x - c1) / f; + *v = (y - c2) / f; + + IterativeUndistortion(¶ms[3], u, v); +} + +template +void SimpleRadialFisheyeCameraModel::Distortion(const T* extra_params, + const T u, const T v, T* du, + T* dv) { + const T k = extra_params[0]; + + const T r = ceres::sqrt(u * u + v * v); + + if (r > T(std::numeric_limits::epsilon())) { + const T theta = ceres::atan(r); + const T theta2 = theta * theta; + const T thetad = theta * (T(1) + k * theta2); + *du = u * thetad / r - u; + *dv = v * thetad / r - v; + } else { + *du = T(0); + *dv = T(0); + } +} + +//////////////////////////////////////////////////////////////////////////////// +// RadialFisheyeCameraModel + +std::string RadialFisheyeCameraModel::InitializeParamsInfo() { + return "f, cx, cy, k1, k2"; +} + +std::vector RadialFisheyeCameraModel::InitializeFocalLengthIdxs() { + return {0}; +} + +std::vector RadialFisheyeCameraModel::InitializePrincipalPointIdxs() { + return {1, 2}; +} + +std::vector RadialFisheyeCameraModel::InitializeExtraParamsIdxs() { + return {3, 4}; +} + +std::vector RadialFisheyeCameraModel::InitializeParams( + const double focal_length, const size_t width, const size_t height) { + return {focal_length, width / 2.0, height / 2.0, 0, 0}; +} + +template +void RadialFisheyeCameraModel::WorldToImage(const T* params, const T u, + const T v, T* x, T* y) { + const T f = params[0]; + const T c1 = params[1]; + const T c2 = params[2]; + + // Distortion + T du, dv; + Distortion(¶ms[3], u, v, &du, &dv); + *x = u + du; + *y = v + dv; + + // Transform to image coordinates + *x = f * *x + c1; + *y = f * *y + c2; +} + +template +void RadialFisheyeCameraModel::ImageToWorld(const T* params, const T x, + const T y, T* u, T* v) { + const T f = params[0]; + const T c1 = params[1]; + const T c2 = params[2]; + + // Lift points to normalized plane + *u = (x - c1) / f; + *v = (y - c2) / f; + + IterativeUndistortion(¶ms[3], u, v); +} + +template +void RadialFisheyeCameraModel::Distortion(const T* extra_params, const T u, + const T v, T* du, T* dv) { + const T k1 = extra_params[0]; + const T k2 = extra_params[1]; + + const T r = ceres::sqrt(u * u + v * v); + + if (r > T(std::numeric_limits::epsilon())) { + const T theta = ceres::atan(r); + const T theta2 = theta * theta; + const T theta4 = theta2 * theta2; + const T thetad = theta * (T(1) + k1 * theta2 + k2 * theta4); + *du = u * thetad / r - u; + *dv = v * thetad / r - v; + } else { + *du = T(0); + *dv = T(0); + } +} + +//////////////////////////////////////////////////////////////////////////////// +// ThinPrismFisheyeCameraModel + +std::string ThinPrismFisheyeCameraModel::InitializeParamsInfo() { + return "fx, fy, cx, cy, k1, k2, p1, p2, k3, k4, sx1, sy1"; +} + +std::vector ThinPrismFisheyeCameraModel::InitializeFocalLengthIdxs() { + return {0, 1}; +} + +std::vector +ThinPrismFisheyeCameraModel::InitializePrincipalPointIdxs() { + return {2, 3}; +} + +std::vector ThinPrismFisheyeCameraModel::InitializeExtraParamsIdxs() { + return {4, 5, 6, 7, 8, 9, 10, 11}; +} + +std::vector ThinPrismFisheyeCameraModel::InitializeParams( + const double focal_length, const size_t width, const size_t height) { + return {focal_length, + focal_length, + width / 2.0, + height / 2.0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0}; +} + +template +void ThinPrismFisheyeCameraModel::WorldToImage(const T* params, const T u, + const T v, T* x, T* y) { + const T f1 = params[0]; + const T f2 = params[1]; + const T c1 = params[2]; + const T c2 = params[3]; + + const T r = ceres::sqrt(u * u + v * v); + + T uu, vv; + if (r > T(std::numeric_limits::epsilon())) { + const T theta = ceres::atan(r); + uu = theta * u / r; + vv = theta * v / r; + } else { + uu = u; + vv = v; + } + + // Distortion + T du, dv; + Distortion(¶ms[4], uu, vv, &du, &dv); + *x = uu + du; + *y = vv + dv; + + // Transform to image coordinates + *x = f1 * *x + c1; + *y = f2 * *y + c2; +} + +template +void ThinPrismFisheyeCameraModel::ImageToWorld(const T* params, const T x, + const T y, T* u, T* v) { + const T f1 = params[0]; + const T f2 = params[1]; + const T c1 = params[2]; + const T c2 = params[3]; + + // Lift points to normalized plane + *u = (x - c1) / f1; + *v = (y - c2) / f2; + + IterativeUndistortion(¶ms[4], u, v); + + const T theta = ceres::sqrt(*u * *u + *v * *v); + const T theta_cos_theta = theta * ceres::cos(theta); + if (theta_cos_theta > T(std::numeric_limits::epsilon())) { + const T scale = ceres::sin(theta) / theta_cos_theta; + *u *= scale; + *v *= scale; + } +} + +template +void ThinPrismFisheyeCameraModel::Distortion(const T* extra_params, const T u, + const T v, T* du, T* dv) { + const T k1 = extra_params[0]; + const T k2 = extra_params[1]; + const T p1 = extra_params[2]; + const T p2 = extra_params[3]; + const T k3 = extra_params[4]; + const T k4 = extra_params[5]; + const T sx1 = extra_params[6]; + const T sy1 = extra_params[7]; + + const T u2 = u * u; + const T uv = u * v; + const T v2 = v * v; + const T r2 = u2 + v2; + const T r4 = r2 * r2; + const T r6 = r4 * r2; + const T r8 = r6 * r2; + const T radial = k1 * r2 + k2 * r4 + k3 * r6 + k4 * r8; + *du = u * radial + T(2) * p1 * uv + p2 * (r2 + T(2) * u2) + sx1 * r2; + *dv = v * radial + T(2) * p2 * uv + p1 * (r2 + T(2) * v2) + sy1 * r2; +} + +//////////////////////////////////////////////////////////////////////////////// + +void CameraModelWorldToImage(const int model_id, + const std::vector& params, const double u, + const double v, double* x, double* y) { + switch (model_id) { +#define CAMERA_MODEL_CASE(CameraModel) \ + case CameraModel::kModelId: \ + CameraModel::WorldToImage(params.data(), u, v, x, y); \ + break; + + CAMERA_MODEL_SWITCH_CASES + +#undef CAMERA_MODEL_CASE + } +} + +void CameraModelImageToWorld(const int model_id, + const std::vector& params, const double x, + const double y, double* u, double* v) { + switch (model_id) { +#define CAMERA_MODEL_CASE(CameraModel) \ + case CameraModel::kModelId: \ + CameraModel::ImageToWorld(params.data(), x, y, u, v); \ + break; + + CAMERA_MODEL_SWITCH_CASES + +#undef CAMERA_MODEL_CASE + } +} + +double CameraModelImageToWorldThreshold(const int model_id, + const std::vector& params, + const double threshold) { + switch (model_id) { +#define CAMERA_MODEL_CASE(CameraModel) \ + case CameraModel::kModelId: \ + return CameraModel::ImageToWorldThreshold(params.data(), threshold); \ + break; + + CAMERA_MODEL_SWITCH_CASES + +#undef CAMERA_MODEL_CASE + } + + return -1; +} + +} // namespace colmap + +#endif // COLMAP_SRC_BASE_CAMERA_MODELS_H_ diff --git a/include/colmap/base/camera_rig.h b/include/colmap/base/camera_rig.h new file mode 100644 index 0000000000000000000000000000000000000000..690639d7094d47dc90e01c58e6de7e3f1fb6b305 --- /dev/null +++ b/include/colmap/base/camera_rig.h @@ -0,0 +1,128 @@ +// Copyright (c) 2022, 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_RIG_H_ +#define COLMAP_SRC_BASE_CAMERA_RIG_H_ + +#include +#include + +#include "base/camera.h" +#include "base/pose.h" +#include "base/reconstruction.h" +#include "util/alignment.h" +#include "util/types.h" + +namespace colmap { + +// This class holds information about the relative configuration of camera rigs. +// Camera rigs are composed of multiple cameras with a rigid relative extrinsic +// configuration over multiple snapshots. Snapshots are defined as the +// collection of images captured simultaneously by all cameras in the rig. +class CameraRig { + public: + CameraRig(); + + // The number of cameras in the rig. + size_t NumCameras() const; + + // The number of snapshots captured by this rig. + size_t NumSnapshots() const; + + // Check whether the given camera is part of the rig. + bool HasCamera(const camera_t camera_id) const; + + // Access the reference camera. + camera_t RefCameraId() const; + void SetRefCameraId(const camera_t camera_id); + + // Get the identifiers of the cameras in the rig. + std::vector GetCameraIds() const; + + // Get the snapshots of the camera rig. + const std::vector>& Snapshots() const; + + // Add a new camera to the rig. The relative pose may contain dummy values and + // can then be computed automatically from a given reconstruction using the + // method `ComputeRelativePoses`. + void AddCamera(const camera_t camera_id, const Eigen::Vector4d& rel_qvec, + const Eigen::Vector3d& rel_tvec); + + // Add the images of a single snapshot to rig. A snapshot consists of the + // captured images of all cameras of the rig. All images of a snapshot share + // the same global camera rig pose, i.e. all images in the camera rig are + // captured simultaneously. + void AddSnapshot(const std::vector& image_ids); + + // Check whether the camera rig setup is valid. + void Check(const Reconstruction& reconstruction) const; + + // Get the relative poses of the cameras in the rig. + Eigen::Vector4d& RelativeQvec(const camera_t camera_id); + const Eigen::Vector4d& RelativeQvec(const camera_t camera_id) const; + Eigen::Vector3d& RelativeTvec(const camera_t camera_id); + const Eigen::Vector3d& RelativeTvec(const camera_t camera_id) const; + + // Compute the scaling factor from the reconstruction to the camera rig + // dimensions by averaging over the distances between the projection centers. + // Note that this assumes that there is at least one camera pair in the rig + // with non-zero baseline, otherwise the function returns NaN. + double ComputeScale(const Reconstruction& reconstruction) const; + + // Compute the relative poses in the rig from the reconstruction by averaging + // the relative poses over all snapshots. The pose of the reference camera + // will be the identity transformation. This assumes that the camera rig has + // snapshots that are registered in the reconstruction. + bool ComputeRelativePoses(const Reconstruction& reconstruction); + + // Compute the absolute camera pose of the rig. The absolute camera pose of + // the rig is computed as the average of all relative camera poses in the rig + // and their corresponding image poses in the reconstruction. + void ComputeAbsolutePose(const size_t snapshot_idx, + const Reconstruction& reconstruction, + Eigen::Vector4d* abs_qvec, + Eigen::Vector3d* abs_tvec) const; + + private: + struct RigCamera { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + Eigen::Vector4d rel_qvec = ComposeIdentityQuaternion(); + Eigen::Vector3d rel_tvec = Eigen::Vector3d(0, 0, 0); + }; + + camera_t ref_camera_id_ = kInvalidCameraId; + EIGEN_STL_UMAP(camera_t, RigCamera) rig_cameras_; + std::vector> snapshots_; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_BASE_CAMERA_RIG_H_ diff --git a/include/colmap/base/correspondence_graph.h b/include/colmap/base/correspondence_graph.h new file mode 100644 index 0000000000000000000000000000000000000000..4a71829e34c35eda71672f84d398da9f13dcca66 --- /dev/null +++ b/include/colmap/base/correspondence_graph.h @@ -0,0 +1,206 @@ +// Copyright (c) 2022, 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_CORRESPONDENCE_GRAPH_H_ +#define COLMAP_SRC_BASE_CORRESPONDENCE_GRAPH_H_ + +#include +#include + +#include "base/database.h" +#include "util/types.h" + +namespace colmap { + +// Scene graph represents the graph of image to image and feature to feature +// correspondences of a dataset. It should be accessed from the DatabaseCache. +class CorrespondenceGraph { + public: + struct Correspondence { + Correspondence() + : image_id(kInvalidImageId), point2D_idx(kInvalidPoint2DIdx) {} + Correspondence(const image_t image_id, const point2D_t point2D_idx) + : image_id(image_id), point2D_idx(point2D_idx) {} + + // The identifier of the corresponding image. + image_t image_id; + + // The index of the corresponding point in the corresponding image. + point2D_t point2D_idx; + }; + + CorrespondenceGraph(); + + // Number of added images. + inline size_t NumImages() const; + + // Number of added images. + inline size_t NumImagePairs() const; + + // Check whether image exists. + inline bool ExistsImage(const image_t image_id) const; + + // Get the number of observations in an image. An observation is an image + // point that has at least one correspondence. + inline point2D_t NumObservationsForImage(const image_t image_id) const; + + // Get the number of correspondences per image. + inline point2D_t NumCorrespondencesForImage(const image_t image_id) const; + + // Get the number of correspondences between a pair of images. + inline point2D_t NumCorrespondencesBetweenImages( + const image_t image_id1, const image_t image_id2) const; + + // Get the number of correspondences between all images. + std::unordered_map NumCorrespondencesBetweenImages() + const; + + // Finalize the database manager. + // + // - Calculates the number of observations per image by counting the number + // of image points that have at least one correspondence. + // - Deletes images without observations, as they are useless for SfM. + // - Shrinks the correspondence vectors to their size to save memory. + void Finalize(); + + // Add new image to the correspondence graph. + void AddImage(const image_t image_id, const size_t num_points2D); + + // Add correspondences between images. This function ignores invalid + // correspondences where the point indices are out of bounds or duplicate + // correspondences between the same image points. Whenever either of the two + // cases occur this function prints a warning to the standard output. + void AddCorrespondences(const image_t image_id1, const image_t image_id2, + const FeatureMatches& matches); + + // Find the correspondence of an image observation to all other images. + inline const std::vector& FindCorrespondences( + const image_t image_id, const point2D_t point2D_idx) const; + + // Find correspondences to the given observation. + // + // Transitively collects correspondences to the given observation by first + // finding correspondences to the given observation, then looking for + // correspondences to the collected correspondences in the first step, and so + // forth until the transitivity is exhausted or no more correspondences are + // found. The returned list does not contain duplicates and contains + // the given observation. + void FindTransitiveCorrespondences( + const image_t image_id, const point2D_t point2D_idx, + const size_t transitivity, + std::vector* found_corrs) const; + + // Find all correspondences between two images. + FeatureMatches FindCorrespondencesBetweenImages( + const image_t image_id1, const image_t image_id2) const; + + // Check whether the image point has correspondences. + inline bool HasCorrespondences(const image_t image_id, + const point2D_t point2D_idx) const; + + // Check whether the given observation is part of a two-view track, i.e. + // it only has one correspondence and that correspondence has the given + // observation as its only correspondence. + bool IsTwoViewObservation(const image_t image_id, + const point2D_t point2D_idx) const; + + private: + struct Image { + // Number of 2D points with at least one correspondence to another image. + point2D_t num_observations = 0; + + // Total number of correspondences to other images. This measure is useful + // to find a good initial pair, that is connected to many images. + point2D_t num_correspondences = 0; + + // Correspondences to other images per image point. + std::vector> corrs; + }; + + struct ImagePair { + // The number of correspondences between pairs of images. + point2D_t num_correspondences = 0; + }; + + EIGEN_STL_UMAP(image_t, Image) images_; + std::unordered_map image_pairs_; +}; + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +size_t CorrespondenceGraph::NumImages() const { return images_.size(); } + +size_t CorrespondenceGraph::NumImagePairs() const { + return image_pairs_.size(); +} + +bool CorrespondenceGraph::ExistsImage(const image_t image_id) const { + return images_.find(image_id) != images_.end(); +} + +point2D_t CorrespondenceGraph::NumObservationsForImage( + const image_t image_id) const { + return images_.at(image_id).num_observations; +} + +point2D_t CorrespondenceGraph::NumCorrespondencesForImage( + const image_t image_id) const { + return images_.at(image_id).num_correspondences; +} + +point2D_t CorrespondenceGraph::NumCorrespondencesBetweenImages( + const image_t image_id1, const image_t image_id2) const { + const image_pair_t pair_id = + Database::ImagePairToPairId(image_id1, image_id2); + const auto it = image_pairs_.find(pair_id); + if (it == image_pairs_.end()) { + return 0; + } else { + return static_cast(it->second.num_correspondences); + } +} + +const std::vector& +CorrespondenceGraph::FindCorrespondences(const image_t image_id, + const point2D_t point2D_idx) const { + return images_.at(image_id).corrs.at(point2D_idx); +} + +bool CorrespondenceGraph::HasCorrespondences( + const image_t image_id, const point2D_t point2D_idx) const { + return !images_.at(image_id).corrs.at(point2D_idx).empty(); +} + +} // namespace colmap + +#endif // COLMAP_SRC_BASE_CORRESPONDENCE_GRAPH_H_ diff --git a/include/colmap/base/cost_functions.h b/include/colmap/base/cost_functions.h new file mode 100644 index 0000000000000000000000000000000000000000..3be1e12d6c2c87a7274dc67815463ec84b6f27f6 --- /dev/null +++ b/include/colmap/base/cost_functions.h @@ -0,0 +1,301 @@ +// Copyright (c) 2022, 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_COST_FUNCTIONS_H_ +#define COLMAP_SRC_BASE_COST_FUNCTIONS_H_ + +#include + +#include +#include + +namespace colmap { + +// Standard bundle adjustment cost function for variable +// camera pose and calibration and point parameters. +template +class BundleAdjustmentCostFunction { + public: + explicit BundleAdjustmentCostFunction(const Eigen::Vector2d& point2D) + : observed_x_(point2D(0)), observed_y_(point2D(1)) {} + + static ceres::CostFunction* Create(const Eigen::Vector2d& point2D) { + return (new ceres::AutoDiffCostFunction< + BundleAdjustmentCostFunction, 2, 4, 3, 3, + CameraModel::kNumParams>( + new BundleAdjustmentCostFunction(point2D))); + } + + template + bool operator()(const T* const qvec, const T* const tvec, + const T* const point3D, const T* const camera_params, + T* residuals) const { + // Rotate and translate. + T projection[3]; + ceres::UnitQuaternionRotatePoint(qvec, point3D, projection); + projection[0] += tvec[0]; + projection[1] += tvec[1]; + projection[2] += tvec[2]; + + // Project to image plane. + projection[0] /= projection[2]; + projection[1] /= projection[2]; + + // Distort and transform to pixel space. + CameraModel::WorldToImage(camera_params, projection[0], projection[1], + &residuals[0], &residuals[1]); + + // Re-projection error. + residuals[0] -= T(observed_x_); + residuals[1] -= T(observed_y_); + + return true; + } + + private: + const double observed_x_; + const double observed_y_; +}; + +// Bundle adjustment cost function for variable +// camera calibration and point parameters, and fixed camera pose. +template +class BundleAdjustmentConstantPoseCostFunction { + public: + BundleAdjustmentConstantPoseCostFunction(const Eigen::Vector4d& qvec, + const Eigen::Vector3d& tvec, + const Eigen::Vector2d& point2D) + : qw_(qvec(0)), + qx_(qvec(1)), + qy_(qvec(2)), + qz_(qvec(3)), + tx_(tvec(0)), + ty_(tvec(1)), + tz_(tvec(2)), + observed_x_(point2D(0)), + observed_y_(point2D(1)) {} + + static ceres::CostFunction* Create(const Eigen::Vector4d& qvec, + const Eigen::Vector3d& tvec, + const Eigen::Vector2d& point2D) { + return (new ceres::AutoDiffCostFunction< + BundleAdjustmentConstantPoseCostFunction, 2, 3, + CameraModel::kNumParams>( + new BundleAdjustmentConstantPoseCostFunction(qvec, tvec, point2D))); + } + + template + bool operator()(const T* const point3D, const T* const camera_params, + T* residuals) const { + const T qvec[4] = {T(qw_), T(qx_), T(qy_), T(qz_)}; + + // Rotate and translate. + T projection[3]; + ceres::UnitQuaternionRotatePoint(qvec, point3D, projection); + projection[0] += T(tx_); + projection[1] += T(ty_); + projection[2] += T(tz_); + + // Project to image plane. + projection[0] /= projection[2]; + projection[1] /= projection[2]; + + // Distort and transform to pixel space. + CameraModel::WorldToImage(camera_params, projection[0], projection[1], + &residuals[0], &residuals[1]); + + // Re-projection error. + residuals[0] -= T(observed_x_); + residuals[1] -= T(observed_y_); + + return true; + } + + private: + const double qw_; + const double qx_; + const double qy_; + const double qz_; + const double tx_; + const double ty_; + const double tz_; + const double observed_x_; + const double observed_y_; +}; + +// Rig bundle adjustment cost function for variable camera pose and calibration +// and point parameters. Different from the standard bundle adjustment function, +// this cost function is suitable for camera rigs with consistent relative poses +// of the cameras within the rig. The cost function first projects points into +// the local system of the camera rig and then into the local system of the +// camera within the rig. +template +class RigBundleAdjustmentCostFunction { + public: + explicit RigBundleAdjustmentCostFunction(const Eigen::Vector2d& point2D) + : observed_x_(point2D(0)), observed_y_(point2D(1)) {} + + static ceres::CostFunction* Create(const Eigen::Vector2d& point2D) { + return (new ceres::AutoDiffCostFunction< + RigBundleAdjustmentCostFunction, 2, 4, 3, 4, 3, 3, + CameraModel::kNumParams>( + new RigBundleAdjustmentCostFunction(point2D))); + } + + template + bool operator()(const T* const rig_qvec, const T* const rig_tvec, + const T* const rel_qvec, const T* const rel_tvec, + const T* const point3D, const T* const camera_params, + T* residuals) const { + // Concatenate rotations. + T qvec[4]; + ceres::QuaternionProduct(rel_qvec, rig_qvec, qvec); + + // Concatenate translations. + T tvec[3]; + ceres::UnitQuaternionRotatePoint(rel_qvec, rig_tvec, tvec); + tvec[0] += rel_tvec[0]; + tvec[1] += rel_tvec[1]; + tvec[2] += rel_tvec[2]; + + // Rotate and translate. + T projection[3]; + ceres::UnitQuaternionRotatePoint(qvec, point3D, projection); + projection[0] += tvec[0]; + projection[1] += tvec[1]; + projection[2] += tvec[2]; + + // Project to image plane. + projection[0] /= projection[2]; + projection[1] /= projection[2]; + + // Distort and transform to pixel space. + CameraModel::WorldToImage(camera_params, projection[0], projection[1], + &residuals[0], &residuals[1]); + + // Re-projection error. + residuals[0] -= T(observed_x_); + residuals[1] -= T(observed_y_); + + return true; + } + + private: + const double observed_x_; + const double observed_y_; +}; + +// Cost function for refining two-view geometry based on the Sampson-Error. +// +// First pose is assumed to be located at the origin with 0 rotation. Second +// pose is assumed to be on the unit sphere around the first pose, i.e. the +// pose of the second camera is parameterized by a 3D rotation and a +// 3D translation with unit norm. `tvec` is therefore over-parameterized as is +// and should be down-projected using `SphereManifold`. +class RelativePoseCostFunction { + public: + RelativePoseCostFunction(const Eigen::Vector2d& x1, const Eigen::Vector2d& x2) + : x1_(x1(0)), y1_(x1(1)), x2_(x2(0)), y2_(x2(1)) {} + + static ceres::CostFunction* Create(const Eigen::Vector2d& x1, + const Eigen::Vector2d& x2) { + return (new ceres::AutoDiffCostFunction( + new RelativePoseCostFunction(x1, x2))); + } + + template + bool operator()(const T* const qvec, const T* const tvec, + T* residuals) const { + Eigen::Matrix R; + ceres::QuaternionToRotation(qvec, R.data()); + + // Matrix representation of the cross product t x R. + Eigen::Matrix t_x; + t_x << T(0), -tvec[2], tvec[1], tvec[2], T(0), -tvec[0], -tvec[1], tvec[0], + T(0); + + // Essential matrix. + const Eigen::Matrix E = t_x * R; + + // Homogeneous image coordinates. + const Eigen::Matrix x1_h(T(x1_), T(y1_), T(1)); + const Eigen::Matrix x2_h(T(x2_), T(y2_), T(1)); + + // Squared sampson error. + const Eigen::Matrix Ex1 = E * x1_h; + const Eigen::Matrix Etx2 = E.transpose() * x2_h; + const T x2tEx1 = x2_h.transpose() * Ex1; + residuals[0] = x2tEx1 * x2tEx1 / + (Ex1(0) * Ex1(0) + Ex1(1) * Ex1(1) + Etx2(0) * Etx2(0) + + Etx2(1) * Etx2(1)); + + return true; + } + + private: + const double x1_; + const double y1_; + const double x2_; + const double y2_; +}; + +inline void SetQuaternionManifold(ceres::Problem* problem, double* qvec) { +#if CERES_VERSION_MAJOR >= 2 && CERES_VERSION_MINOR >= 1 + problem->SetManifold(qvec, new ceres::QuaternionManifold); +#else + problem->SetParameterization(qvec, new ceres::QuaternionParameterization); +#endif +} + +inline void SetSubsetManifold(int size, const std::vector& constant_params, + ceres::Problem* problem, double* params) { +#if CERES_VERSION_MAJOR >= 2 && CERES_VERSION_MINOR >= 1 + problem->SetManifold(params, + new ceres::SubsetManifold(size, constant_params)); +#else + problem->SetParameterization( + params, new ceres::SubsetParameterization(size, constant_params)); +#endif +} + +template +inline void SetSphereManifold(ceres::Problem* problem, double* params) { +#if CERES_VERSION_MAJOR >= 2 && CERES_VERSION_MINOR >= 1 + problem->SetManifold(params, new ceres::SphereManifold); +#else + problem->SetParameterization( + params, new ceres::HomogeneousVectorParameterization(size)); +#endif +} + +} // namespace colmap + +#endif // COLMAP_SRC_BASE_COST_FUNCTIONS_H_ diff --git a/include/colmap/base/database.h b/include/colmap/base/database.h new file mode 100644 index 0000000000000000000000000000000000000000..dc150fe5aec80541577052939488696081b0f3e8 --- /dev/null +++ b/include/colmap/base/database.h @@ -0,0 +1,394 @@ +// Copyright (c) 2022, 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_DATABASE_H_ +#define COLMAP_SRC_BASE_DATABASE_H_ + +#include +#include +#include + +#include + +#include "SQLite/sqlite3.h" +#include "base/camera.h" +#include "base/image.h" +#include "estimators/two_view_geometry.h" +#include "feature/types.h" +#include "util/types.h" + +namespace colmap { + +// Database class to read and write images, features, cameras, matches, etc. +// from a SQLite database. The class is not thread-safe and must not be accessed +// concurrently. The class is optimized for single-thread speed and for optimal +// performance, wrap multiple method calls inside a leading `BeginTransaction` +// and trailing `EndTransaction`. +class Database { + public: + const static int kSchemaVersion = 1; + + // The maximum number of images, that can be stored in the database. + // This limitation arises due to the fact, that we generate unique IDs for + // image pairs manually. Note: do not change this to + // another type than `size_t`. + const static size_t kMaxNumImages; + + Database(); + explicit Database(const std::string& path); + ~Database(); + + // Open and close database. The same database should not be opened + // concurrently in multiple threads or processes. + void Open(const std::string& path); + void Close(); + + // Check if entry already exists in database. For image pairs, the order of + // `image_id1` and `image_id2` does not matter. + bool ExistsCamera(const camera_t camera_id) const; + bool ExistsImage(const image_t image_id) const; + bool ExistsImageWithName(std::string name) const; + bool ExistsKeypoints(const image_t image_id) const; + bool ExistsDescriptors(const image_t image_id) const; + bool ExistsMatches(const image_t image_id1, const image_t image_id2) const; + bool ExistsInlierMatches(const image_t image_id1, + const image_t image_id2) const; + + // Number of rows in `cameras` table. + size_t NumCameras() const; + + // Number of rows in `images` table. + size_t NumImages() const; + + // Sum of `rows` column in `keypoints` table, i.e. number of total keypoints. + size_t NumKeypoints() const; + + // The number of keypoints for the image with most features. + size_t MaxNumKeypoints() const; + + // Number of descriptors for specific image. + size_t NumKeypointsForImage(const image_t image_id) const; + + // Sum of `rows` column in `descriptors` table, + // i.e. number of total descriptors. + size_t NumDescriptors() const; + + // The number of descriptors for the image with most features. + size_t MaxNumDescriptors() const; + + // Number of descriptors for specific image. + size_t NumDescriptorsForImage(const image_t image_id) const; + + // Sum of `rows` column in `matches` table, i.e. number of total matches. + size_t NumMatches() const; + + // Sum of `rows` column in `two_view_geometries` table, + // i.e. number of total inlier matches. + size_t NumInlierMatches() const; + + // Number of rows in `matches` table. + size_t NumMatchedImagePairs() const; + + // Number of rows in `two_view_geometries` table. + size_t NumVerifiedImagePairs() const; + + // Each image pair is assigned an unique ID in the `matches` and + // `two_view_geometries` table. We intentionally avoid to store the pairs in a + // separate table by using e.g. AUTOINCREMENT, since the overhead of querying + // the unique pair ID is significant. + inline static image_pair_t ImagePairToPairId(const image_t image_id1, + const image_t image_id2); + + inline static void PairIdToImagePair(const image_pair_t pair_id, + image_t* image_id1, image_t* image_id2); + + // Return true if image pairs should be swapped. Used to enforce a specific + // image order to generate unique image pair identifiers independent of the + // order in which the image identifiers are used. + inline static bool SwapImagePair(const image_t image_id1, + const image_t image_id2); + + // Read an existing entry in the database. The user is responsible for making + // sure that the entry actually exists. For image pairs, the order of + // `image_id1` and `image_id2` does not matter. + Camera ReadCamera(const camera_t camera_id) const; + std::vector ReadAllCameras() const; + + Image ReadImage(const image_t image_id) const; + Image ReadImageWithName(const std::string& name) const; + std::vector ReadAllImages() const; + + FeatureKeypoints ReadKeypoints(const image_t image_id) const; + FeatureDescriptors ReadDescriptors(const image_t image_id) const; + + FeatureMatches ReadMatches(const image_t image_id1, + const image_t image_id2) const; + std::vector> ReadAllMatches() const; + + TwoViewGeometry ReadTwoViewGeometry(const image_t image_id1, + const image_t image_id2) const; + void ReadTwoViewGeometries( + std::vector* image_pair_ids, + std::vector* two_view_geometries) const; + + // Read all image pairs that have an entry in the `NumVerifiedImagePairs` + // table with at least one inlier match and their number of inlier matches. + void ReadTwoViewGeometryNumInliers( + std::vector>* image_pairs, + std::vector* num_inliers) const; + + // Add new camera and return its database identifier. If `use_camera_id` + // is false a new identifier is automatically generated. + camera_t WriteCamera(const Camera& camera, + const bool use_camera_id = false) const; + + // Add new image and return its database identifier. If `use_image_id` + // is false a new identifier is automatically generated. + image_t WriteImage(const Image& image, const bool use_image_id = false) const; + + // Write a new entry in the database. The user is responsible for making sure + // that the entry does not yet exist. For image pairs, the order of + // `image_id1` and `image_id2` does not matter. + void WriteKeypoints(const image_t image_id, + const FeatureKeypoints& keypoints) const; + void WriteDescriptors(const image_t image_id, + const FeatureDescriptors& descriptors) const; + void WriteMatches(const image_t image_id1, const image_t image_id2, + const FeatureMatches& matches) const; + void WriteTwoViewGeometry(const image_t image_id1, const image_t image_id2, + const TwoViewGeometry& two_view_geometry) const; + + // Update an existing camera in the database. The user is responsible for + // making sure that the entry already exists. + void UpdateCamera(const Camera& camera) const; + + // Update an existing image in the database. The user is responsible for + // making sure that the entry already exists. + void UpdateImage(const Image& image) const; + + // Delete matches of an image pair. + void DeleteMatches(const image_t image_id1, const image_t image_id2) const; + + // Delete inlier matches of an image pair. + void DeleteInlierMatches(const image_t image_id1, + const image_t image_id2) const; + + // Clear all database tables + void ClearAllTables() const; + + // Clear the entire cameras table + void ClearCameras() const; + + // Clear the entire images, keypoints, and descriptors tables + void ClearImages() const; + + // Clear the entire descriptors table + void ClearDescriptors() const; + + // Clear the entire keypoints table + void ClearKeypoints() const; + + // Clear the entire matches table. + void ClearMatches() const; + + // Clear the entire inlier matches table. + void ClearTwoViewGeometries() const; + + // Merge two databases into a single, new database. + static void Merge(const Database& database1, const Database& database2, + Database* merged_database); + + private: + friend class DatabaseTransaction; + + // Combine multiple queries into one transaction by wrapping a code section + // into a `BeginTransaction` and `EndTransaction`. You can create a scoped + // transaction with `DatabaseTransaction` that ends when the transaction + // object is destructed. Combining queries results in faster transaction time + // due to reduced locking of the database etc. + void BeginTransaction() const; + void EndTransaction() const; + + // Prepare SQL statements once at construction of the database, and reuse + // the statements for multiple queries by resetting their states. + void PrepareSQLStatements(); + void FinalizeSQLStatements(); + + // Create database tables, if not existing, called when opening a database. + void CreateTables() const; + void CreateCameraTable() const; + void CreateImageTable() const; + void CreateKeypointsTable() const; + void CreateDescriptorsTable() const; + void CreateMatchesTable() const; + void CreateTwoViewGeometriesTable() const; + + void UpdateSchema() const; + + bool ExistsTable(const std::string& table_name) const; + bool ExistsColumn(const std::string& table_name, + const std::string& column_name) const; + + bool ExistsRowId(sqlite3_stmt* sql_stmt, const sqlite3_int64 row_id) const; + bool ExistsRowString(sqlite3_stmt* sql_stmt, + const std::string& row_entry) const; + + size_t CountRows(const std::string& table) const; + size_t CountRowsForEntry(sqlite3_stmt* sql_stmt, + const sqlite3_int64 row_id) const; + size_t SumColumn(const std::string& column, const std::string& table) const; + size_t MaxColumn(const std::string& column, const std::string& table) const; + + sqlite3* database_ = nullptr; + + // Check if elements got removed from the database to only apply + // the VACUUM command in such case + mutable bool database_cleared_ = false; + + // Ensure that only one database object at a time updates the schema of a + // database. Since the schema is updated every time a database is opened, this + // is to ensure that there are no race conditions ("database locked" error + // messages) when the user actually only intends to read from the database, + // which requires to open it. + static std::mutex update_schema_mutex_; + + // Used to ensure that only one transaction is active at the same time. + std::mutex transaction_mutex_; + + // A collection of all `sqlite3_stmt` objects for deletion in the destructor. + std::vector sql_stmts_; + + // num_* + sqlite3_stmt* sql_stmt_num_keypoints_ = nullptr; + sqlite3_stmt* sql_stmt_num_descriptors_ = nullptr; + + // exists_* + sqlite3_stmt* sql_stmt_exists_camera_ = nullptr; + sqlite3_stmt* sql_stmt_exists_image_id_ = nullptr; + sqlite3_stmt* sql_stmt_exists_image_name_ = nullptr; + sqlite3_stmt* sql_stmt_exists_keypoints_ = nullptr; + sqlite3_stmt* sql_stmt_exists_descriptors_ = nullptr; + sqlite3_stmt* sql_stmt_exists_matches_ = nullptr; + sqlite3_stmt* sql_stmt_exists_two_view_geometry_ = nullptr; + + // add_* + sqlite3_stmt* sql_stmt_add_camera_ = nullptr; + sqlite3_stmt* sql_stmt_add_image_ = nullptr; + + // update_* + sqlite3_stmt* sql_stmt_update_camera_ = nullptr; + sqlite3_stmt* sql_stmt_update_image_ = nullptr; + + // read_* + sqlite3_stmt* sql_stmt_read_camera_ = nullptr; + sqlite3_stmt* sql_stmt_read_cameras_ = nullptr; + sqlite3_stmt* sql_stmt_read_image_id_ = nullptr; + sqlite3_stmt* sql_stmt_read_image_name_ = nullptr; + sqlite3_stmt* sql_stmt_read_images_ = nullptr; + sqlite3_stmt* sql_stmt_read_keypoints_ = nullptr; + sqlite3_stmt* sql_stmt_read_descriptors_ = nullptr; + sqlite3_stmt* sql_stmt_read_matches_ = nullptr; + sqlite3_stmt* sql_stmt_read_matches_all_ = nullptr; + sqlite3_stmt* sql_stmt_read_two_view_geometry_ = nullptr; + sqlite3_stmt* sql_stmt_read_two_view_geometries_ = nullptr; + sqlite3_stmt* sql_stmt_read_two_view_geometry_num_inliers_ = nullptr; + + // write_* + sqlite3_stmt* sql_stmt_write_keypoints_ = nullptr; + sqlite3_stmt* sql_stmt_write_descriptors_ = nullptr; + sqlite3_stmt* sql_stmt_write_matches_ = nullptr; + sqlite3_stmt* sql_stmt_write_two_view_geometry_ = nullptr; + + // delete_* + sqlite3_stmt* sql_stmt_delete_matches_ = nullptr; + sqlite3_stmt* sql_stmt_delete_two_view_geometry_ = nullptr; + + // clear_* + sqlite3_stmt* sql_stmt_clear_cameras_ = nullptr; + sqlite3_stmt* sql_stmt_clear_images_ = nullptr; + sqlite3_stmt* sql_stmt_clear_descriptors_ = nullptr; + sqlite3_stmt* sql_stmt_clear_keypoints_ = nullptr; + sqlite3_stmt* sql_stmt_clear_matches_ = nullptr; + sqlite3_stmt* sql_stmt_clear_two_view_geometries_ = nullptr; +}; + +// This class automatically manages the scope of a database transaction by +// calling `BeginTransaction` and `EndTransaction` during construction and +// destruction, respectively. +class DatabaseTransaction { + public: + explicit DatabaseTransaction(Database* database); + ~DatabaseTransaction(); + + private: + NON_COPYABLE(DatabaseTransaction) + NON_MOVABLE(DatabaseTransaction) + Database* database_; + std::unique_lock database_lock_; +}; + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +image_pair_t Database::ImagePairToPairId(const image_t image_id1, + const image_t image_id2) { + CHECK_GE(image_id1, 0); + CHECK_GE(image_id2, 0); + CHECK_LT(image_id1, kMaxNumImages); + CHECK_LT(image_id2, kMaxNumImages); + if (SwapImagePair(image_id1, image_id2)) { + return static_cast(kMaxNumImages) * image_id2 + image_id1; + } else { + return static_cast(kMaxNumImages) * image_id1 + image_id2; + } +} + +void Database::PairIdToImagePair(const image_pair_t pair_id, image_t* image_id1, + image_t* image_id2) { + *image_id2 = static_cast(pair_id % kMaxNumImages); + *image_id1 = static_cast((pair_id - *image_id2) / kMaxNumImages); + CHECK_GE(*image_id1, 0); + CHECK_GE(*image_id2, 0); + CHECK_LT(*image_id1, kMaxNumImages); + CHECK_LT(*image_id2, kMaxNumImages); +} + +// Return true if image pairs should be swapped. Used to enforce a specific +// image order to generate unique image pair identifiers independent of the +// order in which the image identifiers are used. +bool Database::SwapImagePair(const image_t image_id1, const image_t image_id2) { + return image_id1 > image_id2; +} + +} // namespace colmap + +#endif // COLMAP_SRC_BASE_DATABASE_H_ diff --git a/include/colmap/base/database_cache.h b/include/colmap/base/database_cache.h new file mode 100644 index 0000000000000000000000000000000000000000..d728a040dc51a7daa5b848ffddbadac60728a0ef --- /dev/null +++ b/include/colmap/base/database_cache.h @@ -0,0 +1,151 @@ +// Copyright (c) 2022, 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_DATABASE_CACHE_H_ +#define COLMAP_SRC_BASE_DATABASE_CACHE_H_ + +#include +#include +#include +#include + +#include + +#include "base/camera.h" +#include "base/camera_models.h" +#include "base/correspondence_graph.h" +#include "base/database.h" +#include "base/image.h" +#include "util/alignment.h" +#include "util/types.h" + +namespace colmap { + +// A class that caches the contents of the database in memory, used to quickly +// create new reconstruction instances when multiple models are reconstructed. +class DatabaseCache { + public: + DatabaseCache(); + + // Get number of objects. + inline size_t NumCameras() const; + inline size_t NumImages() const; + + // Get specific objects. + inline class Camera& Camera(const camera_t camera_id); + inline const class Camera& Camera(const camera_t camera_id) const; + inline class Image& Image(const image_t image_id); + inline const class Image& Image(const image_t image_id) const; + + // Get all objects. + inline const EIGEN_STL_UMAP(camera_t, class Camera) & Cameras() const; + inline const EIGEN_STL_UMAP(image_t, class Image) & Images() const; + + // Check whether specific object exists. + inline bool ExistsCamera(const camera_t camera_id) const; + inline bool ExistsImage(const image_t image_id) const; + + // Get reference to correspondence graph. + inline const class CorrespondenceGraph& CorrespondenceGraph() const; + + // Manually add data to cache. + void AddCamera(class Camera camera); + void AddImage(class Image image); + + // Load cameras, images, features, and matches from database. + // + // @param database Source database from which to load data. + // @param min_num_matches Only load image pairs with a minimum number + // of matches. + // @param ignore_watermarks Whether to ignore watermark image pairs. + // @param image_names Whether to use only load the data for a subset + // of the images. All images are used if empty. + void Load(const Database& database, const size_t min_num_matches, + const bool ignore_watermarks, + const std::unordered_set& image_names); + + // Find specific image by name. Note that this uses linear search. + const class Image* FindImageWithName(const std::string& name) const; + + private: + class CorrespondenceGraph correspondence_graph_; + + EIGEN_STL_UMAP(camera_t, class Camera) cameras_; + EIGEN_STL_UMAP(image_t, class Image) images_; +}; + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +size_t DatabaseCache::NumCameras() const { return cameras_.size(); } +size_t DatabaseCache::NumImages() const { return images_.size(); } + +class Camera& DatabaseCache::Camera(const camera_t camera_id) { + return cameras_.at(camera_id); +} + +const class Camera& DatabaseCache::Camera(const camera_t camera_id) const { + return cameras_.at(camera_id); +} + +class Image& DatabaseCache::Image(const image_t image_id) { + return images_.at(image_id); +} + +const class Image& DatabaseCache::Image(const image_t image_id) const { + return images_.at(image_id); +} + +const EIGEN_STL_UMAP(camera_t, class Camera) & DatabaseCache::Cameras() const { + return cameras_; +} + +const EIGEN_STL_UMAP(image_t, class Image) & DatabaseCache::Images() const { + return images_; +} + +bool DatabaseCache::ExistsCamera(const camera_t camera_id) const { + return cameras_.find(camera_id) != cameras_.end(); +} + +bool DatabaseCache::ExistsImage(const image_t image_id) const { + return images_.find(image_id) != images_.end(); +} + +inline const class CorrespondenceGraph& DatabaseCache::CorrespondenceGraph() + const { + return correspondence_graph_; +} + +} // namespace colmap + +#endif // COLMAP_SRC_BASE_DATABASE_CACHE_H_ diff --git a/include/colmap/base/essential_matrix.h b/include/colmap/base/essential_matrix.h new file mode 100644 index 0000000000000000000000000000000000000000..bea1a9d44049e9276ffa4854ff03e8ecdaf1ed9d --- /dev/null +++ b/include/colmap/base/essential_matrix.h @@ -0,0 +1,160 @@ +// Copyright (c) 2022, 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_ESSENTIAL_MATRIX_H_ +#define COLMAP_SRC_BASE_ESSENTIAL_MATRIX_H_ + +#include + +#include + +#include + +#include "util/alignment.h" +#include "util/types.h" + +namespace colmap { + +// Decompose an essential matrix into the possible rotations and translations. +// +// The first pose is assumed to be P = [I | 0] and the set of four other +// possible second poses are defined as: {[R1 | t], [R2 | t], +// [R1 | -t], [R2 | -t]} +// +// @param E 3x3 essential matrix. +// @param R1 First possible 3x3 rotation matrix. +// @param R2 Second possible 3x3 rotation matrix. +// @param t 3x1 possible translation vector (also -t possible). +void DecomposeEssentialMatrix(const Eigen::Matrix3d& E, Eigen::Matrix3d* R1, + Eigen::Matrix3d* R2, Eigen::Vector3d* t); + +// Recover the most probable pose from the given essential matrix. +// +// The pose of the first image is assumed to be P = [I | 0]. +// +// @param E 3x3 essential matrix. +// @param points1 First set of corresponding points. +// @param points2 Second set of corresponding points. +// @param inlier_mask Only points with `true` in the inlier mask are +// considered in the cheirality test. Size of the +// inlier mask must match the number of points N. +// @param R Most probable 3x3 rotation matrix. +// @param t Most probable 3x1 translation vector. +// @param points3D Triangulated 3D points infront of camera. +void PoseFromEssentialMatrix(const Eigen::Matrix3d& E, + const std::vector& points1, + const std::vector& points2, + Eigen::Matrix3d* R, Eigen::Vector3d* t, + std::vector* points3D); + +// Compose essential matrix from relative camera poses. +// +// Assumes that first camera pose has projection matrix P = [I | 0], and +// pose of second camera is given as transformation from world to camera system. +// +// @param R 3x3 rotation matrix. +// @param t 3x1 translation vector. +// +// @return 3x3 essential matrix. +Eigen::Matrix3d EssentialMatrixFromPose(const Eigen::Matrix3d& R, + const Eigen::Vector3d& t); + +// Compose essential matrix from two absolute camera poses. +// +// @param proj_matrix1 3x4 projection matrix. +// @param proj_matrix2 3x4 projection matrix. +// +// @return 3x3 essential matrix. +Eigen::Matrix3d EssentialMatrixFromAbsolutePoses( + const Eigen::Matrix3x4d& proj_matrix1, + const Eigen::Matrix3x4d& proj_matrix2); + +// Find optimal image points, such that: +// +// optimal_point1^t * E * optimal_point2 = 0 +// +// as described in: +// +// Lindstrom, P., "Triangulation made easy", +// Computer Vision and Pattern Recognition (CVPR), +// 2010 IEEE Conference on , vol., no., pp.1554,1561, 13-18 June 2010 +// +// @param E Essential or fundamental matrix. +// @param point1 Corresponding 2D point in first image. +// @param point2 Corresponding 2D point in second image. +// @param optimal_point1 Estimated optimal image point in the first image. +// @param optimal_point2 Estimated optimal image point in the second image. +void FindOptimalImageObservations(const Eigen::Matrix3d& E, + const Eigen::Vector2d& point1, + const Eigen::Vector2d& point2, + Eigen::Vector2d* optimal_point1, + Eigen::Vector2d* optimal_point2); + +// Compute the location of the epipole in homogeneous coordinates. +// +// @param E 3x3 essential matrix. +// @param left_image If true, epipole in left image is computed, +// else in right image. +// +// @return Epipole in homogeneous coordinates. +Eigen::Vector3d EpipoleFromEssentialMatrix(const Eigen::Matrix3d& E, + const bool left_image); + +// Invert the essential matrix, i.e. if the essential matrix E describes the +// transformation from camera A to B, the inverted essential matrix E' describes +// the transformation from camera B to A. +// +// @param E 3x3 essential matrix. +// +// @return Inverted essential matrix. +Eigen::Matrix3d InvertEssentialMatrix(const Eigen::Matrix3d& matrix); + +// Refine essential matrix. +// +// Decomposes the essential matrix into rotation and translation components +// and refines the relative pose using the function `RefineRelativePose`. +// +// @param E 3x3 essential matrix. +// @param points1 First set of corresponding points. +// @param points2 Second set of corresponding points. +// @param inlier_mask Inlier mask for corresponding points. +// @param options Solver options. +// +// @return Flag indicating if solution is usable. +bool RefineEssentialMatrix(const ceres::Solver::Options& options, + const std::vector& points1, + const std::vector& points2, + const std::vector& inlier_mask, + Eigen::Matrix3d* E); + +} // namespace colmap + +#endif // COLMAP_SRC_BASE_ESSENTIAL_MATRIX_H_ diff --git a/include/colmap/base/gps.h b/include/colmap/base/gps.h new file mode 100644 index 0000000000000000000000000000000000000000..0f76f05e67bbb96a7642f7e34628cd0439bb2173 --- /dev/null +++ b/include/colmap/base/gps.h @@ -0,0 +1,89 @@ +// Copyright (c) 2022, 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_GPS_H_ +#define COLMAP_SRC_BASE_GPS_H_ + +#include + +#include + +#include "util/alignment.h" +#include "util/types.h" + +namespace colmap { + +// Transform ellipsoidal GPS coordinates to Cartesian GPS coordinate +// representation and vice versa. +class GPSTransform { + public: + enum ELLIPSOID { GRS80, WGS84 }; + + explicit GPSTransform(const int ellipsoid = GRS80); + + std::vector EllToXYZ( + const std::vector& ell) const; + + std::vector XYZToEll( + const std::vector& xyz) const; + + // Convert GPS (lat / lon / alt) to ENU coords. with lat0 and lon0 + // defining the origin of the ENU frame + std::vector EllToENU(const std::vector& ell, + const double lat0, + const double lon0) const; + + std::vector XYZToENU(const std::vector& xyz, + const double lat0, + const double lon0) const; + + std::vector ENUToEll(const std::vector& enu, + const double lat0, const double lon0, + const double alt0) const; + + std::vector ENUToXYZ(const std::vector& enu, + const double lat0, const double lon0, + const double alt0) const; + + private: + // Semimajor axis. + double a_; + // Semiminor axis. + double b_; + // Flattening. + double f_; + // Numerical eccentricity. + double e2_; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_BASE_GPS_H_ diff --git a/include/colmap/base/graph_cut.h b/include/colmap/base/graph_cut.h new file mode 100644 index 0000000000000000000000000000000000000000..80e87f26cb6f9cd46a9a1479bcf20a4372ac78b1 --- /dev/null +++ b/include/colmap/base/graph_cut.h @@ -0,0 +1,209 @@ +// Copyright (c) 2022, 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_GRAPH_CUT_H_ +#define COLMAP_SRC_BASE_GRAPH_CUT_H_ + +#include +#include + +#include +#include +#include +#include + +#include "util/logging.h" + +namespace colmap { + +// Compute the min-cut of a undirected graph using the Stoer Wagner algorithm. +void ComputeMinGraphCutStoerWagner( + const std::vector>& edges, + const std::vector& weights, int* cut_weight, + std::vector* cut_labels); + +// Compute the normalized min-cut of an undirected graph using Metis. +// Partitions the graph into clusters and returns the cluster labels per vertex. +std::unordered_map ComputeNormalizedMinGraphCut( + const std::vector>& edges, + const std::vector& weights, const int num_parts); + +// Compute the minimum graph cut of a directed S-T graph using the +// Boykov-Kolmogorov max-flow min-cut algorithm, as descibed in: +// "An Experimental Comparison of Min-Cut/Max-Flow Algorithms for Energy +// Minimization in Vision". Yuri Boykov and Vladimir Kolmogorov. PAMI, 2004. +template +class MinSTGraphCut { + public: + typedef boost::adjacency_list_traits + graph_traits_t; + typedef graph_traits_t::edge_descriptor edge_descriptor_t; + typedef graph_traits_t::vertices_size_type vertices_size_t; + + struct Edge { + value_t capacity; + value_t residual; + edge_descriptor_t reverse; + }; + + typedef boost::adjacency_list + graph_t; + + MinSTGraphCut(const size_t num_nodes); + + // Count the number of nodes and edges in the graph. + size_t NumNodes() const; + size_t NumEdges() const; + + // Add node to the graph. + void AddNode(const node_t node_idx, const value_t source_capacity, + const value_t sink_capacity); + + // Add edge to the graph. + void AddEdge(const node_t node_idx1, const node_t node_idx2, + const value_t capacity, const value_t reverse_capacity); + + // Compute the min-cut using the max-flow algorithm. Returns the flow. + value_t Compute(); + + // Check whether node is connected to source or sink after computing the cut. + bool IsConnectedToSource(const node_t node_idx) const; + bool IsConnectedToSink(const node_t node_idx) const; + + private: + const node_t S_node_; + const node_t T_node_; + graph_t graph_; + std::vector colors_; +}; + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +template +MinSTGraphCut::MinSTGraphCut(const size_t num_nodes) + : S_node_(num_nodes), T_node_(num_nodes + 1), graph_(num_nodes + 2) {} + +template +size_t MinSTGraphCut::NumNodes() const { + return boost::num_vertices(graph_) - 2; +} + +template +size_t MinSTGraphCut::NumEdges() const { + return boost::num_edges(graph_); +} + +template +void MinSTGraphCut::AddNode(const node_t node_idx, + const value_t source_capacity, + const value_t sink_capacity) { + CHECK_GE(node_idx, 0); + CHECK_LE(node_idx, boost::num_vertices(graph_)); + CHECK_GE(source_capacity, 0); + CHECK_GE(sink_capacity, 0); + + if (source_capacity > 0) { + const edge_descriptor_t edge = + boost::add_edge(S_node_, node_idx, graph_).first; + const edge_descriptor_t edge_reverse = + boost::add_edge(node_idx, S_node_, graph_).first; + graph_[edge].capacity = source_capacity; + graph_[edge].reverse = edge_reverse; + graph_[edge_reverse].reverse = edge; + } + + if (sink_capacity > 0) { + const edge_descriptor_t edge = + boost::add_edge(node_idx, T_node_, graph_).first; + const edge_descriptor_t edge_reverse = + boost::add_edge(T_node_, node_idx, graph_).first; + graph_[edge].capacity = sink_capacity; + graph_[edge].reverse = edge_reverse; + graph_[edge_reverse].reverse = edge; + } +} + +template +void MinSTGraphCut::AddEdge(const node_t node_idx1, + const node_t node_idx2, + const value_t capacity, + const value_t reverse_capacity) { + CHECK_GE(node_idx1, 0); + CHECK_LE(node_idx1, boost::num_vertices(graph_)); + CHECK_GE(node_idx2, 0); + CHECK_LE(node_idx2, boost::num_vertices(graph_)); + CHECK_GE(capacity, 0); + CHECK_GE(reverse_capacity, 0); + + const edge_descriptor_t edge = + boost::add_edge(node_idx1, node_idx2, graph_).first; + const edge_descriptor_t edge_reverse = + boost::add_edge(node_idx2, node_idx1, graph_).first; + graph_[edge].capacity = capacity; + graph_[edge_reverse].capacity = reverse_capacity; + graph_[edge].reverse = edge_reverse; + graph_[edge_reverse].reverse = edge; +} + +template +value_t MinSTGraphCut::Compute() { + const vertices_size_t num_vertices = boost::num_vertices(graph_); + + colors_.resize(num_vertices); + std::vector predecessors(num_vertices); + std::vector distances(num_vertices); + + return boost::boykov_kolmogorov_max_flow( + graph_, boost::get(&Edge::capacity, graph_), + boost::get(&Edge::residual, graph_), boost::get(&Edge::reverse, graph_), + predecessors.data(), colors_.data(), distances.data(), + boost::get(boost::vertex_index, graph_), S_node_, T_node_); +} + +template +bool MinSTGraphCut::IsConnectedToSource( + const node_t node_idx) const { + return colors_.at(node_idx) != boost::white_color; +} + +template +bool MinSTGraphCut::IsConnectedToSink( + const node_t node_idx) const { + return colors_.at(node_idx) == boost::white_color; +} + +} // namespace colmap + +#endif // COLMAP_SRC_BASE_GRAPH_CUT_H_ diff --git a/include/colmap/base/homography_matrix.h b/include/colmap/base/homography_matrix.h new file mode 100644 index 0000000000000000000000000000000000000000..5faa05e86bf09fc0b8c194b2ebb1744a453761ba --- /dev/null +++ b/include/colmap/base/homography_matrix.h @@ -0,0 +1,111 @@ +// Copyright (c) 2022, 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_HOMOGRAPHY_MATRIX_UTILS_H_ +#define COLMAP_SRC_BASE_HOMOGRAPHY_MATRIX_UTILS_H_ + +#include + +#include + +#include "util/alignment.h" +#include "util/types.h" + +namespace colmap { + +// Decompose an homography matrix into the possible rotations, translations, +// and plane normal vectors, according to: +// +// Malis, Ezio, and Manuel Vargas. "Deeper understanding of the homography +// decomposition for vision-based control." (2007): 90. +// +// The first pose is assumed to be P = [I | 0]. Note that the homography is +// plane-induced if `R.size() == t.size() == n.size() == 4`. If `R.size() == +// t.size() == n.size() == 1` the homography is pure-rotational. +// +// @param H 3x3 homography matrix. +// @param K 3x3 calibration matrix. +// @param R Possible 3x3 rotation matrices. +// @param t Possible translation vectors. +// @param n Possible normal vectors. +void DecomposeHomographyMatrix(const Eigen::Matrix3d& H, + const Eigen::Matrix3d& K1, + const Eigen::Matrix3d& K2, + std::vector* R, + std::vector* t, + std::vector* n); + +// Recover the most probable pose from the given homography matrix. +// +// The pose of the first image is assumed to be P = [I | 0]. +// +// @param H 3x3 homography matrix. +// @param K1 3x3 calibration matrix of first camera. +// @param K2 3x3 calibration matrix of second camera. +// @param points1 First set of corresponding points. +// @param points2 Second set of corresponding points. +// @param inlier_mask Only points with `true` in the inlier mask are +// considered in the cheirality test. Size of the +// inlier mask must match the number of points N. +// @param R Most probable 3x3 rotation matrix. +// @param t Most probable 3x1 translation vector. +// @param n Most probable 3x1 normal vector. +// @param points3D Triangulated 3D points infront of camera +// (only if homography is not pure-rotational). +void PoseFromHomographyMatrix(const Eigen::Matrix3d& H, + const Eigen::Matrix3d& K1, + const Eigen::Matrix3d& K2, + const std::vector& points1, + const std::vector& points2, + Eigen::Matrix3d* R, Eigen::Vector3d* t, + Eigen::Vector3d* n, + std::vector* points3D); + +// Compose homography matrix from relative pose. +// +// @param K1 3x3 calibration matrix of first camera. +// @param K2 3x3 calibration matrix of second camera. +// @param R Most probable 3x3 rotation matrix. +// @param t Most probable 3x1 translation vector. +// @param n Most probable 3x1 normal vector. +// @param d Orthogonal distance from plane. +// +// @return 3x3 homography matrix. +Eigen::Matrix3d HomographyMatrixFromPose(const Eigen::Matrix3d& K1, + const Eigen::Matrix3d& K2, + const Eigen::Matrix3d& R, + const Eigen::Vector3d& t, + const Eigen::Vector3d& n, + const double d); + +} // namespace colmap + +#endif // COLMAP_SRC_BASE_HOMOGRAPHY_MATRIX_UTILS_H_ diff --git a/include/colmap/base/image.h b/include/colmap/base/image.h new file mode 100644 index 0000000000000000000000000000000000000000..fafef84dc03a0e2acfcd3e973991253de6ffcd93 --- /dev/null +++ b/include/colmap/base/image.h @@ -0,0 +1,364 @@ +// Copyright (c) 2022, 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_IMAGE_H_ +#define COLMAP_SRC_BASE_IMAGE_H_ + +#include +#include + +#include + +#include "base/camera.h" +#include "base/point2d.h" +#include "base/visibility_pyramid.h" +#include "util/alignment.h" +#include "util/logging.h" +#include "util/math.h" +#include "util/types.h" + +namespace colmap { + +// Class that holds information about an image. An image is the product of one +// camera shot at a certain location (parameterized as the pose). An image may +// share a camera with multiple other images, if its intrinsics are the same. +class Image { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + Image(); + + // Setup / tear down the image and necessary internal data structures before + // and after being used in reconstruction. + void SetUp(const Camera& camera); + void TearDown(); + + // Access the unique identifier of the image. + inline image_t ImageId() const; + inline void SetImageId(const image_t image_id); + + // Access the name of the image. + inline const std::string& Name() const; + inline std::string& Name(); + inline void SetName(const std::string& name); + + // Access the unique identifier of the camera. Note that multiple images + // might share the same camera. + inline camera_t CameraId() const; + inline void SetCameraId(const camera_t camera_id); + // Check whether identifier of camera has been set. + inline bool HasCamera() const; + + // Check if image is registered. + inline bool IsRegistered() const; + inline void SetRegistered(const bool registered); + + // Get the number of image points. + inline point2D_t NumPoints2D() const; + + // Get the number of triangulations, i.e. the number of points that + // are part of a 3D point track. + inline point2D_t NumPoints3D() const; + + // Get the number of observations, i.e. the number of image points that + // have at least one correspondence to another image. + inline point2D_t NumObservations() const; + inline void SetNumObservations(const point2D_t num_observations); + + // Get the number of correspondences for all image points. + inline point2D_t NumCorrespondences() const; + inline void SetNumCorrespondences(const point2D_t num_observations); + + // Get the number of observations that see a triangulated point, i.e. the + // number of image points that have at least one correspondence to a + // triangulated point in another image. + inline point2D_t NumVisiblePoints3D() const; + + // Get the score of triangulated observations. In contrast to + // `NumVisiblePoints3D`, this score also captures the distribution + // of triangulated observations in the image. This is useful to select + // the next best image in incremental reconstruction, because a more + // uniform distribution of observations results in more robust registration. + inline size_t Point3DVisibilityScore() const; + + // Access quaternion vector as (qw, qx, qy, qz) specifying the rotation of the + // pose which is defined as the transformation from world to image space. + inline const Eigen::Vector4d& Qvec() const; + inline Eigen::Vector4d& Qvec(); + inline double Qvec(const size_t idx) const; + inline double& Qvec(const size_t idx); + inline void SetQvec(const Eigen::Vector4d& qvec); + + // Quaternion prior, e.g. given by EXIF gyroscope tag. + inline const Eigen::Vector4d& QvecPrior() const; + inline Eigen::Vector4d& QvecPrior(); + inline double QvecPrior(const size_t idx) const; + inline double& QvecPrior(const size_t idx); + inline bool HasQvecPrior() const; + inline void SetQvecPrior(const Eigen::Vector4d& qvec); + + // Access translation vector as (tx, ty, tz) specifying the translation of the + // pose which is defined as the transformation from world to image space. + inline const Eigen::Vector3d& Tvec() const; + inline Eigen::Vector3d& Tvec(); + inline double Tvec(const size_t idx) const; + inline double& Tvec(const size_t idx); + inline void SetTvec(const Eigen::Vector3d& tvec); + + // Translation prior, e.g. given by EXIF GPS tag. + inline const Eigen::Vector3d& TvecPrior() const; + inline Eigen::Vector3d& TvecPrior(); + inline double TvecPrior(const size_t idx) const; + inline double& TvecPrior(const size_t idx); + inline bool HasTvecPrior() const; + inline void SetTvecPrior(const Eigen::Vector3d& tvec); + + // Access the coordinates of image points. + inline const class Point2D& Point2D(const point2D_t point2D_idx) const; + inline class Point2D& Point2D(const point2D_t point2D_idx); + inline const std::vector& Points2D() const; + void SetPoints2D(const std::vector& points); + void SetPoints2D(const std::vector& points); + + // Set the point as triangulated, i.e. it is part of a 3D point track. + void SetPoint3DForPoint2D(const point2D_t point2D_idx, + const point3D_t point3D_id); + + // Set the point as not triangulated, i.e. it is not part of a 3D point track. + void ResetPoint3DForPoint2D(const point2D_t point2D_idx); + + // Check whether an image point has a correspondence to an image point in + // another image that has a 3D point. + inline bool IsPoint3DVisible(const point2D_t point2D_idx) const; + + // Check whether one of the image points is part of the 3D point track. + bool HasPoint3D(const point3D_t point3D_id) const; + + // Indicate that another image has a point that is triangulated and has + // a correspondence to this image point. Note that this must only be called + // after calling `SetUp`. + void IncrementCorrespondenceHasPoint3D(const point2D_t point2D_idx); + + // Indicate that another image has a point that is not triangulated any more + // and has a correspondence to this image point. This assumes that + // `IncrementCorrespondenceHasPoint3D` was called for the same image point + // and correspondence before. Note that this must only be called + // after calling `SetUp`. + void DecrementCorrespondenceHasPoint3D(const point2D_t point2D_idx); + + // Normalize the quaternion vector. + void NormalizeQvec(); + + // Compose the projection matrix from world to image space. + Eigen::Matrix3x4d ProjectionMatrix() const; + + // Compose the inverse projection matrix from image to world space + Eigen::Matrix3x4d InverseProjectionMatrix() const; + + // Compose rotation matrix from quaternion vector. + Eigen::Matrix3d RotationMatrix() const; + + // Extract the projection center in world space. + Eigen::Vector3d ProjectionCenter() const; + + // Extract the viewing direction of the image. + Eigen::Vector3d ViewingDirection() const; + + // The number of levels in the 3D point multi-resolution visibility pyramid. + static const int kNumPoint3DVisibilityPyramidLevels; + + private: + // Identifier of the image, if not specified `kInvalidImageId`. + image_t image_id_; + + // The name of the image, i.e. the relative path. + std::string name_; + + // The identifier of the associated camera. Note that multiple images might + // share the same camera. If not specified `kInvalidCameraId`. + camera_t camera_id_; + + // Whether the image is successfully registered in the reconstruction. + bool registered_; + + // The number of 3D points the image observes, i.e. the sum of its `points2D` + // where `point3D_id != kInvalidPoint3DId`. + point2D_t num_points3D_; + + // The number of image points that have at least one correspondence to + // another image. + point2D_t num_observations_; + + // The sum of correspondences per image point. + point2D_t num_correspondences_; + + // The number of 2D points, which have at least one corresponding 2D point in + // another image that is part of a 3D point track, i.e. the sum of `points2D` + // where `num_tris > 0`. + point2D_t num_visible_points3D_; + + // The pose of the image, defined as the transformation from world to image. + Eigen::Vector4d qvec_; + Eigen::Vector3d tvec_; + + // The pose prior of the image, e.g. extracted from EXIF tags. + Eigen::Vector4d qvec_prior_; + Eigen::Vector3d tvec_prior_; + + // All image points, including points that are not part of a 3D point track. + std::vector points2D_; + + // Per image point, the number of correspondences that have a 3D point. + std::vector num_correspondences_have_point3D_; + + // Data structure to compute the distribution of triangulated correspondences + // in the image. Note that this structure is only usable after `SetUp`. + VisibilityPyramid point3D_visibility_pyramid_; +}; + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +image_t Image::ImageId() const { return image_id_; } + +void Image::SetImageId(const image_t image_id) { image_id_ = image_id; } + +const std::string& Image::Name() const { return name_; } + +std::string& Image::Name() { return name_; } + +void Image::SetName(const std::string& name) { name_ = name; } + +inline camera_t Image::CameraId() const { return camera_id_; } + +inline void Image::SetCameraId(const camera_t camera_id) { + CHECK_NE(camera_id, kInvalidCameraId); + camera_id_ = camera_id; +} + +inline bool Image::HasCamera() const { return camera_id_ != kInvalidCameraId; } + +bool Image::IsRegistered() const { return registered_; } + +void Image::SetRegistered(const bool registered) { registered_ = registered; } + +point2D_t Image::NumPoints2D() const { + return static_cast(points2D_.size()); +} + +point2D_t Image::NumPoints3D() const { return num_points3D_; } + +point2D_t Image::NumObservations() const { return num_observations_; } + +void Image::SetNumObservations(const point2D_t num_observations) { + num_observations_ = num_observations; +} + +point2D_t Image::NumCorrespondences() const { return num_correspondences_; } + +void Image::SetNumCorrespondences(const point2D_t num_correspondences) { + num_correspondences_ = num_correspondences; +} + +point2D_t Image::NumVisiblePoints3D() const { return num_visible_points3D_; } + +size_t Image::Point3DVisibilityScore() const { + return point3D_visibility_pyramid_.Score(); +} + +const Eigen::Vector4d& Image::Qvec() const { return qvec_; } + +Eigen::Vector4d& Image::Qvec() { return qvec_; } + +inline double Image::Qvec(const size_t idx) const { return qvec_(idx); } + +inline double& Image::Qvec(const size_t idx) { return qvec_(idx); } + +void Image::SetQvec(const Eigen::Vector4d& qvec) { qvec_ = qvec; } + +const Eigen::Vector4d& Image::QvecPrior() const { return qvec_prior_; } + +Eigen::Vector4d& Image::QvecPrior() { return qvec_prior_; } + +inline double Image::QvecPrior(const size_t idx) const { + return qvec_prior_(idx); +} + +inline double& Image::QvecPrior(const size_t idx) { return qvec_prior_(idx); } + +inline bool Image::HasQvecPrior() const { return !IsNaN(qvec_prior_.sum()); } + +void Image::SetQvecPrior(const Eigen::Vector4d& qvec) { qvec_prior_ = qvec; } + +const Eigen::Vector3d& Image::Tvec() const { return tvec_; } + +Eigen::Vector3d& Image::Tvec() { return tvec_; } + +inline double Image::Tvec(const size_t idx) const { return tvec_(idx); } + +inline double& Image::Tvec(const size_t idx) { return tvec_(idx); } + +void Image::SetTvec(const Eigen::Vector3d& tvec) { tvec_ = tvec; } + +const Eigen::Vector3d& Image::TvecPrior() const { return tvec_prior_; } + +Eigen::Vector3d& Image::TvecPrior() { return tvec_prior_; } + +inline double Image::TvecPrior(const size_t idx) const { + return tvec_prior_(idx); +} + +inline double& Image::TvecPrior(const size_t idx) { return tvec_prior_(idx); } + +inline bool Image::HasTvecPrior() const { return !IsNaN(tvec_prior_.sum()); } + +void Image::SetTvecPrior(const Eigen::Vector3d& tvec) { tvec_prior_ = tvec; } + +const class Point2D& Image::Point2D(const point2D_t point2D_idx) const { + return points2D_.at(point2D_idx); +} + +class Point2D& Image::Point2D(const point2D_t point2D_idx) { + return points2D_.at(point2D_idx); +} + +const std::vector& Image::Points2D() const { return points2D_; } + +bool Image::IsPoint3DVisible(const point2D_t point2D_idx) const { + return num_correspondences_have_point3D_.at(point2D_idx) > 0; +} + +} // namespace colmap + +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(colmap::Image) + +#endif // COLMAP_SRC_BASE_IMAGE_H_ diff --git a/include/colmap/base/image_reader.h b/include/colmap/base/image_reader.h new file mode 100644 index 0000000000000000000000000000000000000000..8905840b59a6a65248171093af29406f0a585443 --- /dev/null +++ b/include/colmap/base/image_reader.h @@ -0,0 +1,133 @@ +// Copyright (c) 2022, 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_IMAGE_READER_H_ +#define COLMAP_SRC_BASE_IMAGE_READER_H_ + +#include + +#include "base/database.h" +#include "util/bitmap.h" +#include "util/threading.h" + +namespace colmap { + +struct ImageReaderOptions { + // Path to database in which to store the extracted data. + std::string database_path = ""; + + // Root path to folder which contains the images. + std::string image_path = ""; + + // Optional root path to folder which contains image masks. For a given image, + // the corresponding mask must have the same sub-path below this root as the + // image has below image_path. The filename must be equal, aside from the + // added extension .png. For example, for an image image_path/abc/012.jpg, the + // mask would be mask_path/abc/012.jpg.png. No features will be extracted in + // regions where the mask image is black (pixel intensity value 0 in + // grayscale). + std::string mask_path = ""; + + // Optional list of images to read. The list must contain the relative path + // of the images with respect to the image_path. + std::vector image_list; + + // Name of the camera model. + std::string camera_model = "SIMPLE_RADIAL"; + + // Whether to use the same camera for all images. + bool single_camera = false; + + // Whether to use the same camera for all images in the same sub-folder. + bool single_camera_per_folder = false; + + // Whether to use a different camera for each image. + bool single_camera_per_image = false; + + // Whether to explicitly use an existing camera for all images. Note that in + // this case the specified camera model and parameters are ignored. + int existing_camera_id = kInvalidCameraId; + + // Manual specification of camera parameters. If empty, camera parameters + // will be extracted from EXIF, i.e. principal point and focal length. + std::string camera_params = ""; + + // If camera parameters are not specified manually and the image does not + // have focal length EXIF information, the focal length is set to the + // value `default_focal_length_factor * max(width, height)`. + double default_focal_length_factor = 1.2; + + // Optional path to an image file specifying a mask for all images. No + // features will be extracted in regions where the mask is black (pixel + // intensity value 0 in grayscale). + std::string camera_mask_path = ""; + + bool Check() const; +}; + +// Recursively iterate over the images in a directory. Skips an image if it +// already exists in the database. Extracts the camera intrinsics from EXIF and +// writes the camera information to the database. +class ImageReader { + public: + enum class Status { + FAILURE, + SUCCESS, + IMAGE_EXISTS, + BITMAP_ERROR, + CAMERA_SINGLE_DIM_ERROR, + CAMERA_EXIST_DIM_ERROR, + CAMERA_PARAM_ERROR + }; + + ImageReader(const ImageReaderOptions& options, Database* database); + + Status Next(Camera* camera, Image* image, Bitmap* bitmap, Bitmap* mask); + size_t NextIndex() const; + size_t NumImages() const; + + private: + // Image reader options. + ImageReaderOptions options_; + Database* database_; + // Index of previously processed image. + size_t image_index_; + // Previously processed camera. + Camera prev_camera_; + std::unordered_map camera_model_to_id_; + // Names of image sub-folders. + std::string prev_image_folder_; + std::unordered_set image_folders_; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_BASE_IMAGE_READER_H_ diff --git a/include/colmap/base/line.h b/include/colmap/base/line.h new file mode 100644 index 0000000000000000000000000000000000000000..34c15f99d53d3d61545b3d6d6d5c495cc8ce3697 --- /dev/null +++ b/include/colmap/base/line.h @@ -0,0 +1,66 @@ +// Copyright (c) 2022, 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_LINE_H_ +#define COLMAP_SRC_BASE_LINE_H_ + +#include + +#include "util/alignment.h" +#include "util/bitmap.h" + +namespace colmap { + +struct LineSegment { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + Eigen::Vector2d start; + Eigen::Vector2d end; +}; + +enum class LineSegmentOrientation { + HORIZONTAL = 1, + VERTICAL = -1, + UNDEFINED = 0, +}; + +// Detect line segments in the given bitmap image. +std::vector DetectLineSegments(const Bitmap& bitmap, + const double min_length = 3); + +// Classify line segments into horizontal/vertical. +std::vector ClassifyLineSegmentOrientations( + const std::vector& segments, const double tolerance = 0.25); + +} // namespace colmap + +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(colmap::LineSegment) + +#endif // COLMAP_SRC_BASE_LINE_H_ diff --git a/include/colmap/base/point2d.h b/include/colmap/base/point2d.h new file mode 100644 index 0000000000000000000000000000000000000000..c4654d5ec6ac7ce760bbcf5917ead9ddcffaaae0 --- /dev/null +++ b/include/colmap/base/point2d.h @@ -0,0 +1,98 @@ +// Copyright (c) 2022, 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_POINT2D_H_ +#define COLMAP_SRC_BASE_POINT2D_H_ + +#include + +#include "util/alignment.h" +#include "util/types.h" + +namespace colmap { + +// 2D point class corresponds to a feature in an image. It may or may not have a +// corresponding 3D point if it is part of a triangulated track. +class Point2D { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + Point2D(); + + // The coordinate in image space in pixels. + inline const Eigen::Vector2d& XY() const; + inline Eigen::Vector2d& XY(); + inline double X() const; + inline double Y() const; + inline void SetXY(const Eigen::Vector2d& xy); + + // The identifier of the observed 3D point. If the image point does not + // observe a 3D point, the identifier is `kInvalidPoint3Did`. + inline point3D_t Point3DId() const; + inline bool HasPoint3D() const; + inline void SetPoint3DId(const point3D_t point3D_id); + + private: + // The image coordinates in pixels, starting at upper left corner with 0. + Eigen::Vector2d xy_; + + // The identifier of the 3D point. If the 2D point is not part of a 3D point + // track the identifier is `kInvalidPoint3DId` and `HasPoint3D() = false`. + point3D_t point3D_id_; +}; + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +const Eigen::Vector2d& Point2D::XY() const { return xy_; } + +Eigen::Vector2d& Point2D::XY() { return xy_; } + +double Point2D::X() const { return xy_.x(); } + +double Point2D::Y() const { return xy_.y(); } + +void Point2D::SetXY(const Eigen::Vector2d& xy) { xy_ = xy; } + +point3D_t Point2D::Point3DId() const { return point3D_id_; } + +bool Point2D::HasPoint3D() const { return point3D_id_ != kInvalidPoint3DId; } + +void Point2D::SetPoint3DId(const point3D_t point3D_id) { + point3D_id_ = point3D_id; +} + +} // namespace colmap + +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(colmap::Point2D) + +#endif // COLMAP_SRC_BASE_POINT2D_H_ diff --git a/include/colmap/base/point3d.h b/include/colmap/base/point3d.h new file mode 100644 index 0000000000000000000000000000000000000000..44556bcb2cfe852c1fde66fc3c475621e5671687 --- /dev/null +++ b/include/colmap/base/point3d.h @@ -0,0 +1,140 @@ +// Copyright (c) 2022, 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_POINT3D_H_ +#define COLMAP_SRC_BASE_POINT3D_H_ + +#include + +#include + +#include "base/track.h" +#include "util/logging.h" +#include "util/types.h" + +namespace colmap { + +// 3D point class that holds information about triangulated 2D points. +class Point3D { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + Point3D(); + + // The point coordinate in world space. + inline const Eigen::Vector3d& XYZ() const; + inline Eigen::Vector3d& XYZ(); + inline double XYZ(const size_t idx) const; + inline double& XYZ(const size_t idx); + inline double X() const; + inline double Y() const; + inline double Z() const; + inline void SetXYZ(const Eigen::Vector3d& xyz); + + // The RGB color of the point. + inline const Eigen::Vector3ub& Color() const; + inline Eigen::Vector3ub& Color(); + inline uint8_t Color(const size_t idx) const; + inline uint8_t& Color(const size_t idx); + inline void SetColor(const Eigen::Vector3ub& color); + + // The mean reprojection error in image space. + inline double Error() const; + inline bool HasError() const; + inline void SetError(const double error); + + inline const class Track& Track() const; + inline class Track& Track(); + inline void SetTrack(class Track track); + + private: + // The 3D position of the point. + Eigen::Vector3d xyz_; + + // The color of the point in the range [0, 255]. + Eigen::Vector3ub color_; + + // The mean reprojection error in pixels. + double error_; + + // The track of the point as a list of image observations. + class Track track_; +}; + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +const Eigen::Vector3d& Point3D::XYZ() const { return xyz_; } + +Eigen::Vector3d& Point3D::XYZ() { return xyz_; } + +double Point3D::XYZ(const size_t idx) const { return xyz_(idx); } + +double& Point3D::XYZ(const size_t idx) { return xyz_(idx); } + +double Point3D::X() const { return xyz_.x(); } + +double Point3D::Y() const { return xyz_.y(); } + +double Point3D::Z() const { return xyz_.z(); } + +void Point3D::SetXYZ(const Eigen::Vector3d& xyz) { xyz_ = xyz; } + +const Eigen::Vector3ub& Point3D::Color() const { return color_; } + +Eigen::Vector3ub& Point3D::Color() { return color_; } + +uint8_t Point3D::Color(const size_t idx) const { return color_(idx); } + +uint8_t& Point3D::Color(const size_t idx) { return color_(idx); } + +void Point3D::SetColor(const Eigen::Vector3ub& color) { color_ = color; } + +double Point3D::Error() const { return error_; } + +bool Point3D::HasError() const { return error_ != -1.0; } + +void Point3D::SetError(const double error) { error_ = error; } + +const class Track& Point3D::Track() const { return track_; } + +class Track& Point3D::Track() { + return track_; +} + +void Point3D::SetTrack(class Track track) { track_ = std::move(track); } + +} // namespace colmap + +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(colmap::Point3D) + +#endif // COLMAP_SRC_BASE_POINT3D_H_ diff --git a/include/colmap/base/polynomial.h b/include/colmap/base/polynomial.h new file mode 100644 index 0000000000000000000000000000000000000000..d15af471e8ecc33484c3073f1b8f4d4e30e51de8 --- /dev/null +++ b/include/colmap/base/polynomial.h @@ -0,0 +1,101 @@ +// Copyright (c) 2022, 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/include/colmap/base/pose.h b/include/colmap/base/pose.h new file mode 100644 index 0000000000000000000000000000000000000000..a11ad44b609cda50af8d9157e8d94091b9e41112 --- /dev/null +++ b/include/colmap/base/pose.h @@ -0,0 +1,230 @@ +// Copyright (c) 2022, 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 "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/include/colmap/base/projection.h b/include/colmap/base/projection.h new file mode 100644 index 0000000000000000000000000000000000000000..0c494fff7df38a78f28311e60ac465ccdf8c228c --- /dev/null +++ b/include/colmap/base/projection.h @@ -0,0 +1,167 @@ +// Copyright (c) 2022, 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_PROJECTION_H_ +#define COLMAP_SRC_BASE_PROJECTION_H_ + +#include +#include + +#include +#include + +#include "base/camera.h" + +namespace colmap { + +// Compose projection matrix from rotation and translation components. +// +// The projection matrix transforms 3D world to image points. +// +// @param qvec Unit Quaternion rotation coefficients (w, x, y, z). +// @param tvec 3x1 translation vector. +// +// @return 3x4 projection matrix. +Eigen::Matrix3x4d ComposeProjectionMatrix(const Eigen::Vector4d& qvec, + const Eigen::Vector3d& tvec); + +// Compose projection matrix from rotation matrix and translation components). +// +// The projection matrix transforms 3D world to image points. +// +// @param R 3x3 rotation matrix. +// @param t 3x1 translation vector. +// +// @return 3x4 projection matrix. +Eigen::Matrix3x4d ComposeProjectionMatrix(const Eigen::Matrix3d& R, + const Eigen::Vector3d& T); + +// Invert projection matrix, defined as: +// +// P = [R | t] with R \in SO(3) and t \in R^3 +// +// and the inverse projection matrix as: +// +// P' = [R^T | -R^T t] +// +// @param proj_matrix 3x4 projection matrix. +// +// @return 3x4 inverse projection matrix. +Eigen::Matrix3x4d InvertProjectionMatrix(const Eigen::Matrix3x4d& proj_matrix); + +// Compute the closes rotation matrix with the closest Frobenius norm by setting +// the singular values of the given matrix to 1. +Eigen::Matrix3d ComputeClosestRotationMatrix(const Eigen::Matrix3d& matrix); + +// Decompose projection matrix into intrinsic camera matrix, rotation matrix and +// translation vector. Returns false if decomposition fails. This implementation +// is inspired by the OpenCV implementation with some additional checks. +bool DecomposeProjectionMatrix(const Eigen::Matrix3x4d& proj_matrix, + Eigen::Matrix3d* K, Eigen::Matrix3d* R, + Eigen::Vector3d* T); + +// Project 3D point to image. +// +// @param points3D 3D world point as 3x1 vector. +// @param proj_matrix 3x4 projection matrix. +// @param camera Camera used to project to image plane. +// +// @return Projected image point. +Eigen::Vector2d ProjectPointToImage(const Eigen::Vector3d& point3D, + const Eigen::Matrix3x4d& proj_matrix, + const Camera& camera); + +// Calculate the reprojection error. +// +// The reprojection error is the Euclidean distance between the observation +// in the image and the projection of the 3D point into the image. If the +// 3D point is behind the camera, then this function returns DBL_MAX. +double CalculateSquaredReprojectionError(const Eigen::Vector2d& point2D, + const Eigen::Vector3d& point3D, + const Eigen::Vector4d& qvec, + const Eigen::Vector3d& tvec, + const Camera& camera); +double CalculateSquaredReprojectionError(const Eigen::Vector2d& point2D, + const Eigen::Vector3d& point3D, + const Eigen::Matrix3x4d& proj_matrix, + const Camera& camera); + +// Calculate the angular error. +// +// The angular error is the angle between the observed viewing ray and the +// actual viewing ray from the camera center to the 3D point. +double CalculateAngularError(const Eigen::Vector2d& point2D, + const Eigen::Vector3d& point3D, + const Eigen::Vector4d& qvec, + const Eigen::Vector3d& tvec, const Camera& camera); +double CalculateAngularError(const Eigen::Vector2d& point2D, + const Eigen::Vector3d& point3D, + const Eigen::Matrix3x4d& proj_matrix, + const Camera& camera); + +// Calculate angulate error using normalized image points. +// +// The angular error is the angle between the observed viewing ray and the +// actual viewing ray from the camera center to the 3D point. +double CalculateNormalizedAngularError(const Eigen::Vector2d& point2D, + const Eigen::Vector3d& point3D, + const Eigen::Vector4d& qvec, + const Eigen::Vector3d& tvec); +double CalculateNormalizedAngularError(const Eigen::Vector2d& point2D, + const Eigen::Vector3d& point3D, + const Eigen::Matrix3x4d& proj_matrix); + +// Calculate depth of 3D point with respect to camera. +// +// The depth is defined as the Euclidean distance of a 3D point from the +// camera and is positive if the 3D point is in front and negative if +// behind of the camera. +// +// @param proj_matrix 3x4 projection matrix. +// @param point3D 3D point as 3x1 vector. +// +// @return Depth of 3D point. +double CalculateDepth(const Eigen::Matrix3x4d& proj_matrix, + const Eigen::Vector3d& point3D); + +// Check if 3D point passes cheirality constraint, +// i.e. it lies in front of the camera and not in the image plane. +// +// @param proj_matrix 3x4 projection matrix. +// @param point3D 3D point as 3x1 vector. +// +// @return True if point lies in front of camera. +bool HasPointPositiveDepth(const Eigen::Matrix3x4d& proj_matrix, + const Eigen::Vector3d& point3D); + +} // namespace colmap + +#endif // COLMAP_SRC_BASE_PROJECTION_H_ diff --git a/include/colmap/base/reconstruction.h b/include/colmap/base/reconstruction.h new file mode 100644 index 0000000000000000000000000000000000000000..1a7893890eaa22c5710af0c844fe57f0a1ddfe5f --- /dev/null +++ b/include/colmap/base/reconstruction.h @@ -0,0 +1,651 @@ +// Copyright (c) 2022, 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_RECONSTRUCTION_H_ +#define COLMAP_SRC_BASE_RECONSTRUCTION_H_ + +#include +#include +#include +#include + +#include + +#include "base/camera.h" +#include "base/database.h" +#include "base/image.h" +#include "base/point2d.h" +#include "base/point3d.h" +#include "base/similarity_transform.h" +#include "base/track.h" +#include "estimators/similarity_transform.h" +#include "optim/loransac.h" +#include "util/alignment.h" +#include "util/types.h" + +namespace colmap { + +struct PlyPoint; +struct RANSACOptions; +class DatabaseCache; +class CorrespondenceGraph; + +// Reconstruction class holds all information about a single reconstructed +// model. It is used by the mapping and bundle adjustment classes and can be +// written to and read from disk. +class Reconstruction { + public: + struct ImagePairStat { + // The number of triangulated correspondences between two images. + size_t num_tri_corrs = 0; + // The number of total correspondences/matches between two images. + size_t num_total_corrs = 0; + }; + + Reconstruction(); + + // Get number of objects. + inline size_t NumCameras() const; + inline size_t NumImages() const; + inline size_t NumRegImages() const; + inline size_t NumPoints3D() const; + inline size_t NumImagePairs() const; + + // Get const objects. + inline const class Camera& Camera(const camera_t camera_id) const; + inline const class Image& Image(const image_t image_id) const; + inline const class Point3D& Point3D(const point3D_t point3D_id) const; + inline const ImagePairStat& ImagePair(const image_pair_t pair_id) const; + inline ImagePairStat& ImagePair(const image_t image_id1, + const image_t image_id2); + + // Get mutable objects. + inline class Camera& Camera(const camera_t camera_id); + inline class Image& Image(const image_t image_id); + inline class Point3D& Point3D(const point3D_t point3D_id); + inline ImagePairStat& ImagePair(const image_pair_t pair_id); + inline const ImagePairStat& ImagePair(const image_t image_id1, + const image_t image_id2) const; + + // Get reference to all objects. + inline const EIGEN_STL_UMAP(camera_t, class Camera) & Cameras() const; + inline const EIGEN_STL_UMAP(image_t, class Image) & Images() const; + inline const std::vector& RegImageIds() const; + inline const EIGEN_STL_UMAP(point3D_t, class Point3D) & Points3D() const; + inline const std::unordered_map& ImagePairs() + const; + + // Identifiers of all 3D points. + std::unordered_set Point3DIds() const; + + // Check whether specific object exists. + inline bool ExistsCamera(const camera_t camera_id) const; + inline bool ExistsImage(const image_t image_id) const; + inline bool ExistsPoint3D(const point3D_t point3D_id) const; + inline bool ExistsImagePair(const image_pair_t pair_id) const; + + // Load data from given `DatabaseCache`. + void Load(const DatabaseCache& database_cache); + + // Setup all relevant data structures before reconstruction. Note the + // correspondence graph object must live until `TearDown` is called. + void SetUp(const CorrespondenceGraph* correspondence_graph); + + // Finalize the Reconstruction after the reconstruction has finished. + // + // Once a scene has been finalized, it cannot be used for reconstruction. + // + // This removes all not yet registered images and unused cameras, in order to + // save memory. + void TearDown(); + + // Add new camera. There is only one camera per image, while multiple images + // might be taken by the same camera. + void AddCamera(class Camera camera); + + // Add new image. + void AddImage(class Image image); + + // Add new 3D object, and return its unique ID. + point3D_t AddPoint3D( + const Eigen::Vector3d& xyz, Track track, + const Eigen::Vector3ub& color = Eigen::Vector3ub::Zero()); + + // Add observation to existing 3D point. + void AddObservation(const point3D_t point3D_id, const TrackElement& track_el); + + // Merge two 3D points and return new identifier of new 3D point. + // The location of the merged 3D point is a weighted average of the two + // original 3D point's locations according to their track lengths. + point3D_t MergePoints3D(const point3D_t point3D_id1, + const point3D_t point3D_id2); + + // Delete a 3D point, and all its references in the observed images. + void DeletePoint3D(const point3D_t point3D_id); + + // Delete one observation from an image and the corresponding 3D point. + // Note that this deletes the entire 3D point, if the track has two elements + // prior to calling this method. + void DeleteObservation(const image_t image_id, const point2D_t point2D_idx); + + // Delete all 2D points of all images and all 3D points. + void DeleteAllPoints2DAndPoints3D(); + + // Register an existing image. + void RegisterImage(const image_t image_id); + + // De-register an existing image, and all its references. + void DeRegisterImage(const image_t image_id); + + // Check if image is registered. + inline bool IsImageRegistered(const image_t image_id) const; + + // Normalize scene by scaling and translation to avoid degenerate + // visualization after bundle adjustment and to improve numerical + // stability of algorithms. + // + // Translates scene such that the mean of the camera centers or point + // locations are at the origin of the coordinate system. + // + // Scales scene such that the minimum and maximum camera centers are at the + // given `extent`, whereas `p0` and `p1` determine the minimum and + // maximum percentiles of the camera centers considered. + void Normalize(const double extent = 10.0, const double p0 = 0.1, + const double p1 = 0.9, const bool use_images = true); + + // Compute the centroid of the 3D points + Eigen::Vector3d ComputeCentroid(const double p0 = 0.1, + const double p1 = 0.9) const; + + // Compute the bounding box corners of the 3D points + std::pair ComputeBoundingBox( + const double p0 = 0.0, const double p1 = 1.0) const; + + // Apply the 3D similarity transformation to all images and points. + void Transform(const SimilarityTransform3& tform); + + // Creates a cropped reconstruction using the input bounds as corner points + // of the bounding box containing the included 3D points of the new + // reconstruction. Only the cameras and images of the included points are + // registered. + Reconstruction Crop( + const std::pair& bbox) const; + + // Merge the given reconstruction into this reconstruction by registering the + // images registered in the given but not in this reconstruction and by + // merging the two clouds and their tracks. The coordinate frames of the two + // reconstructions are aligned using the projection centers of common + // registered images. Return true if the two reconstructions could be merged. + bool Merge(const Reconstruction& reconstruction, + const double max_reproj_error); + + // Align the given reconstruction with a set of pre-defined camera positions. + // Assuming that locations[i] gives the 3D coordinates of the center + // of projection of the image with name image_names[i]. + template + bool Align(const std::vector& image_names, + const std::vector& locations, + const int min_common_images, + SimilarityTransform3* tform = nullptr); + + // Robust alignment using RANSAC. + template + bool AlignRobust(const std::vector& image_names, + const std::vector& locations, + const int min_common_images, + const RANSACOptions& ransac_options, + SimilarityTransform3* tform = nullptr); + + // Find specific image by name. Note that this uses linear search. + const class Image* FindImageWithName(const std::string& name) const; + + // Find images that are both present in this and the given reconstruction. + std::vector FindCommonRegImageIds( + const Reconstruction& reconstruction) const; + + // Update the image identifiers to match the ones in the database by matching + // the names of the images. + void TranscribeImageIdsToDatabase(const Database& database); + + // Filter 3D points with large reprojection error, negative depth, or + // insufficient triangulation angle. + // + // @param max_reproj_error The maximum reprojection error. + // @param min_tri_angle The minimum triangulation angle. + // @param point3D_ids The points to be filtered. + // + // @return The number of filtered observations. + size_t FilterPoints3D(const double max_reproj_error, + const double min_tri_angle, + const std::unordered_set& point3D_ids); + size_t FilterPoints3DInImages(const double max_reproj_error, + const double min_tri_angle, + const std::unordered_set& image_ids); + size_t FilterAllPoints3D(const double max_reproj_error, + const double min_tri_angle); + + // Filter observations that have negative depth. + // + // @return The number of filtered observations. + size_t FilterObservationsWithNegativeDepth(); + + // Filter images without observations or bogus camera parameters. + // + // @return The identifiers of the filtered images. + std::vector FilterImages(const double min_focal_length_ratio, + const double max_focal_length_ratio, + const double max_extra_param); + + // Compute statistics for scene. + size_t ComputeNumObservations() const; + double ComputeMeanTrackLength() const; + double ComputeMeanObservationsPerRegImage() const; + double ComputeMeanReprojectionError() const; + + // Read data from text or binary file. Prefer binary data if it exists. + void Read(const std::string& path); + void Write(const std::string& path) const; + + // Read data from binary/text file. + void ReadText(const std::string& path); + void ReadBinary(const std::string& path); + + // Write data from binary/text file. + void WriteText(const std::string& path) const; + void WriteBinary(const std::string& path) const; + + // Convert 3D points in reconstruction to PLY point cloud. + std::vector ConvertToPLY() const; + + // Import from other data formats. Note that these import functions are + // only intended for visualization of data and usable for reconstruction. + void ImportPLY(const std::string& path); + void ImportPLY(const std::vector& ply_points); + + // Export to other data formats. + + // Exports in NVM format http://ccwu.me/vsfm/doc.html#nvm. Only supports + // SIMPLE_RADIAL camera model when exporting distortion parameters. When + // skip_distortion == true it supports all camera models with the caveat that + // it's using the mean focal length which will be inaccurate for camera models + // with two focal lengths and distortion. + bool ExportNVM(const std::string& path, bool skip_distortion = false) const; + + // Exports in CAM format which is a simple text file that contains pose + // information and camera intrinsics for each image and exports one file per + // image; it does not include information on the 3D points. The format is as + // follows (2 lines of text with space separated numbers): + // + // 1.0 + // Note that focal length is relative to the image max(width, height), + // and principal points x and y are relative to width and height respectively. + // + // Only supports SIMPLE_RADIAL and RADIAL camera models when exporting + // distortion parameters. When skip_distortion == true it supports all camera + // models with the caveat that it's using the mean focal length which will be + // inaccurate for camera models with two focal lengths and distortion. + bool ExportCam(const std::string& path, bool skip_distortion = false) const; + + // Exports in Recon3D format which consists of three text files with the + // following format and content: + // 1) imagemap_0.txt: a list of image numeric IDs with one entry per line. + // 2) urd-images.txt: A list of images with one entry per line as: + // + // 3) synth_0.out: Contains information for image poses, camera intrinsics, + // and 3D points as: + // + // + // + // + // Each image entry consists of 5 lines as: + // + // + // + // Note that the focal length is scaled by 1 / max(width, height) + // + // Each point entry consists of 3 lines as: + // + // + // ... + // + // Each track elemenet is a sequence of 5 values as: + // <2D point ID> -1.0 + // Note that the 2D point coordinates are centered around the principal + // point and scaled by 1 / max(width, height). + // + // When skip_distortion == true it supports all camera models with the + // caveat that it's using the mean focal length which will be inaccurate + // for camera models with two focal lengths and distortion. + bool ExportRecon3D(const std::string& path, + bool skip_distortion = false) const; + + // Exports in Bundler format https://www.cs.cornell.edu/~snavely/bundler/. + // Supports SIMPLE_PINHOLE, PINHOLE, SIMPLE_RADIAL and RADIAL camera models + // when exporting distortion parameters. When skip_distortion == true it + // supports all camera models with the caveat that it's using the mean focal + // length which will be inaccurate for camera models with two focal lengths + // and distortion. + bool ExportBundler(const std::string& path, const std::string& list_path, + bool skip_distortion = false) const; + + // Exports 3D points only in PLY format. + void ExportPLY(const std::string& path) const; + + // Exports in VRML format https://en.wikipedia.org/wiki/VRML. + void ExportVRML(const std::string& images_path, + const std::string& points3D_path, const double image_scale, + const Eigen::Vector3d& image_rgb) const; + + // Extract colors for 3D points of given image. Colors will be extracted + // only for 3D points which are completely black. + // + // @param image_id Identifier of the image for which to extract colors. + // @param path Absolute or relative path to root folder of image. + // The image path is determined by concatenating the + // root path and the name of the image. + // + // @return True if image could be read at given path. + bool ExtractColorsForImage(const image_t image_id, const std::string& path); + + // Extract colors for all 3D points by computing the mean color of all images. + // + // @param path Absolute or relative path to root folder of image. + // The image path is determined by concatenating the + // root path and the name of the image. + void ExtractColorsForAllImages(const std::string& path); + + // Create all image sub-directories in the given path. + void CreateImageDirs(const std::string& path) const; + + private: + size_t FilterPoints3DWithSmallTriangulationAngle( + const double min_tri_angle, + const std::unordered_set& point3D_ids); + size_t FilterPoints3DWithLargeReprojectionError( + const double max_reproj_error, + const std::unordered_set& point3D_ids); + + std::tuple + ComputeBoundsAndCentroid(const double p0, const double p1, + const bool use_images) const; + + void ReadCamerasText(const std::string& path); + void ReadImagesText(const std::string& path); + void ReadPoints3DText(const std::string& path); + void ReadCamerasBinary(const std::string& path); + void ReadImagesBinary(const std::string& path); + void ReadPoints3DBinary(const std::string& path); + + void WriteCamerasText(const std::string& path) const; + void WriteImagesText(const std::string& path) const; + void WritePoints3DText(const std::string& path) const; + void WriteCamerasBinary(const std::string& path) const; + void WriteImagesBinary(const std::string& path) const; + void WritePoints3DBinary(const std::string& path) const; + + void SetObservationAsTriangulated(const image_t image_id, + const point2D_t point2D_idx, + const bool is_continued_point3D); + void ResetTriObservations(const image_t image_id, const point2D_t point2D_idx, + const bool is_deleted_point3D); + + const CorrespondenceGraph* correspondence_graph_; + + EIGEN_STL_UMAP(camera_t, class Camera) cameras_; + EIGEN_STL_UMAP(image_t, class Image) images_; + EIGEN_STL_UMAP(point3D_t, class Point3D) points3D_; + + std::unordered_map image_pair_stats_; + + // { image_id, ... } where `images_.at(image_id).registered == true`. + std::vector reg_image_ids_; + + // Total number of added 3D points, used to generate unique identifiers. + point3D_t num_added_points3D_; +}; + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +size_t Reconstruction::NumCameras() const { return cameras_.size(); } + +size_t Reconstruction::NumImages() const { return images_.size(); } + +size_t Reconstruction::NumRegImages() const { return reg_image_ids_.size(); } + +size_t Reconstruction::NumPoints3D() const { return points3D_.size(); } + +size_t Reconstruction::NumImagePairs() const { + return image_pair_stats_.size(); +} + +const class Camera& Reconstruction::Camera(const camera_t camera_id) const { + return cameras_.at(camera_id); +} + +const class Image& Reconstruction::Image(const image_t image_id) const { + return images_.at(image_id); +} + +const class Point3D& Reconstruction::Point3D(const point3D_t point3D_id) const { + return points3D_.at(point3D_id); +} + +const Reconstruction::ImagePairStat& Reconstruction::ImagePair( + const image_pair_t pair_id) const { + return image_pair_stats_.at(pair_id); +} + +const Reconstruction::ImagePairStat& Reconstruction::ImagePair( + const image_t image_id1, const image_t image_id2) const { + const auto pair_id = Database::ImagePairToPairId(image_id1, image_id2); + return image_pair_stats_.at(pair_id); +} + +class Camera& Reconstruction::Camera(const camera_t camera_id) { + return cameras_.at(camera_id); +} + +class Image& Reconstruction::Image(const image_t image_id) { + return images_.at(image_id); +} + +class Point3D& Reconstruction::Point3D(const point3D_t point3D_id) { + return points3D_.at(point3D_id); +} + +Reconstruction::ImagePairStat& Reconstruction::ImagePair( + const image_pair_t pair_id) { + return image_pair_stats_.at(pair_id); +} + +Reconstruction::ImagePairStat& Reconstruction::ImagePair( + const image_t image_id1, const image_t image_id2) { + const auto pair_id = Database::ImagePairToPairId(image_id1, image_id2); + return image_pair_stats_.at(pair_id); +} + +const EIGEN_STL_UMAP(camera_t, Camera) & Reconstruction::Cameras() const { + return cameras_; +} + +const EIGEN_STL_UMAP(image_t, class Image) & Reconstruction::Images() const { + return images_; +} + +const std::vector& Reconstruction::RegImageIds() const { + return reg_image_ids_; +} + +const EIGEN_STL_UMAP(point3D_t, Point3D) & Reconstruction::Points3D() const { + return points3D_; +} + +const std::unordered_map& +Reconstruction::ImagePairs() const { + return image_pair_stats_; +} + +bool Reconstruction::ExistsCamera(const camera_t camera_id) const { + return cameras_.find(camera_id) != cameras_.end(); +} + +bool Reconstruction::ExistsImage(const image_t image_id) const { + return images_.find(image_id) != images_.end(); +} + +bool Reconstruction::ExistsPoint3D(const point3D_t point3D_id) const { + return points3D_.find(point3D_id) != points3D_.end(); +} + +bool Reconstruction::ExistsImagePair(const image_pair_t pair_id) const { + return image_pair_stats_.find(pair_id) != image_pair_stats_.end(); +} + +bool Reconstruction::IsImageRegistered(const image_t image_id) const { + return Image(image_id).IsRegistered(); +} + +template +bool Reconstruction::Align(const std::vector& image_names, + const std::vector& locations, + const int min_common_images, + SimilarityTransform3* tform) { + CHECK_GE(min_common_images, 3); + CHECK_EQ(image_names.size(), locations.size()); + + // Find out which images are contained in the reconstruction and get the + // positions of their camera centers. + std::unordered_set common_image_ids; + std::vector src; + std::vector dst; + for (size_t i = 0; i < image_names.size(); ++i) { + const class Image* image = FindImageWithName(image_names[i]); + if (image == nullptr) { + continue; + } + + if (!IsImageRegistered(image->ImageId())) { + continue; + } + + // Ignore duplicate images. + if (common_image_ids.count(image->ImageId()) > 0) { + continue; + } + + common_image_ids.insert(image->ImageId()); + src.push_back(image->ProjectionCenter()); + dst.push_back(locations[i]); + } + + // Only compute the alignment if there are enough correspondences. + if (common_image_ids.size() < static_cast(min_common_images)) { + return false; + } + + SimilarityTransform3 transform; + if (!transform.Estimate(src, dst)) { + return false; + } + + Transform(transform); + + if (tform != nullptr) { + *tform = transform; + } + + return true; +} + +template +bool Reconstruction::AlignRobust(const std::vector& image_names, + const std::vector& locations, + const int min_common_images, + const RANSACOptions& ransac_options, + SimilarityTransform3* tform) { + CHECK_GE(min_common_images, 3); + CHECK_EQ(image_names.size(), locations.size()); + + // Find out which images are contained in the reconstruction and get the + // positions of their camera centers. + std::unordered_set common_image_ids; + std::vector src; + std::vector dst; + for (size_t i = 0; i < image_names.size(); ++i) { + const class Image* image = FindImageWithName(image_names[i]); + if (image == nullptr) { + continue; + } + + if (!IsImageRegistered(image->ImageId())) { + continue; + } + + // Ignore duplicate images. + if (common_image_ids.count(image->ImageId()) > 0) { + continue; + } + + common_image_ids.insert(image->ImageId()); + src.push_back(image->ProjectionCenter()); + dst.push_back(locations[i]); + } + + // Only compute the alignment if there are enough correspondences. + if (common_image_ids.size() < static_cast(min_common_images)) { + return false; + } + + LORANSAC, + SimilarityTransformEstimator<3, kEstimateScale>> + ransac(ransac_options); + + const auto report = ransac.Estimate(src, dst); + + if (report.support.num_inliers < static_cast(min_common_images)) { + return false; + } + + SimilarityTransform3 transform = SimilarityTransform3(report.model); + Transform(transform); + + if (tform != nullptr) { + *tform = transform; + } + + return true; +} + +} // namespace colmap + +#endif // COLMAP_SRC_BASE_RECONSTRUCTION_H_ diff --git a/include/colmap/base/reconstruction_manager.h b/include/colmap/base/reconstruction_manager.h new file mode 100644 index 0000000000000000000000000000000000000000..703d1bda6bf5a581c816b25c747566675e22b1e8 --- /dev/null +++ b/include/colmap/base/reconstruction_manager.h @@ -0,0 +1,81 @@ +// Copyright (c) 2022, 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_RECONSTRUCTION_MANAGER_H_ +#define COLMAP_SRC_BASE_RECONSTRUCTION_MANAGER_H_ + +#include "base/reconstruction.h" + +namespace colmap { + +class OptionManager; + +class ReconstructionManager { + public: + ReconstructionManager(); + + // Move constructor and assignment. + ReconstructionManager(ReconstructionManager&& other); + ReconstructionManager& operator=(ReconstructionManager&& other); + + // The number of reconstructions managed. + size_t Size() const; + + // Get a reference to a specific reconstruction. + const Reconstruction& Get(const size_t idx) const; + Reconstruction& Get(const size_t idx); + + // Add a new empty reconstruction and return its index. + size_t Add(); + + // Delete a specific reconstruction. + void Delete(const size_t idx); + + // Delete all reconstructions. + void Clear(); + + // Read and add a new reconstruction and return its index. + size_t Read(const std::string& path); + + // Write all managed reconstructions into sub-folders "0", "1", "2", ... + // If the option manager object is not null, the options are written + // to each respective reconstruction folder as well. + void Write(const std::string& path, const OptionManager* options) const; + + private: + NON_COPYABLE(ReconstructionManager) + + std::vector> reconstructions_; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_BASE_RECONSTRUCTION_MANAGER_H_ diff --git a/include/colmap/base/scene_clustering.h b/include/colmap/base/scene_clustering.h new file mode 100644 index 0000000000000000000000000000000000000000..f2add9ce663f4e61ba7be1405f1aa19b7962559a --- /dev/null +++ b/include/colmap/base/scene_clustering.h @@ -0,0 +1,100 @@ +// Copyright (c) 2022, 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_SCENE_CLUSTERING_H_ +#define COLMAP_SRC_BASE_SCENE_CLUSTERING_H_ + +#include +#include + +#include "base/database.h" +#include "util/types.h" + +namespace colmap { + +// Scene clustering approach using normalized cuts on the scene graph. The scene +// is hierarchically partitioned into overlapping clusters until a maximum +// number of images is in a leaf node. +class SceneClustering { + public: + struct Options { + // Flag for hierarchical vs flat clustering + bool is_hierarchical = true; + + // The branching factor of the hierarchical clustering. + int branching = 2; + + // The number of overlapping images between child clusters. + int image_overlap = 50; + + // The max related images matches to look for in a flat cluster + int num_image_matches = 20; + + // The maximum number of images in a leaf node cluster, otherwise the + // cluster is further partitioned using the given branching factor. Note + // that a cluster leaf node will have at most `leaf_max_num_images + + // overlap` images to satisfy the overlap constraint. + int leaf_max_num_images = 500; + + bool Check() const; + }; + + struct Cluster { + std::vector image_ids; + std::vector child_clusters; + }; + + SceneClustering(const Options& options); + + void Partition(const std::vector>& image_pairs, + const std::vector& num_inliers); + + const Cluster* GetRootCluster() const; + std::vector GetLeafClusters() const; + + static SceneClustering Create(const Options& options, + const Database& database); + + private: + void PartitionHierarchicalCluster( + const std::vector>& edges, + const std::vector& weights, Cluster* cluster); + + void PartitionFlatCluster(const std::vector>& edges, + const std::vector& weights); + + const Options options_; + std::unique_ptr root_cluster_; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_BASE_SCENE_CLUSTERING_H_ diff --git a/include/colmap/base/similarity_transform.h b/include/colmap/base/similarity_transform.h new file mode 100644 index 0000000000000000000000000000000000000000..44dc43d0ea7ef68037b78784b69226a8466e4d40 --- /dev/null +++ b/include/colmap/base/similarity_transform.h @@ -0,0 +1,121 @@ +// Copyright (c) 2022, 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_SIMILARITY_TRANSFORM_H_ +#define COLMAP_SRC_BASE_SIMILARITY_TRANSFORM_H_ + +#include + +#include +#include + +#include "estimators/similarity_transform.h" +#include "util/alignment.h" +#include "util/types.h" + +namespace colmap { + +struct RANSACOptions; +class Reconstruction; + +// 3D similarity transformation with 7 degrees of freedom. +class SimilarityTransform3 { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + SimilarityTransform3(); + + explicit SimilarityTransform3(const Eigen::Matrix3x4d& matrix); + + explicit SimilarityTransform3( + const Eigen::Transform& transform); + + SimilarityTransform3(const double scale, const Eigen::Vector4d& qvec, + const Eigen::Vector3d& tvec); + + void Write(const std::string& path); + + template + bool Estimate(const std::vector& src, + const std::vector& dst); + + SimilarityTransform3 Inverse() const; + + void TransformPoint(Eigen::Vector3d* xyz) const; + void TransformPose(Eigen::Vector4d* qvec, Eigen::Vector3d* tvec) const; + + Eigen::Matrix4d Matrix() const; + double Scale() const; + Eigen::Vector4d Rotation() const; + Eigen::Vector3d Translation() const; + + static SimilarityTransform3 FromFile(const std::string& path); + + private: + Eigen::Transform transform_; +}; + +// Robustly compute alignment between reconstructions by finding images that +// are registered in both reconstructions. The alignment is then estimated +// robustly inside RANSAC from corresponding projection centers. An alignment +// is verified by reprojecting common 3D point observations. +// The min_inlier_observations threshold determines how many observations +// in a common image must reproject within the given threshold. +bool ComputeAlignmentBetweenReconstructions( + const Reconstruction& src_reconstruction, + const Reconstruction& ref_reconstruction, + const double min_inlier_observations, const double max_reproj_error, + Eigen::Matrix3x4d* alignment); + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +template +bool SimilarityTransform3::Estimate(const std::vector& src, + const std::vector& dst) { + const auto results = + SimilarityTransformEstimator<3, kEstimateScale>().Estimate(src, dst); + if (results.empty()) { + return false; + } + + CHECK_EQ(results.size(), 1); + transform_.matrix().topLeftCorner<3, 4>() = results[0]; + + return true; +} + +} // namespace colmap + +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(colmap::SimilarityTransform3) + +#endif // COLMAP_SRC_BASE_SIMILARITY_TRANSFORM_H_ diff --git a/include/colmap/base/track.h b/include/colmap/base/track.h new file mode 100644 index 0000000000000000000000000000000000000000..9674ecf9c130e440c0e4c2c9388d510533f1d099 --- /dev/null +++ b/include/colmap/base/track.h @@ -0,0 +1,139 @@ +// Copyright (c) 2022, 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_TRACK_H_ +#define COLMAP_SRC_BASE_TRACK_H_ + +#include + +#include "util/logging.h" +#include "util/types.h" + +namespace colmap { + +// Track class stores all observations of a 3D point. +struct TrackElement { + TrackElement(); + TrackElement(const image_t image_id, const point2D_t point2D_idx); + // The image in which the track element is observed. + image_t image_id; + // The point in the image that the track element is observed. + point2D_t point2D_idx; +}; + +class Track { + public: + Track(); + + // The number of track elements. + inline size_t Length() const; + + // Access all elements. + inline const std::vector& Elements() const; + inline std::vector& Elements(); + inline void SetElements(std::vector elements); + + // Access specific elements. + inline const TrackElement& Element(const size_t idx) const; + inline TrackElement& Element(const size_t idx); + inline void SetElement(const size_t idx, const TrackElement& element); + + // Append new elements. + inline void AddElement(const TrackElement& element); + inline void AddElement(const image_t image_id, const point2D_t point2D_idx); + inline void AddElements(const std::vector& elements); + + // Delete existing element. + inline void DeleteElement(const size_t idx); + void DeleteElement(const image_t image_id, const point2D_t point2D_idx); + + // Requests that the track capacity be at least enough to contain the + // specified number of elements. + inline void Reserve(const size_t num_elements); + + // Shrink the capacity of track vector to fit its size to save memory. + inline void Compress(); + + private: + std::vector elements_; +}; + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +size_t Track::Length() const { return elements_.size(); } + +const std::vector& Track::Elements() const { return elements_; } + +std::vector& Track::Elements() { return elements_; } + +void Track::SetElements(std::vector elements) { + elements_ = std::move(elements); +} + +// Access specific elements. +const TrackElement& Track::Element(const size_t idx) const { + return elements_.at(idx); +} + +TrackElement& Track::Element(const size_t idx) { return elements_.at(idx); } + +void Track::SetElement(const size_t idx, const TrackElement& element) { + elements_.at(idx) = element; +} + +void Track::AddElement(const TrackElement& element) { + elements_.push_back(element); +} + +void Track::AddElement(const image_t image_id, const point2D_t point2D_idx) { + elements_.emplace_back(image_id, point2D_idx); +} + +void Track::AddElements(const std::vector& elements) { + elements_.insert(elements_.end(), elements.begin(), elements.end()); +} + +void Track::DeleteElement(const size_t idx) { + CHECK_LT(idx, elements_.size()); + elements_.erase(elements_.begin() + idx); +} + +void Track::Reserve(const size_t num_elements) { + elements_.reserve(num_elements); +} + +void Track::Compress() { elements_.shrink_to_fit(); } + +} // namespace colmap + +#endif // COLMAP_SRC_BASE_TRACK_H_ diff --git a/include/colmap/base/triangulation.h b/include/colmap/base/triangulation.h new file mode 100644 index 0000000000000000000000000000000000000000..c1ee86d003211e51db179727d61ac6f2a19cf14f --- /dev/null +++ b/include/colmap/base/triangulation.h @@ -0,0 +1,118 @@ +// Copyright (c) 2022, 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 "base/camera.h" +#include "util/alignment.h" +#include "util/math.h" +#include "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/include/colmap/base/undistortion.h b/include/colmap/base/undistortion.h new file mode 100644 index 0000000000000000000000000000000000000000..69865e6516053011443a9aaab6fd7760112f34f7 --- /dev/null +++ b/include/colmap/base/undistortion.h @@ -0,0 +1,234 @@ +// Copyright (c) 2022, 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_UNDISTORTION_H_ +#define COLMAP_SRC_BASE_UNDISTORTION_H_ + +#include "base/reconstruction.h" +#include "util/alignment.h" +#include "util/bitmap.h" +#include "util/misc.h" +#include "util/threading.h" + +namespace colmap { + +struct UndistortCameraOptions { + // The amount of blank pixels in the undistorted image in the range [0, 1]. + double blank_pixels = 0.0; + + // Minimum and maximum scale change of camera used to satisfy the blank + // pixel constraint. + double min_scale = 0.2; + double max_scale = 2.0; + + // Maximum image size in terms of width or height of the undistorted camera. + int max_image_size = -1; + + // The 4 factors in the range [0, 1] that define the ROI (region of interest) + // in original image. The bounding box pixel coordinates are calculated as + // (roi_min_x * Width, roi_min_y * Height) and + // (roi_max_x * Width, roi_max_y * Height). + double roi_min_x = 0.0; + double roi_min_y = 0.0; + double roi_max_x = 1.0; + double roi_max_y = 1.0; +}; + +// Undistort images and export undistorted cameras, as required by the +// mvs::PatchMatchController class. +class COLMAPUndistorter : public Thread { + public: + COLMAPUndistorter( + const UndistortCameraOptions& options, + const Reconstruction& reconstruction, const std::string& image_path, + const std::string& output_path, const int num_related_images = 20, + const CopyType copy_type = CopyType::COPY, + const std::vector& image_ids = std::vector()); + + private: + void Run(); + + bool Undistort(const image_t image_id) const; + void WritePatchMatchConfig() const; + void WriteFusionConfig() const; + void WriteScript(const bool geometric) const; + + UndistortCameraOptions options_; + const std::string image_path_; + const std::string output_path_; + const CopyType copy_type_; + const int num_patch_match_src_images_; + const Reconstruction& reconstruction_; + const std::vector image_ids_; + std::vector image_names_; +}; + +// Undistort images and prepare data for CMVS/PMVS. +class PMVSUndistorter : public Thread { + public: + PMVSUndistorter(const UndistortCameraOptions& options, + const Reconstruction& reconstruction, + const std::string& image_path, + const std::string& output_path); + + private: + void Run(); + + bool Undistort(const size_t reg_image_idx) const; + void WriteVisibilityData() const; + void WriteOptionFile() const; + void WritePMVSScript() const; + void WriteCMVSPMVSScript() const; + void WriteCOLMAPScript(const bool geometric) const; + void WriteCMVSCOLMAPScript(const bool geometric) const; + + UndistortCameraOptions options_; + std::string image_path_; + std::string output_path_; + const Reconstruction& reconstruction_; +}; + +// Undistort images and prepare data for CMP-MVS. +class CMPMVSUndistorter : public Thread { + public: + CMPMVSUndistorter(const UndistortCameraOptions& options, + const Reconstruction& reconstruction, + const std::string& image_path, + const std::string& output_path); + + private: + void Run(); + + bool Undistort(const size_t reg_image_idx) const; + + UndistortCameraOptions options_; + std::string image_path_; + std::string output_path_; + const Reconstruction& reconstruction_; +}; + +// Undistort images and export undistorted cameras without the need for a +// reconstruction. Instead, the image names and camera model information are +// read from a text file. +class PureImageUndistorter : public Thread { + public: + PureImageUndistorter(const UndistortCameraOptions& options, + const std::string& image_path, + const std::string& output_path, + const std::vector>& + image_names_and_cameras); + + private: + void Run(); + + bool Undistort(const size_t reg_image_idx) const; + + UndistortCameraOptions options_; + std::string image_path_; + std::string output_path_; + const std::vector>& image_names_and_cameras_; +}; + +// Rectify stereo image pairs. +class StereoImageRectifier : public Thread { + public: + StereoImageRectifier( + const UndistortCameraOptions& options, + const Reconstruction& reconstruction, const std::string& image_path, + const std::string& output_path, + const std::vector>& stereo_pairs); + + private: + void Run(); + + void Rectify(const image_t image_id1, const image_t image_id2) const; + + UndistortCameraOptions options_; + std::string image_path_; + std::string output_path_; + const std::vector>& stereo_pairs_; + const Reconstruction& reconstruction_; +}; + +// Undistort camera by resizing the image and shifting the principal point. +// +// The scaling factor is computed such that no blank pixels are in the +// undistorted image (blank_pixels=0) or all pixels in distorted image are +// contained in output image (blank_pixels=1). +// +// The focal length of the image is preserved and the dimensions of the +// undistorted pinhole camera are adjusted such that either all pixels in +// the undistorted image have a corresponding pixel in the distorted image +// (i.e. no blank pixels at the borders, for `blank_pixels=0`), or all pixels +// in the distorted image project have a corresponding pixel in the undistorted +// image (i.e. blank pixels at the borders, for `blank_pixels=1`). Intermediate +// states can be achieved by setting `blank_pixels` between 0 and 1. +// +// The relative location of the principal point of the distorted camera is +// preserved. The scaling of the image dimensions is subject to the `min_scale`, +// `max_scale`, and `max_image_size` constraints. +Camera UndistortCamera(const UndistortCameraOptions& options, + const Camera& camera); + +// Undistort image such that the viewing geometry of the undistorted image +// follows a pinhole camera model. See `UndistortCamera` for more details +// on the undistortion conventions. +void UndistortImage(const UndistortCameraOptions& options, + const Bitmap& distorted_image, + const Camera& distorted_camera, Bitmap* undistorted_image, + Camera* undistorted_camera); + +// Undistort all cameras in the reconstruction and accordingly all +// observations in their corresponding images. +void UndistortReconstruction(const UndistortCameraOptions& options, + Reconstruction* reconstruction); + +// Compute stereo rectification homographies that transform two images, +// such that corresponding pixels in one image lie on the same scanline in the +// other image. The matrix Q transforms disparity values to world coordinates +// as [x, y, disparity, 1] * Q = [X, Y, Z, 1] * w. Note that this function +// assumes that the two cameras are already undistorted. +void RectifyStereoCameras(const Camera& camera1, const Camera& camera2, + const Eigen::Vector4d& qvec, + const Eigen::Vector3d& tvec, Eigen::Matrix3d* H1, + Eigen::Matrix3d* H2, Eigen::Matrix4d* Q); + +// Rectify and undistort the stereo image pair using the given geometry. +void RectifyAndUndistortStereoImages( + const UndistortCameraOptions& options, const Bitmap& distorted_image1, + const Bitmap& distorted_image2, const Camera& distorted_camera1, + const Camera& distorted_camera2, const Eigen::Vector4d& qvec, + const Eigen::Vector3d& tvec, Bitmap* undistorted_image1, + Bitmap* undistorted_image2, Camera* undistorted_camera, Eigen::Matrix4d* Q); + +} // namespace colmap + +#endif // COLMAP_SRC_BASE_UNDISTORTION_H_ diff --git a/include/colmap/base/visibility_pyramid.h b/include/colmap/base/visibility_pyramid.h new file mode 100644 index 0000000000000000000000000000000000000000..e5dc33d493e3ba0aca6abe846d021ce6093dc4a3 --- /dev/null +++ b/include/colmap/base/visibility_pyramid.h @@ -0,0 +1,104 @@ +// Copyright (c) 2022, 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_VISIBILITY_PYRAMID_H_ +#define COLMAP_SRC_BASE_VISIBILITY_PYRAMID_H_ + +#include + +#include + +#include "util/alignment.h" + +namespace colmap { + +// A class that captures the distribution of points in a 2D grid. +// For example, to capture the distribution of visible 3D points in an image. +// +// The class captures the distribution of points by a score. A higher score +// corresponds to a more uniform distribution of the points in the grid. +// +// The score is computed by the number of populated cells in a multi-resolution +// pyramid. A populated cell contributes to the overall score if it is +// populated by at least one point and the contributed score is according +// to its resolution in the pyramid. A cell in a higher resolution level +// contributes a higher score to the overall score. +class VisibilityPyramid { + public: + VisibilityPyramid(); + VisibilityPyramid(const size_t num_levels, const size_t width, + const size_t height); + + void SetPoint(const double x, const double y); + void ResetPoint(const double x, const double y); + + inline size_t NumLevels() const; + inline size_t Width() const; + inline size_t Height() const; + + inline size_t Score() const; + inline size_t MaxScore() const; + + private: + void CellForPoint(const double x, const double y, size_t* cx, + size_t* cy) const; + + // Range of the input points. + size_t width_; + size_t height_; + + // The overall visibility score. + size_t score_; + + // The maximum score when all cells are populated. + size_t max_score_; + + // The visibilty pyramid with multiple levels. + std::vector pyramid_; +}; + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +size_t VisibilityPyramid::NumLevels() const { return pyramid_.size(); } + +size_t VisibilityPyramid::Width() const { return width_; } + +size_t VisibilityPyramid::Height() const { return height_; } + +size_t VisibilityPyramid::Score() const { return score_; } + +size_t VisibilityPyramid::MaxScore() const { return max_score_; } + +} // namespace colmap + +#endif // COLMAP_SRC_BASE_VISIBILITY_PYRAMID_H_ diff --git a/include/colmap/base/warp.h b/include/colmap/base/warp.h new file mode 100644 index 0000000000000000000000000000000000000000..c719154476e087dcf396324aee56293e74c30737 --- /dev/null +++ b/include/colmap/base/warp.h @@ -0,0 +1,80 @@ +// Copyright (c) 2022, 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_WARP_H_ +#define COLMAP_SRC_BASE_WARP_H_ + +#include "base/camera.h" +#include "util/alignment.h" +#include "util/bitmap.h" + +namespace colmap { + +// Warp source image to target image by projecting the pixels of the target +// image up to infinity and projecting it down into the source image +// (i.e. an inverse mapping). The function allocates the target image. +void WarpImageBetweenCameras(const Camera& source_camera, + const Camera& target_camera, + const Bitmap& source_image, Bitmap* target_image); + +// Warp an image with the given homography, where H defines the pixel mapping +// from the target to source image. Note that the pixel centers are assumed to +// have coordinates (0.5, 0.5). +void WarpImageWithHomography(const Eigen::Matrix3d& H, + const Bitmap& source_image, Bitmap* target_image); + +// First, warp source image to target image by projecting the pixels of the +// target image up to infinity and projecting it down into the source image +// (i.e. an inverse mapping). Second, warp the coordinates from the first +// warping with the given homography. The function allocates the target image. +void WarpImageWithHomographyBetweenCameras(const Eigen::Matrix3d& H, + const Camera& source_camera, + const Camera& target_camera, + const Bitmap& source_image, + Bitmap* target_image); + +// Resample row-major image using bilinear interpolation. +void ResampleImageBilinear(const float* data, const int rows, const int cols, + const int new_rows, const int new_cols, + float* resampled); + +// Smooth row-major image using a Gaussian filter kernel. +void SmoothImage(const float* data, const int rows, const int cols, + const float sigma_r, const float sigma_c, float* smoothed); + +// Downsample row-major image by first smoothing and then resampling. +void DownsampleImage(const float* data, const int rows, const int cols, + const int new_rows, const int new_cols, + float* downsampled); + +} // namespace colmap + +#endif // COLMAP_SRC_BASE_WARP_H_ diff --git a/include/colmap/controllers/automatic_reconstruction.h b/include/colmap/controllers/automatic_reconstruction.h new file mode 100644 index 0000000000000000000000000000000000000000..1546f49d25d60f91842a0c620fd712846ac34d7b --- /dev/null +++ b/include/colmap/controllers/automatic_reconstruction.h @@ -0,0 +1,123 @@ +// Copyright (c) 2022, 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_CONTROLLERS_AUTOMATIC_RECONSTRUCTION_H_ +#define COLMAP_SRC_CONTROLLERS_AUTOMATIC_RECONSTRUCTION_H_ + +#include + +#include "base/reconstruction_manager.h" +#include "util/option_manager.h" +#include "util/threading.h" + +namespace colmap { + +class AutomaticReconstructionController : public Thread { + public: + enum class DataType { INDIVIDUAL, VIDEO, INTERNET }; + enum class Quality { LOW, MEDIUM, HIGH, EXTREME }; + enum class Mesher { POISSON, DELAUNAY }; + + struct Options { + // The path to the workspace folder in which all results are stored. + std::string workspace_path; + + // The path to the image folder which are used as input. + std::string image_path; + + // The path to the mask folder which are used as input. + std::string mask_path; + + // The path to the vocabulary tree for feature matching. + std::string vocab_tree_path; + + // The type of input data used to choose optimal mapper settings. + DataType data_type = DataType::INDIVIDUAL; + + // Whether to perform low- or high-quality reconstruction. + Quality quality = Quality::HIGH; + + // Whether to use shared intrinsics or not. + bool single_camera = false; + + // Which camera model to use for images. + std::string camera_model = "SIMPLE_RADIAL"; + + // Whether to perform sparse mapping. + bool sparse = true; + +// Whether to perform dense mapping. +#ifdef CUDA_ENABLED + bool dense = true; +#else + bool dense = false; +#endif + + // The meshing algorithm to be used. + Mesher mesher = Mesher::POISSON; + + // The number of threads to use in all stages. + int num_threads = -1; + + // Whether to use the GPU in feature extraction and matching. + bool use_gpu = true; + + // Index of the GPU used for GPU stages. For multi-GPU computation, + // you should separate multiple GPU indices by comma, e.g., "0,1,2,3". + // By default, all GPUs will be used in all stages. + std::string gpu_index = "-1"; + }; + + AutomaticReconstructionController( + const Options& options, ReconstructionManager* reconstruction_manager); + + void Stop() override; + + private: + void Run() override; + void RunFeatureExtraction(); + void RunFeatureMatching(); + void RunSparseMapper(); + void RunDenseMapper(); + + const Options options_; + OptionManager option_manager_; + ReconstructionManager* reconstruction_manager_; + Thread* active_thread_; + std::unique_ptr feature_extractor_; + std::unique_ptr exhaustive_matcher_; + std::unique_ptr sequential_matcher_; + std::unique_ptr vocab_tree_matcher_; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_CONTROLLERS_AUTOMATIC_RECONSTRUCTION_H_ diff --git a/include/colmap/controllers/bundle_adjustment.h b/include/colmap/controllers/bundle_adjustment.h new file mode 100644 index 0000000000000000000000000000000000000000..ff0eeeb345160629fe457cfec67032f8edb5ddeb --- /dev/null +++ b/include/colmap/controllers/bundle_adjustment.h @@ -0,0 +1,56 @@ +// Copyright (c) 2022, 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_CONTROLLERS_BUNDLE_ADJUSTMENT_H_ +#define COLMAP_SRC_CONTROLLERS_BUNDLE_ADJUSTMENT_H_ + +#include "base/reconstruction.h" +#include "util/option_manager.h" +#include "util/threading.h" + +namespace colmap { + +// Class that controls the global bundle adjustment procedure. +class BundleAdjustmentController : public Thread { + public: + BundleAdjustmentController(const OptionManager& options, + Reconstruction* reconstruction); + + private: + void Run(); + + const OptionManager options_; + Reconstruction* reconstruction_; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_CONTROLLERS_BUNDLE_ADJUSTMENT_H_ diff --git a/include/colmap/controllers/hierarchical_mapper.h b/include/colmap/controllers/hierarchical_mapper.h new file mode 100644 index 0000000000000000000000000000000000000000..7e8c87f19a46a78a01f6e049bd4c8d0df7bfe7e2 --- /dev/null +++ b/include/colmap/controllers/hierarchical_mapper.h @@ -0,0 +1,82 @@ +// Copyright (c) 2022, 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_CONTROLLERS_HIERARCHICAL_MAPPER_H_ +#define COLMAP_SRC_CONTROLLERS_HIERARCHICAL_MAPPER_H_ + +#include "base/reconstruction_manager.h" +#include "base/scene_clustering.h" +#include "controllers/incremental_mapper.h" +#include "util/threading.h" + +namespace colmap { + +// Hierarchical mapping first hierarchically partitions the scene into multiple +// overlapping clusters, then reconstructs them separately using incremental +// mapping, and finally merges them all into a globally consistent +// reconstruction. This is especially useful for larger-scale scenes, since +// incremental mapping becomes slow with an increasing number of images. +class HierarchicalMapperController : public Thread { + public: + struct Options { + // The path to the image folder which are used as input. + std::string image_path; + + // The path to the database file which is used as input. + std::string database_path; + + // The maximum number of trials to initialize a cluster. + int init_num_trials = 10; + + // The number of workers used to reconstruct clusters in parallel. + int num_workers = -1; + + bool Check() const; + }; + + HierarchicalMapperController( + const Options& options, + const SceneClustering::Options& clustering_options, + const IncrementalMapperOptions& mapper_options, + ReconstructionManager* reconstruction_manager); + + private: + void Run() override; + + const Options options_; + const SceneClustering::Options clustering_options_; + const IncrementalMapperOptions mapper_options_; + ReconstructionManager* reconstruction_manager_; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_CONTROLLERS_HIERARCHICAL_MAPPER_H_ diff --git a/include/colmap/controllers/incremental_mapper.h b/include/colmap/controllers/incremental_mapper.h new file mode 100644 index 0000000000000000000000000000000000000000..7e26847cfe5ab786fa9af70188d490f7898b4e2d --- /dev/null +++ b/include/colmap/controllers/incremental_mapper.h @@ -0,0 +1,199 @@ +// Copyright (c) 2022, 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_CONTROLLERS_INCREMENTAL_MAPPER_H_ +#define COLMAP_SRC_CONTROLLERS_INCREMENTAL_MAPPER_H_ + +#include "base/reconstruction_manager.h" +#include "sfm/incremental_mapper.h" +#include "util/threading.h" + +namespace colmap { + +struct IncrementalMapperOptions { + public: + // The minimum number of matches for inlier matches to be considered. + int min_num_matches = 15; + + // Whether to ignore the inlier matches of watermark image pairs. + bool ignore_watermarks = false; + + // Whether to reconstruct multiple sub-models. + bool multiple_models = true; + + // The number of sub-models to reconstruct. + int max_num_models = 50; + + // The maximum number of overlapping images between sub-models. If the + // current sub-models shares more than this number of images with another + // model, then the reconstruction is stopped. + int max_model_overlap = 20; + + // The minimum number of registered images of a sub-model, otherwise the + // sub-model is discarded. + int min_model_size = 10; + + // The image identifiers used to initialize the reconstruction. Note that + // only one or both image identifiers can be specified. In the former case, + // the second image is automatically determined. + int init_image_id1 = -1; + int init_image_id2 = -1; + + // The number of trials to initialize the reconstruction. + int init_num_trials = 200; + + // Whether to extract colors for reconstructed points. + bool extract_colors = true; + + // The number of threads to use during reconstruction. + int num_threads = -1; + + // Thresholds for filtering images with degenerate intrinsics. + double min_focal_length_ratio = 0.1; + double max_focal_length_ratio = 10.0; + double max_extra_param = 1.0; + + // Which intrinsic parameters to optimize during the reconstruction. + bool ba_refine_focal_length = true; + bool ba_refine_principal_point = false; + bool ba_refine_extra_params = true; + + // The minimum number of residuals per bundle adjustment problem to + // enable multi-threading solving of the problems. + int ba_min_num_residuals_for_multi_threading = 50000; + + // The number of images to optimize in local bundle adjustment. + int ba_local_num_images = 6; + + // Ceres solver function tolerance for local bundle adjustment + double ba_local_function_tolerance = 0.0; + + // The maximum number of local bundle adjustment iterations. + int ba_local_max_num_iterations = 25; + + // Whether to use PBA in global bundle adjustment. + bool ba_global_use_pba = false; + + // The GPU index for PBA bundle adjustment. + int ba_global_pba_gpu_index = -1; + + // The growth rates after which to perform global bundle adjustment. + double ba_global_images_ratio = 1.1; + double ba_global_points_ratio = 1.1; + int ba_global_images_freq = 500; + int ba_global_points_freq = 250000; + + // Ceres solver function tolerance for global bundle adjustment + double ba_global_function_tolerance = 0.0; + + // The maximum number of global bundle adjustment iterations. + int ba_global_max_num_iterations = 50; + + // The thresholds for iterative bundle adjustment refinements. + int ba_local_max_refinements = 2; + double ba_local_max_refinement_change = 0.001; + int ba_global_max_refinements = 5; + double ba_global_max_refinement_change = 0.0005; + + // Path to a folder with reconstruction snapshots during incremental + // reconstruction. Snapshots will be saved according to the specified + // frequency of registered images. + std::string snapshot_path = ""; + int snapshot_images_freq = 0; + + // Which images to reconstruct. If no images are specified, all images will + // be reconstructed by default. + std::unordered_set image_names; + + // If reconstruction is provided as input, fix the existing image poses. + bool fix_existing_images = false; + + IncrementalMapper::Options Mapper() const; + IncrementalTriangulator::Options Triangulation() const; + BundleAdjustmentOptions LocalBundleAdjustment() const; + BundleAdjustmentOptions GlobalBundleAdjustment() const; + ParallelBundleAdjuster::Options ParallelGlobalBundleAdjustment() const; + + bool Check() const; + + private: + friend class OptionManager; + friend class MapperGeneralOptionsWidget; + friend class MapperTriangulationOptionsWidget; + friend class MapperRegistrationOptionsWidget; + friend class MapperInitializationOptionsWidget; + friend class MapperBundleAdjustmentOptionsWidget; + friend class MapperFilteringOptionsWidget; + friend class ReconstructionOptionsWidget; + IncrementalMapper::Options mapper; + IncrementalTriangulator::Options triangulation; +}; + +// Class that controls the incremental mapping procedure by iteratively +// initializing reconstructions from the same scene graph. +class IncrementalMapperController : public Thread { + public: + enum { + INITIAL_IMAGE_PAIR_REG_CALLBACK, + NEXT_IMAGE_REG_CALLBACK, + LAST_IMAGE_REG_CALLBACK, + }; + + IncrementalMapperController(const IncrementalMapperOptions* options, + const std::string& image_path, + const std::string& database_path, + ReconstructionManager* reconstruction_manager); + + private: + void Run(); + bool LoadDatabase(); + void Reconstruct(const IncrementalMapper::Options& init_mapper_options); + + const IncrementalMapperOptions* options_; + const std::string image_path_; + const std::string database_path_; + ReconstructionManager* reconstruction_manager_; + DatabaseCache database_cache_; +}; + +// Globally filter points and images in mapper. +size_t FilterPoints(const IncrementalMapperOptions& options, + IncrementalMapper* mapper); +size_t FilterImages(const IncrementalMapperOptions& options, + IncrementalMapper* mapper); + +// Globally complete and merge tracks in mapper. +size_t CompleteAndMergeTracks(const IncrementalMapperOptions& options, + IncrementalMapper* mapper); + +} // namespace colmap + +#endif // COLMAP_SRC_CONTROLLERS_INCREMENTAL_MAPPER_H_ diff --git a/include/colmap/estimators/absolute_pose.h b/include/colmap/estimators/absolute_pose.h new file mode 100644 index 0000000000000000000000000000000000000000..6cc63d6a6c5c6e9242a0050bb7757542565897f3 --- /dev/null +++ b/include/colmap/estimators/absolute_pose.h @@ -0,0 +1,182 @@ +// Copyright (c) 2022, 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_ABSOLUTE_POSE_H_ +#define COLMAP_SRC_ESTIMATORS_ABSOLUTE_POSE_H_ + +#include +#include + +#include + +#include "util/alignment.h" +#include "util/types.h" + +namespace colmap { + +// Analytic solver for the P3P (Perspective-Three-Point) problem. +// +// The algorithm is based on the following paper: +// +// X.S. Gao, X.-R. Hou, J. Tang, H.-F. Chang. Complete Solution +// Classification for the Perspective-Three-Point Problem. +// http://www.mmrc.iss.ac.cn/~xgao/paper/ieee.pdf +class P3PEstimator { + public: + // The 2D image feature observations. + typedef Eigen::Vector2d X_t; + // The observed 3D features in the world frame. + typedef Eigen::Vector3d Y_t; + // The transformation from the world to the camera frame. + typedef Eigen::Matrix3x4d M_t; + + // The minimum number of samples needed to estimate a model. + static const int kMinNumSamples = 3; + + // Estimate the most probable solution of the P3P problem from a set of + // three 2D-3D point correspondences. + // + // @param points2D Normalized 2D image points as 3x2 matrix. + // @param points3D 3D world points as 3x3 matrix. + // + // @return Most probable pose as length-1 vector of a 3x4 matrix. + static std::vector Estimate(const std::vector& points2D, + const std::vector& points3D); + + // Calculate the squared reprojection error given a set of 2D-3D point + // correspondences and a projection matrix. + // + // @param points2D Normalized 2D image points as Nx2 matrix. + // @param points3D 3D world points as Nx3 matrix. + // @param proj_matrix 3x4 projection matrix. + // @param residuals Output vector of residuals. + static void Residuals(const std::vector& points2D, + const std::vector& points3D, + const M_t& proj_matrix, std::vector* residuals); +}; + +// EPNP solver for the PNP (Perspective-N-Point) problem. The solver needs a +// minimum of 4 2D-3D correspondences. +// +// The algorithm is based on the following paper: +// +// Lepetit, Vincent, Francesc Moreno-Noguer, and Pascal Fua. +// "Epnp: An accurate o (n) solution to the pnp problem." +// International journal of computer vision 81.2 (2009): 155-166. +// +// The implementation is based on their original open-source release, but is +// ported to Eigen and contains several improvements over the original code. +class EPNPEstimator { + public: + // The 2D image feature observations. + typedef Eigen::Vector2d X_t; + // The observed 3D features in the world frame. + typedef Eigen::Vector3d Y_t; + // The transformation from the world to the camera frame. + typedef Eigen::Matrix3x4d M_t; + + // The minimum number of samples needed to estimate a model. + static const int kMinNumSamples = 4; + + // Estimate the most probable solution of the P3P problem from a set of + // three 2D-3D point correspondences. + // + // @param points2D Normalized 2D image points as 3x2 matrix. + // @param points3D 3D world points as 3x3 matrix. + // + // @return Most probable pose as length-1 vector of a 3x4 matrix. + static std::vector Estimate(const std::vector& points2D, + const std::vector& points3D); + + // Calculate the squared reprojection error given a set of 2D-3D point + // correspondences and a projection matrix. + // + // @param points2D Normalized 2D image points as Nx2 matrix. + // @param points3D 3D world points as Nx3 matrix. + // @param proj_matrix 3x4 projection matrix. + // @param residuals Output vector of residuals. + static void Residuals(const std::vector& points2D, + const std::vector& points3D, + const M_t& proj_matrix, std::vector* residuals); + + private: + bool ComputePose(const std::vector& points2D, + const std::vector& points3D, + Eigen::Matrix3x4d* proj_matrix); + + void ChooseControlPoints(); + bool ComputeBarycentricCoordinates(); + + Eigen::Matrix ComputeM(); + Eigen::Matrix ComputeL6x10( + const Eigen::Matrix& Ut); + Eigen::Matrix ComputeRho(); + + void FindBetasApprox1(const Eigen::Matrix& L_6x10, + const Eigen::Matrix& rho, + Eigen::Vector4d* betas); + void FindBetasApprox2(const Eigen::Matrix& L_6x10, + const Eigen::Matrix& rho, + Eigen::Vector4d* betas); + void FindBetasApprox3(const Eigen::Matrix& L_6x10, + const Eigen::Matrix& rho, + Eigen::Vector4d* betas); + + void RunGaussNewton(const Eigen::Matrix& L_6x10, + const Eigen::Matrix& rho, + Eigen::Vector4d* betas); + + double ComputeRT(const Eigen::Matrix& Ut, + const Eigen::Vector4d& betas, Eigen::Matrix3d* R, + Eigen::Vector3d* t); + + void ComputeCcs(const Eigen::Vector4d& betas, + const Eigen::Matrix& Ut); + void ComputePcs(); + + void SolveForSign(); + + void EstimateRT(Eigen::Matrix3d* R, Eigen::Vector3d* t); + + double ComputeTotalReprojectionError(const Eigen::Matrix3d& R, + const Eigen::Vector3d& t); + + const std::vector* points2D_ = nullptr; + const std::vector* points3D_ = nullptr; + std::vector pcs_; + std::vector alphas_; + std::array cws_; + std::array ccs_; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_ESTIMATORS_ABSOLUTE_POSE_H_ diff --git a/include/colmap/estimators/affine_transform.h b/include/colmap/estimators/affine_transform.h new file mode 100644 index 0000000000000000000000000000000000000000..b4025fe80d117bbd19a19249687e4120da880fc3 --- /dev/null +++ b/include/colmap/estimators/affine_transform.h @@ -0,0 +1,65 @@ +// Copyright (c) 2022, 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_AFFINE_TRANSFORM_H_ +#define COLMAP_SRC_ESTIMATORS_AFFINE_TRANSFORM_H_ + +#include + +#include + +#include "util/alignment.h" +#include "util/types.h" + +namespace colmap { + +class AffineTransformEstimator { + public: + typedef Eigen::Vector2d X_t; + typedef Eigen::Vector2d Y_t; + typedef Eigen::Matrix M_t; + + // The minimum number of samples needed to estimate a model. + static const int kMinNumSamples = 3; + + // Estimate the affine transformation from at least 3 correspondences. + static std::vector Estimate(const std::vector& points1, + const std::vector& points2); + + // Compute the squared transformation error. + static void Residuals(const std::vector& points1, + const std::vector& points2, const M_t& E, + std::vector* residuals); +}; + +} // namespace colmap + +#endif // COLMAP_SRC_ESTIMATORS_AFFINE_TRANSFORM_H_ diff --git a/include/colmap/estimators/coordinate_frame.h b/include/colmap/estimators/coordinate_frame.h new file mode 100644 index 0000000000000000000000000000000000000000..f8cbc6fca3b839cb6e7f7ae4abb32bae13451125 --- /dev/null +++ b/include/colmap/estimators/coordinate_frame.h @@ -0,0 +1,88 @@ +// Copyright (c) 2022, 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_COORDINATE_AXES_H_ +#define COLMAP_SRC_ESTIMATORS_COORDINATE_AXES_H_ + +#include + +#include "base/reconstruction.h" + +namespace colmap { + +struct ManhattanWorldFrameEstimationOptions { + // The maximum image size for line detection. + int max_image_size = 1024; + // The minimum length of line segments in pixels. + double min_line_length = 3; + // The tolerance for classifying lines into horizontal/vertical. + double line_orientation_tolerance = 0.2; + // The maximum distance in pixels between lines and the vanishing points. + double max_line_vp_distance = 0.5; + // The maximum cosine distance between estimated axes to be inliers. + double max_axis_distance = 0.05; +}; + +// Estimate gravity vector by assuming gravity-aligned image orientation, i.e. +// the majority of images is assumed to have the gravity vector aligned with an +// upright image plane. +Eigen::Vector3d EstimateGravityVectorFromImageOrientation( + const Reconstruction& reconstruction, + const double max_axis_distance = 0.05); + +// Estimate the coordinate frame of the reconstruction assuming a Manhattan +// world by finding the major vanishing points in each image. This function +// assumes that the majority of images is taken in upright direction, i.e. +// people are standing upright in the image. The orthonormal axes of the +// estimated coordinate frame will be given in the columns of the returned +// matrix. If one axis could not be determined, the respective column will be +// zero. The axes are specified in the world coordinate system in the order +// rightward, downward, forward. +Eigen::Matrix3d EstimateManhattanWorldFrame( + const ManhattanWorldFrameEstimationOptions& options, + const Reconstruction& reconstruction, const std::string& image_path); + +// Aligns the reconstruction to the plane defined by running PCA on the 3D +// points. The model centroid is at the origin of the new coordinate system +// and the X axis is the first principal component with the Y axis being the +// second principal component +void AlignToPrincipalPlane(Reconstruction* recon, SimilarityTransform3* tform); + +// Aligns the reconstruction to the local ENU plane orientation. Rotates the +// reconstruction such that the x-y plane aligns with the ENU tangent plane at +// the point cloud centroid and translates the origin to the centroid. +// If unscaled == true, then the original scale of the model remains unchanged. +void AlignToENUPlane(Reconstruction* recon, SimilarityTransform3* tform, + bool unscaled); + +} // namespace colmap + +#endif // COLMAP_SRC_ESTIMATORS_COORDINATE_AXES_H_ diff --git a/include/colmap/estimators/essential_matrix.h b/include/colmap/estimators/essential_matrix.h new file mode 100644 index 0000000000000000000000000000000000000000..21663ed3090f022b1045afe2e898c79b166b4d22 --- /dev/null +++ b/include/colmap/estimators/essential_matrix.h @@ -0,0 +1,127 @@ +// Copyright (c) 2022, 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_ESSENTIAL_MATRIX_H_ +#define COLMAP_SRC_ESTIMATORS_ESSENTIAL_MATRIX_H_ + +#include + +#include + +#include + +#include "util/alignment.h" +#include "util/types.h" + +namespace colmap { + +// Essential matrix estimator from corresponding normalized point pairs. +// +// This algorithm solves the 5-Point problem based on the following paper: +// +// D. Nister, An efficient solution to the five-point relative pose problem, +// IEEE-T-PAMI, 26(6), 2004. +// http://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.86.8769 +class EssentialMatrixFivePointEstimator { + public: + typedef Eigen::Vector2d X_t; + typedef Eigen::Vector2d Y_t; + typedef Eigen::Matrix3d M_t; + + // The minimum number of samples needed to estimate a model. + static const int kMinNumSamples = 5; + + // Estimate up to 10 possible essential matrix solutions from a set of + // corresponding points. + // + // The number of corresponding points must be at least 5. + // + // @param points1 First set of corresponding points. + // @param points2 Second set of corresponding points. + // + // @return Up to 10 solutions as a vector of 3x3 essential matrices. + static std::vector Estimate(const std::vector& points1, + const std::vector& points2); + + // Calculate the residuals of a set of corresponding points and a given + // essential matrix. + // + // Residuals are defined as the squared Sampson error. + // + // @param points1 First set of corresponding points. + // @param points2 Second set of corresponding points. + // @param E 3x3 essential matrix. + // @param residuals Output vector of residuals. + static void Residuals(const std::vector& points1, + const std::vector& points2, const M_t& E, + std::vector* residuals); +}; + +// Essential matrix estimator from corresponding normalized point pairs. +// +// This algorithm solves the 8-Point problem based on the following paper: +// +// Hartley and Zisserman, Multiple View Geometry, algorithm 11.1, page 282. +class EssentialMatrixEightPointEstimator { + public: + typedef Eigen::Vector2d X_t; + typedef Eigen::Vector2d Y_t; + typedef Eigen::Matrix3d M_t; + + // The minimum number of samples needed to estimate a model. + static const int kMinNumSamples = 8; + + // Estimate essential matrix solutions from set of corresponding points. + // + // The number of corresponding points must be at least 8. + // + // @param points1 First set of corresponding points. + // @param points2 Second set of corresponding points. + static std::vector Estimate(const std::vector& points1, + const std::vector& points2); + + // Calculate the residuals of a set of corresponding points and a given + // essential matrix. + // + // Residuals are defined as the squared Sampson error. + // + // @param points1 First set of corresponding points. + // @param points2 Second set of corresponding points. + // @param E 3x3 essential matrix. + // @param residuals Output vector of residuals. + static void Residuals(const std::vector& points1, + const std::vector& points2, const M_t& E, + std::vector* residuals); +}; + +} // namespace colmap + +#endif // COLMAP_SRC_ESTIMATORS_ESSENTIAL_MATRIX_H_ diff --git a/include/colmap/estimators/essential_matrix_coeffs.h b/include/colmap/estimators/essential_matrix_coeffs.h new file mode 100644 index 0000000000000000000000000000000000000000..a3ae8f36ded45bbdb748a1745f06af8af1d76268 --- /dev/null +++ b/include/colmap/estimators/essential_matrix_coeffs.h @@ -0,0 +1,206 @@ +// Copyright (c) 2022, 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) + +{ + const double* b = B.data(); + + coeffs(0) = b[0] * b[17] * b[34] + b[26] * b[4] * b[21] - + b[26] * b[17] * b[8] - b[13] * b[4] * b[34] - + b[0] * b[21] * b[30] + b[13] * b[30] * b[8]; + coeffs(1) = + b[26] * b[4] * b[22] + b[14] * b[30] * b[8] + b[13] * b[31] * b[8] + + b[1] * b[17] * b[34] - b[13] * b[5] * b[34] + b[26] * b[5] * b[21] - + b[0] * b[21] * b[31] - b[26] * b[17] * b[9] - b[1] * b[21] * b[30] + + b[27] * b[4] * b[21] + b[0] * b[17] * b[35] - b[0] * b[22] * b[30] + + b[13] * b[30] * b[9] + b[0] * b[18] * b[34] - b[27] * b[17] * b[8] - + b[14] * b[4] * b[34] - b[13] * b[4] * b[35] - b[26] * b[18] * b[8]; + coeffs(2) = + b[14] * b[30] * b[9] + b[14] * b[31] * b[8] + b[13] * b[31] * b[9] - + b[13] * b[4] * b[36] - b[13] * b[5] * b[35] + b[15] * b[30] * b[8] - + b[13] * b[6] * b[34] + b[13] * b[30] * b[10] + b[13] * b[32] * b[8] - + b[14] * b[4] * b[35] - b[14] * b[5] * b[34] + b[26] * b[4] * b[23] + + b[26] * b[5] * b[22] + b[26] * b[6] * b[21] - b[26] * b[17] * b[10] - + b[15] * b[4] * b[34] - b[26] * b[18] * b[9] - b[26] * b[19] * b[8] + + b[27] * b[4] * b[22] + b[27] * b[5] * b[21] - b[27] * b[17] * b[9] - + b[27] * b[18] * b[8] - b[1] * b[21] * b[31] - b[0] * b[23] * b[30] - + b[0] * b[21] * b[32] + b[28] * b[4] * b[21] - b[28] * b[17] * b[8] + + b[2] * b[17] * b[34] + b[0] * b[18] * b[35] - b[0] * b[22] * b[31] + + b[0] * b[17] * b[36] + b[0] * b[19] * b[34] - b[1] * b[22] * b[30] + + b[1] * b[18] * b[34] + b[1] * b[17] * b[35] - b[2] * b[21] * b[30]; + coeffs(3) = + b[14] * b[30] * b[10] + b[14] * b[32] * b[8] - b[3] * b[21] * b[30] + + b[3] * b[17] * b[34] + b[13] * b[32] * b[9] + b[13] * b[33] * b[8] - + b[13] * b[4] * b[37] - b[13] * b[5] * b[36] + b[15] * b[30] * b[9] + + b[15] * b[31] * b[8] - b[16] * b[4] * b[34] - b[13] * b[6] * b[35] - + b[13] * b[7] * b[34] + b[13] * b[30] * b[11] + b[13] * b[31] * b[10] + + b[14] * b[31] * b[9] - b[14] * b[4] * b[36] - b[14] * b[5] * b[35] - + b[14] * b[6] * b[34] + b[16] * b[30] * b[8] - b[26] * b[20] * b[8] + + b[26] * b[4] * b[24] + b[26] * b[5] * b[23] + b[26] * b[6] * b[22] + + b[26] * b[7] * b[21] - b[26] * b[17] * b[11] - b[15] * b[4] * b[35] - + b[15] * b[5] * b[34] - b[26] * b[18] * b[10] - b[26] * b[19] * b[9] + + b[27] * b[4] * b[23] + b[27] * b[5] * b[22] + b[27] * b[6] * b[21] - + b[27] * b[17] * b[10] - b[27] * b[18] * b[9] - b[27] * b[19] * b[8] + + b[0] * b[17] * b[37] - b[0] * b[23] * b[31] - b[0] * b[24] * b[30] - + b[0] * b[21] * b[33] - b[29] * b[17] * b[8] + b[28] * b[4] * b[22] + + b[28] * b[5] * b[21] - b[28] * b[17] * b[9] - b[28] * b[18] * b[8] + + b[29] * b[4] * b[21] + b[1] * b[19] * b[34] - b[2] * b[21] * b[31] + + b[0] * b[20] * b[34] + b[0] * b[19] * b[35] + b[0] * b[18] * b[36] - + b[0] * b[22] * b[32] - b[1] * b[23] * b[30] - b[1] * b[21] * b[32] + + b[1] * b[18] * b[35] - b[1] * b[22] * b[31] - b[2] * b[22] * b[30] + + b[2] * b[17] * b[35] + b[1] * b[17] * b[36] + b[2] * b[18] * b[34]; + coeffs(4) = + -b[14] * b[6] * b[35] - b[14] * b[7] * b[34] - b[3] * b[22] * b[30] - + b[3] * b[21] * b[31] + b[3] * b[17] * b[35] + b[3] * b[18] * b[34] + + b[13] * b[32] * b[10] + b[13] * b[33] * b[9] - b[13] * b[4] * b[38] - + b[13] * b[5] * b[37] - b[15] * b[6] * b[34] + b[15] * b[30] * b[10] + + b[15] * b[32] * b[8] - b[16] * b[4] * b[35] - b[13] * b[6] * b[36] - + b[13] * b[7] * b[35] + b[13] * b[31] * b[11] + b[13] * b[30] * b[12] + + b[14] * b[32] * b[9] + b[14] * b[33] * b[8] - b[14] * b[4] * b[37] - + b[14] * b[5] * b[36] + b[16] * b[30] * b[9] + b[16] * b[31] * b[8] - + b[26] * b[20] * b[9] + b[26] * b[4] * b[25] + b[26] * b[5] * b[24] + + b[26] * b[6] * b[23] + b[26] * b[7] * b[22] - b[26] * b[17] * b[12] + + b[14] * b[30] * b[11] + b[14] * b[31] * b[10] + b[15] * b[31] * b[9] - + b[15] * b[4] * b[36] - b[15] * b[5] * b[35] - b[26] * b[18] * b[11] - + b[26] * b[19] * b[10] - b[27] * b[20] * b[8] + b[27] * b[4] * b[24] + + b[27] * b[5] * b[23] + b[27] * b[6] * b[22] + b[27] * b[7] * b[21] - + b[27] * b[17] * b[11] - b[27] * b[18] * b[10] - b[27] * b[19] * b[9] - + b[16] * b[5] * b[34] - b[29] * b[17] * b[9] - b[29] * b[18] * b[8] + + b[28] * b[4] * b[23] + b[28] * b[5] * b[22] + b[28] * b[6] * b[21] - + b[28] * b[17] * b[10] - b[28] * b[18] * b[9] - b[28] * b[19] * b[8] + + b[29] * b[4] * b[22] + b[29] * b[5] * b[21] - b[2] * b[23] * b[30] + + b[2] * b[18] * b[35] - b[1] * b[22] * b[32] - b[2] * b[21] * b[32] + + b[2] * b[19] * b[34] + b[0] * b[19] * b[36] - b[0] * b[22] * b[33] + + b[0] * b[20] * b[35] - b[0] * b[23] * b[32] - b[0] * b[25] * b[30] + + b[0] * b[17] * b[38] + b[0] * b[18] * b[37] - b[0] * b[24] * b[31] + + b[1] * b[17] * b[37] - b[1] * b[23] * b[31] - b[1] * b[24] * b[30] - + b[1] * b[21] * b[33] + b[1] * b[20] * b[34] + b[1] * b[19] * b[35] + + b[1] * b[18] * b[36] + b[2] * b[17] * b[36] - b[2] * b[22] * b[31]; + coeffs(5) = + -b[14] * b[6] * b[36] - b[14] * b[7] * b[35] + b[14] * b[31] * b[11] - + b[3] * b[23] * b[30] - b[3] * b[21] * b[32] + b[3] * b[18] * b[35] - + b[3] * b[22] * b[31] + b[3] * b[17] * b[36] + b[3] * b[19] * b[34] + + b[13] * b[32] * b[11] + b[13] * b[33] * b[10] - b[13] * b[5] * b[38] - + b[15] * b[6] * b[35] - b[15] * b[7] * b[34] + b[15] * b[30] * b[11] + + b[15] * b[31] * b[10] + b[16] * b[31] * b[9] - b[13] * b[6] * b[37] - + b[13] * b[7] * b[36] + b[13] * b[31] * b[12] + b[14] * b[32] * b[10] + + b[14] * b[33] * b[9] - b[14] * b[4] * b[38] - b[14] * b[5] * b[37] - + b[16] * b[6] * b[34] + b[16] * b[30] * b[10] + b[16] * b[32] * b[8] - + b[26] * b[20] * b[10] + b[26] * b[5] * b[25] + b[26] * b[6] * b[24] + + b[26] * b[7] * b[23] + b[14] * b[30] * b[12] + b[15] * b[32] * b[9] + + b[15] * b[33] * b[8] - b[15] * b[4] * b[37] - b[15] * b[5] * b[36] + + b[29] * b[5] * b[22] + b[29] * b[6] * b[21] - b[26] * b[18] * b[12] - + b[26] * b[19] * b[11] - b[27] * b[20] * b[9] + b[27] * b[4] * b[25] + + b[27] * b[5] * b[24] + b[27] * b[6] * b[23] + b[27] * b[7] * b[22] - + b[27] * b[17] * b[12] - b[27] * b[18] * b[11] - b[27] * b[19] * b[10] - + b[28] * b[20] * b[8] - b[16] * b[4] * b[36] - b[16] * b[5] * b[35] - + b[29] * b[17] * b[10] - b[29] * b[18] * b[9] - b[29] * b[19] * b[8] + + b[28] * b[4] * b[24] + b[28] * b[5] * b[23] + b[28] * b[6] * b[22] + + b[28] * b[7] * b[21] - b[28] * b[17] * b[11] - b[28] * b[18] * b[10] - + b[28] * b[19] * b[9] + b[29] * b[4] * b[23] - b[2] * b[22] * b[32] - + b[2] * b[21] * b[33] - b[1] * b[24] * b[31] + b[0] * b[18] * b[38] - + b[0] * b[24] * b[32] + b[0] * b[19] * b[37] + b[0] * b[20] * b[36] - + b[0] * b[25] * b[31] - b[0] * b[23] * b[33] + b[1] * b[19] * b[36] - + b[1] * b[22] * b[33] + b[1] * b[20] * b[35] + b[2] * b[19] * b[35] - + b[2] * b[24] * b[30] - b[2] * b[23] * b[31] + b[2] * b[20] * b[34] + + b[2] * b[17] * b[37] - b[1] * b[25] * b[30] + b[1] * b[18] * b[37] + + b[1] * b[17] * b[38] - b[1] * b[23] * b[32] + b[2] * b[18] * b[36]; + coeffs(6) = + -b[14] * b[6] * b[37] - b[14] * b[7] * b[36] + b[14] * b[31] * b[12] + + b[3] * b[17] * b[37] - b[3] * b[23] * b[31] - b[3] * b[24] * b[30] - + b[3] * b[21] * b[33] + b[3] * b[20] * b[34] + b[3] * b[19] * b[35] + + b[3] * b[18] * b[36] - b[3] * b[22] * b[32] + b[13] * b[32] * b[12] + + b[13] * b[33] * b[11] - b[15] * b[6] * b[36] - b[15] * b[7] * b[35] + + b[15] * b[31] * b[11] + b[15] * b[30] * b[12] + b[16] * b[32] * b[9] + + b[16] * b[33] * b[8] - b[13] * b[6] * b[38] - b[13] * b[7] * b[37] + + b[14] * b[32] * b[11] + b[14] * b[33] * b[10] - b[14] * b[5] * b[38] - + b[16] * b[6] * b[35] - b[16] * b[7] * b[34] + b[16] * b[30] * b[11] + + b[16] * b[31] * b[10] - b[26] * b[19] * b[12] - b[26] * b[20] * b[11] + + b[26] * b[6] * b[25] + b[26] * b[7] * b[24] + b[15] * b[32] * b[10] + + b[15] * b[33] * b[9] - b[15] * b[4] * b[38] - b[15] * b[5] * b[37] + + b[29] * b[5] * b[23] + b[29] * b[6] * b[22] + b[29] * b[7] * b[21] - + b[27] * b[20] * b[10] + b[27] * b[5] * b[25] + b[27] * b[6] * b[24] + + b[27] * b[7] * b[23] - b[27] * b[18] * b[12] - b[27] * b[19] * b[11] - + b[28] * b[20] * b[9] - b[16] * b[4] * b[37] - b[16] * b[5] * b[36] + + b[0] * b[19] * b[38] - b[0] * b[24] * b[33] + b[0] * b[20] * b[37] - + b[29] * b[17] * b[11] - b[29] * b[18] * b[10] - b[29] * b[19] * b[9] + + b[28] * b[4] * b[25] + b[28] * b[5] * b[24] + b[28] * b[6] * b[23] + + b[28] * b[7] * b[22] - b[28] * b[17] * b[12] - b[28] * b[18] * b[11] - + b[28] * b[19] * b[10] - b[29] * b[20] * b[8] + b[29] * b[4] * b[24] + + b[2] * b[18] * b[37] - b[0] * b[25] * b[32] + b[1] * b[18] * b[38] - + b[1] * b[24] * b[32] + b[1] * b[19] * b[37] + b[1] * b[20] * b[36] - + b[1] * b[25] * b[31] + b[2] * b[17] * b[38] + b[2] * b[19] * b[36] - + b[2] * b[24] * b[31] - b[2] * b[22] * b[33] - b[2] * b[23] * b[32] + + b[2] * b[20] * b[35] - b[1] * b[23] * b[33] - b[2] * b[25] * b[30]; + coeffs(7) = + -b[14] * b[6] * b[38] - b[14] * b[7] * b[37] + b[3] * b[19] * b[36] - + b[3] * b[22] * b[33] + b[3] * b[20] * b[35] - b[3] * b[23] * b[32] - + b[3] * b[25] * b[30] + b[3] * b[17] * b[38] + b[3] * b[18] * b[37] - + b[3] * b[24] * b[31] - b[15] * b[6] * b[37] - b[15] * b[7] * b[36] + + b[15] * b[31] * b[12] + b[16] * b[32] * b[10] + b[16] * b[33] * b[9] + + b[13] * b[33] * b[12] - b[13] * b[7] * b[38] + b[14] * b[32] * b[12] + + b[14] * b[33] * b[11] - b[16] * b[6] * b[36] - b[16] * b[7] * b[35] + + b[16] * b[31] * b[11] + b[16] * b[30] * b[12] + b[15] * b[32] * b[11] + + b[15] * b[33] * b[10] - b[15] * b[5] * b[38] + b[29] * b[5] * b[24] + + b[29] * b[6] * b[23] - b[26] * b[20] * b[12] + b[26] * b[7] * b[25] - + b[27] * b[19] * b[12] - b[27] * b[20] * b[11] + b[27] * b[6] * b[25] + + b[27] * b[7] * b[24] - b[28] * b[20] * b[10] - b[16] * b[4] * b[38] - + b[16] * b[5] * b[37] + b[29] * b[7] * b[22] - b[29] * b[17] * b[12] - + b[29] * b[18] * b[11] - b[29] * b[19] * b[10] + b[28] * b[5] * b[25] + + b[28] * b[6] * b[24] + b[28] * b[7] * b[23] - b[28] * b[18] * b[12] - + b[28] * b[19] * b[11] - b[29] * b[20] * b[9] + b[29] * b[4] * b[25] - + b[2] * b[24] * b[32] + b[0] * b[20] * b[38] - b[0] * b[25] * b[33] + + b[1] * b[19] * b[38] - b[1] * b[24] * b[33] + b[1] * b[20] * b[37] - + b[2] * b[25] * b[31] + b[2] * b[20] * b[36] - b[1] * b[25] * b[32] + + b[2] * b[19] * b[37] + b[2] * b[18] * b[38] - b[2] * b[23] * b[33]; + coeffs(8) = + b[3] * b[18] * b[38] - b[3] * b[24] * b[32] + b[3] * b[19] * b[37] + + b[3] * b[20] * b[36] - b[3] * b[25] * b[31] - b[3] * b[23] * b[33] - + b[15] * b[6] * b[38] - b[15] * b[7] * b[37] + b[16] * b[32] * b[11] + + b[16] * b[33] * b[10] - b[16] * b[5] * b[38] - b[16] * b[6] * b[37] - + b[16] * b[7] * b[36] + b[16] * b[31] * b[12] + b[14] * b[33] * b[12] - + b[14] * b[7] * b[38] + b[15] * b[32] * b[12] + b[15] * b[33] * b[11] + + b[29] * b[5] * b[25] + b[29] * b[6] * b[24] - b[27] * b[20] * b[12] + + b[27] * b[7] * b[25] - b[28] * b[19] * b[12] - b[28] * b[20] * b[11] + + b[29] * b[7] * b[23] - b[29] * b[18] * b[12] - b[29] * b[19] * b[11] + + b[28] * b[6] * b[25] + b[28] * b[7] * b[24] - b[29] * b[20] * b[10] + + b[2] * b[19] * b[38] - b[1] * b[25] * b[33] + b[2] * b[20] * b[37] - + b[2] * b[24] * b[33] - b[2] * b[25] * b[32] + b[1] * b[20] * b[38]; + coeffs(9) = + b[29] * b[7] * b[24] - b[29] * b[20] * b[11] + b[2] * b[20] * b[38] - + b[2] * b[25] * b[33] - b[28] * b[20] * b[12] + b[28] * b[7] * b[25] - + b[29] * b[19] * b[12] - b[3] * b[24] * b[33] + b[15] * b[33] * b[12] + + b[3] * b[19] * b[38] - b[16] * b[6] * b[38] + b[3] * b[20] * b[37] + + b[16] * b[32] * b[12] + b[29] * b[6] * b[25] - b[16] * b[7] * b[37] - + b[3] * b[25] * b[32] - b[15] * b[7] * b[38] + b[16] * b[33] * b[11]; + coeffs(10) = -b[29] * b[20] * b[12] + b[29] * b[7] * b[25] + + b[16] * b[33] * b[12] - b[16] * b[7] * b[38] + + b[3] * b[20] * b[38] - b[3] * b[25] * b[33]; +} diff --git a/include/colmap/estimators/essential_matrix_poly.h b/include/colmap/estimators/essential_matrix_poly.h new file mode 100644 index 0000000000000000000000000000000000000000..7a706c88dd0da1246aaa9199ce9001c8b8241c75 --- /dev/null +++ b/include/colmap/estimators/essential_matrix_poly.h @@ -0,0 +1,2115 @@ +// Copyright (c) 2022, 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) + +{ + double* a = A.data(); + const double* e = E.data(); + + double e2[36]; + double e3[36]; + for (size_t i = 0; i < 36; ++i) { + e2[i] = e[i] * e[i]; + e3[i] = e2[i] * e[i]; + } + + a[190] = e[33] * e[28] * e[32] - e[33] * e[31] * e[29] + + e[30] * e[34] * e[29] - e[30] * e[28] * e[35] - + e[27] * e[32] * e[34] + e[27] * e[31] * e[35]; + a[7] = 0.5 * e[6] * e2[8] - 0.5 * e[6] * e2[5] + 0.5 * e3[6] + + 0.5 * e[6] * e2[7] - 0.5 * e[6] * e2[4] + e[0] * e[2] * e[8] + + e[3] * e[4] * e[7] + e[3] * e[5] * e[8] + e[0] * e[1] * e[7] - + 0.5 * e[6] * e2[1] - 0.5 * e[6] * e2[2] + 0.5 * e2[0] * e[6] + + 0.5 * e2[3] * e[6]; + a[120] = e[30] * e[34] * e[2] + e[33] * e[1] * e[32] - e[3] * e[28] * e[35] + + e[0] * e[31] * e[35] + e[3] * e[34] * e[29] - e[30] * e[1] * e[35] + + e[27] * e[31] * e[8] - e[27] * e[32] * e[7] - e[30] * e[28] * e[8] - + e[33] * e[31] * e[2] - e[0] * e[32] * e[34] + e[6] * e[28] * e[32] - + e[33] * e[4] * e[29] + e[33] * e[28] * e[5] + e[30] * e[7] * e[29] + + e[27] * e[4] * e[35] - e[27] * e[5] * e[34] - e[6] * e[31] * e[29]; + a[77] = + e[9] * e[27] * e[15] + e[9] * e[29] * e[17] + e[9] * e[11] * e[35] + + e[9] * e[28] * e[16] + e[9] * e[10] * e[34] + e[27] * e[11] * e[17] + + e[27] * e[10] * e[16] + e[12] * e[30] * e[15] + e[12] * e[32] * e[17] + + e[12] * e[14] * e[35] + e[12] * e[31] * e[16] + e[12] * e[13] * e[34] + + e[30] * e[14] * e[17] + e[30] * e[13] * e[16] + e[15] * e[35] * e[17] + + e[15] * e[34] * e[16] - e[15] * e[28] * e[10] - e[15] * e[31] * e[13] - + e[15] * e[32] * e[14] - e[15] * e[29] * e[11] + 0.5 * e2[9] * e[33] + + 0.5 * e[33] * e2[16] - 0.5 * e[33] * e2[11] + 0.5 * e[33] * e2[12] + + 1.5 * e[33] * e2[15] + 0.5 * e[33] * e2[17] - 0.5 * e[33] * e2[10] - + 0.5 * e[33] * e2[14] - 0.5 * e[33] * e2[13]; + a[180] = + -e[33] * e[22] * e[29] - e[33] * e[31] * e[20] - e[27] * e[32] * e[25] + + e[27] * e[22] * e[35] - e[27] * e[23] * e[34] + e[27] * e[31] * e[26] + + e[33] * e[28] * e[23] - e[21] * e[28] * e[35] + e[30] * e[25] * e[29] + + e[24] * e[28] * e[32] - e[24] * e[31] * e[29] + e[18] * e[31] * e[35] - + e[30] * e[28] * e[26] - e[30] * e[19] * e[35] + e[21] * e[34] * e[29] + + e[33] * e[19] * e[32] - e[18] * e[32] * e[34] + e[30] * e[34] * e[20]; + a[87] = e[18] * e[2] * e[17] + e[3] * e[21] * e[15] + e[3] * e[12] * e[24] + + e[3] * e[23] * e[17] + e[3] * e[14] * e[26] + e[3] * e[22] * e[16] + + e[3] * e[13] * e[25] + 3. * e[6] * e[24] * e[15] + + e[6] * e[26] * e[17] + e[6] * e[25] * e[16] + e[0] * e[20] * e[17] + + e[0] * e[11] * e[26] + e[0] * e[19] * e[16] + e[0] * e[10] * e[25] + + e[15] * e[26] * e[8] - e[15] * e[20] * e[2] - e[15] * e[19] * e[1] - + e[15] * e[22] * e[4] + e[15] * e[25] * e[7] - e[15] * e[23] * e[5] + + e[12] * e[21] * e[6] + e[12] * e[22] * e[7] + e[12] * e[4] * e[25] + + e[12] * e[23] * e[8] + e[12] * e[5] * e[26] - e[24] * e[11] * e[2] - + e[24] * e[10] * e[1] - e[24] * e[13] * e[4] + e[24] * e[16] * e[7] - + e[24] * e[14] * e[5] + e[24] * e[17] * e[8] + e[21] * e[13] * e[7] + + e[21] * e[4] * e[16] + e[21] * e[14] * e[8] + e[21] * e[5] * e[17] - + e[6] * e[23] * e[14] - e[6] * e[20] * e[11] - e[6] * e[19] * e[10] - + e[6] * e[22] * e[13] + e[9] * e[18] * e[6] + e[9] * e[0] * e[24] + + e[9] * e[19] * e[7] + e[9] * e[1] * e[25] + e[9] * e[20] * e[8] + + e[9] * e[2] * e[26] + e[18] * e[0] * e[15] + e[18] * e[10] * e[7] + + e[18] * e[1] * e[16] + e[18] * e[11] * e[8]; + a[150] = + e[33] * e[10] * e[32] + e[33] * e[28] * e[14] - e[33] * e[13] * e[29] - + e[33] * e[31] * e[11] + e[9] * e[31] * e[35] - e[9] * e[32] * e[34] + + e[27] * e[13] * e[35] - e[27] * e[32] * e[16] + e[27] * e[31] * e[17] - + e[27] * e[14] * e[34] + e[12] * e[34] * e[29] - e[12] * e[28] * e[35] + + e[30] * e[34] * e[11] + e[30] * e[16] * e[29] - e[30] * e[10] * e[35] - + e[30] * e[28] * e[17] + e[15] * e[28] * e[32] - e[15] * e[31] * e[29]; + a[57] = e[0] * e[27] * e[6] + e[0] * e[28] * e[7] + e[0] * e[1] * e[34] + + e[0] * e[29] * e[8] + e[0] * e[2] * e[35] + e[6] * e[34] * e[7] - + e[6] * e[32] * e[5] + e[6] * e[30] * e[3] + e[6] * e[35] * e[8] - + e[6] * e[29] * e[2] - e[6] * e[28] * e[1] - e[6] * e[31] * e[4] + + e[27] * e[1] * e[7] + e[27] * e[2] * e[8] + e[3] * e[31] * e[7] + + e[3] * e[4] * e[34] + e[3] * e[32] * e[8] + e[3] * e[5] * e[35] + + e[30] * e[4] * e[7] + e[30] * e[5] * e[8] + 0.5 * e2[0] * e[33] + + 1.5 * e[33] * e2[6] - 0.5 * e[33] * e2[4] - 0.5 * e[33] * e2[5] - + 0.5 * e[33] * e2[1] + 0.5 * e[33] * e2[7] + 0.5 * e[33] * e2[3] - + 0.5 * e[33] * e2[2] + 0.5 * e[33] * e2[8]; + a[80] = -e[0] * e[23] * e[16] + e[9] * e[4] * e[26] + e[9] * e[22] * e[8] - + e[9] * e[5] * e[25] - e[9] * e[23] * e[7] + e[18] * e[4] * e[17] + + e[18] * e[13] * e[8] - e[18] * e[5] * e[16] - e[18] * e[14] * e[7] + + e[3] * e[16] * e[20] + e[3] * e[25] * e[11] - e[3] * e[10] * e[26] - + e[3] * e[19] * e[17] + e[12] * e[7] * e[20] + e[12] * e[25] * e[2] - + e[12] * e[1] * e[26] - e[12] * e[19] * e[8] + e[21] * e[7] * e[11] + + e[21] * e[16] * e[2] - e[21] * e[1] * e[17] - e[21] * e[10] * e[8] + + e[6] * e[10] * e[23] + e[6] * e[19] * e[14] - e[6] * e[13] * e[20] - + e[6] * e[22] * e[11] + e[15] * e[1] * e[23] + e[15] * e[19] * e[5] - + e[15] * e[4] * e[20] - e[15] * e[22] * e[2] + e[24] * e[1] * e[14] + + e[24] * e[10] * e[5] - e[24] * e[4] * e[11] - e[24] * e[13] * e[2] + + e[0] * e[13] * e[26] + e[0] * e[22] * e[17] - e[0] * e[14] * e[25]; + a[167] = e[18] * e[19] * e[25] + 0.5 * e3[24] - 0.5 * e[24] * e2[23] + + e[18] * e[20] * e[26] + e[21] * e[22] * e[25] + + e[21] * e[23] * e[26] - 0.5 * e[24] * e2[19] + 0.5 * e2[21] * e[24] + + 0.5 * e[24] * e2[26] - 0.5 * e[24] * e2[20] + 0.5 * e2[18] * e[24] - + 0.5 * e[24] * e2[22] + 0.5 * e[24] * e2[25]; + a[50] = -e[3] * e[1] * e[35] - e[0] * e[32] * e[7] + e[27] * e[4] * e[8] + + e[33] * e[1] * e[5] - e[33] * e[4] * e[2] + e[0] * e[4] * e[35] + + e[3] * e[34] * e[2] - e[30] * e[1] * e[8] + e[30] * e[7] * e[2] - + e[6] * e[4] * e[29] + e[3] * e[7] * e[29] + e[6] * e[1] * e[32] - + e[0] * e[5] * e[34] - e[3] * e[28] * e[8] + e[0] * e[31] * e[8] + + e[6] * e[28] * e[5] - e[6] * e[31] * e[2] - e[27] * e[5] * e[7]; + a[97] = e[33] * e[16] * e[7] - e[33] * e[14] * e[5] + e[33] * e[17] * e[8] + + e[30] * e[13] * e[7] + e[30] * e[4] * e[16] + e[30] * e[14] * e[8] + + e[30] * e[5] * e[17] + e[6] * e[27] * e[9] - e[6] * e[28] * e[10] - + e[6] * e[31] * e[13] - e[6] * e[32] * e[14] - e[6] * e[29] * e[11] + + e[9] * e[28] * e[7] + e[9] * e[1] * e[34] + e[9] * e[29] * e[8] + + e[9] * e[2] * e[35] + e[27] * e[10] * e[7] + e[27] * e[1] * e[16] + + e[27] * e[11] * e[8] + e[27] * e[2] * e[17] + e[3] * e[30] * e[15] + + e[3] * e[12] * e[33] + e[3] * e[32] * e[17] + e[3] * e[14] * e[35] + + e[3] * e[31] * e[16] + e[3] * e[13] * e[34] + + 3. * e[6] * e[33] * e[15] + e[6] * e[35] * e[17] + + e[6] * e[34] * e[16] + e[0] * e[27] * e[15] + e[0] * e[9] * e[33] + + e[0] * e[29] * e[17] + e[0] * e[11] * e[35] + e[0] * e[28] * e[16] + + e[0] * e[10] * e[34] + e[15] * e[34] * e[7] - e[15] * e[32] * e[5] + + e[15] * e[35] * e[8] - e[15] * e[29] * e[2] - e[15] * e[28] * e[1] - + e[15] * e[31] * e[4] + e[12] * e[30] * e[6] + e[12] * e[31] * e[7] + + e[12] * e[4] * e[34] + e[12] * e[32] * e[8] + e[12] * e[5] * e[35] - + e[33] * e[11] * e[2] - e[33] * e[10] * e[1] - e[33] * e[13] * e[4]; + a[0] = e[6] * e[1] * e[5] - e[6] * e[4] * e[2] + e[3] * e[7] * e[2] + + e[0] * e[4] * e[8] - e[0] * e[5] * e[7] - e[3] * e[1] * e[8]; + a[17] = 0.5 * e3[15] + e[9] * e[10] * e[16] - 0.5 * e[15] * e2[11] + + e[9] * e[11] * e[17] + 0.5 * e2[12] * e[15] + 0.5 * e[15] * e2[16] + + 0.5 * e[15] * e2[17] - 0.5 * e[15] * e2[13] + 0.5 * e2[9] * e[15] + + e[12] * e[14] * e[17] - 0.5 * e[15] * e2[10] - 0.5 * e[15] * e2[14] + + e[12] * e[13] * e[16]; + a[70] = + e[15] * e[28] * e[14] - e[15] * e[13] * e[29] - e[15] * e[31] * e[11] + + e[33] * e[10] * e[14] - e[33] * e[13] * e[11] + e[9] * e[13] * e[35] - + e[9] * e[32] * e[16] + e[9] * e[31] * e[17] - e[9] * e[14] * e[34] + + e[27] * e[13] * e[17] - e[27] * e[14] * e[16] + e[12] * e[34] * e[11] + + e[12] * e[16] * e[29] - e[12] * e[10] * e[35] - e[12] * e[28] * e[17] + + e[30] * e[16] * e[11] - e[30] * e[10] * e[17] + e[15] * e[10] * e[32]; + a[177] = + e[18] * e[27] * e[24] + e[18] * e[28] * e[25] + e[18] * e[19] * e[34] + + e[18] * e[29] * e[26] + e[18] * e[20] * e[35] + e[27] * e[19] * e[25] + + e[27] * e[20] * e[26] + e[21] * e[30] * e[24] + e[21] * e[31] * e[25] + + e[21] * e[22] * e[34] + e[21] * e[32] * e[26] + e[21] * e[23] * e[35] + + e[30] * e[22] * e[25] + e[30] * e[23] * e[26] + e[24] * e[34] * e[25] + + e[24] * e[35] * e[26] - e[24] * e[29] * e[20] - e[24] * e[31] * e[22] - + e[24] * e[32] * e[23] - e[24] * e[28] * e[19] + 1.5 * e[33] * e2[24] + + 0.5 * e[33] * e2[25] + 0.5 * e[33] * e2[26] - 0.5 * e[33] * e2[23] - + 0.5 * e[33] * e2[19] - 0.5 * e[33] * e2[20] - 0.5 * e[33] * e2[22] + + 0.5 * e2[18] * e[33] + 0.5 * e2[21] * e[33]; + a[170] = + e[21] * e[25] * e[29] - e[27] * e[23] * e[25] + e[24] * e[19] * e[32] - + e[21] * e[28] * e[26] - e[21] * e[19] * e[35] + e[18] * e[31] * e[26] - + e[30] * e[19] * e[26] - e[24] * e[31] * e[20] + e[24] * e[28] * e[23] + + e[27] * e[22] * e[26] + e[30] * e[25] * e[20] - e[33] * e[22] * e[20] + + e[33] * e[19] * e[23] + e[21] * e[34] * e[20] - e[18] * e[23] * e[34] - + e[24] * e[22] * e[29] - e[18] * e[32] * e[25] + e[18] * e[22] * e[35]; + a[37] = e[12] * e[14] * e[8] + e[12] * e[5] * e[17] + e[15] * e[16] * e[7] + + e[15] * e[17] * e[8] + e[0] * e[11] * e[17] + e[0] * e[9] * e[15] + + e[0] * e[10] * e[16] + e[3] * e[14] * e[17] + e[3] * e[13] * e[16] + + e[9] * e[10] * e[7] + e[9] * e[1] * e[16] + e[9] * e[11] * e[8] + + e[9] * e[2] * e[17] - e[15] * e[11] * e[2] - e[15] * e[10] * e[1] - + e[15] * e[13] * e[4] - e[15] * e[14] * e[5] + e[12] * e[3] * e[15] + + e[12] * e[13] * e[7] + e[12] * e[4] * e[16] + 0.5 * e2[12] * e[6] + + 1.5 * e2[15] * e[6] + 0.5 * e[6] * e2[17] + 0.5 * e[6] * e2[16] + + 0.5 * e[6] * e2[9] - 0.5 * e[6] * e2[11] - 0.5 * e[6] * e2[10] - + 0.5 * e[6] * e2[14] - 0.5 * e[6] * e2[13]; + a[10] = -e[9] * e[14] * e[16] - e[12] * e[10] * e[17] + e[9] * e[13] * e[17] - + e[15] * e[13] * e[11] + e[15] * e[10] * e[14] + e[12] * e[16] * e[11]; + a[67] = e[21] * e[14] * e[17] + e[21] * e[13] * e[16] + + e[15] * e[26] * e[17] + e[15] * e[25] * e[16] - + e[15] * e[23] * e[14] - e[15] * e[20] * e[11] - + e[15] * e[19] * e[10] - e[15] * e[22] * e[13] + e[9] * e[20] * e[17] + + e[9] * e[11] * e[26] + e[9] * e[19] * e[16] + e[9] * e[10] * e[25] + + 0.5 * e2[12] * e[24] + 1.5 * e[24] * e2[15] + 0.5 * e[24] * e2[17] + + 0.5 * e[24] * e2[16] + 0.5 * e2[9] * e[24] - 0.5 * e[24] * e2[11] - + 0.5 * e[24] * e2[10] - 0.5 * e[24] * e2[14] - 0.5 * e[24] * e2[13] + + e[18] * e[11] * e[17] + e[18] * e[9] * e[15] + e[18] * e[10] * e[16] + + e[12] * e[21] * e[15] + e[12] * e[23] * e[17] + + e[12] * e[14] * e[26] + e[12] * e[22] * e[16] + e[12] * e[13] * e[25]; + a[90] = -e[9] * e[5] * e[34] + e[9] * e[31] * e[8] - e[9] * e[32] * e[7] + + e[27] * e[4] * e[17] + e[27] * e[13] * e[8] - e[27] * e[5] * e[16] - + e[27] * e[14] * e[7] + e[0] * e[13] * e[35] - e[0] * e[32] * e[16] + + e[0] * e[31] * e[17] - e[0] * e[14] * e[34] + e[9] * e[4] * e[35] + + e[6] * e[10] * e[32] + e[6] * e[28] * e[14] - e[6] * e[13] * e[29] - + e[6] * e[31] * e[11] + e[15] * e[1] * e[32] + e[3] * e[34] * e[11] + + e[3] * e[16] * e[29] - e[3] * e[10] * e[35] - e[3] * e[28] * e[17] - + e[12] * e[1] * e[35] + e[12] * e[7] * e[29] + e[12] * e[34] * e[2] - + e[12] * e[28] * e[8] + e[15] * e[28] * e[5] - e[15] * e[4] * e[29] - + e[15] * e[31] * e[2] + e[33] * e[1] * e[14] + e[33] * e[10] * e[5] - + e[33] * e[4] * e[11] - e[33] * e[13] * e[2] + e[30] * e[7] * e[11] + + e[30] * e[16] * e[2] - e[30] * e[1] * e[17] - e[30] * e[10] * e[8]; + a[117] = e[21] * e[31] * e[7] + e[21] * e[4] * e[34] + e[21] * e[32] * e[8] + + e[21] * e[5] * e[35] + e[30] * e[22] * e[7] + e[30] * e[4] * e[25] + + e[30] * e[23] * e[8] + e[30] * e[5] * e[26] + + 3. * e[24] * e[33] * e[6] + e[24] * e[34] * e[7] + + e[24] * e[35] * e[8] + e[33] * e[25] * e[7] + e[33] * e[26] * e[8] + + e[0] * e[27] * e[24] + e[0] * e[18] * e[33] + e[0] * e[28] * e[25] + + e[0] * e[19] * e[34] + e[0] * e[29] * e[26] + e[0] * e[20] * e[35] + + e[18] * e[27] * e[6] + e[18] * e[28] * e[7] + e[18] * e[1] * e[34] + + e[18] * e[29] * e[8] + e[18] * e[2] * e[35] + e[27] * e[19] * e[7] + + e[27] * e[1] * e[25] + e[27] * e[20] * e[8] + e[27] * e[2] * e[26] + + e[3] * e[30] * e[24] + e[3] * e[21] * e[33] + e[3] * e[31] * e[25] + + e[3] * e[22] * e[34] + e[3] * e[32] * e[26] + e[3] * e[23] * e[35] + + e[6] * e[30] * e[21] - e[6] * e[29] * e[20] + e[6] * e[35] * e[26] - + e[6] * e[31] * e[22] - e[6] * e[32] * e[23] - e[6] * e[28] * e[19] + + e[6] * e[34] * e[25] - e[24] * e[32] * e[5] - e[24] * e[29] * e[2] - + e[24] * e[28] * e[1] - e[24] * e[31] * e[4] - e[33] * e[20] * e[2] - + e[33] * e[19] * e[1] - e[33] * e[22] * e[4] - e[33] * e[23] * e[5]; + a[160] = e[21] * e[25] * e[20] - e[21] * e[19] * e[26] + + e[18] * e[22] * e[26] - e[18] * e[23] * e[25] - + e[24] * e[22] * e[20] + e[24] * e[19] * e[23]; + a[47] = e[3] * e[4] * e[25] + e[3] * e[23] * e[8] + e[3] * e[5] * e[26] + + e[21] * e[4] * e[7] + e[21] * e[5] * e[8] + e[6] * e[25] * e[7] + + e[6] * e[26] * e[8] + e[0] * e[19] * e[7] + e[0] * e[1] * e[25] + + e[0] * e[20] * e[8] + e[0] * e[2] * e[26] - e[6] * e[20] * e[2] - + e[6] * e[19] * e[1] - e[6] * e[22] * e[4] - e[6] * e[23] * e[5] + + e[18] * e[1] * e[7] + e[18] * e[0] * e[6] + e[18] * e[2] * e[8] + + e[3] * e[21] * e[6] + e[3] * e[22] * e[7] - 0.5 * e[24] * e2[4] + + 0.5 * e[24] * e2[0] + 1.5 * e[24] * e2[6] - 0.5 * e[24] * e2[5] - + 0.5 * e[24] * e2[1] + 0.5 * e[24] * e2[7] + 0.5 * e[24] * e2[3] - + 0.5 * e[24] * e2[2] + 0.5 * e[24] * e2[8]; + a[110] = e[6] * e[28] * e[23] - e[6] * e[22] * e[29] - e[6] * e[31] * e[20] - + e[3] * e[19] * e[35] + e[3] * e[34] * e[20] + e[3] * e[25] * e[29] - + e[21] * e[1] * e[35] + e[21] * e[7] * e[29] + e[21] * e[34] * e[2] + + e[24] * e[1] * e[32] + e[24] * e[28] * e[5] - e[24] * e[4] * e[29] - + e[24] * e[31] * e[2] + e[33] * e[1] * e[23] + e[33] * e[19] * e[5] - + e[33] * e[4] * e[20] - e[33] * e[22] * e[2] - e[21] * e[28] * e[8] + + e[30] * e[7] * e[20] + e[30] * e[25] * e[2] - e[30] * e[1] * e[26] + + e[18] * e[4] * e[35] - e[18] * e[5] * e[34] + e[18] * e[31] * e[8] - + e[18] * e[32] * e[7] + e[27] * e[4] * e[26] + e[27] * e[22] * e[8] - + e[27] * e[5] * e[25] - e[27] * e[23] * e[7] - e[3] * e[28] * e[26] - + e[0] * e[32] * e[25] + e[0] * e[22] * e[35] - e[0] * e[23] * e[34] + + e[0] * e[31] * e[26] - e[30] * e[19] * e[8] + e[6] * e[19] * e[32]; + a[107] = 0.5 * e2[18] * e[6] + 0.5 * e2[21] * e[6] + 1.5 * e2[24] * e[6] + + 0.5 * e[6] * e2[26] - 0.5 * e[6] * e2[23] - 0.5 * e[6] * e2[19] - + 0.5 * e[6] * e2[20] - 0.5 * e[6] * e2[22] + 0.5 * e[6] * e2[25] + + e[21] * e[3] * e[24] + e[18] * e[20] * e[8] + e[21] * e[4] * e[25] + + e[18] * e[19] * e[7] + e[18] * e[1] * e[25] + e[21] * e[22] * e[7] + + e[21] * e[23] * e[8] + e[18] * e[0] * e[24] + e[18] * e[2] * e[26] + + e[21] * e[5] * e[26] + e[24] * e[26] * e[8] - e[24] * e[20] * e[2] - + e[24] * e[19] * e[1] - e[24] * e[22] * e[4] + e[24] * e[25] * e[7] - + e[24] * e[23] * e[5] + e[0] * e[19] * e[25] + e[0] * e[20] * e[26] + + e[3] * e[22] * e[25] + e[3] * e[23] * e[26]; + a[40] = e[18] * e[4] * e[8] + e[3] * e[7] * e[20] + e[3] * e[25] * e[2] - + e[3] * e[1] * e[26] - e[18] * e[5] * e[7] + e[6] * e[1] * e[23] + + e[6] * e[19] * e[5] - e[6] * e[4] * e[20] - e[6] * e[22] * e[2] + + e[21] * e[7] * e[2] - e[21] * e[1] * e[8] + e[24] * e[1] * e[5] - + e[24] * e[4] * e[2] - e[3] * e[19] * e[8] + e[0] * e[4] * e[26] + + e[0] * e[22] * e[8] - e[0] * e[5] * e[25] - e[0] * e[23] * e[7]; + a[27] = e[9] * e[1] * e[7] + e[9] * e[0] * e[6] + e[9] * e[2] * e[8] + + e[3] * e[12] * e[6] + e[3] * e[13] * e[7] + e[3] * e[4] * e[16] + + e[3] * e[14] * e[8] + e[3] * e[5] * e[17] + e[12] * e[4] * e[7] + + e[12] * e[5] * e[8] + e[6] * e[16] * e[7] + e[6] * e[17] * e[8] - + e[6] * e[11] * e[2] - e[6] * e[10] * e[1] - e[6] * e[13] * e[4] - + e[6] * e[14] * e[5] + e[0] * e[10] * e[7] + e[0] * e[1] * e[16] + + e[0] * e[11] * e[8] + e[0] * e[2] * e[17] + 0.5 * e2[3] * e[15] + + 1.5 * e[15] * e2[6] + 0.5 * e[15] * e2[7] + 0.5 * e[15] * e2[8] + + 0.5 * e2[0] * e[15] - 0.5 * e[15] * e2[4] - 0.5 * e[15] * e2[5] - + 0.5 * e[15] * e2[1] - 0.5 * e[15] * e2[2]; + a[30] = -e[15] * e[13] * e[2] - e[6] * e[13] * e[11] - e[15] * e[4] * e[11] + + e[12] * e[16] * e[2] - e[3] * e[10] * e[17] + e[3] * e[16] * e[11] + + e[0] * e[13] * e[17] - e[0] * e[14] * e[16] + e[15] * e[1] * e[14] - + e[12] * e[10] * e[8] + e[9] * e[4] * e[17] + e[9] * e[13] * e[8] - + e[9] * e[5] * e[16] - e[9] * e[14] * e[7] + e[15] * e[10] * e[5] + + e[12] * e[7] * e[11] + e[6] * e[10] * e[14] - e[12] * e[1] * e[17]; + a[147] = + e[12] * e[30] * e[24] + e[12] * e[21] * e[33] + e[12] * e[31] * e[25] + + e[12] * e[22] * e[34] + e[12] * e[32] * e[26] + e[12] * e[23] * e[35] + + e[9] * e[27] * e[24] + e[9] * e[18] * e[33] + e[9] * e[28] * e[25] + + e[9] * e[19] * e[34] + e[9] * e[29] * e[26] + e[9] * e[20] * e[35] + + e[21] * e[30] * e[15] + e[21] * e[32] * e[17] + e[21] * e[14] * e[35] + + e[21] * e[31] * e[16] + e[21] * e[13] * e[34] + e[30] * e[23] * e[17] + + e[30] * e[14] * e[26] + e[30] * e[22] * e[16] + e[30] * e[13] * e[25] + + e[15] * e[27] * e[18] + 3. * e[15] * e[33] * e[24] - + e[15] * e[29] * e[20] + e[15] * e[35] * e[26] - e[15] * e[31] * e[22] - + e[15] * e[32] * e[23] - e[15] * e[28] * e[19] + e[15] * e[34] * e[25] + + e[18] * e[29] * e[17] + e[18] * e[11] * e[35] + e[18] * e[28] * e[16] + + e[18] * e[10] * e[34] + e[27] * e[20] * e[17] + e[27] * e[11] * e[26] + + e[27] * e[19] * e[16] + e[27] * e[10] * e[25] - e[24] * e[28] * e[10] - + e[24] * e[31] * e[13] - e[24] * e[32] * e[14] + e[24] * e[34] * e[16] + + e[24] * e[35] * e[17] - e[24] * e[29] * e[11] - e[33] * e[23] * e[14] + + e[33] * e[25] * e[16] + e[33] * e[26] * e[17] - e[33] * e[20] * e[11] - + e[33] * e[19] * e[10] - e[33] * e[22] * e[13]; + a[60] = e[18] * e[13] * e[17] + e[9] * e[13] * e[26] + e[9] * e[22] * e[17] - + e[9] * e[14] * e[25] - e[18] * e[14] * e[16] - e[15] * e[13] * e[20] - + e[15] * e[22] * e[11] + e[12] * e[16] * e[20] + + e[12] * e[25] * e[11] - e[12] * e[10] * e[26] - + e[12] * e[19] * e[17] + e[21] * e[16] * e[11] - + e[21] * e[10] * e[17] - e[9] * e[23] * e[16] + e[24] * e[10] * e[14] - + e[24] * e[13] * e[11] + e[15] * e[10] * e[23] + e[15] * e[19] * e[14]; + a[137] = + e[21] * e[12] * e[24] + e[21] * e[23] * e[17] + e[21] * e[14] * e[26] + + e[21] * e[22] * e[16] + e[21] * e[13] * e[25] + e[24] * e[26] * e[17] + + e[24] * e[25] * e[16] + e[9] * e[19] * e[25] + e[9] * e[18] * e[24] + + e[9] * e[20] * e[26] + e[12] * e[22] * e[25] + e[12] * e[23] * e[26] + + e[18] * e[20] * e[17] + e[18] * e[11] * e[26] + e[18] * e[19] * e[16] + + e[18] * e[10] * e[25] - e[24] * e[23] * e[14] - e[24] * e[20] * e[11] - + e[24] * e[19] * e[10] - e[24] * e[22] * e[13] + 0.5 * e2[21] * e[15] + + 1.5 * e2[24] * e[15] + 0.5 * e[15] * e2[25] + 0.5 * e[15] * e2[26] + + 0.5 * e[15] * e2[18] - 0.5 * e[15] * e2[23] - 0.5 * e[15] * e2[19] - + 0.5 * e[15] * e2[20] - 0.5 * e[15] * e2[22]; + a[20] = e[6] * e[1] * e[14] + e[15] * e[1] * e[5] - e[0] * e[5] * e[16] - + e[0] * e[14] * e[7] + e[0] * e[13] * e[8] - e[15] * e[4] * e[2] + + e[12] * e[7] * e[2] + e[6] * e[10] * e[5] + e[3] * e[7] * e[11] - + e[6] * e[4] * e[11] + e[3] * e[16] * e[2] - e[6] * e[13] * e[2] - + e[3] * e[1] * e[17] - e[9] * e[5] * e[7] - e[3] * e[10] * e[8] - + e[12] * e[1] * e[8] + e[0] * e[4] * e[17] + e[9] * e[4] * e[8]; + a[16] = -0.5 * e[14] * e2[16] - 0.5 * e[14] * e2[10] - 0.5 * e[14] * e2[9] + + e[11] * e[9] * e[12] + 0.5 * e3[14] + e[17] * e[13] * e[16] + + 0.5 * e[14] * e2[12] + e[11] * e[10] * e[13] - 0.5 * e[14] * e2[15] + + 0.5 * e[14] * e2[17] + e[17] * e[12] * e[15] + 0.5 * e2[11] * e[14] + + 0.5 * e[14] * e2[13]; + a[100] = -e[21] * e[19] * e[8] + e[18] * e[4] * e[26] - e[18] * e[5] * e[25] - + e[18] * e[23] * e[7] + e[21] * e[25] * e[2] - e[21] * e[1] * e[26] + + e[6] * e[19] * e[23] + e[18] * e[22] * e[8] - e[0] * e[23] * e[25] - + e[6] * e[22] * e[20] + e[24] * e[1] * e[23] + e[24] * e[19] * e[5] - + e[24] * e[4] * e[20] - e[24] * e[22] * e[2] + e[3] * e[25] * e[20] - + e[3] * e[19] * e[26] + e[0] * e[22] * e[26] + e[21] * e[7] * e[20]; + a[176] = + 0.5 * e2[20] * e[32] + 1.5 * e[32] * e2[23] + 0.5 * e[32] * e2[22] + + 0.5 * e[32] * e2[21] + 0.5 * e[32] * e2[26] - 0.5 * e[32] * e2[18] - + 0.5 * e[32] * e2[19] - 0.5 * e[32] * e2[24] - 0.5 * e[32] * e2[25] + + e[20] * e[27] * e[21] + e[20] * e[18] * e[30] + e[20] * e[28] * e[22] + + e[20] * e[19] * e[31] + e[20] * e[29] * e[23] + e[29] * e[19] * e[22] + + e[29] * e[18] * e[21] + e[23] * e[30] * e[21] + e[23] * e[31] * e[22] + + e[26] * e[30] * e[24] + e[26] * e[21] * e[33] + e[26] * e[31] * e[25] + + e[26] * e[22] * e[34] + e[26] * e[23] * e[35] + e[35] * e[22] * e[25] + + e[35] * e[21] * e[24] - e[23] * e[27] * e[18] - e[23] * e[33] * e[24] - + e[23] * e[28] * e[19] - e[23] * e[34] * e[25]; + a[130] = + -e[9] * e[23] * e[25] - e[21] * e[10] * e[26] - e[21] * e[19] * e[17] - + e[18] * e[23] * e[16] + e[18] * e[13] * e[26] + e[12] * e[25] * e[20] - + e[12] * e[19] * e[26] - e[15] * e[22] * e[20] + e[21] * e[16] * e[20] + + e[21] * e[25] * e[11] + e[24] * e[10] * e[23] + e[24] * e[19] * e[14] - + e[24] * e[13] * e[20] - e[24] * e[22] * e[11] + e[18] * e[22] * e[17] - + e[18] * e[14] * e[25] + e[9] * e[22] * e[26] + e[15] * e[19] * e[23]; + a[166] = 0.5 * e[23] * e2[21] + e[20] * e[19] * e[22] + + e[20] * e[18] * e[21] + 0.5 * e3[23] + e[26] * e[22] * e[25] + + 0.5 * e[23] * e2[26] - 0.5 * e[23] * e2[18] + 0.5 * e[23] * e2[22] - + 0.5 * e[23] * e2[19] + e[26] * e[21] * e[24] + 0.5 * e2[20] * e[23] - + 0.5 * e[23] * e2[24] - 0.5 * e[23] * e2[25]; + a[140] = + e[18] * e[13] * e[35] - e[18] * e[32] * e[16] + e[18] * e[31] * e[17] - + e[18] * e[14] * e[34] + e[27] * e[13] * e[26] + e[27] * e[22] * e[17] - + e[27] * e[14] * e[25] - e[27] * e[23] * e[16] - e[9] * e[32] * e[25] + + e[9] * e[22] * e[35] - e[9] * e[23] * e[34] + e[9] * e[31] * e[26] + + e[15] * e[19] * e[32] + e[15] * e[28] * e[23] - e[15] * e[22] * e[29] - + e[15] * e[31] * e[20] + e[24] * e[10] * e[32] + e[24] * e[28] * e[14] - + e[24] * e[13] * e[29] - e[24] * e[31] * e[11] + e[33] * e[10] * e[23] + + e[33] * e[19] * e[14] - e[33] * e[13] * e[20] - e[33] * e[22] * e[11] + + e[21] * e[16] * e[29] - e[21] * e[10] * e[35] - e[21] * e[28] * e[17] + + e[30] * e[16] * e[20] + e[30] * e[25] * e[11] - e[30] * e[10] * e[26] - + e[30] * e[19] * e[17] - e[12] * e[28] * e[26] - e[12] * e[19] * e[35] + + e[12] * e[34] * e[20] + e[12] * e[25] * e[29] + e[21] * e[34] * e[11]; + a[96] = -e[32] * e[10] * e[1] + e[32] * e[13] * e[4] - e[32] * e[16] * e[7] - + e[32] * e[15] * e[6] - e[32] * e[9] * e[0] + e[32] * e[12] * e[3] + + e[17] * e[30] * e[6] + e[17] * e[3] * e[33] + e[17] * e[31] * e[7] + + e[17] * e[4] * e[34] + e[17] * e[5] * e[35] - e[5] * e[27] * e[9] - + e[5] * e[28] * e[10] - e[5] * e[33] * e[15] - e[5] * e[34] * e[16] + + e[5] * e[29] * e[11] + e[35] * e[12] * e[6] + e[35] * e[3] * e[15] + + e[35] * e[13] * e[7] + e[35] * e[4] * e[16] + e[11] * e[27] * e[3] + + e[11] * e[0] * e[30] + e[11] * e[28] * e[4] + e[11] * e[1] * e[31] + + e[29] * e[9] * e[3] + e[29] * e[0] * e[12] + e[29] * e[10] * e[4] + + e[29] * e[1] * e[13] + e[5] * e[30] * e[12] + + 3. * e[5] * e[32] * e[14] + e[5] * e[31] * e[13] + + e[8] * e[30] * e[15] + e[8] * e[12] * e[33] + e[8] * e[32] * e[17] + + e[8] * e[14] * e[35] + e[8] * e[31] * e[16] + e[8] * e[13] * e[34] + + e[2] * e[27] * e[12] + e[2] * e[9] * e[30] + e[2] * e[29] * e[14] + + e[2] * e[11] * e[32] + e[2] * e[28] * e[13] + e[2] * e[10] * e[31] - + e[14] * e[27] * e[0] - e[14] * e[34] * e[7] - e[14] * e[33] * e[6] + + e[14] * e[30] * e[3] - e[14] * e[28] * e[1] + e[14] * e[31] * e[4]; + a[181] = + 0.5 * e[18] * e2[29] + 0.5 * e[18] * e2[28] + 0.5 * e[18] * e2[30] + + 0.5 * e[18] * e2[33] - 0.5 * e[18] * e2[32] - 0.5 * e[18] * e2[31] - + 0.5 * e[18] * e2[34] - 0.5 * e[18] * e2[35] + 1.5 * e[18] * e2[27] + + e[27] * e[28] * e[19] + e[27] * e[29] * e[20] + e[21] * e[27] * e[30] + + e[21] * e[29] * e[32] + e[21] * e[28] * e[31] + e[30] * e[28] * e[22] + + e[30] * e[19] * e[31] + e[30] * e[29] * e[23] + e[30] * e[20] * e[32] + + e[24] * e[27] * e[33] + e[24] * e[29] * e[35] + e[24] * e[28] * e[34] + + e[33] * e[28] * e[25] + e[33] * e[19] * e[34] + e[33] * e[29] * e[26] + + e[33] * e[20] * e[35] - e[27] * e[35] * e[26] - e[27] * e[31] * e[22] - + e[27] * e[32] * e[23] - e[27] * e[34] * e[25]; + a[46] = e[20] * e[1] * e[4] + e[20] * e[0] * e[3] + e[20] * e[2] * e[5] + + e[5] * e[21] * e[3] + e[5] * e[22] * e[4] + e[8] * e[21] * e[6] + + e[8] * e[3] * e[24] + e[8] * e[22] * e[7] + e[8] * e[4] * e[25] + + e[8] * e[5] * e[26] + e[26] * e[4] * e[7] + e[26] * e[3] * e[6] + + e[2] * e[18] * e[3] + e[2] * e[0] * e[21] + e[2] * e[19] * e[4] + + e[2] * e[1] * e[22] - e[5] * e[19] * e[1] - e[5] * e[18] * e[0] - + e[5] * e[25] * e[7] - e[5] * e[24] * e[6] + 0.5 * e[23] * e2[4] - + 0.5 * e[23] * e2[0] - 0.5 * e[23] * e2[6] + 1.5 * e[23] * e2[5] - + 0.5 * e[23] * e2[1] - 0.5 * e[23] * e2[7] + 0.5 * e[23] * e2[3] + + 0.5 * e[23] * e2[2] + 0.5 * e[23] * e2[8]; + a[151] = + 1.5 * e[9] * e2[27] + 0.5 * e[9] * e2[29] + 0.5 * e[9] * e2[28] - + 0.5 * e[9] * e2[32] - 0.5 * e[9] * e2[31] + 0.5 * e[9] * e2[33] + + 0.5 * e[9] * e2[30] - 0.5 * e[9] * e2[34] - 0.5 * e[9] * e2[35] + + e[33] * e[27] * e[15] + e[33] * e[29] * e[17] + e[33] * e[11] * e[35] + + e[33] * e[28] * e[16] + e[33] * e[10] * e[34] + e[27] * e[29] * e[11] + + e[27] * e[28] * e[10] + e[27] * e[30] * e[12] - e[27] * e[31] * e[13] - + e[27] * e[32] * e[14] - e[27] * e[34] * e[16] - e[27] * e[35] * e[17] + + e[30] * e[29] * e[14] + e[30] * e[11] * e[32] + e[30] * e[28] * e[13] + + e[30] * e[10] * e[31] + e[12] * e[29] * e[32] + e[12] * e[28] * e[31] + + e[15] * e[29] * e[35] + e[15] * e[28] * e[34]; + a[116] = -e[32] * e[24] * e[6] + e[8] * e[30] * e[24] + e[8] * e[21] * e[33] + + e[8] * e[31] * e[25] + e[8] * e[22] * e[34] + e[26] * e[30] * e[6] + + e[26] * e[3] * e[33] + e[26] * e[31] * e[7] + e[26] * e[4] * e[34] + + e[26] * e[32] * e[8] + e[26] * e[5] * e[35] + e[35] * e[21] * e[6] + + e[35] * e[3] * e[24] + e[35] * e[22] * e[7] + e[35] * e[4] * e[25] + + e[35] * e[23] * e[8] + e[2] * e[27] * e[21] + e[2] * e[18] * e[30] + + e[2] * e[28] * e[22] + e[2] * e[19] * e[31] + e[2] * e[29] * e[23] + + e[2] * e[20] * e[32] + e[20] * e[27] * e[3] + e[20] * e[0] * e[30] + + e[20] * e[28] * e[4] + e[20] * e[1] * e[31] + e[20] * e[29] * e[5] + + e[29] * e[18] * e[3] + e[29] * e[0] * e[21] + e[29] * e[19] * e[4] + + e[29] * e[1] * e[22] + e[5] * e[30] * e[21] + e[5] * e[31] * e[22] + + 3. * e[5] * e[32] * e[23] - e[5] * e[27] * e[18] - + e[5] * e[33] * e[24] - e[5] * e[28] * e[19] - e[5] * e[34] * e[25] - + e[23] * e[27] * e[0] - e[23] * e[34] * e[7] - e[23] * e[33] * e[6] + + e[23] * e[30] * e[3] - e[23] * e[28] * e[1] + e[23] * e[31] * e[4] + + e[32] * e[21] * e[3] - e[32] * e[19] * e[1] + e[32] * e[22] * e[4] - + e[32] * e[18] * e[0] - e[32] * e[25] * e[7]; + a[191] = 0.5 * e[27] * e2[33] - 0.5 * e[27] * e2[32] - 0.5 * e[27] * e2[31] - + 0.5 * e[27] * e2[34] - 0.5 * e[27] * e2[35] + e[33] * e[29] * e[35] + + 0.5 * e[27] * e2[29] + e[30] * e[29] * e[32] + + e[30] * e[28] * e[31] + e[33] * e[28] * e[34] + + 0.5 * e[27] * e2[28] + 0.5 * e[27] * e2[30] + 0.5 * e3[27]; + a[66] = + e[14] * e[21] * e[12] + e[14] * e[22] * e[13] + e[17] * e[21] * e[15] + + e[17] * e[12] * e[24] + e[17] * e[14] * e[26] + e[17] * e[22] * e[16] + + e[17] * e[13] * e[25] + e[26] * e[12] * e[15] + e[26] * e[13] * e[16] - + e[14] * e[24] * e[15] - e[14] * e[25] * e[16] - e[14] * e[18] * e[9] - + e[14] * e[19] * e[10] + e[11] * e[18] * e[12] + e[11] * e[9] * e[21] + + e[11] * e[19] * e[13] + e[11] * e[10] * e[22] + e[20] * e[11] * e[14] + + e[20] * e[9] * e[12] + e[20] * e[10] * e[13] + 1.5 * e[23] * e2[14] + + 0.5 * e[23] * e2[12] + 0.5 * e[23] * e2[13] + 0.5 * e[23] * e2[17] + + 0.5 * e2[11] * e[23] - 0.5 * e[23] * e2[16] - 0.5 * e[23] * e2[9] - + 0.5 * e[23] * e2[15] - 0.5 * e[23] * e2[10]; + a[121] = 1.5 * e[0] * e2[27] + 0.5 * e[0] * e2[29] + 0.5 * e[0] * e2[28] + + 0.5 * e[0] * e2[30] - 0.5 * e[0] * e2[32] - 0.5 * e[0] * e2[31] + + 0.5 * e[0] * e2[33] - 0.5 * e[0] * e2[34] - 0.5 * e[0] * e2[35] - + e[27] * e[31] * e[4] + e[3] * e[27] * e[30] + e[3] * e[29] * e[32] + + e[3] * e[28] * e[31] + e[30] * e[28] * e[4] + e[30] * e[1] * e[31] + + e[30] * e[29] * e[5] + e[30] * e[2] * e[32] + e[6] * e[27] * e[33] + + e[6] * e[29] * e[35] + e[6] * e[28] * e[34] + e[27] * e[28] * e[1] + + e[27] * e[29] * e[2] + e[33] * e[28] * e[7] + e[33] * e[1] * e[34] + + e[33] * e[29] * e[8] + e[33] * e[2] * e[35] - e[27] * e[34] * e[7] - + e[27] * e[32] * e[5] - e[27] * e[35] * e[8]; + a[36] = e[14] * e[12] * e[3] + e[14] * e[13] * e[4] + e[17] * e[12] * e[6] + + e[17] * e[3] * e[15] + e[17] * e[13] * e[7] + e[17] * e[4] * e[16] + + e[17] * e[14] * e[8] + e[8] * e[12] * e[15] + e[8] * e[13] * e[16] + + e[2] * e[11] * e[14] + e[2] * e[9] * e[12] + e[2] * e[10] * e[13] + + e[11] * e[9] * e[3] + e[11] * e[0] * e[12] + e[11] * e[10] * e[4] + + e[11] * e[1] * e[13] - e[14] * e[10] * e[1] - e[14] * e[16] * e[7] - + e[14] * e[15] * e[6] - e[14] * e[9] * e[0] - 0.5 * e[5] * e2[16] - + 0.5 * e[5] * e2[9] + 0.5 * e[5] * e2[11] + 0.5 * e[5] * e2[12] - + 0.5 * e[5] * e2[15] - 0.5 * e[5] * e2[10] + 0.5 * e[5] * e2[13] + + 1.5 * e2[14] * e[5] + 0.5 * e[5] * e2[17]; + a[71] = 1.5 * e[27] * e2[9] - 0.5 * e[27] * e2[16] + 0.5 * e[27] * e2[11] + + 0.5 * e[27] * e2[12] + 0.5 * e[27] * e2[15] - 0.5 * e[27] * e2[17] + + 0.5 * e[27] * e2[10] - 0.5 * e[27] * e2[14] - 0.5 * e[27] * e2[13] + + e[12] * e[10] * e[31] + e[30] * e[11] * e[14] + + e[30] * e[10] * e[13] + e[15] * e[9] * e[33] + e[15] * e[29] * e[17] + + e[15] * e[11] * e[35] + e[15] * e[28] * e[16] + + e[15] * e[10] * e[34] + e[33] * e[11] * e[17] + + e[33] * e[10] * e[16] - e[9] * e[31] * e[13] - e[9] * e[32] * e[14] - + e[9] * e[34] * e[16] - e[9] * e[35] * e[17] + e[9] * e[29] * e[11] + + e[9] * e[28] * e[10] + e[12] * e[9] * e[30] + e[12] * e[29] * e[14] + + e[12] * e[11] * e[32] + e[12] * e[28] * e[13]; + a[146] = + e[29] * e[18] * e[12] + e[29] * e[9] * e[21] + e[29] * e[19] * e[13] + + e[29] * e[10] * e[22] + e[17] * e[30] * e[24] + e[17] * e[21] * e[33] + + e[17] * e[31] * e[25] + e[17] * e[22] * e[34] + e[17] * e[32] * e[26] + + e[17] * e[23] * e[35] - e[23] * e[27] * e[9] - e[23] * e[28] * e[10] - + e[23] * e[33] * e[15] - e[23] * e[34] * e[16] - e[32] * e[24] * e[15] - + e[32] * e[25] * e[16] - e[32] * e[18] * e[9] - e[32] * e[19] * e[10] + + e[26] * e[30] * e[15] + e[26] * e[12] * e[33] + e[26] * e[31] * e[16] + + e[26] * e[13] * e[34] + e[35] * e[21] * e[15] + e[35] * e[12] * e[24] + + e[35] * e[22] * e[16] + e[35] * e[13] * e[25] + e[14] * e[30] * e[21] + + e[14] * e[31] * e[22] + 3. * e[14] * e[32] * e[23] + + e[11] * e[27] * e[21] + e[11] * e[18] * e[30] + e[11] * e[28] * e[22] + + e[11] * e[19] * e[31] + e[11] * e[29] * e[23] + e[11] * e[20] * e[32] + + e[23] * e[30] * e[12] + e[23] * e[31] * e[13] + e[32] * e[21] * e[12] + + e[32] * e[22] * e[13] - e[14] * e[27] * e[18] - e[14] * e[33] * e[24] + + e[14] * e[29] * e[20] + e[14] * e[35] * e[26] - e[14] * e[28] * e[19] - + e[14] * e[34] * e[25] + e[20] * e[27] * e[12] + e[20] * e[9] * e[30] + + e[20] * e[28] * e[13] + e[20] * e[10] * e[31]; + a[1] = 0.5 * e[0] * e2[1] + 0.5 * e[0] * e2[2] + e[6] * e[2] * e[8] + + e[6] * e[1] * e[7] + 0.5 * e[0] * e2[3] + e[3] * e[1] * e[4] + + 0.5 * e[0] * e2[6] + e[3] * e[2] * e[5] - 0.5 * e[0] * e2[5] - + 0.5 * e[0] * e2[8] + 0.5 * e3[0] - 0.5 * e[0] * e2[7] - + 0.5 * e[0] * e2[4]; + a[136] = + 1.5 * e2[23] * e[14] + 0.5 * e[14] * e2[26] - 0.5 * e[14] * e2[18] - + 0.5 * e[14] * e2[19] + 0.5 * e[14] * e2[20] + 0.5 * e[14] * e2[22] - + 0.5 * e[14] * e2[24] + 0.5 * e[14] * e2[21] - 0.5 * e[14] * e2[25] + + e[23] * e[21] * e[12] + e[23] * e[22] * e[13] + e[26] * e[21] * e[15] + + e[26] * e[12] * e[24] + e[26] * e[23] * e[17] + e[26] * e[22] * e[16] + + e[26] * e[13] * e[25] + e[17] * e[22] * e[25] + e[17] * e[21] * e[24] + + e[11] * e[19] * e[22] + e[11] * e[18] * e[21] + e[11] * e[20] * e[23] + + e[20] * e[18] * e[12] + e[20] * e[9] * e[21] + e[20] * e[19] * e[13] + + e[20] * e[10] * e[22] - e[23] * e[24] * e[15] - e[23] * e[25] * e[16] - + e[23] * e[18] * e[9] - e[23] * e[19] * e[10]; + a[51] = 1.5 * e[27] * e2[0] - 0.5 * e[27] * e2[4] + 0.5 * e[27] * e2[6] - + 0.5 * e[27] * e2[5] + 0.5 * e[27] * e2[1] - 0.5 * e[27] * e2[7] + + 0.5 * e[27] * e2[3] + 0.5 * e[27] * e2[2] - 0.5 * e[27] * e2[8] + + e[0] * e[33] * e[6] + e[0] * e[30] * e[3] - e[0] * e[35] * e[8] - + e[0] * e[31] * e[4] + e[3] * e[28] * e[4] + e[3] * e[1] * e[31] + + e[3] * e[29] * e[5] + e[3] * e[2] * e[32] + e[30] * e[1] * e[4] + + e[30] * e[2] * e[5] + e[6] * e[28] * e[7] + e[6] * e[1] * e[34] + + e[6] * e[29] * e[8] + e[6] * e[2] * e[35] + e[33] * e[1] * e[7] + + e[33] * e[2] * e[8] + e[0] * e[28] * e[1] + e[0] * e[29] * e[2] - + e[0] * e[34] * e[7] - e[0] * e[32] * e[5]; + a[106] = e[8] * e[22] * e[25] + e[8] * e[21] * e[24] + e[20] * e[18] * e[3] + + e[20] * e[0] * e[21] + e[20] * e[19] * e[4] + e[20] * e[1] * e[22] + + e[20] * e[2] * e[23] + e[23] * e[21] * e[3] + e[23] * e[22] * e[4] + + e[23] * e[26] * e[8] - e[23] * e[19] * e[1] - e[23] * e[18] * e[0] - + e[23] * e[25] * e[7] - e[23] * e[24] * e[6] + e[2] * e[19] * e[22] + + e[2] * e[18] * e[21] + e[26] * e[21] * e[6] + e[26] * e[3] * e[24] + + e[26] * e[22] * e[7] + e[26] * e[4] * e[25] + 0.5 * e2[20] * e[5] + + 1.5 * e2[23] * e[5] + 0.5 * e[5] * e2[22] + 0.5 * e[5] * e2[21] + + 0.5 * e[5] * e2[26] - 0.5 * e[5] * e2[18] - 0.5 * e[5] * e2[19] - + 0.5 * e[5] * e2[24] - 0.5 * e[5] * e2[25]; + a[81] = e[24] * e[11] * e[8] + e[24] * e[2] * e[17] + + 3. * e[9] * e[18] * e[0] + e[9] * e[19] * e[1] + e[9] * e[20] * e[2] + + e[18] * e[10] * e[1] + e[18] * e[11] * e[2] + e[3] * e[18] * e[12] + + e[3] * e[9] * e[21] + e[3] * e[20] * e[14] + e[3] * e[11] * e[23] + + e[3] * e[19] * e[13] + e[3] * e[10] * e[22] + e[6] * e[18] * e[15] + + e[6] * e[9] * e[24] + e[6] * e[20] * e[17] + e[6] * e[11] * e[26] + + e[6] * e[19] * e[16] + e[6] * e[10] * e[25] + e[0] * e[20] * e[11] + + e[0] * e[19] * e[10] - e[9] * e[26] * e[8] - e[9] * e[22] * e[4] - + e[9] * e[25] * e[7] - e[9] * e[23] * e[5] + e[12] * e[0] * e[21] + + e[12] * e[19] * e[4] + e[12] * e[1] * e[22] + e[12] * e[20] * e[5] + + e[12] * e[2] * e[23] - e[18] * e[13] * e[4] - e[18] * e[16] * e[7] - + e[18] * e[14] * e[5] - e[18] * e[17] * e[8] + e[21] * e[10] * e[4] + + e[21] * e[1] * e[13] + e[21] * e[11] * e[5] + e[21] * e[2] * e[14] + + e[15] * e[0] * e[24] + e[15] * e[19] * e[7] + e[15] * e[1] * e[25] + + e[15] * e[20] * e[8] + e[15] * e[2] * e[26] - e[0] * e[23] * e[14] - + e[0] * e[25] * e[16] - e[0] * e[26] * e[17] - e[0] * e[22] * e[13] + + e[24] * e[10] * e[7] + e[24] * e[1] * e[16]; + a[26] = e[11] * e[1] * e[4] + e[11] * e[0] * e[3] + e[11] * e[2] * e[5] + + e[5] * e[12] * e[3] + e[5] * e[13] * e[4] + e[8] * e[12] * e[6] + + e[8] * e[3] * e[15] + e[8] * e[13] * e[7] + e[8] * e[4] * e[16] + + e[8] * e[5] * e[17] + e[17] * e[4] * e[7] + e[17] * e[3] * e[6] - + e[5] * e[10] * e[1] - e[5] * e[16] * e[7] - e[5] * e[15] * e[6] - + e[5] * e[9] * e[0] + e[2] * e[9] * e[3] + e[2] * e[0] * e[12] + + e[2] * e[10] * e[4] + e[2] * e[1] * e[13] + 0.5 * e2[2] * e[14] - + 0.5 * e[14] * e2[0] - 0.5 * e[14] * e2[6] - 0.5 * e[14] * e2[1] - + 0.5 * e[14] * e2[7] + 1.5 * e[14] * e2[5] + 0.5 * e[14] * e2[4] + + 0.5 * e[14] * e2[3] + 0.5 * e[14] * e2[8]; + a[91] = e[3] * e[27] * e[12] + e[3] * e[9] * e[30] + e[3] * e[29] * e[14] + + e[3] * e[11] * e[32] + e[3] * e[28] * e[13] + e[3] * e[10] * e[31] + + e[6] * e[27] * e[15] + e[6] * e[9] * e[33] + e[6] * e[29] * e[17] + + e[6] * e[11] * e[35] + e[6] * e[28] * e[16] + e[6] * e[10] * e[34] + + 3. * e[0] * e[27] * e[9] + e[0] * e[29] * e[11] + + e[0] * e[28] * e[10] - e[9] * e[34] * e[7] - e[9] * e[32] * e[5] - + e[9] * e[35] * e[8] + e[9] * e[29] * e[2] + e[9] * e[28] * e[1] - + e[9] * e[31] * e[4] + e[12] * e[0] * e[30] + e[12] * e[28] * e[4] + + e[12] * e[1] * e[31] + e[12] * e[29] * e[5] + e[12] * e[2] * e[32] + + e[27] * e[11] * e[2] + e[27] * e[10] * e[1] - e[27] * e[13] * e[4] - + e[27] * e[16] * e[7] - e[27] * e[14] * e[5] - e[27] * e[17] * e[8] + + e[30] * e[10] * e[4] + e[30] * e[1] * e[13] + e[30] * e[11] * e[5] + + e[30] * e[2] * e[14] + e[15] * e[0] * e[33] + e[15] * e[28] * e[7] + + e[15] * e[1] * e[34] + e[15] * e[29] * e[8] + e[15] * e[2] * e[35] - + e[0] * e[31] * e[13] - e[0] * e[32] * e[14] - e[0] * e[34] * e[16] - + e[0] * e[35] * e[17] + e[33] * e[10] * e[7] + e[33] * e[1] * e[16] + + e[33] * e[11] * e[8] + e[33] * e[2] * e[17]; + a[127] = 0.5 * e2[30] * e[6] + 0.5 * e[6] * e2[27] - 0.5 * e[6] * e2[32] - + 0.5 * e[6] * e2[28] - 0.5 * e[6] * e2[29] - 0.5 * e[6] * e2[31] + + 1.5 * e[6] * e2[33] + 0.5 * e[6] * e2[34] + 0.5 * e[6] * e2[35] + + e[0] * e[27] * e[33] + e[0] * e[29] * e[35] + e[0] * e[28] * e[34] + + e[3] * e[30] * e[33] + e[3] * e[32] * e[35] + e[3] * e[31] * e[34] + + e[30] * e[31] * e[7] + e[30] * e[4] * e[34] + e[30] * e[32] * e[8] + + e[30] * e[5] * e[35] + e[27] * e[28] * e[7] + e[27] * e[1] * e[34] + + e[27] * e[29] * e[8] + e[27] * e[2] * e[35] + e[33] * e[34] * e[7] + + e[33] * e[35] * e[8] - e[33] * e[32] * e[5] - e[33] * e[29] * e[2] - + e[33] * e[28] * e[1] - e[33] * e[31] * e[4]; + a[161] = e[24] * e[20] * e[26] + e[21] * e[19] * e[22] - + 0.5 * e[18] * e2[22] - 0.5 * e[18] * e2[25] + 0.5 * e3[18] + + 0.5 * e[18] * e2[21] + e[21] * e[20] * e[23] + 0.5 * e[18] * e2[20] + + 0.5 * e[18] * e2[19] + 0.5 * e[18] * e2[24] + e[24] * e[19] * e[25] - + 0.5 * e[18] * e2[23] - 0.5 * e[18] * e2[26]; + a[197] = 0.5 * e[33] * e2[35] + 0.5 * e3[33] + 0.5 * e2[27] * e[33] + + 0.5 * e2[30] * e[33] - 0.5 * e[33] * e2[29] + 0.5 * e[33] * e2[34] - + 0.5 * e[33] * e2[32] - 0.5 * e[33] * e2[28] + e[30] * e[32] * e[35] - + 0.5 * e[33] * e2[31] + e[27] * e[29] * e[35] + + e[27] * e[28] * e[34] + e[30] * e[31] * e[34]; + a[171] = + 1.5 * e[27] * e2[18] + 0.5 * e[27] * e2[19] + 0.5 * e[27] * e2[20] + + 0.5 * e[27] * e2[21] + 0.5 * e[27] * e2[24] - 0.5 * e[27] * e2[26] - + 0.5 * e[27] * e2[23] - 0.5 * e[27] * e2[22] - 0.5 * e[27] * e2[25] + + e[33] * e[20] * e[26] - e[18] * e[35] * e[26] - e[18] * e[31] * e[22] - + e[18] * e[32] * e[23] - e[18] * e[34] * e[25] + e[18] * e[28] * e[19] + + e[18] * e[29] * e[20] + e[21] * e[18] * e[30] + e[21] * e[28] * e[22] + + e[21] * e[19] * e[31] + e[21] * e[29] * e[23] + e[21] * e[20] * e[32] + + e[30] * e[19] * e[22] + e[30] * e[20] * e[23] + e[24] * e[18] * e[33] + + e[24] * e[28] * e[25] + e[24] * e[19] * e[34] + e[24] * e[29] * e[26] + + e[24] * e[20] * e[35] + e[33] * e[19] * e[25]; + a[157] = + e[9] * e[27] * e[33] + e[9] * e[29] * e[35] + e[9] * e[28] * e[34] + + e[33] * e[35] * e[17] + e[33] * e[34] * e[16] + e[27] * e[29] * e[17] + + e[27] * e[11] * e[35] + e[27] * e[28] * e[16] + e[27] * e[10] * e[34] + + e[33] * e[30] * e[12] - e[33] * e[28] * e[10] - e[33] * e[31] * e[13] - + e[33] * e[32] * e[14] - e[33] * e[29] * e[11] + e[30] * e[32] * e[17] + + e[30] * e[14] * e[35] + e[30] * e[31] * e[16] + e[30] * e[13] * e[34] + + e[12] * e[32] * e[35] + e[12] * e[31] * e[34] + 0.5 * e[15] * e2[27] - + 0.5 * e[15] * e2[32] - 0.5 * e[15] * e2[28] - 0.5 * e[15] * e2[29] - + 0.5 * e[15] * e2[31] + 1.5 * e[15] * e2[33] + 0.5 * e[15] * e2[30] + + 0.5 * e[15] * e2[34] + 0.5 * e[15] * e2[35]; + a[11] = 0.5 * e[9] * e2[12] - 0.5 * e[9] * e2[16] + 0.5 * e[9] * e2[10] - + 0.5 * e[9] * e2[17] - 0.5 * e[9] * e2[13] + e[15] * e[10] * e[16] + + e[12] * e[11] * e[14] + 0.5 * e[9] * e2[11] + 0.5 * e[9] * e2[15] - + 0.5 * e[9] * e2[14] + e[15] * e[11] * e[17] + 0.5 * e3[9] + + e[12] * e[10] * e[13]; + a[187] = + e[18] * e[27] * e[33] + e[18] * e[29] * e[35] + e[18] * e[28] * e[34] + + e[27] * e[28] * e[25] + e[27] * e[19] * e[34] + e[27] * e[29] * e[26] + + e[27] * e[20] * e[35] + e[21] * e[30] * e[33] + e[21] * e[32] * e[35] + + e[21] * e[31] * e[34] + e[30] * e[31] * e[25] + e[30] * e[22] * e[34] + + e[30] * e[32] * e[26] + e[30] * e[23] * e[35] + e[33] * e[34] * e[25] + + e[33] * e[35] * e[26] - e[33] * e[29] * e[20] - e[33] * e[31] * e[22] - + e[33] * e[32] * e[23] - e[33] * e[28] * e[19] + 0.5 * e2[27] * e[24] + + 0.5 * e2[30] * e[24] + 1.5 * e[24] * e2[33] + 0.5 * e[24] * e2[35] + + 0.5 * e[24] * e2[34] - 0.5 * e[24] * e2[32] - 0.5 * e[24] * e2[28] - + 0.5 * e[24] * e2[29] - 0.5 * e[24] * e2[31]; + a[131] = + 0.5 * e[9] * e2[21] + 0.5 * e[9] * e2[24] + 0.5 * e[9] * e2[19] + + 1.5 * e[9] * e2[18] + 0.5 * e[9] * e2[20] - 0.5 * e[9] * e2[26] - + 0.5 * e[9] * e2[23] - 0.5 * e[9] * e2[22] - 0.5 * e[9] * e2[25] + + e[21] * e[18] * e[12] + e[21] * e[20] * e[14] + e[21] * e[11] * e[23] + + e[21] * e[19] * e[13] + e[21] * e[10] * e[22] + e[24] * e[18] * e[15] + + e[24] * e[20] * e[17] + e[24] * e[11] * e[26] + e[24] * e[19] * e[16] + + e[24] * e[10] * e[25] + e[15] * e[19] * e[25] + e[15] * e[20] * e[26] + + e[12] * e[19] * e[22] + e[12] * e[20] * e[23] + e[18] * e[20] * e[11] + + e[18] * e[19] * e[10] - e[18] * e[23] * e[14] - e[18] * e[25] * e[16] - + e[18] * e[26] * e[17] - e[18] * e[22] * e[13]; + a[189] = + 0.5 * e2[29] * e[26] + 0.5 * e2[32] * e[26] + 0.5 * e[26] * e2[33] + + 1.5 * e[26] * e2[35] + 0.5 * e[26] * e2[34] - 0.5 * e[26] * e2[27] - + 0.5 * e[26] * e2[28] - 0.5 * e[26] * e2[31] - 0.5 * e[26] * e2[30] + + e[20] * e[27] * e[33] + e[20] * e[29] * e[35] + e[20] * e[28] * e[34] + + e[29] * e[27] * e[24] + e[29] * e[18] * e[33] + e[29] * e[28] * e[25] + + e[29] * e[19] * e[34] + e[23] * e[30] * e[33] + e[23] * e[32] * e[35] + + e[23] * e[31] * e[34] + e[32] * e[30] * e[24] + e[32] * e[21] * e[33] + + e[32] * e[31] * e[25] + e[32] * e[22] * e[34] + e[35] * e[33] * e[24] + + e[35] * e[34] * e[25] - e[35] * e[27] * e[18] - e[35] * e[30] * e[21] - + e[35] * e[31] * e[22] - e[35] * e[28] * e[19]; + a[141] = + e[12] * e[19] * e[31] + e[12] * e[29] * e[23] + e[12] * e[20] * e[32] + + 3. * e[9] * e[27] * e[18] + e[9] * e[28] * e[19] + e[9] * e[29] * e[20] + + e[21] * e[9] * e[30] + e[21] * e[29] * e[14] + e[21] * e[11] * e[32] + + e[21] * e[28] * e[13] + e[21] * e[10] * e[31] + e[30] * e[20] * e[14] + + e[30] * e[11] * e[23] + e[30] * e[19] * e[13] + e[30] * e[10] * e[22] + + e[9] * e[33] * e[24] - e[9] * e[35] * e[26] - e[9] * e[31] * e[22] - + e[9] * e[32] * e[23] - e[9] * e[34] * e[25] + e[18] * e[29] * e[11] + + e[18] * e[28] * e[10] + e[27] * e[20] * e[11] + e[27] * e[19] * e[10] + + e[15] * e[27] * e[24] + e[15] * e[18] * e[33] + e[15] * e[28] * e[25] + + e[15] * e[19] * e[34] + e[15] * e[29] * e[26] + e[15] * e[20] * e[35] - + e[18] * e[31] * e[13] - e[18] * e[32] * e[14] - e[18] * e[34] * e[16] - + e[18] * e[35] * e[17] - e[27] * e[23] * e[14] - e[27] * e[25] * e[16] - + e[27] * e[26] * e[17] - e[27] * e[22] * e[13] + e[24] * e[29] * e[17] + + e[24] * e[11] * e[35] + e[24] * e[28] * e[16] + e[24] * e[10] * e[34] + + e[33] * e[20] * e[17] + e[33] * e[11] * e[26] + e[33] * e[19] * e[16] + + e[33] * e[10] * e[25] + e[12] * e[27] * e[21] + e[12] * e[18] * e[30] + + e[12] * e[28] * e[22]; + a[159] = + -0.5 * e[17] * e2[27] + 0.5 * e[17] * e2[32] - 0.5 * e[17] * e2[28] + + 0.5 * e[17] * e2[29] - 0.5 * e[17] * e2[31] + 0.5 * e[17] * e2[33] - + 0.5 * e[17] * e2[30] + 0.5 * e[17] * e2[34] + 1.5 * e[17] * e2[35] + + e[32] * e[30] * e[15] + e[32] * e[12] * e[33] + e[32] * e[31] * e[16] + + e[32] * e[13] * e[34] + e[14] * e[30] * e[33] + e[14] * e[31] * e[34] + + e[11] * e[27] * e[33] + e[11] * e[29] * e[35] + e[11] * e[28] * e[34] + + e[35] * e[33] * e[15] + e[35] * e[34] * e[16] + e[29] * e[27] * e[15] + + e[29] * e[9] * e[33] + e[29] * e[28] * e[16] + e[29] * e[10] * e[34] - + e[35] * e[27] * e[9] - e[35] * e[30] * e[12] - e[35] * e[28] * e[10] - + e[35] * e[31] * e[13] + e[35] * e[32] * e[14]; + a[21] = 0.5 * e[9] * e2[1] + 1.5 * e[9] * e2[0] + 0.5 * e[9] * e2[2] + + 0.5 * e[9] * e2[3] + 0.5 * e[9] * e2[6] - 0.5 * e[9] * e2[4] - + 0.5 * e[9] * e2[5] - 0.5 * e[9] * e2[7] - 0.5 * e[9] * e2[8] + + e[6] * e[0] * e[15] + e[6] * e[10] * e[7] + e[6] * e[1] * e[16] + + e[6] * e[11] * e[8] + e[6] * e[2] * e[17] + e[15] * e[1] * e[7] + + e[15] * e[2] * e[8] + e[0] * e[11] * e[2] + e[0] * e[10] * e[1] - + e[0] * e[13] * e[4] - e[0] * e[16] * e[7] - e[0] * e[14] * e[5] - + e[0] * e[17] * e[8] + e[3] * e[0] * e[12] + e[3] * e[10] * e[4] + + e[3] * e[1] * e[13] + e[3] * e[11] * e[5] + e[3] * e[2] * e[14] + + e[12] * e[1] * e[4] + e[12] * e[2] * e[5]; + a[199] = 0.5 * e[35] * e2[33] + 0.5 * e[35] * e2[34] - 0.5 * e[35] * e2[27] - + 0.5 * e[35] * e2[28] - 0.5 * e[35] * e2[31] - 0.5 * e[35] * e2[30] + + e[32] * e[31] * e[34] + 0.5 * e2[29] * e[35] + 0.5 * e2[32] * e[35] + + e[29] * e[28] * e[34] + e[32] * e[30] * e[33] + 0.5 * e3[35] + + e[29] * e[27] * e[33]; + a[101] = 0.5 * e[0] * e2[19] + 0.5 * e[0] * e2[20] + 0.5 * e[0] * e2[24] - + 0.5 * e[0] * e2[26] - 0.5 * e[0] * e2[23] - 0.5 * e[0] * e2[22] - + 0.5 * e[0] * e2[25] + 1.5 * e2[18] * e[0] + 0.5 * e[0] * e2[21] + + e[18] * e[19] * e[1] + e[18] * e[20] * e[2] + e[21] * e[18] * e[3] + + e[21] * e[19] * e[4] + e[21] * e[1] * e[22] + e[21] * e[20] * e[5] + + e[21] * e[2] * e[23] - e[18] * e[26] * e[8] - e[18] * e[22] * e[4] - + e[18] * e[25] * e[7] - e[18] * e[23] * e[5] + e[18] * e[24] * e[6] + + e[3] * e[19] * e[22] + e[3] * e[20] * e[23] + e[24] * e[19] * e[7] + + e[24] * e[1] * e[25] + e[24] * e[20] * e[8] + e[24] * e[2] * e[26] + + e[6] * e[19] * e[25] + e[6] * e[20] * e[26]; + a[129] = 0.5 * e2[32] * e[8] - 0.5 * e[8] * e2[27] - 0.5 * e[8] * e2[28] + + 0.5 * e[8] * e2[29] - 0.5 * e[8] * e2[31] + 0.5 * e[8] * e2[33] - + 0.5 * e[8] * e2[30] + 0.5 * e[8] * e2[34] + 1.5 * e[8] * e2[35] + + e[2] * e[27] * e[33] + e[2] * e[29] * e[35] + e[2] * e[28] * e[34] + + e[5] * e[30] * e[33] + e[5] * e[32] * e[35] + e[5] * e[31] * e[34] + + e[32] * e[30] * e[6] + e[32] * e[3] * e[33] + e[32] * e[31] * e[7] + + e[32] * e[4] * e[34] + e[29] * e[27] * e[6] + e[29] * e[0] * e[33] + + e[29] * e[28] * e[7] + e[29] * e[1] * e[34] + e[35] * e[33] * e[6] + + e[35] * e[34] * e[7] - e[35] * e[27] * e[0] - e[35] * e[30] * e[3] - + e[35] * e[28] * e[1] - e[35] * e[31] * e[4]; + a[41] = -0.5 * e[18] * e2[4] + 1.5 * e[18] * e2[0] + 0.5 * e[18] * e2[6] - + 0.5 * e[18] * e2[5] + 0.5 * e[18] * e2[1] - 0.5 * e[18] * e2[7] + + 0.5 * e[18] * e2[3] + 0.5 * e[18] * e2[2] - 0.5 * e[18] * e2[8] + + e[3] * e[0] * e[21] + e[3] * e[19] * e[4] + e[3] * e[1] * e[22] + + e[3] * e[20] * e[5] + e[3] * e[2] * e[23] + e[21] * e[1] * e[4] + + e[21] * e[2] * e[5] + e[6] * e[0] * e[24] + e[6] * e[19] * e[7] + + e[6] * e[1] * e[25] + e[6] * e[20] * e[8] + e[6] * e[2] * e[26] + + e[24] * e[1] * e[7] + e[24] * e[2] * e[8] + e[0] * e[19] * e[1] + + e[0] * e[20] * e[2] - e[0] * e[26] * e[8] - e[0] * e[22] * e[4] - + e[0] * e[25] * e[7] - e[0] * e[23] * e[5]; + a[28] = e[10] * e[1] * e[7] + e[10] * e[0] * e[6] + e[10] * e[2] * e[8] + + e[4] * e[12] * e[6] + e[4] * e[3] * e[15] + e[4] * e[13] * e[7] + + e[4] * e[14] * e[8] + e[4] * e[5] * e[17] + e[13] * e[3] * e[6] + + e[13] * e[5] * e[8] + e[7] * e[15] * e[6] + e[7] * e[17] * e[8] - + e[7] * e[11] * e[2] - e[7] * e[9] * e[0] - e[7] * e[14] * e[5] - + e[7] * e[12] * e[3] + e[1] * e[9] * e[6] + e[1] * e[0] * e[15] + + e[1] * e[11] * e[8] + e[1] * e[2] * e[17] + 1.5 * e[16] * e2[7] + + 0.5 * e[16] * e2[6] + 0.5 * e[16] * e2[8] + 0.5 * e2[1] * e[16] - + 0.5 * e[16] * e2[0] - 0.5 * e[16] * e2[5] - 0.5 * e[16] * e2[3] - + 0.5 * e[16] * e2[2] + 0.5 * e2[4] * e[16]; + a[111] = e[0] * e[30] * e[21] - e[0] * e[35] * e[26] - e[0] * e[31] * e[22] - + e[0] * e[32] * e[23] - e[0] * e[34] * e[25] - e[18] * e[34] * e[7] - + e[18] * e[32] * e[5] - e[18] * e[35] * e[8] - e[18] * e[31] * e[4] - + e[27] * e[26] * e[8] - e[27] * e[22] * e[4] - e[27] * e[25] * e[7] - + e[27] * e[23] * e[5] + e[6] * e[28] * e[25] + e[6] * e[19] * e[34] + + e[6] * e[29] * e[26] + e[6] * e[20] * e[35] + e[21] * e[28] * e[4] + + e[21] * e[1] * e[31] + e[21] * e[29] * e[5] + e[21] * e[2] * e[32] + + e[30] * e[19] * e[4] + e[30] * e[1] * e[22] + e[30] * e[20] * e[5] + + e[30] * e[2] * e[23] + e[24] * e[27] * e[6] + e[24] * e[0] * e[33] + + e[24] * e[28] * e[7] + e[24] * e[1] * e[34] + e[24] * e[29] * e[8] + + e[24] * e[2] * e[35] + e[33] * e[18] * e[6] + e[33] * e[19] * e[7] + + e[33] * e[1] * e[25] + e[33] * e[20] * e[8] + e[33] * e[2] * e[26] + + 3. * e[0] * e[27] * e[18] + e[0] * e[28] * e[19] + + e[0] * e[29] * e[20] + e[18] * e[28] * e[1] + e[18] * e[29] * e[2] + + e[27] * e[19] * e[1] + e[27] * e[20] * e[2] + e[3] * e[27] * e[21] + + e[3] * e[18] * e[30] + e[3] * e[28] * e[22] + e[3] * e[19] * e[31] + + e[3] * e[29] * e[23] + e[3] * e[20] * e[32]; + a[108] = e[19] * e[18] * e[6] + e[19] * e[0] * e[24] + e[19] * e[1] * e[25] + + e[19] * e[20] * e[8] + e[19] * e[2] * e[26] + e[22] * e[21] * e[6] + + e[22] * e[3] * e[24] + e[22] * e[4] * e[25] + e[22] * e[23] * e[8] + + e[22] * e[5] * e[26] - e[25] * e[21] * e[3] + e[25] * e[26] * e[8] - + e[25] * e[20] * e[2] - e[25] * e[18] * e[0] - e[25] * e[23] * e[5] + + e[25] * e[24] * e[6] + e[1] * e[18] * e[24] + e[1] * e[20] * e[26] + + e[4] * e[21] * e[24] + e[4] * e[23] * e[26] + 0.5 * e2[19] * e[7] + + 0.5 * e2[22] * e[7] + 1.5 * e2[25] * e[7] + 0.5 * e[7] * e2[26] - + 0.5 * e[7] * e2[18] - 0.5 * e[7] * e2[23] - 0.5 * e[7] * e2[20] + + 0.5 * e[7] * e2[24] - 0.5 * e[7] * e2[21]; + a[61] = 0.5 * e[18] * e2[11] + 1.5 * e[18] * e2[9] + 0.5 * e[18] * e2[10] + + 0.5 * e[18] * e2[12] + 0.5 * e[18] * e2[15] - 0.5 * e[18] * e2[16] - + 0.5 * e[18] * e2[17] - 0.5 * e[18] * e2[14] - 0.5 * e[18] * e2[13] + + e[12] * e[9] * e[21] + e[12] * e[20] * e[14] + e[12] * e[11] * e[23] + + e[12] * e[19] * e[13] + e[12] * e[10] * e[22] + + e[21] * e[11] * e[14] + e[21] * e[10] * e[13] + e[15] * e[9] * e[24] + + e[15] * e[20] * e[17] + e[15] * e[11] * e[26] + + e[15] * e[19] * e[16] + e[15] * e[10] * e[25] + + e[24] * e[11] * e[17] + e[24] * e[10] * e[16] - e[9] * e[23] * e[14] - + e[9] * e[25] * e[16] - e[9] * e[26] * e[17] + e[9] * e[20] * e[11] + + e[9] * e[19] * e[10] - e[9] * e[22] * e[13]; + a[138] = + e[13] * e[21] * e[24] + e[13] * e[23] * e[26] + e[19] * e[18] * e[15] + + e[19] * e[9] * e[24] + e[19] * e[20] * e[17] + e[19] * e[11] * e[26] - + e[25] * e[23] * e[14] - e[25] * e[20] * e[11] - e[25] * e[18] * e[9] - + e[25] * e[21] * e[12] + e[22] * e[21] * e[15] + e[22] * e[12] * e[24] + + e[22] * e[23] * e[17] + e[22] * e[14] * e[26] + e[22] * e[13] * e[25] + + e[25] * e[24] * e[15] + e[25] * e[26] * e[17] + e[10] * e[19] * e[25] + + e[10] * e[18] * e[24] + e[10] * e[20] * e[26] - 0.5 * e[16] * e2[18] - + 0.5 * e[16] * e2[23] + 0.5 * e[16] * e2[19] - 0.5 * e[16] * e2[20] - + 0.5 * e[16] * e2[21] + 0.5 * e2[22] * e[16] + 1.5 * e2[25] * e[16] + + 0.5 * e[16] * e2[24] + 0.5 * e[16] * e2[26]; + a[31] = 0.5 * e[0] * e2[12] + 0.5 * e[0] * e2[15] + 0.5 * e[0] * e2[11] + + 1.5 * e[0] * e2[9] + 0.5 * e[0] * e2[10] - 0.5 * e[0] * e2[16] - + 0.5 * e[0] * e2[17] - 0.5 * e[0] * e2[14] - 0.5 * e[0] * e2[13] + + e[12] * e[9] * e[3] + e[12] * e[10] * e[4] + e[12] * e[1] * e[13] + + e[12] * e[11] * e[5] + e[12] * e[2] * e[14] + e[15] * e[9] * e[6] + + e[15] * e[10] * e[7] + e[15] * e[1] * e[16] + e[15] * e[11] * e[8] + + e[15] * e[2] * e[17] + e[6] * e[11] * e[17] + e[6] * e[10] * e[16] + + e[3] * e[11] * e[14] + e[3] * e[10] * e[13] + e[9] * e[10] * e[1] + + e[9] * e[11] * e[2] - e[9] * e[13] * e[4] - e[9] * e[16] * e[7] - + e[9] * e[14] * e[5] - e[9] * e[17] * e[8]; + a[148] = + e[19] * e[11] * e[35] + e[28] * e[18] * e[15] + e[28] * e[9] * e[24] + + e[28] * e[20] * e[17] + e[28] * e[11] * e[26] - e[25] * e[27] * e[9] - + e[25] * e[30] * e[12] - e[25] * e[32] * e[14] + e[25] * e[33] * e[15] + + e[25] * e[35] * e[17] - e[25] * e[29] * e[11] - e[34] * e[23] * e[14] + + e[34] * e[24] * e[15] + e[34] * e[26] * e[17] - e[34] * e[20] * e[11] - + e[34] * e[18] * e[9] - e[34] * e[21] * e[12] + e[13] * e[30] * e[24] + + e[13] * e[21] * e[33] + e[13] * e[31] * e[25] + e[13] * e[22] * e[34] + + e[13] * e[32] * e[26] + e[13] * e[23] * e[35] + e[10] * e[27] * e[24] + + e[10] * e[18] * e[33] + e[10] * e[28] * e[25] + e[10] * e[19] * e[34] + + e[10] * e[29] * e[26] + e[10] * e[20] * e[35] + e[22] * e[30] * e[15] + + e[22] * e[12] * e[33] + e[22] * e[32] * e[17] + e[22] * e[14] * e[35] + + e[22] * e[31] * e[16] + e[31] * e[21] * e[15] + e[31] * e[12] * e[24] + + e[31] * e[23] * e[17] + e[31] * e[14] * e[26] - e[16] * e[27] * e[18] + + e[16] * e[33] * e[24] - e[16] * e[30] * e[21] - e[16] * e[29] * e[20] + + e[16] * e[35] * e[26] - e[16] * e[32] * e[23] + e[16] * e[28] * e[19] + + 3. * e[16] * e[34] * e[25] + e[19] * e[27] * e[15] + + e[19] * e[9] * e[33] + e[19] * e[29] * e[17]; + a[52] = e[4] * e[27] * e[3] + e[4] * e[0] * e[30] + e[4] * e[29] * e[5] + + e[4] * e[2] * e[32] + e[31] * e[0] * e[3] + e[31] * e[2] * e[5] + + e[7] * e[27] * e[6] + e[7] * e[0] * e[33] + e[7] * e[29] * e[8] + + e[7] * e[2] * e[35] + e[34] * e[0] * e[6] + e[34] * e[2] * e[8] + + e[1] * e[27] * e[0] + e[1] * e[29] * e[2] + e[1] * e[34] * e[7] - + e[1] * e[32] * e[5] - e[1] * e[33] * e[6] - e[1] * e[30] * e[3] - + e[1] * e[35] * e[8] + e[1] * e[31] * e[4] + 1.5 * e[28] * e2[1] + + 0.5 * e[28] * e2[4] + 0.5 * e[28] * e2[0] - 0.5 * e[28] * e2[6] - + 0.5 * e[28] * e2[5] + 0.5 * e[28] * e2[7] - 0.5 * e[28] * e2[3] + + 0.5 * e[28] * e2[2] - 0.5 * e[28] * e2[8]; + a[99] = -e[35] * e[10] * e[1] - e[35] * e[13] * e[4] + e[35] * e[16] * e[7] + + e[35] * e[15] * e[6] - e[35] * e[9] * e[0] - e[35] * e[12] * e[3] + + e[32] * e[12] * e[6] + e[32] * e[3] * e[15] + e[32] * e[13] * e[7] + + e[32] * e[4] * e[16] - e[8] * e[27] * e[9] - e[8] * e[30] * e[12] - + e[8] * e[28] * e[10] - e[8] * e[31] * e[13] + e[8] * e[29] * e[11] + + e[11] * e[27] * e[6] + e[11] * e[0] * e[33] + e[11] * e[28] * e[7] + + e[11] * e[1] * e[34] + e[29] * e[9] * e[6] + e[29] * e[0] * e[15] + + e[29] * e[10] * e[7] + e[29] * e[1] * e[16] + e[5] * e[30] * e[15] + + e[5] * e[12] * e[33] + e[5] * e[32] * e[17] + e[5] * e[14] * e[35] + + e[5] * e[31] * e[16] + e[5] * e[13] * e[34] + e[8] * e[33] * e[15] + + 3. * e[8] * e[35] * e[17] + e[8] * e[34] * e[16] + + e[2] * e[27] * e[15] + e[2] * e[9] * e[33] + e[2] * e[29] * e[17] + + e[2] * e[11] * e[35] + e[2] * e[28] * e[16] + e[2] * e[10] * e[34] - + e[17] * e[27] * e[0] + e[17] * e[34] * e[7] + e[17] * e[33] * e[6] - + e[17] * e[30] * e[3] - e[17] * e[28] * e[1] - e[17] * e[31] * e[4] + + e[14] * e[30] * e[6] + e[14] * e[3] * e[33] + e[14] * e[31] * e[7] + + e[14] * e[4] * e[34] + e[14] * e[32] * e[8]; + a[82] = e[19] * e[11] * e[2] + e[4] * e[18] * e[12] + e[4] * e[9] * e[21] + + e[4] * e[20] * e[14] + e[4] * e[11] * e[23] + e[4] * e[19] * e[13] + + e[4] * e[10] * e[22] + e[7] * e[18] * e[15] + e[7] * e[9] * e[24] + + e[7] * e[20] * e[17] + e[7] * e[11] * e[26] + e[7] * e[19] * e[16] + + e[7] * e[10] * e[25] + e[1] * e[18] * e[9] + e[1] * e[20] * e[11] - + e[10] * e[21] * e[3] - e[10] * e[26] * e[8] - e[10] * e[23] * e[5] - + e[10] * e[24] * e[6] + e[13] * e[18] * e[3] + e[13] * e[0] * e[21] + + e[13] * e[1] * e[22] + e[13] * e[20] * e[5] + e[13] * e[2] * e[23] - + e[19] * e[15] * e[6] - e[19] * e[14] * e[5] - e[19] * e[12] * e[3] - + e[19] * e[17] * e[8] + e[22] * e[9] * e[3] + e[22] * e[0] * e[12] + + e[22] * e[11] * e[5] + e[22] * e[2] * e[14] + e[16] * e[18] * e[6] + + e[16] * e[0] * e[24] + e[16] * e[1] * e[25] + e[16] * e[20] * e[8] + + e[16] * e[2] * e[26] - e[1] * e[23] * e[14] - e[1] * e[24] * e[15] - + e[1] * e[26] * e[17] - e[1] * e[21] * e[12] + e[25] * e[9] * e[6] + + e[25] * e[0] * e[15] + e[25] * e[11] * e[8] + e[25] * e[2] * e[17] + + e[10] * e[18] * e[0] + 3. * e[10] * e[19] * e[1] + + e[10] * e[20] * e[2] + e[19] * e[9] * e[0]; + a[169] = 0.5 * e2[23] * e[26] + 0.5 * e[26] * e2[25] + 0.5 * e2[20] * e[26] - + 0.5 * e[26] * e2[18] + 0.5 * e3[26] + 0.5 * e[26] * e2[24] + + e[20] * e[19] * e[25] - 0.5 * e[26] * e2[19] - 0.5 * e[26] * e2[21] + + e[20] * e[18] * e[24] - 0.5 * e[26] * e2[22] + + e[23] * e[21] * e[24] + e[23] * e[22] * e[25]; + a[72] = e[16] * e[9] * e[33] + e[16] * e[29] * e[17] + e[16] * e[11] * e[35] + + e[16] * e[10] * e[34] + e[34] * e[11] * e[17] + e[34] * e[9] * e[15] - + e[10] * e[30] * e[12] - e[10] * e[32] * e[14] - + e[10] * e[33] * e[15] - e[10] * e[35] * e[17] + e[10] * e[27] * e[9] + + e[10] * e[29] * e[11] + e[13] * e[27] * e[12] + e[13] * e[9] * e[30] + + e[13] * e[29] * e[14] + e[13] * e[11] * e[32] + + e[13] * e[10] * e[31] + e[31] * e[11] * e[14] + e[31] * e[9] * e[12] + + e[16] * e[27] * e[15] + 1.5 * e[28] * e2[10] + 0.5 * e[28] * e2[16] + + 0.5 * e[28] * e2[9] + 0.5 * e[28] * e2[11] - 0.5 * e[28] * e2[12] - + 0.5 * e[28] * e2[15] - 0.5 * e[28] * e2[17] - 0.5 * e[28] * e2[14] + + 0.5 * e[28] * e2[13]; + a[179] = + 0.5 * e2[20] * e[35] + 0.5 * e2[23] * e[35] + 1.5 * e[35] * e2[26] + + 0.5 * e[35] * e2[25] + 0.5 * e[35] * e2[24] - 0.5 * e[35] * e2[18] - + 0.5 * e[35] * e2[19] - 0.5 * e[35] * e2[22] - 0.5 * e[35] * e2[21] + + e[20] * e[27] * e[24] + e[20] * e[18] * e[33] + e[20] * e[28] * e[25] + + e[20] * e[19] * e[34] + e[20] * e[29] * e[26] + e[29] * e[19] * e[25] + + e[29] * e[18] * e[24] + e[23] * e[30] * e[24] + e[23] * e[21] * e[33] + + e[23] * e[31] * e[25] + e[23] * e[22] * e[34] + e[23] * e[32] * e[26] + + e[32] * e[22] * e[25] + e[32] * e[21] * e[24] + e[26] * e[33] * e[24] + + e[26] * e[34] * e[25] - e[26] * e[27] * e[18] - e[26] * e[30] * e[21] - + e[26] * e[31] * e[22] - e[26] * e[28] * e[19]; + a[2] = e[4] * e[2] * e[5] + 0.5 * e[1] * e2[0] - 0.5 * e[1] * e2[6] + + e[7] * e[0] * e[6] + 0.5 * e[1] * e2[7] + 0.5 * e[1] * e2[4] - + 0.5 * e[1] * e2[8] + 0.5 * e[1] * e2[2] - 0.5 * e[1] * e2[3] + + 0.5 * e3[1] + e[7] * e[2] * e[8] - 0.5 * e[1] * e2[5] + + e[4] * e[0] * e[3]; + a[19] = -0.5 * e[17] * e2[13] - 0.5 * e[17] * e2[9] + 0.5 * e[17] * e2[16] + + 0.5 * e[17] * e2[15] + 0.5 * e3[17] - 0.5 * e[17] * e2[10] + + e[14] * e[13] * e[16] + e[14] * e[12] * e[15] + 0.5 * e2[14] * e[17] + + e[11] * e[10] * e[16] - 0.5 * e[17] * e2[12] + 0.5 * e2[11] * e[17] + + e[11] * e[9] * e[15]; + a[122] = e[4] * e[27] * e[30] + e[4] * e[29] * e[32] + e[4] * e[28] * e[31] + + e[31] * e[27] * e[3] + e[31] * e[0] * e[30] + e[31] * e[29] * e[5] + + e[31] * e[2] * e[32] + e[7] * e[27] * e[33] + e[7] * e[29] * e[35] + + e[7] * e[28] * e[34] + e[28] * e[27] * e[0] + e[28] * e[29] * e[2] + + e[34] * e[27] * e[6] + e[34] * e[0] * e[33] + e[34] * e[29] * e[8] + + e[34] * e[2] * e[35] - e[28] * e[32] * e[5] - e[28] * e[33] * e[6] - + e[28] * e[30] * e[3] - e[28] * e[35] * e[8] + 0.5 * e[1] * e2[27] + + 0.5 * e[1] * e2[29] + 1.5 * e[1] * e2[28] + 0.5 * e[1] * e2[31] - + 0.5 * e[1] * e2[32] - 0.5 * e[1] * e2[33] - 0.5 * e[1] * e2[30] + + 0.5 * e[1] * e2[34] - 0.5 * e[1] * e2[35]; + a[79] = 0.5 * e2[11] * e[35] + 0.5 * e[35] * e2[16] - 0.5 * e[35] * e2[9] - + 0.5 * e[35] * e2[12] + 0.5 * e[35] * e2[15] + 1.5 * e[35] * e2[17] - + 0.5 * e[35] * e2[10] + 0.5 * e[35] * e2[14] - 0.5 * e[35] * e2[13] + + e[11] * e[27] * e[15] + e[11] * e[9] * e[33] + e[11] * e[29] * e[17] + + e[11] * e[28] * e[16] + e[11] * e[10] * e[34] + e[29] * e[9] * e[15] + + e[29] * e[10] * e[16] + e[14] * e[30] * e[15] + + e[14] * e[12] * e[33] + e[14] * e[32] * e[17] + + e[14] * e[31] * e[16] + e[14] * e[13] * e[34] + + e[32] * e[12] * e[15] + e[32] * e[13] * e[16] + + e[17] * e[33] * e[15] + e[17] * e[34] * e[16] - e[17] * e[27] * e[9] - + e[17] * e[30] * e[12] - e[17] * e[28] * e[10] - e[17] * e[31] * e[13]; + a[192] = e[34] * e[27] * e[33] + e[34] * e[29] * e[35] - + 0.5 * e[28] * e2[30] - 0.5 * e[28] * e2[35] + 0.5 * e3[28] + + 0.5 * e[28] * e2[27] + 0.5 * e[28] * e2[29] + e[31] * e[27] * e[30] + + e[31] * e[29] * e[32] - 0.5 * e[28] * e2[32] - 0.5 * e[28] * e2[33] + + 0.5 * e[28] * e2[31] + 0.5 * e[28] * e2[34]; + a[9] = 0.5 * e2[5] * e[8] + e[2] * e[0] * e[6] + 0.5 * e2[2] * e[8] + + 0.5 * e3[8] - 0.5 * e[8] * e2[0] + e[5] * e[4] * e[7] + + e[5] * e[3] * e[6] + 0.5 * e[8] * e2[7] + e[2] * e[1] * e[7] - + 0.5 * e[8] * e2[1] - 0.5 * e[8] * e2[4] - 0.5 * e[8] * e2[3] + + 0.5 * e[8] * e2[6]; + a[152] = + e[28] * e[27] * e[9] + e[28] * e[29] * e[11] - e[28] * e[30] * e[12] + + e[28] * e[31] * e[13] - e[28] * e[32] * e[14] - e[28] * e[33] * e[15] - + e[28] * e[35] * e[17] + e[31] * e[27] * e[12] + e[31] * e[9] * e[30] + + e[31] * e[29] * e[14] + e[31] * e[11] * e[32] + e[13] * e[27] * e[30] + + e[13] * e[29] * e[32] + e[16] * e[27] * e[33] + e[16] * e[29] * e[35] + + e[34] * e[27] * e[15] + e[34] * e[9] * e[33] + e[34] * e[29] * e[17] + + e[34] * e[11] * e[35] + e[34] * e[28] * e[16] + 0.5 * e[10] * e2[27] + + 0.5 * e[10] * e2[29] + 1.5 * e[10] * e2[28] - 0.5 * e[10] * e2[32] + + 0.5 * e[10] * e2[31] - 0.5 * e[10] * e2[33] - 0.5 * e[10] * e2[30] + + 0.5 * e[10] * e2[34] - 0.5 * e[10] * e2[35]; + a[59] = -0.5 * e[35] * e2[1] + 0.5 * e[35] * e2[7] - 0.5 * e[35] * e2[3] + + 0.5 * e2[2] * e[35] + 1.5 * e[35] * e2[8] - 0.5 * e[35] * e2[4] - + 0.5 * e[35] * e2[0] + 0.5 * e[35] * e2[6] + 0.5 * e[35] * e2[5] + + e[2] * e[27] * e[6] + e[2] * e[0] * e[33] + e[2] * e[28] * e[7] + + e[2] * e[1] * e[34] + e[2] * e[29] * e[8] - e[8] * e[27] * e[0] + + e[8] * e[34] * e[7] + e[8] * e[32] * e[5] + e[8] * e[33] * e[6] - + e[8] * e[30] * e[3] - e[8] * e[28] * e[1] - e[8] * e[31] * e[4] + + e[29] * e[1] * e[7] + e[29] * e[0] * e[6] + e[5] * e[30] * e[6] + + e[5] * e[3] * e[33] + e[5] * e[31] * e[7] + e[5] * e[4] * e[34] + + e[32] * e[4] * e[7] + e[32] * e[3] * e[6]; + a[182] = + e[28] * e[27] * e[18] + e[28] * e[29] * e[20] + e[22] * e[27] * e[30] + + e[22] * e[29] * e[32] + e[22] * e[28] * e[31] + e[31] * e[27] * e[21] + + e[31] * e[18] * e[30] + e[31] * e[29] * e[23] + e[31] * e[20] * e[32] + + e[25] * e[27] * e[33] + e[25] * e[29] * e[35] + e[25] * e[28] * e[34] + + e[34] * e[27] * e[24] + e[34] * e[18] * e[33] + e[34] * e[29] * e[26] + + e[34] * e[20] * e[35] - e[28] * e[33] * e[24] - e[28] * e[30] * e[21] - + e[28] * e[35] * e[26] - e[28] * e[32] * e[23] - 0.5 * e[19] * e2[33] - + 0.5 * e[19] * e2[30] - 0.5 * e[19] * e2[35] + 0.5 * e[19] * e2[27] + + 0.5 * e[19] * e2[29] + 1.5 * e[19] * e2[28] + 0.5 * e[19] * e2[31] + + 0.5 * e[19] * e2[34] - 0.5 * e[19] * e2[32]; + a[89] = e[23] * e[3] * e[15] - e[17] * e[19] * e[1] - e[17] * e[22] * e[4] - + e[17] * e[18] * e[0] + e[17] * e[25] * e[7] + e[17] * e[24] * e[6] + + e[14] * e[21] * e[6] + e[14] * e[3] * e[24] + e[14] * e[22] * e[7] + + e[14] * e[4] * e[25] + e[14] * e[23] * e[8] - e[26] * e[10] * e[1] - + e[26] * e[13] * e[4] + e[26] * e[16] * e[7] + e[26] * e[15] * e[6] - + e[26] * e[9] * e[0] - e[26] * e[12] * e[3] + e[23] * e[12] * e[6] + + e[11] * e[18] * e[6] + e[11] * e[0] * e[24] + e[11] * e[19] * e[7] + + e[11] * e[1] * e[25] + e[11] * e[20] * e[8] + e[11] * e[2] * e[26] + + e[20] * e[9] * e[6] + e[20] * e[0] * e[15] + e[20] * e[10] * e[7] + + e[20] * e[1] * e[16] + e[20] * e[2] * e[17] + e[5] * e[21] * e[15] + + e[5] * e[12] * e[24] + e[5] * e[23] * e[17] + e[5] * e[14] * e[26] + + e[5] * e[22] * e[16] + e[5] * e[13] * e[25] + e[8] * e[24] * e[15] + + 3. * e[8] * e[26] * e[17] + e[8] * e[25] * e[16] + + e[2] * e[18] * e[15] + e[2] * e[9] * e[24] + e[2] * e[19] * e[16] + + e[2] * e[10] * e[25] - e[17] * e[21] * e[3] + e[23] * e[4] * e[16] + + e[23] * e[13] * e[7] - e[8] * e[18] * e[9] - e[8] * e[21] * e[12] - + e[8] * e[19] * e[10] - e[8] * e[22] * e[13]; + a[62] = e[13] * e[18] * e[12] + e[13] * e[9] * e[21] + e[13] * e[20] * e[14] + + e[13] * e[11] * e[23] + e[13] * e[10] * e[22] + + e[22] * e[11] * e[14] + e[22] * e[9] * e[12] + e[16] * e[18] * e[15] + + e[16] * e[9] * e[24] + e[16] * e[20] * e[17] + e[16] * e[11] * e[26] + + e[16] * e[10] * e[25] + e[25] * e[11] * e[17] + e[25] * e[9] * e[15] - + e[10] * e[23] * e[14] - e[10] * e[24] * e[15] - + e[10] * e[26] * e[17] + e[10] * e[20] * e[11] + e[10] * e[18] * e[9] - + e[10] * e[21] * e[12] + 0.5 * e[19] * e2[11] + 0.5 * e[19] * e2[9] + + 1.5 * e[19] * e2[10] + 0.5 * e[19] * e2[13] + 0.5 * e[19] * e2[16] - + 0.5 * e[19] * e2[12] - 0.5 * e[19] * e2[15] - 0.5 * e[19] * e2[17] - + 0.5 * e[19] * e2[14]; + a[88] = e[10] * e[18] * e[6] + e[10] * e[0] * e[24] + e[10] * e[19] * e[7] + + e[10] * e[1] * e[25] + e[10] * e[20] * e[8] + e[10] * e[2] * e[26] + + e[19] * e[9] * e[6] + e[19] * e[0] * e[15] + e[19] * e[1] * e[16] + + e[19] * e[11] * e[8] + e[19] * e[2] * e[17] + e[4] * e[21] * e[15] + + e[4] * e[12] * e[24] + e[4] * e[23] * e[17] + e[4] * e[14] * e[26] + + e[4] * e[22] * e[16] + e[4] * e[13] * e[25] + e[7] * e[24] * e[15] + + e[7] * e[26] * e[17] + 3. * e[7] * e[25] * e[16] + + e[1] * e[18] * e[15] + e[1] * e[9] * e[24] + e[1] * e[20] * e[17] + + e[1] * e[11] * e[26] - e[16] * e[21] * e[3] + e[16] * e[26] * e[8] - + e[16] * e[20] * e[2] - e[16] * e[18] * e[0] - e[16] * e[23] * e[5] + + e[16] * e[24] * e[6] + e[13] * e[21] * e[6] + e[13] * e[3] * e[24] + + e[13] * e[22] * e[7] + e[13] * e[23] * e[8] + e[13] * e[5] * e[26] - + e[25] * e[11] * e[2] + e[25] * e[15] * e[6] - e[25] * e[9] * e[0] - + e[25] * e[14] * e[5] - e[25] * e[12] * e[3] + e[25] * e[17] * e[8] + + e[22] * e[12] * e[6] + e[22] * e[3] * e[15] + e[22] * e[14] * e[8] + + e[22] * e[5] * e[17] - e[7] * e[23] * e[14] - e[7] * e[20] * e[11] - + e[7] * e[18] * e[9] - e[7] * e[21] * e[12]; + a[32] = e[13] * e[9] * e[3] + e[13] * e[0] * e[12] + e[13] * e[10] * e[4] + + e[13] * e[11] * e[5] + e[13] * e[2] * e[14] + e[16] * e[9] * e[6] + + e[16] * e[0] * e[15] + e[16] * e[10] * e[7] + e[16] * e[11] * e[8] + + e[16] * e[2] * e[17] + e[7] * e[11] * e[17] + e[7] * e[9] * e[15] + + e[4] * e[11] * e[14] + e[4] * e[9] * e[12] + e[10] * e[9] * e[0] + + e[10] * e[11] * e[2] - e[10] * e[15] * e[6] - e[10] * e[14] * e[5] - + e[10] * e[12] * e[3] - e[10] * e[17] * e[8] + 0.5 * e[1] * e2[11] + + 0.5 * e[1] * e2[9] + 1.5 * e[1] * e2[10] - 0.5 * e[1] * e2[12] - + 0.5 * e[1] * e2[15] - 0.5 * e[1] * e2[17] - 0.5 * e[1] * e2[14] + + 0.5 * e[1] * e2[13] + 0.5 * e[1] * e2[16]; + a[58] = e[1] * e[27] * e[6] + e[1] * e[0] * e[33] + e[1] * e[28] * e[7] + + e[1] * e[29] * e[8] + e[1] * e[2] * e[35] - e[7] * e[27] * e[0] - + e[7] * e[32] * e[5] + e[7] * e[33] * e[6] - e[7] * e[30] * e[3] + + e[7] * e[35] * e[8] - e[7] * e[29] * e[2] + e[7] * e[31] * e[4] + + e[28] * e[0] * e[6] + e[28] * e[2] * e[8] + e[4] * e[30] * e[6] + + e[4] * e[3] * e[33] + e[4] * e[32] * e[8] + e[4] * e[5] * e[35] + + e[31] * e[3] * e[6] + e[31] * e[5] * e[8] + 0.5 * e2[1] * e[34] + + 1.5 * e[34] * e2[7] + 0.5 * e[34] * e2[4] - 0.5 * e[34] * e2[0] + + 0.5 * e[34] * e2[6] - 0.5 * e[34] * e2[5] - 0.5 * e[34] * e2[3] - + 0.5 * e[34] * e2[2] + 0.5 * e[34] * e2[8]; + a[42] = e[4] * e[18] * e[3] + e[4] * e[0] * e[21] + e[4] * e[1] * e[22] + + e[4] * e[20] * e[5] + e[4] * e[2] * e[23] + e[22] * e[0] * e[3] + + e[22] * e[2] * e[5] + e[7] * e[18] * e[6] + e[7] * e[0] * e[24] + + e[7] * e[1] * e[25] + e[7] * e[20] * e[8] + e[7] * e[2] * e[26] + + e[25] * e[0] * e[6] + e[25] * e[2] * e[8] + e[1] * e[18] * e[0] + + e[1] * e[20] * e[2] - e[1] * e[21] * e[3] - e[1] * e[26] * e[8] - + e[1] * e[23] * e[5] - e[1] * e[24] * e[6] + 0.5 * e[19] * e2[4] + + 0.5 * e[19] * e2[0] - 0.5 * e[19] * e2[6] - 0.5 * e[19] * e2[5] + + 1.5 * e[19] * e2[1] + 0.5 * e[19] * e2[7] - 0.5 * e[19] * e2[3] + + 0.5 * e[19] * e2[2] - 0.5 * e[19] * e2[8]; + a[8] = -0.5 * e[7] * e2[0] + e[4] * e[5] * e[8] + 0.5 * e2[4] * e[7] - + 0.5 * e[7] * e2[2] + 0.5 * e[7] * e2[8] - 0.5 * e[7] * e2[5] + + 0.5 * e[7] * e2[6] + e[1] * e[0] * e[6] + 0.5 * e3[7] + + e[4] * e[3] * e[6] + e[1] * e[2] * e[8] - 0.5 * e[7] * e2[3] + + 0.5 * e2[1] * e[7]; + a[112] = -e[1] * e[32] * e[23] - e[19] * e[32] * e[5] - e[19] * e[33] * e[6] - + e[19] * e[30] * e[3] - e[19] * e[35] * e[8] - e[28] * e[21] * e[3] - + e[28] * e[26] * e[8] - e[28] * e[23] * e[5] - e[28] * e[24] * e[6] + + e[7] * e[27] * e[24] + e[7] * e[18] * e[33] + e[7] * e[29] * e[26] + + e[7] * e[20] * e[35] + e[22] * e[27] * e[3] + e[22] * e[0] * e[30] + + e[22] * e[29] * e[5] + e[22] * e[2] * e[32] + e[31] * e[18] * e[3] + + e[31] * e[0] * e[21] + e[31] * e[20] * e[5] + e[31] * e[2] * e[23] + + e[25] * e[27] * e[6] + e[25] * e[0] * e[33] + e[25] * e[28] * e[7] + + e[25] * e[1] * e[34] + e[25] * e[29] * e[8] + e[25] * e[2] * e[35] + + e[34] * e[18] * e[6] + e[34] * e[0] * e[24] + e[34] * e[19] * e[7] + + e[34] * e[20] * e[8] + e[34] * e[2] * e[26] + e[1] * e[27] * e[18] + + 3. * e[1] * e[28] * e[19] + e[1] * e[29] * e[20] + + e[19] * e[27] * e[0] + e[19] * e[29] * e[2] + e[28] * e[18] * e[0] + + e[28] * e[20] * e[2] + e[4] * e[27] * e[21] + e[4] * e[18] * e[30] + + e[4] * e[28] * e[22] + e[4] * e[19] * e[31] + e[4] * e[29] * e[23] + + e[4] * e[20] * e[32] - e[1] * e[33] * e[24] - e[1] * e[30] * e[21] - + e[1] * e[35] * e[26] + e[1] * e[31] * e[22]; + a[78] = + e[10] * e[27] * e[15] + e[10] * e[9] * e[33] + e[10] * e[29] * e[17] + + e[10] * e[11] * e[35] + e[10] * e[28] * e[16] + e[28] * e[11] * e[17] + + e[28] * e[9] * e[15] + e[13] * e[30] * e[15] + e[13] * e[12] * e[33] + + e[13] * e[32] * e[17] + e[13] * e[14] * e[35] + e[13] * e[31] * e[16] + + e[31] * e[14] * e[17] + e[31] * e[12] * e[15] + e[16] * e[33] * e[15] + + e[16] * e[35] * e[17] - e[16] * e[27] * e[9] - e[16] * e[30] * e[12] - + e[16] * e[32] * e[14] - e[16] * e[29] * e[11] + 0.5 * e2[10] * e[34] + + 1.5 * e[34] * e2[16] - 0.5 * e[34] * e2[9] - 0.5 * e[34] * e2[11] - + 0.5 * e[34] * e2[12] + 0.5 * e[34] * e2[15] + 0.5 * e[34] * e2[17] - + 0.5 * e[34] * e2[14] + 0.5 * e[34] * e2[13]; + a[162] = 0.5 * e[19] * e2[18] + 0.5 * e[19] * e2[25] + 0.5 * e[19] * e2[22] + + e[25] * e[20] * e[26] - 0.5 * e[19] * e2[21] + 0.5 * e[19] * e2[20] - + 0.5 * e[19] * e2[26] - 0.5 * e[19] * e2[23] - 0.5 * e[19] * e2[24] + + 0.5 * e3[19] + e[22] * e[20] * e[23] + e[25] * e[18] * e[24] + + e[22] * e[18] * e[21]; + a[198] = 0.5 * e[34] * e2[33] + 0.5 * e[34] * e2[35] - 0.5 * e[34] * e2[27] - + 0.5 * e[34] * e2[32] - 0.5 * e[34] * e2[29] - 0.5 * e[34] * e2[30] + + 0.5 * e2[28] * e[34] + e[31] * e[30] * e[33] + + e[31] * e[32] * e[35] + e[28] * e[27] * e[33] + 0.5 * e3[34] + + e[28] * e[29] * e[35] + 0.5 * e2[31] * e[34]; + a[92] = e[4] * e[28] * e[13] + e[4] * e[10] * e[31] + e[7] * e[27] * e[15] + + e[7] * e[9] * e[33] + e[7] * e[29] * e[17] + e[7] * e[11] * e[35] + + e[7] * e[28] * e[16] + e[7] * e[10] * e[34] + e[1] * e[27] * e[9] + + e[1] * e[29] * e[11] + 3. * e[1] * e[28] * e[10] + + e[10] * e[27] * e[0] - e[10] * e[32] * e[5] - e[10] * e[33] * e[6] - + e[10] * e[30] * e[3] - e[10] * e[35] * e[8] + e[10] * e[29] * e[2] + + e[13] * e[27] * e[3] + e[13] * e[0] * e[30] + e[13] * e[1] * e[31] + + e[13] * e[29] * e[5] + e[13] * e[2] * e[32] + e[28] * e[11] * e[2] - + e[28] * e[15] * e[6] + e[28] * e[9] * e[0] - e[28] * e[14] * e[5] - + e[28] * e[12] * e[3] - e[28] * e[17] * e[8] + e[31] * e[9] * e[3] + + e[31] * e[0] * e[12] + e[31] * e[11] * e[5] + e[31] * e[2] * e[14] + + e[16] * e[27] * e[6] + e[16] * e[0] * e[33] + e[16] * e[1] * e[34] + + e[16] * e[29] * e[8] + e[16] * e[2] * e[35] - e[1] * e[30] * e[12] - + e[1] * e[32] * e[14] - e[1] * e[33] * e[15] - e[1] * e[35] * e[17] + + e[34] * e[9] * e[6] + e[34] * e[0] * e[15] + e[34] * e[11] * e[8] + + e[34] * e[2] * e[17] + e[4] * e[27] * e[12] + e[4] * e[9] * e[30] + + e[4] * e[29] * e[14] + e[4] * e[11] * e[32]; + a[128] = e[4] * e[30] * e[33] + e[4] * e[32] * e[35] + e[4] * e[31] * e[34] + + e[31] * e[30] * e[6] + e[31] * e[3] * e[33] + e[31] * e[32] * e[8] + + e[31] * e[5] * e[35] + e[28] * e[27] * e[6] + e[28] * e[0] * e[33] + + e[28] * e[29] * e[8] + e[28] * e[2] * e[35] + e[34] * e[33] * e[6] + + e[34] * e[35] * e[8] - e[34] * e[27] * e[0] - e[34] * e[32] * e[5] - + e[34] * e[30] * e[3] - e[34] * e[29] * e[2] + e[1] * e[27] * e[33] + + e[1] * e[29] * e[35] + e[1] * e[28] * e[34] + 0.5 * e2[31] * e[7] - + 0.5 * e[7] * e2[27] - 0.5 * e[7] * e2[32] + 0.5 * e[7] * e2[28] - + 0.5 * e[7] * e2[29] + 0.5 * e[7] * e2[33] - 0.5 * e[7] * e2[30] + + 1.5 * e[7] * e2[34] + 0.5 * e[7] * e2[35]; + a[12] = -0.5 * e[10] * e2[14] - 0.5 * e[10] * e2[17] - 0.5 * e[10] * e2[15] + + e[13] * e[11] * e[14] + e[16] * e[11] * e[17] + 0.5 * e[10] * e2[13] + + e[13] * e[9] * e[12] - 0.5 * e[10] * e2[12] + 0.5 * e3[10] + + e[16] * e[9] * e[15] + 0.5 * e[10] * e2[16] + 0.5 * e[10] * e2[11] + + 0.5 * e[10] * e2[9]; + a[188] = + e[22] * e[32] * e[35] + e[22] * e[31] * e[34] + e[31] * e[30] * e[24] + + e[31] * e[21] * e[33] + e[31] * e[32] * e[26] + e[31] * e[23] * e[35] + + e[34] * e[33] * e[24] + e[34] * e[35] * e[26] - e[34] * e[27] * e[18] - + e[34] * e[30] * e[21] - e[34] * e[29] * e[20] - e[34] * e[32] * e[23] + + e[19] * e[27] * e[33] + e[19] * e[29] * e[35] + e[19] * e[28] * e[34] + + e[28] * e[27] * e[24] + e[28] * e[18] * e[33] + e[28] * e[29] * e[26] + + e[28] * e[20] * e[35] + e[22] * e[30] * e[33] + 0.5 * e2[28] * e[25] + + 0.5 * e2[31] * e[25] + 0.5 * e[25] * e2[33] + 0.5 * e[25] * e2[35] + + 1.5 * e[25] * e2[34] - 0.5 * e[25] * e2[27] - 0.5 * e[25] * e2[32] - + 0.5 * e[25] * e2[29] - 0.5 * e[25] * e2[30]; + a[172] = + -e[19] * e[35] * e[26] - e[19] * e[32] * e[23] + e[19] * e[27] * e[18] + + e[19] * e[29] * e[20] + e[22] * e[27] * e[21] + e[22] * e[18] * e[30] + + e[22] * e[19] * e[31] + e[22] * e[29] * e[23] + e[22] * e[20] * e[32] + + e[31] * e[18] * e[21] + e[31] * e[20] * e[23] + e[25] * e[27] * e[24] + + e[25] * e[18] * e[33] + e[25] * e[19] * e[34] + e[25] * e[29] * e[26] + + e[25] * e[20] * e[35] + e[34] * e[18] * e[24] + e[34] * e[20] * e[26] - + e[19] * e[33] * e[24] - e[19] * e[30] * e[21] + 1.5 * e[28] * e2[19] + + 0.5 * e[28] * e2[18] + 0.5 * e[28] * e2[20] + 0.5 * e[28] * e2[22] + + 0.5 * e[28] * e2[25] - 0.5 * e[28] * e2[26] - 0.5 * e[28] * e2[23] - + 0.5 * e[28] * e2[24] - 0.5 * e[28] * e2[21]; + a[158] = + e[10] * e[27] * e[33] + e[10] * e[29] * e[35] + e[10] * e[28] * e[34] + + e[34] * e[33] * e[15] + e[34] * e[35] * e[17] + e[28] * e[27] * e[15] + + e[28] * e[9] * e[33] + e[28] * e[29] * e[17] + e[28] * e[11] * e[35] - + e[34] * e[27] * e[9] - e[34] * e[30] * e[12] + e[34] * e[31] * e[13] - + e[34] * e[32] * e[14] - e[34] * e[29] * e[11] + e[31] * e[30] * e[15] + + e[31] * e[12] * e[33] + e[31] * e[32] * e[17] + e[31] * e[14] * e[35] + + e[13] * e[30] * e[33] + e[13] * e[32] * e[35] - 0.5 * e[16] * e2[27] - + 0.5 * e[16] * e2[32] + 0.5 * e[16] * e2[28] - 0.5 * e[16] * e2[29] + + 0.5 * e[16] * e2[31] + 0.5 * e[16] * e2[33] - 0.5 * e[16] * e2[30] + + 1.5 * e[16] * e2[34] + 0.5 * e[16] * e2[35]; + a[153] = + e[29] * e[32] * e[14] - e[29] * e[33] * e[15] - e[29] * e[34] * e[16] + + e[32] * e[27] * e[12] + e[32] * e[9] * e[30] + e[32] * e[28] * e[13] + + e[32] * e[10] * e[31] + e[14] * e[27] * e[30] + e[14] * e[28] * e[31] + + e[17] * e[27] * e[33] + e[17] * e[28] * e[34] + e[35] * e[27] * e[15] + + e[35] * e[9] * e[33] + e[35] * e[29] * e[17] + e[35] * e[28] * e[16] + + e[35] * e[10] * e[34] + e[29] * e[27] * e[9] + e[29] * e[28] * e[10] - + e[29] * e[30] * e[12] - e[29] * e[31] * e[13] + 0.5 * e[11] * e2[27] + + 1.5 * e[11] * e2[29] + 0.5 * e[11] * e2[28] + 0.5 * e[11] * e2[32] - + 0.5 * e[11] * e2[31] - 0.5 * e[11] * e2[33] - 0.5 * e[11] * e2[30] - + 0.5 * e[11] * e2[34] + 0.5 * e[11] * e2[35]; + a[118] = e[1] * e[20] * e[35] + e[19] * e[27] * e[6] + e[19] * e[0] * e[33] + + e[19] * e[28] * e[7] + e[19] * e[29] * e[8] + e[19] * e[2] * e[35] + + e[28] * e[18] * e[6] + e[28] * e[0] * e[24] + e[28] * e[20] * e[8] + + e[28] * e[2] * e[26] + e[4] * e[30] * e[24] + e[4] * e[21] * e[33] + + e[4] * e[31] * e[25] + e[4] * e[22] * e[34] + e[4] * e[32] * e[26] + + e[4] * e[23] * e[35] - e[7] * e[27] * e[18] + e[7] * e[33] * e[24] - + e[7] * e[30] * e[21] - e[7] * e[29] * e[20] + e[7] * e[35] * e[26] + + e[7] * e[31] * e[22] - e[7] * e[32] * e[23] - e[25] * e[27] * e[0] - + e[25] * e[32] * e[5] - e[25] * e[30] * e[3] - e[25] * e[29] * e[2] - + e[34] * e[21] * e[3] - e[34] * e[20] * e[2] - e[34] * e[18] * e[0] - + e[34] * e[23] * e[5] + e[22] * e[30] * e[6] + e[22] * e[3] * e[33] + + e[22] * e[32] * e[8] + e[22] * e[5] * e[35] + e[31] * e[21] * e[6] + + e[31] * e[3] * e[24] + e[31] * e[23] * e[8] + e[31] * e[5] * e[26] + + e[34] * e[26] * e[8] + e[1] * e[27] * e[24] + e[1] * e[18] * e[33] + + e[1] * e[28] * e[25] + e[1] * e[19] * e[34] + e[1] * e[29] * e[26] + + e[34] * e[24] * e[6] + e[25] * e[33] * e[6] + + 3. * e[25] * e[34] * e[7] + e[25] * e[35] * e[8]; + a[183] = + 0.5 * e[20] * e2[27] + 1.5 * e[20] * e2[29] + 0.5 * e[20] * e2[28] + + 0.5 * e[20] * e2[32] + 0.5 * e[20] * e2[35] - 0.5 * e[20] * e2[31] - + 0.5 * e[20] * e2[33] - 0.5 * e[20] * e2[30] - 0.5 * e[20] * e2[34] + + e[29] * e[27] * e[18] + e[29] * e[28] * e[19] + e[23] * e[27] * e[30] + + e[23] * e[29] * e[32] + e[23] * e[28] * e[31] + e[32] * e[27] * e[21] + + e[32] * e[18] * e[30] + e[32] * e[28] * e[22] + e[32] * e[19] * e[31] + + e[26] * e[27] * e[33] + e[26] * e[29] * e[35] + e[26] * e[28] * e[34] + + e[35] * e[27] * e[24] + e[35] * e[18] * e[33] + e[35] * e[28] * e[25] + + e[35] * e[19] * e[34] - e[29] * e[33] * e[24] - e[29] * e[30] * e[21] - + e[29] * e[31] * e[22] - e[29] * e[34] * e[25]; + a[48] = e[19] * e[1] * e[7] + e[19] * e[0] * e[6] + e[19] * e[2] * e[8] + + e[4] * e[21] * e[6] + e[4] * e[3] * e[24] + e[4] * e[22] * e[7] + + e[4] * e[23] * e[8] + e[4] * e[5] * e[26] + e[22] * e[3] * e[6] + + e[22] * e[5] * e[8] + e[7] * e[24] * e[6] + e[7] * e[26] * e[8] + + e[1] * e[18] * e[6] + e[1] * e[0] * e[24] + e[1] * e[20] * e[8] + + e[1] * e[2] * e[26] - e[7] * e[21] * e[3] - e[7] * e[20] * e[2] - + e[7] * e[18] * e[0] - e[7] * e[23] * e[5] + 0.5 * e[25] * e2[4] - + 0.5 * e[25] * e2[0] + 0.5 * e[25] * e2[6] - 0.5 * e[25] * e2[5] + + 0.5 * e[25] * e2[1] + 1.5 * e[25] * e2[7] - 0.5 * e[25] * e2[3] - + 0.5 * e[25] * e2[2] + 0.5 * e[25] * e2[8]; + a[123] = e[5] * e[27] * e[30] + e[5] * e[29] * e[32] + e[5] * e[28] * e[31] + + e[32] * e[27] * e[3] + e[32] * e[0] * e[30] + e[32] * e[28] * e[4] + + e[32] * e[1] * e[31] + e[8] * e[27] * e[33] + e[8] * e[29] * e[35] + + e[8] * e[28] * e[34] + e[29] * e[27] * e[0] + e[29] * e[28] * e[1] + + e[35] * e[27] * e[6] + e[35] * e[0] * e[33] + e[35] * e[28] * e[7] + + e[35] * e[1] * e[34] - e[29] * e[34] * e[7] - e[29] * e[33] * e[6] - + e[29] * e[30] * e[3] - e[29] * e[31] * e[4] + 0.5 * e[2] * e2[27] + + 1.5 * e[2] * e2[29] + 0.5 * e[2] * e2[28] + 0.5 * e[2] * e2[32] - + 0.5 * e[2] * e2[31] - 0.5 * e[2] * e2[33] - 0.5 * e[2] * e2[30] - + 0.5 * e[2] * e2[34] + 0.5 * e[2] * e2[35]; + a[38] = e[13] * e[12] * e[6] + e[13] * e[3] * e[15] + e[13] * e[4] * e[16] + + e[13] * e[14] * e[8] + e[13] * e[5] * e[17] + e[16] * e[15] * e[6] + + e[16] * e[17] * e[8] + e[1] * e[11] * e[17] + e[1] * e[9] * e[15] + + e[1] * e[10] * e[16] + e[4] * e[14] * e[17] + e[4] * e[12] * e[15] + + e[10] * e[9] * e[6] + e[10] * e[0] * e[15] + e[10] * e[11] * e[8] + + e[10] * e[2] * e[17] - e[16] * e[11] * e[2] - e[16] * e[9] * e[0] - + e[16] * e[14] * e[5] - e[16] * e[12] * e[3] + 0.5 * e2[13] * e[7] + + 1.5 * e2[16] * e[7] + 0.5 * e[7] * e2[17] + 0.5 * e[7] * e2[15] - + 0.5 * e[7] * e2[9] - 0.5 * e[7] * e2[11] - 0.5 * e[7] * e2[12] + + 0.5 * e[7] * e2[10] - 0.5 * e[7] * e2[14]; + a[193] = 0.5 * e[29] * e2[32] + 0.5 * e[29] * e2[35] - 0.5 * e[29] * e2[31] - + 0.5 * e[29] * e2[33] - 0.5 * e[29] * e2[30] - 0.5 * e[29] * e2[34] + + e[32] * e[27] * e[30] + 0.5 * e3[29] + 0.5 * e[29] * e2[28] + + e[35] * e[28] * e[34] + 0.5 * e[29] * e2[27] + + e[35] * e[27] * e[33] + e[32] * e[28] * e[31]; + a[68] = + -e[16] * e[21] * e[12] + e[10] * e[18] * e[15] + e[10] * e[9] * e[24] + + e[10] * e[20] * e[17] + e[10] * e[11] * e[26] + e[19] * e[11] * e[17] + + e[19] * e[9] * e[15] + e[19] * e[10] * e[16] + e[13] * e[21] * e[15] + + e[13] * e[12] * e[24] + e[13] * e[23] * e[17] + e[13] * e[14] * e[26] + + e[13] * e[22] * e[16] + e[22] * e[14] * e[17] + e[22] * e[12] * e[15] + + e[16] * e[24] * e[15] + e[16] * e[26] * e[17] - e[16] * e[23] * e[14] - + e[16] * e[20] * e[11] - e[16] * e[18] * e[9] + 0.5 * e2[13] * e[25] + + 1.5 * e[25] * e2[16] + 0.5 * e[25] * e2[17] + 0.5 * e[25] * e2[15] + + 0.5 * e2[10] * e[25] - 0.5 * e[25] * e2[9] - 0.5 * e[25] * e2[11] - + 0.5 * e[25] * e2[12] - 0.5 * e[25] * e2[14]; + a[102] = e[19] * e[20] * e[2] + e[22] * e[18] * e[3] + e[22] * e[0] * e[21] + + e[22] * e[19] * e[4] + e[22] * e[20] * e[5] + e[22] * e[2] * e[23] - + e[19] * e[21] * e[3] - e[19] * e[26] * e[8] + e[19] * e[25] * e[7] - + e[19] * e[23] * e[5] - e[19] * e[24] * e[6] + e[4] * e[18] * e[21] + + e[4] * e[20] * e[23] + e[25] * e[18] * e[6] + e[25] * e[0] * e[24] + + e[25] * e[20] * e[8] + e[25] * e[2] * e[26] + e[7] * e[18] * e[24] + + e[7] * e[20] * e[26] + e[19] * e[18] * e[0] + 1.5 * e2[19] * e[1] + + 0.5 * e[1] * e2[22] + 0.5 * e[1] * e2[18] + 0.5 * e[1] * e2[20] + + 0.5 * e[1] * e2[25] - 0.5 * e[1] * e2[26] - 0.5 * e[1] * e2[23] - + 0.5 * e[1] * e2[24] - 0.5 * e[1] * e2[21]; + a[178] = + e[19] * e[27] * e[24] + e[19] * e[18] * e[33] + e[19] * e[28] * e[25] + + e[19] * e[29] * e[26] + e[19] * e[20] * e[35] + e[28] * e[18] * e[24] + + e[28] * e[20] * e[26] + e[22] * e[30] * e[24] + e[22] * e[21] * e[33] + + e[22] * e[31] * e[25] + e[22] * e[32] * e[26] + e[22] * e[23] * e[35] + + e[31] * e[21] * e[24] + e[31] * e[23] * e[26] + e[25] * e[33] * e[24] + + e[25] * e[35] * e[26] - e[25] * e[27] * e[18] - e[25] * e[30] * e[21] - + e[25] * e[29] * e[20] - e[25] * e[32] * e[23] - 0.5 * e[34] * e2[18] - + 0.5 * e[34] * e2[23] - 0.5 * e[34] * e2[20] - 0.5 * e[34] * e2[21] + + 0.5 * e2[19] * e[34] + 0.5 * e2[22] * e[34] + 1.5 * e[34] * e2[25] + + 0.5 * e[34] * e2[24] + 0.5 * e[34] * e2[26]; + a[22] = e[16] * e[0] * e[6] + e[16] * e[2] * e[8] + e[1] * e[11] * e[2] - + e[1] * e[15] * e[6] + e[1] * e[9] * e[0] - e[1] * e[14] * e[5] - + e[1] * e[12] * e[3] - e[1] * e[17] * e[8] + e[4] * e[9] * e[3] + + e[4] * e[0] * e[12] + e[4] * e[1] * e[13] + e[4] * e[11] * e[5] + + e[4] * e[2] * e[14] + e[13] * e[0] * e[3] + e[13] * e[2] * e[5] + + e[7] * e[9] * e[6] + e[7] * e[0] * e[15] + e[7] * e[1] * e[16] + + e[7] * e[11] * e[8] + e[7] * e[2] * e[17] - 0.5 * e[10] * e2[6] - + 0.5 * e[10] * e2[5] - 0.5 * e[10] * e2[3] - 0.5 * e[10] * e2[8] + + 1.5 * e[10] * e2[1] + 0.5 * e[10] * e2[0] + 0.5 * e[10] * e2[2] + + 0.5 * e[10] * e2[4] + 0.5 * e[10] * e2[7]; + a[18] = e[13] * e[14] * e[17] + e[13] * e[12] * e[15] + e[10] * e[9] * e[15] + + 0.5 * e[16] * e2[15] - 0.5 * e[16] * e2[11] - 0.5 * e[16] * e2[12] - + 0.5 * e[16] * e2[14] + e[10] * e[11] * e[17] + 0.5 * e2[10] * e[16] + + 0.5 * e3[16] - 0.5 * e[16] * e2[9] + 0.5 * e[16] * e2[17] + + 0.5 * e2[13] * e[16]; + a[142] = + e[10] * e[29] * e[20] + e[22] * e[27] * e[12] + e[22] * e[9] * e[30] + + e[22] * e[29] * e[14] + e[22] * e[11] * e[32] + e[22] * e[10] * e[31] + + e[31] * e[18] * e[12] + e[31] * e[9] * e[21] + e[31] * e[20] * e[14] + + e[31] * e[11] * e[23] - e[10] * e[33] * e[24] - e[10] * e[30] * e[21] - + e[10] * e[35] * e[26] - e[10] * e[32] * e[23] + e[10] * e[34] * e[25] + + e[19] * e[27] * e[9] + e[19] * e[29] * e[11] + e[28] * e[18] * e[9] + + e[28] * e[20] * e[11] + e[16] * e[27] * e[24] + e[16] * e[18] * e[33] + + e[16] * e[28] * e[25] + e[16] * e[19] * e[34] + e[16] * e[29] * e[26] + + e[16] * e[20] * e[35] - e[19] * e[30] * e[12] - e[19] * e[32] * e[14] - + e[19] * e[33] * e[15] - e[19] * e[35] * e[17] - e[28] * e[23] * e[14] - + e[28] * e[24] * e[15] - e[28] * e[26] * e[17] - e[28] * e[21] * e[12] + + e[25] * e[27] * e[15] + e[25] * e[9] * e[33] + e[25] * e[29] * e[17] + + e[25] * e[11] * e[35] + e[34] * e[18] * e[15] + e[34] * e[9] * e[24] + + e[34] * e[20] * e[17] + e[34] * e[11] * e[26] + e[13] * e[27] * e[21] + + e[13] * e[18] * e[30] + e[13] * e[28] * e[22] + e[13] * e[19] * e[31] + + e[13] * e[29] * e[23] + e[13] * e[20] * e[32] + e[10] * e[27] * e[18] + + 3. * e[10] * e[28] * e[19]; + a[98] = e[4] * e[30] * e[15] + e[4] * e[12] * e[33] + e[4] * e[32] * e[17] + + e[4] * e[14] * e[35] + e[4] * e[31] * e[16] + e[4] * e[13] * e[34] + + e[7] * e[33] * e[15] + e[7] * e[35] * e[17] + + 3. * e[7] * e[34] * e[16] + e[1] * e[27] * e[15] + + e[1] * e[9] * e[33] + e[1] * e[29] * e[17] + e[1] * e[11] * e[35] + + e[1] * e[28] * e[16] + e[1] * e[10] * e[34] - e[16] * e[27] * e[0] - + e[16] * e[32] * e[5] + e[16] * e[33] * e[6] - e[16] * e[30] * e[3] + + e[16] * e[35] * e[8] - e[16] * e[29] * e[2] + e[13] * e[30] * e[6] + + e[13] * e[3] * e[33] + e[13] * e[31] * e[7] + e[13] * e[32] * e[8] + + e[13] * e[5] * e[35] - e[34] * e[11] * e[2] + e[34] * e[15] * e[6] - + e[34] * e[9] * e[0] - e[34] * e[14] * e[5] - e[34] * e[12] * e[3] + + e[34] * e[17] * e[8] + e[31] * e[12] * e[6] + e[31] * e[3] * e[15] + + e[31] * e[14] * e[8] + e[31] * e[5] * e[17] - e[7] * e[27] * e[9] - + e[7] * e[30] * e[12] + e[7] * e[28] * e[10] - e[7] * e[32] * e[14] + + e[10] * e[27] * e[6] + e[10] * e[0] * e[33] + e[10] * e[29] * e[8] + + e[10] * e[2] * e[35] + e[28] * e[9] * e[6] + e[28] * e[0] * e[15] + + e[28] * e[11] * e[8] + e[28] * e[2] * e[17] - e[7] * e[29] * e[11]; + a[132] = + e[22] * e[18] * e[12] + e[22] * e[9] * e[21] + e[22] * e[20] * e[14] + + e[22] * e[11] * e[23] + e[22] * e[19] * e[13] + e[25] * e[18] * e[15] + + e[25] * e[9] * e[24] + e[25] * e[20] * e[17] + e[25] * e[11] * e[26] + + e[25] * e[19] * e[16] + e[16] * e[18] * e[24] + e[16] * e[20] * e[26] + + e[13] * e[18] * e[21] + e[13] * e[20] * e[23] + e[19] * e[18] * e[9] + + e[19] * e[20] * e[11] - e[19] * e[23] * e[14] - e[19] * e[24] * e[15] - + e[19] * e[26] * e[17] - e[19] * e[21] * e[12] + 0.5 * e[10] * e2[22] + + 0.5 * e[10] * e2[25] + 1.5 * e[10] * e2[19] + 0.5 * e[10] * e2[18] + + 0.5 * e[10] * e2[20] - 0.5 * e[10] * e2[26] - 0.5 * e[10] * e2[23] - + 0.5 * e[10] * e2[24] - 0.5 * e[10] * e2[21]; + a[168] = e[19] * e[20] * e[26] - 0.5 * e[25] * e2[20] + + e[22] * e[21] * e[24] + e[19] * e[18] * e[24] + + 0.5 * e2[22] * e[25] - 0.5 * e[25] * e2[21] - 0.5 * e[25] * e2[23] + + 0.5 * e2[19] * e[25] - 0.5 * e[25] * e2[18] + 0.5 * e[25] * e2[24] + + 0.5 * e[25] * e2[26] + 0.5 * e3[25] + e[22] * e[23] * e[26]; + a[113] = -e[20] * e[33] * e[6] - e[20] * e[30] * e[3] - e[20] * e[31] * e[4] - + e[29] * e[21] * e[3] - e[29] * e[22] * e[4] - e[29] * e[25] * e[7] - + e[29] * e[24] * e[6] + e[8] * e[27] * e[24] + e[8] * e[18] * e[33] + + e[8] * e[28] * e[25] + e[8] * e[19] * e[34] + e[23] * e[27] * e[3] + + e[23] * e[0] * e[30] + e[23] * e[28] * e[4] + e[23] * e[1] * e[31] + + e[32] * e[18] * e[3] + e[32] * e[0] * e[21] + e[32] * e[19] * e[4] + + e[32] * e[1] * e[22] + e[26] * e[27] * e[6] + e[26] * e[0] * e[33] + + e[26] * e[28] * e[7] + e[26] * e[1] * e[34] + e[26] * e[29] * e[8] + + e[26] * e[2] * e[35] + e[35] * e[18] * e[6] + e[35] * e[0] * e[24] + + e[35] * e[19] * e[7] + e[35] * e[1] * e[25] + e[35] * e[20] * e[8] + + e[2] * e[27] * e[18] + e[2] * e[28] * e[19] + + 3. * e[2] * e[29] * e[20] + e[20] * e[27] * e[0] + + e[20] * e[28] * e[1] + e[29] * e[18] * e[0] + e[29] * e[19] * e[1] + + e[5] * e[27] * e[21] + e[5] * e[18] * e[30] + e[5] * e[28] * e[22] + + e[5] * e[19] * e[31] + e[5] * e[29] * e[23] + e[5] * e[20] * e[32] - + e[2] * e[33] * e[24] - e[2] * e[30] * e[21] - e[2] * e[31] * e[22] + + e[2] * e[32] * e[23] - e[2] * e[34] * e[25] - e[20] * e[34] * e[7]; + a[43] = e[5] * e[18] * e[3] + e[5] * e[0] * e[21] + e[5] * e[19] * e[4] + + e[5] * e[1] * e[22] + e[5] * e[2] * e[23] + e[23] * e[1] * e[4] + + e[23] * e[0] * e[3] + e[8] * e[18] * e[6] + e[8] * e[0] * e[24] + + e[8] * e[19] * e[7] + e[8] * e[1] * e[25] + e[8] * e[2] * e[26] + + e[26] * e[1] * e[7] + e[26] * e[0] * e[6] + e[2] * e[18] * e[0] + + e[2] * e[19] * e[1] - e[2] * e[21] * e[3] - e[2] * e[22] * e[4] - + e[2] * e[25] * e[7] - e[2] * e[24] * e[6] - 0.5 * e[20] * e2[4] + + 0.5 * e[20] * e2[0] - 0.5 * e[20] * e2[6] + 0.5 * e[20] * e2[5] + + 0.5 * e[20] * e2[1] - 0.5 * e[20] * e2[7] - 0.5 * e[20] * e2[3] + + 1.5 * e[20] * e2[2] + 0.5 * e[20] * e2[8]; + a[33] = e[14] * e[9] * e[3] + e[14] * e[0] * e[12] + e[14] * e[10] * e[4] + + e[14] * e[1] * e[13] + e[14] * e[11] * e[5] + e[17] * e[9] * e[6] + + e[17] * e[0] * e[15] + e[17] * e[10] * e[7] + e[17] * e[1] * e[16] + + e[17] * e[11] * e[8] + e[8] * e[9] * e[15] + e[8] * e[10] * e[16] + + e[5] * e[9] * e[12] + e[5] * e[10] * e[13] + e[11] * e[9] * e[0] + + e[11] * e[10] * e[1] - e[11] * e[13] * e[4] - e[11] * e[16] * e[7] - + e[11] * e[15] * e[6] - e[11] * e[12] * e[3] + 0.5 * e[2] * e2[14] + + 0.5 * e[2] * e2[17] + 1.5 * e[2] * e2[11] + 0.5 * e[2] * e2[9] + + 0.5 * e[2] * e2[10] - 0.5 * e[2] * e2[16] - 0.5 * e[2] * e2[12] - + 0.5 * e[2] * e2[15] - 0.5 * e[2] * e2[13]; + a[63] = e[14] * e[18] * e[12] + e[14] * e[9] * e[21] + e[14] * e[11] * e[23] + + e[14] * e[19] * e[13] + e[14] * e[10] * e[22] + e[23] * e[9] * e[12] + + e[23] * e[10] * e[13] + e[17] * e[18] * e[15] + e[17] * e[9] * e[24] + + e[17] * e[11] * e[26] + e[17] * e[19] * e[16] + + e[17] * e[10] * e[25] + e[26] * e[9] * e[15] + e[26] * e[10] * e[16] - + e[11] * e[24] * e[15] - e[11] * e[25] * e[16] + e[11] * e[18] * e[9] - + e[11] * e[21] * e[12] + e[11] * e[19] * e[10] - + e[11] * e[22] * e[13] + 1.5 * e[20] * e2[11] + 0.5 * e[20] * e2[9] + + 0.5 * e[20] * e2[10] + 0.5 * e[20] * e2[14] + 0.5 * e[20] * e2[17] - + 0.5 * e[20] * e2[16] - 0.5 * e[20] * e2[12] - 0.5 * e[20] * e2[15] - + 0.5 * e[20] * e2[13]; + a[143] = + e[23] * e[10] * e[31] + e[32] * e[18] * e[12] + e[32] * e[9] * e[21] + + e[32] * e[19] * e[13] + e[32] * e[10] * e[22] - e[11] * e[33] * e[24] - + e[11] * e[30] * e[21] + e[11] * e[35] * e[26] - e[11] * e[31] * e[22] - + e[11] * e[34] * e[25] + e[20] * e[27] * e[9] + e[20] * e[28] * e[10] + + e[29] * e[18] * e[9] + e[29] * e[19] * e[10] + e[17] * e[27] * e[24] + + e[17] * e[18] * e[33] + e[17] * e[28] * e[25] + e[17] * e[19] * e[34] + + e[17] * e[29] * e[26] + e[17] * e[20] * e[35] - e[20] * e[30] * e[12] - + e[20] * e[31] * e[13] - e[20] * e[33] * e[15] - e[20] * e[34] * e[16] - + e[29] * e[24] * e[15] - e[29] * e[25] * e[16] - e[29] * e[21] * e[12] - + e[29] * e[22] * e[13] + e[26] * e[27] * e[15] + e[26] * e[9] * e[33] + + e[26] * e[28] * e[16] + e[26] * e[10] * e[34] + e[35] * e[18] * e[15] + + e[35] * e[9] * e[24] + e[35] * e[19] * e[16] + e[35] * e[10] * e[25] + + e[14] * e[27] * e[21] + e[14] * e[18] * e[30] + e[14] * e[28] * e[22] + + e[14] * e[19] * e[31] + e[14] * e[29] * e[23] + e[14] * e[20] * e[32] + + e[11] * e[27] * e[18] + e[11] * e[28] * e[19] + + 3. * e[11] * e[29] * e[20] + e[23] * e[27] * e[12] + + e[23] * e[9] * e[30] + e[23] * e[11] * e[32] + e[23] * e[28] * e[13]; + a[133] = + e[23] * e[18] * e[12] + e[23] * e[9] * e[21] + e[23] * e[20] * e[14] + + e[23] * e[19] * e[13] + e[23] * e[10] * e[22] + e[26] * e[18] * e[15] + + e[26] * e[9] * e[24] + e[26] * e[20] * e[17] + e[26] * e[19] * e[16] + + e[26] * e[10] * e[25] + e[17] * e[19] * e[25] + e[17] * e[18] * e[24] + + e[14] * e[19] * e[22] + e[14] * e[18] * e[21] + e[20] * e[18] * e[9] + + e[20] * e[19] * e[10] - e[20] * e[24] * e[15] - e[20] * e[25] * e[16] - + e[20] * e[21] * e[12] - e[20] * e[22] * e[13] + 0.5 * e[11] * e2[23] + + 0.5 * e[11] * e2[26] + 0.5 * e[11] * e2[19] + 0.5 * e[11] * e2[18] + + 1.5 * e[11] * e2[20] - 0.5 * e[11] * e2[22] - 0.5 * e[11] * e2[24] - + 0.5 * e[11] * e2[21] - 0.5 * e[11] * e2[25]; + a[103] = -e[20] * e[21] * e[3] + e[20] * e[26] * e[8] - e[20] * e[22] * e[4] - + e[20] * e[25] * e[7] - e[20] * e[24] * e[6] + e[5] * e[19] * e[22] + + e[5] * e[18] * e[21] + e[26] * e[18] * e[6] + e[26] * e[0] * e[24] + + e[26] * e[19] * e[7] + e[26] * e[1] * e[25] + e[8] * e[19] * e[25] + + e[8] * e[18] * e[24] + e[20] * e[18] * e[0] + e[20] * e[19] * e[1] + + e[23] * e[18] * e[3] + e[23] * e[0] * e[21] + e[23] * e[19] * e[4] + + e[23] * e[1] * e[22] + e[23] * e[20] * e[5] + 1.5 * e2[20] * e[2] + + 0.5 * e[2] * e2[23] + 0.5 * e[2] * e2[19] + 0.5 * e[2] * e2[18] + + 0.5 * e[2] * e2[26] - 0.5 * e[2] * e2[22] - 0.5 * e[2] * e2[24] - + 0.5 * e[2] * e2[21] - 0.5 * e[2] * e2[25]; + a[23] = -e[2] * e[15] * e[6] + e[2] * e[9] * e[0] - e[2] * e[12] * e[3] + + e[5] * e[9] * e[3] + e[5] * e[0] * e[12] + e[5] * e[10] * e[4] + + e[5] * e[1] * e[13] + e[5] * e[2] * e[14] + e[14] * e[1] * e[4] + + e[14] * e[0] * e[3] + e[8] * e[9] * e[6] + e[8] * e[0] * e[15] + + e[8] * e[10] * e[7] + e[8] * e[1] * e[16] + e[8] * e[2] * e[17] + + e[17] * e[1] * e[7] + e[17] * e[0] * e[6] + e[2] * e[10] * e[1] - + e[2] * e[13] * e[4] - e[2] * e[16] * e[7] + 0.5 * e[11] * e2[1] + + 0.5 * e[11] * e2[0] + 1.5 * e[11] * e2[2] + 0.5 * e[11] * e2[5] + + 0.5 * e[11] * e2[8] - 0.5 * e[11] * e2[4] - 0.5 * e[11] * e2[6] - + 0.5 * e[11] * e2[7] - 0.5 * e[11] * e2[3]; + a[83] = e[5] * e[19] * e[13] + e[5] * e[10] * e[22] + e[8] * e[18] * e[15] + + e[8] * e[9] * e[24] + e[8] * e[20] * e[17] + e[8] * e[11] * e[26] + + e[8] * e[19] * e[16] + e[8] * e[10] * e[25] + e[2] * e[18] * e[9] + + e[2] * e[19] * e[10] - e[11] * e[21] * e[3] - e[11] * e[22] * e[4] - + e[11] * e[25] * e[7] - e[11] * e[24] * e[6] + e[14] * e[18] * e[3] + + e[14] * e[0] * e[21] + e[14] * e[19] * e[4] + e[14] * e[1] * e[22] + + e[14] * e[2] * e[23] - e[20] * e[13] * e[4] - e[20] * e[16] * e[7] - + e[20] * e[15] * e[6] - e[20] * e[12] * e[3] + e[23] * e[9] * e[3] + + e[23] * e[0] * e[12] + e[23] * e[10] * e[4] + e[23] * e[1] * e[13] + + e[17] * e[18] * e[6] + e[17] * e[0] * e[24] + e[17] * e[19] * e[7] + + e[17] * e[1] * e[25] + e[17] * e[2] * e[26] - e[2] * e[24] * e[15] - + e[2] * e[25] * e[16] - e[2] * e[21] * e[12] - e[2] * e[22] * e[13] + + e[26] * e[9] * e[6] + e[26] * e[0] * e[15] + e[26] * e[10] * e[7] + + e[26] * e[1] * e[16] + e[11] * e[18] * e[0] + e[11] * e[19] * e[1] + + 3. * e[11] * e[20] * e[2] + e[20] * e[9] * e[0] + + e[20] * e[10] * e[1] + e[5] * e[18] * e[12] + e[5] * e[9] * e[21] + + e[5] * e[20] * e[14] + e[5] * e[11] * e[23]; + a[53] = e[32] * e[1] * e[4] + e[32] * e[0] * e[3] + e[8] * e[27] * e[6] + + e[8] * e[0] * e[33] + e[8] * e[28] * e[7] + e[8] * e[1] * e[34] + + e[35] * e[1] * e[7] + e[35] * e[0] * e[6] + e[2] * e[27] * e[0] + + e[2] * e[28] * e[1] - e[2] * e[34] * e[7] + e[2] * e[32] * e[5] - + e[2] * e[33] * e[6] - e[2] * e[30] * e[3] + e[2] * e[35] * e[8] - + e[2] * e[31] * e[4] + e[5] * e[27] * e[3] + e[5] * e[0] * e[30] + + e[5] * e[28] * e[4] + e[5] * e[1] * e[31] + 1.5 * e[29] * e2[2] - + 0.5 * e[29] * e2[4] + 0.5 * e[29] * e2[0] - 0.5 * e[29] * e2[6] + + 0.5 * e[29] * e2[5] + 0.5 * e[29] * e2[1] - 0.5 * e[29] * e2[7] - + 0.5 * e[29] * e2[3] + 0.5 * e[29] * e2[8]; + a[3] = e[5] * e[0] * e[3] + e[8] * e[1] * e[7] + e[8] * e[0] * e[6] + + e[5] * e[1] * e[4] - 0.5 * e[2] * e2[4] + 0.5 * e3[2] + + 0.5 * e[2] * e2[1] - 0.5 * e[2] * e2[3] + 0.5 * e[2] * e2[0] + + 0.5 * e[2] * e2[8] + 0.5 * e[2] * e2[5] - 0.5 * e[2] * e2[6] - + 0.5 * e[2] * e2[7]; + a[73] = e[35] * e[9] * e[15] + e[35] * e[10] * e[16] - e[11] * e[30] * e[12] - + e[11] * e[31] * e[13] - e[11] * e[33] * e[15] - + e[11] * e[34] * e[16] + e[11] * e[27] * e[9] + e[11] * e[28] * e[10] + + e[14] * e[27] * e[12] + e[14] * e[9] * e[30] + e[14] * e[11] * e[32] + + e[14] * e[28] * e[13] + e[14] * e[10] * e[31] + e[32] * e[9] * e[12] + + e[32] * e[10] * e[13] + e[17] * e[27] * e[15] + e[17] * e[9] * e[33] + + e[17] * e[11] * e[35] + e[17] * e[28] * e[16] + + e[17] * e[10] * e[34] + 1.5 * e[29] * e2[11] - 0.5 * e[29] * e2[16] + + 0.5 * e[29] * e2[9] - 0.5 * e[29] * e2[12] - 0.5 * e[29] * e2[15] + + 0.5 * e[29] * e2[17] + 0.5 * e[29] * e2[10] + 0.5 * e[29] * e2[14] - + 0.5 * e[29] * e2[13]; + a[13] = e[14] * e[9] * e[12] + e[17] * e[10] * e[16] + e[17] * e[9] * e[15] + + 0.5 * e3[11] + e[14] * e[10] * e[13] + 0.5 * e[11] * e2[10] - + 0.5 * e[11] * e2[15] + 0.5 * e[11] * e2[14] - 0.5 * e[11] * e2[13] - + 0.5 * e[11] * e2[12] + 0.5 * e[11] * e2[9] - 0.5 * e[11] * e2[16] + + 0.5 * e[11] * e2[17]; + a[173] = + e[20] * e[27] * e[18] + e[20] * e[28] * e[19] + e[23] * e[27] * e[21] + + e[23] * e[18] * e[30] + e[23] * e[28] * e[22] + e[23] * e[19] * e[31] + + e[23] * e[20] * e[32] + e[32] * e[19] * e[22] + e[32] * e[18] * e[21] + + e[26] * e[27] * e[24] + e[26] * e[18] * e[33] + e[26] * e[28] * e[25] + + e[26] * e[19] * e[34] + e[26] * e[20] * e[35] + e[35] * e[19] * e[25] + + e[35] * e[18] * e[24] - e[20] * e[33] * e[24] - e[20] * e[30] * e[21] - + e[20] * e[31] * e[22] - e[20] * e[34] * e[25] + 0.5 * e[29] * e2[23] + + 0.5 * e[29] * e2[26] - 0.5 * e[29] * e2[22] - 0.5 * e[29] * e2[24] - + 0.5 * e[29] * e2[21] - 0.5 * e[29] * e2[25] + 1.5 * e[29] * e2[20] + + 0.5 * e[29] * e2[19] + 0.5 * e[29] * e2[18]; + a[163] = 0.5 * e[20] * e2[26] + 0.5 * e[20] * e2[18] + 0.5 * e3[20] + + 0.5 * e[20] * e2[19] + e[26] * e[18] * e[24] + 0.5 * e[20] * e2[23] - + 0.5 * e[20] * e2[25] + e[23] * e[19] * e[22] - 0.5 * e[20] * e2[24] - + 0.5 * e[20] * e2[21] - 0.5 * e[20] * e2[22] + e[23] * e[18] * e[21] + + e[26] * e[19] * e[25]; + a[93] = e[8] * e[28] * e[16] + e[8] * e[10] * e[34] + e[2] * e[27] * e[9] + + 3. * e[2] * e[29] * e[11] + e[2] * e[28] * e[10] + + e[11] * e[27] * e[0] - e[11] * e[34] * e[7] - e[11] * e[33] * e[6] - + e[11] * e[30] * e[3] + e[11] * e[28] * e[1] - e[11] * e[31] * e[4] + + e[14] * e[27] * e[3] + e[14] * e[0] * e[30] + e[14] * e[28] * e[4] + + e[14] * e[1] * e[31] + e[14] * e[2] * e[32] + e[29] * e[10] * e[1] - + e[29] * e[13] * e[4] - e[29] * e[16] * e[7] - e[29] * e[15] * e[6] + + e[29] * e[9] * e[0] - e[29] * e[12] * e[3] + e[32] * e[9] * e[3] + + e[32] * e[0] * e[12] + e[32] * e[10] * e[4] + e[32] * e[1] * e[13] + + e[17] * e[27] * e[6] + e[17] * e[0] * e[33] + e[17] * e[28] * e[7] + + e[17] * e[1] * e[34] + e[17] * e[2] * e[35] - e[2] * e[30] * e[12] - + e[2] * e[31] * e[13] - e[2] * e[33] * e[15] - e[2] * e[34] * e[16] + + e[35] * e[9] * e[6] + e[35] * e[0] * e[15] + e[35] * e[10] * e[7] + + e[35] * e[1] * e[16] + e[5] * e[27] * e[12] + e[5] * e[9] * e[30] + + e[5] * e[29] * e[14] + e[5] * e[11] * e[32] + e[5] * e[28] * e[13] + + e[5] * e[10] * e[31] + e[8] * e[27] * e[15] + e[8] * e[9] * e[33] + + e[8] * e[29] * e[17] + e[8] * e[11] * e[35]; + a[94] = -e[12] * e[34] * e[7] + e[12] * e[32] * e[5] - e[12] * e[35] * e[8] - + e[12] * e[29] * e[2] - e[12] * e[28] * e[1] + e[12] * e[31] * e[4] - + e[30] * e[11] * e[2] - e[30] * e[10] * e[1] + e[30] * e[13] * e[4] - + e[30] * e[16] * e[7] + e[30] * e[14] * e[5] - e[30] * e[17] * e[8] + + e[15] * e[3] * e[33] + e[15] * e[31] * e[7] + e[15] * e[4] * e[34] + + e[15] * e[32] * e[8] + e[15] * e[5] * e[35] + e[3] * e[27] * e[9] - + e[3] * e[28] * e[10] - e[3] * e[34] * e[16] - e[3] * e[35] * e[17] - + e[3] * e[29] * e[11] + e[33] * e[13] * e[7] + e[33] * e[4] * e[16] + + e[33] * e[14] * e[8] + e[33] * e[5] * e[17] + e[9] * e[28] * e[4] + + e[9] * e[1] * e[31] + e[9] * e[29] * e[5] + e[9] * e[2] * e[32] + + e[27] * e[10] * e[4] + e[27] * e[1] * e[13] + e[27] * e[11] * e[5] + + e[27] * e[2] * e[14] + 3. * e[3] * e[30] * e[12] + + e[3] * e[32] * e[14] + e[3] * e[31] * e[13] + e[6] * e[30] * e[15] + + e[6] * e[12] * e[33] + e[6] * e[32] * e[17] + e[6] * e[14] * e[35] + + e[6] * e[31] * e[16] + e[6] * e[13] * e[34] + e[0] * e[27] * e[12] + + e[0] * e[9] * e[30] + e[0] * e[29] * e[14] + e[0] * e[11] * e[32] + + e[0] * e[28] * e[13] + e[0] * e[10] * e[31]; + a[164] = 0.5 * e[21] * e2[24] - 0.5 * e[21] * e2[25] + 0.5 * e[21] * e2[23] - + 0.5 * e[21] * e2[26] + 0.5 * e2[18] * e[21] + 0.5 * e[21] * e2[22] - + 0.5 * e[21] * e2[20] + e[24] * e[22] * e[25] + + e[24] * e[23] * e[26] - 0.5 * e[21] * e2[19] + + e[18] * e[19] * e[22] + e[18] * e[20] * e[23] + 0.5 * e3[21]; + a[174] = + -0.5 * e[30] * e2[26] - 0.5 * e[30] * e2[19] - 0.5 * e[30] * e2[20] - + 0.5 * e[30] * e2[25] + 0.5 * e2[18] * e[30] + 1.5 * e[30] * e2[21] + + 0.5 * e[30] * e2[22] + 0.5 * e[30] * e2[23] + 0.5 * e[30] * e2[24] + + e[18] * e[27] * e[21] + e[18] * e[28] * e[22] + e[18] * e[19] * e[31] + + e[18] * e[29] * e[23] + e[18] * e[20] * e[32] + e[27] * e[19] * e[22] + + e[27] * e[20] * e[23] + e[21] * e[31] * e[22] + e[21] * e[32] * e[23] + + e[24] * e[21] * e[33] + e[24] * e[31] * e[25] + e[24] * e[22] * e[34] + + e[24] * e[32] * e[26] + e[24] * e[23] * e[35] + e[33] * e[22] * e[25] + + e[33] * e[23] * e[26] - e[21] * e[29] * e[20] - e[21] * e[35] * e[26] - + e[21] * e[28] * e[19] - e[21] * e[34] * e[25]; + a[14] = 0.5 * e[12] * e2[15] - 0.5 * e[12] * e2[17] + e[15] * e[13] * e[16] - + 0.5 * e[12] * e2[10] + e[15] * e[14] * e[17] - 0.5 * e[12] * e2[16] - + 0.5 * e[12] * e2[11] + e[9] * e[10] * e[13] + 0.5 * e[12] * e2[13] + + 0.5 * e2[9] * e[12] + 0.5 * e3[12] + e[9] * e[11] * e[14] + + 0.5 * e[12] * e2[14]; + a[34] = e[12] * e[13] * e[4] + e[12] * e[14] * e[5] + e[15] * e[12] * e[6] + + e[15] * e[13] * e[7] + e[15] * e[4] * e[16] + e[15] * e[14] * e[8] + + e[15] * e[5] * e[17] + e[6] * e[14] * e[17] + e[6] * e[13] * e[16] + + e[0] * e[11] * e[14] + e[0] * e[9] * e[12] + e[0] * e[10] * e[13] + + e[9] * e[10] * e[4] + e[9] * e[1] * e[13] + e[9] * e[11] * e[5] + + e[9] * e[2] * e[14] - e[12] * e[11] * e[2] - e[12] * e[10] * e[1] - + e[12] * e[16] * e[7] - e[12] * e[17] * e[8] + 1.5 * e2[12] * e[3] + + 0.5 * e[3] * e2[15] - 0.5 * e[3] * e2[16] + 0.5 * e[3] * e2[9] - + 0.5 * e[3] * e2[11] - 0.5 * e[3] * e2[17] - 0.5 * e[3] * e2[10] + + 0.5 * e[3] * e2[14] + 0.5 * e[3] * e2[13]; + a[64] = + e[18] * e[11] * e[14] + e[18] * e[9] * e[12] + e[18] * e[10] * e[13] + + e[12] * e[23] * e[14] + e[12] * e[22] * e[13] + e[15] * e[12] * e[24] + + e[15] * e[23] * e[17] + e[15] * e[14] * e[26] + e[15] * e[22] * e[16] + + e[15] * e[13] * e[25] + e[24] * e[14] * e[17] + e[24] * e[13] * e[16] - + e[12] * e[25] * e[16] - e[12] * e[26] * e[17] - e[12] * e[20] * e[11] - + e[12] * e[19] * e[10] + e[9] * e[20] * e[14] + e[9] * e[11] * e[23] + + e[9] * e[19] * e[13] + e[9] * e[10] * e[22] + 0.5 * e2[9] * e[21] - + 0.5 * e[21] * e2[16] - 0.5 * e[21] * e2[11] - 0.5 * e[21] * e2[17] - + 0.5 * e[21] * e2[10] + 1.5 * e[21] * e2[12] + 0.5 * e[21] * e2[14] + + 0.5 * e[21] * e2[13] + 0.5 * e[21] * e2[15]; + a[114] = -e[21] * e[35] * e[8] - e[21] * e[29] * e[2] - e[21] * e[28] * e[1] + + e[21] * e[31] * e[4] - e[30] * e[26] * e[8] - e[30] * e[20] * e[2] - + e[30] * e[19] * e[1] + e[30] * e[22] * e[4] - e[30] * e[25] * e[7] + + e[30] * e[23] * e[5] + e[6] * e[31] * e[25] + e[6] * e[22] * e[34] + + e[6] * e[32] * e[26] + e[6] * e[23] * e[35] + e[24] * e[30] * e[6] + + e[24] * e[3] * e[33] + e[24] * e[31] * e[7] + e[24] * e[4] * e[34] + + e[24] * e[32] * e[8] + e[24] * e[5] * e[35] + e[33] * e[21] * e[6] + + e[33] * e[22] * e[7] + e[33] * e[4] * e[25] + e[33] * e[23] * e[8] + + e[33] * e[5] * e[26] + e[0] * e[27] * e[21] + e[0] * e[18] * e[30] + + e[0] * e[28] * e[22] + e[0] * e[19] * e[31] + e[0] * e[29] * e[23] + + e[0] * e[20] * e[32] + e[18] * e[27] * e[3] + e[18] * e[28] * e[4] + + e[18] * e[1] * e[31] + e[18] * e[29] * e[5] + e[18] * e[2] * e[32] + + e[27] * e[19] * e[4] + e[27] * e[1] * e[22] + e[27] * e[20] * e[5] + + e[27] * e[2] * e[23] + 3. * e[3] * e[30] * e[21] + + e[3] * e[31] * e[22] + e[3] * e[32] * e[23] - e[3] * e[29] * e[20] - + e[3] * e[35] * e[26] - e[3] * e[28] * e[19] - e[3] * e[34] * e[25] - + e[21] * e[34] * e[7] + e[21] * e[32] * e[5]; + a[44] = e[18] * e[1] * e[4] + e[18] * e[0] * e[3] + e[18] * e[2] * e[5] + + e[3] * e[22] * e[4] + e[3] * e[23] * e[5] + e[6] * e[3] * e[24] + + e[6] * e[22] * e[7] + e[6] * e[4] * e[25] + e[6] * e[23] * e[8] + + e[6] * e[5] * e[26] + e[24] * e[4] * e[7] + e[24] * e[5] * e[8] + + e[0] * e[19] * e[4] + e[0] * e[1] * e[22] + e[0] * e[20] * e[5] + + e[0] * e[2] * e[23] - e[3] * e[26] * e[8] - e[3] * e[20] * e[2] - + e[3] * e[19] * e[1] - e[3] * e[25] * e[7] + 0.5 * e[21] * e2[4] + + 0.5 * e[21] * e2[0] + 0.5 * e[21] * e2[6] + 0.5 * e[21] * e2[5] - + 0.5 * e[21] * e2[1] - 0.5 * e[21] * e2[7] + 1.5 * e[21] * e2[3] - + 0.5 * e[21] * e2[2] - 0.5 * e[21] * e2[8]; + a[184] = + 0.5 * e2[27] * e[21] + 1.5 * e[21] * e2[30] + 0.5 * e[21] * e2[32] + + 0.5 * e[21] * e2[31] + 0.5 * e[21] * e2[33] - 0.5 * e[21] * e2[28] - + 0.5 * e[21] * e2[29] - 0.5 * e[21] * e2[34] - 0.5 * e[21] * e2[35] + + e[18] * e[27] * e[30] + e[18] * e[29] * e[32] + e[18] * e[28] * e[31] + + e[27] * e[28] * e[22] + e[27] * e[19] * e[31] + e[27] * e[29] * e[23] + + e[27] * e[20] * e[32] + e[30] * e[31] * e[22] + e[30] * e[32] * e[23] + + e[24] * e[30] * e[33] + e[24] * e[32] * e[35] + e[24] * e[31] * e[34] + + e[33] * e[31] * e[25] + e[33] * e[22] * e[34] + e[33] * e[32] * e[26] + + e[33] * e[23] * e[35] - e[30] * e[29] * e[20] - e[30] * e[35] * e[26] - + e[30] * e[28] * e[19] - e[30] * e[34] * e[25]; + a[49] = -0.5 * e[26] * e2[4] - 0.5 * e[26] * e2[0] + 0.5 * e[26] * e2[6] + + 0.5 * e[26] * e2[5] - 0.5 * e[26] * e2[1] + 0.5 * e[26] * e2[7] - + 0.5 * e[26] * e2[3] + 0.5 * e[26] * e2[2] + 1.5 * e[26] * e2[8] + + e[20] * e[0] * e[6] + e[20] * e[2] * e[8] + e[5] * e[21] * e[6] + + e[5] * e[3] * e[24] + e[5] * e[22] * e[7] + e[5] * e[4] * e[25] + + e[5] * e[23] * e[8] + e[23] * e[4] * e[7] + e[23] * e[3] * e[6] + + e[8] * e[24] * e[6] + e[8] * e[25] * e[7] + e[2] * e[18] * e[6] + + e[2] * e[0] * e[24] + e[2] * e[19] * e[7] + e[2] * e[1] * e[25] - + e[8] * e[21] * e[3] - e[8] * e[19] * e[1] - e[8] * e[22] * e[4] - + e[8] * e[18] * e[0] + e[20] * e[1] * e[7]; + a[154] = + e[9] * e[27] * e[30] + e[9] * e[29] * e[32] + e[9] * e[28] * e[31] + + e[33] * e[30] * e[15] + e[33] * e[32] * e[17] + e[33] * e[14] * e[35] + + e[33] * e[31] * e[16] + e[33] * e[13] * e[34] + e[27] * e[29] * e[14] + + e[27] * e[11] * e[32] + e[27] * e[28] * e[13] + e[27] * e[10] * e[31] - + e[30] * e[28] * e[10] + e[30] * e[31] * e[13] + e[30] * e[32] * e[14] - + e[30] * e[34] * e[16] - e[30] * e[35] * e[17] - e[30] * e[29] * e[11] + + e[15] * e[32] * e[35] + e[15] * e[31] * e[34] - 0.5 * e[12] * e2[34] - + 0.5 * e[12] * e2[35] + 0.5 * e[12] * e2[27] + 0.5 * e[12] * e2[32] - + 0.5 * e[12] * e2[28] - 0.5 * e[12] * e2[29] + 0.5 * e[12] * e2[31] + + 0.5 * e[12] * e2[33] + 1.5 * e[12] * e2[30]; + a[119] = e[23] * e[30] * e[6] + e[23] * e[3] * e[33] + e[23] * e[31] * e[7] + + e[23] * e[4] * e[34] + e[32] * e[21] * e[6] + e[32] * e[3] * e[24] + + e[32] * e[22] * e[7] + e[32] * e[4] * e[25] + e[26] * e[33] * e[6] + + e[26] * e[34] * e[7] + 3. * e[26] * e[35] * e[8] + + e[35] * e[24] * e[6] + e[35] * e[25] * e[7] + e[2] * e[27] * e[24] + + e[2] * e[18] * e[33] + e[2] * e[28] * e[25] + e[2] * e[19] * e[34] + + e[2] * e[29] * e[26] + e[2] * e[20] * e[35] + e[20] * e[27] * e[6] + + e[20] * e[0] * e[33] + e[20] * e[28] * e[7] + e[20] * e[1] * e[34] + + e[20] * e[29] * e[8] + e[29] * e[18] * e[6] + e[29] * e[0] * e[24] + + e[29] * e[19] * e[7] + e[29] * e[1] * e[25] + e[5] * e[30] * e[24] + + e[5] * e[21] * e[33] + e[5] * e[31] * e[25] + e[5] * e[22] * e[34] + + e[5] * e[32] * e[26] + e[5] * e[23] * e[35] - e[8] * e[27] * e[18] + + e[8] * e[33] * e[24] - e[8] * e[30] * e[21] - e[8] * e[31] * e[22] + + e[8] * e[32] * e[23] - e[8] * e[28] * e[19] + e[8] * e[34] * e[25] - + e[26] * e[27] * e[0] - e[26] * e[30] * e[3] - e[26] * e[28] * e[1] - + e[26] * e[31] * e[4] - e[35] * e[21] * e[3] - e[35] * e[19] * e[1] - + e[35] * e[22] * e[4] - e[35] * e[18] * e[0]; + a[194] = e[27] * e[29] * e[32] + e[27] * e[28] * e[31] + + e[33] * e[32] * e[35] + e[33] * e[31] * e[34] + 0.5 * e3[30] - + 0.5 * e[30] * e2[28] - 0.5 * e[30] * e2[29] - 0.5 * e[30] * e2[34] + + 0.5 * e[30] * e2[33] + 0.5 * e2[27] * e[30] + 0.5 * e[30] * e2[32] + + 0.5 * e[30] * e2[31] - 0.5 * e[30] * e2[35]; + a[69] = + 0.5 * e2[14] * e[26] + 1.5 * e[26] * e2[17] + 0.5 * e[26] * e2[15] + + 0.5 * e[26] * e2[16] + 0.5 * e2[11] * e[26] - 0.5 * e[26] * e2[9] - + 0.5 * e[26] * e2[12] - 0.5 * e[26] * e2[10] - 0.5 * e[26] * e2[13] + + e[20] * e[11] * e[17] + e[20] * e[9] * e[15] + e[20] * e[10] * e[16] + + e[14] * e[21] * e[15] + e[14] * e[12] * e[24] + e[14] * e[23] * e[17] + + e[14] * e[22] * e[16] + e[14] * e[13] * e[25] + e[23] * e[12] * e[15] + + e[23] * e[13] * e[16] + e[17] * e[24] * e[15] + e[17] * e[25] * e[16] - + e[17] * e[18] * e[9] - e[17] * e[21] * e[12] - e[17] * e[19] * e[10] - + e[17] * e[22] * e[13] + e[11] * e[18] * e[15] + e[11] * e[9] * e[24] + + e[11] * e[19] * e[16] + e[11] * e[10] * e[25]; + a[124] = e[0] * e[27] * e[30] + e[0] * e[29] * e[32] + e[0] * e[28] * e[31] + + e[30] * e[31] * e[4] + e[30] * e[32] * e[5] + e[6] * e[30] * e[33] + + e[6] * e[32] * e[35] + e[6] * e[31] * e[34] + e[27] * e[28] * e[4] + + e[27] * e[1] * e[31] + e[27] * e[29] * e[5] + e[27] * e[2] * e[32] + + e[33] * e[31] * e[7] + e[33] * e[4] * e[34] + e[33] * e[32] * e[8] + + e[33] * e[5] * e[35] - e[30] * e[34] * e[7] - e[30] * e[35] * e[8] - + e[30] * e[29] * e[2] - e[30] * e[28] * e[1] + 1.5 * e[3] * e2[30] + + 0.5 * e[3] * e2[32] + 0.5 * e[3] * e2[31] + 0.5 * e[3] * e2[27] - + 0.5 * e[3] * e2[28] - 0.5 * e[3] * e2[29] + 0.5 * e[3] * e2[33] - + 0.5 * e[3] * e2[34] - 0.5 * e[3] * e2[35]; + a[39] = 0.5 * e2[14] * e[8] + 1.5 * e2[17] * e[8] + 0.5 * e[8] * e2[15] + + 0.5 * e[8] * e2[16] - 0.5 * e[8] * e2[9] + 0.5 * e[8] * e2[11] - + 0.5 * e[8] * e2[12] - 0.5 * e[8] * e2[10] - 0.5 * e[8] * e2[13] + + e[14] * e[12] * e[6] + e[14] * e[3] * e[15] + e[14] * e[13] * e[7] + + e[14] * e[4] * e[16] + e[14] * e[5] * e[17] + e[17] * e[15] * e[6] + + e[17] * e[16] * e[7] + e[2] * e[11] * e[17] + e[2] * e[9] * e[15] + + e[2] * e[10] * e[16] + e[5] * e[12] * e[15] + e[5] * e[13] * e[16] + + e[11] * e[9] * e[6] + e[11] * e[0] * e[15] + e[11] * e[10] * e[7] + + e[11] * e[1] * e[16] - e[17] * e[10] * e[1] - e[17] * e[13] * e[4] - + e[17] * e[9] * e[0] - e[17] * e[12] * e[3]; + a[4] = -0.5 * e[3] * e2[1] - 0.5 * e[3] * e2[7] + 0.5 * e3[3] - + 0.5 * e[3] * e2[8] + e[0] * e[2] * e[5] + 0.5 * e[3] * e2[6] + + 0.5 * e[3] * e2[4] - 0.5 * e[3] * e2[2] + e[0] * e[1] * e[4] + + e[6] * e[4] * e[7] + 0.5 * e2[0] * e[3] + 0.5 * e[3] * e2[5] + + e[6] * e[5] * e[8]; + a[139] = + 0.5 * e2[23] * e[17] + 1.5 * e2[26] * e[17] + 0.5 * e[17] * e2[25] + + 0.5 * e[17] * e2[24] - 0.5 * e[17] * e2[18] - 0.5 * e[17] * e2[19] + + 0.5 * e[17] * e2[20] - 0.5 * e[17] * e2[22] - 0.5 * e[17] * e2[21] + + e[23] * e[21] * e[15] + e[23] * e[12] * e[24] + e[23] * e[14] * e[26] + + e[23] * e[22] * e[16] + e[23] * e[13] * e[25] + e[26] * e[24] * e[15] + + e[26] * e[25] * e[16] + e[11] * e[19] * e[25] + e[11] * e[18] * e[24] + + e[11] * e[20] * e[26] + e[14] * e[22] * e[25] + e[14] * e[21] * e[24] + + e[20] * e[18] * e[15] + e[20] * e[9] * e[24] + e[20] * e[19] * e[16] + + e[20] * e[10] * e[25] - e[26] * e[18] * e[9] - e[26] * e[21] * e[12] - + e[26] * e[19] * e[10] - e[26] * e[22] * e[13]; + a[74] = + -e[12] * e[34] * e[16] - e[12] * e[35] * e[17] - e[12] * e[29] * e[11] + + e[9] * e[27] * e[12] + e[9] * e[29] * e[14] + e[9] * e[11] * e[32] + + e[9] * e[28] * e[13] + e[9] * e[10] * e[31] + e[27] * e[11] * e[14] + + e[27] * e[10] * e[13] + e[12] * e[32] * e[14] + e[12] * e[31] * e[13] + + e[15] * e[12] * e[33] + e[15] * e[32] * e[17] + e[15] * e[14] * e[35] + + e[15] * e[31] * e[16] + e[15] * e[13] * e[34] + e[33] * e[14] * e[17] + + e[33] * e[13] * e[16] - e[12] * e[28] * e[10] + 0.5 * e2[9] * e[30] - + 0.5 * e[30] * e2[16] - 0.5 * e[30] * e2[11] + 1.5 * e[30] * e2[12] + + 0.5 * e[30] * e2[15] - 0.5 * e[30] * e2[17] - 0.5 * e[30] * e2[10] + + 0.5 * e[30] * e2[14] + 0.5 * e[30] * e2[13]; + a[149] = + e[32] * e[22] * e[16] + e[32] * e[13] * e[25] - e[17] * e[27] * e[18] + + e[17] * e[33] * e[24] - e[17] * e[30] * e[21] + e[17] * e[29] * e[20] + + 3. * e[17] * e[35] * e[26] - e[17] * e[31] * e[22] - + e[17] * e[28] * e[19] + e[17] * e[34] * e[25] + e[20] * e[27] * e[15] + + e[20] * e[9] * e[33] + e[20] * e[28] * e[16] + e[20] * e[10] * e[34] + + e[29] * e[18] * e[15] + e[29] * e[9] * e[24] + e[29] * e[19] * e[16] + + e[29] * e[10] * e[25] - e[26] * e[27] * e[9] - e[26] * e[30] * e[12] - + e[26] * e[28] * e[10] - e[26] * e[31] * e[13] + e[26] * e[33] * e[15] + + e[26] * e[34] * e[16] + e[35] * e[24] * e[15] + e[35] * e[25] * e[16] - + e[35] * e[18] * e[9] - e[35] * e[21] * e[12] - e[35] * e[19] * e[10] - + e[35] * e[22] * e[13] + e[14] * e[30] * e[24] + e[14] * e[21] * e[33] + + e[14] * e[31] * e[25] + e[14] * e[22] * e[34] + e[14] * e[32] * e[26] + + e[14] * e[23] * e[35] + e[11] * e[27] * e[24] + e[11] * e[18] * e[33] + + e[11] * e[28] * e[25] + e[11] * e[19] * e[34] + e[11] * e[29] * e[26] + + e[11] * e[20] * e[35] + e[23] * e[30] * e[15] + e[23] * e[12] * e[33] + + e[23] * e[32] * e[17] + e[23] * e[31] * e[16] + e[23] * e[13] * e[34] + + e[32] * e[21] * e[15] + e[32] * e[12] * e[24]; + a[84] = e[6] * e[23] * e[17] + e[6] * e[14] * e[26] + e[6] * e[22] * e[16] + + e[6] * e[13] * e[25] + e[0] * e[20] * e[14] + e[0] * e[11] * e[23] + + e[0] * e[19] * e[13] + e[0] * e[10] * e[22] - e[12] * e[26] * e[8] - + e[12] * e[20] * e[2] - e[12] * e[19] * e[1] + e[12] * e[22] * e[4] - + e[12] * e[25] * e[7] + e[12] * e[23] * e[5] - e[21] * e[11] * e[2] - + e[21] * e[10] * e[1] + e[21] * e[13] * e[4] - e[21] * e[16] * e[7] + + e[21] * e[14] * e[5] - e[21] * e[17] * e[8] + e[15] * e[3] * e[24] + + e[15] * e[22] * e[7] + e[15] * e[4] * e[25] + e[15] * e[23] * e[8] + + e[15] * e[5] * e[26] - e[3] * e[25] * e[16] - e[3] * e[26] * e[17] - + e[3] * e[20] * e[11] - e[3] * e[19] * e[10] + e[24] * e[13] * e[7] + + e[24] * e[4] * e[16] + e[24] * e[14] * e[8] + e[24] * e[5] * e[17] + + e[9] * e[18] * e[3] + e[9] * e[0] * e[21] + e[9] * e[19] * e[4] + + e[9] * e[1] * e[22] + e[9] * e[20] * e[5] + e[9] * e[2] * e[23] + + e[18] * e[0] * e[12] + e[18] * e[10] * e[4] + e[18] * e[1] * e[13] + + e[18] * e[11] * e[5] + e[18] * e[2] * e[14] + + 3. * e[3] * e[21] * e[12] + e[3] * e[23] * e[14] + + e[3] * e[22] * e[13] + e[6] * e[21] * e[15] + e[6] * e[12] * e[24]; + a[29] = 0.5 * e2[5] * e[17] + 1.5 * e[17] * e2[8] + 0.5 * e[17] * e2[7] + + 0.5 * e[17] * e2[6] + 0.5 * e2[2] * e[17] - 0.5 * e[17] * e2[4] - + 0.5 * e[17] * e2[0] - 0.5 * e[17] * e2[1] - 0.5 * e[17] * e2[3] + + e[11] * e[1] * e[7] + e[11] * e[0] * e[6] + e[11] * e[2] * e[8] + + e[5] * e[12] * e[6] + e[5] * e[3] * e[15] + e[5] * e[13] * e[7] + + e[5] * e[4] * e[16] + e[5] * e[14] * e[8] + e[14] * e[4] * e[7] + + e[14] * e[3] * e[6] + e[8] * e[15] * e[6] + e[8] * e[16] * e[7] - + e[8] * e[10] * e[1] - e[8] * e[13] * e[4] - e[8] * e[9] * e[0] - + e[8] * e[12] * e[3] + e[2] * e[9] * e[6] + e[2] * e[0] * e[15] + + e[2] * e[10] * e[7] + e[2] * e[1] * e[16]; + a[54] = e[6] * e[4] * e[34] + e[6] * e[32] * e[8] + e[6] * e[5] * e[35] + + e[33] * e[4] * e[7] + e[33] * e[5] * e[8] + e[0] * e[27] * e[3] + + e[0] * e[28] * e[4] + e[0] * e[1] * e[31] + e[0] * e[29] * e[5] + + e[0] * e[2] * e[32] - e[3] * e[34] * e[7] + e[3] * e[32] * e[5] + + e[3] * e[33] * e[6] - e[3] * e[35] * e[8] - e[3] * e[29] * e[2] - + e[3] * e[28] * e[1] + e[3] * e[31] * e[4] + e[27] * e[1] * e[4] + + e[27] * e[2] * e[5] + e[6] * e[31] * e[7] + 0.5 * e[30] * e2[4] + + 0.5 * e[30] * e2[6] + 0.5 * e[30] * e2[5] - 0.5 * e[30] * e2[1] - + 0.5 * e[30] * e2[7] - 0.5 * e[30] * e2[2] - 0.5 * e[30] * e2[8] + + 0.5 * e2[0] * e[30] + 1.5 * e[30] * e2[3]; + a[109] = 0.5 * e2[23] * e[8] + 1.5 * e2[26] * e[8] - 0.5 * e[8] * e2[18] - + 0.5 * e[8] * e2[19] - 0.5 * e[8] * e2[22] + 0.5 * e[8] * e2[24] - + 0.5 * e[8] * e2[21] + 0.5 * e[8] * e2[25] + 0.5 * e2[20] * e[8] + + e[20] * e[18] * e[6] + e[20] * e[0] * e[24] + e[20] * e[19] * e[7] + + e[20] * e[1] * e[25] + e[20] * e[2] * e[26] + e[23] * e[21] * e[6] + + e[23] * e[3] * e[24] + e[23] * e[22] * e[7] + e[23] * e[4] * e[25] + + e[23] * e[5] * e[26] - e[26] * e[21] * e[3] - e[26] * e[19] * e[1] - + e[26] * e[22] * e[4] - e[26] * e[18] * e[0] + e[26] * e[25] * e[7] + + e[26] * e[24] * e[6] + e[2] * e[19] * e[25] + e[2] * e[18] * e[24] + + e[5] * e[22] * e[25] + e[5] * e[21] * e[24]; + a[175] = + e[19] * e[27] * e[21] + e[19] * e[18] * e[30] + e[19] * e[28] * e[22] + + e[19] * e[29] * e[23] + e[19] * e[20] * e[32] + e[28] * e[18] * e[21] + + e[28] * e[20] * e[23] + e[22] * e[30] * e[21] + e[22] * e[32] * e[23] + + e[25] * e[30] * e[24] + e[25] * e[21] * e[33] + e[25] * e[22] * e[34] + + e[25] * e[32] * e[26] + e[25] * e[23] * e[35] + e[34] * e[21] * e[24] + + e[34] * e[23] * e[26] - e[22] * e[27] * e[18] - e[22] * e[33] * e[24] - + e[22] * e[29] * e[20] - e[22] * e[35] * e[26] + 0.5 * e2[19] * e[31] + + 1.5 * e[31] * e2[22] + 0.5 * e[31] * e2[21] + 0.5 * e[31] * e2[23] + + 0.5 * e[31] * e2[25] - 0.5 * e[31] * e2[26] - 0.5 * e[31] * e2[18] - + 0.5 * e[31] * e2[20] - 0.5 * e[31] * e2[24]; + a[15] = -0.5 * e[13] * e2[15] + 0.5 * e[13] * e2[16] + 0.5 * e[13] * e2[12] + + e[16] * e[12] * e[15] + 0.5 * e3[13] + e[10] * e[11] * e[14] + + 0.5 * e[13] * e2[14] - 0.5 * e[13] * e2[17] - 0.5 * e[13] * e2[11] - + 0.5 * e[13] * e2[9] + 0.5 * e2[10] * e[13] + e[10] * e[9] * e[12] + + e[16] * e[14] * e[17]; + a[95] = -e[13] * e[29] * e[2] - e[31] * e[11] * e[2] - e[31] * e[15] * e[6] - + e[31] * e[9] * e[0] + e[31] * e[14] * e[5] + e[31] * e[12] * e[3] - + e[31] * e[17] * e[8] + e[16] * e[30] * e[6] + e[16] * e[3] * e[33] + + e[16] * e[4] * e[34] + e[16] * e[32] * e[8] + e[16] * e[5] * e[35] - + e[4] * e[27] * e[9] + e[4] * e[28] * e[10] - e[4] * e[33] * e[15] - + e[4] * e[35] * e[17] - e[4] * e[29] * e[11] + e[34] * e[12] * e[6] + + e[34] * e[3] * e[15] + e[34] * e[14] * e[8] + e[34] * e[5] * e[17] + + e[10] * e[27] * e[3] + e[10] * e[0] * e[30] + e[10] * e[29] * e[5] + + e[10] * e[2] * e[32] + e[28] * e[9] * e[3] + e[28] * e[0] * e[12] + + e[28] * e[11] * e[5] + e[28] * e[2] * e[14] + e[4] * e[30] * e[12] + + e[4] * e[32] * e[14] + 3. * e[4] * e[31] * e[13] + + e[7] * e[30] * e[15] + e[7] * e[12] * e[33] + e[7] * e[32] * e[17] + + e[7] * e[14] * e[35] + e[7] * e[31] * e[16] + e[7] * e[13] * e[34] + + e[1] * e[27] * e[12] + e[1] * e[9] * e[30] + e[1] * e[29] * e[14] + + e[1] * e[11] * e[32] + e[1] * e[28] * e[13] + e[1] * e[10] * e[31] - + e[13] * e[27] * e[0] + e[13] * e[32] * e[5] - e[13] * e[33] * e[6] + + e[13] * e[30] * e[3] - e[13] * e[35] * e[8]; + a[165] = e[25] * e[23] * e[26] + e[19] * e[20] * e[23] + + e[19] * e[18] * e[21] + e[25] * e[21] * e[24] + 0.5 * e3[22] + + 0.5 * e[22] * e2[23] + 0.5 * e2[19] * e[22] - 0.5 * e[22] * e2[18] - + 0.5 * e[22] * e2[24] + 0.5 * e[22] * e2[21] + 0.5 * e[22] * e2[25] - + 0.5 * e[22] * e2[20] - 0.5 * e[22] * e2[26]; + a[55] = e[34] * e[5] * e[8] + e[1] * e[27] * e[3] + e[1] * e[0] * e[30] + + e[1] * e[28] * e[4] + e[1] * e[29] * e[5] + e[1] * e[2] * e[32] - + e[4] * e[27] * e[0] + e[4] * e[34] * e[7] + e[4] * e[32] * e[5] - + e[4] * e[33] * e[6] + e[4] * e[30] * e[3] - e[4] * e[35] * e[8] - + e[4] * e[29] * e[2] + e[28] * e[0] * e[3] + e[28] * e[2] * e[5] + + e[7] * e[30] * e[6] + e[7] * e[3] * e[33] + e[7] * e[32] * e[8] + + e[7] * e[5] * e[35] + e[34] * e[3] * e[6] + 0.5 * e2[1] * e[31] + + 1.5 * e[31] * e2[4] - 0.5 * e[31] * e2[0] - 0.5 * e[31] * e2[6] + + 0.5 * e[31] * e2[5] + 0.5 * e[31] * e2[7] + 0.5 * e[31] * e2[3] - + 0.5 * e[31] * e2[2] - 0.5 * e[31] * e2[8]; + a[85] = e[1] * e[20] * e[14] + e[1] * e[11] * e[23] + e[13] * e[21] * e[3] - + e[13] * e[26] * e[8] - e[13] * e[20] * e[2] - e[13] * e[18] * e[0] + + e[13] * e[23] * e[5] - e[13] * e[24] * e[6] - e[22] * e[11] * e[2] - + e[22] * e[15] * e[6] - e[22] * e[9] * e[0] + e[22] * e[14] * e[5] + + e[22] * e[12] * e[3] - e[22] * e[17] * e[8] + e[16] * e[21] * e[6] + + e[16] * e[3] * e[24] + e[16] * e[4] * e[25] + e[16] * e[23] * e[8] + + e[16] * e[5] * e[26] - e[4] * e[24] * e[15] - e[4] * e[26] * e[17] - + e[4] * e[20] * e[11] - e[4] * e[18] * e[9] + e[25] * e[12] * e[6] + + e[25] * e[3] * e[15] + e[25] * e[14] * e[8] + e[25] * e[5] * e[17] + + e[10] * e[18] * e[3] + e[10] * e[0] * e[21] + e[10] * e[19] * e[4] + + e[10] * e[1] * e[22] + e[10] * e[20] * e[5] + e[10] * e[2] * e[23] + + e[19] * e[9] * e[3] + e[19] * e[0] * e[12] + e[19] * e[1] * e[13] + + e[19] * e[11] * e[5] + e[19] * e[2] * e[14] + e[4] * e[21] * e[12] + + e[4] * e[23] * e[14] + 3. * e[4] * e[22] * e[13] + + e[7] * e[21] * e[15] + e[7] * e[12] * e[24] + e[7] * e[23] * e[17] + + e[7] * e[14] * e[26] + e[7] * e[22] * e[16] + e[7] * e[13] * e[25] + + e[1] * e[18] * e[12] + e[1] * e[9] * e[21]; + a[75] = + e[10] * e[27] * e[12] + e[10] * e[9] * e[30] + e[10] * e[29] * e[14] + + e[10] * e[11] * e[32] + e[10] * e[28] * e[13] + e[28] * e[11] * e[14] + + e[28] * e[9] * e[12] + e[13] * e[30] * e[12] + e[13] * e[32] * e[14] + + e[16] * e[30] * e[15] + e[16] * e[12] * e[33] + e[16] * e[32] * e[17] + + e[16] * e[14] * e[35] + e[16] * e[13] * e[34] + e[34] * e[14] * e[17] + + e[34] * e[12] * e[15] - e[13] * e[27] * e[9] - e[13] * e[33] * e[15] - + e[13] * e[35] * e[17] - e[13] * e[29] * e[11] + 0.5 * e2[10] * e[31] + + 0.5 * e[31] * e2[16] - 0.5 * e[31] * e2[9] - 0.5 * e[31] * e2[11] + + 0.5 * e[31] * e2[12] - 0.5 * e[31] * e2[15] - 0.5 * e[31] * e2[17] + + 0.5 * e[31] * e2[14] + 1.5 * e[31] * e2[13]; + a[5] = -0.5 * e[4] * e2[6] - 0.5 * e[4] * e2[0] + e[1] * e[2] * e[5] + + 0.5 * e[4] * e2[7] + e[1] * e[0] * e[3] + e[7] * e[5] * e[8] - + 0.5 * e[4] * e2[8] + 0.5 * e[4] * e2[3] + 0.5 * e[4] * e2[5] + + e[7] * e[3] * e[6] - 0.5 * e[4] * e2[2] + 0.5 * e3[4] + + 0.5 * e2[1] * e[4]; + a[195] = e[34] * e[32] * e[35] - 0.5 * e[31] * e2[35] + 0.5 * e[31] * e2[34] + + 0.5 * e2[28] * e[31] + 0.5 * e3[31] + 0.5 * e[31] * e2[32] + + e[34] * e[30] * e[33] - 0.5 * e[31] * e2[27] + 0.5 * e[31] * e2[30] - + 0.5 * e[31] * e2[33] - 0.5 * e[31] * e2[29] + e[28] * e[29] * e[32] + + e[28] * e[27] * e[30]; + a[125] = e[1] * e[27] * e[30] + e[1] * e[29] * e[32] + e[1] * e[28] * e[31] + + e[31] * e[30] * e[3] + e[31] * e[32] * e[5] + e[7] * e[30] * e[33] + + e[7] * e[32] * e[35] + e[7] * e[31] * e[34] + e[28] * e[27] * e[3] + + e[28] * e[0] * e[30] + e[28] * e[29] * e[5] + e[28] * e[2] * e[32] + + e[34] * e[30] * e[6] + e[34] * e[3] * e[33] + e[34] * e[32] * e[8] + + e[34] * e[5] * e[35] - e[31] * e[27] * e[0] - e[31] * e[33] * e[6] - + e[31] * e[35] * e[8] - e[31] * e[29] * e[2] + 0.5 * e[4] * e2[30] + + 0.5 * e[4] * e2[32] + 1.5 * e[4] * e2[31] - 0.5 * e[4] * e2[27] + + 0.5 * e[4] * e2[28] - 0.5 * e[4] * e2[29] - 0.5 * e[4] * e2[33] + + 0.5 * e[4] * e2[34] - 0.5 * e[4] * e2[35]; + a[185] = + 0.5 * e[22] * e2[30] + 0.5 * e[22] * e2[32] + 1.5 * e[22] * e2[31] + + 0.5 * e[22] * e2[34] - 0.5 * e[22] * e2[27] - 0.5 * e[22] * e2[29] - + 0.5 * e[22] * e2[33] - 0.5 * e[22] * e2[35] + e[28] * e[18] * e[30] + + e[28] * e[29] * e[23] + e[28] * e[20] * e[32] + e[31] * e[30] * e[21] + + e[31] * e[32] * e[23] + e[25] * e[30] * e[33] + e[25] * e[32] * e[35] + + e[25] * e[31] * e[34] + e[34] * e[30] * e[24] + e[34] * e[21] * e[33] + + e[34] * e[32] * e[26] + e[34] * e[23] * e[35] - e[31] * e[27] * e[18] - + e[31] * e[33] * e[24] - e[31] * e[29] * e[20] - e[31] * e[35] * e[26] + + e[19] * e[27] * e[30] + e[19] * e[29] * e[32] + e[19] * e[28] * e[31] + + e[28] * e[27] * e[21] + 0.5 * e2[28] * e[22]; + a[155] = + e[16] * e[30] * e[33] + e[16] * e[32] * e[35] + e[10] * e[27] * e[30] + + e[10] * e[29] * e[32] + e[10] * e[28] * e[31] + e[34] * e[30] * e[15] + + e[34] * e[12] * e[33] + e[34] * e[32] * e[17] + e[34] * e[14] * e[35] + + e[34] * e[31] * e[16] + e[28] * e[27] * e[12] + e[28] * e[9] * e[30] + + e[28] * e[29] * e[14] + e[28] * e[11] * e[32] - e[31] * e[27] * e[9] + + e[31] * e[30] * e[12] + e[31] * e[32] * e[14] - e[31] * e[33] * e[15] - + e[31] * e[35] * e[17] - e[31] * e[29] * e[11] - 0.5 * e[13] * e2[27] + + 0.5 * e[13] * e2[32] + 0.5 * e[13] * e2[28] - 0.5 * e[13] * e2[29] + + 1.5 * e[13] * e2[31] - 0.5 * e[13] * e2[33] + 0.5 * e[13] * e2[30] + + 0.5 * e[13] * e2[34] - 0.5 * e[13] * e2[35]; + a[134] = + e[21] * e[23] * e[14] + e[21] * e[22] * e[13] + e[24] * e[21] * e[15] + + e[24] * e[23] * e[17] + e[24] * e[14] * e[26] + e[24] * e[22] * e[16] + + e[24] * e[13] * e[25] + e[15] * e[22] * e[25] + e[15] * e[23] * e[26] + + e[9] * e[19] * e[22] + e[9] * e[18] * e[21] + e[9] * e[20] * e[23] + + e[18] * e[20] * e[14] + e[18] * e[11] * e[23] + e[18] * e[19] * e[13] + + e[18] * e[10] * e[22] - e[21] * e[25] * e[16] - e[21] * e[26] * e[17] - + e[21] * e[20] * e[11] - e[21] * e[19] * e[10] + 1.5 * e2[21] * e[12] + + 0.5 * e[12] * e2[24] - 0.5 * e[12] * e2[26] + 0.5 * e[12] * e2[18] + + 0.5 * e[12] * e2[23] - 0.5 * e[12] * e2[19] - 0.5 * e[12] * e2[20] + + 0.5 * e[12] * e2[22] - 0.5 * e[12] * e2[25]; + a[144] = + -e[12] * e[29] * e[20] - e[12] * e[35] * e[26] - e[12] * e[28] * e[19] - + e[12] * e[34] * e[25] + e[18] * e[29] * e[14] + e[18] * e[11] * e[32] + + e[18] * e[28] * e[13] + e[18] * e[10] * e[31] + e[27] * e[20] * e[14] + + e[27] * e[11] * e[23] + e[27] * e[19] * e[13] + e[27] * e[10] * e[22] + + e[15] * e[30] * e[24] + e[15] * e[21] * e[33] + e[15] * e[31] * e[25] + + e[15] * e[22] * e[34] + e[15] * e[32] * e[26] + e[15] * e[23] * e[35] - + e[21] * e[28] * e[10] - e[21] * e[34] * e[16] - e[21] * e[35] * e[17] - + e[21] * e[29] * e[11] - e[30] * e[25] * e[16] - e[30] * e[26] * e[17] - + e[30] * e[20] * e[11] - e[30] * e[19] * e[10] + e[24] * e[32] * e[17] + + e[24] * e[14] * e[35] + e[24] * e[31] * e[16] + e[24] * e[13] * e[34] + + e[33] * e[23] * e[17] + e[33] * e[14] * e[26] + e[33] * e[22] * e[16] + + e[33] * e[13] * e[25] + 3. * e[12] * e[30] * e[21] + + e[12] * e[31] * e[22] + e[12] * e[32] * e[23] + e[9] * e[27] * e[21] + + e[9] * e[18] * e[30] + e[9] * e[28] * e[22] + e[9] * e[19] * e[31] + + e[9] * e[29] * e[23] + e[9] * e[20] * e[32] + e[21] * e[32] * e[14] + + e[21] * e[31] * e[13] + e[30] * e[23] * e[14] + e[30] * e[22] * e[13] + + e[12] * e[27] * e[18] + e[12] * e[33] * e[24]; + a[24] = e[0] * e[11] * e[5] + e[0] * e[2] * e[14] + e[9] * e[1] * e[4] + + e[9] * e[0] * e[3] + e[9] * e[2] * e[5] + e[3] * e[13] * e[4] + + e[3] * e[14] * e[5] + e[6] * e[3] * e[15] + e[6] * e[13] * e[7] + + e[6] * e[4] * e[16] + e[6] * e[14] * e[8] + e[6] * e[5] * e[17] + + e[15] * e[4] * e[7] + e[15] * e[5] * e[8] - e[3] * e[11] * e[2] - + e[3] * e[10] * e[1] - e[3] * e[16] * e[7] - e[3] * e[17] * e[8] + + e[0] * e[10] * e[4] + e[0] * e[1] * e[13] + 1.5 * e[12] * e2[3] + + 0.5 * e[12] * e2[4] + 0.5 * e[12] * e2[5] + 0.5 * e[12] * e2[6] + + 0.5 * e2[0] * e[12] - 0.5 * e[12] * e2[1] - 0.5 * e[12] * e2[7] - + 0.5 * e[12] * e2[2] - 0.5 * e[12] * e2[8]; + a[104] = e[21] * e[24] * e[6] + e[0] * e[19] * e[22] + e[0] * e[20] * e[23] + + e[24] * e[22] * e[7] + e[24] * e[4] * e[25] + e[24] * e[23] * e[8] + + e[24] * e[5] * e[26] + e[6] * e[22] * e[25] + e[6] * e[23] * e[26] + + e[18] * e[0] * e[21] + e[18] * e[19] * e[4] + e[18] * e[1] * e[22] + + e[18] * e[20] * e[5] + e[18] * e[2] * e[23] + e[21] * e[22] * e[4] + + e[21] * e[23] * e[5] - e[21] * e[26] * e[8] - e[21] * e[20] * e[2] - + e[21] * e[19] * e[1] - e[21] * e[25] * e[7] + 1.5 * e2[21] * e[3] + + 0.5 * e[3] * e2[22] + 0.5 * e[3] * e2[23] + 0.5 * e[3] * e2[24] - + 0.5 * e[3] * e2[26] - 0.5 * e[3] * e2[19] - 0.5 * e[3] * e2[20] - + 0.5 * e[3] * e2[25] + 0.5 * e2[18] * e[3]; + a[76] = + e[11] * e[27] * e[12] + e[11] * e[9] * e[30] + e[11] * e[29] * e[14] + + e[11] * e[28] * e[13] + e[11] * e[10] * e[31] + e[29] * e[9] * e[12] + + e[29] * e[10] * e[13] + e[14] * e[30] * e[12] + e[14] * e[31] * e[13] + + e[17] * e[30] * e[15] + e[17] * e[12] * e[33] + e[17] * e[14] * e[35] + + e[17] * e[31] * e[16] + e[17] * e[13] * e[34] + e[35] * e[12] * e[15] + + e[35] * e[13] * e[16] - e[14] * e[27] * e[9] - e[14] * e[28] * e[10] - + e[14] * e[33] * e[15] - e[14] * e[34] * e[16] + 0.5 * e2[11] * e[32] - + 0.5 * e[32] * e2[16] - 0.5 * e[32] * e2[9] + 0.5 * e[32] * e2[12] - + 0.5 * e[32] * e2[15] + 0.5 * e[32] * e2[17] - 0.5 * e[32] * e2[10] + + 1.5 * e[32] * e2[14] + 0.5 * e[32] * e2[13]; + a[6] = e[8] * e[3] * e[6] + 0.5 * e2[2] * e[5] - 0.5 * e[5] * e2[0] + + 0.5 * e[5] * e2[4] - 0.5 * e[5] * e2[6] + 0.5 * e[5] * e2[8] + + e[8] * e[4] * e[7] + 0.5 * e3[5] + e[2] * e[0] * e[3] + + 0.5 * e[5] * e2[3] - 0.5 * e[5] * e2[7] + e[2] * e[1] * e[4] - + 0.5 * e[5] * e2[1]; + a[56] = e[2] * e[27] * e[3] + e[2] * e[0] * e[30] + e[2] * e[28] * e[4] + + e[2] * e[1] * e[31] + e[2] * e[29] * e[5] - e[5] * e[27] * e[0] - + e[5] * e[34] * e[7] - e[5] * e[33] * e[6] + e[5] * e[30] * e[3] + + e[5] * e[35] * e[8] - e[5] * e[28] * e[1] + e[5] * e[31] * e[4] + + e[29] * e[1] * e[4] + e[29] * e[0] * e[3] + e[8] * e[30] * e[6] + + e[8] * e[3] * e[33] + e[8] * e[31] * e[7] + e[8] * e[4] * e[34] + + e[35] * e[4] * e[7] + e[35] * e[3] * e[6] + 0.5 * e2[2] * e[32] + + 1.5 * e[32] * e2[5] + 0.5 * e[32] * e2[4] - 0.5 * e[32] * e2[0] - + 0.5 * e[32] * e2[6] - 0.5 * e[32] * e2[1] - 0.5 * e[32] * e2[7] + + 0.5 * e[32] * e2[3] + 0.5 * e[32] * e2[8]; + a[86] = -e[14] * e[19] * e[1] + e[14] * e[22] * e[4] - e[14] * e[18] * e[0] - + e[14] * e[25] * e[7] - e[14] * e[24] * e[6] - e[23] * e[10] * e[1] + + e[23] * e[13] * e[4] - e[23] * e[16] * e[7] - e[23] * e[15] * e[6] - + e[23] * e[9] * e[0] + e[23] * e[12] * e[3] + e[17] * e[21] * e[6] + + e[17] * e[3] * e[24] + e[17] * e[22] * e[7] + e[17] * e[4] * e[25] + + e[17] * e[5] * e[26] - e[5] * e[24] * e[15] - e[5] * e[25] * e[16] - + e[5] * e[18] * e[9] - e[5] * e[19] * e[10] + e[26] * e[12] * e[6] + + e[26] * e[3] * e[15] + e[26] * e[13] * e[7] + e[26] * e[4] * e[16] + + e[11] * e[18] * e[3] + e[11] * e[0] * e[21] + e[11] * e[19] * e[4] + + e[11] * e[1] * e[22] + e[11] * e[20] * e[5] + e[11] * e[2] * e[23] + + e[20] * e[9] * e[3] + e[20] * e[0] * e[12] + e[20] * e[10] * e[4] + + e[20] * e[1] * e[13] + e[20] * e[2] * e[14] + e[5] * e[21] * e[12] + + 3. * e[5] * e[23] * e[14] + e[5] * e[22] * e[13] + + e[8] * e[21] * e[15] + e[8] * e[12] * e[24] + e[8] * e[23] * e[17] + + e[8] * e[14] * e[26] + e[8] * e[22] * e[16] + e[8] * e[13] * e[25] + + e[2] * e[18] * e[12] + e[2] * e[9] * e[21] + e[2] * e[19] * e[13] + + e[2] * e[10] * e[22] + e[14] * e[21] * e[3]; + a[156] = + -0.5 * e[14] * e2[27] + 1.5 * e[14] * e2[32] - 0.5 * e[14] * e2[28] + + 0.5 * e[14] * e2[29] + 0.5 * e[14] * e2[31] - 0.5 * e[14] * e2[33] + + 0.5 * e[14] * e2[30] - 0.5 * e[14] * e2[34] + 0.5 * e[14] * e2[35] + + e[11] * e[27] * e[30] + e[11] * e[29] * e[32] + e[11] * e[28] * e[31] + + e[35] * e[30] * e[15] + e[35] * e[12] * e[33] + e[35] * e[32] * e[17] + + e[35] * e[31] * e[16] + e[35] * e[13] * e[34] + e[29] * e[27] * e[12] + + e[29] * e[9] * e[30] + e[29] * e[28] * e[13] + e[29] * e[10] * e[31] - + e[32] * e[27] * e[9] + e[32] * e[30] * e[12] - e[32] * e[28] * e[10] + + e[32] * e[31] * e[13] - e[32] * e[33] * e[15] - e[32] * e[34] * e[16] + + e[17] * e[30] * e[33] + e[17] * e[31] * e[34]; + a[186] = + -0.5 * e[23] * e2[33] - 0.5 * e[23] * e2[34] + 0.5 * e2[29] * e[23] + + 0.5 * e[23] * e2[30] + 1.5 * e[23] * e2[32] + 0.5 * e[23] * e2[31] + + 0.5 * e[23] * e2[35] - 0.5 * e[23] * e2[27] - 0.5 * e[23] * e2[28] + + e[32] * e[30] * e[21] + e[32] * e[31] * e[22] + e[26] * e[30] * e[33] + + e[26] * e[32] * e[35] + e[26] * e[31] * e[34] + e[35] * e[30] * e[24] + + e[35] * e[21] * e[33] + e[35] * e[31] * e[25] + e[35] * e[22] * e[34] - + e[32] * e[27] * e[18] - e[32] * e[33] * e[24] - e[32] * e[28] * e[19] - + e[32] * e[34] * e[25] + e[20] * e[27] * e[30] + e[20] * e[29] * e[32] + + e[20] * e[28] * e[31] + e[29] * e[27] * e[21] + e[29] * e[18] * e[30] + + e[29] * e[28] * e[22] + e[29] * e[19] * e[31]; + a[126] = e[2] * e[27] * e[30] + e[2] * e[29] * e[32] + e[2] * e[28] * e[31] + + e[32] * e[30] * e[3] + e[32] * e[31] * e[4] + e[8] * e[30] * e[33] + + e[8] * e[32] * e[35] + e[8] * e[31] * e[34] + e[29] * e[27] * e[3] + + e[29] * e[0] * e[30] + e[29] * e[28] * e[4] + e[29] * e[1] * e[31] + + e[35] * e[30] * e[6] + e[35] * e[3] * e[33] + e[35] * e[31] * e[7] + + e[35] * e[4] * e[34] - e[32] * e[27] * e[0] - e[32] * e[34] * e[7] - + e[32] * e[33] * e[6] - e[32] * e[28] * e[1] + 0.5 * e[5] * e2[30] + + 1.5 * e[5] * e2[32] + 0.5 * e[5] * e2[31] - 0.5 * e[5] * e2[27] - + 0.5 * e[5] * e2[28] + 0.5 * e[5] * e2[29] - 0.5 * e[5] * e2[33] - + 0.5 * e[5] * e2[34] + 0.5 * e[5] * e2[35]; + a[196] = 0.5 * e[32] * e2[31] + 0.5 * e[32] * e2[35] - 0.5 * e[32] * e2[27] + + e[29] * e[27] * e[30] + e[29] * e[28] * e[31] + + e[35] * e[30] * e[33] + e[35] * e[31] * e[34] + + 0.5 * e2[29] * e[32] + 0.5 * e3[32] - 0.5 * e[32] * e2[33] - + 0.5 * e[32] * e2[34] + 0.5 * e[32] * e2[30] - 0.5 * e[32] * e2[28]; + a[25] = e[10] * e[1] * e[4] + e[10] * e[0] * e[3] + e[10] * e[2] * e[5] + + e[4] * e[12] * e[3] + e[4] * e[14] * e[5] + e[7] * e[12] * e[6] + + e[7] * e[3] * e[15] + e[7] * e[4] * e[16] + e[7] * e[14] * e[8] + + e[7] * e[5] * e[17] + e[16] * e[3] * e[6] + e[16] * e[5] * e[8] - + e[4] * e[11] * e[2] - e[4] * e[15] * e[6] - e[4] * e[9] * e[0] - + e[4] * e[17] * e[8] + e[1] * e[9] * e[3] + e[1] * e[0] * e[12] + + e[1] * e[11] * e[5] + e[1] * e[2] * e[14] + 1.5 * e[13] * e2[4] + + 0.5 * e[13] * e2[3] + 0.5 * e[13] * e2[5] + 0.5 * e[13] * e2[7] + + 0.5 * e2[1] * e[13] - 0.5 * e[13] * e2[0] - 0.5 * e[13] * e2[6] - + 0.5 * e[13] * e2[2] - 0.5 * e[13] * e2[8]; + a[105] = e[25] * e[21] * e[6] + e[25] * e[3] * e[24] + e[25] * e[23] * e[8] + + e[25] * e[5] * e[26] + e[7] * e[21] * e[24] + e[7] * e[23] * e[26] + + e[19] * e[18] * e[3] + e[19] * e[0] * e[21] + e[19] * e[1] * e[22] + + e[19] * e[20] * e[5] + e[19] * e[2] * e[23] + e[22] * e[21] * e[3] + + e[22] * e[23] * e[5] - e[22] * e[26] * e[8] - e[22] * e[20] * e[2] - + e[22] * e[18] * e[0] + e[22] * e[25] * e[7] - e[22] * e[24] * e[6] + + e[1] * e[18] * e[21] + e[1] * e[20] * e[23] + 0.5 * e[4] * e2[25] - + 0.5 * e[4] * e2[26] - 0.5 * e[4] * e2[18] - 0.5 * e[4] * e2[20] - + 0.5 * e[4] * e2[24] + 0.5 * e2[19] * e[4] + 1.5 * e2[22] * e[4] + + 0.5 * e[4] * e2[21] + 0.5 * e[4] * e2[23]; + a[135] = + e[22] * e[21] * e[12] + e[22] * e[23] * e[14] + e[25] * e[21] * e[15] + + e[25] * e[12] * e[24] + e[25] * e[23] * e[17] + e[25] * e[14] * e[26] + + e[25] * e[22] * e[16] + e[16] * e[21] * e[24] + e[16] * e[23] * e[26] + + e[10] * e[19] * e[22] + e[10] * e[18] * e[21] + e[10] * e[20] * e[23] + + e[19] * e[18] * e[12] + e[19] * e[9] * e[21] + e[19] * e[20] * e[14] + + e[19] * e[11] * e[23] - e[22] * e[24] * e[15] - e[22] * e[26] * e[17] - + e[22] * e[20] * e[11] - e[22] * e[18] * e[9] - 0.5 * e[13] * e2[26] - + 0.5 * e[13] * e2[18] + 0.5 * e[13] * e2[23] + 0.5 * e[13] * e2[19] - + 0.5 * e[13] * e2[20] - 0.5 * e[13] * e2[24] + 0.5 * e[13] * e2[21] + + 1.5 * e2[22] * e[13] + 0.5 * e[13] * e2[25]; + a[145] = + e[13] * e[30] * e[21] + 3. * e[13] * e[31] * e[22] + + e[13] * e[32] * e[23] + e[10] * e[27] * e[21] + e[10] * e[18] * e[30] + + e[10] * e[28] * e[22] + e[10] * e[19] * e[31] + e[10] * e[29] * e[23] + + e[10] * e[20] * e[32] + e[22] * e[30] * e[12] + e[22] * e[32] * e[14] + + e[31] * e[21] * e[12] + e[31] * e[23] * e[14] - e[13] * e[27] * e[18] - + e[13] * e[33] * e[24] - e[13] * e[29] * e[20] - e[13] * e[35] * e[26] + + e[13] * e[28] * e[19] + e[13] * e[34] * e[25] + e[19] * e[27] * e[12] + + e[19] * e[9] * e[30] + e[19] * e[29] * e[14] + e[19] * e[11] * e[32] + + e[28] * e[18] * e[12] + e[28] * e[9] * e[21] + e[28] * e[20] * e[14] + + e[28] * e[11] * e[23] + e[16] * e[30] * e[24] + e[16] * e[21] * e[33] + + e[16] * e[31] * e[25] + e[16] * e[22] * e[34] + e[16] * e[32] * e[26] + + e[16] * e[23] * e[35] - e[22] * e[27] * e[9] - e[22] * e[33] * e[15] - + e[22] * e[35] * e[17] - e[22] * e[29] * e[11] - e[31] * e[24] * e[15] - + e[31] * e[26] * e[17] - e[31] * e[20] * e[11] - e[31] * e[18] * e[9] + + e[25] * e[30] * e[15] + e[25] * e[12] * e[33] + e[25] * e[32] * e[17] + + e[25] * e[14] * e[35] + e[34] * e[21] * e[15] + e[34] * e[12] * e[24] + + e[34] * e[23] * e[17] + e[34] * e[14] * e[26]; + a[65] = + e[19] * e[11] * e[14] + e[19] * e[9] * e[12] + e[19] * e[10] * e[13] + + e[13] * e[21] * e[12] + e[13] * e[23] * e[14] + e[16] * e[21] * e[15] + + e[16] * e[12] * e[24] + e[16] * e[23] * e[17] + e[16] * e[14] * e[26] + + e[16] * e[13] * e[25] + e[25] * e[14] * e[17] + e[25] * e[12] * e[15] - + e[13] * e[24] * e[15] - e[13] * e[26] * e[17] - e[13] * e[20] * e[11] - + e[13] * e[18] * e[9] + e[10] * e[18] * e[12] + e[10] * e[9] * e[21] + + e[10] * e[20] * e[14] + e[10] * e[11] * e[23] + 1.5 * e[22] * e2[13] + + 0.5 * e[22] * e2[14] + 0.5 * e[22] * e2[12] + 0.5 * e[22] * e2[16] + + 0.5 * e2[10] * e[22] - 0.5 * e[22] * e2[9] - 0.5 * e[22] * e2[11] - + 0.5 * e[22] * e2[15] - 0.5 * e[22] * e2[17]; + a[35] = e[13] * e[12] * e[3] + e[13] * e[14] * e[5] + e[16] * e[12] * e[6] + + e[16] * e[3] * e[15] + e[16] * e[13] * e[7] + e[16] * e[14] * e[8] + + e[16] * e[5] * e[17] + e[7] * e[14] * e[17] + e[7] * e[12] * e[15] + + e[1] * e[11] * e[14] + e[1] * e[9] * e[12] + e[1] * e[10] * e[13] + + e[10] * e[9] * e[3] + e[10] * e[0] * e[12] + e[10] * e[11] * e[5] + + e[10] * e[2] * e[14] - e[13] * e[11] * e[2] - e[13] * e[15] * e[6] - + e[13] * e[9] * e[0] - e[13] * e[17] * e[8] + 1.5 * e2[13] * e[4] + + 0.5 * e[4] * e2[16] - 0.5 * e[4] * e2[9] - 0.5 * e[4] * e2[11] + + 0.5 * e[4] * e2[12] - 0.5 * e[4] * e2[15] - 0.5 * e[4] * e2[17] + + 0.5 * e[4] * e2[10] + 0.5 * e[4] * e2[14]; + a[45] = e[19] * e[1] * e[4] + e[19] * e[0] * e[3] + e[19] * e[2] * e[5] + + e[4] * e[21] * e[3] + e[4] * e[23] * e[5] + e[7] * e[21] * e[6] + + e[7] * e[3] * e[24] + e[7] * e[4] * e[25] + e[7] * e[23] * e[8] + + e[7] * e[5] * e[26] + e[25] * e[3] * e[6] + e[25] * e[5] * e[8] + + e[1] * e[18] * e[3] + e[1] * e[0] * e[21] + e[1] * e[20] * e[5] + + e[1] * e[2] * e[23] - e[4] * e[26] * e[8] - e[4] * e[20] * e[2] - + e[4] * e[18] * e[0] - e[4] * e[24] * e[6] + 1.5 * e[22] * e2[4] - + 0.5 * e[22] * e2[0] - 0.5 * e[22] * e2[6] + 0.5 * e[22] * e2[5] + + 0.5 * e[22] * e2[1] + 0.5 * e[22] * e2[7] + 0.5 * e[22] * e2[3] - + 0.5 * e[22] * e2[2] - 0.5 * e[22] * e2[8]; + a[115] = -e[31] * e[20] * e[2] - e[31] * e[18] * e[0] + e[31] * e[23] * e[5] - + e[31] * e[24] * e[6] + e[7] * e[30] * e[24] + e[7] * e[21] * e[33] + + e[7] * e[32] * e[26] + e[7] * e[23] * e[35] + e[25] * e[30] * e[6] + + e[25] * e[3] * e[33] + e[25] * e[31] * e[7] + e[25] * e[4] * e[34] + + e[25] * e[32] * e[8] + e[25] * e[5] * e[35] + e[34] * e[21] * e[6] + + e[34] * e[3] * e[24] + e[34] * e[22] * e[7] + e[34] * e[23] * e[8] + + e[34] * e[5] * e[26] + e[1] * e[27] * e[21] + e[1] * e[18] * e[30] + + e[1] * e[28] * e[22] + e[1] * e[19] * e[31] + e[1] * e[29] * e[23] + + e[1] * e[20] * e[32] + e[19] * e[27] * e[3] + e[19] * e[0] * e[30] + + e[19] * e[28] * e[4] + e[19] * e[29] * e[5] + e[19] * e[2] * e[32] + + e[28] * e[18] * e[3] + e[28] * e[0] * e[21] + e[28] * e[20] * e[5] + + e[28] * e[2] * e[23] + e[4] * e[30] * e[21] + + 3. * e[4] * e[31] * e[22] + e[4] * e[32] * e[23] - + e[4] * e[27] * e[18] - e[4] * e[33] * e[24] - e[4] * e[29] * e[20] - + e[4] * e[35] * e[26] - e[22] * e[27] * e[0] + e[22] * e[32] * e[5] - + e[22] * e[33] * e[6] + e[22] * e[30] * e[3] - e[22] * e[35] * e[8] - + e[22] * e[29] * e[2] + e[31] * e[21] * e[3] - e[31] * e[26] * e[8]; +} diff --git a/include/colmap/estimators/euclidean_transform.h b/include/colmap/estimators/euclidean_transform.h new file mode 100644 index 0000000000000000000000000000000000000000..10af4230dd469edc89fc66b3953ffb92450c4dc8 --- /dev/null +++ b/include/colmap/estimators/euclidean_transform.h @@ -0,0 +1,56 @@ +// Copyright (c) 2022, 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_EUCLIDEAN_TRANSFORM_H_ +#define COLMAP_SRC_ESTIMATORS_EUCLIDEAN_TRANSFORM_H_ + +#include "base/similarity_transform.h" +#include "util/alignment.h" + +namespace colmap { + +// N-D Euclidean transform estimator from corresponding point pairs in the +// source and destination coordinate systems. +// +// This algorithm is based on the following paper: +// +// S. Umeyama. Least-Squares Estimation of Transformation Parameters +// Between Two Point Patterns. IEEE Transactions on Pattern Analysis and +// Machine Intelligence, Volume 13 Issue 4, Page 376-380, 1991. +// http://www.stanford.edu/class/cs273/refs/umeyama.pdf +// +// and uses the Eigen implementation. +template +using EuclideanTransformEstimator = SimilarityTransformEstimator; + +} // namespace colmap + +#endif // COLMAP_SRC_ESTIMATORS_EUCLIDEAN_TRANSFORM_H_ diff --git a/include/colmap/estimators/fundamental_matrix.h b/include/colmap/estimators/fundamental_matrix.h new file mode 100644 index 0000000000000000000000000000000000000000..ef3d3c203340424890f5e720be0144721190132d --- /dev/null +++ b/include/colmap/estimators/fundamental_matrix.h @@ -0,0 +1,129 @@ +// Copyright (c) 2022, 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_FUNDAMENTAL_MATRIX_H_ +#define COLMAP_SRC_ESTIMATORS_FUNDAMENTAL_MATRIX_H_ + +#include + +#include + +#include "estimators/homography_matrix.h" +#include "util/alignment.h" +#include "util/types.h" + +namespace colmap { + +// Fundamental matrix estimator from corresponding point pairs. +// +// This algorithm solves the 7-Point problem and is based on the following +// paper: +// +// Zhengyou Zhang and T. Kanade, Determining the Epipolar Geometry and its +// Uncertainty: A Review, International Journal of Computer Vision, 1998. +// http://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.33.4540 +class FundamentalMatrixSevenPointEstimator { + public: + typedef Eigen::Vector2d X_t; + typedef Eigen::Vector2d Y_t; + typedef Eigen::Matrix3d M_t; + + // The minimum number of samples needed to estimate a model. + static const int kMinNumSamples = 7; + + // Estimate either 1 or 3 possible fundamental matrix solutions from a set of + // corresponding points. + // + // The number of corresponding points must be exactly 7. + // + // @param points1 First set of corresponding points. + // @param points2 Second set of corresponding points + // + // @return Up to 4 solutions as a vector of 3x3 fundamental matrices. + static std::vector Estimate(const std::vector& points1, + const std::vector& points2); + + // Calculate the residuals of a set of corresponding points and a given + // fundamental 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 F 3x3 fundamental matrix. + // @param residuals Output vector of residuals. + static void Residuals(const std::vector& points1, + const std::vector& points2, const M_t& F, + std::vector* residuals); +}; + +// Fundamental matrix estimator from corresponding point pairs. +// +// This algorithm solves the 8-Point problem based on the following paper: +// +// Hartley and Zisserman, Multiple View Geometry, algorithm 11.1, page 282. +class FundamentalMatrixEightPointEstimator { + public: + typedef Eigen::Vector2d X_t; + typedef Eigen::Vector2d Y_t; + typedef Eigen::Matrix3d M_t; + + // The minimum number of samples needed to estimate a model. + static const int kMinNumSamples = 8; + + // Estimate fundamental matrix solutions from a set of corresponding points. + // + // The number of corresponding points must be at least 8. + // + // @param points1 First set of corresponding points. + // @param points2 Second set of corresponding points + // + // @return Single solution as a vector of 3x3 fundamental matrices. + static std::vector Estimate(const std::vector& points1, + const std::vector& points2); + + // Calculate the residuals of a set of corresponding points and a given + // fundamental 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 F 3x3 fundamental matrix. + // @param residuals Output vector of residuals. + static void Residuals(const std::vector& points1, + const std::vector& points2, const M_t& F, + std::vector* residuals); +}; + +} // namespace colmap + +#endif // COLMAP_SRC_ESTIMATORS_FUNDAMENTAL_MATRIX_H_ diff --git a/include/colmap/estimators/generalized_absolute_pose.h b/include/colmap/estimators/generalized_absolute_pose.h new file mode 100644 index 0000000000000000000000000000000000000000..5b9e3991d1fc04c29bedb42be1d1790f7484212d --- /dev/null +++ b/include/colmap/estimators/generalized_absolute_pose.h @@ -0,0 +1,100 @@ +// Copyright (c) 2022, 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_GENERALIZED_ABSOLUTE_POSE_H_ +#define COLMAP_SRC_ESTIMATORS_GENERALIZED_ABSOLUTE_POSE_H_ + +#include + +#include + +#include "util/alignment.h" +#include "util/types.h" + +namespace colmap { + +// Solver for the Generalized P3P problem (NP3P or GP3P), based on: +// +// Lee, Gim Hee, et al. "Minimal solutions for pose estimation of a +// multi-camera system." Robotics Research. Springer International +// Publishing, 2016. 521-538. +// +// This class is based on an original implementation by Federico Camposeco. +class GP3PEstimator { + public: + // The generalized image observations, which is composed of the relative pose + // of the specific camera in the generalized camera and its image observation. + struct X_t { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + // The relative transformation from the generalized camera to the camera + // frame of the observation. + Eigen::Matrix3x4d rel_tform; + // The 2D image feature observation. + Eigen::Vector2d xy; + }; + + // The observed 3D feature points in the world frame. + typedef Eigen::Vector3d Y_t; + // The transformation from the world to the generalized camera frame. + typedef Eigen::Matrix3x4d M_t; + + // The minimum number of samples needed to estimate a model. + static const int kMinNumSamples = 3; + + enum class ResidualType { + CosineDistance, + ReprojectionError, + }; + + // Whether to compute the cosine similarity or the reprojection error. + // [WARNING] The reprojection error being in normalized coordinates, + // the unique error threshold of RANSAC corresponds to different pixel values + // in the different cameras of the rig if they have different intrinsics. + ResidualType residual_type = ResidualType::CosineDistance; + + // Estimate the most probable solution of the GP3P problem from a set of + // three 2D-3D point correspondences. + static std::vector Estimate(const std::vector& points2D, + const std::vector& points3D); + + // Calculate the squared cosine distance error between the rays given a set of + // 2D-3D point correspondences and a projection matrix of the generalized + // camera. + void Residuals(const std::vector& points2D, + const std::vector& points3D, + const M_t& proj_matrix, std::vector* residuals); +}; + +} // namespace colmap + +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(colmap::GP3PEstimator::X_t) + +#endif // COLMAP_SRC_ESTIMATORS_GENERALIZED_ABSOLUTE_POSE_H_ diff --git a/include/colmap/estimators/generalized_absolute_pose_coeffs.h b/include/colmap/estimators/generalized_absolute_pose_coeffs.h new file mode 100644 index 0000000000000000000000000000000000000000..1751ca16f9743de4bccb6a42a1465f5b51845fe2 --- /dev/null +++ b/include/colmap/estimators/generalized_absolute_pose_coeffs.h @@ -0,0 +1,44 @@ +// Copyright (c) 2022, 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_GENERALIZED_ABSOLUTE_POSE_COEFFS_H_ +#define COLMAP_SRC_ESTIMATORS_GENERALIZED_ABSOLUTE_POSE_COEFFS_H_ + +#include + +namespace colmap { + +Eigen::Matrix ComputeDepthsSylvesterCoeffs( + const Eigen::Matrix& K); + +} // namespace colmap + +#endif // COLMAP_SRC_ESTIMATORS_GENERALIZED_ABSOLUTE_POSE_COEFFS_H_ diff --git a/include/colmap/estimators/generalized_relative_pose.h b/include/colmap/estimators/generalized_relative_pose.h new file mode 100644 index 0000000000000000000000000000000000000000..eb42f122acd157f01ecce21636c9ea96224957b8 --- /dev/null +++ b/include/colmap/estimators/generalized_relative_pose.h @@ -0,0 +1,94 @@ +// Copyright (c) 2022, 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_GENERALIZED_RELATIVE_POSE_H_ +#define COLMAP_SRC_ESTIMATORS_GENERALIZED_RELATIVE_POSE_H_ + +#include + +#include + +#include "util/alignment.h" +#include "util/types.h" + +namespace colmap { + +// Solver for the Generalized Relative Pose problem using a minimal of 8 2D-2D +// correspondences. This implementation is based on: +// +// "Efficient Computation of Relative Pose for Multi-Camera Systems", +// Kneip and Li. CVPR 2014. +// +// Note that the solution to this problem is degenerate in the case of pure +// translation and when all correspondences are observed from the same cameras. +// +// The implementation is a modified and improved version of Kneip's original +// implementation in OpenGV licensed under the BSD license. +class GR6PEstimator { + public: + // The generalized image observations of the left camera, which is composed of + // the relative pose of the specific camera in the generalized camera and its + // image observation. + struct X_t { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + // The relative transformation from the generalized camera to the camera + // frame of the observation. + Eigen::Matrix3x4d rel_tform; + // The 2D image feature observation. + Eigen::Vector2d xy; + }; + + // The normalized image feature points in the left camera. + typedef X_t Y_t; + // The relative transformation between the two generalized cameras. + typedef Eigen::Matrix3x4d M_t; + + // The minimum number of samples needed to estimate a model. Note that in + // theory the minimum required number of samples is 6 but Laurent Kneip showed + // in his paper that using 8 samples is more stable. + static const int kMinNumSamples = 8; + + // Estimate the most probable solution of the GR6P problem from a set of + // six 2D-2D point correspondences. + static std::vector Estimate(const std::vector& points1, + const std::vector& points2); + + // Calculate the squared Sampson error between corresponding points. + static void Residuals(const std::vector& points1, + const std::vector& points2, const M_t& proj_matrix, + std::vector* residuals); +}; + +} // namespace colmap + +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(colmap::GR6PEstimator::X_t) + +#endif // COLMAP_SRC_ESTIMATORS_GENERALIZED_RELATIVE_POSE_H_ diff --git a/include/colmap/estimators/homography_matrix.h b/include/colmap/estimators/homography_matrix.h new file mode 100644 index 0000000000000000000000000000000000000000..c38122a8308de237ad251803c0e58ddd1f4cc3c4 --- /dev/null +++ b/include/colmap/estimators/homography_matrix.h @@ -0,0 +1,83 @@ +// Copyright (c) 2022, 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_HOMOGRAPHY_MATRIX_H_ +#define COLMAP_SRC_ESTIMATORS_HOMOGRAPHY_MATRIX_H_ + +#include + +#include + +#include "util/alignment.h" +#include "util/types.h" + +namespace colmap { + +// Direct linear transformation algorithm to compute the homography between +// point pairs. This algorithm computes the least squares estimate for +// the homography from at least 4 correspondences. +class HomographyMatrixEstimator { + public: + typedef Eigen::Vector2d X_t; + typedef Eigen::Vector2d Y_t; + typedef Eigen::Matrix3d M_t; + + // The minimum number of samples needed to estimate a model. + static const int kMinNumSamples = 4; + + // Estimate the projective transformation (homography). + // + // The number of corresponding points must be at least 4. + // + // @param points1 First set of corresponding points. + // @param points2 Second set of corresponding points. + // + // @return 3x3 homogeneous transformation matrix. + static std::vector Estimate(const std::vector& points1, + const std::vector& points2); + + // Calculate the transformation error for each corresponding point pair. + // + // Residuals are defined as the squared transformation error when + // transforming the source to the destination coordinates. + // + // @param points1 First set of corresponding points. + // @param points2 Second set of corresponding points. + // @param H 3x3 projective matrix. + // @param residuals Output vector of residuals. + static void Residuals(const std::vector& points1, + const std::vector& points2, const M_t& H, + std::vector* residuals); +}; + +} // namespace colmap + +#endif // COLMAP_SRC_ESTIMATORS_HOMOGRAPHY_MATRIX_H_ diff --git a/include/colmap/estimators/pose.h b/include/colmap/estimators/pose.h new file mode 100644 index 0000000000000000000000000000000000000000..ecc9775195008a5589597e6928535f4d6345b8da --- /dev/null +++ b/include/colmap/estimators/pose.h @@ -0,0 +1,234 @@ +// Copyright (c) 2022, 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_POSE_H_ +#define COLMAP_SRC_ESTIMATORS_POSE_H_ + +#include + +#include + +#include + +#include "base/camera.h" +#include "base/camera_models.h" +#include "optim/loransac.h" +#include "util/alignment.h" +#include "util/logging.h" +#include "util/threading.h" +#include "util/types.h" + +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 = ThreadPool::kMaxNumThreads; + + // 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(); + } +}; + +struct AbsolutePoseRefinementOptions { + // Convergence criterion. + double gradient_tolerance = 1.0; + + // Maximum number of solver iterations. + int max_num_iterations = 100; + + // Scaling factor determines at which residual robustification takes place. + double loss_function_scale = 1.0; + + // Whether to refine the focal length parameter group. + bool refine_focal_length = true; + + // Whether to refine the extra parameter group. + bool refine_extra_params = true; + + // Whether to print final summary. + bool print_summary = true; + + void Check() const { + CHECK_GE(gradient_tolerance, 0.0); + CHECK_GE(max_num_iterations, 0); + CHECK_GE(loss_function_scale, 0.0); + } +}; + +// Estimate absolute pose (optionally focal length) from 2D-3D correspondences. +// +// Focal length estimation is performed using discrete sampling around the +// focal length of the given camera. The focal length that results in the +// maximal number of inliers is assigned to the given camera. +// +// @param options Absolute pose estimation options. +// @param points2D Corresponding 2D points. +// @param points3D Corresponding 3D points. +// @param qvec Estimated rotation component as +// unit Quaternion coefficients (w, x, y, z). +// @param tvec Estimated translation component. +// @param camera Camera for which to estimate pose. Modified +// in-place to store the estimated focal length. +// @param num_inliers Number of inliers in RANSAC. +// @param inlier_mask Inlier mask for 2D-3D correspondences. +// +// @return Whether pose is estimated successfully. +bool EstimateAbsolutePose(const AbsolutePoseEstimationOptions& options, + const std::vector& points2D, + const std::vector& points3D, + Eigen::Vector4d* qvec, Eigen::Vector3d* tvec, + Camera* camera, size_t* num_inliers, + std::vector* inlier_mask); + +// Estimate relative from 2D-2D correspondences. +// +// Pose of first camera is assumed to be at the origin without rotation. Pose +// of second camera is given as world-to-image transformation, +// i.e. `x2 = [R | t] * X2`. +// +// @param ransac_options RANSAC options. +// @param points1 Corresponding 2D points. +// @param points2 Corresponding 2D points. +// @param qvec Estimated rotation component as +// unit Quaternion coefficients (w, x, y, z). +// @param tvec Estimated translation component. +// +// @return Number of RANSAC inliers. +size_t EstimateRelativePose(const RANSACOptions& ransac_options, + const std::vector& points1, + const std::vector& points2, + Eigen::Vector4d* qvec, Eigen::Vector3d* tvec); + +// Refine absolute pose (optionally focal length) from 2D-3D correspondences. +// +// @param options Refinement options. +// @param inlier_mask Inlier mask for 2D-3D correspondences. +// @param points2D Corresponding 2D points. +// @param points3D Corresponding 3D points. +// @param qvec Estimated rotation component as +// unit Quaternion coefficients (w, x, y, z). +// @param tvec Estimated translation component. +// @param camera Camera for which to estimate pose. Modified +// in-place to store the estimated focal length. +// @param rot_tvec_covariance Estimated 6x6 covariance matrix of +// the rotation (as axis-angle, in tangent space) +// and translation terms (optional). +// +// @return Whether the solution is usable. +bool RefineAbsolutePose(const AbsolutePoseRefinementOptions& options, + const std::vector& inlier_mask, + const std::vector& points2D, + const std::vector& points3D, + Eigen::Vector4d* qvec, Eigen::Vector3d* tvec, + Camera* camera, + Eigen::Matrix6d* rot_tvec_covariance = nullptr); + +// Refine relative pose of two cameras. +// +// Minimizes the Sampson error between corresponding normalized points using +// a robust cost function, i.e. the corresponding points need not necessarily +// be inliers given a sufficient initial guess for the relative pose. +// +// Assumes that first camera pose has projection matrix P = [I | 0], and +// pose of second camera is given as transformation from world to camera system. +// +// Assumes that the given translation vector is normalized, and refines +// the translation up to an unknown scale (i.e. refined translation vector +// is a unit vector again). +// +// @param options Solver options. +// @param points1 First set of corresponding points. +// @param points2 Second set of corresponding points. +// @param qvec Unit Quaternion rotation coefficients (w, x, y, z). +// @param tvec 3x1 translation vector. +// +// @return Flag indicating if solution is usable. +bool RefineRelativePose(const ceres::Solver::Options& options, + const std::vector& points1, + const std::vector& points2, + Eigen::Vector4d* qvec, Eigen::Vector3d* tvec); + +// Refine generalized absolute pose (optionally focal lengths) +// from 2D-3D correspondences. +// +// @param options Refinement options. +// @param inlier_mask Inlier mask for 2D-3D correspondences. +// @param points2D Corresponding 2D points. +// @param points3D Corresponding 3D points. +// @param camera_idxs Index of the rig camera for each correspondence. +// @param rig_qvecs Relative rotations from rig to each camera frame +// @param rig_tvecs Relative translations from rig +// to each camera frame. +// @param qvec Estimated rotation component of the rig as +// unit Quaternion coefficients (w, x, y, z). +// @param tvec Estimated translation component of the rig. +// @param cameras Cameras for which to estimate pose. Modified +// in-place to store the estimated focal lengths. +// @param rot_tvec_covariance Estimated 6x6 covariance matrix of +// the rotation (as axis-angle, in tangent space) +// and translation terms (optional). +// +// @return Whether the solution is usable. +bool RefineGeneralizedAbsolutePose( + const AbsolutePoseRefinementOptions& options, + const std::vector& inlier_mask, + const std::vector& points2D, + const std::vector& points3D, + const std::vector& camera_idxs, + const std::vector& rig_qvecs, + const std::vector& rig_tvecs, Eigen::Vector4d* qvec, + Eigen::Vector3d* tvec, std::vector* cameras, + Eigen::Matrix6d* rot_tvec_covariance = nullptr); + +} // namespace colmap + +#endif // COLMAP_SRC_ESTIMATORS_POSE_H_ diff --git a/include/colmap/estimators/similarity_transform.h b/include/colmap/estimators/similarity_transform.h new file mode 100644 index 0000000000000000000000000000000000000000..d08a47888ccd2606bbe893ead08aad41e4286126 --- /dev/null +++ b/include/colmap/estimators/similarity_transform.h @@ -0,0 +1,138 @@ +// Copyright (c) 2022, 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_SIMILARITY_TRANSFORM_H_ +#define COLMAP_SRC_ESTIMATORS_SIMILARITY_TRANSFORM_H_ + +#include + +#include +#include + +#include "base/projection.h" +#include "util/alignment.h" +#include "util/logging.h" +#include "util/types.h" + +namespace colmap { + +// N-D similarity transform estimator from corresponding point pairs in the +// source and destination coordinate systems. +// +// This algorithm is based on the following paper: +// +// S. Umeyama. Least-Squares Estimation of Transformation Parameters +// Between Two Point Patterns. IEEE Transactions on Pattern Analysis and +// Machine Intelligence, Volume 13 Issue 4, Page 376-380, 1991. +// http://www.stanford.edu/class/cs273/refs/umeyama.pdf +// +// and uses the Eigen implementation. +template +class SimilarityTransformEstimator { + public: + typedef Eigen::Matrix X_t; + typedef Eigen::Matrix Y_t; + typedef Eigen::Matrix M_t; + + // The minimum number of samples needed to estimate a model. Note that + // this only returns the true minimal sample in the two-dimensional case. + // For higher dimensions, the system will alway be over-determined. + static const int kMinNumSamples = kDim; + + // Estimate the similarity transform. + // + // @param src Set of corresponding source points. + // @param dst Set of corresponding destination points. + // + // @return 4x4 homogeneous transformation matrix. + static std::vector Estimate(const std::vector& src, + const std::vector& dst); + + // Calculate the transformation error for each corresponding point pair. + // + // Residuals are defined as the squared transformation error when + // transforming the source to the destination coordinates. + // + // @param src Set of corresponding points in the source coordinate + // system as a Nx3 matrix. + // @param dst Set of corresponding points in the destination + // coordinate system as a Nx3 matrix. + // @param matrix 4x4 homogeneous transformation matrix. + // @param residuals Output vector of residuals for each point pair. + static void Residuals(const std::vector& src, + const std::vector& dst, const M_t& matrix, + std::vector* residuals); +}; + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +template +std::vector::M_t> +SimilarityTransformEstimator::Estimate( + const std::vector& src, const std::vector& dst) { + CHECK_EQ(src.size(), dst.size()); + + Eigen::Matrix src_mat(kDim, src.size()); + Eigen::Matrix dst_mat(kDim, dst.size()); + for (size_t i = 0; i < src.size(); ++i) { + src_mat.col(i) = src[i]; + dst_mat.col(i) = dst[i]; + } + + const M_t model = Eigen::umeyama(src_mat, dst_mat, kEstimateScale) + .topLeftCorner(kDim, kDim + 1); + + if (model.array().isNaN().any()) { + return std::vector{}; + } + + return {model}; +} + +template +void SimilarityTransformEstimator::Residuals( + const std::vector& src, const std::vector& dst, const M_t& matrix, + std::vector* residuals) { + CHECK_EQ(src.size(), dst.size()); + + residuals->resize(src.size()); + + for (size_t i = 0; i < src.size(); ++i) { + const Y_t dst_transformed = matrix * src[i].homogeneous(); + (*residuals)[i] = (dst[i] - dst_transformed).squaredNorm(); + } +} + +} // namespace colmap + +#endif // COLMAP_SRC_ESTIMATORS_SIMILARITY_TRANSFORM_H_ diff --git a/include/colmap/estimators/translation_transform.h b/include/colmap/estimators/translation_transform.h new file mode 100644 index 0000000000000000000000000000000000000000..97086e6966c2378d1077231dd77e3f155dfdacc7 --- /dev/null +++ b/include/colmap/estimators/translation_transform.h @@ -0,0 +1,120 @@ +// Copyright (c) 2022, 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_SIMILARITY_TRANSFORM_H_ +#define COLMAP_SRC_BASE_SIMILARITY_TRANSFORM_H_ + +#include + +#include +#include + +#include "util/alignment.h" +#include "util/logging.h" +#include "util/types.h" + +namespace colmap { + +// Estimate a N-D translation transformation between point pairs. +template +class TranslationTransformEstimator { + public: + typedef Eigen::Matrix X_t; + typedef Eigen::Matrix Y_t; + typedef Eigen::Matrix M_t; + + // The minimum number of samples needed to estimate a model. + static const int kMinNumSamples = 1; + + // Estimate the 2D translation transform. + // + // @param points1 Set of corresponding source 2D points. + // @param points2 Set of corresponding destination 2D points. + // + // @return Translation vector. + static std::vector Estimate(const std::vector& points1, + const std::vector& points2); + + // Calculate the squared translation error. + // + // @param points1 Set of corresponding source 2D points. + // @param points2 Set of corresponding destination 2D points. + // @param translation Translation vector. + // @param residuals Output vector of residuals for each point pair. + static void Residuals(const std::vector& points1, + const std::vector& points2, const M_t& translation, + std::vector* residuals); +}; + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +template +std::vector::M_t> +TranslationTransformEstimator::Estimate(const std::vector& points1, + const std::vector& points2) { + CHECK_EQ(points1.size(), points2.size()); + + X_t mean_src = X_t::Zero(); + Y_t mean_dst = Y_t::Zero(); + + for (size_t i = 0; i < points1.size(); ++i) { + mean_src += points1[i]; + mean_dst += points2[i]; + } + + mean_src /= points1.size(); + mean_dst /= points2.size(); + + std::vector models(1); + models[0] = mean_dst - mean_src; + + return models; +} + +template +void TranslationTransformEstimator::Residuals( + const std::vector& points1, const std::vector& points2, + const M_t& translation, std::vector* residuals) { + CHECK_EQ(points1.size(), points2.size()); + + residuals->resize(points1.size()); + + for (size_t i = 0; i < points1.size(); ++i) { + const M_t diff = points2[i] - points1[i] - translation; + (*residuals)[i] = diff.squaredNorm(); + } +} + +} // namespace colmap + +#endif // COLMAP_SRC_BASE_SIMILARITY_TRANSFORM_H_ diff --git a/include/colmap/estimators/triangulation.h b/include/colmap/estimators/triangulation.h new file mode 100644 index 0000000000000000000000000000000000000000..debc98a7c67d7e6edd1e25d9439d1777881a8253 --- /dev/null +++ b/include/colmap/estimators/triangulation.h @@ -0,0 +1,156 @@ +// Copyright (c) 2022, 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 "base/camera.h" + +#include + +#include + +#include "optim/ransac.h" +#include "util/alignment.h" +#include "util/math.h" +#include "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/include/colmap/estimators/two_view_geometry.h b/include/colmap/estimators/two_view_geometry.h new file mode 100644 index 0000000000000000000000000000000000000000..9f3e1efd3109f41dcf74e8cba15c755527b96dd1 --- /dev/null +++ b/include/colmap/estimators/two_view_geometry.h @@ -0,0 +1,272 @@ +// Copyright (c) 2022, 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_TWO_VIEW_GEOMETRY_H_ +#define COLMAP_SRC_ESTIMATORS_TWO_VIEW_GEOMETRY_H_ + +#include "base/camera.h" +#include "feature/types.h" +#include "optim/ransac.h" +#include "util/alignment.h" +#include "util/logging.h" + +namespace colmap { + +// Two-view geometry estimator. +struct TwoViewGeometry { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + // The configuration of the estimated two-view geometry. + enum ConfigurationType { + UNDEFINED = 0, + // Degenerate configuration (e.g., no overlap or not enough inliers). + DEGENERATE = 1, + // Essential matrix. + CALIBRATED = 2, + // Fundamental matrix. + UNCALIBRATED = 3, + // Homography, planar scene with baseline. + PLANAR = 4, + // Homography, pure rotation without baseline. + PANORAMIC = 5, + // Homography, planar or panoramic. + PLANAR_OR_PANORAMIC = 6, + // Watermark, pure 2D translation in image borders. + WATERMARK = 7, + // Multi-model configuration, i.e. the inlier matches result from multiple + // individual, non-degenerate configurations. + MULTIPLE = 8, + }; + + // Estimation options. + struct Options { + // Minimum number of inliers for non-degenerate two-view geometry. + size_t min_num_inliers = 15; + + // In case both cameras are calibrated, the calibration is verified by + // estimating an essential and fundamental matrix and comparing their + // fractions of number of inliers. If the essential matrix produces + // a similar number of inliers (`min_E_F_inlier_ratio * F_num_inliers`), + // the calibration is assumed to be correct. + double min_E_F_inlier_ratio = 0.95; + + // In case an epipolar geometry can be verified, it is checked whether + // the geometry describes a planar scene or panoramic view (pure rotation) + // described by a homography. This is a degenerate case, since epipolar + // geometry is only defined for a moving camera. If the inlier ratio of + // a homography comes close to the inlier ratio of the epipolar geometry, + // a planar or panoramic configuration is assumed. + double max_H_inlier_ratio = 0.8; + + // In case of valid two-view geometry, it is checked whether the geometry + // describes a pure translation in the border region of the image. If more + // than a certain ratio of inlier points conform with a pure image + // translation, a watermark is assumed. + double watermark_min_inlier_ratio = 0.7; + + // Watermark matches have to be in the border region of the image. The + // border region is defined as the area around the image borders and + // is defined as a fraction of the image diagonal. + double watermark_border_size = 0.1; + + // Whether to enable watermark detection. A watermark causes a pure + // translation in the image space with inliers in the border region. + bool detect_watermark = true; + + // Whether to ignore watermark models in multiple model estimation. + bool multiple_ignore_watermark = true; + + // In case the user asks for it, only going to estimate a Homography + // between both cameras. + bool force_H_use = false; + + // Options used to robustly estimate the geometry. + RANSACOptions ransac_options; + + void Check() const { + CHECK_GE(min_num_inliers, 0); + CHECK_GE(min_E_F_inlier_ratio, 0); + CHECK_LE(min_E_F_inlier_ratio, 1); + CHECK_GE(max_H_inlier_ratio, 0); + CHECK_LE(max_H_inlier_ratio, 1); + CHECK_GE(watermark_min_inlier_ratio, 0); + CHECK_LE(watermark_min_inlier_ratio, 1); + CHECK_GE(watermark_border_size, 0); + CHECK_LE(watermark_border_size, 1); + ransac_options.Check(); + } + }; + + TwoViewGeometry() + : config(ConfigurationType::UNDEFINED), + E(Eigen::Matrix3d::Zero()), + F(Eigen::Matrix3d::Zero()), + H(Eigen::Matrix3d::Zero()), + qvec(Eigen::Vector4d::Zero()), + tvec(Eigen::Vector3d::Zero()), + tri_angle(0) {} + + // Invert the two-view geometry in-place. + void Invert(); + + // Estimate two-view geometry from calibrated or uncalibrated image pair, + // depending on whether a prior focal length is given or not. + // + // @param camera1 Camera of first image. + // @param points1 Feature points in first image. + // @param camera2 Camera of second image. + // @param points2 Feature points in second image. + // @param matches Feature matches between first and second image. + // @param options Two-view geometry estimation options. + void Estimate(const Camera& camera1, + const std::vector& points1, + const Camera& camera2, + const std::vector& points2, + const FeatureMatches& matches, const Options& options); + + // Recursively estimate multiple configurations by removing the previous set + // of inliers from the matches until not enough inliers are found. Inlier + // matches are concatenated and the configuration type is `MULTIPLE` if + // multiple models could be estimated. This is useful to estimate the two-view + // geometry for images with large distortion or multiple rigidly moving + // objects in the scene. + // + // Note that in case the model type is `MULTIPLE`, only the `inlier_matches` + // field will be initialized. + // + // @param camera1 Camera of first image. + // @param points1 Feature points in first image. + // @param camera2 Camera of second image. + // @param points2 Feature points in second image. + // @param matches Feature matches between first and second image. + // @param options Two-view geometry estimation options. + void EstimateMultiple(const Camera& camera1, + const std::vector& points1, + const Camera& camera2, + const std::vector& points2, + const FeatureMatches& matches, const Options& options); + + // Estimate two-view geometry and its relative pose from a calibrated or an + // uncalibrated image pair. + // + // @param camera1 Camera of first image. + // @param points1 Feature points in first image. + // @param camera2 Camera of second image. + // @param points2 Feature points in second image. + // @param matches Feature matches between first and second image. + // @param options Two-view geometry estimation options. + bool EstimateRelativePose(const Camera& camera1, + const std::vector& points1, + const Camera& camera2, + const std::vector& points2); + + // Estimate two-view geometry from calibrated image pair. + // + // @param camera1 Camera of first image. + // @param points1 Feature points in first image. + // @param camera2 Camera of second image. + // @param points2 Feature points in second image. + // @param matches Feature matches between first and second image. + // @param options Two-view geometry estimation options. + void EstimateCalibrated(const Camera& camera1, + const std::vector& points1, + const Camera& camera2, + const std::vector& points2, + const FeatureMatches& matches, + const Options& options); + + // Estimate two-view geometry from uncalibrated image pair. + // + // @param camera1 Camera of first image. + // @param points1 Feature points in first image. + // @param camera2 Camera of second image. + // @param points2 Feature points in second image. + // @param matches Feature matches between first and second image. + // @param options Two-view geometry estimation options. + void EstimateUncalibrated(const Camera& camera1, + const std::vector& points1, + const Camera& camera2, + const std::vector& points2, + const FeatureMatches& matches, + const Options& options); + + // Estimate two-view geometry using a Homography, + // depending on the option was user specified or not. + // + // @param camera1 Camera of first image. + // @param points1 Feature points in first image. + // @param camera2 Camera of second image. + // @param points2 Feature points in second image. + // @param matches Feature matches between first and second image. + // @param options Two-view geometry estimation options. + void EstimateHomography(const Camera& camera1, + const std::vector& points1, + const Camera& camera2, + const std::vector& points2, + const FeatureMatches& matches, + const Options& options); + + // Detect if inlier matches are caused by a watermark. + // A watermark causes a pure translation in the border are of the image. + static bool DetectWatermark(const Camera& camera1, + const std::vector& points1, + const Camera& camera2, + const std::vector& points2, + const size_t num_inliers, + const std::vector& inlier_mask, + const Options& options); + + // One of `ConfigurationType`. + int config; + + // Essential matrix. + Eigen::Matrix3d E; + // Fundamental matrix. + Eigen::Matrix3d F; + // Homography matrix. + Eigen::Matrix3d H; + + // Relative pose. + Eigen::Vector4d qvec; + Eigen::Vector3d tvec; + + // Inlier matches of the configuration. + FeatureMatches inlier_matches; + + // Median triangulation angle. + double tri_angle; +}; + +} // namespace colmap + +EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(colmap::TwoViewGeometry) + +#endif // COLMAP_SRC_ESTIMATORS_TWO_VIEW_GEOMETRY_H_ diff --git a/include/colmap/estimators/utils.h b/include/colmap/estimators/utils.h new file mode 100644 index 0000000000000000000000000000000000000000..6e83edac1f1bc592162dc0c1812405ee7e6bb5d9 --- /dev/null +++ b/include/colmap/estimators/utils.h @@ -0,0 +1,92 @@ +// Copyright (c) 2022, 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 "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/include/colmap/exe/database.h b/include/colmap/exe/database.h new file mode 100644 index 0000000000000000000000000000000000000000..72a905b192ef6f81ecd306c5dd72f1d2cac94131 --- /dev/null +++ b/include/colmap/exe/database.h @@ -0,0 +1,38 @@ +// Copyright (c) 2022, 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) + +namespace colmap { + +int RunDatabaseCleaner(int argc, char** argv); +int RunDatabaseCreator(int argc, char** argv); +int RunDatabaseMerger(int argc, char** argv); + +} // namespace colmap diff --git a/include/colmap/exe/feature.h b/include/colmap/exe/feature.h new file mode 100644 index 0000000000000000000000000000000000000000..c468d67161eb996c81f23193e14532ef7960a8ef --- /dev/null +++ b/include/colmap/exe/feature.h @@ -0,0 +1,86 @@ +// Copyright (c) 2022, 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_EXE_FEATURE_H_ +#define COLMAP_SRC_EXE_FEATURE_H_ + +#include "base/image_reader.h" + +namespace colmap { + +// This enum can be used as optional input for feature_extractor and +// feature_importer to ensure that the camera flags of ImageReader are set in an +// exclusive and unambigous way. The table below explains the corespondence of +// each setting with the flags +// +// ----------------------------------------------------------------------------------- +// | | ImageReaderOptions | +// | CameraMode | single_camera | single_camera_per_folder | single_camera_per_image | +// |------------|---------------|--------------------------|-------------------------| +// | AUTO | false | false | false | +// | SINGLE | true | false | false | +// | PER_FOLDER | false | true | false | +// | PER_IMAGE | false | false | true | +// ----------------------------------------------------------------------------------- +// +// Note: When using AUTO mode a camera model will be uniquely identified by the +// following 5 parameters from EXIF tags: +// 1. Camera Make +// 2. Camera Model +// 3. Focal Length +// 4. Image Width +// 5. Image Height +// +// If any of the tags is missing then a camera model is considered invalid and a +// new camera is created similar to the PER_IMAGE mode. +// +// If these considered fields are not sufficient to uniquely identify a camera +// then using the AUTO mode will lead to incorrect setup for the cameras, e.g. +// the same camera is used with same focal length but different principal point +// between captures. In these cases it is recommended to either use the +// PER_FOLDER or PER_IMAGE settings. +enum class CameraMode { AUTO = 0, SINGLE = 1, PER_FOLDER = 2, PER_IMAGE = 3 }; + +void UpdateImageReaderOptionsFromCameraMode(ImageReaderOptions& options, + CameraMode mode); + +int RunFeatureExtractor(int argc, char** argv); +int RunFeatureImporter(int argc, char** argv); +int RunExhaustiveMatcher(int argc, char** argv); +int RunMatchesImporter(int argc, char** argv); +int RunSequentialMatcher(int argc, char** argv); +int RunSpatialMatcher(int argc, char** argv); +int RunTransitiveMatcher(int argc, char** argv); +int RunVocabTreeMatcher(int argc, char** argv); + +} // namespace colmap + +#endif // COLMAP_SRC_EXE_FEATURE_H_ diff --git a/include/colmap/exe/gui.h b/include/colmap/exe/gui.h new file mode 100644 index 0000000000000000000000000000000000000000..410b0337719a00927c32b98ba05baa8fa951f876 --- /dev/null +++ b/include/colmap/exe/gui.h @@ -0,0 +1,56 @@ +// Copyright (c) 2022, 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 + +#ifdef GUI_ENABLED +#include +#include "ui/main_window.h" +#else +// Dummy QApplication class when GUI is disabled +class QApplication { + public: + QApplication(int argc, char** argv) {} +}; +#endif + +namespace colmap { + +#if defined(CUDA_ENABLED) || !defined(OPENGL_ENABLED) +const bool kUseOpenGL = false; +#else +const bool kUseOpenGL = true; +#endif + +int RunGraphicalUserInterface(int argc, char** argv); +int RunProjectGenerator(int argc, char** argv); + +} // namespace colmap diff --git a/include/colmap/exe/image.h b/include/colmap/exe/image.h new file mode 100644 index 0000000000000000000000000000000000000000..46807c101058159d121300b010b93936cef19a96 --- /dev/null +++ b/include/colmap/exe/image.h @@ -0,0 +1,41 @@ +// Copyright (c) 2022, 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) + +namespace colmap { + +int RunImageDeleter(int argc, char** argv); +int RunImageFilterer(int argc, char** argv); +int RunImageRectifier(int argc, char** argv); +int RunImageRegistrator(int argc, char** argv); +int RunImageUndistorter(int argc, char** argv); +int RunImageUndistorterStandalone(int argc, char** argv); + +} // namespace colmap diff --git a/include/colmap/exe/model.h b/include/colmap/exe/model.h new file mode 100644 index 0000000000000000000000000000000000000000..d730bed2c7d2a2f6d9cee1d38e456f0e7b71348b --- /dev/null +++ b/include/colmap/exe/model.h @@ -0,0 +1,44 @@ +// Copyright (c) 2022, 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) + +namespace colmap { + +int RunModelAligner(int argc, char** argv); +int RunModelAnalyzer(int argc, char** argv); +int RunModelComparer(int argc, char** argv); +int RunModelConverter(int argc, char** argv); +int RunModelCropper(int argc, char** argv); +int RunModelMerger(int argc, char** argv); +int RunModelOrientationAligner(int argc, char** argv); +int RunModelSplitter(int argc, char** argv); +int RunModelTransformer(int argc, char** argv); + +} // namespace colmap diff --git a/include/colmap/exe/mvs.h b/include/colmap/exe/mvs.h new file mode 100644 index 0000000000000000000000000000000000000000..ad511b6ebeea16471bce9b9c6c2714ba51e0ec19 --- /dev/null +++ b/include/colmap/exe/mvs.h @@ -0,0 +1,39 @@ +// Copyright (c) 2022, 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) + +namespace colmap { + +int RunDelaunayMesher(int argc, char** argv); +int RunPatchMatchStereo(int argc, char** argv); +int RunPoissonMesher(int argc, char** argv); +int RunStereoFuser(int argc, char** argv); + +} // namespace colmap diff --git a/include/colmap/exe/sfm.h b/include/colmap/exe/sfm.h new file mode 100644 index 0000000000000000000000000000000000000000..56490e4783ac0c306159fbfeb198b13e4914bc42 --- /dev/null +++ b/include/colmap/exe/sfm.h @@ -0,0 +1,58 @@ +// Copyright (c) 2022, 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_EXE_SFM_H_ +#define COLMAP_SRC_EXE_SFM_H_ + +#include "base/reconstruction.h" +#include "controllers/incremental_mapper.h" + +namespace colmap { + +int RunPointTriangulatorImpl(Reconstruction& reconstruction, + const std::string database_path, + const std::string image_path, + const std::string output_path, + const IncrementalMapperOptions& mapper_options, + const bool clear_points); + +int RunAutomaticReconstructor(int argc, char** argv); +int RunBundleAdjuster(int argc, char** argv); +int RunColorExtractor(int argc, char** argv); +int RunMapper(int argc, char** argv); +int RunHierarchicalMapper(int argc, char** argv); +int RunPointFiltering(int argc, char** argv); +int RunPointTriangulator(int argc, char** argv); +int RunRigBundleAdjuster(int argc, char** argv); + +} // namespace colmap + +#endif // COLMAP_SRC_EXE_SFM_H_ diff --git a/include/colmap/exe/vocab_tree.h b/include/colmap/exe/vocab_tree.h new file mode 100644 index 0000000000000000000000000000000000000000..0289d6b043ba60bc5c1242700c7652ca352dd0e5 --- /dev/null +++ b/include/colmap/exe/vocab_tree.h @@ -0,0 +1,37 @@ +// Copyright (c) 2022, 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) + +namespace colmap { + +int RunVocabTreeBuilder(int argc, char** argv); +int RunVocabTreeRetriever(int argc, char** argv); + +} // namespace colmap diff --git a/include/colmap/feature/extraction.h b/include/colmap/feature/extraction.h new file mode 100644 index 0000000000000000000000000000000000000000..26e35544d13d778f659ba27640281e57d7ac955e --- /dev/null +++ b/include/colmap/feature/extraction.h @@ -0,0 +1,155 @@ +// Copyright (c) 2022, 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_FEATURE_EXTRACTION_H_ +#define COLMAP_SRC_FEATURE_EXTRACTION_H_ + +#include "base/database.h" +#include "base/image_reader.h" +#include "feature/sift.h" +#include "util/opengl_utils.h" +#include "util/threading.h" + +namespace colmap { + +namespace internal { + +struct ImageData; + +} // namespace internal + +// Feature extraction class to extract features for all images in a directory. +class SiftFeatureExtractor : public Thread { + public: + SiftFeatureExtractor(const ImageReaderOptions& reader_options, + const SiftExtractionOptions& sift_options); + + private: + void Run(); + + const ImageReaderOptions reader_options_; + const SiftExtractionOptions sift_options_; + + Database database_; + ImageReader image_reader_; + + std::vector> resizers_; + std::vector> extractors_; + std::unique_ptr writer_; + + std::unique_ptr> resizer_queue_; + std::unique_ptr> extractor_queue_; + std::unique_ptr> writer_queue_; +}; + +// Import features from text files. Each image must have a corresponding text +// file with the same name and an additional ".txt" suffix. +class FeatureImporter : public Thread { + public: + FeatureImporter(const ImageReaderOptions& reader_options, + const std::string& import_path); + + private: + void Run(); + + const ImageReaderOptions reader_options_; + const std::string import_path_; +}; + +//////////////////////////////////////////////////////////////////////////////// +// Implementation +//////////////////////////////////////////////////////////////////////////////// + +namespace internal { + +struct ImageData { + ImageReader::Status status = ImageReader::Status::FAILURE; + + Camera camera; + Image image; + Bitmap bitmap; + Bitmap mask; + + FeatureKeypoints keypoints; + FeatureDescriptors descriptors; +}; + +class ImageResizerThread : public Thread { + public: + ImageResizerThread(const int max_image_size, JobQueue* input_queue, + JobQueue* output_queue); + + private: + void Run(); + + const int max_image_size_; + + JobQueue* input_queue_; + JobQueue* output_queue_; +}; + +class SiftFeatureExtractorThread : public Thread { + public: + SiftFeatureExtractorThread(const SiftExtractionOptions& sift_options, + const std::shared_ptr& camera_mask, + JobQueue* input_queue, + JobQueue* output_queue); + + private: + void Run(); + + const SiftExtractionOptions sift_options_; + std::shared_ptr camera_mask_; + + std::unique_ptr opengl_context_; + + JobQueue* input_queue_; + JobQueue* output_queue_; +}; + +class FeatureWriterThread : public Thread { + public: + FeatureWriterThread(const size_t num_images, Database* database, + JobQueue* input_queue); + + private: + void Run(); + + const size_t num_images_; + Database* database_; + JobQueue* input_queue_; +}; + +} // namespace internal + +} // namespace colmap + +#endif // COLMAP_SRC_FEATURE_EXTRACTION_H_ diff --git a/include/colmap/feature/matching.h b/include/colmap/feature/matching.h new file mode 100644 index 0000000000000000000000000000000000000000..20efb8606c8f4cde5b225327634f66e5e42ec544 --- /dev/null +++ b/include/colmap/feature/matching.h @@ -0,0 +1,569 @@ +// Copyright (c) 2022, 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_FEATURE_MATCHING_H_ +#define COLMAP_SRC_FEATURE_MATCHING_H_ + +#include +#include +#include + +#include "base/database.h" +#include "feature/sift.h" +#include "util/alignment.h" +#include "util/cache.h" +#include "util/opengl_utils.h" +#include "util/threading.h" +#include "util/timer.h" + +namespace colmap { + +struct ExhaustiveMatchingOptions { + // Block size, i.e. number of images to simultaneously load into memory. + int block_size = 50; + + bool Check() const; +}; + +struct SequentialMatchingOptions { + // Number of overlapping image pairs. + int overlap = 10; + + // Whether to match images against their quadratic neighbors. + bool quadratic_overlap = true; + + // Whether to enable vocabulary tree based loop detection. + bool loop_detection = false; + + // Loop detection is invoked every `loop_detection_period` images. + int loop_detection_period = 10; + + // The number of images to retrieve in loop detection. This number should + // be significantly bigger than the sequential matching overlap. + int loop_detection_num_images = 50; + + // Number of nearest neighbors to retrieve per query feature. + int loop_detection_num_nearest_neighbors = 1; + + // Number of nearest-neighbor checks to use in retrieval. + int loop_detection_num_checks = 256; + + // How many images to return after spatial verification. Set to 0 to turn off + // spatial verification. + int loop_detection_num_images_after_verification = 0; + + // The maximum number of features to use for indexing an image. If an + // image has more features, only the largest-scale features will be indexed. + int loop_detection_max_num_features = -1; + + // Path to the vocabulary tree. + std::string vocab_tree_path = ""; + + bool Check() const; +}; + +struct VocabTreeMatchingOptions { + // Number of images to retrieve for each query image. + int num_images = 100; + + // Number of nearest neighbors to retrieve per query feature. + int num_nearest_neighbors = 5; + + // Number of nearest-neighbor checks to use in retrieval. + int num_checks = 256; + + // How many images to return after spatial verification. Set to 0 to turn off + // spatial verification. + int num_images_after_verification = 0; + + // The maximum number of features to use for indexing an image. If an + // image has more features, only the largest-scale features will be indexed. + int max_num_features = -1; + + // Path to the vocabulary tree. + std::string vocab_tree_path = ""; + + // Optional path to file with specific image names to match. + std::string match_list_path = ""; + + bool Check() const; +}; + +struct SpatialMatchingOptions { + // Whether the location priors in the database are GPS coordinates in + // the form of longitude and latitude coordinates in degrees. + bool is_gps = true; + + // Whether to ignore the Z-component of the location prior. + bool ignore_z = true; + + // The maximum number of nearest neighbors to match. + int max_num_neighbors = 50; + + // The maximum distance between the query and nearest neighbor. For GPS + // coordinates the unit is Euclidean distance in meters. + double max_distance = 100; + + bool Check() const; +}; + +struct TransitiveMatchingOptions { + // The maximum number of image pairs to process in one batch. + int batch_size = 1000; + + // The number of transitive closure iterations. + int num_iterations = 3; + + bool Check() const; +}; + +struct ImagePairsMatchingOptions { + // Number of image pairs to match in one batch. + int block_size = 1225; + + // Path to the file with the matches. + std::string match_list_path = ""; + + bool Check() const; +}; + +struct FeaturePairsMatchingOptions { + // Whether to geometrically verify the given matches. + bool verify_matches = true; + + // Path to the file with the matches. + std::string match_list_path = ""; + + bool Check() const; +}; + +namespace internal { + +struct FeatureMatcherData { + image_t image_id1 = kInvalidImageId; + image_t image_id2 = kInvalidImageId; + FeatureMatches matches; + TwoViewGeometry two_view_geometry; +}; + +} // namespace internal + +using FeatureKeypointsPtr = std::shared_ptr; +using FeatureDescriptorsPtr = std::shared_ptr; + +// Cache for feature matching to minimize database access during matching. +class FeatureMatcherCache { + public: + FeatureMatcherCache(const size_t cache_size, const Database* database); + + void Setup(); + + const Camera& GetCamera(const camera_t camera_id) const; + const Image& GetImage(const image_t image_id) const; + FeatureKeypointsPtr GetKeypoints(const image_t image_id); + FeatureDescriptorsPtr GetDescriptors(const image_t image_id); + FeatureMatches GetMatches(const image_t image_id1, const image_t image_id2); + std::vector GetImageIds() const; + + bool ExistsKeypoints(const image_t image_id); + bool ExistsDescriptors(const image_t image_id); + + bool ExistsMatches(const image_t image_id1, const image_t image_id2); + bool ExistsInlierMatches(const image_t image_id1, const image_t image_id2); + + void WriteMatches(const image_t image_id1, const image_t image_id2, + const FeatureMatches& matches); + void WriteTwoViewGeometry(const image_t image_id1, const image_t image_id2, + const TwoViewGeometry& two_view_geometry); + + void DeleteMatches(const image_t image_id1, const image_t image_id2); + void DeleteInlierMatches(const image_t image_id1, const image_t image_id2); + + private: + const size_t cache_size_; + const Database* database_; + std::mutex database_mutex_; + EIGEN_STL_UMAP(camera_t, Camera) cameras_cache_; + EIGEN_STL_UMAP(image_t, Image) images_cache_; + std::unique_ptr> keypoints_cache_; + std::unique_ptr> descriptors_cache_; + std::unique_ptr> keypoints_exists_cache_; + std::unique_ptr> descriptors_exists_cache_; +}; + +class FeatureMatcherThread : public Thread { + public: + FeatureMatcherThread(const SiftMatchingOptions& options, + FeatureMatcherCache* cache); + + void SetMaxNumMatches(const int max_num_matches); + + protected: + SiftMatchingOptions options_; + FeatureMatcherCache* cache_; +}; + +class SiftCPUFeatureMatcher : public FeatureMatcherThread { + public: + typedef internal::FeatureMatcherData Input; + typedef internal::FeatureMatcherData Output; + + SiftCPUFeatureMatcher(const SiftMatchingOptions& options, + FeatureMatcherCache* cache, + JobQueue* input_queue, + JobQueue* output_queue); + + protected: + void Run() override; + + JobQueue* input_queue_; + JobQueue* output_queue_; +}; + +class SiftGPUFeatureMatcher : public FeatureMatcherThread { + public: + typedef internal::FeatureMatcherData Input; + typedef internal::FeatureMatcherData Output; + + SiftGPUFeatureMatcher(const SiftMatchingOptions& options, + FeatureMatcherCache* cache, + JobQueue* input_queue, + JobQueue* output_queue); + + protected: + void Run() override; + + void GetDescriptorData(const int index, const image_t image_id, + const FeatureDescriptors** descriptors_ptr); + + JobQueue* input_queue_; + JobQueue* output_queue_; + + std::unique_ptr opengl_context_; + + // The previously uploaded images to the GPU. + std::array prev_uploaded_image_ids_; + std::array prev_uploaded_descriptors_; +}; + +class GuidedSiftCPUFeatureMatcher : public FeatureMatcherThread { + public: + typedef internal::FeatureMatcherData Input; + typedef internal::FeatureMatcherData Output; + + GuidedSiftCPUFeatureMatcher(const SiftMatchingOptions& options, + FeatureMatcherCache* cache, + JobQueue* input_queue, + JobQueue* output_queue); + + private: + void Run() override; + + JobQueue* input_queue_; + JobQueue* output_queue_; +}; + +class GuidedSiftGPUFeatureMatcher : public FeatureMatcherThread { + public: + typedef internal::FeatureMatcherData Input; + typedef internal::FeatureMatcherData Output; + + GuidedSiftGPUFeatureMatcher(const SiftMatchingOptions& options, + FeatureMatcherCache* cache, + JobQueue* input_queue, + JobQueue* output_queue); + + private: + void Run() override; + + void GetFeatureData(const int index, const image_t image_id, + const FeatureKeypoints** keypoints_ptr, + const FeatureDescriptors** descriptors_ptr); + + JobQueue* input_queue_; + JobQueue* output_queue_; + + std::unique_ptr opengl_context_; + + // The previously uploaded images to the GPU. + std::array prev_uploaded_image_ids_; + std::array prev_uploaded_keypoints_; + std::array prev_uploaded_descriptors_; +}; + +class TwoViewGeometryVerifier : public Thread { + public: + typedef internal::FeatureMatcherData Input; + typedef internal::FeatureMatcherData Output; + + TwoViewGeometryVerifier(const SiftMatchingOptions& options, + FeatureMatcherCache* cache, + JobQueue* input_queue, + JobQueue* output_queue); + + protected: + void Run() override; + + const SiftMatchingOptions options_; + TwoViewGeometry::Options two_view_geometry_options_; + FeatureMatcherCache* cache_; + JobQueue* input_queue_; + JobQueue* output_queue_; +}; + +// Multi-threaded and multi-GPU SIFT feature matcher, which writes the computed +// results to the database and skips already matched image pairs. To improve +// performance of the matching by taking advantage of caching and database +// transactions, pass multiple images to the `Match` function. Note that the +// database should be in an active transaction while calling `Match`. +class SiftFeatureMatcher { + public: + SiftFeatureMatcher(const SiftMatchingOptions& options, Database* database, + FeatureMatcherCache* cache); + + ~SiftFeatureMatcher(); + + // Setup the matchers and return if successful. + bool Setup(); + + // Match one batch of multiple image pairs. + void Match(const std::vector>& image_pairs); + + private: + SiftMatchingOptions options_; + Database* database_; + FeatureMatcherCache* cache_; + + bool is_setup_; + + std::vector> matchers_; + std::vector> guided_matchers_; + std::vector> verifiers_; + std::unique_ptr thread_pool_; + + JobQueue matcher_queue_; + JobQueue verifier_queue_; + JobQueue guided_matcher_queue_; + JobQueue output_queue_; +}; + +// Exhaustively match images by processing each block in the exhaustive match +// matrix in one batch: +// +// +----+----+-----------------> images[i] +// |#000|0000| +// |1#00|1000| <- Above the main diagonal, the block diagonal is not matched +// |11#0|1100| ^ +// |111#|1110| | +// +----+----+ | +// |1000|#000|\ | +// |1100|1#00| \ One block | +// |1110|11#0| / of image pairs | +// |1111|111#|/ | +// +----+----+ | +// | ^ | +// | | | +// | Below the main diagonal, the block diagonal is matched <--------------+ +// | +// v +// images[i] +// +// Pairs will only be matched if 1, to avoid duplicate pairs. Pairs with # +// are on the main diagonal and denote pairs of the same image. +class ExhaustiveFeatureMatcher : public Thread { + public: + ExhaustiveFeatureMatcher(const ExhaustiveMatchingOptions& options, + const SiftMatchingOptions& match_options, + const std::string& database_path); + + private: + void Run() override; + + const ExhaustiveMatchingOptions options_; + const SiftMatchingOptions match_options_; + Database database_; + FeatureMatcherCache cache_; + SiftFeatureMatcher matcher_; +}; + +// Sequentially match images within neighborhood: +// +// +-------------------------------+-----------------------> images[i] +// ^ | ^ +// | Current image[i] | +// | | | +// +----------+-----------+ +// | +// Match image_i against +// +// image_[i - o, i + o] with o = [1 .. overlap] +// image_[i - 2^o, i + 2^o] (for quadratic overlap) +// +// Sequential order is determined based on the image names in ascending order. +// +// Invoke loop detection if `(i mod loop_detection_period) == 0`, retrieve +// most similar `loop_detection_num_images` images from vocabulary tree, +// and perform matching and verification. +class SequentialFeatureMatcher : public Thread { + public: + SequentialFeatureMatcher(const SequentialMatchingOptions& options, + const SiftMatchingOptions& match_options, + const std::string& database_path); + + private: + void Run() override; + + std::vector GetOrderedImageIds() const; + void RunSequentialMatching(const std::vector& image_ids); + void RunLoopDetection(const std::vector& image_ids); + + const SequentialMatchingOptions options_; + const SiftMatchingOptions match_options_; + Database database_; + FeatureMatcherCache cache_; + SiftFeatureMatcher matcher_; +}; + +// Match each image against its nearest neighbors using a vocabulary tree. +class VocabTreeFeatureMatcher : public Thread { + public: + VocabTreeFeatureMatcher(const VocabTreeMatchingOptions& options, + const SiftMatchingOptions& match_options, + const std::string& database_path); + + private: + void Run() override; + + const VocabTreeMatchingOptions options_; + const SiftMatchingOptions match_options_; + Database database_; + FeatureMatcherCache cache_; + SiftFeatureMatcher matcher_; +}; + +// Match images against spatial nearest neighbors using prior location +// information, e.g. provided manually or extracted from EXIF. +class SpatialFeatureMatcher : public Thread { + public: + SpatialFeatureMatcher(const SpatialMatchingOptions& options, + const SiftMatchingOptions& match_options, + const std::string& database_path); + + private: + void Run() override; + + const SpatialMatchingOptions options_; + const SiftMatchingOptions match_options_; + Database database_; + FeatureMatcherCache cache_; + SiftFeatureMatcher matcher_; +}; + +// Match transitive image pairs in a database with existing feature matches. +// This matcher transitively closes loops. For example, if image pairs A-B and +// B-C match but A-C has not been matched, then this matcher attempts to match +// A-C. This procedure is performed for multiple iterations. +class TransitiveFeatureMatcher : public Thread { + public: + TransitiveFeatureMatcher(const TransitiveMatchingOptions& options, + const SiftMatchingOptions& match_options, + const std::string& database_path); + + private: + void Run() override; + + const TransitiveMatchingOptions options_; + const SiftMatchingOptions match_options_; + Database database_; + FeatureMatcherCache cache_; + SiftFeatureMatcher matcher_; +}; + +// Match images manually specified in a list of image pairs. +// +// Read matches file with the following format: +// +// image_name1 image_name2 +// image_name1 image_name3 +// image_name2 image_name3 +// ... +// +class ImagePairsFeatureMatcher : public Thread { + public: + ImagePairsFeatureMatcher(const ImagePairsMatchingOptions& options, + const SiftMatchingOptions& match_options, + const std::string& database_path); + + private: + void Run() override; + + const ImagePairsMatchingOptions options_; + const SiftMatchingOptions match_options_; + Database database_; + FeatureMatcherCache cache_; + SiftFeatureMatcher matcher_; +}; + +// Import feature matches from a text file. +// +// Read matches file with the following format: +// +// image_name1 image_name2 +// 0 1 +// 1 2 +// 2 3 +// +// image_name1 image_name3 +// 0 1 +// 1 2 +// 2 3 +// ... +// +class FeaturePairsFeatureMatcher : public Thread { + public: + FeaturePairsFeatureMatcher(const FeaturePairsMatchingOptions& options, + const SiftMatchingOptions& match_options, + const std::string& database_path); + + private: + const static size_t kCacheSize = 100; + + void Run() override; + + const FeaturePairsMatchingOptions options_; + const SiftMatchingOptions match_options_; + Database database_; + FeatureMatcherCache cache_; +}; + +} // namespace colmap + +#endif // COLMAP_SRC_FEATURE_MATCHING_H_ diff --git a/include/colmap/feature/sift.h b/include/colmap/feature/sift.h new file mode 100644 index 0000000000000000000000000000000000000000..8232c1748066523e8c16de11d8edc066aef8f9ca --- /dev/null +++ b/include/colmap/feature/sift.h @@ -0,0 +1,261 @@ +// Copyright (c) 2022, 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_FEATURE_SIFT_H_ +#define COLMAP_SRC_FEATURE_SIFT_H_ + +#include "estimators/two_view_geometry.h" +#include "feature/types.h" +#include "util/bitmap.h" + +class SiftGPU; +class SiftMatchGPU; + +namespace colmap { + +struct SiftExtractionOptions { + // Number of threads for feature extraction. + int num_threads = -1; + + // Whether to use the GPU for feature extraction. + bool use_gpu = true; + + // Index of the GPU used for feature extraction. For multi-GPU extraction, + // you should separate multiple GPU indices by comma, e.g., "0,1,2,3". + std::string gpu_index = "-1"; + + // Maximum image size, otherwise image will be down-scaled. + int max_image_size = 3200; + + // Maximum number of features to detect, keeping larger-scale features. + int max_num_features = 8192; + + // First octave in the pyramid, i.e. -1 upsamples the image by one level. + int first_octave = -1; + + // Number of octaves. + int num_octaves = 4; + + // Number of levels per octave. + int octave_resolution = 3; + + // Peak threshold for detection. + double peak_threshold = 0.02 / octave_resolution; + + // Edge threshold for detection. + double edge_threshold = 10.0; + + // Estimate affine shape of SIFT features in the form of oriented ellipses as + // opposed to original SIFT which estimates oriented disks. + bool estimate_affine_shape = false; + + // Maximum number of orientations per keypoint if not estimate_affine_shape. + int max_num_orientations = 2; + + // Fix the orientation to 0 for upright features. + bool upright = false; + + // Whether to adapt the feature detection depending on the image darkness. + // Note that this feature is only available in the OpenGL SiftGPU version. + bool darkness_adaptivity = false; + + // Domain-size pooling parameters. Domain-size pooling computes an average + // SIFT descriptor across multiple scales around the detected scale. This was + // proposed in "Domain-Size Pooling in Local Descriptors and Network + // Architectures", J. Dong and S. Soatto, CVPR 2015. This has been shown to + // outperform other SIFT variants and learned descriptors in "Comparative + // Evaluation of Hand-Crafted and Learned Local Features", Schönberger, + // Hardmeier, Sattler, Pollefeys, CVPR 2016. + bool domain_size_pooling = false; + double dsp_min_scale = 1.0 / 6.0; + double dsp_max_scale = 3.0; + int dsp_num_scales = 10; + + enum class Normalization { + // L1-normalizes each descriptor followed by element-wise square rooting. + // This normalization is usually better than standard L2-normalization. + // See "Three things everyone should know to improve object retrieval", + // Relja Arandjelovic and Andrew Zisserman, CVPR 2012. + L1_ROOT, + // Each vector is L2-normalized. + L2, + }; + Normalization normalization = Normalization::L1_ROOT; + + bool Check() const; +}; + +struct SiftMatchingOptions { + // Number of threads for feature matching and geometric verification. + int num_threads = -1; + + // Whether to use the GPU for feature matching. + bool use_gpu = true; + + // Index of the GPU used for feature matching. For multi-GPU matching, + // you should separate multiple GPU indices by comma, e.g., "0,1,2,3". + std::string gpu_index = "-1"; + + // Maximum distance ratio between first and second best match. + double max_ratio = 0.8; + + // Maximum distance to best match. + double max_distance = 0.7; + + // Whether to enable cross checking in matching. + bool cross_check = true; + + // Maximum number of matches. + int max_num_matches = 32768; + + // Maximum epipolar error in pixels for geometric verification. + double max_error = 4.0; + + // Confidence threshold for geometric verification. + double confidence = 0.999; + + // Minimum/maximum number of RANSAC iterations. Note that this option + // overrules the min_inlier_ratio option. + int min_num_trials = 100; + int max_num_trials = 10000; + + // A priori assumed minimum inlier ratio, which determines the maximum + // number of iterations. + double min_inlier_ratio = 0.25; + + // Minimum number of inliers for an image pair to be considered as + // geometrically verified. + int min_num_inliers = 15; + + // Whether to attempt to estimate multiple geometric models per image pair. + bool multiple_models = false; + + // Whether to perform guided matching, if geometric verification succeeds. + bool guided_matching = false; + + // Force Homography use for Two-view Geometry (can help for planar scenes) + bool planar_scene = false; + + bool Check() const; +}; + +// Extract SIFT features for the given image on the CPU. Only extract +// descriptors if the given input is not NULL. +bool ExtractSiftFeaturesCPU(const SiftExtractionOptions& options, + const Bitmap& bitmap, FeatureKeypoints* keypoints, + FeatureDescriptors* descriptors); +bool ExtractCovariantSiftFeaturesCPU(const SiftExtractionOptions& options, + const Bitmap& bitmap, + FeatureKeypoints* keypoints, + FeatureDescriptors* descriptors); + +// Create a SiftGPU feature extractor. The same SiftGPU instance can be used to +// extract features for multiple images. Note a OpenGL context must be made +// current in the thread of the caller. If the gpu_index is not -1, the CUDA +// version of SiftGPU is used, which produces slightly different results +// than the OpenGL implementation. +bool CreateSiftGPUExtractor(const SiftExtractionOptions& options, + SiftGPU* sift_gpu); + +// Extract SIFT features for the given image on the GPU. +// SiftGPU must already be initialized using `CreateSiftGPU`. +bool ExtractSiftFeaturesGPU(const SiftExtractionOptions& options, + const Bitmap& bitmap, SiftGPU* sift_gpu, + FeatureKeypoints* keypoints, + FeatureDescriptors* descriptors); + +// Load keypoints and descriptors from text file in the following format: +// +// LINE_0: NUM_FEATURES DIM +// LINE_1: X Y SCALE ORIENTATION D_1 D_2 D_3 ... D_DIM +// LINE_I: ... +// LINE_NUM_FEATURES: X Y SCALE ORIENTATION D_1 D_2 D_3 ... D_DIM +// +// where the first line specifies the number of features and the descriptor +// dimensionality followed by one line per feature: X, Y, SCALE, ORIENTATION are +// of type float and D_J represent the descriptor in the range [0, 255]. +// +// For example: +// +// 2 4 +// 0.32 0.12 1.23 1.0 1 2 3 4 +// 0.32 0.12 1.23 1.0 1 2 3 4 +// +void LoadSiftFeaturesFromTextFile(const std::string& path, + FeatureKeypoints* keypoints, + FeatureDescriptors* descriptors); + +// Match the given SIFT features on the CPU. +void MatchSiftFeaturesCPUBruteForce(const SiftMatchingOptions& match_options, + const FeatureDescriptors& descriptors1, + const FeatureDescriptors& descriptors2, + FeatureMatches* matches); +void MatchSiftFeaturesCPUFLANN(const SiftMatchingOptions& match_options, + const FeatureDescriptors& descriptors1, + const FeatureDescriptors& descriptors2, + FeatureMatches* matches); +void MatchSiftFeaturesCPU(const SiftMatchingOptions& match_options, + const FeatureDescriptors& descriptors1, + const FeatureDescriptors& descriptors2, + FeatureMatches* matches); +void MatchGuidedSiftFeaturesCPU(const SiftMatchingOptions& match_options, + const FeatureKeypoints& keypoints1, + const FeatureKeypoints& keypoints2, + const FeatureDescriptors& descriptors1, + const FeatureDescriptors& descriptors2, + TwoViewGeometry* two_view_geometry); + +// Create a SiftGPU feature matcher. Note that if CUDA is not available or the +// gpu_index is -1, the OpenGLContextManager must be created in the main thread +// of the Qt application before calling this function. The same SiftMatchGPU +// instance can be used to match features between multiple image pairs. +bool CreateSiftGPUMatcher(const SiftMatchingOptions& match_options, + SiftMatchGPU* sift_match_gpu); + +// Match the given SIFT features on the GPU. If either of the descriptors is +// NULL, the keypoints/descriptors will not be uploaded and the previously +// uploaded descriptors will be reused for the matching. +void MatchSiftFeaturesGPU(const SiftMatchingOptions& match_options, + const FeatureDescriptors* descriptors1, + const FeatureDescriptors* descriptors2, + SiftMatchGPU* sift_match_gpu, + FeatureMatches* matches); +void MatchGuidedSiftFeaturesGPU(const SiftMatchingOptions& match_options, + const FeatureKeypoints* keypoints1, + const FeatureKeypoints* keypoints2, + const FeatureDescriptors* descriptors1, + const FeatureDescriptors* descriptors2, + SiftMatchGPU* sift_match_gpu, + TwoViewGeometry* two_view_geometry); + +} // namespace colmap + +#endif // COLMAP_SRC_FEATURE_SIFT_H_ diff --git a/include/colmap/feature/types.h b/include/colmap/feature/types.h new file mode 100644 index 0000000000000000000000000000000000000000..6c8413560f875aff150b80089e1d24a036ebf04d --- /dev/null +++ b/include/colmap/feature/types.h @@ -0,0 +1,103 @@ +// Copyright (c) 2022, 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_FEATURE_TYPES_H_ +#define COLMAP_SRC_FEATURE_TYPES_H_ + +#include + +#include + +#include "util/types.h" + +namespace colmap { + +struct FeatureKeypoint { + FeatureKeypoint(); + FeatureKeypoint(const float x, const float y); + FeatureKeypoint(const float x, const float y, const float scale, + const float orientation); + FeatureKeypoint(const float x, const float y, const float a11, + const float a12, const float a21, const float a22); + + static FeatureKeypoint FromParameters(const float x, const float y, + const float scale_x, + const float scale_y, + const float orientation, + const float shear); + + // Rescale the feature location and shape size by the given scale factor. + void Rescale(const float scale); + void Rescale(const float scale_x, const float scale_y); + + // Compute similarity shape parameters from affine shape. + float ComputeScale() const; + float ComputeScaleX() const; + float ComputeScaleY() const; + float ComputeOrientation() const; + float ComputeShear() const; + + // Location of the feature, with the origin at the upper left image corner, + // i.e. the upper left pixel has the coordinate (0.5, 0.5). + float x; + float y; + + // Affine shape of the feature. + float a11; + float a12; + float a21; + float a22; +}; + +typedef Eigen::Matrix + FeatureDescriptor; + +struct FeatureMatch { + FeatureMatch() + : point2D_idx1(kInvalidPoint2DIdx), point2D_idx2(kInvalidPoint2DIdx) {} + FeatureMatch(const point2D_t point2D_idx1, const point2D_t point2D_idx2) + : point2D_idx1(point2D_idx1), point2D_idx2(point2D_idx2) {} + + // Feature index in first image. + point2D_t point2D_idx1 = kInvalidPoint2DIdx; + + // Feature index in second image. + point2D_t point2D_idx2 = kInvalidPoint2DIdx; +}; + +typedef std::vector FeatureKeypoints; +typedef Eigen::Matrix + FeatureDescriptors; +typedef std::vector FeatureMatches; + +} // namespace colmap + +#endif // COLMAP_SRC_FEATURE_TYPES_H_ diff --git a/include/colmap/feature/utils.h b/include/colmap/feature/utils.h new file mode 100644 index 0000000000000000000000000000000000000000..c037db3d12adce997110b14a12a43b13308c1a82 --- /dev/null +++ b/include/colmap/feature/utils.h @@ -0,0 +1,67 @@ +// Copyright (c) 2022, 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_FEATURE_UTILS_H_ +#define COLMAP_SRC_FEATURE_UTILS_H_ + +#include "feature/types.h" + +namespace colmap { + +// Convert feature keypoints to vector of points. +std::vector FeatureKeypointsToPointsVector( + const FeatureKeypoints& keypoints); + +// L2-normalize feature descriptor, where each row represents one feature. +Eigen::MatrixXf L2NormalizeFeatureDescriptors( + const Eigen::MatrixXf& descriptors); + +// L1-Root-normalize feature descriptors, where each row represents one feature. +// See "Three things everyone should know to improve object retrieval", +// Relja Arandjelovic and Andrew Zisserman, CVPR 2012. +Eigen::MatrixXf L1RootNormalizeFeatureDescriptors( + const Eigen::MatrixXf& descriptors); + +// Convert normalized floating point feature descriptor to unsigned byte +// representation by linear scaling from range [0, 0.5] to [0, 255]. Truncation +// to a maximum value of 0.5 is used to avoid precision loss and follows the +// common practice of representing SIFT vectors. +FeatureDescriptors FeatureDescriptorsToUnsignedByte( + const Eigen::MatrixXf& descriptors); + +// Extract the descriptors corresponding to the largest-scale features. +void ExtractTopScaleFeatures(FeatureKeypoints* keypoints, + FeatureDescriptors* descriptors, + const size_t num_features); + +} // namespace colmap + +#endif // COLMAP_SRC_FEATURE_UTILS_H_ diff --git a/include/colmap/lib/FLANN/algorithms/all_indices.h b/include/colmap/lib/FLANN/algorithms/all_indices.h new file mode 100644 index 0000000000000000000000000000000000000000..9513175b9ad7e0613e8e39fb0a56cc82bec1bcae --- /dev/null +++ b/include/colmap/lib/FLANN/algorithms/all_indices.h @@ -0,0 +1,197 @@ +/*********************************************************************** + * Software License Agreement (BSD License) + * + * Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved. + * Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. 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. + * + * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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. + *************************************************************************/ + + +#ifndef FLANN_ALL_INDICES_H_ +#define FLANN_ALL_INDICES_H_ + +#include "FLANN/general.h" + +#include "FLANN/algorithms/nn_index.h" +#include "FLANN/algorithms/kdtree_index.h" +#include "FLANN/algorithms/kdtree_single_index.h" +#include "FLANN/algorithms/kmeans_index.h" +#include "FLANN/algorithms/composite_index.h" +#include "FLANN/algorithms/linear_index.h" +#include "FLANN/algorithms/hierarchical_clustering_index.h" +#include "FLANN/algorithms/lsh_index.h" +#include "FLANN/algorithms/autotuned_index.h" +#ifdef FLANN_USE_CUDA +#include "FLANN/algorithms/kdtree_cuda_3d_index.h" +#endif + + +namespace flann +{ + +/** + * enable_if sfinae helper + */ +template struct enable_if{}; +template struct enable_if { typedef T type; }; + +/** + * disable_if sfinae helper + */ +template struct disable_if{ typedef T type; }; +template struct disable_if { }; + +/** + * Check if two type are the same + */ +template +struct same_type +{ + enum {value = false}; +}; + +template +struct same_type +{ + enum {value = true}; +}; + +#define HAS_MEMBER(member) \ + template \ + struct member { \ + typedef char No; \ + typedef long Yes; \ + template static Yes test( typename C::member* ); \ + template static No test( ... ); \ + enum { value = sizeof (test(0))==sizeof(Yes) }; \ + }; + +HAS_MEMBER(needs_kdtree_distance) +HAS_MEMBER(needs_vector_space_distance) +HAS_MEMBER(is_kdtree_distance) +HAS_MEMBER(is_vector_space_distance) + +struct DummyDistance +{ + typedef float ElementType; + typedef float ResultType; + + template + ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType /*worst_dist*/ = -1) const + { + return ResultType(0); + } + + template + inline ResultType accum_dist(const U& a, const V& b, int) const + { + return ResultType(0); + } +}; + +/** + * Checks if an index and a distance can be used together + */ +template