├── .gitignore ├── .gitmodules ├── CMakeLists.txt ├── LICENSE ├── README.md ├── cmake ├── CMakeUninstall.cmake.in ├── EssentialConfig.cmake.in ├── EssentialConfigVersion.cmake.in ├── FindEigen.cmake ├── FindOpengv.cmake ├── FindOptimization.cmake ├── Findgncso.cmake ├── pybind11.CMakeLists.txt.in └── setup_installation.cmake ├── example_install ├── CMakeLists.txt ├── README.md └── example_GNC_TNT_essential_matrix.cpp ├── examples ├── CMakeLists.txt ├── example_GNC_TNT_essential_matrix.cpp └── example_essential_matrix.cpp ├── include ├── CorrespondencesVectors.h ├── Essential.h ├── EssentialManifold.h ├── EssentialProblem.h ├── EssentialTypes.h ├── EssentialUtils.h ├── EssentialVerification.h ├── GNCEssential.h └── generateCorrespondences.h ├── matlab ├── CMakeLists.txt ├── EssentialMatrixEstimate.m ├── EssentialMatrixEstimationTest.m ├── EssentialMatrixMex.cc ├── EssentialMatrixMexUtils.h ├── GNCEssentialMatrixEstimate.m ├── GNCEssentialMatrixMex.cc ├── RobustEssentialMatrixEstimationTest.m └── setup.m ├── python ├── CMakeLists.txt ├── relativeposepython │ ├── __init__.py │ ├── central_relative_pose_example.py │ ├── relativeposepython.cc │ └── setup.py.in └── robustrelativeposepython │ ├── __init__.py │ ├── robust_central_relative_pose_example.py │ ├── robustrelativeposepython.cc │ └── setup.py.in └── src ├── Essential.cpp ├── EssentialManifold.cpp ├── EssentialProblem.cpp ├── EssentialUtils.cpp ├── EssentialVerification.cpp ├── GNCEssential.cpp └── generateCorrespondences.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | # Prerequisites 2 | *.d 3 | 4 | # Compiled Object files 5 | *.slo 6 | *.lo 7 | *.o 8 | *.obj 9 | 10 | # Precompiled Headers 11 | *.gch 12 | *.pch 13 | 14 | # Compiled Dynamic libraries 15 | *.so 16 | *.dylib 17 | *.dll 18 | 19 | # Fortran module files 20 | *.mod 21 | *.smod 22 | 23 | # Compiled Static libraries 24 | *.lai 25 | *.la 26 | *.a 27 | *.lib 28 | 29 | # Executables 30 | *.exe 31 | *.out 32 | *.app 33 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "dependences/eigen"] 2 | path = dependences/eigen 3 | url = https://gitlab.com/libeigen/eigen.git 4 | [submodule "dependences/opengv"] 5 | path = dependences/opengv 6 | url = https://github.com/laurentkneip/opengv.git 7 | [submodule "dependences/gncso"] 8 | path = dependences/gncso 9 | url = https://github.com/mergarsal/GNCSO.git 10 | [submodule "dependences/Rosen_optimization"] 11 | path = dependences/Rosen_optimization 12 | url = https://github.com/mergarsal/Optimization.git 13 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # mgarsal 03/02/2020 2 | # PROJECT CONFIGURATION 3 | cmake_minimum_required(VERSION 3.1) 4 | project(opt_certifier_relative_pose LANGUAGES C CXX VERSION 1.0.0) 5 | 6 | # set(CMAKE_BUILD_TYPE Release CACHE STRING "Choose the type of build, options are: None Debug Release RelWithDebInfo MinSizeRel.") 7 | set(CMAKE_BUILD_TYPE Release) 8 | 9 | 10 | set(LIBRARY_TARGET_NAME "Essential") 11 | set(LIBRARY_TARGET_NAME_EXPORT "${LIBRARY_TARGET_NAME}Export") 12 | 13 | set(CMAKE_CXX_STANDARD 14) 14 | set(CMAKE_CXX_STANDARD_REQUIRED ON) # We require C++ 14 15 | 16 | 17 | set(DO_GNC ON) 18 | # build the examples 19 | set(BUILD_${LIBRARY_TARGET_NAME}_EXAMPLE ON) 20 | # build the matlab binding 21 | set(BUILD_${LIBRARY_TARGET_NAME}_MATLAB OFF) 22 | # build the python binding 23 | set(BUILD_${LIBRARY_TARGET_NAME}_PYTHON OFF) 24 | 25 | 26 | ### CMake Cache (build configuration) variables -- these are set interactively in the CMake GUI, and cached in CMakeCache ### 27 | # Directory for built libraries 28 | set(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_BINARY_DIR}/lib CACHE PATH "The directory in which to place libraries built by this project") 29 | # Directory for built executables 30 | set(EXECUTABLE_OUTPUT_PATH ${CMAKE_CURRENT_BINARY_DIR}/bin CACHE PATH "The directory in which to place executables built by this project") 31 | 32 | # BUILD CONFIGURATIONS 33 | option(CMAKE_VERBOSE_MAKEFILE "Generate verbose makefiles?" OFF) 34 | 35 | set(CODE_PROFILING OFF CACHE BOOL "Turn on code profiling?") 36 | if(${CODE_PROFILING}) 37 | message(STATUS "Turning on code profiling for Essential Matrix Estimation") 38 | endif() 39 | 40 | # Add the .cmake files that ship with Eigen3 to the CMake module path (useful for finding other stuff) 41 | set(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/eigen/cmake" CACHE STRING "The CMake module path used for this project") 42 | 43 | 44 | 45 | # FIND EIGEN3 46 | set( ENV{EIGEN3_ROOT_DIR} ${CMAKE_SOURCE_DIR}/eigen) 47 | 48 | find_package(Eigen3 3.3 REQUIRED) 49 | 50 | if(EIGEN3_FOUND) 51 | message(STATUS "Found Eigen3 library (version ${EIGEN3_VERSION_STRING})") 52 | message(STATUS "Eigen3 include directory: ${EIGEN3_INCLUDE_DIR}\n") 53 | else() 54 | message(STATUS "Eigen library not found!") 55 | endif() 56 | 57 | 58 | # FIND ADDITIONAL LIBRARIES 59 | # These next operations make use of the .cmake files shipped with Eigen3 60 | find_package(BLAS REQUIRED) 61 | 62 | # IMPORT OpenGV library 63 | add_definitions(-march=native) 64 | 65 | find_package(opengv REQUIRED) 66 | 67 | find_path(opengv_INCLUDE_DIR 68 | NAMES "opengv/relative_pose/MACentralRelative.hpp" 69 | PATHS # ${CMAKE_SOURCE_DIR}/opengv/include/opengv 70 | /usr/include /usr/local/include 71 | NO_DEFAULT_PATH) 72 | 73 | if(opengv_INCLUDE_DIR) 74 | message("-- Found OpenGV headers: ${opengv_INCLUDE_DIR}") 75 | else(opengv_INCLUDE_DIR) 76 | message(FATAL_ERROR "Can't find OpenGV headers. Try passing -Dopengv_INCLUDE_DIR=...") 77 | endif(opengv_INCLUDE_DIR) 78 | 79 | 80 | include_directories(${opengv_INCLUDE_DIR}) 81 | 82 | # Installation of Headers 83 | install(DIRECTORY ${PROJECT_SOURCE_DIR}/opengv/include 84 | DESTINATION ${CMAKE_INSTALL_PREFIX} 85 | FILES_MATCHING PATTERN "*.h" 86 | PATTERN "CMakeFiles" EXCLUDE) 87 | 88 | 89 | 90 | 91 | 92 | add_subdirectory(dependences/Rosen_optimization) 93 | # Installation of Headers 94 | install( DIRECTORY ${PROJECT_SOURCE_DIR}/dependences/Rosen_optimization/include 95 | DESTINATION ${CMAKE_INSTALL_PREFIX} 96 | FILES_MATCHING PATTERN "*.h" 97 | PATTERN "CMakeFiles" EXCLUDE 98 | ) 99 | 100 | # include_directories(${Optimization_INCLUDE_DIR}) 101 | 102 | 103 | if(DO_GNC) 104 | find_package(gncso REQUIRED) 105 | endif() 106 | 107 | 108 | 109 | 110 | set(${LIBRARY_TARGET_NAME}_INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include) 111 | set(${LIBRARY_TARGET_NAME}_SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src) 112 | set(${LIBRARY_TARGET_NAME}_EXAMPLES_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/examples) 113 | 114 | 115 | # Expose the include directories for this project 116 | set(${LIBRARY_TARGET_NAME}_ADD_INCLUDES ${Optimization_INCLUDE_DIR} 117 | ${EIGEN3_INCLUDE_DIR} 118 | ${opengv_INCLUDE_DIR} 119 | ${gncso_INCLUDE_DIR} 120 | ${Optimization_INCLUDE_DIR}) 121 | 122 | set(${LIBRARY_TARGET_NAME}_CERT_INCLUDES 123 | ${${LIBRARY_TARGET_NAME}_INCLUDE_DIRS} 124 | ${${LIBRARY_TARGET_NAME}_ADD_INCLUDES}) 125 | 126 | 127 | 128 | 129 | # Get the set of Essential header and source files 130 | set(${LIBRARY_TARGET_NAME}_HDRS 131 | ${${LIBRARY_TARGET_NAME}_INCLUDE_DIRS}/EssentialTypes.h 132 | ${${LIBRARY_TARGET_NAME}_INCLUDE_DIRS}/EssentialUtils.h 133 | ${${LIBRARY_TARGET_NAME}_INCLUDE_DIRS}/EssentialManifold.h 134 | ${${LIBRARY_TARGET_NAME}_INCLUDE_DIRS}/EssentialProblem.h 135 | ${${LIBRARY_TARGET_NAME}_INCLUDE_DIRS}/Essential.h 136 | ${${LIBRARY_TARGET_NAME}_INCLUDE_DIRS}/CorrespondencesVectors.h 137 | ${${LIBRARY_TARGET_NAME}_INCLUDE_DIRS}/generateCorrespondences.h 138 | ${${LIBRARY_TARGET_NAME}_INCLUDE_DIRS}/EssentialVerification.h 139 | ${${LIBRARY_TARGET_NAME}_INCLUDE_DIRS}/GNCEssential.h 140 | ) 141 | 142 | 143 | set(${LIBRARY_TARGET_NAME}_SRCS 144 | ${${LIBRARY_TARGET_NAME}_SOURCE_DIRS}/EssentialUtils.cpp 145 | ${${LIBRARY_TARGET_NAME}_SOURCE_DIRS}/EssentialManifold.cpp 146 | ${${LIBRARY_TARGET_NAME}_SOURCE_DIRS}/EssentialProblem.cpp 147 | ${${LIBRARY_TARGET_NAME}_SOURCE_DIRS}/Essential.cpp 148 | ${${LIBRARY_TARGET_NAME}_SOURCE_DIRS}/generateCorrespondences.cpp 149 | ${${LIBRARY_TARGET_NAME}_SOURCE_DIRS}/EssentialVerification.cpp 150 | ${${LIBRARY_TARGET_NAME}_SOURCE_DIRS}/GNCEssential.cpp 151 | ) 152 | 153 | 154 | 155 | # Build the Essential library 156 | add_library(${LIBRARY_TARGET_NAME} 157 | ${${LIBRARY_TARGET_NAME}_HDRS} 158 | ${${LIBRARY_TARGET_NAME}_SRCS} ) 159 | 160 | 161 | # Set two minimum target properties for the library. 162 | # See https://cmake.org/cmake/help/latest/command/set_target_properties.html 163 | 164 | target_include_directories(${LIBRARY_TARGET_NAME} PUBLIC 165 | # only when building from the source tree 166 | $ 167 | # only when using the lib from the install path 168 | $ 169 | ${${LIBRARY_TARGET_NAME}_ADD_INCLUDES} 170 | ) 171 | 172 | 173 | target_link_libraries(${LIBRARY_TARGET_NAME} 174 | PUBLIC 175 | opengv 176 | # Optimization 177 | ${BLAS_LIBRARIES} 178 | ${M} 179 | ${LAPACK} 180 | ) 181 | 182 | if(DO_GNC) 183 | target_link_libraries(${LIBRARY_TARGET_NAME} 184 | PUBLIC 185 | gncso 186 | ) 187 | endif() 188 | 189 | if(${CODE_PROFILING}) 190 | set_target_properties(${LIBRARY_TARGET_NAME} PROPERTIES COMPILE_FLAGS "-pg -g" LINK_FLAGS "-pg -g") 191 | endif() 192 | 193 | 194 | set_target_properties(${LIBRARY_TARGET_NAME} PROPERTIES POSITION_INDEPENDENT_CODE ON) 195 | 196 | 197 | install(TARGETS ${LIBRARY_TARGET_NAME} 198 | EXPORT ${LIBRARY_TARGET_NAME_EXPORT} 199 | RUNTIME DESTINATION bin 200 | LIBRARY DESTINATION lib${LIB_SUFFIX} 201 | ARCHIVE DESTINATION lib${LIB_SUFFIX} 202 | INCLUDES DESTINATION "include" 203 | PUBLIC_HEADER DESTINATION "include/${LIBRARY_TARGET_NAME}" 204 | ) 205 | 206 | 207 | 208 | # Build the example executable 209 | IF(BUILD_${LIBRARY_TARGET_NAME}_EXAMPLE) 210 | message(STATUS "Adding examples to build") 211 | add_subdirectory(examples) 212 | endif() 213 | 214 | 215 | # building matlab binding 216 | if(BUILD_${LIBRARY_TARGET_NAME}_MATLAB) 217 | message(STATUS "Adding matlab binding to build") 218 | add_subdirectory(matlab) 219 | endif() 220 | 221 | 222 | # building python binding 223 | if(BUILD_${LIBRARY_TARGET_NAME}_PYTHON) 224 | # download the pybind11 repo 225 | configure_file(cmake/pybind11.CMakeLists.txt.in pybind11-download/CMakeLists.txt) 226 | execute_process(COMMAND "${CMAKE_COMMAND}" -G "${CMAKE_GENERATOR}" . 227 | WORKING_DIRECTORY "${CMAKE_BINARY_DIR}/pybind11-download") 228 | execute_process(COMMAND "${CMAKE_COMMAND}" --build . 229 | WORKING_DIRECTORY "${CMAKE_BINARY_DIR}/pybind11-download") 230 | add_subdirectory("${CMAKE_BINARY_DIR}/pybind11-src" 231 | "${CMAKE_BINARY_DIR}/pybind11-build") 232 | message(STATUS "Adding python binding to build") 233 | add_subdirectory(python) 234 | endif() 235 | 236 | 237 | 238 | # Install 239 | include(cmake/setup_installation.cmake) 240 | 241 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Fast and Robust Relative Pose Estimation for Calibrated Cameras 2 | 3 | This repository contains the code 4 | for the relative pose estimation 5 | between two central and calibrated cameras 6 | for the [paper](http://arxiv.org/abs/2101.08524) [1]. 7 | 8 | **Authors:** [Mercedes Garcia-Salguero](http://mapir.uma.es/mapirwebsite/index.php/people/290), [Javier Gonzalez-Jimenez](http://mapir.isa.uma.es/mapirwebsite/index.php/people/95-javier-gonzalez-jimenez) 9 | 10 | 11 | **License:** [GPLv3](https://raw.githubusercontent.com/mergarsal/FastCertRelPose/main/LICENSE) 12 | 13 | 14 | If you use this code for your research, please cite: 15 | 16 | ``` 17 | @misc{garciasalguero2021fast, 18 | title={Fast and Robust Certifiable Estimation of the Relative Pose Between Two Calibrated Cameras}, 19 | author={Mercedes Garcia-Salguero and Javier Gonzalez-Jimenez}, 20 | year={2021}, 21 | eprint={2101.08524}, 22 | archivePrefix={arXiv}, 23 | primaryClass={cs.CV} 24 | } 25 | ``` 26 | 27 | Some parts of this repository are based on previous works: 28 | 1. Matlab & python bindings from [TEASER++](https://arxiv.org/pdf/2001.07715.pdf) 29 | 30 | ``` 31 | @article{yang2020teaser, 32 | title={Teaser: Fast and certifiable point cloud registration}, 33 | author={Yang, Heng and Shi, Jingnan and Carlone, Luca}, 34 | journal={arXiv preprint arXiv:2001.07715}, 35 | year={2020} 36 | } 37 | ``` 38 | 2. Scene generation from [opengv](https://github.com/laurentkneip/opengv.git) 39 | 40 | 41 | 42 | ## Dependences 43 | * Eigen 44 | ``` 45 | sudo apt install libeigen3-dev 46 | ``` 47 | 48 | * Optimization (own fork) 49 | ``` 50 | git clone https://github.com/mergarsal/Optimization.git 51 | ``` 52 | * GNCSO 53 | ``` 54 | git clone --recursive https://github.com/mergarsal/GNCSO.git 55 | ``` 56 | 57 | * OpenGV 58 | ``` 59 | git clone https://github.com/laurentkneip/opengv.git 60 | ``` 61 | 62 | 63 | ## Build 64 | ``` 65 | git clone --recursive https://github.com/mergarsal/FastCertRelPose.git 66 | cd GNCSO 67 | 68 | mkdir build & cd build 69 | 70 | cmake .. 71 | 72 | make -jX 73 | 74 | ``` 75 | 76 | The compiled examples should be inside the `bin` directory. Run: 77 | ``` 78 | ./bin/example_essential_matrix 79 | ``` 80 | 81 | 82 | 83 | ## Install 84 | In `build` folder: 85 | ``` 86 | sudo make install 87 | ``` 88 | 89 | We also provide the uninstall script: 90 | ``` 91 | sudo make uninstall 92 | ``` 93 | 94 | 95 | 96 | ## How to use the library in your project 97 | 98 | See the example in the folder `example_install` 99 | for the basic elements. 100 | 101 | 1. In your CMakeLists.txt, add the dependences: 102 | ``` 103 | find_package(gncso REQUIRED) 104 | find_package(opengv REQUIRED) 105 | find_package(Essential REQUIRED) 106 | ``` 107 | 108 | 2. For your executable, add the library in 109 | ``` 110 | target_link_libraries(XXX Essential opengv gncso) 111 | ``` 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | -------------------------------------------------------------------------------- /cmake/CMakeUninstall.cmake.in: -------------------------------------------------------------------------------- 1 | if(NOT EXISTS "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt") 2 | message(FATAL_ERROR "Cannot find install manifest: @CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt") 3 | endif() 4 | 5 | file(READ "@CMAKE_CURRENT_BINARY_DIR@/install_manifest.txt" files) 6 | string(REGEX REPLACE "\n" ";" files "${files}") 7 | 8 | foreach(file ${files}) 9 | message(STATUS "Uninstalling $ENV{DESTDIR}${file}") 10 | if(IS_SYMLINK "$ENV{DESTDIR}${file}" OR EXISTS "$ENV{DESTDIR}${file}") 11 | exec_program("@CMAKE_COMMAND@" ARGS "-E remove \"$ENV{DESTDIR}${file}\"" 12 | OUTPUT_VARIABLE rm_out 13 | RETURN_VALUE rm_retval) 14 | if(NOT "${rm_retval}" STREQUAL 0) 15 | message(FATAL_ERROR "Problem when removing $ENV{DESTDIR}${file}") 16 | endif() 17 | else() 18 | message(STATUS "File $ENV{DESTDIR}${file} does not exist.") 19 | endif() 20 | endforeach() 21 | -------------------------------------------------------------------------------- /cmake/EssentialConfig.cmake.in: -------------------------------------------------------------------------------- 1 | include(CMakeFindDependencyMacro) 2 | @Essential_FIND_DEPENDENCY_CALLS@ 3 | include("${CMAKE_CURRENT_LIST_DIR}/@exported_targets_filename@") 4 | check_required_components("@PROJECT_NAME@") 5 | -------------------------------------------------------------------------------- /cmake/EssentialConfigVersion.cmake.in: -------------------------------------------------------------------------------- 1 | # Ceres Solver - A fast non-linear least squares minimizer 2 | # Copyright 2013 Google Inc. All rights reserved. 3 | # http://code.google.com/p/ceres-solver/ 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions are met: 7 | # 8 | # * Redistributions of source code must retain the above copyright notice, 9 | # this list of conditions and the following disclaimer. 10 | # * Redistributions in binary form must reproduce the above copyright notice, 11 | # this list of conditions and the following disclaimer in the documentation 12 | # and/or other materials provided with the distribution. 13 | # * Neither the name of Google Inc. nor the names of its contributors may be 14 | # used to endorse or promote products derived from this software without 15 | # specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | # 29 | # Author: pablo.speciale@gmail.com (Pablo Speciale) 30 | # 31 | # FIND_PACKAGE() searches for a Config.cmake file and an associated 32 | # Version.cmake file, which it loads to check the version number. 33 | # 34 | # This file can be used with CONFIGURE_FILE() to generate such a file for a 35 | # project with very basic logic. 36 | # 37 | # It sets PACKAGE_VERSION_EXACT if the current version string and the requested 38 | # version string are exactly the same and it sets PACKAGE_VERSION_COMPATIBLE 39 | # if the current version is >= requested version. 40 | 41 | set(PACKAGE_VERSION @ESSENTIAL_VERSION@) 42 | 43 | if ("${PACKAGE_VERSION}" VERSION_LESS "${PACKAGE_FIND_VERSION}") 44 | set(PACKAGE_VERSION_COMPATIBLE FALSE) 45 | else ("${PACKAGE_VERSION}" VERSION_LESS "${PACKAGE_FIND_VERSION}") 46 | set(PACKAGE_VERSION_COMPATIBLE TRUE) 47 | if ("${PACKAGE_FIND_VERSION}" STREQUAL "${PACKAGE_VERSION}") 48 | set(PACKAGE_VERSION_EXACT TRUE) 49 | endif ("${PACKAGE_FIND_VERSION}" STREQUAL "${PACKAGE_VERSION}") 50 | endif ("${PACKAGE_VERSION}" VERSION_LESS "${PACKAGE_FIND_VERSION}") 51 | -------------------------------------------------------------------------------- /cmake/FindEigen.cmake: -------------------------------------------------------------------------------- 1 | # Ceres Solver - A fast non-linear least squares minimizer 2 | # Copyright 2015 Google Inc. All rights reserved. 3 | # http://ceres-solver.org/ 4 | # 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions are met: 7 | # 8 | # * Redistributions of source code must retain the above copyright notice, 9 | # this list of conditions and the following disclaimer. 10 | # * Redistributions in binary form must reproduce the above copyright notice, 11 | # this list of conditions and the following disclaimer in the documentation 12 | # and/or other materials provided with the distribution. 13 | # * Neither the name of Google Inc. nor the names of its contributors may be 14 | # used to endorse or promote products derived from this software without 15 | # specific prior written permission. 16 | # 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | # POSSIBILITY OF SUCH DAMAGE. 28 | # 29 | # Author: alexs.mac@gmail.com (Alex Stewart) 30 | # 31 | 32 | # FindEigen.cmake - Find Eigen library, version >= 3. 33 | # 34 | # This module defines the following variables: 35 | # 36 | # EIGEN_FOUND: TRUE iff Eigen is found. 37 | # EIGEN_INCLUDE_DIRS: Include directories for Eigen. 38 | # EIGEN_VERSION: Extracted from Eigen/src/Core/util/Macros.h 39 | # EIGEN_WORLD_VERSION: Equal to 3 if EIGEN_VERSION = 3.2.0 40 | # EIGEN_MAJOR_VERSION: Equal to 2 if EIGEN_VERSION = 3.2.0 41 | # EIGEN_MINOR_VERSION: Equal to 0 if EIGEN_VERSION = 3.2.0 42 | # FOUND_INSTALLED_EIGEN_CMAKE_CONFIGURATION: True iff the version of Eigen 43 | # found was built & installed / 44 | # exported as a CMake package. 45 | # 46 | # The following variables control the behaviour of this module: 47 | # 48 | # EIGEN_PREFER_EXPORTED_EIGEN_CMAKE_CONFIGURATION: TRUE/FALSE, iff TRUE then 49 | # then prefer using an exported CMake configuration 50 | # generated by Eigen over searching for the 51 | # Eigen components manually. Otherwise (FALSE) 52 | # ignore any exported Eigen CMake configurations and 53 | # always perform a manual search for the components. 54 | # Default: TRUE iff user does not define this variable 55 | # before we are called, and does NOT specify 56 | # EIGEN_INCLUDE_DIR_HINTS, otherwise FALSE. 57 | # EIGEN_INCLUDE_DIR_HINTS: List of additional directories in which to 58 | # search for eigen includes, e.g: /timbuktu/eigen3. 59 | # 60 | # The following variables are also defined by this module, but in line with 61 | # CMake recommended FindPackage() module style should NOT be referenced directly 62 | # by callers (use the plural variables detailed above instead). These variables 63 | # do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which 64 | # are NOT re-called (i.e. search for library is not repeated) if these variables 65 | # are set with valid values _in the CMake cache_. This means that if these 66 | # variables are set directly in the cache, either by the user in the CMake GUI, 67 | # or by the user passing -DVAR=VALUE directives to CMake when called (which 68 | # explicitly defines a cache variable), then they will be used verbatim, 69 | # bypassing the HINTS variables and other hard-coded search locations. 70 | # 71 | # EIGEN_INCLUDE_DIR: Include directory for CXSparse, not including the 72 | # include directory of any dependencies. 73 | 74 | # Called if we failed to find Eigen or any of it's required dependencies, 75 | # unsets all public (designed to be used externally) variables and reports 76 | # error message at priority depending upon [REQUIRED/QUIET/] argument. 77 | macro(EIGEN_REPORT_NOT_FOUND REASON_MSG) 78 | unset(EIGEN_FOUND) 79 | unset(EIGEN_INCLUDE_DIRS) 80 | unset(FOUND_INSTALLED_EIGEN_CMAKE_CONFIGURATION) 81 | # Make results of search visible in the CMake GUI if Eigen has not 82 | # been found so that user does not have to toggle to advanced view. 83 | mark_as_advanced(CLEAR EIGEN_INCLUDE_DIR) 84 | # Note _FIND_[REQUIRED/QUIETLY] variables defined by FindPackage() 85 | # use the camelcase library name, not uppercase. 86 | if (Eigen_FIND_QUIETLY) 87 | message(STATUS "Failed to find Eigen - " ${REASON_MSG} ${ARGN}) 88 | elseif (Eigen_FIND_REQUIRED) 89 | message(FATAL_ERROR "Failed to find Eigen - " ${REASON_MSG} ${ARGN}) 90 | else() 91 | # Neither QUIETLY nor REQUIRED, use no priority which emits a message 92 | # but continues configuration and allows generation. 93 | message("-- Failed to find Eigen - " ${REASON_MSG} ${ARGN}) 94 | endif () 95 | return() 96 | endmacro(EIGEN_REPORT_NOT_FOUND) 97 | 98 | # Protect against any alternative find_package scripts for this library having 99 | # been called previously (in a client project) which set EIGEN_FOUND, but not 100 | # the other variables we require / set here which could cause the search logic 101 | # here to fail. 102 | unset(EIGEN_FOUND) 103 | 104 | # ----------------------------------------------------------------- 105 | # By default, if the user has expressed no preference for using an exported 106 | # Eigen CMake configuration over performing a search for the installed 107 | # components, and has not specified any hints for the search locations, then 108 | # prefer an exported configuration if available. 109 | if (NOT DEFINED EIGEN_PREFER_EXPORTED_EIGEN_CMAKE_CONFIGURATION 110 | AND NOT EIGEN_INCLUDE_DIR_HINTS) 111 | message(STATUS "No preference for use of exported Eigen CMake configuration " 112 | "set, and no hints for include directory provided. " 113 | "Defaulting to preferring an installed/exported Eigen CMake configuration " 114 | "if available.") 115 | set(EIGEN_PREFER_EXPORTED_EIGEN_CMAKE_CONFIGURATION TRUE) 116 | endif() 117 | 118 | if (EIGEN_PREFER_EXPORTED_EIGEN_CMAKE_CONFIGURATION) 119 | # Try to find an exported CMake configuration for Eigen. 120 | # 121 | # We search twice, s/t we can invert the ordering of precedence used by 122 | # find_package() for exported package build directories, and installed 123 | # packages (found via CMAKE_SYSTEM_PREFIX_PATH), listed as items 6) and 7) 124 | # respectively in [1]. 125 | # 126 | # By default, exported build directories are (in theory) detected first, and 127 | # this is usually the case on Windows. However, on OS X & Linux, the install 128 | # path (/usr/local) is typically present in the PATH environment variable 129 | # which is checked in item 4) in [1] (i.e. before both of the above, unless 130 | # NO_SYSTEM_ENVIRONMENT_PATH is passed). As such on those OSs installed 131 | # packages are usually detected in preference to exported package build 132 | # directories. 133 | # 134 | # To ensure a more consistent response across all OSs, and as users usually 135 | # want to prefer an installed version of a package over a locally built one 136 | # where both exist (esp. as the exported build directory might be removed 137 | # after installation), we first search with NO_CMAKE_PACKAGE_REGISTRY which 138 | # means any build directories exported by the user are ignored, and thus 139 | # installed directories are preferred. If this fails to find the package 140 | # we then research again, but without NO_CMAKE_PACKAGE_REGISTRY, so any 141 | # exported build directories will now be detected. 142 | # 143 | # To prevent confusion on Windows, we also pass NO_CMAKE_BUILDS_PATH (which 144 | # is item 5) in [1]), to not preferentially use projects that were built 145 | # recently with the CMake GUI to ensure that we always prefer an installed 146 | # version if available. 147 | # 148 | # [1] http://www.cmake.org/cmake/help/v2.8.11/cmake.html#command:find_package 149 | find_package(Eigen3 QUIET 150 | NO_MODULE 151 | NO_CMAKE_PACKAGE_REGISTRY 152 | NO_CMAKE_BUILDS_PATH) 153 | if (EIGEN3_FOUND) 154 | message(STATUS "Found installed version of Eigen: ${Eigen3_DIR}") 155 | else() 156 | # Failed to find an installed version of Eigen, repeat search allowing 157 | # exported build directories. 158 | message(STATUS "Failed to find installed Eigen CMake configuration, " 159 | "searching for Eigen build directories exported with CMake.") 160 | # Again pass NO_CMAKE_BUILDS_PATH, as we know that Eigen is exported and 161 | # do not want to treat projects built with the CMake GUI preferentially. 162 | find_package(Eigen3 QUIET 163 | NO_MODULE 164 | NO_CMAKE_BUILDS_PATH) 165 | if (EIGEN3_FOUND) 166 | message(STATUS "Found exported Eigen build directory: ${Eigen3_DIR}") 167 | endif() 168 | endif() 169 | if (EIGEN3_FOUND) 170 | set(FOUND_INSTALLED_EIGEN_CMAKE_CONFIGURATION TRUE) 171 | set(EIGEN_FOUND ${EIGEN3_FOUND}) 172 | set(EIGEN_INCLUDE_DIR "${EIGEN3_INCLUDE_DIR}" CACHE STRING 173 | "Eigen include directory" FORCE) 174 | else() 175 | message(STATUS "Failed to find an installed/exported CMake configuration " 176 | "for Eigen, will perform search for installed Eigen components.") 177 | endif() 178 | endif() 179 | 180 | if (NOT EIGEN_FOUND) 181 | # Search user-installed locations first, so that we prefer user installs 182 | # to system installs where both exist. 183 | list(APPEND EIGEN_CHECK_INCLUDE_DIRS 184 | /usr/local/include 185 | /usr/local/homebrew/include # Mac OS X 186 | /opt/local/var/macports/software # Mac OS X. 187 | /opt/local/include 188 | /usr/include) 189 | # Additional suffixes to try appending to each search path. 190 | list(APPEND EIGEN_CHECK_PATH_SUFFIXES 191 | eigen3 # Default root directory for Eigen. 192 | Eigen/include/eigen3 # Windows (for C:/Program Files prefix) < 3.3 193 | Eigen3/include/eigen3 ) # Windows (for C:/Program Files prefix) >= 3.3 194 | 195 | # Search supplied hint directories first if supplied. 196 | find_path(EIGEN_INCLUDE_DIR 197 | NAMES Eigen/Core 198 | HINTS ${EIGEN_INCLUDE_DIR_HINTS} 199 | PATHS ${EIGEN_CHECK_INCLUDE_DIRS} 200 | PATH_SUFFIXES ${EIGEN_CHECK_PATH_SUFFIXES}) 201 | 202 | if (NOT EIGEN_INCLUDE_DIR OR 203 | NOT EXISTS ${EIGEN_INCLUDE_DIR}) 204 | eigen_report_not_found( 205 | "Could not find eigen3 include directory, set EIGEN_INCLUDE_DIR to " 206 | "path to eigen3 include directory, e.g. /usr/local/include/eigen3.") 207 | endif (NOT EIGEN_INCLUDE_DIR OR 208 | NOT EXISTS ${EIGEN_INCLUDE_DIR}) 209 | 210 | # Mark internally as found, then verify. EIGEN_REPORT_NOT_FOUND() unsets 211 | # if called. 212 | set(EIGEN_FOUND TRUE) 213 | endif() 214 | 215 | # Extract Eigen version from Eigen/src/Core/util/Macros.h 216 | if (EIGEN_INCLUDE_DIR) 217 | set(EIGEN_VERSION_FILE ${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h) 218 | if (NOT EXISTS ${EIGEN_VERSION_FILE}) 219 | eigen_report_not_found( 220 | "Could not find file: ${EIGEN_VERSION_FILE} " 221 | "containing version information in Eigen install located at: " 222 | "${EIGEN_INCLUDE_DIR}.") 223 | else (NOT EXISTS ${EIGEN_VERSION_FILE}) 224 | file(READ ${EIGEN_VERSION_FILE} EIGEN_VERSION_FILE_CONTENTS) 225 | 226 | string(REGEX MATCH "#define EIGEN_WORLD_VERSION [0-9]+" 227 | EIGEN_WORLD_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") 228 | string(REGEX REPLACE "#define EIGEN_WORLD_VERSION ([0-9]+)" "\\1" 229 | EIGEN_WORLD_VERSION "${EIGEN_WORLD_VERSION}") 230 | 231 | string(REGEX MATCH "#define EIGEN_MAJOR_VERSION [0-9]+" 232 | EIGEN_MAJOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") 233 | string(REGEX REPLACE "#define EIGEN_MAJOR_VERSION ([0-9]+)" "\\1" 234 | EIGEN_MAJOR_VERSION "${EIGEN_MAJOR_VERSION}") 235 | 236 | string(REGEX MATCH "#define EIGEN_MINOR_VERSION [0-9]+" 237 | EIGEN_MINOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") 238 | string(REGEX REPLACE "#define EIGEN_MINOR_VERSION ([0-9]+)" "\\1" 239 | EIGEN_MINOR_VERSION "${EIGEN_MINOR_VERSION}") 240 | 241 | # This is on a single line s/t CMake does not interpret it as a list of 242 | # elements and insert ';' separators which would result in 3.;2.;0 nonsense. 243 | set(EIGEN_VERSION "${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}") 244 | endif (NOT EXISTS ${EIGEN_VERSION_FILE}) 245 | endif (EIGEN_INCLUDE_DIR) 246 | 247 | # Set standard CMake FindPackage variables if found. 248 | if (EIGEN_FOUND) 249 | set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR}) 250 | endif (EIGEN_FOUND) 251 | 252 | # Handle REQUIRED / QUIET optional arguments and version. 253 | include(FindPackageHandleStandardArgs) 254 | find_package_handle_standard_args(Eigen 255 | REQUIRED_VARS EIGEN_INCLUDE_DIRS 256 | VERSION_VAR EIGEN_VERSION) 257 | 258 | # Only mark internal variables as advanced if we found Eigen, otherwise 259 | # leave it visible in the standard GUI for the user to set manually. 260 | if (EIGEN_FOUND) 261 | mark_as_advanced(FORCE EIGEN_INCLUDE_DIR 262 | Eigen3_DIR) # Autogenerated by find_package(Eigen3) 263 | endif (EIGEN_FOUND) 264 | -------------------------------------------------------------------------------- /cmake/FindOpengv.cmake: -------------------------------------------------------------------------------- 1 | find_package(opengv) 2 | 3 | # Set standard CMake FindPackage variables if found. 4 | if (opengv_FOUND) 5 | set(opengv_INCLUDE_DIRS ${opengv_INCLUDE_DIR}) 6 | endif (opengv_FOUND) 7 | 8 | 9 | # Only mark internal variables as advanced if we found opengv, otherwise 10 | # leave it visible in the standard GUI for the user to set manually. 11 | if (opengv_FOUND) 12 | mark_as_advanced(FORCE opengv_INCLUDE_DIR 13 | opengv_DIR) 14 | endif (opengv_FOUND) 15 | -------------------------------------------------------------------------------- /cmake/FindOptimization.cmake: -------------------------------------------------------------------------------- 1 | find_package(Optimization) 2 | 3 | # Set standard CMake FindPackage variables if found. 4 | if (Optimization_FOUND) 5 | set(Optimization_INCLUDE_DIRS ${Optimization_INCLUDE_DIR}) 6 | endif (Optimization_FOUND) 7 | 8 | 9 | # Only mark internal variables as advanced if we found Eigen, otherwise 10 | # leave it visible in the standard GUI for the user to set manually. 11 | if (Optimization_FOUND) 12 | mark_as_advanced(FORCE Optimization_INCLUDE_DIR 13 | Optimization_DIR) 14 | endif (Optimization_FOUND) 15 | -------------------------------------------------------------------------------- /cmake/Findgncso.cmake: -------------------------------------------------------------------------------- 1 | find_package(gncso) 2 | 3 | # Set standard CMake FindPackage variables if found. 4 | if (gncso_FOUND) 5 | set(gncso_INCLUDE_DIRS ${gncso_INCLUDE_DIR}) 6 | message(STATUS "Found GNCSO in ${GNCSO_INCLUDE_DIRS}") 7 | else() 8 | message(STATUS "GNCSO not found!") 9 | endif (gncso_FOUND) 10 | 11 | 12 | # Only mark internal variables as advanced if we found Eigen, otherwise 13 | # leave it visible in the standard GUI for the user to set manually. 14 | if (gncso_FOUND) 15 | mark_as_advanced(FORCE gncso_INCLUDE_DIR 16 | gncso_DIR) 17 | endif (gncso_FOUND) 18 | -------------------------------------------------------------------------------- /cmake/pybind11.CMakeLists.txt.in: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | 3 | project(pybind11-download NONE) 4 | 5 | include(ExternalProject) 6 | ExternalProject_Add(pmc 7 | GIT_REPOSITORY https://github.com/pybind/pybind11.git 8 | GIT_TAG v2.4.3 9 | SOURCE_DIR "${CMAKE_CURRENT_BINARY_DIR}/pybind11-src" 10 | BINARY_DIR "${CMAKE_CURRENT_BINARY_DIR}/pybind11-build" 11 | CONFIGURE_COMMAND "" 12 | BUILD_COMMAND "" 13 | INSTALL_COMMAND "" 14 | TEST_COMMAND "" 15 | ) -------------------------------------------------------------------------------- /cmake/setup_installation.cmake: -------------------------------------------------------------------------------- 1 | include(CMakePackageConfigHelpers) 2 | # Include module with fuction 'write_basic_package_version_file' 3 | 4 | # Configuration 5 | 6 | # Set up install directories. INCLUDE_INSTALL_DIR, LIB_INSTALL_DIR and 7 | # CMAKECONFIG_INSTALL_DIR must not be absolute paths. 8 | set(include_install_dir "include/Essential") 9 | set(LIB_INSTALL_DIR "${LIBRARY_TARGET_NAME}") 10 | set(CMAKECONFIG_INSTALL_DIR "share/${LIBRARY_TARGET_NAME}") 11 | set(RELATIVE_CMAKECONFIG_INSTALL_DIR "share/${LIBRARY_TARGET_NAME}") 12 | set(config_install_dir "lib/cmake/${LIBRARY_TARGET_NAME}-${PROJECT_VERSION}") 13 | set(exported_targets_name "${LIBRARY_TARGET_NAME}Targets") 14 | set(exported_targets_filename "${exported_targets_name}.cmake") 15 | set(export_dirpath "lib/cmake/${LIBRARY_TARGET_NAME}") 16 | set(config_basename "${LIBRARY_TARGET_NAME}Config") 17 | set(config_filename "${config_basename}.cmake") 18 | set(version_filename "${config_basename}Version.cmake") 19 | 20 | #################################################### 21 | ## THEIA INSPIRED ## 22 | #################################################### 23 | 24 | # Install the .h files 25 | file(GLOB ${LIBRARY_TARGET_NAME}_HDRS_GLOB ${CMAKE_SOURCE_DIR}/include/*.h) 26 | install(FILES ${${LIBRARY_TARGET_NAME}_HDRS_GLOB} DESTINATION ${include_install_dir}) 27 | 28 | install(DIRECTORY /include DESTINATION include/${LIBRARY_TARGET_NAME} FILES_MATCHING PATTERN "*.h") 29 | 30 | 31 | # This "exports" all targets which have been put into the export set 32 | # "TheiaExport". This means that CMake generates a file with the given 33 | # filename, which can later on be loaded by projects using this package. 34 | # This file contains ADD_LIBRARY(bar IMPORTED) statements for each target 35 | # in the export set, so when loaded later on CMake will create "imported" 36 | # library targets from these, which can be used in many ways in the same way 37 | # as a normal library target created via a normal ADD_LIBRARY(). 38 | install(EXPORT ${LIBRARY_TARGET_NAME_EXPORT} 39 | DESTINATION ${RELATIVE_CMAKECONFIG_INSTALL_DIR} FILE ${exported_targets_filename}) 40 | 41 | # Figure out the relative path from the installed Config.cmake file to the 42 | # install prefix (which may be at runtime different from the chosen 43 | # CMAKE_INSTALL_PREFIX if under Windows the package was installed anywhere) 44 | # This relative path will be configured into the TheiaConfig.cmake. 45 | file(RELATIVE_PATH INSTALL_ROOT_REL_CONFIG_INSTALL_DIR 46 | ${CMAKE_INSTALL_PREFIX}/${CMAKECONFIG_INSTALL_DIR} ${CMAKE_INSTALL_PREFIX}) 47 | 48 | # Create a TheiaConfig.cmake file. Config.cmake files are searched by 49 | # FIND_PACKAGE() automatically. We configure that file so that we can put any 50 | # information we want in it, e.g. version numbers, include directories, etc. 51 | configure_package_config_file( 52 | "${CMAKE_SOURCE_DIR}/cmake/${config_filename}.in" 53 | "${CMAKE_CURRENT_BINARY_DIR}/${config_filename}" 54 | INSTALL_DESTINATION "${config_install_dir}") 55 | 56 | # Additionally, when CMake has found a TheiaConfig.cmake, it can check for a 57 | # TheiaConfigVersion.cmake in the same directory when figuring out the version 58 | # of the package when a version has been specified in the FIND_PACKAGE() call, 59 | # e.g. FIND_PACKAGE(Theia [0.5.2] REQUIRED). The version argument is optional. 60 | configure_file("${CMAKE_SOURCE_DIR}/cmake/${version_filename}.in" 61 | "${CMAKE_CURRENT_BINARY_DIR}/${version_filename}" @ONLY) 62 | 63 | # Install these files into the same directory as the generated exports-file, 64 | # we include the FindPackage scripts for libraries whose headers are included 65 | # in the public API of Theia and should thus be present in THEIA_INCLUDE_DIRS. 66 | install(FILES "${CMAKE_CURRENT_BINARY_DIR}/${config_filename}" 67 | "${CMAKE_CURRENT_BINARY_DIR}/${version_filename}" 68 | "${CMAKE_SOURCE_DIR}/cmake/FindEigen.cmake" 69 | "${CMAKE_SOURCE_DIR}/cmake/FindOptimization.cmake" 70 | "${CMAKE_SOURCE_DIR}/cmake/Findgncso.cmake" 71 | "${CMAKE_SOURCE_DIR}/cmake/FindOpengv.cmake" 72 | DESTINATION ${CMAKECONFIG_INSTALL_DIR}) 73 | 74 | 75 | # uninstall target 76 | if(NOT TARGET uninstall) 77 | configure_file( 78 | "${CMAKE_CURRENT_SOURCE_DIR}/cmake/CMakeUninstall.cmake.in" 79 | "${CMAKE_CURRENT_BINARY_DIR}/cmake/CMakeUninstall.cmake" 80 | IMMEDIATE @ONLY) 81 | 82 | add_custom_target(uninstall 83 | COMMAND ${CMAKE_COMMAND} -P ${CMAKE_CURRENT_BINARY_DIR}/cmake/CMakeUninstall.cmake) 84 | endif() 85 | 86 | -------------------------------------------------------------------------------- /example_install/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1) 2 | 3 | project(Essential-Example-Inst LANGUAGES C CXX VERSION 1.0.0) 4 | 5 | set(CMAKE_CXX_STANDARD 14) # We require C++ 14 or later 6 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 7 | 8 | set(CMAKE_BUILD_TYPE Release) 9 | 10 | 11 | # Find Eigen library 12 | find_package(Eigen3 3.3.3 REQUIRED) 13 | if(EIGEN3_FOUND) 14 | message(STATUS "Found Eigen3 library (version ${EIGEN3_VERSION})") 15 | message(STATUS "Eigen3 include directory: ${EIGEN3_INCLUDE_DIR}\n") 16 | else() 17 | message(STATUS "Eigen library not found!") 18 | endif() 19 | 20 | # Add the Eigen include directories 21 | include_directories(${EIGEN3_INCLUDE_DIR}) 22 | 23 | 24 | add_definitions(-march=native) 25 | 26 | add_subdirectory(Rosen_optimization) 27 | 28 | find_package(gncso REQUIRED) 29 | 30 | find_package(Essential REQUIRED) 31 | 32 | 33 | 34 | # GNC-RTNT Essential Matrix Estimation 35 | add_executable(example_GNC_TNT_essential_matrix example_GNC_TNT_essential_matrix.cpp) 36 | 37 | # order is important! 38 | target_link_libraries(example_GNC_TNT_essential_matrix 39 | Essential Optimization gncso) 40 | -------------------------------------------------------------------------------- /example_install/README.md: -------------------------------------------------------------------------------- 1 | # About this folder 2 | 3 | This folder contains an example 4 | of how to install and use 5 | the Essential library: 6 | * CMakeLists.txt: basic cmake 7 | * example_GNC_TNT_essential_matrix: 8 | basic example that uses GNC 9 | 10 | *NOTE:* Include inside this folder 11 | the library 'Rosen_optimization` 12 | 13 | -------------------------------------------------------------------------------- /example_install/example_GNC_TNT_essential_matrix.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include "Essential/generateCorrespondences.h" 9 | #include "Essential/Essential.h" 10 | #include "Essential/EssentialTypes.h" 11 | #include "Essential/EssentialUtils.h" 12 | 13 | #include "Essential/GNCEssential.h" 14 | 15 | 16 | using namespace std; 17 | using namespace Eigen; 18 | using namespace Essential; 19 | 20 | 21 | 22 | int main(int argc, char** argv) 23 | { 24 | std::cout << "Essential Matrix Estimation with GNC GM!\n"; 25 | 26 | // save general results 27 | std::ofstream fout("GNC_TNT.txt"); 28 | // save weights from the GNC 29 | std::ofstream fout_w("GNC_TNT_weights.txt"); 30 | 31 | double N_iter = 1; 32 | //set experiment parameters 33 | double noise = 0.1; 34 | size_t n_points = 100; 35 | double FoV = 150; // in degrees 36 | double parallax = 2.0; // in meters 37 | double min_depth = 1.; // in meters 38 | double max_depth = 8.; // in meters 39 | double outlier_fraction = 0.10; 40 | 41 | 42 | std::srand(std::time(nullptr)); 43 | 44 | for (int i = 0; i < N_iter; i++) 45 | { 46 | Vector3 translation; 47 | Matrix3 rotation; 48 | bearing_vectors_t points_correspondences; 49 | Eigen::MatrixXd points_3D(3, n_points); 50 | std::vector indices_outliers(1, n_points); 51 | createSyntheticExperiment(n_points, noise, outlier_fraction, FoV, parallax, min_depth, max_depth, translation, rotation, 52 | points_correspondences, points_3D, indices_outliers, false, 20, 0.3 ); 53 | 54 | 55 | 56 | // n_point 57 | Matrix3 E = computeEfromRt(rotation, translation); 58 | 59 | 60 | 61 | GNCEssentialEstimationOptions options = GNCEssentialEstimationOptions(); 62 | /* For GNC */ 63 | options.gnc_robust = GNCRobustFunction::WELSCH; 64 | options.GNC_verbose = 1; 65 | options.gnc_factor = 1.10; // for GM 66 | options.cost_diff_threshold = 0.000001; // stop criteria. if the diff between two cost is lower than this value, stop 67 | options.max_res_tol_sq = 0.00001; 68 | options.max_inner_iterations = 2; // with 4 (even less) you have enough 69 | options.GNClog_iterates = true; // log weights. NOTE: deactive this because memory 70 | options.inliers_threshold = 0.9; // for GM 71 | options.nr_min_points = 12; 72 | 73 | 74 | /* For the TNT solver */ 75 | 76 | options.chosen_initialisation = InitialisationMethod::PTS8_INIT; 77 | options.use_preconditioning = Preconditioner::Any; 78 | options.estimation_verbose = 1; 79 | 80 | 81 | 82 | // Instance of GNC estimator 83 | GNCEssentialClass my_essential_estimation(points_correspondences, options, E); 84 | 85 | // run the estimation 86 | GNCEssentialEstimationResult my_result = my_essential_estimation.getResultGNC(); 87 | std::cout << "GT R:\n" << rotation << std::endl; 88 | std::cout << "Estimated R:\n" << my_result.R_opt << std::endl; 89 | std::cout << "Solution is valid?: " << my_result.valid_estimation << std::endl; 90 | 91 | // Save results into a file 92 | /* 93 | {'iter','idx_iter','f_hat','d_hat','gradnorm','dual_gap','mu_min', 94 | 'elapsed_init_time','elapsed_C_time','elapsed_8pt_time','elapsed_time_methods', 95 | 'elapsed_iterative_time','elapsed_lagrange_time','elapsed_certifier_time','elapsed_estimation_time', 96 | 'distT','distR','is_opt'} 97 | */ 98 | fout << "iter: " << i; 99 | fout << " " << my_result.f_hat; 100 | fout << " " << my_result.d_hat; 101 | fout << " " << my_result.gradnorm; 102 | fout << " " << my_result.dual_gap; 103 | fout << " " << my_result.mu_min; 104 | fout << " " << my_result.elapsed_init_time; 105 | fout << " " << my_result.elapsed_C_time; 106 | fout << " " << my_result.elapsed_8pt_time; 107 | fout << " " << my_result.elapsed_time_methods; 108 | fout << " " << my_result.elapsed_iterative_time; 109 | fout << " " << my_result.elapsed_lagrange_time; 110 | fout << " " << my_result.elapsed_certifier_time; 111 | fout << " " << my_result.elapsed_estimation_time; 112 | fout << " " << distT(translation, my_result.t_opt); 113 | fout << " " << distR(rotation, my_result.R_opt); 114 | 115 | int is_opt = -1; 116 | switch(my_result.certifier_status) 117 | { 118 | case EssentialEstimationStatus::RS_ITER_LIMIT: 119 | is_opt = 2; 120 | break; 121 | case EssentialEstimationStatus::GLOBAL_OPT: 122 | is_opt = 0; 123 | break; 124 | case EssentialEstimationStatus::NO_CERTIFIED: 125 | is_opt = 1; 126 | break; 127 | default: 128 | is_opt = -1; 129 | } 130 | 131 | 132 | fout << " " << is_opt; 133 | 134 | fout << " " << my_result.valid_estimation << std::endl; 135 | 136 | 137 | std::cout << "Total time: " << my_result.elapsed_estimation_time << std::endl; 138 | std::cout << "Error rotation: " << distR(rotation, my_result.R_opt) << std::endl; 139 | std::cout << "Error translation: " << distT(translation, my_result.t_opt) << std::endl; 140 | std::cout << "GNC time: " << my_result.elapsed_iterative_time << std::endl; 141 | 142 | 143 | 144 | /* Saving weights */ 145 | // handle: fout_w 146 | // format: one file per iteration 147 | // number of outer iterations: my_result.nr_outer_iterations 148 | for (size_t id_outer = 0; id_outer < my_result.nr_outer_iterations; id_outer++) 149 | { 150 | // extract weights 151 | fout_w << id_outer; 152 | for (size_t id_w = 0; id_w < my_result.intermediate_outer_results[id_outer].weights.size(); id_w ++) 153 | { 154 | fout_w << "," << my_result.intermediate_outer_results[id_outer].weights[id_w]; 155 | } 156 | // add f(hat(x)) 157 | fout_w << "," << my_result.intermediate_outer_results[id_outer].f; 158 | fout_w << std::endl; 159 | } 160 | 161 | 162 | 163 | 164 | } 165 | // close file for general results 166 | fout.close(); 167 | // close file for weights 168 | fout_w.close(); 169 | 170 | return 0; 171 | 172 | } 173 | -------------------------------------------------------------------------------- /examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(OptCertifier-Examples CXX) 2 | 3 | 4 | # Find Eigen library 5 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_SOURCE_DIR}/../cmake/") 6 | find_package(Eigen3 3.3.3 REQUIRED) 7 | if(EIGEN3_FOUND) 8 | message(STATUS "Found Eigen3 library (version ${EIGEN3_VERSION})") 9 | message(STATUS "Eigen3 include directory: ${EIGEN3_INCLUDE_DIR}\n") 10 | else() 11 | message(STATUS "Eigen library not found!") 12 | endif() 13 | 14 | # Add the Eigen include directories 15 | include_directories(${EIGEN3_INCLUDE_DIR}) 16 | 17 | # Fast Essential Matrix Estimation 18 | add_executable(example_essential_matrix ${CMAKE_CURRENT_SOURCE_DIR}/example_essential_matrix.cpp) 19 | target_link_libraries(example_essential_matrix 20 | Essential 21 | Optimization) 22 | 23 | 24 | # GNC-RTNT Essential Matrix Estimation 25 | add_executable(example_GNC_TNT_essential_matrix ${CMAKE_CURRENT_SOURCE_DIR}/example_GNC_TNT_essential_matrix.cpp) 26 | 27 | target_link_libraries(example_GNC_TNT_essential_matrix 28 | Essential 29 | gncso 30 | Optimization) 31 | 32 | 33 | -------------------------------------------------------------------------------- /examples/example_GNC_TNT_essential_matrix.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include "generateCorrespondences.h" 9 | #include "Essential.h" 10 | #include "EssentialTypes.h" 11 | #include "EssentialUtils.h" 12 | 13 | #include "GNCEssential.h" 14 | 15 | 16 | using namespace std; 17 | using namespace Eigen; 18 | using namespace Essential; 19 | 20 | 21 | 22 | int main(int argc, char** argv) 23 | { 24 | std::cout << "Essential Matrix Estimation with GNC GM!\n"; 25 | 26 | // save general results 27 | std::ofstream fout("GNC_TNT_res.txt"); 28 | // save weights from the GNC 29 | std::ofstream fout_w("GNC_TNT_weights.txt"); 30 | 31 | double N_iter = 1; 32 | //set experiment parameters 33 | double noise = 0.1; 34 | size_t n_points = 100; 35 | double FoV = 150; // in degrees 36 | double parallax = 2.0; // in meters 37 | double min_depth = 1.; // in meters 38 | double max_depth = 8.; // in meters 39 | double outlier_fraction = 0.10; 40 | 41 | 42 | std::srand(std::time(nullptr)); 43 | 44 | for (int i = 0; i < N_iter; i++) 45 | { 46 | Vector3 translation; 47 | Matrix3 rotation; 48 | bearing_vectors_t points_correspondences; 49 | Eigen::MatrixXd points_3D(3, n_points); 50 | std::vector indices_outliers(1, n_points); 51 | createSyntheticExperiment(n_points, noise, outlier_fraction, FoV, parallax, min_depth, max_depth, translation, rotation, 52 | points_correspondences, points_3D, indices_outliers, false, 20, 0.3 ); 53 | 54 | 55 | 56 | // n_point 57 | Matrix3 E = computeEfromRt(rotation, translation); 58 | 59 | 60 | 61 | GNCEssentialEstimationOptions options = GNCEssentialEstimationOptions(); 62 | /* For GNC */ 63 | options.gnc_robust = GNCRobustFunction::WELSCH; 64 | options.GNC_verbose = 0; 65 | options.gnc_factor = 1.10; // for GM 66 | options.cost_diff_threshold = 0.000001; // stop criteria. if the diff between two cost is lower than this value, stop 67 | options.max_res_tol_sq = 0.00001; 68 | options.max_inner_iterations = 2; // with 4 (even less) you have enough 69 | 70 | // options.mu_threshold = 1+1e-08; // for GM 71 | // options.mu_threshold = 0.001; // for TLS 72 | options.GNClog_iterates = true; // log weights. NOTE: deactive this because memory 73 | options.inliers_threshold = 0.9; // for GM 74 | options.nr_min_points = 12; 75 | 76 | 77 | /* For the TNT solver */ 78 | 79 | options.chosen_initialisation = InitialisationMethod::PTS8_INIT; 80 | options.use_preconditioning = Preconditioner::Any; 81 | options.estimation_verbose = 0; 82 | 83 | 84 | 85 | // Instance of GNC estimator 86 | GNCEssentialClass my_essential_estimation(points_correspondences, options, E); 87 | 88 | // run the estimation 89 | GNCEssentialEstimationResult my_result = my_essential_estimation.getResultGNC(); 90 | std::cout << "GT R:\n" << rotation << std::endl; 91 | std::cout << "Estimated R:\n" << my_result.R_opt << std::endl; 92 | std::cout << "Solution is valid?: " << my_result.valid_estimation << std::endl; 93 | 94 | // Save results into a file 95 | /* 96 | {'iter','idx_iter','f_hat','d_hat','gradnorm','dual_gap','mu_min', 97 | 'elapsed_init_time','elapsed_C_time','elapsed_8pt_time','elapsed_time_methods', 98 | 'elapsed_iterative_time','elapsed_lagrange_time','elapsed_certifier_time','elapsed_estimation_time', 99 | 'distT','distR','is_opt'} 100 | */ 101 | fout << "iter: " << i; 102 | fout << " " << my_result.f_hat; 103 | fout << " " << my_result.d_hat; 104 | fout << " " << my_result.gradnorm; 105 | fout << " " << my_result.dual_gap; 106 | fout << " " << my_result.mu_min; 107 | fout << " " << my_result.elapsed_init_time; 108 | fout << " " << my_result.elapsed_C_time; 109 | fout << " " << my_result.elapsed_8pt_time; 110 | fout << " " << my_result.elapsed_time_methods; 111 | fout << " " << my_result.elapsed_iterative_time; 112 | fout << " " << my_result.elapsed_lagrange_time; 113 | fout << " " << my_result.elapsed_certifier_time; 114 | fout << " " << my_result.elapsed_estimation_time; 115 | fout << " " << distT(translation, my_result.t_opt); 116 | fout << " " << distR(rotation, my_result.R_opt); 117 | 118 | int is_opt = -1; 119 | switch(my_result.certifier_status) 120 | { 121 | case EssentialEstimationStatus::RS_ITER_LIMIT: 122 | is_opt = 2; 123 | break; 124 | case EssentialEstimationStatus::GLOBAL_OPT: 125 | is_opt = 0; 126 | break; 127 | case EssentialEstimationStatus::NO_CERTIFIED: 128 | is_opt = 1; 129 | break; 130 | default: 131 | is_opt = -1; 132 | } 133 | 134 | 135 | fout << " " << is_opt; 136 | 137 | fout << " " << my_result.valid_estimation << std::endl; 138 | 139 | 140 | std::cout << "Total time: " << my_result.elapsed_estimation_time << std::endl; 141 | std::cout << "Error rotation: " << distR(rotation, my_result.R_opt) << std::endl; 142 | std::cout << "Error translation: " << distT(translation, my_result.t_opt) << std::endl; 143 | std::cout << "GNC time: " << my_result.elapsed_iterative_time << std::endl; 144 | 145 | 146 | 147 | /* Saving weights */ 148 | // handle: fout_w 149 | // format: one file per iteration 150 | // number of outer iterations: my_result.nr_outer_iterations 151 | for (size_t id_outer = 0; id_outer < my_result.nr_outer_iterations; id_outer++) 152 | { 153 | // extract weights 154 | fout_w << id_outer; 155 | for (size_t id_w = 0; id_w < my_result.intermediate_outer_results[id_outer].weights.size(); id_w ++) 156 | { 157 | fout_w << "," << my_result.intermediate_outer_results[id_outer].weights[id_w]; 158 | } 159 | // add f(hat(x)) 160 | fout_w << "," << my_result.intermediate_outer_results[id_outer].f; 161 | fout_w << std::endl; 162 | } 163 | 164 | 165 | 166 | 167 | } 168 | // close file for general results 169 | fout.close(); 170 | // close file for weights 171 | fout_w.close(); 172 | 173 | return 0; 174 | 175 | } 176 | -------------------------------------------------------------------------------- /examples/example_essential_matrix.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include // for the file 7 | 8 | #include "generateCorrespondences.h" 9 | 10 | #include "Essential.h" 11 | #include "EssentialTypes.h" 12 | #include "EssentialUtils.h" 13 | 14 | 15 | 16 | 17 | using namespace std; 18 | using namespace Eigen; 19 | using namespace opengv; 20 | using namespace Essential; 21 | 22 | int main(int argc, char** argv) 23 | { 24 | std::cout << "Essential Matrix Estimation!\n"; 25 | 26 | std::ofstream fout("example_basic_res.txt"); 27 | double total_time = 0; 28 | double N_iter = 1; 29 | //set experiment parameters 30 | double noise = 5.5; 31 | size_t n_points = 200; 32 | double FoV = 100; // in degrees 33 | double max_parallax = 2.; // in meters 34 | double min_depth = 1.; // in meters 35 | double max_depth = 8.; // in meters 36 | double outlier_fraction = 0; 37 | 38 | 39 | std::srand(std::time(nullptr)); 40 | 41 | for (int i = 0; i < N_iter; i++) 42 | { 43 | 44 | 45 | Vector3 translation; 46 | Matrix3 rotation; 47 | bearing_vectors_t points_correspondences; 48 | Eigen::MatrixXd points_3D(3, n_points); 49 | std::vector indices_outliers(1, n_points); 50 | // FoV in degrees 51 | createSyntheticExperiment(n_points, noise, outlier_fraction, FoV, 52 | max_parallax, min_depth, max_depth, translation, rotation, 53 | points_correspondences, points_3D, indices_outliers); 54 | 55 | // n_point 56 | std::cout << "Computing essential matrix GT\n"; 57 | Matrix3 E = computeEfromRt(rotation, translation); 58 | 59 | 60 | 61 | 62 | 63 | std::cout << "Ground truth rotation:\n" << rotation << std::endl; 64 | std::cout << "Ground truth translation Tgt:\n" << translation << std::endl; 65 | std::cout << " \n-------------------------\n"; 66 | 67 | EssentialEstimationOptions options; 68 | options.chosen_initialisation = InitialisationMethod::PTS8_INIT; 69 | options.use_preconditioning = Preconditioner::Dominant_eigenvalues; 70 | options.verbose = 1; 71 | options.estimation_verbose=1; 72 | options.use_idx_relaxation = 6; 73 | 74 | EssentialClass my_essential_estimation(points_correspondences, options); 75 | 76 | // run the estimation 77 | 78 | std::cout << "Running algotihm on-manifold estimation\n"; 79 | EssentialEstimationResult my_result = my_essential_estimation.getResults(); 80 | 81 | 82 | my_essential_estimation.printResult(my_result); 83 | 84 | 85 | // Save results into a file 86 | /* 87 | {'iter','idx_iter','f_hat','d_hat','gradnorm','dual_gap','mu_min', 88 | 'elapsed_init_time','elapsed_C_time','elapsed_8pt_time','elapsed_time_methods', 89 | 'elapsed_iterative_time','elapsed_lagrange_time','elapsed_certifier_time','elapsed_estimation_time', 90 | 'distT','distR','is_opt'} 91 | */ 92 | fout << "iter: " << i; 93 | fout << " " << my_result.f_hat; 94 | fout << " " << my_result.d_hat; 95 | fout << " " << my_result.gradnorm; 96 | fout << " " << my_result.dual_gap; 97 | fout << " " << my_result.mu_min; 98 | fout << " " << my_result.elapsed_init_time; 99 | fout << " " << my_result.elapsed_C_time; 100 | fout << " " << my_result.elapsed_8pt_time; 101 | fout << " " << my_result.elapsed_time_methods; 102 | fout << " " << my_result.elapsed_iterative_time; 103 | fout << " " << my_result.elapsed_lagrange_time; 104 | fout << " " << my_result.elapsed_certifier_time; 105 | fout << " " << my_result.elapsed_estimation_time; 106 | fout << " " << distT(translation, my_result.t_opt); 107 | fout << " " << distR(rotation, my_result.R_opt); 108 | fout << " " << my_result.idx_relaxation; 109 | 110 | int is_opt = -1; 111 | switch(my_result.certifier_status) 112 | { 113 | case EssentialEstimationStatus::RS_ITER_LIMIT: 114 | is_opt = 2; 115 | break; 116 | case EssentialEstimationStatus::GLOBAL_OPT: 117 | is_opt = 0; 118 | break; 119 | case EssentialEstimationStatus::NO_CERTIFIED: 120 | is_opt = 1; 121 | break; 122 | default: 123 | is_opt = -1; 124 | } 125 | 126 | 127 | fout << " " << is_opt << std::endl; 128 | 129 | total_time += my_result.elapsed_estimation_time; 130 | std::cout << "Total time: " << my_result.elapsed_estimation_time << std::endl; 131 | std::cout << "Error rotation: " << distR(rotation, my_result.R_opt) << std::endl; 132 | std::cout << "Error translation: " << distT(translation, my_result.t_opt) << std::endl; 133 | 134 | } 135 | // close file 136 | fout.close(); 137 | std::cout << "Total time (total) [microsecs]: " << total_time << std::endl; 138 | std::cout << "Mean time (total) [microsecs]: " << total_time / N_iter << std::endl; 139 | 140 | return 0; 141 | 142 | } 143 | -------------------------------------------------------------------------------- /include/CorrespondencesVectors.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "EssentialTypes.h" 7 | 8 | namespace Essential{ 9 | 10 | /** A simple struct that contains the elements of a corresponding fatures as bearing (unit) vectors */ 11 | struct CorrespondingFeatures { 12 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 13 | Vector3 bearing_vector_0, bearing_vector_1; 14 | 15 | double weight_; 16 | /** Simple default constructor; does nothing */ 17 | CorrespondingFeatures() {} 18 | 19 | CorrespondingFeatures(const Vector3 & bearing_vector_0, 20 | const Vector3 & bearing_vector_1, 21 | double weight_match = 1.0) 22 | : weight_(weight_match), bearing_vector_0(bearing_vector_0), 23 | bearing_vector_1(bearing_vector_1) {} 24 | 25 | }; // end of CorrespondingFeatures struct 26 | 27 | // Define types 28 | typedef std::vector bearing_vectors_t; 29 | typedef Eigen::VectorXd weights_t; 30 | 31 | } // end of Esssential namespace 32 | 33 | -------------------------------------------------------------------------------- /include/Essential.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "EssentialTypes.h" 11 | #include "EssentialUtils.h" 12 | #include "EssentialProblem.h" 13 | #include "EssentialManifold.h" 14 | #include "EssentialVerification.h" 15 | 16 | #include "CorrespondencesVectors.h" 17 | #include 18 | #include 19 | #include "Optimization/Convex/Concepts.h" 20 | 21 | #include "Optimization/Riemannian/GradientDescent.h" 22 | #include "Optimization/Convex/ProximalGradient.h" 23 | 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | 30 | namespace Essential 31 | { 32 | enum class InitialisationMethod { 33 | PTS8_INIT = 0, 34 | PTS7_INIT, 35 | PTS5_INIT, 36 | RANDOM_INIT, 37 | EYE_INIT, 38 | USER_SUPPLIED, 39 | }; 40 | 41 | 42 | typedef Optimization::Riemannian::TNTUserFunction 43 | EssentialTNTUserFunction; 44 | 45 | /** This struct contains the various parameters that control the algorithm 46 | Based on: SESync struct*/ 47 | struct EssentialEstimationOptions { 48 | 49 | /// OPTIMIZATION STOPPING CRITERIA 50 | /** Stopping tolerance for the norm of the Riemannian gradient */ 51 | double tol_grad_norm = 1.e-9; 52 | 53 | /** Stopping criterion based upon the norm of an accepted update step */ 54 | double preconditioned_grad_norm_tol = 1.e-9; 55 | 56 | /** Stopping criterion based upon the relative decrease in function value */ 57 | double rel_func_decrease_tol = 1e-10; 58 | 59 | /** Stopping criterion based upon the norm of an accepted update step */ 60 | double stepsize_tol = 1e-03; 61 | 62 | /** Gradient tolerance for the truncated preconditioned conjugate gradient 63 | * solver: stop if ||g|| < kappa * ||g_0||. This parameter should be in the 64 | * range (0,1). */ 65 | double STPCG_kappa = 0.7; 66 | 67 | /** Gradient tolerance based upon a fractional-power reduction in the norm of 68 | * the gradient: stop if ||g|| < ||kappa||^{1+ theta}. This value should be 69 | * positive, and controls the asymptotic convergence rate of the 70 | * truncated-Newton trust-region solver: specifically, for theta > 0, the TNT 71 | * algorithm converges q-superlinearly with order (1+theta). */ 72 | double STPCG_theta = .5; 73 | 74 | /** Maximum permitted number of (outer) iterations of the RTR algorithm */ 75 | unsigned int max_RTR_iterations = 2000; 76 | /** Maximum number of inner (truncated conjugate-gradient) iterations to 77 | * perform per out iteration */ 78 | unsigned int max_tCG_iterations = 5; 79 | 80 | /// ESSENTIAL ESTIMATION PARAMS. 81 | // Absolute tolerance for the dual gap f - d 82 | double dual_gap_tol = 1e-10; 83 | 84 | // tol for the minimum eigenvalue of the (penalised) matrix 85 | double eig_min_tol = -0.02; 86 | 87 | // variable for the type of initialisation: 0 -> 8pts, 1->random, 2->eye, 3-> user supplied 88 | InitialisationMethod chosen_initialisation = InitialisationMethod::PTS8_INIT; 89 | 90 | /** Whether to use preconditioning in the trust regions solver */ 91 | Preconditioner use_preconditioning = Preconditioner::Any; 92 | 93 | /* Whether to try *all* the relaxations till one comes back positive 94 | or we run out of relaxations without a positive certifier */ 95 | size_t use_idx_relaxation = 0; // Note: use_idx_relaxation \in {0, 5} 96 | 97 | bool use_all_relaxations = true; // when FALSE, it runs relaxation 'use_idx_relaxation' 98 | // when TRUE, it runs UP TO relaxation 'use_idx_relaxation' 99 | 100 | // verbose = {0, 1} 101 | unsigned int estimation_verbose = 0; 102 | 103 | unsigned int verbose = 0; // for TNT 104 | 105 | 106 | /// DEFAULT CONSTRUCTOR with default values 107 | 108 | EssentialEstimationOptions() {}; 109 | 110 | 111 | }; // end of struct: EssentialEstimationOptions 112 | 113 | 114 | 115 | 116 | enum class EssentialEstimationStatus { 117 | /** The algorithm converged to a certified global optimum */ 118 | GLOBAL_OPT = 0, 119 | 120 | // The primal point couldnt be certified 121 | NO_CERTIFIED, 122 | 123 | /** The algorithm exhausted the maximum number of iterations of the Riemannian 124 | optimization*/ 125 | RS_ITER_LIMIT 126 | }; 127 | 128 | 129 | 130 | 131 | /** This struct contains the output of the Essential Estimation */ 132 | struct EssentialEstimationResult { 133 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 134 | 135 | // Primal objective value 136 | double f_hat; 137 | 138 | // Dual objective value 139 | double d_hat; 140 | 141 | // dual gap 142 | double dual_gap; 143 | // The norm of the Riemannian gradient at x_hat 144 | double gradnorm; 145 | 146 | // The minimum eigenvalue of the penalised matrix M 147 | double mu_min; 148 | 149 | /** The corresponding lagrange multipliers */ 150 | Vector6 lagrange_multipliers; 151 | 152 | /* condition number k(hessian) */ 153 | double condition_number; 154 | 155 | // Elapsed time for the initialisation 156 | double elapsed_init_time; 157 | // Elapsed time for C 158 | double elapsed_C_time; 159 | // Elapsed time for computing the 8 point init + preconditioner 160 | double elapsed_8pt_time; 161 | // Elapsed time for computing the initial estimation 162 | double elapsed_time_methods; 163 | // Elapsed time for the optimization on manifold 164 | double elapsed_iterative_time; 165 | // Elapsed time: lagrange multipliers 166 | double elapsed_lagrange_time; 167 | // Elapsed time for the certification 168 | double elapsed_certifier_time; 169 | // Elapsed time for the whole estimation: initialisation + optimization + verification 170 | double elapsed_estimation_time; 171 | 172 | // index of the relaxation which could certify optimality. 173 | // idx_relaxation \in {0, 5}; idx_relaxation = 10 (no certified) 174 | size_t idx_relaxation; 175 | 176 | // Output rotation matrix 177 | Matrix3 E_opt; 178 | 179 | // Output rotation matrix 180 | Matrix3 R_opt; 181 | // Output translation vector 182 | Vector3 t_opt; 183 | // Status 184 | EssentialEstimationStatus certifier_status; 185 | 186 | /* Default constructor */ 187 | EssentialEstimationResult() {}; 188 | 189 | }; // end of EssentialEstimationResult struct 190 | 191 | 192 | 193 | 194 | class EssentialClass 195 | { 196 | public: 197 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 198 | /* Default constructor */ 199 | EssentialClass(void){}; 200 | 201 | EssentialClass( const bearing_vectors_t & points, 202 | const EssentialEstimationOptions & options = EssentialEstimationOptions(), 203 | const Matrix3 & E_initial = Matrix3()):points_(points), E_initial_(E_initial), options_(options) {}; 204 | 205 | EssentialClass( Eigen::Matrix & bearingVectors0, 206 | Eigen::Matrix & bearingVectors1, 207 | Eigen::Matrix & weights_observations, 208 | const EssentialEstimationOptions & options = EssentialEstimationOptions(), 209 | const Matrix3 & E_initial = Matrix3()): E_initial_(E_initial), options_(options) 210 | { 211 | bearing_vectors_t points = 212 | convertPointsVector32PointsOpt(bearingVectors0, bearingVectors1, weights_observations); 213 | points_ = points; 214 | }; 215 | 216 | 217 | ~EssentialClass(void){}; 218 | 219 | EssentialEstimationResult getResults(void); 220 | 221 | void printResult(EssentialEstimationResult & results); 222 | 223 | private: 224 | bearing_vectors_t points_; 225 | Matrix3 E_initial_; 226 | EssentialEstimationOptions options_; 227 | 228 | }; //end of essential class 229 | 230 | } // end of essential namespace 231 | 232 | -------------------------------------------------------------------------------- /include/EssentialManifold.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include // For sampling random points on the manifold 4 | 5 | #include 6 | 7 | #include "EssentialTypes.h" 8 | 9 | /*Define the namespace*/ 10 | namespace Essential{ 11 | 12 | class EssentialManifold{ 13 | 14 | 15 | public: 16 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 17 | /* Default constructor */ 18 | EssentialManifold(void){}; 19 | 20 | /*Delete each component manifold*/ 21 | ~EssentialManifold(void){}; 22 | 23 | /// GEOMETRY 24 | /** Given a generic matrix A in R^{3 x 4}, this function computes the 25 | * projection of A onto R (closest point in the Frobenius norm sense). */ 26 | Matrix34 project(const Matrix34 &A) const; 27 | 28 | 29 | /** Given an element Y in M and a tangent vector V in T_Y(M), this function 30 | * computes the retraction along V at Y using the QR-based retraction 31 | * specified in eq. (4.8) of Absil et al.'s "Optimization Algorithms on 32 | * Matrix Manifolds"). 33 | */ 34 | Matrix34 retract(const Matrix34 &Y, const Matrix34 &V) const; 35 | 36 | /* Projections for each manifold */ 37 | /// Sphere 38 | Vector3 ProjSphere(const Vector3 &t, const Vector3 &Vt) const; 39 | /// Rotation 40 | Matrix3 ProjRotation(const Matrix3& R, const Matrix3 & VR) const; 41 | 42 | // Product of the form: A * symm(B * C), where all the matrices are 3x3 43 | Matrix3 SymProduct(const Matrix3 & A, const Matrix3 B, const Matrix3 & C) const; 44 | 45 | /** Sample a random point on M, using the (optional) passed seed to initialize 46 | * the random number generator. */ 47 | Matrix34 random_sample(const std::default_random_engine::result_type &seed = 48 | std::default_random_engine::default_seed) const; 49 | 50 | 51 | /** Given an element Y in M and a matrix V in T_X(R^{p x kn}) (that is, a (p 52 | * x kn)-dimensional matrix V considered as an element of the tangent space to 53 | * the *entire* ambient Euclidean space at X), this function computes and 54 | * returns the projection of V onto T_X(M), the tangent space of M at X (cf. 55 | * eq. (42) in the SE-Sync tech report).*/ 56 | Matrix34 Proj(const Matrix34 &Y, const Matrix34 &V) const; 57 | 58 | }; 59 | } /*end of Essential namespace*/ 60 | 61 | -------------------------------------------------------------------------------- /include/EssentialProblem.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | 4 | 5 | /* Essential manifold related */ 6 | #include "EssentialTypes.h" 7 | #include "EssentialManifold.h" 8 | #include "CorrespondencesVectors.h" 9 | #include "EssentialUtils.h" 10 | 11 | #include 12 | 13 | namespace Essential { 14 | 15 | 16 | struct ProblemCachedMatrices{ 17 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 18 | Matrix34 NablaF_Y; 19 | Matrix3 Mt; 20 | Matrix9 Mr; 21 | /// DEFAULT CONSTRUCTOR with default values 22 | ProblemCachedMatrices( const Matrix34 & Nabla = Matrix34::Identity(), 23 | const Matrix3 & mt = Matrix3::Identity(), 24 | const Matrix9 & mr = Matrix9::Identity()) : 25 | NablaF_Y(Nabla), Mt(mt), Mr(mr){} 26 | }; 27 | 28 | 29 | class EssentialProblem{ 30 | public: 31 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 32 | EssentialProblem(){}; // Default 33 | 34 | // Constructor using two vectors of 3XN corresponding features 35 | EssentialProblem(const bearing_vectors_t & bearing_vectors, Preconditioner use_precon = Preconditioner::None); 36 | 37 | void setNumberPoints(const bearing_vectors_t& bearing_vectors); 38 | 39 | EssentialProblem(const Matrix9 & data_matrix_C, Preconditioner use_precon = Preconditioner::None); 40 | 41 | ~EssentialProblem(); 42 | 43 | Matrix9 getDataMatrixC(void) {return data_matrix_C_;} 44 | 45 | void setPointCorrespondences(const bearing_vectors_t & bearing_vectors) {point_correspondences_ = bearing_vectors; number_points_ = bearing_vectors.size();} 46 | 47 | 48 | void setMatrixPrecon(Matrix3 & matrix_precon) {Matrix_precon_ = matrix_precon;} 49 | 50 | 51 | // Initialize solver with 8 points 52 | Matrix34 initializeSolver8pts(void); 53 | 54 | // This preconditioner uses P = Mt 55 | Matrix3 invMtPrecon(Matrix3 & R_init); 56 | 57 | // Pseudo jacobi preconditioner based on the three largest eigenvalues of C 58 | Matrix3 computePseudoJacobiPrecon(void); 59 | 60 | 61 | /// ACCESSORS 62 | 63 | /** Get a const pointer to the SO(3) x S(2) product manifold */ 64 | const EssentialManifold& getEssentialManifold() const { 65 | return domain_; 66 | } 67 | 68 | 69 | Matrix9 getDataMatrix(void) {return data_matrix_C_;} 70 | 71 | void setMr(const Matrix9 & M) {Mr_ = M;}; 72 | void setMt(const Matrix3 & M) {Mt_ = M;}; 73 | 74 | Matrix3 getMt(void) {return Mt_; }; 75 | Matrix9 getMr(void) {return Mr_; }; 76 | /// OPTIMIZATION AND GEOMETRY 77 | 78 | /** Given a matrix Y, this function computes and returns F(Y), the value of 79 | * the objective evaluated at X */ 80 | double evaluate_objective(const Matrix34 &Y) const; 81 | 82 | double evaluate_objective(const Matrix34 &Y, ProblemCachedMatrices & problem_matrices) const; 83 | 84 | /** Given a matrix Y, this function computes and returns nabla F(Y), the 85 | * *Euclidean* gradient of F at Y. */ 86 | Matrix34 Euclidean_gradient(const Matrix34 &Y) const; 87 | 88 | Matrix34 Euclidean_gradient(const Matrix34 &Y, const ProblemCachedMatrices & problem_matrices) const; 89 | 90 | /** Given a matrix Y in the domain D of the SE-Sync optimization problem and 91 | * the *Euclidean* gradient nabla F(Y) at Y, this function computes and 92 | * returns the *Riemannian* gradient grad F(Y) of F at Y */ 93 | 94 | Matrix34 Riemannian_gradient(const Matrix34 &Y, const ProblemCachedMatrices & problem_matrices) const; 95 | 96 | Matrix34 Riemannian_gradient(const Matrix34 &Y, const Matrix34 &nablaF_Y) const; 97 | 98 | Matrix34 Riemannian_gradient(const Matrix34 &Y) const; 99 | 100 | /* Preconditioner */ 101 | Matrix34 precondition(const Matrix34& X, const Matrix34 & Xdot) const; 102 | 103 | Matrix34 preconditionRight(const Matrix34& X, const Matrix34 & Xdot) const; 104 | 105 | /** Given a matrix Y in the domain D of the SE-Sync optimization problem, the 106 | * *Euclidean* gradient nablaF_Y of F at Y, and a tangent vector dotY in 107 | * T_D(Y), the tangent space of the domain of the optimization problem at Y, 108 | * this function computes and returns Hess F(Y)[dotY], the action of the 109 | * Riemannian Hessian on dotY */ 110 | 111 | Matrix34 Riemannian_Hessian_vector_product(const Matrix34 &Y, 112 | const ProblemCachedMatrices & problem_matrices, 113 | const Matrix34 &dotY) const; 114 | 115 | Matrix34 Riemannian_Hessian_vector_product(const Matrix34 &Y, 116 | const Matrix34 &nablaF_Y, 117 | const Matrix34 &dotY) const; 118 | 119 | 120 | Matrix34 Riemannian_Hessian_vector_product(const Matrix34 &Y, 121 | const Matrix34 &dotY) const; 122 | 123 | Matrix34 Riemannian_Hessian_vector_product(const Matrix34 &Y, 124 | const Matrix3 & Mt, 125 | const Matrix9 & Mr, 126 | const Matrix34 &dotY) const; 127 | 128 | /** Given a matrix Y in the domain D of the SE-Sync optimization problem and a 129 | tangent vector dotY in T_Y(E), the tangent space of Y considered as a generic 130 | matrix, this function computes and returns the orthogonal projection of dotY 131 | onto T_D(Y), the tangent space of the domain D at Y*/ 132 | Matrix34 tangent_space_projection(const Matrix34 &Y, const Matrix34 &dotY) const; 133 | 134 | /** Given a matrix Y in the domain D of the SE-Sync optimization problem and a 135 | * tangent vector dotY in T_D(Y), this function returns the point Yplus in D 136 | * obtained by retracting along dotY */ 137 | Matrix34 retract(const Matrix34 &Y, const Matrix34 &dotY) const; 138 | 139 | Matrix34 random_sample() const; 140 | 141 | // compute Residuals 142 | weights_t computeResiduals(const Matrix34 & Rt) const; 143 | 144 | // update points with weights 145 | void updateWeights(const weights_t & new_weights); // This function modifies the weights 146 | 147 | private: 148 | 149 | size_t number_points_; 150 | 151 | bearing_vectors_t point_correspondences_; // with weights!! 152 | 153 | Preconditioner use_precon_; 154 | 155 | /** The product manifold of SO(3) X S(2) ~ E(3) that is the domain of our method */ 156 | EssentialManifold domain_; 157 | 158 | Matrix9 data_matrix_C_; 159 | 160 | Matrix3 Mt_; 161 | 162 | Matrix9 Mr_; 163 | 164 | Matrix3 Matrix_precon_; 165 | 166 | // Matrices for the mixed term in the hessian 167 | Matrix9 B1_, B2_, B3_; 168 | 169 | // matrices for the preconditioner: B^T * C * B 170 | Matrix93 B_; 171 | 172 | Matrix4 Matrix_precon_right_; 173 | 174 | }; // end of Essential problem class 175 | 176 | 177 | 178 | 179 | } // end of Essential namespace 180 | 181 | 182 | -------------------------------------------------------------------------------- /include/EssentialTypes.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace Essential{ 6 | 7 | typedef Eigen::Matrix Vector3; 8 | typedef Eigen::Matrix Vector4; 9 | typedef Eigen::Matrix Vector5; 10 | typedef Eigen::Matrix Vector6; 11 | typedef Eigen::Matrix Vector7; 12 | typedef Eigen::Matrix Vector9; 13 | typedef Eigen::Matrix Vector12; 14 | 15 | typedef Eigen::Matrix Matrix3; 16 | typedef Eigen::Matrix Matrix4; 17 | typedef Eigen::Matrix Matrix9; 18 | typedef Eigen::Matrix Matrix39; 19 | typedef Eigen::Matrix Matrix93; 20 | typedef Eigen::Matrix Matrix34; 21 | typedef Eigen::Matrix Matrix12; 22 | 23 | typedef Eigen::Matrix Matrix9By12; 24 | typedef Eigen::Matrix Matrix3By12; 25 | 26 | 27 | 28 | 29 | enum class Preconditioner { 30 | None= 0, 31 | Max_eigenvalue, 32 | Dominant_eigenvalues, 33 | N, 34 | Any, 35 | }; 36 | 37 | } // end of essential namespace 38 | 39 | -------------------------------------------------------------------------------- /include/EssentialUtils.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | 4 | #include // required for SVD decomposition in unlift method 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | #include "EssentialTypes.h" 11 | #include "CorrespondencesVectors.h" 12 | 13 | #include // Not sure if we need this 14 | #include 15 | #include 16 | 17 | #define SQRT2 1.41421356237 18 | #define R180PI 57.2957795131 19 | 20 | /* Define namespace */ 21 | namespace Essential{ 22 | 23 | 24 | 25 | // Return the cross-product for 3D vectors as a 3x3 matrix 26 | Matrix3 cross(const Vector3 & t); 27 | 28 | // Return the 9D vector for a 3x3 matrix (column-wise) 29 | Vector9 vec(Matrix3 & M); 30 | 31 | // Return the 9D vector for a 3x3 matrix (column-wise) 32 | Eigen::MatrixXf skew(const Eigen::MatrixXf & M); 33 | 34 | 35 | // only points. Basic 36 | Matrix3 initialize8pts(const bearing_vectors_t & points); 37 | 38 | // 8pts initialization 39 | Matrix3 initialize8pts(const bearing_vectors_t & points, const Matrix9 & C, Matrix3 & Jacobi_precon, Preconditioner use_precon); 40 | 41 | // Same as above but with C instead of the points. 42 | // In practice, they are the same 43 | Matrix3 initialize8pts(const Matrix9 & C , Matrix3 & Jacobi_precon, Preconditioner use_precon); 44 | 45 | /* minimal solvers */ 46 | Matrix3 minimalinitialize8pts(const bearing_vectors_t & points, bool use_all_corr = false); 47 | // Use all the points 48 | Matrix3 minimalinitialize8ptsAll(const bearing_vectors_t & points); 49 | // use only 8 points 50 | Matrix3 minimalinitialize8ptsMinimal(const bearing_vectors_t & points); 51 | 52 | 53 | // 5pts initialization 54 | Matrix3 initialize5pts(const bearing_vectors_t & points, bool use_all_corr = false); 55 | 56 | // use all points 57 | Matrix3 initialize5ptsAll(const bearing_vectors_t & points); 58 | 59 | // use only 5 60 | Matrix3 initialize5ptsMinimal(const bearing_vectors_t & points); 61 | 62 | 63 | // triangulate points 64 | Vector3 triangulatePoint(const Matrix3 & R, const Vector3 & t, const Vector3 & f0, const Vector3& f1); 65 | 66 | 67 | // 7pts initialization 68 | Matrix3 initialize7pts(const bearing_vectors_t & points, bool use_all_corr = false); 69 | 70 | // use all points 71 | Matrix3 initialize7ptsAll(const bearing_vectors_t & points); 72 | 73 | // use only 7 points 74 | Matrix3 initialize7ptsMinimal(const bearing_vectors_t & points); 75 | 76 | 77 | // Project to rotation group 78 | Matrix3 projectToRotation(const Matrix3 & R_hat); 79 | 80 | // Project to Me 81 | Matrix3 projectToEssentialManifold(const Matrix3 & E_hat); 82 | 83 | // Construct That such that: vec(E) = e = Rhat * t 84 | Matrix39 createRhat(const Matrix3& R); 85 | 86 | // Construct That such that: vec(E) = e = that * r = that * vec(R) 87 | Matrix9 createThat(const Vector3 & t); 88 | 89 | // construct Mt, such that: f(E) = r^T * Mrt * r, r = vec(R), Mt = t_hat^T * C * t_hat 90 | Matrix3 createMatrixT(const Matrix9 & C, const Matrix3 & R); 91 | 92 | // construct Mr, such that: f(E) = t^T * Mr * t, Mr = R_hat^T * C * R_hat 93 | Matrix9 createMatrixR(const Matrix9 & C, const Vector3 & t); 94 | 95 | // construct C = sum_{i=1}^N C_i 96 | Matrix9 constructDataMatrix(const bearing_vectors_t & bearing_vectors); 97 | 98 | // construct C = sum_{i=1}^N C_i 99 | Matrix9 constructDataMatrix(const bearing_vectors_t & bearing_vectors, const weights_t & new_weights); 100 | 101 | 102 | // Construct B's matrices, use in the derivate 103 | void constructBMatricesForMixedDiftR(Matrix9& B1, Matrix9 & B2, Matrix9 & B3); 104 | 105 | // Construct matrix for mixed derivate 106 | Matrix39 constructMixedDifTtr(const Matrix9 & B1, const Matrix9 & B2, 107 | const Matrix9 & B3,const Matrix9 & C , const Vector3 & t, Matrix3 & R); 108 | 109 | 110 | // Symmetrize the given matrix A = matrix_data. output: 0.5*(A + A^T) 111 | Eigen::MatrixXf symmetrize(const Eigen::MatrixXf & matrix_data); 112 | 113 | 114 | // Extract R, t from E: 115 | void computeRtfromE(const bearing_vectors_t & points, const Matrix3& E_hat, Matrix3 & R, Vector3 & t ); 116 | 117 | // Compute E from R, t 118 | Matrix3 computeEfromRt(const Matrix3 & R, const Vector3 & t); 119 | 120 | Matrix3 computeEfromRt(const Matrix34& Rt); 121 | 122 | // Transform from Vector3 (eigen) to our struct 123 | bearing_vectors_t convertPointsVector32PointsOpt(std::vector> & bearingVectors0, 124 | std::vector> & bearingVectors1); 125 | 126 | bearing_vectors_t convertPointsVector32PointsOpt(std::vector> & bearingVectors0, 127 | std::vector> & bearingVectors1, 128 | std::vector & weights_observations); 129 | 130 | bearing_vectors_t convertPointsVector32PointsOpt(Eigen::Matrix & bearingVectors0, 131 | Eigen::Matrix & bearingVectors1, 132 | Eigen::Matrix & weights_observations); 133 | 134 | // compute distance (in geodesic sense) between two rotations (See paper) [degrees] 135 | double distR(const Matrix3 & R_gt, const Matrix3 & R_est); 136 | // Compute distance (in direction) between two (unitary) translation vectors [degrees] 137 | double distT(const Vector3 & T_gt, const Vector3 & T_est); 138 | // Compute similarity between two essential matrices [degrees] 139 | double distE(const Matrix3 & E_gt, const Matrix3 & E_est); 140 | // Evaluate essential matrix, rotation and translation direction (everything in [degreees]) 141 | 142 | std::vector selectKPoints(int max_n, int k); 143 | 144 | void evaluateRTE(const Matrix3 & R_gt, const Matrix3 & R_est, 145 | const Vector3 & T_gt, const Vector3 & T_est, 146 | const Matrix3 & E_gt, const Matrix3 & E_est, 147 | double& dist_R, double & dist_T, double & dist_E); 148 | } // end of essential namespace 149 | 150 | 151 | -------------------------------------------------------------------------------- /include/EssentialVerification.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "EssentialTypes.h" 4 | #include "EssentialUtils.h" 5 | #include 6 | 7 | 8 | namespace Essential{ 9 | class EssentialVerification{ 10 | 11 | public: 12 | 13 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 14 | /* Default constructor*/ 15 | EssentialVerification(void); 16 | 17 | /* Actual constructor */ 18 | 19 | EssentialVerification(const Matrix9 & C, const Matrix3 & E, 20 | const Vector3 & t, const double f_hat, 21 | const double tau = -0.02, 22 | const double epsilon_dual = 1e-14): 23 | C_(C), E_(E), t_(t), f_hat_(f_hat), tau_(tau), epsilon_dual_(epsilon_dual) {}; 24 | 25 | 26 | /* Default destructor */ 27 | ~EssentialVerification(void){}; 28 | 29 | 30 | /* Compute lagrange multipliers */ 31 | Vector6 computeLagMult(Matrix12 & Q); 32 | 33 | // compute batch of lagrange multipliers (all the relaxations) 34 | Vector6 computeLagMultGeneral(Matrix12 & Q, size_t idx_relaxation); 35 | 36 | /* Certify optimality */ 37 | // NOTE: include all the relaxations 38 | bool checkOptimalitySolution(const Vector6 & Lagrange_multipliers, 39 | const Matrix12 & Q, double& mu_min, 40 | double& dual_gap, double& d_hat, 41 | size_t idx_relaxation = 0); 42 | 43 | 44 | /* Compute penalised matrix M */ 45 | double computePenalisedMatrixMAndMinEigenvalue(const Vector6 & Lagrange_multipliers, const Matrix12 & Q); 46 | 47 | /* compute Hessian of Lagrangian for each relaxation */ 48 | double computePenalisedMatrixMAndMinEigenvalueGeneral(const Vector6 & Lagrange_multipliers, 49 | const Matrix12 & Q, size_t idx_relaxation); 50 | 51 | 52 | 53 | private: 54 | Vector6 Lagrange_multipliers_; 55 | // Min eigenvalue 56 | double mu_min_; 57 | // Dual gap 58 | double dual_gap_; 59 | 60 | // Value for the primal & dual objectives 61 | double f_hat_; 62 | double dual_hat_; 63 | 64 | 65 | // Thresholds 66 | double tau_; 67 | double epsilon_dual_; 68 | 69 | // The problem matrices 70 | Matrix9 C_; 71 | Matrix3 E_; 72 | Vector3 t_; 73 | 74 | 75 | 76 | 77 | }; // end of Essential verification clas 78 | } // end of essential namespace 79 | -------------------------------------------------------------------------------- /include/GNCEssential.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | // Essential estimation related 13 | #include "EssentialTypes.h" 14 | #include "EssentialUtils.h" 15 | #include "EssentialProblem.h" 16 | #include "EssentialManifold.h" 17 | #include "EssentialVerification.h" 18 | #include "Essential.h" 19 | 20 | // generate correspondences 21 | #include "CorrespondencesVectors.h" 22 | 23 | // smooth optimization 24 | #include "Optimization/Riemannian/Concepts.h" 25 | #include "Optimization/Riemannian/TNT.h" 26 | #include "Optimization/Riemannian/GradientDescent.h" 27 | #include "Optimization/Convex/Concepts.h" 28 | #include "Optimization/Convex/ProximalGradient.h" 29 | 30 | // GNC related 31 | #include "gncso/Smooth/gnc_smooth.h" 32 | 33 | // gnc 34 | using namespace gncso; 35 | // Smooth 36 | using namespace Optimization; 37 | using namespace Optimization::Riemannian; 38 | 39 | // essential matrix 40 | namespace Essential { 41 | 42 | enum class GNCRobustFunction { 43 | None = 0, 44 | TLS, 45 | GM, 46 | WELSCH, 47 | TUKEY, 48 | Any, 49 | }; 50 | 51 | 52 | struct GNCEssentialEstimationOptions : public GNCParams, public EssentialEstimationOptions 53 | { 54 | GNCRobustFunction gnc_robust {GNCRobustFunction::Any}; 55 | 56 | GNCEssentialEstimationOptions(): GNCParams(), EssentialEstimationOptions() {}; 57 | }; 58 | 59 | 60 | struct GNCEssentialEstimationResult : public GNCResult, public EssentialEstimationResult 61 | { 62 | // flag for valid estimation 63 | bool valid_estimation = false; 64 | 65 | GNCEssentialEstimationResult(): EssentialEstimationResult(), GNCResult() {}; 66 | }; 67 | 68 | 69 | 70 | class GNCEssentialClass : public EssentialClass 71 | { 72 | public: 73 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 74 | /* Default constructor */ 75 | GNCEssentialClass(void) : EssentialClass() {}; 76 | 77 | GNCEssentialClass( const bearing_vectors_t & points, 78 | const GNCEssentialEstimationOptions & options = GNCEssentialEstimationOptions(), 79 | const Matrix3 & E_initial = Matrix3() ) :points_(points), E_initial_(E_initial), options_(options), 80 | EssentialClass( points, options, E_initial) {}; 81 | 82 | GNCEssentialClass( Eigen::Matrix & bearingVectors0, 83 | Eigen::Matrix & bearingVectors1, 84 | Eigen::Matrix & weights_observations, 85 | const GNCEssentialEstimationOptions & options = GNCEssentialEstimationOptions(), 86 | const Matrix3 & E_initial = Matrix3() ) : E_initial_(E_initial), options_(options), 87 | EssentialClass( bearingVectors0, bearingVectors1, weights_observations, options, E_initial) 88 | { 89 | bearing_vectors_t points = 90 | convertPointsVector32PointsOpt(bearingVectors0, bearingVectors1, weights_observations); 91 | points_ = points; 92 | }; 93 | 94 | ~GNCEssentialClass(void) {}; 95 | 96 | 97 | GNCEssentialEstimationResult getResultGNC(void); 98 | 99 | void printResultGNC(GNCEssentialEstimationResult & results); 100 | 101 | private: 102 | 103 | bearing_vectors_t points_; 104 | Matrix3 E_initial_; 105 | 106 | GNCEssentialEstimationOptions options_; 107 | 108 | 109 | }; //end of essential class} 110 | 111 | 112 | 113 | } // end of essential namespace 114 | -------------------------------------------------------------------------------- /include/generateCorrespondences.h: -------------------------------------------------------------------------------- 1 | /* 2 | Original from Opengv. 3 | Adaptation for the (central case) relative pose problem 4 | */ 5 | 6 | #pragma once 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | 17 | #include 18 | 19 | #include "EssentialTypes.h" 20 | #include "CorrespondencesVectors.h" 21 | 22 | namespace Essential 23 | { 24 | 25 | // Generate 3D point inside a frustum 26 | Vector3 generateRandomPointTruncated(double FoV, double min_depth, double max_depth ); 27 | Vector3 generateRandomPointTruncated(double FoV, double min_depth, double max_depth, bool allow_coplanar, double max_X ); 28 | 29 | // add noise as explained in the paper(s) 30 | Vector3 addNoise( double noiseLevel, Vector3 cleanPoint, double focal_length ); 31 | // generate random translation with the specified norm 32 | Vector3 generateRandomTranslation( double max_parallax ); 33 | // generate random rotation with maxAngle 34 | Matrix3 generateRandomRotation( double maxAngle ); 35 | 36 | Vector3 generateRandomPointNaive(double min_depth, double max_depth); 37 | 38 | 39 | void createSyntheticExperiment( 40 | const size_t n_points, 41 | double noise, 42 | double outlier_fraction, 43 | double FoV, 44 | double max_parallax, 45 | double min_depth, 46 | double max_depth, 47 | Vector3 & translation, 48 | Matrix3 & rotation, 49 | bearing_vectors_t & points_correspondences, 50 | Eigen::MatrixXd & gt, 51 | std::vector & indices_outliers, 52 | bool allow_coplanar, // allow to generate synthetic scenes with coplanar points 53 | double max_X, // this value is the absolute value. Max value for X-axis allowed 54 | double min_epipolar_error_sq 55 | ); 56 | 57 | void createSyntheticExperiment( 58 | const size_t n_points, 59 | double noise, 60 | double outlier_fraction, 61 | double FoV, 62 | double max_parallax, 63 | double min_depth, 64 | double max_depth, 65 | Vector3 & translation, 66 | Matrix3 & rotation, 67 | bearing_vectors_t & points_correspondences, 68 | Eigen::MatrixXd & gt, 69 | std::vector & indices_outliers, 70 | double min_epipolar_error_sq 71 | ); 72 | 73 | 74 | void createSyntheticExperiment( 75 | const size_t n_points, 76 | double noise, 77 | double outlier_fraction, 78 | double FoV, 79 | double max_parallax, 80 | double min_depth, 81 | double max_depth, 82 | Vector3 & translation, 83 | Matrix3 & rotation, 84 | bearing_vectors_t & points_correspondences, 85 | Eigen::MatrixXd & gt, 86 | std::vector & indices_outliers, 87 | bool allow_coplanar, // allow to generate synthetic scenes with coplanar points 88 | double max_X // this value is the absolute value. Max value for X-axis allowed 89 | ); 90 | 91 | 92 | void createSyntheticExperiment( 93 | const size_t n_points, 94 | double noise, 95 | double outlier_fraction, 96 | double FoV, 97 | double max_parallax, 98 | double min_depth, 99 | double max_depth, 100 | Vector3 & translation, 101 | Matrix3 & rotation, 102 | bearing_vectors_t & points_correspondences, 103 | Eigen::MatrixXd & gt, 104 | std::vector & indices_outliers ); 105 | 106 | 107 | void createSyntheticExperiment( 108 | const size_t n_points, 109 | double noise, 110 | double outlier_fraction, 111 | double FoV, 112 | double parallax, 113 | double min_depth, 114 | double max_depth, 115 | Vector3 & translation, 116 | Matrix3 & rotation, 117 | bearing_vectors_t & points_correspondences, 118 | Eigen::MatrixXd & gt, 119 | std::vector & indices_outliers, 120 | bool allow_coplanar, // allow to generate synthetic scenes with coplanar points 121 | double max_X, // this value is the absolute value. Max value for X-axis allowed 122 | double min_epipolar_error_sq, // min epipolar error for the outliers 123 | double focal_length // focal length in pixelss 124 | ); 125 | 126 | } // end of essential namespace 127 | 128 | 129 | 130 | -------------------------------------------------------------------------------- /matlab/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | # Following TEASER++ 4 | message(STATUS "-- Building matlab binding for essential matrix estimation!") 5 | 6 | 7 | project(essential_matrix_estimation_matlab_bindings) 8 | 9 | find_package(Matlab COMPONENTS MX_LIBRARY) 10 | 11 | if (Matlab_FOUND) 12 | message(STATUS "MATLAB root directory found: ${Matlab_ROOT_DIR}.") 13 | 14 | # no GNC 15 | matlab_add_mex( 16 | NAME EssentialMatrixMex 17 | OUTPUT_NAME EssentialMatrixMex 18 | SRC EssentialMatrixMex.cc 19 | LINK_TO Eigen3::Eigen Essential 20 | ) 21 | set_target_properties(EssentialMatrixMex PROPERTIES COMPILE_FLAGS "-fvisibility=default") 22 | 23 | 24 | 25 | # GNC 26 | matlab_add_mex( 27 | NAME GNCEssentialMatrixMex 28 | OUTPUT_NAME GNCEssentialMatrixMex 29 | SRC GNCEssentialMatrixMex.cc 30 | LINK_TO Eigen3::Eigen Essential 31 | ) 32 | set_target_properties(GNCEssentialMatrixMex PROPERTIES COMPILE_FLAGS "-fvisibility=default") 33 | 34 | 35 | 36 | 37 | # copy MATLAB .m files to binary directory 38 | file(COPY . 39 | DESTINATION . 40 | FILES_MATCHING 41 | PATTERN *.m) 42 | else () 43 | message(WARNING "MATLAB root directory not found. Failed to build MATLAB bindings.") 44 | 45 | endif () 46 | -------------------------------------------------------------------------------- /matlab/EssentialMatrixEstimate.m: -------------------------------------------------------------------------------- 1 | function [struct_output] = EssentialMatrixEstimate(obsi, obsip, weights, varargin) 2 | 3 | 4 | 5 | assert(size(obsi, 1) == 3, 'obsi must be a 3-by-N matrix.') 6 | assert(size(obsip, 1) == 3, 'obsip must be a 3-by-N matrix.') 7 | assert(size(weights, 1) == 1, 'weights must be a 1-by-N matrix.') 8 | 9 | 10 | params = inputParser; 11 | params.CaseSensitive = false; 12 | 13 | addParameter(params, 'init_method', 0, ... 14 | @(x) isnumeric(x) && isscalar(x) && x>=0 && x<=6); 15 | 16 | addParameter(params, 'precon', 1, ... 17 | @(x) isnumeric(x) && isscalar(x) && x>=0 && x<=4); 18 | 19 | addParameter(params, 'use_mult_certifiers', 1, ... 20 | @(x) isnumeric(x) && isscalar(x) && x>=0 && x<=6); 21 | 22 | addParameter(params,'verbose', false, ... 23 | @(x) islogical(x)); 24 | 25 | parse(params, varargin{:}); 26 | 27 | 28 | [R, t, time_iterative, time_total, is_opt] = EssentialMatrixMex(obsi, obsip, weights, ... 29 | params.Results.init_method, ... 30 | params.Results.precon, ... 31 | params.Results.use_mult_certifiers, ... 32 | params.Results.verbose); 33 | 34 | % struct_output = structEssentialEstimation; 35 | struct_output = struct(); 36 | struct_output.R = R; 37 | struct_output.t = t; 38 | struct_output.time_iterative = time_iterative; 39 | struct_output.time_total = time_total; 40 | struct_output.is_opt = is_opt; 41 | 42 | % class structEssentialEstimation 43 | % properties 44 | % R 45 | % t 46 | % time_iterative 47 | % time_total 48 | % end 49 | 50 | % end 51 | 52 | 53 | end 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /matlab/EssentialMatrixEstimationTest.m: -------------------------------------------------------------------------------- 1 | close all; clear all; clc; 2 | 3 | 4 | rng('shuffle'); 5 | % Test parameters: 6 | % Default params. 7 | noise=1.5; 8 | FoV=100; 9 | N = 12; 10 | N = N; 11 | % for t 12 | min_parallax=0.5; 13 | max_parallax=2.0; 14 | % for the points 15 | min_depth=1.0; 16 | max_depth=8.0; 17 | 18 | f = 800; % focal length 19 | % visualization 20 | 21 | 22 | 23 | % weights vector 24 | W = ones(N, 1); 25 | 26 | % 1. Generate data 27 | [P1, P2, tgt, Rgt, indices_outliers] = create2D2DCorrespondencesNOutliers(N, noise, 0, FoV, min_parallax, max_parallax, ... 28 | min_depth, max_depth, false); 29 | 30 | Egt = [[0 -tgt(3) tgt(2)]; [tgt(3) 0 -tgt(1)]; [-tgt(2) tgt(1) 0]]*Rgt; 31 | Egt = Egt/Egt(3,3); 32 | 33 | 34 | %% Check original fcns: 35 | 36 | % 37 | tic 38 | [E_mani, is_opt_mani, optval_mani, dualval, dual_gap] = estimateCertifiedEssentialMatrix(P1, P2, W); 39 | toc 40 | 41 | 42 | tic 43 | [E_mani, is_opt_mani, optval_mani, dualval, dual_gap] = estimateCertNaiveManifold(P1, P2, W); 44 | toc; 45 | 46 | 47 | 48 | % Test the MEX function 49 | % enum class INPUT_PARAMS : int 50 | %{ 51 | % obsi = 0, 52 | % obsip, 53 | % weights, 54 | % init_method, 55 | % precon, 56 | % use_mult_certifiers, 57 | % verbose 58 | %}; 59 | % 60 | %enum class OUTPUT_PARAMS : int 61 | %{ 62 | % R_est = 0, 63 | % t_est, 64 | % time_total, 65 | % time_iterative, 66 | % is_opt 67 | %}; 68 | 69 | init_method = 0; % 8-pt 70 | precon=2; % dominant eigenvalues 71 | use_mult_certifiers = 1; % check all the relaxations (\neq 6) 72 | verbose=true; 73 | 74 | [R, t, time_iterative, time_total, is_opt] = EssentialMatrixMex(P1, P2, W', ... 75 | init_method, precon, use_mult_certifiers, verbose); 76 | % check error 77 | dist_R(Rgt, R) 78 | 79 | 80 | 81 | % Test the MEX wrapper function 82 | [struct_output_wrap] = EssentialMatrixEstimate(P1, P2, W', ... 83 | 'init_method', init_method, 'precon', precon, 'use_mult_certifiers', use_mult_certifiers, 'verbose', verbose); 84 | dist_R(Rgt, struct_output_wrap.R) 85 | -------------------------------------------------------------------------------- /matlab/EssentialMatrixMex.cc: -------------------------------------------------------------------------------- 1 | /* Author: 2 | Mercedes Garcia Salguero 3 | 4 | Code inspired by TEASER++ (see reference) 5 | 6 | DISCL.: 7 | THIS FILE IS NOT GNC 8 | 9 | */ 10 | 11 | 12 | 13 | #include 14 | 15 | 16 | #include 17 | #include "mex.h" 18 | #include 19 | 20 | #include "EssentialMatrixMexUtils.h" 21 | 22 | #include "Essential.h" 23 | // #include "EssentialTypes.h" 24 | // #include "EssentialUtils.h" 25 | 26 | 27 | enum class INPUT_PARAMS : int 28 | { 29 | obsi = 0, 30 | obsip, 31 | weights, 32 | init_method, 33 | precon, 34 | use_mult_certifiers, 35 | verbose 36 | }; 37 | 38 | enum class OUTPUT_PARAMS : int 39 | { 40 | R_est = 0, 41 | t_est, 42 | time_total, 43 | time_iterative, 44 | is_opt 45 | }; 46 | 47 | 48 | 49 | typedef bool (*mexTypeCheckFunction)(const mxArray*); 50 | const std::map INPUT_PARAMS_MAP{ 51 | {INPUT_PARAMS::obsi, &is3NMatrix}, 52 | {INPUT_PARAMS::obsip, &is3NMatrix}, 53 | {INPUT_PARAMS::weights, &is1NMatrix}, 54 | {INPUT_PARAMS::init_method, &isRealDoubleScalar}, 55 | {INPUT_PARAMS::precon, &isRealDoubleScalar}, 56 | {INPUT_PARAMS::use_mult_certifiers, &isRealDoubleScalar}, 57 | {INPUT_PARAMS::verbose, &mxIsLogicalScalar}, 58 | }; 59 | 60 | 61 | 62 | const std::map OUTPUT_PARAMS_MAP{ 63 | {OUTPUT_PARAMS::R_est, &isRealDoubleMatrix<3, 3>}, 64 | {OUTPUT_PARAMS::t_est, &isRealDoubleMatrix<3, 1>}, 65 | {OUTPUT_PARAMS::time_total, &isRealDoubleScalar}, 66 | {OUTPUT_PARAMS::time_iterative, &isRealDoubleScalar}, 67 | {OUTPUT_PARAMS::is_opt, &isRealDoubleScalar}, 68 | }; 69 | 70 | 71 | 72 | 73 | void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) { 74 | 75 | // TODO 76 | // Check for proper number of arguments 77 | 78 | if (nrhs != INPUT_PARAMS_MAP.size()) { 79 | mexErrMsgIdAndTxt("EssentialSolve:nargin", "Wrong number of input arguments."); 80 | } 81 | if (nlhs != OUTPUT_PARAMS_MAP.size()) { 82 | mexErrMsgIdAndTxt("EssentialSolve:nargin", "Wrong number of output arguments."); 83 | } 84 | 85 | 86 | 87 | 88 | // Check for proper input types 89 | for (const auto& pair : INPUT_PARAMS_MAP) { 90 | if (!pair.second(prhs[toUType(pair.first)])) { 91 | std::stringstream error_msg; 92 | error_msg << "Argument " << toUType(pair.first) + 1 << " has the wrong type.\n"; 93 | mexErrMsgIdAndTxt("EssentialSolve:nargin", error_msg.str().c_str()); 94 | } 95 | } 96 | 97 | 98 | mexPrintf("Arguments type checks passed.\n"); 99 | mexEvalString("drawnow;"); 100 | 101 | // Prepare parameters 102 | // Prepare source and destination Eigen point matrices 103 | Eigen::Matrix obsi_eigen, obsip_eigen; 104 | Eigen::Matrix weights_eigen; 105 | // conversion 106 | mex3NMatrixToEigenMatrix(prhs[toUType(INPUT_PARAMS::obsi)], &obsi_eigen); 107 | mex3NMatrixToEigenMatrix(prhs[toUType(INPUT_PARAMS::obsip)], &obsip_eigen); 108 | mex1NMatrixToEigenMatrix(prhs[toUType(INPUT_PARAMS::weights)], &weights_eigen); 109 | 110 | // check dimension 111 | assert(obsi_eigen.size() != 0); 112 | assert(obsip_eigen.size() != 0); 113 | assert(obsip_eigen.size() == obsi_eigen.size()); 114 | assert(weights_eigen.size() != 0); 115 | 116 | // Other parameters 117 | // Note: init and precon have their own type 118 | auto init_method = static_cast(*mxGetPr(prhs[toUType(INPUT_PARAMS::init_method)])); 119 | auto precon = static_cast(*mxGetPr(prhs[toUType(INPUT_PARAMS::precon)])); 120 | auto use_mult_certifiers = static_cast(*mxGetPr(prhs[toUType(INPUT_PARAMS::use_mult_certifiers)])); 121 | auto verbose = static_cast(*mxGetPr(prhs[toUType(INPUT_PARAMS::verbose)])); 122 | 123 | Essential::EssentialEstimationOptions options = Essential::EssentialEstimationOptions(); 124 | // general verbose 125 | options.use_idx_relaxation = use_mult_certifiers; 126 | options.estimation_verbose = verbose; 127 | options.verbose = verbose; 128 | 129 | /* Initialization */ 130 | /* enum class InitialisationMethod { 131 | PTS8_INIT = 0, 132 | PTS7_INIT, 133 | PTS5_INIT, 134 | RANDOM_INIT, 135 | EYE_INIT, 136 | USER_SUPPLIED, 137 | }; 138 | */ 139 | options.chosen_initialisation = static_cast(init_method); 140 | 141 | /* Preconditioner */ 142 | /* 143 | enum class Preconditioner { 144 | None= 0, 145 | Max_eigenvalue, 146 | Dominant_eigenvalues, 147 | N, 148 | Any, 149 | }; 150 | */ 151 | options.use_preconditioning = static_cast(precon); 152 | 153 | 154 | /** SOLVER **/ 155 | // initialize estimator 156 | Essential::EssentialClass my_essential_estimation(obsi_eigen, obsip_eigen, weights_eigen, options); 157 | 158 | 159 | mexPrintf("Start essential matrix estimator.\n"); 160 | mexEvalString("drawnow;"); 161 | 162 | // Solve 163 | Essential::EssentialEstimationResult my_result = my_essential_estimation.getResults(); 164 | /* 165 | end of SOLVER 166 | */ 167 | 168 | // Populate output E matrix 169 | plhs[toUType(OUTPUT_PARAMS::R_est)] = mxCreateDoubleMatrix(3, 3, mxREAL); 170 | Eigen::Map R_map(mxGetPr(plhs[toUType(OUTPUT_PARAMS::R_est)]), 3, 3); 171 | R_map = my_result.R_opt; 172 | 173 | // Populate output T vector 174 | plhs[toUType(OUTPUT_PARAMS::t_est)] = mxCreateDoubleMatrix(3, 1, mxREAL); 175 | Eigen::Map> t_map(mxGetPr(plhs[toUType(OUTPUT_PARAMS::t_est)]), 3, 1); 176 | t_map = my_result.t_opt; 177 | 178 | 179 | // Populate outputs 180 | // iterative algorithm 181 | plhs[toUType(OUTPUT_PARAMS::time_iterative)] = mxCreateDoubleScalar(my_result.elapsed_iterative_time); 182 | plhs[toUType(OUTPUT_PARAMS::time_total)] = mxCreateDoubleScalar(my_result.elapsed_estimation_time); 183 | 184 | // is opt? 185 | double is_opt_double = -1; 186 | switch(my_result.certifier_status) 187 | { 188 | case (Essential::EssentialEstimationStatus::GLOBAL_OPT): 189 | { 190 | mexPrintf("Solution is optimal!.\n"); 191 | is_opt_double = 1; 192 | break; 193 | } 194 | case (Essential::EssentialEstimationStatus::NO_CERTIFIED): 195 | { 196 | mexPrintf("We could not certify the solution.\n"); 197 | is_opt_double = 0; 198 | break; 199 | } 200 | default: 201 | { 202 | mexPrintf("Max. nr iterations in solver. Try increasing the parameter.\n"); 203 | break; 204 | } 205 | } 206 | 207 | plhs[toUType(OUTPUT_PARAMS::is_opt)] = mxCreateDoubleScalar(is_opt_double); 208 | } 209 | -------------------------------------------------------------------------------- /matlab/EssentialMatrixMexUtils.h: -------------------------------------------------------------------------------- 1 | /* File copied from TEASER++ 2 | See original paper 3 | */ 4 | 5 | #pragma once 6 | 7 | #include 8 | 9 | #include "mex.h" 10 | 11 | #include 12 | 13 | 14 | 15 | 16 | // Credit to Effective Modern C++ Item 10 17 | template constexpr typename std::underlying_type::type toUType(E e) noexcept { 18 | return static_cast::type>(e); 19 | }; 20 | 21 | /** 22 | * Templated function to check if input is a R-by-C MATLAB matrix 23 | * @tparam R rows 24 | * @tparam C columns 25 | * @param pa 26 | * @return 27 | */ 28 | template bool isRealDoubleMatrix(const mxArray* pa) { 29 | bool isDoubleMatrix = mxIsDouble(pa) && (!mxIsComplex(pa)) && (!mxIsScalar(pa)); 30 | bool correctDimensions = (mxGetM(pa) == R) && (mxGetN(pa) == C); 31 | return isDoubleMatrix && correctDimensions; 32 | } 33 | 34 | 35 | /** 36 | * Return true if input is a 1-by-N MATLAB matrix 37 | * @param pa 38 | * @return 39 | */ 40 | bool is1NMatrix(const mxArray* pa) { 41 | size_t rows = 1; 42 | bool isDoubleMatrix = mxIsDouble(pa) && (!mxIsComplex(pa)) && (!mxIsScalar(pa)); 43 | bool correctDimensions = mxGetM(pa) == rows; 44 | return isDoubleMatrix && correctDimensions; 45 | } 46 | 47 | 48 | /** 49 | * Return true if input is a 3-by-N MATLAB matrix 50 | * @param pa 51 | * @return 52 | */ 53 | bool is3NMatrix(const mxArray* pa) { 54 | size_t rows = 3; 55 | bool isDoubleMatrix = mxIsDouble(pa) && (!mxIsComplex(pa)) && (!mxIsScalar(pa)); 56 | bool correctDimensions = mxGetM(pa) == rows; 57 | return isDoubleMatrix && correctDimensions; 58 | } 59 | 60 | /** 61 | * Return true if input is a real double scalar 62 | * @param pa 63 | * @return 64 | */ 65 | bool isRealDoubleScalar(const mxArray* pa) { 66 | return mxIsDouble(pa) && mxIsScalar(pa) && (!mxIsComplex(pa)); 67 | } 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | /** 76 | * Convert a 3-by-N mxArray to Eigen 3-by-N matrix 77 | * @param pa 78 | */ 79 | void mex3NMatrixToEigenMatrix(const mxArray* pa, 80 | Eigen::Matrix* eigen_matrix) { 81 | int rows = mxGetM(pa); 82 | int cols = mxGetN(pa); 83 | mexPrintf("row: %d cols: %d \n", rows, cols); 84 | if (rows != 3) 85 | return; 86 | eigen_matrix->resize(rows, cols); 87 | 88 | double* in_matrix; 89 | in_matrix = mxGetPr(pa); 90 | 91 | *eigen_matrix = Eigen::Map>(in_matrix, rows, cols); 92 | } 93 | 94 | 95 | /** 96 | * Convert a 1-by-N mxArray to Eigen 1-by-N matrix 97 | * @param pa 98 | */ 99 | void mex1NMatrixToEigenMatrix(const mxArray* pa, 100 | Eigen::Matrix* eigen_matrix) { 101 | int rows = mxGetM(pa); 102 | int cols = mxGetN(pa); 103 | mexPrintf("row: %d cols: %d \n", rows, cols); 104 | if (rows != 1) 105 | return; 106 | eigen_matrix->resize(rows, cols); 107 | 108 | double* in_matrix; 109 | in_matrix = mxGetPr(pa); 110 | 111 | *eigen_matrix = Eigen::Map>(in_matrix, rows, cols); 112 | } 113 | -------------------------------------------------------------------------------- /matlab/GNCEssentialMatrixEstimate.m: -------------------------------------------------------------------------------- 1 | function [struct_output] = GNCEssentialMatrixEstimate(obsi, obsip, weights, varargin) 2 | %% Add info here 3 | 4 | 5 | assert(size(obsi, 1) == 3, 'obsi must be a 3-by-N matrix.') 6 | assert(size(obsip, 1) == 3, 'obsip must be a 3-by-N matrix.') 7 | assert(size(weights, 1) == 1, 'weights must be a 1-by-N matrix.') 8 | 9 | % enum class INPUT_PARAMS : int 10 | % { 11 | % obsi = 0, 12 | % obsip, 13 | % weights, 14 | % max_res_tol_sq, 15 | % gnc_factor, 16 | % gnc_max_inner_iterations, 17 | % gnc_max_outer_iterations, 18 | % gnc_cost_threshold, 19 | % gnc_inliers_threshold, 20 | % gnc_min_nr_points, 21 | % init_method, 22 | % gnc_robust, 23 | % precon, 24 | % use_mult_certifiers, 25 | % verbose 26 | %}; 27 | 28 | 29 | params = inputParser; 30 | params.CaseSensitive = false; 31 | 32 | addParameter(params, 'max_res_tol_sq', 0.01^2, ... 33 | @(x) isnumeric(x) && isscalar(x) && x>=0); 34 | 35 | addParameter(params, 'gnc_factor', 1.10, ... 36 | @(x) isnumeric(x) && isscalar(x) && x>1); 37 | 38 | addParameter(params, 'gnc_max_inner_iterations', 2, ... 39 | @(x) isnumeric(x) && isscalar(x) && x>=1); 40 | 41 | addParameter(params, 'gnc_max_outer_iterations', 500, ... 42 | @(x) isnumeric(x) && isscalar(x) && x>=1); 43 | 44 | addParameter(params, 'gnc_cost_threshold', 0.0000001, ... 45 | @(x) isnumeric(x) && isscalar(x) && x>=0); 46 | 47 | addParameter(params, 'gnc_inliers_threshold', 0.9, ... 48 | @(x) isnumeric(x) && isscalar(x) && x>0 && x<1); 49 | 50 | addParameter(params, 'gnc_min_nr_points', 12, ... 51 | @(x) isnumeric(x) && isscalar(x) && x>0 ); 52 | 53 | addParameter(params, 'init_method', 0, ... 54 | @(x) isnumeric(x) && isscalar(x) && x>=0 && x<=6); 55 | 56 | addParameter(params, 'gnc_robust', 2, ... 57 | @(x) isnumeric(x) && isscalar(x) && x>=0 && x<=6); 58 | 59 | addParameter(params, 'precon', 1, ... 60 | @(x) isnumeric(x) && isscalar(x) && x>=0 && x<=4); 61 | 62 | addParameter(params, 'use_mult_certifiers', 1, ... 63 | @(x) isnumeric(x) && isscalar(x) && x>=0 && x<=6); 64 | 65 | addParameter(params,'verbose', false, ... 66 | @(x) islogical(x)); 67 | 68 | parse(params, varargin{:}); 69 | 70 | 71 | [R, t, time_iterative, time_total, set_inliers, is_opt] = GNCEssentialMatrixMex(obsi, obsip, weights, ... 72 | params.Results.max_res_tol_sq, params.Results.gnc_factor, ... 73 | params.Results.gnc_max_inner_iterations, ... 74 | params.Results.gnc_max_outer_iterations, params.Results.gnc_cost_threshold, ... 75 | params.Results.gnc_inliers_threshold, params.Results.gnc_min_nr_points, ... 76 | params.Results.init_method, params.Results.gnc_robust, ... 77 | params.Results.precon, ... 78 | params.Results.use_mult_certifiers, params.Results.verbose); 79 | 80 | struct_output = struct(); 81 | struct_output.R = R; 82 | struct_output.t = t; 83 | struct_output.time_iterative = time_iterative; 84 | struct_output.time_total = time_total; 85 | struct_output.set_inliers = set_inliers; 86 | struct_output.is_opt = is_opt; 87 | 88 | end 89 | -------------------------------------------------------------------------------- /matlab/GNCEssentialMatrixMex.cc: -------------------------------------------------------------------------------- 1 | /* Author: 2 | Mercedes Garcia Salguero 3 | 4 | Code inspired by TEASER++ (see reference) 5 | 6 | 7 | */ 8 | 9 | #include 10 | 11 | 12 | #include 13 | #include "mex.h" 14 | #include 15 | 16 | #include "EssentialMatrixMexUtils.h" 17 | 18 | 19 | // #include "EssentialTypes.h" 20 | // #include "EssentialUtils.h" 21 | 22 | #include "GNCEssential.h" 23 | 24 | 25 | enum class INPUT_PARAMS : int 26 | { 27 | obsi = 0, 28 | obsip, 29 | weights, 30 | max_res_tol_sq, 31 | gnc_factor, 32 | gnc_max_inner_iterations, 33 | gnc_max_outer_iterations, 34 | gnc_cost_threshold, 35 | gnc_inliers_threshold, 36 | gnc_min_nr_points, 37 | init_method, 38 | gnc_robust, 39 | precon, 40 | use_mult_certifiers, 41 | verbose 42 | }; 43 | 44 | enum class OUTPUT_PARAMS : int 45 | { 46 | R_est = 0, 47 | t_est, 48 | time_total, 49 | time_iterative, 50 | set_inliers, 51 | is_opt 52 | }; 53 | 54 | typedef bool (*mexTypeCheckFunction)(const mxArray*); 55 | const std::map INPUT_PARAMS_MAP{ 56 | {INPUT_PARAMS::obsi, &is3NMatrix}, 57 | {INPUT_PARAMS::obsip, &is3NMatrix}, 58 | {INPUT_PARAMS::weights, &is1NMatrix}, 59 | {INPUT_PARAMS::max_res_tol_sq, &isRealDoubleScalar}, 60 | {INPUT_PARAMS::gnc_factor, &isRealDoubleScalar}, 61 | {INPUT_PARAMS::gnc_max_inner_iterations, &isRealDoubleScalar}, 62 | {INPUT_PARAMS::gnc_max_outer_iterations, &isRealDoubleScalar}, 63 | {INPUT_PARAMS::gnc_cost_threshold, &isRealDoubleScalar}, 64 | {INPUT_PARAMS::gnc_inliers_threshold, &isRealDoubleScalar}, 65 | {INPUT_PARAMS::gnc_min_nr_points, &isRealDoubleScalar}, 66 | {INPUT_PARAMS::init_method, &isRealDoubleScalar}, 67 | {INPUT_PARAMS::gnc_robust, &isRealDoubleScalar}, 68 | {INPUT_PARAMS::precon, &isRealDoubleScalar}, 69 | {INPUT_PARAMS::use_mult_certifiers, &isRealDoubleScalar}, 70 | {INPUT_PARAMS::verbose, &mxIsLogicalScalar}, 71 | }; 72 | 73 | 74 | 75 | const std::map OUTPUT_PARAMS_MAP{ 76 | {OUTPUT_PARAMS::R_est, &isRealDoubleMatrix<3, 3>}, 77 | {OUTPUT_PARAMS::t_est, &isRealDoubleMatrix<3, 1>}, 78 | {OUTPUT_PARAMS::time_total, &isRealDoubleScalar}, 79 | {OUTPUT_PARAMS::time_iterative, &isRealDoubleScalar}, 80 | {OUTPUT_PARAMS::set_inliers, &is1NMatrix}, 81 | {OUTPUT_PARAMS::is_opt, &isRealDoubleScalar}, 82 | }; 83 | 84 | 85 | 86 | 87 | void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) { 88 | 89 | // TODO 90 | // Check for proper number of arguments 91 | 92 | 93 | if (nrhs != INPUT_PARAMS_MAP.size()) { 94 | mexErrMsgIdAndTxt("EssentialSolve:nargin", "Wrong number of input arguments."); 95 | } 96 | if (nlhs != OUTPUT_PARAMS_MAP.size()) { 97 | mexErrMsgIdAndTxt("EssentialSolve:nargin", "Wrong number of output arguments."); 98 | } 99 | 100 | 101 | 102 | 103 | // Check for proper input types 104 | for (const auto& pair : INPUT_PARAMS_MAP) { 105 | if (!pair.second(prhs[toUType(pair.first)])) { 106 | std::stringstream error_msg; 107 | error_msg << "Argument " << toUType(pair.first) + 1 << " has the wrong type.\n"; 108 | mexErrMsgIdAndTxt("EssentialSolve:nargin", error_msg.str().c_str()); 109 | } 110 | } 111 | 112 | 113 | mexPrintf("Arguments type checks passed.\n"); 114 | mexEvalString("drawnow;"); 115 | 116 | // Prepare parameters 117 | // Prepare source and destination Eigen point matrices 118 | Eigen::Matrix obsi_eigen, obsip_eigen; 119 | Eigen::Matrix weights_eigen; 120 | // conversion 121 | mex3NMatrixToEigenMatrix(prhs[toUType(INPUT_PARAMS::obsi)], &obsi_eigen); 122 | mex3NMatrixToEigenMatrix(prhs[toUType(INPUT_PARAMS::obsip)], &obsip_eigen); 123 | mex1NMatrixToEigenMatrix(prhs[toUType(INPUT_PARAMS::weights)], &weights_eigen); 124 | 125 | // check dimension 126 | assert(obsi_eigen.size() != 0); 127 | assert(obsip_eigen.size() != 0); 128 | assert(obsip_eigen.size() == obsi_eigen.size()); 129 | assert(weights_eigen.size() != 0); 130 | 131 | // Other parameters 132 | auto max_res_tol_sq = static_cast(*mxGetPr(prhs[toUType(INPUT_PARAMS::max_res_tol_sq)])); 133 | auto gnc_factor = static_cast(*mxGetPr(prhs[toUType(INPUT_PARAMS::gnc_factor)])); 134 | auto gnc_max_inner_iterations = static_cast(*mxGetPr(prhs[toUType(INPUT_PARAMS::gnc_max_inner_iterations)])); 135 | auto gnc_max_outer_iterations = static_cast(*mxGetPr(prhs[toUType(INPUT_PARAMS::gnc_max_outer_iterations)])); 136 | auto gnc_cost_threshold = static_cast(*mxGetPr(prhs[toUType(INPUT_PARAMS::gnc_cost_threshold)])); 137 | auto gnc_inliers_threshold = static_cast(*mxGetPr(prhs[toUType(INPUT_PARAMS::gnc_inliers_threshold)])); 138 | auto gnc_min_nr_points = static_cast(*mxGetPr(prhs[toUType(INPUT_PARAMS::gnc_min_nr_points)])); 139 | // Note: init, gnc_robust and precon have their own type 140 | auto init_method = static_cast(*mxGetPr(prhs[toUType(INPUT_PARAMS::init_method)])); 141 | auto gnc_robust = static_cast(*mxGetPr(prhs[toUType(INPUT_PARAMS::gnc_robust)])); 142 | auto precon = static_cast(*mxGetPr(prhs[toUType(INPUT_PARAMS::precon)])); 143 | auto use_mult_certifiers = static_cast(*mxGetPr(prhs[toUType(INPUT_PARAMS::use_mult_certifiers)])); 144 | auto verbose = static_cast(*mxGetPr(prhs[toUType(INPUT_PARAMS::verbose)])); 145 | 146 | 147 | // TODO 148 | // Define params for the solver 149 | Essential::GNCEssentialEstimationOptions options = Essential::GNCEssentialEstimationOptions(); 150 | options.max_res_tol_sq = max_res_tol_sq; 151 | options.gnc_factor = gnc_factor; 152 | options.max_inner_iterations = gnc_max_inner_iterations; 153 | options.max_outer_iterations = gnc_max_outer_iterations; 154 | options.cost_diff_threshold = gnc_cost_threshold; 155 | options.inliers_threshold = gnc_inliers_threshold; 156 | options.nr_min_points = gnc_min_nr_points; 157 | // general verbose 158 | options.estimation_verbose = verbose; 159 | options.GNC_verbose = verbose; 160 | options.use_idx_relaxation = use_mult_certifiers; 161 | 162 | /* Initialization */ 163 | /* enum class InitialisationMethod { 164 | PTS8_INIT = 0, 165 | PTS7_INIT, 166 | PTS5_INIT, 167 | RANDOM_INIT, 168 | EYE_INIT, 169 | USER_SUPPLIED, 170 | }; 171 | */ 172 | options.chosen_initialisation = static_cast(init_method); 173 | 174 | /* GNC robust */ 175 | /* 176 | enum class GNCRobustFunction { 177 | None = 0, 178 | TLS, 179 | GM, 180 | TEMP, 181 | WELSCH, 182 | TUKEY, 183 | Any 184 | }; 185 | */ 186 | options.gnc_robust = static_cast(gnc_robust); 187 | 188 | 189 | /* Preconditioner */ 190 | /* 191 | enum class Preconditioner { 192 | None= 0, 193 | Rotation, 194 | Any, 195 | }; 196 | */ 197 | options.use_preconditioning = static_cast(precon); 198 | 199 | 200 | /** SOLVER **/ 201 | // initialize estimator 202 | Essential::GNCEssentialClass my_essential_estimation(obsi_eigen, obsip_eigen, weights_eigen, options); 203 | 204 | 205 | mexPrintf("Start robust essential matrix estimator.\n"); 206 | mexEvalString("drawnow;"); 207 | 208 | // Solve 209 | Essential::GNCEssentialEstimationResult my_result = my_essential_estimation.getResultGNC(); 210 | /* 211 | end of SOLVER 212 | */ 213 | 214 | // Populate output E matrix 215 | plhs[toUType(OUTPUT_PARAMS::R_est)] = mxCreateDoubleMatrix(3, 3, mxREAL); 216 | Eigen::Map R_map(mxGetPr(plhs[toUType(OUTPUT_PARAMS::R_est)]), 3, 3); 217 | R_map = my_result.R_opt; 218 | 219 | // Populate output T vector 220 | plhs[toUType(OUTPUT_PARAMS::t_est)] = mxCreateDoubleMatrix(3, 1, mxREAL); 221 | Eigen::Map> t_map(mxGetPr(plhs[toUType(OUTPUT_PARAMS::t_est)]), 3, 1); 222 | t_map = my_result.t_opt; 223 | 224 | 225 | // Populate outputs 226 | // iterative algorithm 227 | plhs[toUType(OUTPUT_PARAMS::time_iterative)] = mxCreateDoubleScalar(my_result.elapsed_iterative_time); 228 | plhs[toUType(OUTPUT_PARAMS::time_total)] = mxCreateDoubleScalar(my_result.elapsed_estimation_time); 229 | 230 | // set inliers 231 | size_t N = weights_eigen.size(); 232 | plhs[toUType(OUTPUT_PARAMS::set_inliers)] = mxCreateDoubleMatrix(N, 1, mxREAL); 233 | Eigen::Map> inliers_map(mxGetPr(plhs[toUType(OUTPUT_PARAMS::set_inliers)]), N, 1); 234 | inliers_map = my_result.set_inliers; 235 | 236 | // is opt? 237 | double is_opt_double = -1; 238 | switch(my_result.certifier_status) 239 | { 240 | case (Essential::EssentialEstimationStatus::GLOBAL_OPT): 241 | { 242 | mexPrintf("Solution is optimal!.\n"); 243 | is_opt_double = 1; 244 | break; 245 | } 246 | case (Essential::EssentialEstimationStatus::NO_CERTIFIED): 247 | { 248 | mexPrintf("We could not certify the solution.\n"); 249 | is_opt_double = 0; 250 | break; 251 | } 252 | default: 253 | { 254 | mexPrintf("Max. nr iterations in solver. Try increasing the parameter.\n"); 255 | break; 256 | } 257 | } 258 | 259 | plhs[toUType(OUTPUT_PARAMS::is_opt)] = mxCreateDoubleScalar(is_opt_double); 260 | 261 | 262 | } 263 | -------------------------------------------------------------------------------- /matlab/RobustEssentialMatrixEstimationTest.m: -------------------------------------------------------------------------------- 1 | close all; clear all; clc; 2 | 3 | 4 | rng('shuffle'); 5 | % Test parameters: 6 | % Default params. 7 | noise=0.1; 8 | FoV=150; 9 | N = 200; 10 | % for t 11 | min_parallax=0.5; 12 | max_parallax=2.0; 13 | % for the points 14 | min_depth=1.0; 15 | max_depth=8.0; 16 | 17 | f = 800; % focal length 18 | % visualization 19 | 20 | 21 | % weights vector 22 | W = ones(N, 1); 23 | 24 | % Params 25 | % obsi = 0, 26 | % obsip, 27 | % weights, 28 | % max_res_tol_sq, 29 | % gnc_factor, 30 | % gnc_max_inner_iterations, 31 | % gnc_max_outer_iterations, 32 | % gnc_cost_threshold, 33 | % gnc_inliers_threshold, 34 | % gnc_min_nr_points, 35 | % init_method, 36 | % gnc_robust, 37 | % precon, 38 | % use_mult_certifiers, 39 | % verbose 40 | 41 | max_res_tol_sq = 0.00001; 42 | gnc_factor = 1.10; 43 | gnc_max_inner_iterations = 2; 44 | gnc_max_outer_iterations = 500; 45 | gnc_cost_threshold = 0.000001; 46 | gnc_inliers_threshold = 0.9; 47 | gnc_min_nr_points = 12; 48 | gnc_robust = 5; 49 | 50 | init_method = 0; % 8-pt 51 | precon=2; % dominant eigenvalues 52 | use_mult_certifiers = 1; % check all the relaxations (\neq 6) 53 | verbose=true; 54 | 55 | % 1. Generate data 56 | [P1, P2, tgt, Rgt, indices_outliers] = create2D2DCorrespondencesNOutliers(N, noise, 80, FoV, min_parallax, max_parallax, ... 57 | min_depth, max_depth, false); 58 | 59 | Egt = [[0 -tgt(3) tgt(2)]; [tgt(3) 0 -tgt(1)]; [-tgt(2) tgt(1) 0]]*Rgt; 60 | Egt = Egt/Egt(3,3); 61 | 62 | 63 | 64 | 65 | % Test the MEX function 66 | [R, t, time_iterative, time_total, set_inliers, is_opt] = GNCEssentialMatrixMex(P1, P2, W', ... 67 | max_res_tol_sq, gnc_factor, ... 68 | gnc_max_inner_iterations, gnc_max_outer_iterations, ... 69 | gnc_cost_threshold, gnc_inliers_threshold, ... 70 | gnc_min_nr_points, init_method, gnc_robust, ... 71 | precon, use_mult_certifiers, verbose); 72 | % check error 73 | dist_R(Rgt, R) 74 | 75 | 76 | 77 | % Test the MEX wrapper function 78 | [struct_output_wrap] = GNCEssentialMatrixEstimate(P1, P2, W', ... 79 | 'max_res_tol_sq', max_res_tol_sq, 'gnc_factor', gnc_factor, ... 80 | 'gnc_max_inner_iterations', gnc_max_inner_iterations, ... 81 | 'gnc_max_outer_iterations', gnc_max_outer_iterations, ... 82 | 'gnc_cost_threshold', gnc_cost_threshold, ... 83 | 'gnc_inliers_threshold', gnc_inliers_threshold, ... 84 | 'gnc_min_nr_points', gnc_min_nr_points, ... 85 | 'init_method', init_method, 'gnc_robust', gnc_robust, ... 86 | 'precon', precon, 'use_mult_certifiers', use_mult_certifiers, ... 87 | 'verbose', verbose); 88 | dist_R(Rgt, struct_output_wrap.R) 89 | -------------------------------------------------------------------------------- /matlab/setup.m: -------------------------------------------------------------------------------- 1 | path_common = genpath(pwd); 2 | rmpath(path_common); 3 | addpath(path_common); 4 | 5 | % TODO 6 | addpath(genpath('common_relative_pose')); 7 | 8 | 9 | 10 | addpath(genpath('../build')); 11 | -------------------------------------------------------------------------------- /python/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | 3 | 4 | set(PYBIND11_PYTHON_VERSION ${RELATIVEPOSEPYTHON_VERSION}) 5 | 6 | set(ROBUST_ESTIMATION ON) 7 | 8 | project(relative_pose_python_bindings) 9 | 10 | pybind11_add_module(relativeposepython relativeposepython/relativeposepython.cc) 11 | 12 | if (ROBUST_ESTIMATION) 13 | pybind11_add_module(robustrelativeposepython robustrelativeposepython/robustrelativeposepython.cc) 14 | endif() 15 | 16 | message(STATUS "Python Interpreter Version: ${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR}") 17 | 18 | 19 | if (0) 20 | if (NOT (PYTHON_VERSION_MAJOR EQUAL 2 AND PYTHON_VERSION_MINOR EQUAL 7)) 21 | # Hack: VTK used in PCL might add /usr/include/python2.7 to all targets' 22 | # INCLUDE_DIRECTORIES properties. We need to remove it. 23 | get_target_property(RELATIVEPOSEPY_NEW_INCLUDE_DIRS relativeposepython INCLUDE_DIRECTORIES) 24 | list(FILTER RELATIVEPOSEPY_NEW_INCLUDE_DIRS EXCLUDE REGEX ".*python2.7$") 25 | set_target_properties(relativeposepython 26 | PROPERTIES INCLUDE_DIRECTORIES "${RELATIVEPOSEPY_NEW_INCLUDE_DIRS}") 27 | endif () 28 | endif() 29 | 30 | #set_target_properties(relativeposepython 31 | # PROPERTIES INCLUDE_DIRECTORIES "${RELATIVEPOSEPY_NEW_INCLUDE_DIRS}") 32 | target_link_libraries(relativeposepython PUBLIC Essential) 33 | 34 | if (ROBUST_ESTIMATION) 35 | target_link_libraries(robustrelativeposepython PUBLIC Essential) 36 | endif() 37 | 38 | # fix for clang 39 | # see: https://github.com/pybind/pybind11/issues/1818 40 | # if (CMAKE_CXX_COMPILER_ID STREQUAL "Clang") 41 | # target_compile_options(relativeposepython PUBLIC -fsized-deallocation) 42 | # endif () 43 | add_compile_options(-I/usr/include/python2.7 -lpython2.7 ) 44 | 45 | 46 | # make sure to output the build file to relativeposepython folder 47 | SET_TARGET_PROPERTIES(relativeposepython 48 | PROPERTIES 49 | PREFIX "" 50 | LIBRARY_OUTPUT_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/relativeposepython" 51 | ) 52 | 53 | if (ROBUST_ESTIMATION) 54 | SET_TARGET_PROPERTIES(robustrelativeposepython 55 | PROPERTIES 56 | PREFIX "" 57 | LIBRARY_OUTPUT_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/robustrelativeposepython" 58 | ) 59 | endif() 60 | 61 | # copy package __init__.py file 62 | configure_file(relativeposepython/__init__.py 63 | ${CMAKE_CURRENT_BINARY_DIR}/relativeposepython/__init__.py 64 | ) 65 | 66 | if (ROBUST_ESTIMATION) 67 | configure_file(robustrelativeposepython/__init__.py 68 | ${CMAKE_CURRENT_BINARY_DIR}/robustrelativeposepython/__init__.py 69 | ) 70 | endif() 71 | 72 | 73 | # copy setup.py file 74 | configure_file(relativeposepython/setup.py.in 75 | ${CMAKE_CURRENT_BINARY_DIR}/relativeposepython/setup.py) 76 | 77 | if (ROBUST_ESTIMATION) 78 | configure_file(robustrelativeposepython/setup.py.in 79 | ${CMAKE_CURRENT_BINARY_DIR}/robustrelativeposepython/setup.py) 80 | endif() 81 | 82 | 83 | file(COPY . 84 | DESTINATION . 85 | FILES_MATCHING 86 | PATTERN *.py) 87 | -------------------------------------------------------------------------------- /python/relativeposepython/__init__.py: -------------------------------------------------------------------------------- 1 | from relativeposepython import * 2 | -------------------------------------------------------------------------------- /python/relativeposepython/central_relative_pose_example.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3.7 2 | 3 | import numpy as np 4 | import relativeposepython 5 | from numpy import linalg as LA 6 | 7 | 8 | 9 | if __name__ == "__main__": 10 | 11 | 12 | # Generate random data points 13 | N = 20 14 | points3D = np.random.rand(3, N) 15 | 16 | # Apply arbitrary scale, translation and rotation 17 | translation = np.array([[1], [0.8], [-1]]) 18 | rotation = np.array([[0.98370992, 0.17903344, -0.01618098], 19 | [-0.04165862, 0.13947877, -0.98934839], 20 | [-0.17486954, 0.9739059, 0.14466493]]) 21 | 22 | dst = np.matmul(rotation.transpose(), points3D - translation) 23 | 24 | # observations 25 | obs1 = points3D / LA.norm(points3D, axis=0) 26 | obs2 = dst / LA.norm(dst, axis=0) 27 | 28 | weights = np.ones(N) 29 | 30 | 31 | # create instance 32 | essential = relativeposepython.EssentialClass(obs1, obs2, weights, relativeposepython.EssentialClass.Options(), np.ones([3,3])) 33 | # solve 34 | results = essential.getResults() 35 | # retrieve data 36 | 37 | # essential matrix 38 | print ("Essential matrix: ", results.E_opt) 39 | # rotation matrix 40 | print ("Rotation matrix: ", results.R_opt) 41 | print ("Gt rotation : ", rotation) 42 | # translation vector 43 | print ("Translation vector: ", results.t_opt) 44 | print ("Gt Translation : ", translation / LA.norm(translation)) 45 | # certifier status 46 | status = results.getStatus() 47 | print ("Status Estimation (0: optimal): ", status) 48 | # CPU time (total) 49 | print ("Elapsed time: ", results.elapsed_estimation_time) 50 | 51 | -------------------------------------------------------------------------------- /python/relativeposepython/relativeposepython.cc: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | #include 5 | 6 | #include "Essential.h" 7 | 8 | namespace py = pybind11; 9 | 10 | /** 11 | * Python interface with pybind11 12 | */ 13 | PYBIND11_MODULE(relativeposepython, m) { 14 | m.doc() = "Python binding for (Central) Relative Pose"; 15 | 16 | 17 | 18 | // Python bound for Essential::EssentialClass 19 | py::class_ essential(m, "EssentialClass"); 20 | 21 | 22 | // Python bound for Essential::EssentialClass functions 23 | essential.def(py::init<>()) 24 | .def(py::init< Eigen::Matrix&, 25 | Eigen::Matrix&, 26 | Eigen::Matrix&, 27 | const Essential::EssentialEstimationOptions&, 28 | const Eigen::Matrix&> () ) 29 | .def("getResults", &Essential::EssentialClass::getResults); 30 | 31 | 32 | 33 | 34 | // Python bound for Essential::InitialisationMethod 35 | /* Initialization */ 36 | /* enum class InitialisationMethod { 37 | PTS8_INIT = 0, 38 | PTS7_INIT, 39 | RANDOM_INIT, 40 | EYE_INIT, 41 | PTS5_INIT, 42 | USER_SUPPLIED, 43 | }; 44 | */ 45 | py::enum_( 46 | essential, "InitialisationMethod") 47 | .value("PTS8_INIT", Essential::InitialisationMethod::PTS8_INIT) 48 | .value("PTS7_INIT", Essential::InitialisationMethod::PTS7_INIT) 49 | .value("PTS5_INIT", Essential::InitialisationMethod::PTS5_INIT) 50 | .value("RANDOM_INIT", Essential::InitialisationMethod::RANDOM_INIT) 51 | .value("EYE_INIT", Essential::InitialisationMethod::EYE_INIT) 52 | .value("USER_SUPPLIED", Essential::InitialisationMethod::USER_SUPPLIED); 53 | 54 | 55 | 56 | /* Preconditioner */ 57 | /* 58 | enum class Preconditioner { 59 | None= 0, 60 | Max_eigenvalue, 61 | Dominant_eigenvalues, 62 | N, 63 | Any, 64 | }; 65 | */ 66 | // Python bound for Essential::GNCRobustFunction 67 | py::enum_( 68 | essential, "Preconditioner") 69 | .value("None", Essential::Preconditioner::None) 70 | .value("Max_eigenvalue", Essential::Preconditioner::Max_eigenvalue) 71 | .value("Dominant_eigenvalues", Essential::Preconditioner::Dominant_eigenvalues) 72 | .value("N", Essential::Preconditioner::N) 73 | .value("Any", Essential::Preconditioner::Any); 74 | 75 | 76 | 77 | 78 | // Python bound for Essential::EssentialEstimationOptions 79 | py::class_(essential, "Options") 80 | .def(py::init<>()) 81 | .def_readwrite("estimation_verbose", &Essential::EssentialEstimationOptions::estimation_verbose) 82 | .def_readwrite("chosen_initialisation", &Essential::EssentialEstimationOptions::chosen_initialisation) 83 | .def_readwrite("use_preconditioning", &Essential::EssentialEstimationOptions::use_preconditioning) 84 | .def_readwrite("use_idx_relaxation", &Essential::EssentialEstimationOptions::use_idx_relaxation); 85 | 86 | 87 | // Python bound for Essential::EssentialEstimationResult 88 | py::class_(essential, "EssentialResult") 89 | .def(py::init<>()) 90 | .def_readwrite("E_opt", &Essential::EssentialEstimationResult::E_opt) 91 | .def_readwrite("R_opt", &Essential::EssentialEstimationResult::R_opt) 92 | .def_readwrite("t_opt", &Essential::EssentialEstimationResult::t_opt) 93 | //.def_readwrite("certifier_status", &Essential::EssentialEstimationResult::certifier_status) 94 | .def_readwrite("elapsed_estimation_time", &Essential::EssentialEstimationResult::elapsed_estimation_time) 95 | .def("getStatus", [](const Essential::EssentialEstimationResult& r) 96 | { 97 | int is_opt; 98 | 99 | 100 | switch(r.certifier_status) 101 | { 102 | case Essential::EssentialEstimationStatus::RS_ITER_LIMIT: 103 | is_opt = 2; 104 | break; 105 | case Essential::EssentialEstimationStatus::GLOBAL_OPT: 106 | is_opt = 0; 107 | break; 108 | case Essential::EssentialEstimationStatus::NO_CERTIFIED: 109 | is_opt = 1; 110 | break; 111 | default: 112 | is_opt = -1; 113 | } 114 | 115 | 116 | return is_opt; 117 | }); 118 | 119 | 120 | } 121 | -------------------------------------------------------------------------------- /python/relativeposepython/setup.py.in: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | 3 | setup( 4 | name='relativeposepython', 5 | version='1.0.0', 6 | author='Mercedes Garcia Salguero', 7 | author_email='mercedesgarsal@uma.es', 8 | description='Python binding for the Relative Pose Problem with central cameras', 9 | package_dir={'': '${CMAKE_CURRENT_BINARY_DIR}'}, 10 | packages=['relativeposepython'], 11 | package_data={'': ['*.so']} 12 | ) 13 | -------------------------------------------------------------------------------- /python/robustrelativeposepython/__init__.py: -------------------------------------------------------------------------------- 1 | from robustrelativeposepython import * 2 | -------------------------------------------------------------------------------- /python/robustrelativeposepython/robust_central_relative_pose_example.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3.7 2 | 3 | import numpy as np 4 | import robustrelativeposepython 5 | from numpy import linalg as LA 6 | 7 | 8 | 9 | if __name__ == "__main__": 10 | 11 | 12 | # Generate random data points 13 | N = 20 14 | points3D = np.random.rand(3, N) 15 | 16 | # Apply arbitrary scale, translation and rotation 17 | translation = np.array([[1], [0.8], [-1]]) 18 | rotation = np.array([[0.98370992, 0.17903344, -0.01618098], 19 | [-0.04165862, 0.13947877, -0.98934839], 20 | [-0.17486954, 0.9739059, 0.14466493]]) 21 | 22 | dst = np.matmul(rotation.transpose(), points3D - translation) 23 | 24 | # observations 25 | obs1 = points3D / LA.norm(points3D, axis=0) 26 | obs2 = dst / LA.norm(dst, axis=0) 27 | 28 | weights = np.ones(N) 29 | 30 | 31 | # set options 32 | estimation_params = robustrelativeposepython.GNCEssentialClass.Options() 33 | estimation_params.estimation_verbose=0; 34 | estimation_params.gnc_robust = robustrelativeposepython.GNCEssentialClass.GNCRobustFunction.TUKEY; 35 | estimation_params.use_idx_relaxation=0; 36 | # create instance 37 | essential = robustrelativeposepython.GNCEssentialClass(obs1, obs2, weights, 38 | estimation_params, np.ones([3,3]) ) 39 | # solve 40 | results = essential.getResultGNC() 41 | # retrieve data 42 | 43 | # essential matrix 44 | print ("Essential matrix: ", results.E_opt) 45 | # rotation matrix 46 | print ("Rotation matrix: ", results.R_opt) 47 | print ("Gt rotation : ", rotation) 48 | # translation vector 49 | print ("Translation vector: ", results.t_opt) 50 | print ("Gt Translation : ", translation / LA.norm(translation)) 51 | # certifier status 52 | status = results.getStatus() 53 | print ("Status Estimation (0: optimal): ", status) 54 | # CPU time (total) 55 | print ("Elapsed time: ", results.elapsed_estimation_time) 56 | 57 | -------------------------------------------------------------------------------- /python/robustrelativeposepython/robustrelativeposepython.cc: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | #include 5 | 6 | #include "GNCEssential.h" 7 | 8 | namespace py = pybind11; 9 | 10 | /** 11 | * Python interface with pybind11 12 | */ 13 | PYBIND11_MODULE(robustrelativeposepython, m) { 14 | m.doc() = "Python binding for Robust (Central) Relative Pose"; 15 | 16 | 17 | 18 | // Python bound for Essential::EssentialClass 19 | py::class_ essential(m, "GNCEssentialClass"); 20 | 21 | 22 | // Python bound for Essential::EssentialClass functions 23 | essential.def(py::init<>()) 24 | .def(py::init< Eigen::Matrix&, 25 | Eigen::Matrix&, 26 | Eigen::Matrix&, 27 | const Essential::GNCEssentialEstimationOptions&, 28 | const Eigen::Matrix&> () ) 29 | .def("getResultGNC", &Essential::GNCEssentialClass::getResultGNC); 30 | 31 | 32 | 33 | // Python bound for Essential::InitialisationMethod 34 | /* Initialization */ 35 | /* enum class InitialisationMethod { 36 | PTS8_INIT = 0, 37 | PTS7_INIT, 38 | RANDOM_INIT, 39 | EYE_INIT, 40 | PTS5_INIT, 41 | USER_SUPPLIED, 42 | }; 43 | */ 44 | py::enum_( 45 | essential, "InitialisationMethod") 46 | .value("PTS8_INIT", Essential::InitialisationMethod::PTS8_INIT) 47 | .value("PTS7_INIT", Essential::InitialisationMethod::PTS7_INIT) 48 | .value("PTS5_INIT", Essential::InitialisationMethod::PTS5_INIT) 49 | .value("RANDOM_INIT", Essential::InitialisationMethod::RANDOM_INIT) 50 | .value("EYE_INIT", Essential::InitialisationMethod::EYE_INIT) 51 | .value("USER_SUPPLIED", Essential::InitialisationMethod::USER_SUPPLIED); 52 | 53 | 54 | /* GNC robust */ 55 | /* 56 | enum class GNCRobustFunction { 57 | None = 0, 58 | TLS, 59 | GM, 60 | TEMP, 61 | WELSCH, 62 | TUKEY, 63 | Any 64 | }; 65 | */ 66 | // Python bound for Essential::GNCRobustFunction 67 | py::enum_( 68 | essential, "GNCRobustFunction") 69 | .value("None", Essential::GNCRobustFunction::None) 70 | .value("TLS", Essential::GNCRobustFunction::TLS) 71 | .value("GM", Essential::GNCRobustFunction::GM) 72 | .value("TEMP", Essential::GNCRobustFunction::TEMP) 73 | .value("WELSCH", Essential::GNCRobustFunction::WELSCH) 74 | .value("TUKEY", Essential::GNCRobustFunction::TUKEY) 75 | .value("Any", Essential::GNCRobustFunction::Any); 76 | 77 | 78 | /* Preconditioner */ 79 | /* 80 | enum class Preconditioner { 81 | None= 0, 82 | Max_eigenvalue, 83 | Dominant_eigenvalues, 84 | N, 85 | Any, 86 | }; 87 | */ 88 | // Python bound for Essential::GNCRobustFunction 89 | py::enum_( 90 | essential, "Preconditioner") 91 | .value("None", Essential::Preconditioner::None) 92 | .value("Max_eigenvalue", Essential::Preconditioner::Max_eigenvalue) 93 | .value("Dominant_eigenvalues", Essential::Preconditioner::Dominant_eigenvalues) 94 | .value("N", Essential::Preconditioner::N) 95 | .value("Any", Essential::Preconditioner::Any); 96 | 97 | 98 | 99 | // Python bound for Essential::EssentialEstimationOptions 100 | py::class_(essential, "Options") 101 | .def(py::init<>()) 102 | .def_readwrite("estimation_verbose", &Essential::GNCEssentialEstimationOptions::estimation_verbose) 103 | .def_readwrite("max_res_tol_sq", &Essential::GNCEssentialEstimationOptions::max_res_tol_sq) 104 | .def_readwrite("gnc_factor", &Essential::GNCEssentialEstimationOptions::gnc_factor) 105 | .def_readwrite("max_inner_iterations", &Essential::GNCEssentialEstimationOptions::max_inner_iterations) 106 | .def_readwrite("max_outer_iterations", &Essential::GNCEssentialEstimationOptions::max_outer_iterations) 107 | .def_readwrite("cost_diff_threshold", &Essential::GNCEssentialEstimationOptions::cost_diff_threshold) 108 | .def_readwrite("inliers_threshold", &Essential::GNCEssentialEstimationOptions::inliers_threshold) 109 | .def_readwrite("nr_min_points", &Essential::GNCEssentialEstimationOptions::nr_min_points) 110 | .def_readwrite("GNC_verbose", &Essential::GNCEssentialEstimationOptions::GNC_verbose) 111 | .def_readwrite("gnc_robust", &Essential::GNCEssentialEstimationOptions::gnc_robust) 112 | .def_readwrite("chosen_initialisation", &Essential::GNCEssentialEstimationOptions::chosen_initialisation) 113 | .def_readwrite("use_preconditioning", &Essential::GNCEssentialEstimationOptions::use_preconditioning) 114 | .def_readwrite("use_idx_relaxation", &Essential::EssentialEstimationOptions::use_idx_relaxation); 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | // Python bound for Essential::EssentialEstimationResult 124 | py::class_(essential, "EssentialResult") 125 | .def(py::init<>()) 126 | .def_readwrite("E_opt", &Essential::GNCEssentialEstimationResult::E_opt) 127 | .def_readwrite("R_opt", &Essential::GNCEssentialEstimationResult::R_opt) 128 | .def_readwrite("t_opt", &Essential::GNCEssentialEstimationResult::t_opt) 129 | .def_readwrite("set_inliers", &Essential::GNCEssentialEstimationResult::set_inliers) 130 | .def_readwrite("elapsed_estimation_time", &Essential::GNCEssentialEstimationResult::elapsed_estimation_time) 131 | .def("getStatus", [](const Essential::GNCEssentialEstimationResult& r) 132 | { 133 | int is_opt; 134 | 135 | 136 | switch(r.certifier_status) 137 | { 138 | case Essential::EssentialEstimationStatus::RS_ITER_LIMIT: 139 | is_opt = 2; 140 | break; 141 | case Essential::EssentialEstimationStatus::GLOBAL_OPT: 142 | is_opt = 1; 143 | break; 144 | case Essential::EssentialEstimationStatus::NO_CERTIFIED: 145 | is_opt = 0; 146 | break; 147 | default: 148 | is_opt = -1; 149 | } 150 | 151 | 152 | return is_opt; 153 | }); 154 | 155 | 156 | } 157 | -------------------------------------------------------------------------------- /python/robustrelativeposepython/setup.py.in: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | 3 | setup( 4 | name='robustrelativeposepython', 5 | version='1.0.0', 6 | author='Mercedes Garcia Salguero', 7 | author_email='mercedesgarsal@uma.es', 8 | description='Python binding for the Robust Relative Pose Problem with central cameras', 9 | package_dir={'': '${CMAKE_CURRENT_BINARY_DIR}'}, 10 | packages=['robustrelativeposepython'], 11 | package_data={'': ['*.so']} 12 | ) 13 | -------------------------------------------------------------------------------- /src/Essential.cpp: -------------------------------------------------------------------------------- 1 | #include "Essential.h" 2 | 3 | 4 | using namespace std::chrono; 5 | using namespace Optimization; 6 | using namespace Optimization::Convex; 7 | 8 | namespace Essential{ 9 | EssentialEstimationResult EssentialClass::getResults(void) 10 | { 11 | 12 | Matrix9 C; 13 | 14 | // output 15 | EssentialEstimationResult result; 16 | 17 | // initialize 18 | result.certifier_status = EssentialEstimationStatus::RS_ITER_LIMIT; 19 | 20 | 21 | Matrix3 E_8pts; 22 | Matrix3 matrix_precon; 23 | 24 | 25 | ////////////////////////////////////////////////// 26 | if (options_.estimation_verbose) 27 | std::cout << " Constructing data matrix C.\n"; 28 | // Get starting timepoint 29 | auto start_time_init = high_resolution_clock::now(); 30 | // create data matrix 31 | C = constructDataMatrix(points_); 32 | 33 | auto duration_construction_C = duration_cast(high_resolution_clock::now() - start_time_init); 34 | 35 | auto start_time_bounds = high_resolution_clock::now(); 36 | ////////////////////////////////////////////////// 37 | 38 | 39 | // Compute and cache preconditioning matrices 40 | // Just compute this if required 41 | if ((options_.use_preconditioning != Preconditioner::Any) || (options_.chosen_initialisation == InitialisationMethod::PTS8_INIT)) 42 | E_8pts = initialize8pts(C, matrix_precon, options_.use_preconditioning); 43 | 44 | auto duration_8pts = duration_cast(high_resolution_clock::now() - start_time_bounds); 45 | ////////////////////////////////////////////////// 46 | 47 | 48 | if (options_.estimation_verbose) 49 | 50 | std::cout << "Initial estimate from linear estimation (8pts): \n" << E_8pts << std::endl; 51 | 52 | 53 | 54 | 55 | if (options_.estimation_verbose) 56 | { 57 | std::cout << "Choosing initial guess: \n" << std::endl; 58 | } 59 | // Update initial guess 60 | Matrix3 E_initial; 61 | 62 | Matrix3 R_s; 63 | Vector3 t_s; 64 | 65 | auto start_time_init_essential = high_resolution_clock::now(); 66 | 67 | switch (options_.chosen_initialisation) 68 | { 69 | case InitialisationMethod::RANDOM_INIT: 70 | if (options_.estimation_verbose) std::cout << "Random guess... \n"; 71 | E_initial = projectToEssentialManifold(Matrix3::Random()); 72 | computeRtfromE(points_, E_initial, R_s, t_s); 73 | 74 | break; 75 | 76 | case InitialisationMethod::USER_SUPPLIED: 77 | if (options_.estimation_verbose) std::cout << "User supplied guess ... \n"; 78 | E_initial = projectToEssentialManifold(E_initial_); 79 | computeRtfromE(points_, E_initial, R_s, t_s); 80 | 81 | break; 82 | case InitialisationMethod::PTS5_INIT: 83 | if (options_.estimation_verbose) std::cout << "5 points algorithm init ... \n"; 84 | E_initial = initialize5pts(points_); 85 | computeRtfromE(points_, E_initial, R_s, t_s); 86 | break; 87 | 88 | case InitialisationMethod::PTS7_INIT: 89 | if (options_.estimation_verbose) std::cout << "7 points algorithm init ... \n"; 90 | E_initial = initialize7pts(points_); 91 | computeRtfromE(points_, E_initial, R_s, t_s); 92 | break; 93 | 94 | case InitialisationMethod::PTS8_INIT: 95 | if (options_.estimation_verbose) std::cout << "8 points guess ... \n"; 96 | E_initial = E_8pts; 97 | computeRtfromE(points_, E_initial, R_s, t_s); 98 | break; 99 | 100 | //case InitialisationMethod::EYE_INIT: 101 | default: 102 | if (options_.estimation_verbose) std::cout << "Identity guess... \n"; 103 | E_initial = projectToEssentialManifold(Matrix3::Identity()); 104 | computeRtfromE(points_, E_initial, R_s, t_s); 105 | 106 | break; 107 | } 108 | 109 | auto duration_init_diff_methods = duration_cast(high_resolution_clock::now() - start_time_init_essential); 110 | 111 | 112 | if (options_.estimation_verbose) 113 | { 114 | std::cout << "Initial guess rotation:\n" << R_s << std::endl; 115 | std::cout << "Initial guess translation Tgt:\n" << t_s << std::endl; 116 | } 117 | 118 | 119 | 120 | if (options_.estimation_verbose) 121 | std::cout << "Recovering R & t from E: \nR : \n" << R_s << "\nt:\n" << t_s << std::endl; 122 | 123 | 124 | Matrix34 Rt; 125 | Rt.block<3, 3>(0, 0) = R_s; 126 | Rt.block<3, 1>(0, 3) = t_s; 127 | 128 | // Show initial objective 129 | double f_initial = 0.5 * vec(E_initial).transpose() * C * vec(E_initial); 130 | 131 | if (options_.estimation_verbose) 132 | std::cout << "Initial objective value f_{init} = " << f_initial << std::endl; 133 | 134 | 135 | 136 | // Define the problem 137 | if (options_.estimation_verbose) std::cout << "Creating problem " << std::endl; 138 | 139 | 140 | 141 | /// Define the problem 142 | EssentialProblem problem(C, options_.use_preconditioning); 143 | problem.setPointCorrespondences(points_); 144 | // compute precon if needed 145 | // set precon matrix 146 | if (options_.use_preconditioning != Preconditioner::None) 147 | problem.setMatrixPrecon(matrix_precon); 148 | 149 | // Cache these three matrices for gradient & hessian 150 | ProblemCachedMatrices problem_matrices; 151 | 152 | /// Function handles required by the TNT optimization algorithm 153 | // Preconditioning operator (optional) 154 | std::experimental::optional> precon; 155 | 156 | if (options_.use_preconditioning == Preconditioner::None) 157 | { 158 | precon = std::experimental::nullopt; 159 | } 160 | else { 161 | Optimization::Riemannian::LinearOperator precon_op = 162 | [&problem](const Matrix34 &Y, const Matrix34 &Ydot, 163 | const ProblemCachedMatrices & problem_matrices) { 164 | return problem.precondition(Y, Ydot); 165 | }; 166 | precon = precon_op; 167 | } 168 | 169 | 170 | 171 | // Objective 172 | Optimization::Objective F = 173 | [&problem](const Matrix34 &Y, ProblemCachedMatrices & problem_matrices){ 174 | return problem.evaluate_objective(Y, problem_matrices); 175 | }; 176 | 177 | 178 | /// Gradient 179 | Optimization::Riemannian::VectorField grad_F = [&problem](const Matrix34 &Y, 180 | ProblemCachedMatrices & problem_matrices) { 181 | // Compute and cache Euclidean gradient at the current iterate 182 | problem_matrices.NablaF_Y = problem.Euclidean_gradient(Y, problem_matrices); 183 | 184 | // Compute Riemannian gradient from Euclidean one 185 | return problem.Riemannian_gradient(Y, problem_matrices.NablaF_Y); 186 | }; 187 | 188 | 189 | // Local quadratic model constructor 190 | Optimization::Riemannian::QuadraticModel QM = 191 | [&problem]( 192 | const Matrix34 &Y, Matrix34 &grad, 193 | Optimization::Riemannian::LinearOperator &HessOp, 194 | ProblemCachedMatrices & problem_matrices) { 195 | // Compute and cache Euclidean gradient at the current iterate 196 | problem_matrices.NablaF_Y = problem.Euclidean_gradient(Y, problem_matrices); 197 | // Compute Riemannian gradient from Euclidean gradient 198 | grad = problem.Riemannian_gradient(Y, problem_matrices.NablaF_Y ); 199 | 200 | 201 | // Define linear operator for computing Riemannian Hessian-vector 202 | // products 203 | HessOp = [&problem](const Matrix34 &Y, const Matrix34 &Ydot, 204 | const ProblemCachedMatrices & problem_matrices) { 205 | Matrix34 Hss = problem.Riemannian_Hessian_vector_product(Y, problem_matrices, Ydot); 206 | 207 | return Hss; 208 | }; 209 | }; 210 | 211 | // Riemannian metric 212 | 213 | // We consider a realization of the product of Stiefel manifolds as an 214 | // embedded submanifold of R^{r x dn}; consequently, the induced Riemannian 215 | // metric is simply the usual Euclidean inner product 216 | Optimization::Riemannian::RiemannianMetric 217 | metric = [&problem](const Matrix34 &Y, const Matrix34 &V1, const Matrix34 &V2, 218 | const ProblemCachedMatrices & problem_matrices) { 219 | 220 | Matrix3 R1, R2; 221 | Vector3 t1, t2; 222 | R1 = V1.block<3,3>(0,0); 223 | R2 = V2.block<3,3>(0,0); 224 | 225 | t1 = V1.block<3,1>(0,3); 226 | t2 = V2.block<3,1>(0,3); 227 | 228 | return ((R1 * R2.transpose()).trace() + t1.dot(t2)); 229 | }; 230 | 231 | // Retraction operator 232 | Optimization::Riemannian::Retraction retraction = 233 | [&problem](const Matrix34 &Y, const Matrix34 &Ydot, const ProblemCachedMatrices & problem_matrices) { 234 | return problem.retract(Y, Ydot); 235 | }; 236 | 237 | 238 | 239 | 240 | 241 | // Stop timer 242 | auto stop_init = high_resolution_clock::now(); 243 | 244 | auto duration_init = duration_cast( stop_init- start_time_init); 245 | 246 | 247 | // set up params for solver 248 | Optimization::Riemannian::TNTParams params; 249 | params.gradient_tolerance = options_.tol_grad_norm; 250 | params.relative_decrease_tolerance = options_.rel_func_decrease_tol; 251 | params.max_iterations = options_.max_RTR_iterations; 252 | params.max_TPCG_iterations = options_.max_tCG_iterations; 253 | params.preconditioned_gradient_tolerance = options_.preconditioned_grad_norm_tol; 254 | 255 | params.stepsize_tolerance = options_.stepsize_tol; 256 | 257 | 258 | // params.estimation_verbose = options.estimation_verbose; 259 | params.verbose = options_.verbose; 260 | 261 | 262 | /** An optional user-supplied function that can be used to instrument/monitor 263 | * the performance of the internal Riemannian truncated-Newton trust-region 264 | * optimization algorithm as it runs. */ 265 | // std::experimental::optional user_fcn; 266 | 267 | /// Run optimization! 268 | auto start_opt = high_resolution_clock::now(); 269 | 270 | Optimization::Riemannian::TNTResult TNTResults = Optimization::Riemannian::TNT( 271 | F, QM, metric, retraction, Rt, problem_matrices, precon, params); 272 | 273 | 274 | 275 | auto stop_opt = high_resolution_clock::now(); 276 | auto duration_opt = duration_cast(stop_opt - start_opt); 277 | 278 | // Extract the results 279 | if (options_.estimation_verbose) 280 | { 281 | std::cout << "Final point SO(3) x S(2): " << TNTResults.x << std::endl; 282 | std::cout << "Final objective: " << TNTResults.f << std::endl; 283 | } 284 | 285 | Matrix34 Rs_opt = TNTResults.x; 286 | Matrix3 R_opt = Rs_opt.block<3,3>(0,0); 287 | Vector3 t_opt = Rs_opt.block<3,1>(0,3); 288 | 289 | 290 | Matrix3 E_opt = computeEfromRt(R_opt, t_opt); 291 | 292 | double f_hat = TNTResults.f; 293 | double mu_min, dual_gap, d_hat; 294 | Matrix12 Q; 295 | 296 | /// Verification 297 | EssentialVerification verify_solution(C, E_opt, t_opt, f_hat, options_.eig_min_tol, options_.dual_gap_tol); 298 | 299 | 300 | // output from the verification 301 | Vector6 Lagrange_multipliers; 302 | size_t idx_relaxation = 10; 303 | bool is_opt_bool; 304 | 305 | // total times 306 | double lagrange_total = 0.0; 307 | double verification_total = 0.0; 308 | 309 | 310 | 311 | if (options_.use_all_relaxations == false) 312 | { 313 | 314 | if (options_.use_idx_relaxation == 0) 315 | { 316 | // 1. Compute Lagrange multipliers 317 | 318 | if (options_.estimation_verbose) std::cout << "Checking only one certifier\n"; 319 | 320 | auto start_lagrange_reduced = high_resolution_clock::now(); 321 | 322 | Lagrange_multipliers = verify_solution.computeLagMult(Q); 323 | 324 | auto duration_lagrange_reduced = duration_cast(high_resolution_clock::now() - start_lagrange_reduced); 325 | 326 | lagrange_total = duration_lagrange_reduced.count(); 327 | 328 | 329 | 330 | // 2. Check optimality (PSD) 331 | auto start_verification_reduced = high_resolution_clock::now(); 332 | 333 | is_opt_bool = verify_solution.checkOptimalitySolution(Lagrange_multipliers, Q, mu_min, dual_gap, d_hat); 334 | 335 | auto duration_verification_reduced = duration_cast(high_resolution_clock::now() - start_verification_reduced); 336 | 337 | 338 | verification_total = duration_verification_reduced.count(); 339 | 340 | if (is_opt_bool) idx_relaxation = 0; // save relaxation 0 (original) 341 | 342 | } 343 | 344 | 345 | else 346 | { 347 | if (options_.estimation_verbose) std::cout << "Checking certifier for relaxation " << 348 | options_.use_idx_relaxation << "\n"; 349 | 350 | // for sanity 351 | if (options_.use_idx_relaxation > 5) 352 | options_.use_idx_relaxation = 5; 353 | 354 | idx_relaxation = options_.use_idx_relaxation; 355 | 356 | auto start_lagrange_reduced = high_resolution_clock::now(); 357 | 358 | Lagrange_multipliers = verify_solution.computeLagMultGeneral(Q, idx_relaxation); 359 | 360 | auto duration_lagrange_reduced = duration_cast(high_resolution_clock::now() - start_lagrange_reduced); 361 | 362 | lagrange_total = duration_lagrange_reduced.count(); 363 | 364 | 365 | auto start_verification_reduced = high_resolution_clock::now(); 366 | 367 | is_opt_bool = verify_solution.checkOptimalitySolution(Lagrange_multipliers, Q, mu_min, dual_gap, d_hat, idx_relaxation); 368 | 369 | auto duration_verification_reduced = duration_cast(high_resolution_clock::now() - start_verification_reduced); 370 | 371 | verification_total = duration_verification_reduced.count(); 372 | 373 | } 374 | } 375 | // if use all the relaxations 376 | else 377 | { 378 | /** Checking additional certifiers **/ 379 | 380 | if (options_.estimation_verbose) std::cout << "Checking multiple multipliers\n"; 381 | 382 | // for sanity 383 | // if the user asks for all the relaxations 384 | if (options_.use_idx_relaxation > 5) 385 | options_.use_idx_relaxation = 5; 386 | 387 | 388 | 389 | for (idx_relaxation = 0; idx_relaxation <= options_.use_idx_relaxation; idx_relaxation++) 390 | { 391 | auto start_lagrange_reduced = high_resolution_clock::now(); 392 | Lagrange_multipliers = verify_solution.computeLagMultGeneral(Q, idx_relaxation); 393 | auto duration_lagrange_reduced = duration_cast(high_resolution_clock::now() - start_lagrange_reduced); 394 | lagrange_total += duration_lagrange_reduced.count(); 395 | 396 | auto start_verification_reduced = high_resolution_clock::now(); 397 | is_opt_bool = verify_solution.checkOptimalitySolution(Lagrange_multipliers, Q, mu_min, dual_gap, d_hat, idx_relaxation); 398 | auto duration_verification_reduced = duration_cast(high_resolution_clock::now() - start_verification_reduced); 399 | verification_total += duration_verification_reduced.count(); 400 | 401 | if (is_opt_bool == 1) 402 | break; // keep the last relaxation 403 | } 404 | 405 | 406 | } // end- select relaxation for the certifier 407 | 408 | 409 | if (is_opt_bool == 0) idx_relaxation = 10; // we could not certify the solution 410 | 411 | 412 | auto duration_total = duration_cast(high_resolution_clock::now() - start_time_init); 413 | 414 | 415 | if (options_.estimation_verbose) std::cout << "Tight relaxation with index: " << idx_relaxation << std::endl; 416 | 417 | 418 | 419 | // assign all the results to this struct 420 | result.idx_relaxation = idx_relaxation; 421 | result.f_hat = f_hat; 422 | result.d_hat = d_hat; 423 | result.dual_gap = dual_gap; 424 | result.gradnorm = problem.Riemannian_gradient(Rs_opt).norm(); 425 | result.mu_min = mu_min; 426 | result.lagrange_multipliers = Lagrange_multipliers; 427 | result.R_opt = R_opt; 428 | result.t_opt = t_opt; 429 | result.E_opt = E_opt; 430 | // Save time 431 | result.elapsed_init_time = duration_init.count(); 432 | result.elapsed_C_time = duration_construction_C.count(); // in microsecs 433 | result.elapsed_8pt_time = duration_8pts.count(); // in microsecs 434 | result.elapsed_iterative_time = duration_opt.count(); // in microsecs 435 | result.elapsed_lagrange_time = lagrange_total; 436 | result.elapsed_certifier_time = verification_total; 437 | result.elapsed_estimation_time = duration_total.count(); 438 | result.elapsed_time_methods = duration_init_diff_methods.count(); 439 | 440 | result.certifier_status = EssentialEstimationStatus::RS_ITER_LIMIT; 441 | 442 | 443 | if (is_opt_bool == true) result.certifier_status = EssentialEstimationStatus::GLOBAL_OPT; 444 | else result.certifier_status = EssentialEstimationStatus::NO_CERTIFIED; 445 | 446 | 447 | 448 | return result; 449 | 450 | }; // end of fcn getResult 451 | 452 | 453 | 454 | 455 | 456 | 457 | void EssentialClass::printResult(EssentialEstimationResult & my_result) 458 | { 459 | // print data 460 | 461 | std::cout << "Data from estimation\n--------------------\n"; 462 | std::cout << "f_hat = " << my_result.f_hat << std::endl; 463 | std::cout << "d_hat = " << my_result.d_hat << std::endl; 464 | 465 | std::cout << "Dual certification\n--------------------\n"; 466 | std::cout << "dual gap: " << my_result.dual_gap << std::endl; 467 | std::cout << "min. eigenvalue of M = " << my_result.mu_min << std::endl; 468 | std::cout << "Lagrange multipliers = " << my_result.lagrange_multipliers << std::endl; 469 | 470 | 471 | std::cout << "######## Times [in microseconds]:\n"; 472 | std::cout << "Data Matric C construction: " << my_result.elapsed_C_time << std::endl; 473 | std::cout << "-----\n Total init: " << my_result.elapsed_init_time << std::endl; 474 | 475 | std::cout << "Iterative Method: " << my_result.elapsed_iterative_time << std::endl; 476 | 477 | std::cout << "Computing Lagrange multipliers: " << my_result.elapsed_lagrange_time << std::endl; 478 | std::cout << "Certifying optimality: " << my_result.elapsed_certifier_time << std::endl; 479 | std::cout << "Verification: " << my_result.elapsed_certifier_time + my_result.elapsed_lagrange_time << std::endl; 480 | std::cout << "---------------------\n"; 481 | std::cout << "Total time: " << my_result.elapsed_estimation_time << std::endl << std::endl; 482 | 483 | std::cout << "\n Recovered R:\n" << my_result.R_opt << std::endl; 484 | std::cout << "\n Recovered t:\n" << my_result.t_opt << std::endl; 485 | 486 | std::cout << "\n Status estimation:\n 0-> GLOBAL OPT \n 1->INCONCLUSIVE \n 2->MAX ITER\n-1-> UNKNOWN STATUS\n\n"; 487 | 488 | int is_opt = -1; 489 | switch(my_result.certifier_status) 490 | { 491 | case EssentialEstimationStatus::RS_ITER_LIMIT: 492 | is_opt = 2; 493 | break; 494 | case EssentialEstimationStatus::GLOBAL_OPT: 495 | is_opt = 0; 496 | break; 497 | case EssentialEstimationStatus::NO_CERTIFIED: 498 | is_opt = 1; 499 | break; 500 | default: 501 | is_opt = -1; 502 | } 503 | 504 | std::cout << "STATUS: " << is_opt << std::endl; 505 | 506 | 507 | 508 | }; //end of print fcn 509 | 510 | } // end of essential namespace 511 | -------------------------------------------------------------------------------- /src/EssentialManifold.cpp: -------------------------------------------------------------------------------- 1 | #include "EssentialManifold.h" 2 | 3 | 4 | namespace Essential{ 5 | 6 | 7 | Matrix34 EssentialManifold::project(const Matrix34 & P) const 8 | { 9 | Matrix34 A; 10 | Matrix3 R_p, R = P.block<3,3>(0, 0); 11 | // Classic result 12 | Eigen::JacobiSVD svd(R, Eigen::ComputeFullU | Eigen::ComputeFullV); 13 | 14 | double detU = svd.matrixU().determinant(); 15 | double detV = svd.matrixV().determinant(); 16 | 17 | if (detU * detV > 0) 18 | R_p = svd.matrixU() * svd.matrixV().transpose(); 19 | else 20 | { 21 | Matrix3 Uprime = svd.matrixU(); 22 | Uprime.col(Uprime.cols() - 1) *= -1; 23 | R_p = Uprime * svd.matrixV().transpose(); 24 | } 25 | 26 | // For the translation 27 | Vector3 t_p = P.block<3,1>(0, 3); 28 | t_p.normalize(); 29 | 30 | // fill the output matrix 31 | A.setZero(); 32 | A.block<3,3>(0, 0) = R_p; 33 | A.block<3,1>(0, 3) = t_p; 34 | 35 | return A; 36 | } 37 | 38 | Vector3 EssentialManifold::ProjSphere(const Vector3 &t, const Vector3 &Vt) const 39 | { return Vt - t.dot(Vt) * t; } 40 | 41 | Matrix3 EssentialManifold::ProjRotation(const Matrix3& R, const Matrix3 & VR) const 42 | { 43 | Matrix3 RtVr = R.transpose() * VR; 44 | return R * 0.5 * (RtVr - RtVr.transpose()); 45 | } 46 | 47 | Matrix34 EssentialManifold::retract(const Matrix34 &Y, const Matrix34 &V) const { 48 | 49 | // We use projection-based retraction, as described in "Projection-Like 50 | // Retractions on Matrix Manifolds" by Absil and Malick 51 | return project(Y + V); 52 | } 53 | 54 | 55 | Matrix34 EssentialManifold::random_sample( 56 | const std::default_random_engine::result_type &seed) const { 57 | // Generate a matrix of the appropriate dimension by sampling its elements 58 | // from the standard Gaussian 59 | std::default_random_engine generator(seed); 60 | std::normal_distribution g; 61 | 62 | Matrix34 R; 63 | R.setZero(); 64 | 65 | for (size_t r = 0; r < 3; ++r) 66 | for (size_t c = 0; c < 4; ++c) 67 | R(r, c) = g(generator); 68 | return project(R); 69 | } 70 | 71 | Matrix3 EssentialManifold::SymProduct(const Matrix3 & Ydot, const Matrix3 Y, const Matrix3 & nabla_Y) const 72 | { 73 | Matrix3 P = Y * nabla_Y; 74 | return Ydot.transpose() * 0.5 * (P + P.transpose()); 75 | } 76 | 77 | // TODO: we can just call here to ProjSphere & ProjRotation 78 | Matrix34 EssentialManifold::Proj(const Matrix34 &Y, const Matrix34 &V) const{ 79 | Matrix34 V_tan; 80 | V_tan.setZero(); 81 | 82 | Matrix3 R, Vr; 83 | Vector3 t, Vt; 84 | 85 | // for the sphere 86 | Vt = V.block<3, 1>(0, 3); 87 | t = Y.block<3, 1>(0, 3); 88 | V_tan.block<3, 1>(0, 3) = Vt - t.dot(Vt) * t; 89 | // V_tan.block<3, 1>(0, 3) = ProjSphere(t, Vt); 90 | 91 | // for the rotation 92 | Vr = V.block<3,3>(0, 0); 93 | R = Y.block<3,3>(0, 0); 94 | Matrix3 RtVr = R.transpose() * Vr; 95 | V_tan.block<3,3>(0, 0) = R * 0.5 * (RtVr - RtVr.transpose()); 96 | // V_tan.block<3,3>(0, 0) = ProjRotation(R, Vr); 97 | return V_tan; 98 | } 99 | 100 | 101 | } // end of essential namespace 102 | -------------------------------------------------------------------------------- /src/EssentialProblem.cpp: -------------------------------------------------------------------------------- 1 | #include "EssentialProblem.h" 2 | 3 | 4 | namespace Essential{ 5 | EssentialProblem::EssentialProblem(const bearing_vectors_t& bearing_vectors, Preconditioner use_precon) : use_precon_(use_precon) 6 | { 7 | 8 | point_correspondences_ = bearing_vectors; 9 | data_matrix_C_ = constructDataMatrix(bearing_vectors); 10 | number_points_ = bearing_vectors.size(); 11 | 12 | Mt_.setZero(); 13 | Mr_.setZero(); 14 | constructBMatricesForMixedDiftR(B1_, B2_, B3_); 15 | 16 | 17 | }; //end of constructor 18 | // Delete this 19 | 20 | EssentialProblem::EssentialProblem(const Matrix9& data_C, Preconditioner use_precon) : use_precon_(use_precon) 21 | { 22 | data_matrix_C_ = data_C; 23 | 24 | Mt_.setZero(); 25 | Mr_.setZero(); 26 | constructBMatricesForMixedDiftR(B1_, B2_, B3_); 27 | 28 | }; //end of constructor 29 | 30 | void EssentialProblem::setNumberPoints(const bearing_vectors_t& bearing_vectors) 31 | { 32 | number_points_ = bearing_vectors.size(); 33 | } 34 | 35 | EssentialProblem::~EssentialProblem(void) 36 | { } 37 | 38 | 39 | weights_t EssentialProblem::computeResiduals(const Matrix34 & Rt) const 40 | { 41 | weights_t residual; 42 | residual.resize(number_points_); 43 | 44 | Matrix3 E = computeEfromRt(Rt); 45 | 46 | for(size_t i = 0; i < number_points_; i++) 47 | { 48 | Vector3 v1 = point_correspondences_[i].bearing_vector_1; 49 | Vector3 v0 = point_correspondences_[i].bearing_vector_0; 50 | residual(i) = (pow((v0.transpose() * E * v1), 2)); 51 | } 52 | return residual; 53 | } 54 | 55 | 56 | // update points with weights 57 | void EssentialProblem::updateWeights(const weights_t & new_weights) 58 | { 59 | 60 | assert(new_weights.cols() == number_points_); 61 | for(size_t i = 0; i < number_points_; i++) point_correspondences_[i].weight_ = new_weights(i); 62 | 63 | 64 | // update data matrix 65 | data_matrix_C_ = constructDataMatrix(point_correspondences_); 66 | } 67 | 68 | Matrix34 EssentialProblem::initializeSolver8pts(void) 69 | { 70 | // we suppose that data_matrix_C_ is already updated!! 71 | // for simplicity, we only implement the 8pt algorithm 72 | // Compute E AND save precon 73 | Matrix3 E = initialize8pts(data_matrix_C_, Matrix_precon_, Preconditioner::Any); 74 | 75 | // point_correspondences_ are already updated!! 76 | Matrix3 R_s; 77 | Vector3 t_s; 78 | computeRtfromE(point_correspondences_, E, R_s, t_s); 79 | Matrix34 Rt; 80 | Rt.block<3, 3>(0, 0) = R_s; 81 | Rt.block<3, 1>(0, 3) = t_s; 82 | return (Rt); 83 | } 84 | 85 | 86 | 87 | // Compute pseudo Jacobi preconditioner 88 | Matrix3 EssentialProblem::computePseudoJacobiPrecon(void) 89 | { 90 | // If you use the 8 points algorithm, please 91 | // select the use_precon option 92 | 93 | Eigen::JacobiSVD svd(data_matrix_C_, Eigen::ComputeFullU | Eigen::ComputeFullV); //faster 94 | 95 | Vector9 svd_diag = svd.singularValues(); 96 | return (Matrix3::Identity() / svd_diag(0)); 97 | } 98 | 99 | Matrix3 EssentialProblem::invMtPrecon(Matrix3 & R_init) 100 | { 101 | Matrix3 Mt = createMatrixT(data_matrix_C_, R_init); 102 | Eigen::JacobiSVD svd(Mt, Eigen::ComputeFullU | Eigen::ComputeFullV); 103 | // return (Mt); 104 | Vector3 svd_diag = svd.singularValues(); 105 | return (Matrix3::Identity() / svd_diag(0)); 106 | } 107 | 108 | 109 | 110 | 111 | 112 | // apply precon to the full X 113 | Matrix34 EssentialProblem::precondition(const Matrix34& X, const Matrix34 & Xdot) const 114 | { 115 | if (use_precon_ == Preconditioner::None) 116 | return Xdot; 117 | else return tangent_space_projection(X, Matrix_precon_ * Xdot); 118 | } 119 | 120 | 121 | 122 | 123 | 124 | double EssentialProblem::evaluate_objective(const Matrix34 &Y) const { 125 | // TODO: save this for latter (gradient & hessian) 126 | Vector3 t = Y.block<3, 1>(0, 3); 127 | Matrix3 R = Y.block<3, 3>(0, 0); 128 | Matrix3 Mt = createMatrixT(data_matrix_C_, R); 129 | 130 | Matrix9 Mr = createMatrixR(data_matrix_C_, t); 131 | 132 | 133 | return (0.5 * (t.transpose() * Mt * t).trace()); 134 | 135 | } 136 | 137 | double EssentialProblem::evaluate_objective(const Matrix34 &Y, ProblemCachedMatrices & problem_matrices) const { 138 | 139 | Vector3 t = Y.block<3, 1>(0, 3); 140 | Matrix3 R = Y.block<3, 3>(0, 0); 141 | problem_matrices.Mt = createMatrixT(data_matrix_C_, R); 142 | problem_matrices.Mr = createMatrixR(data_matrix_C_, t); 143 | 144 | return (0.5 * (t.transpose() * problem_matrices.Mt * t).trace()); 145 | 146 | } 147 | 148 | 149 | // TODO: Use the fcn below 150 | Matrix34 EssentialProblem::Euclidean_gradient(const Matrix34 &Y) const 151 | { 152 | Vector3 t = Y.block<3, 1>(0, 3); 153 | Matrix3 R = Y.block<3, 3>(0, 0); 154 | Matrix3 Mt = createMatrixT(data_matrix_C_, R); 155 | Matrix9 Mr = createMatrixR(data_matrix_C_, t); 156 | Matrix34 G; 157 | G.setZero(); 158 | Vector9 mr = Mr * vec(R); 159 | G.block<3,3>(0,0)= Eigen::Map (mr.data(), 3, 3); 160 | G.block<3,1>(0,3) = Mt * t; 161 | return G; 162 | } 163 | 164 | Matrix34 EssentialProblem::Euclidean_gradient(const Matrix34 &Y, const ProblemCachedMatrices & problem_matrices) const 165 | { 166 | Vector3 t = Y.block<3, 1>(0, 3); 167 | Matrix3 R = Y.block<3, 3>(0, 0); 168 | Matrix34 G; 169 | G.setZero(); 170 | Vector9 mr = problem_matrices.Mr * vec(R); 171 | G.block<3,3>(0,0)= Eigen::Map (mr.data(), 3, 3); 172 | G.block<3,1>(0,3) = problem_matrices.Mt * t; 173 | return G; 174 | } 175 | 176 | 177 | Matrix34 EssentialProblem::Riemannian_gradient(const Matrix34 &Y, const Matrix34 &nablaF_Y) const 178 | { 179 | return tangent_space_projection(Y, nablaF_Y); 180 | } 181 | 182 | Matrix34 EssentialProblem::Riemannian_gradient(const Matrix34 &Y) const { 183 | return tangent_space_projection(Y, Euclidean_gradient(Y)); 184 | } 185 | 186 | Matrix34 EssentialProblem::Riemannian_gradient(const Matrix34 &Y, const ProblemCachedMatrices & problem_matrices) const { 187 | return tangent_space_projection(Y, Euclidean_gradient(Y, problem_matrices)); 188 | } 189 | 190 | 191 | /** Given a matrix Y in the domain D of the SE-Sync optimization problem, and 192 | * a tangent vector dotY in T_D(Y), the tangent space of the domain of the 193 | * optimization problem at Y, this function computes and returns Hess 194 | * F(Y)[dotY], the action of the Riemannian Hessian on dotX */ 195 | // TODO: same with gradient 196 | Matrix34 EssentialProblem::Riemannian_Hessian_vector_product(const Matrix34 &Y, 197 | const Matrix34 &nablaF_Y, 198 | const Matrix34 &dotY) const 199 | { 200 | // Euclidean Hessian-vector product 201 | Matrix34 HessRiemannian; 202 | 203 | Vector3 t = Y.block<3, 1>(0, 3); 204 | Matrix3 R = Y.block<3, 3>(0, 0); 205 | 206 | 207 | Matrix3 Mt = createMatrixT(data_matrix_C_, R); 208 | Matrix9 Mr = createMatrixR(data_matrix_C_, t); 209 | Vector3 Vt = dotY.block<3, 1>(0, 3); 210 | Matrix3 VR = dotY.block<3, 3>(0, 0); 211 | 212 | Matrix39 mixed_terms_der = constructMixedDifTtr(B1_, 213 | B2_, B3_, data_matrix_C_ , t, R); 214 | 215 | // Compute Euclidean Hessian 216 | Matrix9By12 coeff_R; 217 | coeff_R.block<9, 9>(0, 0) = Mr; 218 | coeff_R.block<9, 3>(0, 9) = 2 * mixed_terms_der.transpose(); 219 | 220 | Vector12 Vx; 221 | Vx.block<9, 1>(0, 0) = vec(VR); 222 | Vx.block<3, 1>(9, 0) = Vt; 223 | 224 | Vector9 mr = coeff_R * Vx; 225 | Matrix3 HessR = Eigen::Map (mr.data(), 3, 3); 226 | 227 | Matrix3By12 coeff_T; 228 | coeff_T.block<3, 3>(0, 9) = Mt; 229 | coeff_T.block<3, 9>(0, 0) = 2 * mixed_terms_der; 230 | 231 | Vector3 HessT = coeff_T * Vx; 232 | 233 | // recover NabldaF(Y) 234 | Vector3 nabla_Yt = nablaF_Y.block<3,1>(0,3); 235 | Matrix3 nabla_YR = nablaF_Y.block<3,3>(0,0); 236 | 237 | // clean 238 | HessRiemannian.setZero(); 239 | 240 | // Riemannain Hessian for t (sphere) 241 | HessRiemannian.block<3,1>(0, 3) = domain_.ProjSphere(t, HessT) - (t.dot(nabla_Yt)) * Vt; 242 | 243 | // Riemannain Hessian for R (rotation) 244 | HessRiemannian.block<3,3>(0, 0) = domain_.ProjRotation(R, HessR - domain_.SymProduct(VR, R, nabla_YR)); 245 | // Riemannian hessian 246 | return HessRiemannian; 247 | 248 | } 249 | 250 | Matrix34 EssentialProblem::Riemannian_Hessian_vector_product(const Matrix34 &Y, 251 | const ProblemCachedMatrices & problem_matrices, 252 | const Matrix34 &dotY) const 253 | { 254 | // Euclidean Hessian-vector product 255 | Matrix34 HessRiemannian; 256 | 257 | Vector3 t = Y.block<3, 1>(0, 3); 258 | Matrix3 R = Y.block<3, 3>(0, 0); 259 | 260 | 261 | Vector3 Vt = dotY.block<3, 1>(0, 3); 262 | Matrix3 VR = dotY.block<3, 3>(0, 0); 263 | 264 | Matrix39 mixed_terms_der = constructMixedDifTtr(B1_, 265 | B2_, B3_, data_matrix_C_ , t, R); 266 | 267 | // Compute Euclidean Hessian 268 | Matrix9By12 coeff_R; 269 | coeff_R.block<9, 9>(0, 0) = problem_matrices.Mr; 270 | coeff_R.block<9, 3>(0, 9) = 2 * mixed_terms_der.transpose(); 271 | 272 | Vector12 Vx; 273 | Vx.block<9, 1>(0, 0) = vec(VR); 274 | Vx.block<3, 1>(9, 0) = Vt; 275 | 276 | Vector9 mr = coeff_R * Vx; 277 | Matrix3 HessR = Eigen::Map (mr.data(), 3, 3); 278 | 279 | Matrix3By12 coeff_T; 280 | coeff_T.block<3, 3>(0, 9) = problem_matrices.Mt; 281 | coeff_T.block<3, 9>(0, 0) = 2 * mixed_terms_der; 282 | 283 | Vector3 HessT = coeff_T * Vx; 284 | 285 | // recover NabldaF(Y) 286 | Vector3 nabla_Yt = (problem_matrices.NablaF_Y).block<3,1>(0,3); 287 | Matrix3 nabla_YR = (problem_matrices.NablaF_Y).block<3,3>(0,0); 288 | 289 | // clean for sanity 290 | HessRiemannian.setZero(); 291 | 292 | // Riemannain Hessian for t (sphere) 293 | HessRiemannian.block<3,1>(0, 3) = domain_.ProjSphere(t, HessT) - (t.dot(nabla_Yt)) * Vt; 294 | 295 | // Riemannain Hessian for R (rotation) 296 | HessRiemannian.block<3,3>(0, 0) = domain_.ProjRotation(R, HessR - domain_.SymProduct(VR, R, nabla_YR)); 297 | // Riemannian hessian 298 | return HessRiemannian; 299 | } 300 | 301 | 302 | Matrix34 EssentialProblem::Riemannian_Hessian_vector_product(const Matrix34 &Y, 303 | const Matrix34 &dotY) const 304 | { return Riemannian_Hessian_vector_product(Y, Euclidean_gradient(Y), dotY);} 305 | 306 | 307 | 308 | Matrix34 EssentialProblem::tangent_space_projection(const Matrix34 &Y, 309 | const Matrix34 &dotY) const { return domain_.Proj(Y, dotY); } 310 | 311 | 312 | Matrix34 EssentialProblem::retract(const Matrix34 &Y, const Matrix34 &dotY) const 313 | { 314 | return domain_.retract(Y, dotY); 315 | 316 | } 317 | 318 | Matrix34 EssentialProblem::random_sample() const 319 | { 320 | return domain_.random_sample(); 321 | } 322 | 323 | } // end of Essential namespace 324 | -------------------------------------------------------------------------------- /src/EssentialVerification.cpp: -------------------------------------------------------------------------------- 1 | #include "EssentialVerification.h" 2 | #include 3 | 4 | 5 | 6 | namespace Essential{ 7 | 8 | Vector6 EssentialVerification::computeLagMultGeneral(Matrix12 & Q, size_t idx_relaxation) 9 | { 10 | /* 11 | This function computes the Lagrange multipliers 12 | associated with the relaxation *idx_relaxation* [PARAM]. 13 | NOTE: If you only *want* or *need* one try, use the function below 14 | */ 15 | 16 | Vector12 x_opt, b, tt_opt; 17 | Vector6 Lagrange_multipliers; 18 | Vector5 reduced_Lagrange; 19 | x_opt.block<9, 1>(0, 0) = Eigen::Map(E_.data(), 9, 1); 20 | x_opt.block<3, 1>(9, 0) = t_; 21 | // tt_opt: blck(0_{9 times 9}, eye(3)) * x_opt 22 | tt_opt.setZero(); 23 | tt_opt.block<3,1>(9,0) = t_; 24 | 25 | 26 | Q.setZero(); 27 | Q.block<9, 9>(0, 0) = C_; 28 | // 0.5 because f = 0.5 * e' * C * e 29 | b = 0.5 * Q * x_opt - 0.5 * x_opt.transpose() * Q * x_opt * tt_opt; 30 | 31 | double x1=x_opt(0); double x2=x_opt(1); double x3=x_opt(2); 32 | double x4=x_opt(3); double x5=x_opt(4); double x6=x_opt(5); 33 | double x7=x_opt(6);double x8=x_opt(7); double x9=x_opt(8); 34 | double x10=x_opt(9); double x11=x_opt(10);double x12=x_opt(11); 35 | 36 | // int L1=0; 37 | int L2 = 0; int L3=L2+1; int L4=L3+1; int L5=L4+1; int L6=L5+1; int L7=L6+1; 38 | 39 | // NOTE; this matrix (with 6 columns) has linear dependent columns 40 | Eigen::Matrix A_ext; 41 | A_ext.setZero(); 42 | 43 | 44 | // first equation 45 | A_ext(0, L2) = x1; A_ext(0, L3) = x2*0.5; A_ext(0, L4) = x3*0.5; 46 | 47 | // Second equation 48 | A_ext(1, L3) = x1*0.5; A_ext(1, L5)=x2; A_ext(1, L6)=x3*0.5; 49 | 50 | // 3rd equation 51 | A_ext(2, L4) = x1*0.5; A_ext(2, L6)=x2*0.5; A_ext(2, L7) = x3; 52 | 53 | // 4th equation 54 | A_ext(3, L2)=x4; A_ext(3, L3)=x5*0.5; A_ext(3, L4) = x6*0.5; 55 | 56 | // 5th equation 57 | A_ext(4, L3)=x4*0.5; A_ext(4, L5) = x5; A_ext(4, L6) = x6*0.5; 58 | 59 | // 6th equation 60 | A_ext(5, L4)=x4*0.5; A_ext(5, L6) = x5*0.5; A_ext(5, L7) = x6; 61 | 62 | // 7th equation 63 | A_ext(6,L2)=x7; A_ext(6, L3)=x8*0.5; A_ext(6, L4)=x9*0.5; 64 | 65 | // 8th equation 66 | A_ext(7, L3)=x7*0.5; A_ext(7, L5)=x8; A_ext(7, L6)=x9*0.5; 67 | 68 | // 9th equation 69 | A_ext(8, L4)=x7*0.5; A_ext(8, L6)=x8*0.5; A_ext(8, L7)=x9; 70 | 71 | // 10th equation 72 | A_ext(9, L3)=x11*0.5; A_ext(9, L4)=x12*0.5; A_ext(9, L5)=-x10; 73 | A_ext(9, L7)=-x10; 74 | 75 | // 11th equation 76 | A_ext(10, L3)=x10*0.5; A_ext(10, L6)=x12*0.5; A_ext(10, L2)=-x11; 77 | A_ext(10, L7)=-x11; 78 | 79 | // 12th equation 80 | A_ext(11, L4)=x10*0.5; A_ext(11, L6)=x11*0.5; A_ext(11, L2)=-x12; 81 | A_ext(11, L5)=-x12; 82 | 83 | // select relaxation 84 | Eigen::Matrix A; 85 | A.setZero(); 86 | // fill the matrix 87 | // NOTE: idx_relaxation \in {0, 5} 88 | // 1. check extreme cases 89 | if (idx_relaxation == 0) 90 | { 91 | A = A_ext.rightCols(5); 92 | } 93 | else if (idx_relaxation == 5) 94 | { 95 | A = A_ext.leftCols(5); 96 | } 97 | else 98 | { 99 | A.leftCols(idx_relaxation) = A_ext.leftCols(idx_relaxation); 100 | A.rightCols(5 - idx_relaxation) = A_ext.rightCols(5 - idx_relaxation); 101 | } 102 | 103 | // solving 104 | reduced_Lagrange = A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b); 105 | 106 | 107 | 108 | // fill the vector with multipliers 109 | Lagrange_multipliers.setZero(); 110 | Lagrange_multipliers(0) = 0.5 * x_opt.transpose() * Q * x_opt; 111 | Lagrange_multipliers.bottomRows(5) = reduced_Lagrange; 112 | 113 | 114 | return Lagrange_multipliers; 115 | 116 | } 117 | 118 | 119 | 120 | 121 | 122 | 123 | Vector6 EssentialVerification::computeLagMult(Matrix12 & Q) 124 | { 125 | Vector12 x_opt, b, tt_opt; 126 | Vector6 Lagrange_multipliers; 127 | Vector5 reduced_Lagrange; 128 | x_opt.block<9, 1>(0, 0) = Eigen::Map(E_.data(), 9, 1); 129 | x_opt.block<3, 1>(9, 0) = t_; 130 | // tt_opt: blck(0_{9 times 9}, eye(3)) * x_opt 131 | tt_opt.setZero(); 132 | tt_opt.block<3,1>(9,0) = t_; 133 | 134 | 135 | Q.setZero(); 136 | Q.block<9, 9>(0, 0) = C_; 137 | // 0.5 because f = 0.5 * e' * C * e 138 | b = 0.5 * Q * x_opt - 0.5 * x_opt.transpose() * Q * x_opt * tt_opt; 139 | 140 | double x1=x_opt(0); double x2=x_opt(1); double x3=x_opt(2); 141 | double x4=x_opt(3); double x5=x_opt(4); double x6=x_opt(5); 142 | double x7=x_opt(6);double x8=x_opt(7); double x9=x_opt(8); 143 | double x10=x_opt(9); double x11=x_opt(10);double x12=x_opt(11); 144 | 145 | // int L1=0; 146 | int L3=0; int L4=1; int L5=2; int L6=3; int L7=4; 147 | Eigen::Matrix A; 148 | A.setZero(); 149 | // first equation 150 | // A(1, L2) = x1; 151 | A(0, L3) = x2*0.5; A(0, L4) = x3*0.5; 152 | 153 | // Second equation 154 | A(1, L3) = x1*0.5; A(1, L5)=x2; A(1, L6)=x3*0.5; 155 | 156 | // 3rd equation 157 | A(2, L4) = x1*0.5; A(2, L6)=x2*0.5; A(2, L7) = x3; 158 | 159 | // 4th equation 160 | // A(4, L2) = x4; 161 | A(3, L3)=x5*0.5; A(3, L4) = x6*0.5; 162 | 163 | // 5th equation 164 | A(4, L3)=x4*0.5; A(4, L5) = x5; A(4, L6) = x6*0.5; 165 | 166 | // 6th equation 167 | A(5, L4)=x4*0.5; A(5, L6) = x5*0.5; A(5, L7) = x6; 168 | 169 | // 7th equation 170 | // A(7, L2)=x7; 171 | A(6, L3)=x8*0.5; A(6, L4)=x9*0.5; 172 | 173 | // 8th equation 174 | A(7, L3)=x7*0.5; A(7, L5)=x8; A(7, L6)=x9*0.5; 175 | 176 | // 9th equation 177 | A(8, L4)=x7*0.5; A(8, L6)=x8*0.5; A(8, L7)=x9; 178 | 179 | // 10th equation 180 | A(9, L3)=x11*0.5; A(9, L4)=x12*0.5; A(9, L5)=-x10; 181 | // A(9, L1)=x10; 182 | A(9, L7)=-x10; 183 | 184 | // 11th equation 185 | A(10, L3)=x10*0.5; A(10, L6)=x12*0.5; 186 | //A(11, L2)=-x11; 187 | // A(10, L1)=x11; 188 | A(10, L7)=-x11; 189 | 190 | // 12th equation 191 | A(11, L4)=x10*0.5; A(11, L6)=x11*0.5; 192 | // A(12, L2)=-x12; 193 | // A(11, L1)=x12; 194 | A(11, L5)=-x12; 195 | 196 | reduced_Lagrange = A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b); 197 | 198 | // fill the vector with multipliers 199 | Lagrange_multipliers.setZero(); 200 | Lagrange_multipliers(0) = 0.5 * x_opt.transpose() * Q * x_opt; 201 | Lagrange_multipliers.block<5, 1>(1, 0) = reduced_Lagrange; 202 | 203 | 204 | return Lagrange_multipliers; 205 | } 206 | 207 | 208 | 209 | 210 | double EssentialVerification::computePenalisedMatrixMAndMinEigenvalue(const Vector6 & Lagrange_multipliers, const Matrix12 & Q) 211 | { 212 | Matrix12 M; 213 | M.setZero(); 214 | 215 | Matrix12 A, As; 216 | A.setZero(); 217 | As.setZero(); 218 | unsigned int e1=0; unsigned int e4=1; unsigned int e7=2; 219 | unsigned int e2=3; unsigned int e5=4; unsigned int e8=5; 220 | unsigned int e3=6; unsigned int e6=7; unsigned int e9=8; 221 | unsigned int t1=9; unsigned int t2=10; unsigned int t3=11; 222 | 223 | // initialize 224 | M = 0.5 * Q; 225 | 226 | 227 | // Create the matrices 228 | 229 | As.setZero(); 230 | A.setZero(); 231 | As(t1,t1)=1; As(t2,t2)=1;As(t3,t3)=1; A = 0.5 * (As + As.transpose()); 232 | M -= Lagrange_multipliers(0) * A; 233 | 234 | As.setZero(); 235 | A.setZero(); 236 | As(e1,e4)=1; As(e2,e5)=1;As(e3,e6)=1;As(t1, t2)=1; A = 0.5 * (As + As.transpose()); 237 | M -= Lagrange_multipliers(1) * A; 238 | 239 | As.setZero(); 240 | A.setZero(); 241 | As(e1,e7)=1; As(e2,e8)=1;As(e3,e9)=1;As(t1, t3)=1; A = 0.5 * (As + As.transpose()); 242 | M -= Lagrange_multipliers(2) * A; 243 | 244 | As.setZero(); 245 | A.setZero(); 246 | As(e4,e4)=1; As(e5,e5)=1;As(e6,e6)=1;As(t1, t1)=-1; As(t3, t3)=-1; A = 0.5 * (As + As.transpose()); 247 | M -= Lagrange_multipliers(3) * A; 248 | 249 | As.setZero(); 250 | A.setZero(); 251 | As(e4,e7)=1; As(e5,e8)=1;As(e6,e9)=1;As(t2, t3)=1; A = 0.5 * (As + As.transpose()); 252 | M -= Lagrange_multipliers(4) * A; 253 | 254 | As.setZero(); 255 | A.setZero(); 256 | As(e7,e7)=1; As(e8,e8)=1;As(e9,e9)=1;As(t1, t1)=-1; As(t2, t2)=-1; A = 0.5 * (As + As.transpose()); 257 | M -= Lagrange_multipliers(5) * A; 258 | 259 | 260 | // Compute min eigenvalue of M 261 | // d=eig(M); 262 | 263 | 264 | double min_eigenvalue = -1000; 265 | Vector12 min_eigenvector; 266 | 267 | 268 | 269 | Vector3 eigenvalues_bottomrightM = ((M.block<3,3>(9, 9)).eigenvalues()).real(); 270 | min_eigenvalue = eigenvalues_bottomrightM.minCoeff(); 271 | // In many cases, this part fails 272 | 273 | if (min_eigenvalue > tau_) 274 | { 275 | // Try with the other one 276 | Vector9 eigenvalues_topleft = ((M.block<9,9>(0, 0)).eigenvalues()).real(); 277 | double min_eigenvalue_topleft = eigenvalues_topleft.minCoeff(); 278 | 279 | // check which one is lower and keep it 280 | if (min_eigenvalue_topleft < min_eigenvalue) min_eigenvalue = min_eigenvalue_topleft; 281 | } 282 | 283 | 284 | return min_eigenvalue; 285 | 286 | } 287 | 288 | 289 | 290 | 291 | 292 | double EssentialVerification::computePenalisedMatrixMAndMinEigenvalueGeneral(const Vector6 & Lagrange_multipliers, 293 | const Matrix12 & Q, size_t idx_relaxation) 294 | { 295 | /* This function checks the optimality of the solution via the general algorithm 296 | in the paper. 297 | The relaxation is indexed by the PARAM *idx_relaxation* from 1 to 5 (inclusive) 298 | */ 299 | 300 | Matrix12 M; 301 | M.setZero(); 302 | 303 | Matrix12 A, As; 304 | A.setZero(); 305 | As.setZero(); 306 | unsigned int e1=0; unsigned int e4=1; unsigned int e7=2; 307 | unsigned int e2=3; unsigned int e5=4; unsigned int e8=5; 308 | unsigned int e3=6; unsigned int e6=7; unsigned int e9=8; 309 | unsigned int t1=9; unsigned int t2=10; unsigned int t3=11; 310 | 311 | M = 0.5 * Q; 312 | 313 | Vector7 Lagrange_multipliers_ext; 314 | // fill the vector 315 | Lagrange_multipliers_ext.setZero(); 316 | // trivial cases 317 | if (idx_relaxation == 5) 318 | Lagrange_multipliers_ext.topRows(6) = Lagrange_multipliers; 319 | else 320 | { 321 | Lagrange_multipliers_ext.topRows(idx_relaxation + 1) = Lagrange_multipliers.topRows(idx_relaxation + 1); 322 | Lagrange_multipliers_ext.bottomRows(5 - idx_relaxation) = Lagrange_multipliers.bottomRows(5 - idx_relaxation); 323 | } 324 | 325 | // Create the matrices 326 | As.setZero(); 327 | A.setZero(); 328 | // t1^2 + t2^2 + t3^2 = 1 329 | As(t1,t1)=1; As(t2,t2)=1;As(t3,t3)=1; A = 0.5 * (As + As.transpose()); 330 | M -= Lagrange_multipliers_ext(0) * A; 331 | 332 | 333 | As.setZero(); 334 | A.setZero(); 335 | // e1^2 + e2^2 + e3^2 = t2 ^2 + t3^2 336 | As(e1,e1)=1; As(e2,e2)=1;As(e3,e3)=1;As(t2, t2)=-1;As(t3, t3)=-1; A = 0.5 * (As + As.transpose()); 337 | M -= Lagrange_multipliers_ext(1) * A; 338 | 339 | 340 | As.setZero(); 341 | A.setZero(); 342 | // e1*e4 * e2*e5 + e3*e6 = -t1*t2 343 | As(e1,e4)=1; As(e2,e5)=1;As(e3,e6)=1;As(t1, t2)=1; A = 0.5 * (As + As.transpose()); 344 | M -= Lagrange_multipliers_ext(2) * A; 345 | 346 | As.setZero(); 347 | A.setZero(); 348 | // e1*e7 * e2*e8 + e3*e9 = -t1*t3 349 | As(e1,e7)=1; As(e2,e8)=1;As(e3,e9)=1;As(t1, t3)=1; A = 0.5 * (As + As.transpose()); 350 | M -= Lagrange_multipliers_ext(3) * A; 351 | 352 | As.setZero(); 353 | A.setZero(); 354 | // e4^2 + e5^2 + e6^2 = t1 ^2 + t3^2 355 | As(e4,e4)=1; As(e5,e5)=1;As(e6,e6)=1;As(t1, t1)=-1; As(t3, t3)=-1; A = 0.5 * (As + As.transpose()); 356 | M -= Lagrange_multipliers_ext(4) * A; 357 | 358 | As.setZero(); 359 | A.setZero(); 360 | // e4*e7 * e5*e8 + e6*e9 = -t2*t3 361 | As(e4,e7)=1; As(e5,e8)=1;As(e6,e9)=1;As(t2, t3)=1; A = 0.5 * (As + As.transpose()); 362 | M -= Lagrange_multipliers_ext(5) * A; 363 | 364 | As.setZero(); 365 | A.setZero(); 366 | // e7^2 + e8^2 + e9^2 = t1 ^2 + t2^2 367 | As(e7,e7)=1; As(e8,e8)=1;As(e9,e9)=1;As(t1, t1)=-1; As(t2, t2)=-1; A = 0.5 * (As + As.transpose()); 368 | M -= Lagrange_multipliers_ext(6) * A; 369 | 370 | 371 | // Compute min eigenvalue of M 372 | 373 | double min_eigenvalue = -1000; 374 | Vector12 min_eigenvector; 375 | 376 | Vector3 eigenvalues_bottomrightM = ((M.block<3,3>(9, 9)).eigenvalues()).real(); 377 | min_eigenvalue = eigenvalues_bottomrightM.minCoeff(); 378 | // In many cases, this part fails 379 | if (min_eigenvalue > tau_) 380 | { 381 | // Try with the other one 382 | 383 | Vector9 eigenvalues_topleft = ((M.block<9,9>(0, 0)).eigenvalues()).real(); 384 | double min_eigenvalue_topleft = eigenvalues_topleft.minCoeff(); 385 | // check which one is lower and keep it 386 | if (min_eigenvalue_topleft < min_eigenvalue) min_eigenvalue = min_eigenvalue_topleft; 387 | } 388 | 389 | 390 | return min_eigenvalue; 391 | 392 | } 393 | 394 | 395 | 396 | 397 | 398 | 399 | 400 | 401 | 402 | bool EssentialVerification::checkOptimalitySolution(const Vector6 & Lagrange_multipliers, 403 | const Matrix12 & Q, double& mu_min, 404 | double& dual_gap, double& d_hat, 405 | size_t idx_relaxation) 406 | { 407 | bool is_opt = false; 408 | // Verify PSD of M 409 | // check first relaxation 410 | if (idx_relaxation == 0) 411 | mu_min = computePenalisedMatrixMAndMinEigenvalue(Lagrange_multipliers, Q); 412 | else 413 | mu_min = computePenalisedMatrixMAndMinEigenvalueGeneral(Lagrange_multipliers, Q, idx_relaxation); 414 | 415 | // Compute dual objective 416 | d_hat = Lagrange_multipliers(0); 417 | 418 | // compute dual gap 419 | dual_gap = f_hat_ - d_hat; 420 | // Check conditions for optimality 421 | if (mu_min > tau_) 422 | is_opt = true; 423 | return is_opt; 424 | } 425 | 426 | 427 | 428 | 429 | } // end of essential namespace 430 | -------------------------------------------------------------------------------- /src/generateCorrespondences.cpp: -------------------------------------------------------------------------------- 1 | #include "generateCorrespondences.h" 2 | 3 | #define PI 3.14159 4 | 5 | namespace Essential 6 | { 7 | /* from random_generators.cpp */ 8 | 9 | Vector3 generateRandomPointTruncated(double FoV, double min_depth, double max_depth) 10 | { 11 | generateRandomPointTruncated(FoV, min_depth, max_depth, false, 20); 12 | } 13 | 14 | Vector3 generateRandomPointNaive(double min_depth, double max_depth) 15 | { 16 | /* Naive points: no Fov */ 17 | Eigen::Vector3d cleanPoint; 18 | cleanPoint[0] = (((double) rand()) / ((double) RAND_MAX)-0.5)*2.0; 19 | cleanPoint[1] = (((double) rand()) / ((double) RAND_MAX)-0.5)*2.0; 20 | cleanPoint[2] = (((double) rand()) / ((double) RAND_MAX)); // always non-negative Z 21 | Eigen::Vector3d direction = cleanPoint.normalized(); 22 | cleanPoint = 23 | (max_depth-min_depth) * cleanPoint + min_depth * direction; 24 | 25 | 26 | return cleanPoint; 27 | 28 | } 29 | 30 | 31 | Vector3 generateRandomPointTruncated(double FoV, double min_depth, double max_depth, bool allow_coplanar, double max_X) 32 | { 33 | 34 | Vector3 point3D; 35 | // Note: tan(X) applies mod(X, pi) before computing the actual tan 36 | 37 | // depth 38 | point3D[2] = min_depth + (max_depth - min_depth) * ( ((double) std::rand() / (double) RAND_MAX)); 39 | 40 | double xmax = (tan(FoV * 0.5 * PI / 180)) * point3D[2]; 41 | if (xmax <= 0) xmax *= -1; 42 | 43 | if (!allow_coplanar) 44 | // update xmax 45 | xmax = std::min(xmax, max_X); 46 | 47 | point3D[0] = xmax * (((double) rand()) / ((double) RAND_MAX)-0.5)*2.0; 48 | point3D[1] = xmax * (((double) rand()) / ((double) RAND_MAX)-0.5)*2.0; 49 | 50 | return point3D; 51 | 52 | } 53 | 54 | /* From opengv */ 55 | Vector3 addNoise( double noiseLevel, Vector3 cleanPoint, double focal_length ) 56 | { 57 | //compute a vector in the normal plane (based on good conditioning) 58 | Eigen::Vector3d normalVector1; 59 | if( 60 | (fabs(cleanPoint[0]) > fabs(cleanPoint[1])) && 61 | (fabs(cleanPoint[0]) > fabs(cleanPoint[2])) ) 62 | { 63 | normalVector1[1] = 1.0; 64 | normalVector1[2] = 0.0; 65 | normalVector1[0] = -cleanPoint[1]/cleanPoint[0]; 66 | } 67 | else 68 | { 69 | if( 70 | (fabs(cleanPoint[1]) > fabs(cleanPoint[0])) && 71 | (fabs(cleanPoint[1]) > fabs(cleanPoint[2])) ) 72 | { 73 | normalVector1[2] = 1.0; 74 | normalVector1[0] = 0.0; 75 | normalVector1[1] = -cleanPoint[2]/cleanPoint[1]; 76 | } 77 | else 78 | { 79 | normalVector1[0] = 1.0; 80 | normalVector1[1] = 0.0; 81 | normalVector1[2] = -cleanPoint[0]/cleanPoint[2]; 82 | } 83 | } 84 | 85 | normalVector1 = normalVector1 / normalVector1.norm(); 86 | Eigen::Vector3d normalVector2 = cleanPoint.cross(normalVector1); 87 | double noiseX = 88 | noiseLevel * (((double) std::rand())/ ((double) RAND_MAX)-0.5)*2.0 / 1.4142; 89 | double noiseY = 90 | noiseLevel * (((double) std::rand())/ ((double) RAND_MAX)-0.5)*2.0 / 1.4142; 91 | 92 | Eigen::Vector3d noisyPoint = 93 | focal_length * cleanPoint + noiseX *normalVector1 + noiseY * normalVector2; 94 | noisyPoint = noisyPoint / noisyPoint.norm(); 95 | return noisyPoint; 96 | 97 | } 98 | 99 | Vector3 generateRandomTranslation( double parallax ) 100 | { 101 | 102 | Vector3 translation; 103 | translation[0] = (((double) rand())/ ((double) RAND_MAX)-0.5)*2.0; 104 | translation[1] = (((double) rand())/ ((double) RAND_MAX)-0.5)*2.0; 105 | translation[2] = -(((double) rand())/ ((double) RAND_MAX)); 106 | 107 | 108 | return (parallax * translation.normalized()); 109 | } 110 | 111 | 112 | /* From opengv */ 113 | Matrix3 generateRandomRotation( double maxAngle ) 114 | { 115 | 116 | Vector3 rpy; 117 | rpy[0] = ((double) std::rand())/ ((double) RAND_MAX); 118 | rpy[1] = ((double) std::rand())/ ((double) RAND_MAX); 119 | rpy[2] = ((double) std::rand())/ ((double) RAND_MAX); 120 | 121 | rpy[0] = maxAngle*2.0*(rpy[0]-0.5); 122 | rpy[1] = maxAngle*2.0*(rpy[1]-0.5); 123 | rpy[2] = maxAngle*2.0*(rpy[2]-0.5); 124 | 125 | Eigen::Matrix3d R1; 126 | R1(0,0) = 1.0; 127 | R1(0,1) = 0.0; 128 | R1(0,2) = 0.0; 129 | R1(1,0) = 0.0; 130 | R1(1,1) = cos(rpy[0]); 131 | R1(1,2) = -sin(rpy[0]); 132 | R1(2,0) = 0.0; 133 | R1(2,1) = -R1(1,2); 134 | R1(2,2) = R1(1,1); 135 | 136 | Eigen::Matrix3d R2; 137 | R2(0,0) = cos(rpy[1]); 138 | R2(0,1) = 0.0; 139 | R2(0,2) = sin(rpy[1]); 140 | R2(1,0) = 0.0; 141 | R2(1,1) = 1.0; 142 | R2(1,2) = 0.0; 143 | R2(2,0) = -R2(0,2); 144 | R2(2,1) = 0.0; 145 | R2(2,2) = R2(0,0); 146 | 147 | Eigen::Matrix3d R3; 148 | R3(0,0) = cos(rpy[2]); 149 | R3(0,1) = -sin(rpy[2]); 150 | R3(0,2) = 0.0; 151 | R3(1,0) =-R3(0,1); 152 | R3(1,1) = R3(0,0); 153 | R3(1,2) = 0.0; 154 | R3(2,0) = 0.0; 155 | R3(2,1) = 0.0; 156 | R3(2,2) = 1.0; 157 | 158 | Matrix3 rotation = R3 * R2 * R1; 159 | 160 | rotation.col(0) = rotation.col(0) / rotation.col(0).norm(); 161 | rotation.col(2) = rotation.col(0).cross(rotation.col(1)); 162 | rotation.col(2) = rotation.col(2) / rotation.col(2).norm(); 163 | rotation.col(1) = rotation.col(2).cross(rotation.col(0)); 164 | rotation.col(1) = rotation.col(1) / rotation.col(1).norm(); 165 | 166 | return rotation; 167 | } 168 | 169 | 170 | 171 | 172 | void createSyntheticExperiment( 173 | const size_t n_points, 174 | double noise, 175 | double outlier_fraction, 176 | double FoV, 177 | double parallax, 178 | double min_depth, 179 | double max_depth, 180 | Vector3 & translation, 181 | Matrix3 & rotation, 182 | bearing_vectors_t & points_correspondences, 183 | Eigen::MatrixXd & gt, 184 | std::vector & indices_outliers ) 185 | { 186 | createSyntheticExperiment(n_points, noise, outlier_fraction, FoV, 187 | parallax, min_depth, max_depth, translation, 188 | rotation, points_correspondences, gt, indices_outliers, false, 20, 1000000000); 189 | 190 | } 191 | 192 | void createSyntheticExperiment( 193 | const size_t n_points, 194 | double noise, 195 | double outlier_fraction, 196 | double FoV, 197 | double parallax, 198 | double min_depth, 199 | double max_depth, 200 | Vector3 & translation, 201 | Matrix3 & rotation, 202 | bearing_vectors_t & points_correspondences, 203 | Eigen::MatrixXd & gt, 204 | std::vector & indices_outliers, 205 | double min_epipolar_error_sq // min epipolar error for the outliers 206 | ) 207 | { 208 | createSyntheticExperiment(n_points, noise, outlier_fraction, FoV, 209 | parallax, min_depth, max_depth, translation, 210 | rotation, points_correspondences, gt, indices_outliers, false, 20, min_epipolar_error_sq); 211 | 212 | } 213 | 214 | 215 | void createSyntheticExperiment( 216 | const size_t n_points, 217 | double noise, 218 | double outlier_fraction, 219 | double FoV, 220 | double parallax, 221 | double min_depth, 222 | double max_depth, 223 | Vector3 & translation, 224 | Matrix3 & rotation, 225 | bearing_vectors_t & points_correspondences, 226 | Eigen::MatrixXd & gt, 227 | std::vector & indices_outliers, 228 | bool allow_coplanar, // allow to generate synthetic scenes with coplanar points 229 | double max_X // this value is the absolute value. Max value for X-axis allowed 230 | ) 231 | { 232 | // call the other function 233 | // with default params 234 | createSyntheticExperiment(n_points, noise, outlier_fraction, FoV, 235 | parallax, min_depth, max_depth, translation, 236 | rotation, points_correspondences, gt, indices_outliers, 237 | allow_coplanar, max_X, 1000000000); 238 | } 239 | 240 | 241 | void createSyntheticExperiment( 242 | const size_t n_points, 243 | double noise, 244 | double outlier_fraction, 245 | double FoV, 246 | double parallax, 247 | double min_depth, 248 | double max_depth, 249 | Vector3 & translation, 250 | Matrix3 & rotation, 251 | bearing_vectors_t & points_correspondences, 252 | Eigen::MatrixXd & gt, 253 | std::vector & indices_outliers, 254 | bool allow_coplanar, // allow to generate synthetic scenes with coplanar points 255 | double max_X, // this value is the absolute value. Max value for X-axis allowed 256 | double min_epipolar_error_sq // min epipolar error for the outliers 257 | ) 258 | { 259 | createSyntheticExperiment(n_points, noise, outlier_fraction, FoV, 260 | parallax, min_depth, max_depth, translation, 261 | rotation, points_correspondences, gt, indices_outliers, 262 | allow_coplanar, max_X, min_epipolar_error_sq, 800); 263 | 264 | 265 | } 266 | 267 | void createSyntheticExperiment( 268 | const size_t n_points, 269 | double noise, 270 | double outlier_fraction, 271 | double FoV, 272 | double parallax, 273 | double min_depth, 274 | double max_depth, 275 | Vector3 & translation, 276 | Matrix3 & rotation, 277 | bearing_vectors_t & points_correspondences, 278 | Eigen::MatrixXd & gt, 279 | std::vector & indices_outliers, 280 | bool allow_coplanar, // allow to generate synthetic scenes with coplanar points 281 | double max_X, // this value is the absolute value. Max value for X-axis allowed 282 | double min_epipolar_error_sq, // min epipolar error for the outliers 283 | double focal_length // focal length in pixelss 284 | ) 285 | { 286 | 287 | 288 | // generate a random pointcloud 289 | gt.setZero(); 290 | for( size_t i = 0; i < n_points; i++ ) 291 | gt.col(i) = generateRandomPointTruncated( FoV, min_depth, 292 | max_depth, allow_coplanar, max_X ); 293 | 294 | 295 | double max_rotation = 0.5; 296 | 297 | // 2. Create the relative rotation and translation 298 | // 2.1 Fix first frame 299 | Matrix3 R1 = Matrix3::Identity(); 300 | Vector3 T1 = Vector3::Zero(); 301 | 302 | // 2.2 Fix second frame 303 | Matrix3 R2 = Matrix3::Zero(); 304 | Vector3 T2 = Vector3::Zero(); 305 | 306 | bool valid_position = false; 307 | Eigen::MatrixXd points3D_frame2 = gt; 308 | int max_n_iters = 100, n_iters = 0; 309 | 310 | // create random translation 311 | // note: we always create a backward movement 312 | T2 = generateRandomTranslation(parallax); 313 | 314 | do 315 | { 316 | // Generate a random pose for the second camera 317 | // such that all the points lie inside its FOV 318 | R2 = generateRandomRotation(max_rotation); 319 | 320 | // compute relative coordinates 321 | points3D_frame2.setZero(); 322 | for( size_t i = 0; i < n_points; i++ ) 323 | { 324 | Vector3 temp = R2.transpose() * (gt.col(i)- T2); 325 | points3D_frame2.col(i) = Eigen::Map(temp.data(), 3, 1); 326 | } 327 | 328 | 329 | // check condition for FoV 330 | 331 | Eigen::VectorXd ratio_X = (points3D_frame2.row(0)).cwiseQuotient((points3D_frame2.row(2))); 332 | double max_ratio_x = (ratio_X.array().abs()).maxCoeff(); 333 | // check if any point was outside the FoV 334 | 335 | if (max_ratio_x > tan(FoV * 0.5 * PI / 180)) 336 | { 337 | n_iters++; 338 | continue; 339 | } 340 | 341 | 342 | // std::cout << "X-coordinates of points is valid \n"; 343 | Eigen::VectorXd ratio_Y = (points3D_frame2.row(1)).cwiseQuotient((points3D_frame2.row(2))); 344 | double max_ratio_y = (ratio_Y.array().abs()).maxCoeff(); 345 | // check if any point was outside the FoV 346 | 347 | if (max_ratio_y > tan(FoV * 0.5 * PI / 180)) 348 | { 349 | n_iters++; 350 | continue; 351 | } 352 | 353 | 354 | // the position is valid 355 | valid_position = true; 356 | 357 | }while(!valid_position && (n_iters <= max_n_iters)); 358 | 359 | // check invalid rotation 360 | if ((!valid_position) && (n_iters > max_n_iters)) 361 | std::cout << "[ERROR] Pose is not valid.\nSome points do not lie in the FOV\n"; 362 | 363 | 364 | rotation = R2; 365 | translation = T2.normalized(); 366 | 367 | 368 | // Generate the correspondences 369 | for( size_t i = 0; i < n_points; i++ ) 370 | { 371 | Vector3 obs1, obs2; 372 | obs1 = gt.col(i); 373 | obs1.normalize(); 374 | 375 | obs2 = points3D_frame2.col(i); 376 | obs2.normalize(); 377 | 378 | //add noise 379 | if( noise > 0.0 ) 380 | { 381 | obs1 = addNoise(noise, obs1, focal_length); 382 | obs2 = addNoise(noise, obs2, focal_length); 383 | } 384 | 385 | CorrespondingFeatures correspondence; 386 | correspondence.bearing_vector_0 = obs1.normalized(); 387 | correspondence.bearing_vector_1 = obs2.normalized(); 388 | correspondence.weight_ = 1.0; 389 | 390 | // add it to the std::vector 391 | points_correspondences.push_back(correspondence); 392 | } 393 | 394 | /* OUTLIERS */ 395 | 396 | // add outliers 397 | size_t number_outliers = (size_t) floor(outlier_fraction*n_points); 398 | size_t max_number_iters = 50; 399 | for(size_t i = 0; i < number_outliers; i++) 400 | { 401 | size_t i_iter = 0; 402 | bool valid_outlier = false; 403 | do 404 | { 405 | 406 | Vector3 outlier_correspondence = generateRandomPointTruncated(170, min_depth, 407 | max_depth, true, max_X); 408 | 409 | 410 | //normalize the bearing vector 411 | outlier_correspondence = rotation.transpose() * (outlier_correspondence - T2); 412 | outlier_correspondence.normalize(); 413 | 414 | // check residual 415 | Matrix3 t_cross; 416 | t_cross.setZero(); 417 | t_cross(0, 1) = -translation(2); t_cross(0, 2) = translation(1); 418 | t_cross(1, 0) = translation(2); t_cross(1, 2) = -translation(0); 419 | t_cross(2, 0) = -translation(1); t_cross(2, 1) = translation(0); 420 | 421 | double res = (pow( 422 | ((points_correspondences[i].bearing_vector_0).transpose() * t_cross * rotation * outlier_correspondence), 423 | 2)); 424 | 425 | 426 | 427 | if (res > min_epipolar_error_sq) 428 | { 429 | points_correspondences[i].bearing_vector_1 = outlier_correspondence; 430 | indices_outliers.push_back(i); 431 | 432 | valid_outlier = true; 433 | } 434 | 435 | if ((!valid_outlier) && (i_iter > max_number_iters)) 436 | { 437 | // break the loop 438 | outlier_correspondence[0] = 1.0; 439 | outlier_correspondence[1] = 1.0; 440 | outlier_correspondence[2] = 1.0; 441 | 442 | outlier_correspondence.normalize(); 443 | 444 | points_correspondences[i].bearing_vector_1 = outlier_correspondence; 445 | indices_outliers.push_back(i); 446 | 447 | valid_outlier = true; 448 | } 449 | 450 | // increase the counter 451 | i_iter += 1; 452 | 453 | 454 | }while(!valid_outlier); 455 | } 456 | 457 | 458 | } 459 | 460 | 461 | 462 | } // end of essential namespace 463 | --------------------------------------------------------------------------------