diff --git a/.github/scripts/python.sh b/.github/scripts/python.sh index 6948cc385..a71e14c97 100644 --- a/.github/scripts/python.sh +++ b/.github/scripts/python.sh @@ -43,11 +43,6 @@ if [ -z ${PYTHON_VERSION+x} ]; then exit 127 fi -if [ -z ${WRAPPER+x} ]; then - echo "Please provide the wrapper to build!" - exit 126 -fi - PYTHON="python${PYTHON_VERSION}" if [[ $(uname) == "Darwin" ]]; then @@ -61,25 +56,11 @@ PATH=$PATH:$($PYTHON -c "import site; print(site.USER_BASE)")/bin [ "${GTSAM_WITH_TBB:-OFF}" = "ON" ] && install_tbb -case $WRAPPER in -"cython") - BUILD_CYTHON="ON" - BUILD_PYBIND="OFF" - TYPEDEF_POINTS_TO_VECTORS="OFF" - sudo $PYTHON -m pip install -r $GITHUB_WORKSPACE/cython/requirements.txt - ;; -"pybind") - BUILD_CYTHON="OFF" - BUILD_PYBIND="ON" - TYPEDEF_POINTS_TO_VECTORS="ON" +BUILD_PYBIND="ON" +TYPEDEF_POINTS_TO_VECTORS="ON" - sudo $PYTHON -m pip install -r $GITHUB_WORKSPACE/python/requirements.txt - ;; -*) - exit 126 - ;; -esac +sudo $PYTHON -m pip install -r $GITHUB_WORKSPACE/python/requirements.txt mkdir $GITHUB_WORKSPACE/build cd $GITHUB_WORKSPACE/build @@ -90,7 +71,6 @@ cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=Release \ -DGTSAM_WITH_TBB=${GTSAM_WITH_TBB:-OFF} \ -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \ -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \ - -DGTSAM_INSTALL_CYTHON_TOOLBOX=${BUILD_CYTHON} \ -DGTSAM_BUILD_PYTHON=${BUILD_PYBIND} \ -DGTSAM_TYPEDEF_POINTS_TO_VECTORS=${TYPEDEF_POINTS_TO_VECTORS} \ -DGTSAM_PYTHON_VERSION=$PYTHON_VERSION \ @@ -98,30 +78,10 @@ cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=Release \ -DGTSAM_ALLOW_DEPRECATED_SINCE_V41=OFF \ -DCMAKE_INSTALL_PREFIX=$GITHUB_WORKSPACE/gtsam_install -make -j$(nproc) install & +make -j$(nproc) install -while ps -p $! > /dev/null -do - sleep 60 - now=$(date +%s) - printf "%d seconds have elapsed\n" $(( (now - start) )) -done -case $WRAPPER in -"cython") - cd $GITHUB_WORKSPACE/build/cython - $PYTHON setup.py install --user --prefix= - cd $GITHUB_WORKSPACE/build/cython/gtsam/tests - $PYTHON -m unittest discover - ;; -"pybind") - cd $GITHUB_WORKSPACE/build/python - $PYTHON setup.py install --user --prefix= - cd $GITHUB_WORKSPACE/python/gtsam/tests - $PYTHON -m unittest discover - ;; -*) - echo "THIS SHOULD NEVER HAPPEN!" - exit 125 - ;; -esac \ No newline at end of file +cd $GITHUB_WORKSPACE/build/python +$PYTHON setup.py install --user --prefix= +cd $GITHUB_WORKSPACE/python/gtsam/tests +$PYTHON -m unittest discover diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml index afe328c3b..87a0430f8 100644 --- a/.github/workflows/build-linux.yml +++ b/.github/workflows/build-linux.yml @@ -48,8 +48,15 @@ jobs: - name: Install (Linux) if: runner.os == 'Linux' run: | - # LLVM 9 is not in Bionic's repositories so we add the official LLVM repository. + # LLVM (clang) 9 is not in Bionic's repositories so we add the official LLVM repository. if [ "${{ matrix.compiler }}" = "clang" ] && [ "${{ matrix.version }}" = "9" ]; then + # (ipv4|ha).pool.sks-keyservers.net is the SKS GPG global keyserver pool + # ipv4 avoids potential timeouts because of crappy IPv6 infrastructure + # 15CF4D18AF4F7421 is the GPG key for the LLVM apt repository + # This key is not in the keystore by default for Ubuntu so we need to add it. + LLVM_KEY=15CF4D18AF4F7421 + gpg --keyserver ipv4.pool.sks-keyservers.net --recv-key $LLVM_KEY || gpg --keyserver ha.pool.sks-keyservers.net --recv-key $LLVM_KEY + gpg -a --export $LLVM_KEY | sudo apt-key add - sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main" fi sudo apt-get -y update diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index 363cd690f..3ce98055a 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -35,7 +35,9 @@ jobs: - name: Install (macOS) if: runner.os == 'macOS' run: | - brew install cmake ninja boost + brew tap ProfFan/robotics + brew install cmake ninja + brew install ProfFan/robotics/boost if [ "${{ matrix.compiler }}" = "gcc" ]; then brew install gcc@${{ matrix.version }} echo "::set-env name=CC::gcc-${{ matrix.version }}" @@ -48,4 +50,4 @@ jobs: - name: Build and Test (macOS) if: runner.os == 'macOS' run: | - bash .github/scripts/unix.sh -t \ No newline at end of file + bash .github/scripts/unix.sh -t diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index dc03ec6c9..8255fb8ab 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -12,7 +12,6 @@ jobs: CTEST_PARALLEL_LEVEL: 2 CMAKE_BUILD_TYPE: ${{ matrix.build_type }} PYTHON_VERSION: ${{ matrix.python_version }} - WRAPPER: ${{ matrix.wrapper }} strategy: fail-fast: false matrix: @@ -28,7 +27,6 @@ jobs: build_type: [Debug, Release] python_version: [3] - wrapper: [pybind] include: - name: ubuntu-18.04-gcc-5 os: ubuntu-18.04 @@ -63,8 +61,14 @@ jobs: - name: Install (Linux) if: runner.os == 'Linux' run: | - # LLVM 9 is not in Bionic's repositories so we add the official LLVM repository. if [ "${{ matrix.compiler }}" = "clang" ] && [ "${{ matrix.version }}" = "9" ]; then + # (ipv4|ha).pool.sks-keyservers.net is the SKS GPG global keyserver pool + # ipv4 avoids potential timeouts because of crappy IPv6 infrastructure + # 15CF4D18AF4F7421 is the GPG key for the LLVM apt repository + # This key is not in the keystore by default for Ubuntu so we need to add it. + LLVM_KEY=15CF4D18AF4F7421 + gpg --keyserver ipv4.pool.sks-keyservers.net --recv-key $LLVM_KEY || gpg --keyserver ha.pool.sks-keyservers.net --recv-key $LLVM_KEY + gpg -a --export $LLVM_KEY | sudo apt-key add - sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main" fi sudo apt-get -y update @@ -83,7 +87,9 @@ jobs: - name: Install (macOS) if: runner.os == 'macOS' run: | - brew install cmake ninja boost + brew tap ProfFan/robotics + brew install cmake ninja + brew install ProfFan/robotics/boost if [ "${{ matrix.compiler }}" = "gcc" ]; then brew install gcc@${{ matrix.version }} echo "::set-env name=CC::gcc-${{ matrix.version }}" @@ -105,4 +111,4 @@ jobs: - name: Build (macOS) if: runner.os == 'macOS' run: | - bash .github/scripts/python.sh \ No newline at end of file + bash .github/scripts/python.sh diff --git a/.github/workflows/build-special.yml b/.github/workflows/build-special.yml index 648365f24..c314acb16 100644 --- a/.github/workflows/build-special.yml +++ b/.github/workflows/build-special.yml @@ -56,6 +56,8 @@ jobs: run: | # LLVM 9 is not in Bionic's repositories so we add the official LLVM repository. if [ "${{ matrix.compiler }}" = "clang" ] && [ "${{ matrix.version }}" = "9" ]; then + gpg --keyserver pool.sks-keyservers.net --recv-key 15CF4D18AF4F7421 + gpg -a --export 15CF4D18AF4F7421 | sudo apt-key add - sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main" fi sudo apt-get -y update diff --git a/.github/workflows/trigger-python.yml b/.github/workflows/trigger-python.yml index 94527e732..1e8981d99 100644 --- a/.github/workflows/trigger-python.yml +++ b/.github/workflows/trigger-python.yml @@ -1,11 +1,11 @@ -# This triggers Cython builds on `gtsam-manylinux-build` +# This triggers Python builds on `gtsam-manylinux-build` name: Trigger Python Builds on: push: branches: - develop jobs: - triggerCython: + triggerPython: runs-on: ubuntu-latest steps: - name: Repository Dispatch @@ -13,5 +13,5 @@ jobs: with: token: ${{ secrets.PYTHON_CI_REPO_ACCESS_TOKEN }} repository: borglab/gtsam-manylinux-build - event-type: cython-wrapper + event-type: python-wrapper client-payload: '{"ref": "${{ github.ref }}", "sha": "${{ github.sha }}"}' diff --git a/.gitignore b/.gitignore index c2d6ce60f..cde059767 100644 --- a/.gitignore +++ b/.gitignore @@ -9,12 +9,6 @@ *.txt.user *.txt.user.6d59f0c *.pydevproject -cython/venv -cython/gtsam.cpp -cython/gtsam.cpython-35m-darwin.so -cython/gtsam.pyx -cython/gtsam.so -cython/gtsam_wrapper.pxd .vscode .env /.vs/ diff --git a/CMakeLists.txt b/CMakeLists.txt index eedc42c9e..35c487fd3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -22,17 +22,10 @@ set (CMAKE_PROJECT_VERSION_PATCH ${GTSAM_VERSION_PATCH}) ############################################################################### # Gather information, perform checks, set defaults -# Set the default install path to home -#set (CMAKE_INSTALL_PREFIX ${HOME} CACHE PATH "Install prefix for library") - set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH}" "${CMAKE_CURRENT_SOURCE_DIR}/cmake") include(GtsamMakeConfigFile) include(GNUInstallDirs) -# Record the root dir for gtsam - needed during external builds, e.g., ROS -set(GTSAM_SOURCE_ROOT_DIR ${CMAKE_CURRENT_SOURCE_DIR}) -message(STATUS "GTSAM_SOURCE_ROOT_DIR: [${GTSAM_SOURCE_ROOT_DIR}]") - # Load build type flags and default to Debug mode include(GtsamBuildTypes) @@ -45,399 +38,21 @@ if(${CMAKE_SOURCE_DIR} STREQUAL ${CMAKE_BINARY_DIR}) message(FATAL_ERROR "In-source builds not allowed. Please make a new directory (called a build directory) and run CMake from there. You may need to remove CMakeCache.txt. ") endif() -# See whether gtsam_unstable is available (it will be present only if we're using a git checkout) -if(EXISTS "${PROJECT_SOURCE_DIR}/gtsam_unstable" AND IS_DIRECTORY "${PROJECT_SOURCE_DIR}/gtsam_unstable") - set(GTSAM_UNSTABLE_AVAILABLE 1) -else() - set(GTSAM_UNSTABLE_AVAILABLE 0) -endif() +include(cmake/HandleBoost.cmake) # Boost +include(cmake/HandleCCache.cmake) # ccache +include(cmake/HandleCPack.cmake) # CPack +include(cmake/HandleEigen.cmake) # Eigen3 +include(cmake/HandleGeneralOptions.cmake) # CMake build options +include(cmake/HandleMKL.cmake) # MKL +include(cmake/HandleOpenMP.cmake) # OpenMP +include(cmake/HandlePerfTools.cmake) # Google perftools +include(cmake/HandlePython.cmake) # Python options and commands +include(cmake/HandleTBB.cmake) # TBB +include(cmake/HandleUninstall.cmake) # for "make uninstall" -# ---------------------------------------------------------------------------- -# Uninstall target, for "make uninstall" -# ---------------------------------------------------------------------------- -configure_file( - "${CMAKE_CURRENT_SOURCE_DIR}/cmake/cmake_uninstall.cmake.in" - "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake" - IMMEDIATE @ONLY) +include(cmake/HandleAllocators.cmake) # Must be after tbb, pertools -add_custom_target(uninstall - "${CMAKE_COMMAND}" -P "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake") - - -############################################################################### -# Set up options - -# Configurable Options -if(GTSAM_UNSTABLE_AVAILABLE) - option(GTSAM_BUILD_UNSTABLE "Enable/Disable libgtsam_unstable" ON) - option(GTSAM_UNSTABLE_BUILD_PYTHON "Enable/Disable Python wrapper for libgtsam_unstable" ON) - option(GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX "Enable/Disable MATLAB wrapper for libgtsam_unstable" OFF) -endif() -option(BUILD_SHARED_LIBS "Build shared gtsam library, instead of static" ON) -option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF) -option(GTSAM_POSE3_EXPMAP "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." ON) -option(GTSAM_ROT3_EXPMAP "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." ON) -option(GTSAM_ENABLE_CONSISTENCY_CHECKS "Enable/Disable expensive consistency checks" OFF) -option(GTSAM_WITH_TBB "Use Intel Threaded Building Blocks (TBB) if available" ON) -option(GTSAM_WITH_EIGEN_MKL "Eigen will use Intel MKL if available" OFF) -option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" OFF) -option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON) -option(GTSAM_BUILD_PYTHON "Enable/Disable building & installation of Python module with pybind11" OFF) -option(GTSAM_ALLOW_DEPRECATED_SINCE_V41 "Allow use of methods/functions deprecated in GTSAM 4.1" ON) -option(GTSAM_SUPPORT_NESTED_DISSECTION "Support Metis-based nested dissection" ON) -option(GTSAM_TANGENT_PREINTEGRATION "Use new ImuFactor with integration on tangent space" ON) -if(NOT MSVC AND NOT XCODE_VERSION) - option(GTSAM_BUILD_WITH_CCACHE "Use ccache compiler cache" ON) -endif() - -if(NOT MSVC AND NOT XCODE_VERSION) - # Set the build type to upper case for downstream use - string(TOUPPER "${CMAKE_BUILD_TYPE}" CMAKE_BUILD_TYPE_UPPER) - - # Set the GTSAM_BUILD_TAG variable. - # If build type is Release, set to blank (""), else set to the build type. - if(${CMAKE_BUILD_TYPE_UPPER} STREQUAL "RELEASE") - set(GTSAM_BUILD_TAG "") # Don't create release mode tag on installed directory - else() - set(GTSAM_BUILD_TAG "${CMAKE_BUILD_TYPE}") - endif() -endif() - -# Options relating to MATLAB wrapper -# TODO: Check for matlab mex binary before handling building of binaries -option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" OFF) -set(GTSAM_PYTHON_VERSION "Default" CACHE STRING "The version of Python to build the wrappers against.") - -# Check / set dependent variables for MATLAB wrapper -if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_BUILD_TYPE_POSTFIXES) - set(CURRENT_POSTFIX ${CMAKE_${CMAKE_BUILD_TYPE_UPPER}_POSTFIX}) -endif() - -if(GTSAM_INSTALL_MATLAB_TOOLBOX AND NOT BUILD_SHARED_LIBS) - message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and BUILD_SHARED_LIBS=OFF. The MATLAB wrapper cannot be compiled with a static GTSAM library because mex modules are themselves shared libraries. If you want a self-contained mex module, enable GTSAM_MEX_BUILD_STATIC_MODULE instead of BUILD_SHARED_LIBS=OFF.") -endif() - -if(GTSAM_BUILD_PYTHON) - # Get info about the Python3 interpreter - # https://cmake.org/cmake/help/latest/module/FindPython3.html#module:FindPython3 - find_package(Python3 COMPONENTS Interpreter Development) - - if(NOT ${Python3_FOUND}) - message(FATAL_ERROR "Cannot find Python3 interpreter. Please install Python >= 3.6.") - endif() - - if(${GTSAM_PYTHON_VERSION} STREQUAL "Default") - set(GTSAM_PYTHON_VERSION "${Python3_VERSION_MAJOR}.${Python3_VERSION_MINOR}" - CACHE - STRING - "The version of Python to build the wrappers against." - FORCE) - endif() - - if(GTSAM_UNSTABLE_BUILD_PYTHON) - if (NOT GTSAM_BUILD_UNSTABLE) - message(WARNING "GTSAM_UNSTABLE_BUILD_PYTHON requires the unstable module to be enabled.") - set(GTSAM_UNSTABLE_BUILD_PYTHON OFF) - endif() - endif() - - set(GTSAM_PY_INSTALL_PATH "${CMAKE_INSTALL_PREFIX}/python") -endif() - -# Flags for choosing default packaging tools -set(CPACK_SOURCE_GENERATOR "TGZ" CACHE STRING "CPack Default Source Generator") -set(CPACK_GENERATOR "TGZ" CACHE STRING "CPack Default Binary Generator") - -if (CMAKE_GENERATOR STREQUAL "Ninja" AND - ((CMAKE_CXX_COMPILER_ID STREQUAL "GNU" AND NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 4.9) OR - (CMAKE_CXX_COMPILER_ID STREQUAL "Clang" AND NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 3.5))) - # Force colored warnings in Ninja's output, if the compiler has -fdiagnostics-color support. - # Rationale in https://github.com/ninja-build/ninja/issues/814 - add_compile_options(-fdiagnostics-color=always) -endif() - -############################################################################### -# Find boost - -# To change the path for boost, you will need to set: -# BOOST_ROOT: path to install prefix for boost -# Boost_NO_SYSTEM_PATHS: set to true to keep the find script from ignoring BOOST_ROOT - -if(MSVC) - # By default, boost only builds static libraries on windows - set(Boost_USE_STATIC_LIBS ON) # only find static libs - # If we ever reset above on windows and, ... - # If we use Boost shared libs, disable auto linking. - # Some libraries, at least Boost Program Options, rely on this to export DLL symbols. - if(NOT Boost_USE_STATIC_LIBS) - list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC BOOST_ALL_NO_LIB BOOST_ALL_DYN_LINK) - endif() - # Virtual memory range for PCH exceeded on VS2015 - if(MSVC_VERSION LESS 1910) # older than VS2017 - list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE -Zm295) - endif() -endif() - -# If building DLLs in MSVC, we need to avoid EIGEN_STATIC_ASSERT() -# or explicit instantiation will generate build errors. -# See: https://bitbucket.org/gtborg/gtsam/issues/417/fail-to-build-on-msvc-2017 -# -if(MSVC AND BUILD_SHARED_LIBS) - list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC EIGEN_NO_STATIC_ASSERT) -endif() - -# Store these in variables so they are automatically replicated in GTSAMConfig.cmake and such. -set(BOOST_FIND_MINIMUM_VERSION 1.58) -set(BOOST_FIND_MINIMUM_COMPONENTS serialization system filesystem thread program_options date_time timer chrono regex) - -find_package(Boost ${BOOST_FIND_MINIMUM_VERSION} COMPONENTS ${BOOST_FIND_MINIMUM_COMPONENTS}) - -# Required components -if(NOT Boost_SERIALIZATION_LIBRARY OR NOT Boost_SYSTEM_LIBRARY OR NOT Boost_FILESYSTEM_LIBRARY OR - NOT Boost_THREAD_LIBRARY OR NOT Boost_DATE_TIME_LIBRARY) - message(FATAL_ERROR "Missing required Boost components >= v1.58, please install/upgrade Boost or configure your search paths.") -endif() - -option(GTSAM_DISABLE_NEW_TIMERS "Disables using Boost.chrono for timing" OFF) -# Allow for not using the timer libraries on boost < 1.48 (GTSAM timing code falls back to old timer library) -set(GTSAM_BOOST_LIBRARIES - Boost::serialization - Boost::system - Boost::filesystem - Boost::thread - Boost::date_time - Boost::regex -) -if (GTSAM_DISABLE_NEW_TIMERS) - message("WARNING: GTSAM timing instrumentation manually disabled") - list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC DGTSAM_DISABLE_NEW_TIMERS) -else() - if(Boost_TIMER_LIBRARY) - list(APPEND GTSAM_BOOST_LIBRARIES Boost::timer Boost::chrono) - else() - list(APPEND GTSAM_BOOST_LIBRARIES rt) # When using the header-only boost timer library, need -lrt - message("WARNING: GTSAM timing instrumentation will use the older, less accurate, Boost timer library because boost older than 1.48 was found.") - endif() -endif() - -############################################################################### -# Find TBB -find_package(TBB 4.4 COMPONENTS tbb tbbmalloc) - -# Set up variables if we're using TBB -if(TBB_FOUND AND GTSAM_WITH_TBB) - set(GTSAM_USE_TBB 1) # This will go into config.h - if ((${TBB_VERSION_MAJOR} GREATER 2020) OR (${TBB_VERSION_MAJOR} EQUAL 2020)) - set(TBB_GREATER_EQUAL_2020 1) - else() - set(TBB_GREATER_EQUAL_2020 0) - endif() - # all definitions and link requisites will go via imported targets: - # tbb & tbbmalloc - list(APPEND GTSAM_ADDITIONAL_LIBRARIES tbb tbbmalloc) -else() - set(GTSAM_USE_TBB 0) # This will go into config.h -endif() - -############################################################################### -# Prohibit Timing build mode in combination with TBB -if(GTSAM_USE_TBB AND (CMAKE_BUILD_TYPE STREQUAL "Timing")) - message(FATAL_ERROR "Timing build mode cannot be used together with TBB. Use a sampling profiler such as Instruments or Intel VTune Amplifier instead.") -endif() - - -############################################################################### -# Find Google perftools -find_package(GooglePerfTools) - -############################################################################### -# Support ccache, if installed -if(NOT MSVC AND NOT XCODE_VERSION) - find_program(CCACHE_FOUND ccache) - if(CCACHE_FOUND) - if(GTSAM_BUILD_WITH_CCACHE) - set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE ccache) - set_property(GLOBAL PROPERTY RULE_LAUNCH_LINK ccache) - else() - set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE "") - set_property(GLOBAL PROPERTY RULE_LAUNCH_LINK "") - endif() - endif(CCACHE_FOUND) -endif() - -############################################################################### -# Find MKL -find_package(MKL) - -if(MKL_FOUND AND GTSAM_WITH_EIGEN_MKL) - set(GTSAM_USE_EIGEN_MKL 1) # This will go into config.h - set(EIGEN_USE_MKL_ALL 1) # This will go into config.h - it makes Eigen use MKL - list(APPEND GTSAM_ADDITIONAL_LIBRARIES ${MKL_LIBRARIES}) - - # --no-as-needed is required with gcc according to the MKL link advisor - if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") - set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,--no-as-needed") - endif() -else() - set(GTSAM_USE_EIGEN_MKL 0) - set(EIGEN_USE_MKL_ALL 0) -endif() - -############################################################################### -# Find OpenMP (if we're also using MKL) -find_package(OpenMP) # do this here to generate correct message if disabled - -if(GTSAM_WITH_EIGEN_MKL AND GTSAM_WITH_EIGEN_MKL_OPENMP AND GTSAM_USE_EIGEN_MKL) - if(OPENMP_FOUND AND GTSAM_USE_EIGEN_MKL AND GTSAM_WITH_EIGEN_MKL_OPENMP) - set(GTSAM_USE_EIGEN_MKL_OPENMP 1) # This will go into config.h - list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC ${OpenMP_CXX_FLAGS}) - endif() -endif() - - -############################################################################### -# Option for using system Eigen or GTSAM-bundled Eigen -### These patches only affect usage of MKL. If you want to enable MKL, you *must* -### use our patched version of Eigen -### See: http://eigen.tuxfamily.org/bz/show_bug.cgi?id=704 (Householder QR MKL selection) -### http://eigen.tuxfamily.org/bz/show_bug.cgi?id=705 (Fix MKL LLT return code) -option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', use the one bundled with GTSAM" OFF) -option(GTSAM_WITH_EIGEN_UNSUPPORTED "Install Eigen's unsupported modules" OFF) - -# Switch for using system Eigen or GTSAM-bundled Eigen -if(GTSAM_USE_SYSTEM_EIGEN) - find_package(Eigen3 REQUIRED) - - # Use generic Eigen include paths e.g. - set(GTSAM_EIGEN_INCLUDE_FOR_INSTALL "${EIGEN3_INCLUDE_DIR}") - - # check if MKL is also enabled - can have one or the other, but not both! - # Note: Eigen >= v3.2.5 includes our patches - if(EIGEN_USE_MKL_ALL AND (EIGEN3_VERSION VERSION_LESS 3.2.5)) - message(FATAL_ERROR "MKL requires at least Eigen 3.2.5, and your system appears to have an older version. Disable GTSAM_USE_SYSTEM_EIGEN to use GTSAM's copy of Eigen, or disable GTSAM_WITH_EIGEN_MKL") - endif() - - # Check for Eigen version which doesn't work with MKL - # See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1527 for details. - if(EIGEN_USE_MKL_ALL AND (EIGEN3_VERSION VERSION_EQUAL 3.3.4)) - message(FATAL_ERROR "MKL does not work with Eigen 3.3.4 because of a bug in Eigen. See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1527. Disable GTSAM_USE_SYSTEM_EIGEN to use GTSAM's copy of Eigen, disable GTSAM_WITH_EIGEN_MKL, or upgrade/patch your installation of Eigen.") - endif() - - # The actual include directory (for BUILD cmake target interface): - set(GTSAM_EIGEN_INCLUDE_FOR_BUILD "${EIGEN3_INCLUDE_DIR}") -else() - # Use bundled Eigen include path. - # Clear any variables set by FindEigen3 - if(EIGEN3_INCLUDE_DIR) - set(EIGEN3_INCLUDE_DIR NOTFOUND CACHE STRING "" FORCE) - endif() - - # set full path to be used by external projects - # this will be added to GTSAM_INCLUDE_DIR by gtsam_extra.cmake.in - set(GTSAM_EIGEN_INCLUDE_FOR_INSTALL "include/gtsam/3rdparty/Eigen/") - - # The actual include directory (for BUILD cmake target interface): - set(GTSAM_EIGEN_INCLUDE_FOR_BUILD "${CMAKE_SOURCE_DIR}/gtsam/3rdparty/Eigen/") -endif() - -# Detect Eigen version: -set(EIGEN_VER_H "${GTSAM_EIGEN_INCLUDE_FOR_BUILD}/Eigen/src/Core/util/Macros.h") -if (EXISTS ${EIGEN_VER_H}) - file(READ "${EIGEN_VER_H}" STR_EIGEN_VERSION) - - # Extract the Eigen version from the Macros.h file, lines "#define EIGEN_WORLD_VERSION XX", etc... - - string(REGEX MATCH "EIGEN_WORLD_VERSION[ ]+[0-9]+" GTSAM_EIGEN_VERSION_WORLD "${STR_EIGEN_VERSION}") - string(REGEX MATCH "[0-9]+" GTSAM_EIGEN_VERSION_WORLD "${GTSAM_EIGEN_VERSION_WORLD}") - - string(REGEX MATCH "EIGEN_MAJOR_VERSION[ ]+[0-9]+" GTSAM_EIGEN_VERSION_MAJOR "${STR_EIGEN_VERSION}") - string(REGEX MATCH "[0-9]+" GTSAM_EIGEN_VERSION_MAJOR "${GTSAM_EIGEN_VERSION_MAJOR}") - - string(REGEX MATCH "EIGEN_MINOR_VERSION[ ]+[0-9]+" GTSAM_EIGEN_VERSION_MINOR "${STR_EIGEN_VERSION}") - string(REGEX MATCH "[0-9]+" GTSAM_EIGEN_VERSION_MINOR "${GTSAM_EIGEN_VERSION_MINOR}") - - set(GTSAM_EIGEN_VERSION "${GTSAM_EIGEN_VERSION_WORLD}.${GTSAM_EIGEN_VERSION_MAJOR}.${GTSAM_EIGEN_VERSION_MINOR}") - - message(STATUS "Found Eigen version: ${GTSAM_EIGEN_VERSION}") -else() - message(WARNING "Cannot determine Eigen version, missing file: `${EIGEN_VER_H}`") -endif () - -if (MSVC) - if (BUILD_SHARED_LIBS) - # mute eigen static assert to avoid errors in shared lib - list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC EIGEN_NO_STATIC_ASSERT) - endif() - list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE "/wd4244") # Disable loss of precision which is thrown all over our Eigen -endif() - -if (APPLE AND BUILD_SHARED_LIBS) - # Set the default install directory on macOS - set(CMAKE_INSTALL_NAME_DIR "${CMAKE_INSTALL_PREFIX}/lib") -endif() - -############################################################################### -# Global compile options - -# Build list of possible allocators -set(possible_allocators "") -if(GTSAM_USE_TBB) - list(APPEND possible_allocators TBB) - set(preferred_allocator TBB) -else() - list(APPEND possible_allocators BoostPool STL) - set(preferred_allocator STL) -endif() -if(GOOGLE_PERFTOOLS_FOUND) - list(APPEND possible_allocators tcmalloc) -endif() - -# Check if current allocator choice is valid and set cache option -list(FIND possible_allocators "${GTSAM_DEFAULT_ALLOCATOR}" allocator_valid) -if(allocator_valid EQUAL -1) - set(GTSAM_DEFAULT_ALLOCATOR ${preferred_allocator} CACHE STRING "Default allocator" FORCE) -else() - set(GTSAM_DEFAULT_ALLOCATOR ${preferred_allocator} CACHE STRING "Default allocator") -endif() -set_property(CACHE GTSAM_DEFAULT_ALLOCATOR PROPERTY STRINGS ${possible_allocators}) -mark_as_advanced(GTSAM_DEFAULT_ALLOCATOR) - -# Define compile flags depending on allocator -if("${GTSAM_DEFAULT_ALLOCATOR}" STREQUAL "BoostPool") - set(GTSAM_ALLOCATOR_BOOSTPOOL 1) -elseif("${GTSAM_DEFAULT_ALLOCATOR}" STREQUAL "STL") - set(GTSAM_ALLOCATOR_STL 1) -elseif("${GTSAM_DEFAULT_ALLOCATOR}" STREQUAL "TBB") - set(GTSAM_ALLOCATOR_TBB 1) -elseif("${GTSAM_DEFAULT_ALLOCATOR}" STREQUAL "tcmalloc") - set(GTSAM_ALLOCATOR_STL 1) # tcmalloc replaces malloc, so to use it we use the STL allocator - list(APPEND GTSAM_ADDITIONAL_LIBRARIES "tcmalloc") -endif() - -if(MSVC) - list_append_cache(GTSAM_COMPILE_DEFINITIONS_PRIVATE _CRT_SECURE_NO_WARNINGS _SCL_SECURE_NO_WARNINGS) - list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE /wd4251 /wd4275 /wd4251 /wd4661 /wd4344 /wd4503) # Disable non-DLL-exported base class and other warnings - list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE /bigobj) # Allow large object files for template-based code -endif() - -# GCC 4.8+ complains about local typedefs which we use for shared_ptr etc. -if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") - if (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 4.8) - list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE -Wno-unused-local-typedefs) - endif() -endif() - -# As of XCode 7, clang also complains about this -if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang") - if (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 7.0) - list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE -Wno-unused-local-typedefs) - endif() -endif() - -if(GTSAM_ENABLE_CONSISTENCY_CHECKS) - # This should be made PUBLIC if GTSAM_EXTRA_CONSISTENCY_CHECKS is someday used in a public .h - list_append_cache(GTSAM_COMPILE_DEFINITIONS_PRIVATE GTSAM_EXTRA_CONSISTENCY_CHECKS) -endif() +include(cmake/HandleGlobalBuildFlags.cmake) # Build flags ############################################################################### # Add components @@ -477,7 +92,6 @@ endif() GtsamMakeConfigFile(GTSAM "${CMAKE_CURRENT_SOURCE_DIR}/gtsam_extra.cmake.in") export(TARGETS ${GTSAM_EXPORTED_TARGETS} FILE GTSAM-exports.cmake) - # Check for doxygen availability - optional dependency find_package(Doxygen) @@ -489,146 +103,11 @@ endif() # CMake Tools add_subdirectory(cmake) - -############################################################################### -# Set up CPack -set(CPACK_PACKAGE_DESCRIPTION_SUMMARY "GTSAM") -set(CPACK_PACKAGE_VENDOR "Frank Dellaert, Georgia Institute of Technology") -set(CPACK_PACKAGE_CONTACT "Frank Dellaert, dellaert@cc.gatech.edu") -set(CPACK_PACKAGE_DESCRIPTION_FILE "${CMAKE_CURRENT_SOURCE_DIR}/README.md") -set(CPACK_RESOURCE_FILE_LICENSE "${CMAKE_CURRENT_SOURCE_DIR}/LICENSE") -set(CPACK_PACKAGE_VERSION_MAJOR ${GTSAM_VERSION_MAJOR}) -set(CPACK_PACKAGE_VERSION_MINOR ${GTSAM_VERSION_MINOR}) -set(CPACK_PACKAGE_VERSION_PATCH ${GTSAM_VERSION_PATCH}) -set(CPACK_PACKAGE_INSTALL_DIRECTORY "CMake ${CMake_VERSION_MAJOR}.${CMake_VERSION_MINOR}") -#set(CPACK_INSTALLED_DIRECTORIES "doc;.") # Include doc directory -#set(CPACK_INSTALLED_DIRECTORIES ".") # FIXME: throws error -set(CPACK_SOURCE_IGNORE_FILES "/build*;/\\\\.;/makestats.sh$") -set(CPACK_SOURCE_IGNORE_FILES "${CPACK_SOURCE_IGNORE_FILES}" "/gtsam_unstable/") -set(CPACK_SOURCE_IGNORE_FILES "${CPACK_SOURCE_IGNORE_FILES}" "/package_scripts/") -set(CPACK_SOURCE_PACKAGE_FILE_NAME "gtsam-${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}") -#set(CPACK_SOURCE_PACKAGE_FILE_NAME "gtsam-aspn${GTSAM_VERSION_PATCH}") # Used for creating ASPN tarballs - -# Deb-package specific cpack -set(CPACK_DEBIAN_PACKAGE_NAME "libgtsam-dev") -set(CPACK_DEBIAN_PACKAGE_DEPENDS "libboost-dev (>= 1.58)") #Example: "libc6 (>= 2.3.1-6), libgcc1 (>= 1:3.4.2-12)") - - -############################################################################### # Print configuration variables -message(STATUS "===============================================================") -message(STATUS "================ Configuration Options ======================") -print_config("CMAKE_CXX_COMPILER_ID type" "${CMAKE_CXX_COMPILER_ID}") -print_config("CMAKE_CXX_COMPILER_VERSION" "${CMAKE_CXX_COMPILER_VERSION}") -print_config("CMake version" "${CMAKE_VERSION}") -print_config("CMake generator" "${CMAKE_GENERATOR}") -print_config("CMake build tool" "${CMAKE_BUILD_TOOL}") -message(STATUS "Build flags ") -print_enabled_config(${GTSAM_BUILD_TESTS} "Build Tests") -print_enabled_config(${GTSAM_BUILD_EXAMPLES_ALWAYS} "Build examples with 'make all'") -print_enabled_config(${GTSAM_BUILD_TIMING_ALWAYS} "Build timing scripts with 'make all'") -if (DOXYGEN_FOUND) - print_enabled_config(${GTSAM_BUILD_DOCS} "Build Docs") -endif() -print_enabled_config(${BUILD_SHARED_LIBS} "Build shared GTSAM libraries") -print_enabled_config(${GTSAM_BUILD_TYPE_POSTFIXES} "Put build type in library name") -if(GTSAM_UNSTABLE_AVAILABLE) - print_enabled_config(${GTSAM_BUILD_UNSTABLE} "Build libgtsam_unstable ") - print_enabled_config(${GTSAM_UNSTABLE_BUILD_PYTHON} "Build GTSAM unstable Python ") - print_enabled_config(${GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX} "Build MATLAB Toolbox for unstable") -endif() - -if(NOT MSVC AND NOT XCODE_VERSION) - print_enabled_config(${GTSAM_BUILD_WITH_MARCH_NATIVE} "Build for native architecture ") - print_config("Build type" "${CMAKE_BUILD_TYPE}") - print_config("C compilation flags" "${CMAKE_C_FLAGS} ${CMAKE_C_FLAGS_${CMAKE_BUILD_TYPE_UPPER}}") - print_config("C++ compilation flags" "${CMAKE_CXX_FLAGS} ${CMAKE_CXX_FLAGS_${CMAKE_BUILD_TYPE_UPPER}}") -endif() - -print_build_options_for_target(gtsam) - -print_config("Use System Eigen" "${GTSAM_USE_SYSTEM_EIGEN} (Using version: ${GTSAM_EIGEN_VERSION})") - -if(GTSAM_USE_TBB) - print_config("Use Intel TBB" "Yes (Version: ${TBB_VERSION})") -elseif(TBB_FOUND) - print_config("Use Intel TBB" "TBB (Version: ${TBB_VERSION}) found but GTSAM_WITH_TBB is disabled") -else() - print_config("Use Intel TBB" "TBB not found") -endif() -if(GTSAM_USE_EIGEN_MKL) - print_config("Eigen will use MKL" "Yes") -elseif(MKL_FOUND) - print_config("Eigen will use MKL" "MKL found but GTSAM_WITH_EIGEN_MKL is disabled") -else() - print_config("Eigen will use MKL" "MKL not found") -endif() -if(GTSAM_USE_EIGEN_MKL_OPENMP) - print_config("Eigen will use MKL and OpenMP" "Yes") -elseif(OPENMP_FOUND AND NOT GTSAM_WITH_EIGEN_MKL) - print_config("Eigen will use MKL and OpenMP" "OpenMP found but GTSAM_WITH_EIGEN_MKL is disabled") -elseif(OPENMP_FOUND AND NOT MKL_FOUND) - print_config("Eigen will use MKL and OpenMP" "OpenMP found but MKL not found") -elseif(OPENMP_FOUND) - print_config("Eigen will use MKL and OpenMP" "OpenMP found but GTSAM_WITH_EIGEN_MKL_OPENMP is disabled") -else() - print_config("Eigen will use MKL and OpenMP" "OpenMP not found") -endif() -print_config("Default allocator" "${GTSAM_DEFAULT_ALLOCATOR}") - -if(GTSAM_THROW_CHEIRALITY_EXCEPTION) - print_config("Cheirality exceptions enabled" "YES") -else() - print_config("Cheirality exceptions enabled" "NO") -endif() - -if(NOT MSVC AND NOT XCODE_VERSION) - if(CCACHE_FOUND AND GTSAM_BUILD_WITH_CCACHE) - print_config("Build with ccache" "Yes") - elseif(CCACHE_FOUND) - print_config("Build with ccache" "ccache found but GTSAM_BUILD_WITH_CCACHE is disabled") - else() - print_config("Build with ccache" "No") - endif() -endif() - -message(STATUS "Packaging flags") -print_config("CPack Source Generator" "${CPACK_SOURCE_GENERATOR}") -print_config("CPack Generator" "${CPACK_GENERATOR}") - -message(STATUS "GTSAM flags ") -print_enabled_config(${GTSAM_USE_QUATERNIONS} "Quaternions as default Rot3 ") -print_enabled_config(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency checking ") -print_enabled_config(${GTSAM_ROT3_EXPMAP} "Rot3 retract is full ExpMap ") -print_enabled_config(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ") -print_enabled_config(${GTSAM_ALLOW_DEPRECATED_SINCE_V41} "Allow features deprecated in GTSAM 4.1") -print_enabled_config(${GTSAM_SUPPORT_NESTED_DISSECTION} "Metis-based Nested Dissection ") -print_enabled_config(${GTSAM_TANGENT_PREINTEGRATION} "Use tangent-space preintegration") - -message(STATUS "MATLAB toolbox flags") -print_enabled_config(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install MATLAB toolbox ") -if (${GTSAM_INSTALL_MATLAB_TOOLBOX}) - print_config("MATLAB root" "${MATLAB_ROOT}") - print_config("MEX binary" "${MEX_COMMAND}") -endif() -message(STATUS "Python toolbox flags ") -print_enabled_config(${GTSAM_BUILD_PYTHON} "Build Python module with pybind ") -if(GTSAM_BUILD_PYTHON) - print_config("Python version" ${GTSAM_PYTHON_VERSION}) -endif() - -message(STATUS "===============================================================") +include(cmake/HandlePrintConfiguration.cmake) # Print warnings at the end -if(GTSAM_WITH_TBB AND NOT TBB_FOUND) - message(WARNING "TBB 4.4 or newer was not found - this is ok, but note that GTSAM parallelization will be disabled. Set GTSAM_WITH_TBB to 'Off' to avoid this warning.") -endif() -if(GTSAM_WITH_EIGEN_MKL AND NOT MKL_FOUND) - message(WARNING "MKL was not found - this is ok, but note that MKL will be disabled. Set GTSAM_WITH_EIGEN_MKL to 'Off' to disable this warning. See INSTALL.md for notes on performance.") -endif() -if(GTSAM_WITH_EIGEN_MKL_OPENMP AND NOT OPENMP_FOUND AND MKL_FOUND) - message(WARNING "Your compiler does not support OpenMP. Set GTSAM_WITH_EIGEN_MKL_OPENMP to 'Off' to avoid this warning. See INSTALL.md for notes on performance.") -endif() +include(cmake/HandleFinalChecks.cmake) # Include CPack *after* all flags include(CPack) diff --git a/INSTALL.md b/INSTALL.md index cf66766a1..3dbc3a850 100644 --- a/INSTALL.md +++ b/INSTALL.md @@ -173,7 +173,7 @@ NOTE: If _GLIBCXX_DEBUG is used to compile gtsam, anything that links against g Intel has a guide for installing MKL on Linux through APT repositories at . After following the instructions, add the following to your `~/.bashrc` (and afterwards, open a new terminal before compiling GTSAM): -`LD_PRELOAD` need only be set if you are building the cython wrapper to use GTSAM from python. +`LD_PRELOAD` need only be set if you are building the python wrapper to use GTSAM from python. ```sh source /opt/intel/mkl/bin/mklvars.sh intel64 export LD_PRELOAD="$LD_PRELOAD:/opt/intel/mkl/lib/intel64/libmkl_core.so:/opt/intel/mkl/lib/intel64/libmkl_sequential.so" @@ -190,6 +190,6 @@ Failing to specify `LD_PRELOAD` may lead to errors such as: `ImportError: /opt/intel/mkl/lib/intel64/libmkl_vml_avx2.so: undefined symbol: mkl_serv_getenv` or `Intel MKL FATAL ERROR: Cannot load libmkl_avx2.so or libmkl_def.so.` -when importing GTSAM using the cython wrapper in python. +when importing GTSAM using the python wrapper. diff --git a/cmake/CMakeLists.txt b/cmake/CMakeLists.txt index 9d9ecd48b..451ca38a4 100644 --- a/cmake/CMakeLists.txt +++ b/cmake/CMakeLists.txt @@ -19,7 +19,6 @@ install(FILES GtsamMatlabWrap.cmake GtsamTesting.cmake GtsamPrinting.cmake - FindCython.cmake FindNumPy.cmake README.html DESTINATION "${SCRIPT_INSTALL_DIR}/GTSAMCMakeTools") diff --git a/cmake/FindCython.cmake b/cmake/FindCython.cmake deleted file mode 100644 index e5a32c30d..000000000 --- a/cmake/FindCython.cmake +++ /dev/null @@ -1,81 +0,0 @@ -# Modifed from: https://github.com/nest/nest-simulator/blob/master/cmake/FindCython.cmake -# -# Find the Cython compiler. -# -# This code sets the following variables: -# -# CYTHON_FOUND -# CYTHON_PATH -# CYTHON_EXECUTABLE -# CYTHON_VERSION -# -# See also UseCython.cmake - -#============================================================================= -# Copyright 2011 Kitware, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -#============================================================================= - -# Use the Cython executable that lives next to the Python executable -# if it is a local installation. -if(GTSAM_PYTHON_VERSION STREQUAL "Default") - find_package(PythonInterp) -else() - find_package(PythonInterp ${GTSAM_PYTHON_VERSION} EXACT) -endif() - -if ( PYTHONINTERP_FOUND ) - execute_process( COMMAND "${PYTHON_EXECUTABLE}" "-c" - "import Cython; print(Cython.__path__[0])" - RESULT_VARIABLE RESULT - OUTPUT_VARIABLE CYTHON_PATH - OUTPUT_STRIP_TRAILING_WHITESPACE - ) -endif () - -# RESULT=0 means ok -if ( NOT RESULT ) - get_filename_component( _python_path ${PYTHON_EXECUTABLE} PATH ) - find_program( CYTHON_EXECUTABLE - NAMES cython cython.bat cython3 - HINTS ${_python_path} - ) -endif () - -# RESULT=0 means ok -if ( NOT RESULT ) - execute_process( COMMAND "${PYTHON_EXECUTABLE}" "-c" - "import Cython; print(Cython.__version__)" - RESULT_VARIABLE RESULT - OUTPUT_VARIABLE CYTHON_VAR_OUTPUT - ERROR_VARIABLE CYTHON_VAR_OUTPUT - OUTPUT_STRIP_TRAILING_WHITESPACE - ) - if ( RESULT EQUAL 0 ) - string( REGEX REPLACE ".* ([0-9]+\\.[0-9]+(\\.[0-9]+)?).*" "\\1" - CYTHON_VERSION "${CYTHON_VAR_OUTPUT}" ) - endif () -endif () - -include( FindPackageHandleStandardArgs ) -find_package_handle_standard_args( Cython - FOUND_VAR - CYTHON_FOUND - REQUIRED_VARS - CYTHON_PATH - CYTHON_EXECUTABLE - VERSION_VAR - CYTHON_VERSION - ) - diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index 53dacd3f5..3155161be 100644 --- a/cmake/GtsamBuildTypes.cmake +++ b/cmake/GtsamBuildTypes.cmake @@ -1,3 +1,5 @@ +include(CheckCXXCompilerFlag) # for check_cxx_compiler_flag() + # Set cmake policy to recognize the AppleClang compiler # independently from the Clang compiler. if(POLICY CMP0025) @@ -105,11 +107,14 @@ if(MSVC) else() # Common to all configurations, next for each configuration: - if ( - ((CMAKE_CXX_COMPILER_ID MATCHES "Clang") AND (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 12.0.0)) OR - (CMAKE_CXX_COMPILER_ID MATCHES "GNU") - ) - set(flag_override_ -Wsuggest-override) # -Werror=suggest-override: Add again someday + if (NOT MSVC) + check_cxx_compiler_flag(-Wsuggest-override COMPILER_HAS_WSUGGEST_OVERRIDE) + check_cxx_compiler_flag(-Wmissing COMPILER_HAS_WMISSING_OVERRIDE) + if (COMPILER_HAS_WSUGGEST_OVERRIDE) + set(flag_override_ -Wsuggest-override) # -Werror=suggest-override: Add again someday + elseif(COMPILER_HAS_WMISSING_OVERRIDE) + set(flag_override_ -Wmissing-override) # -Werror=missing-override: Add again someday + endif() endif() set(GTSAM_COMPILE_OPTIONS_PRIVATE_COMMON @@ -263,3 +268,17 @@ function(gtsam_apply_build_flags target_name_) target_compile_options(${target_name_} PRIVATE ${GTSAM_COMPILE_OPTIONS_PRIVATE}) endfunction(gtsam_apply_build_flags) + + +if(NOT MSVC AND NOT XCODE_VERSION) + # Set the build type to upper case for downstream use + string(TOUPPER "${CMAKE_BUILD_TYPE}" CMAKE_BUILD_TYPE_UPPER) + + # Set the GTSAM_BUILD_TAG variable. + # If build type is Release, set to blank (""), else set to the build type. + if(${CMAKE_BUILD_TYPE_UPPER} STREQUAL "RELEASE") + set(GTSAM_BUILD_TAG "") # Don't create release mode tag on installed directory + else() + set(GTSAM_BUILD_TAG "${CMAKE_BUILD_TYPE}") + endif() +endif() diff --git a/cmake/HandleAllocators.cmake b/cmake/HandleAllocators.cmake new file mode 100644 index 000000000..63411b17b --- /dev/null +++ b/cmake/HandleAllocators.cmake @@ -0,0 +1,34 @@ +# Build list of possible allocators +set(possible_allocators "") +if(GTSAM_USE_TBB) + list(APPEND possible_allocators TBB) + set(preferred_allocator TBB) +else() + list(APPEND possible_allocators BoostPool STL) + set(preferred_allocator STL) +endif() +if(GOOGLE_PERFTOOLS_FOUND) + list(APPEND possible_allocators tcmalloc) +endif() + +# Check if current allocator choice is valid and set cache option +list(FIND possible_allocators "${GTSAM_DEFAULT_ALLOCATOR}" allocator_valid) +if(allocator_valid EQUAL -1) + set(GTSAM_DEFAULT_ALLOCATOR ${preferred_allocator} CACHE STRING "Default allocator" FORCE) +else() + set(GTSAM_DEFAULT_ALLOCATOR ${preferred_allocator} CACHE STRING "Default allocator") +endif() +set_property(CACHE GTSAM_DEFAULT_ALLOCATOR PROPERTY STRINGS ${possible_allocators}) +mark_as_advanced(GTSAM_DEFAULT_ALLOCATOR) + +# Define compile flags depending on allocator +if("${GTSAM_DEFAULT_ALLOCATOR}" STREQUAL "BoostPool") + set(GTSAM_ALLOCATOR_BOOSTPOOL 1) +elseif("${GTSAM_DEFAULT_ALLOCATOR}" STREQUAL "STL") + set(GTSAM_ALLOCATOR_STL 1) +elseif("${GTSAM_DEFAULT_ALLOCATOR}" STREQUAL "TBB") + set(GTSAM_ALLOCATOR_TBB 1) +elseif("${GTSAM_DEFAULT_ALLOCATOR}" STREQUAL "tcmalloc") + set(GTSAM_ALLOCATOR_STL 1) # tcmalloc replaces malloc, so to use it we use the STL allocator + list(APPEND GTSAM_ADDITIONAL_LIBRARIES "tcmalloc") +endif() diff --git a/cmake/HandleBoost.cmake b/cmake/HandleBoost.cmake new file mode 100644 index 000000000..e73c2237d --- /dev/null +++ b/cmake/HandleBoost.cmake @@ -0,0 +1,56 @@ +############################################################################### +# Find boost + +# To change the path for boost, you will need to set: +# BOOST_ROOT: path to install prefix for boost +# Boost_NO_SYSTEM_PATHS: set to true to keep the find script from ignoring BOOST_ROOT + +if(MSVC) + # By default, boost only builds static libraries on windows + set(Boost_USE_STATIC_LIBS ON) # only find static libs + # If we ever reset above on windows and, ... + # If we use Boost shared libs, disable auto linking. + # Some libraries, at least Boost Program Options, rely on this to export DLL symbols. + if(NOT Boost_USE_STATIC_LIBS) + list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC BOOST_ALL_NO_LIB BOOST_ALL_DYN_LINK) + endif() + # Virtual memory range for PCH exceeded on VS2015 + if(MSVC_VERSION LESS 1910) # older than VS2017 + list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE -Zm295) + endif() +endif() + + +# Store these in variables so they are automatically replicated in GTSAMConfig.cmake and such. +set(BOOST_FIND_MINIMUM_VERSION 1.58) +set(BOOST_FIND_MINIMUM_COMPONENTS serialization system filesystem thread program_options date_time timer chrono regex) + +find_package(Boost ${BOOST_FIND_MINIMUM_VERSION} COMPONENTS ${BOOST_FIND_MINIMUM_COMPONENTS}) + +# Required components +if(NOT Boost_SERIALIZATION_LIBRARY OR NOT Boost_SYSTEM_LIBRARY OR NOT Boost_FILESYSTEM_LIBRARY OR + NOT Boost_THREAD_LIBRARY OR NOT Boost_DATE_TIME_LIBRARY) + message(FATAL_ERROR "Missing required Boost components >= v1.58, please install/upgrade Boost or configure your search paths.") +endif() + +option(GTSAM_DISABLE_NEW_TIMERS "Disables using Boost.chrono for timing" OFF) +# Allow for not using the timer libraries on boost < 1.48 (GTSAM timing code falls back to old timer library) +set(GTSAM_BOOST_LIBRARIES + Boost::serialization + Boost::system + Boost::filesystem + Boost::thread + Boost::date_time + Boost::regex +) +if (GTSAM_DISABLE_NEW_TIMERS) + message("WARNING: GTSAM timing instrumentation manually disabled") + list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC DGTSAM_DISABLE_NEW_TIMERS) +else() + if(Boost_TIMER_LIBRARY) + list(APPEND GTSAM_BOOST_LIBRARIES Boost::timer Boost::chrono) + else() + list(APPEND GTSAM_BOOST_LIBRARIES rt) # When using the header-only boost timer library, need -lrt + message("WARNING: GTSAM timing instrumentation will use the older, less accurate, Boost timer library because boost older than 1.48 was found.") + endif() +endif() diff --git a/cmake/HandleCCache.cmake b/cmake/HandleCCache.cmake new file mode 100644 index 000000000..9eabb1905 --- /dev/null +++ b/cmake/HandleCCache.cmake @@ -0,0 +1,14 @@ +############################################################################### +# Support ccache, if installed +if(NOT MSVC AND NOT XCODE_VERSION) + find_program(CCACHE_FOUND ccache) + if(CCACHE_FOUND) + if(GTSAM_BUILD_WITH_CCACHE) + set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE ccache) + set_property(GLOBAL PROPERTY RULE_LAUNCH_LINK ccache) + else() + set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE "") + set_property(GLOBAL PROPERTY RULE_LAUNCH_LINK "") + endif() + endif(CCACHE_FOUND) +endif() diff --git a/cmake/HandleCPack.cmake b/cmake/HandleCPack.cmake new file mode 100644 index 000000000..1c32433a4 --- /dev/null +++ b/cmake/HandleCPack.cmake @@ -0,0 +1,28 @@ +#JLBC: is all this actually used by someone? could it be removed? + +# Flags for choosing default packaging tools +set(CPACK_SOURCE_GENERATOR "TGZ" CACHE STRING "CPack Default Source Generator") +set(CPACK_GENERATOR "TGZ" CACHE STRING "CPack Default Binary Generator") + +############################################################################### +# Set up CPack +set(CPACK_PACKAGE_DESCRIPTION_SUMMARY "GTSAM") +set(CPACK_PACKAGE_VENDOR "Frank Dellaert, Georgia Institute of Technology") +set(CPACK_PACKAGE_CONTACT "Frank Dellaert, dellaert@cc.gatech.edu") +set(CPACK_PACKAGE_DESCRIPTION_FILE "${CMAKE_CURRENT_SOURCE_DIR}/README.md") +set(CPACK_RESOURCE_FILE_LICENSE "${CMAKE_CURRENT_SOURCE_DIR}/LICENSE") +set(CPACK_PACKAGE_VERSION_MAJOR ${GTSAM_VERSION_MAJOR}) +set(CPACK_PACKAGE_VERSION_MINOR ${GTSAM_VERSION_MINOR}) +set(CPACK_PACKAGE_VERSION_PATCH ${GTSAM_VERSION_PATCH}) +set(CPACK_PACKAGE_INSTALL_DIRECTORY "CMake ${CMake_VERSION_MAJOR}.${CMake_VERSION_MINOR}") +#set(CPACK_INSTALLED_DIRECTORIES "doc;.") # Include doc directory +#set(CPACK_INSTALLED_DIRECTORIES ".") # FIXME: throws error +set(CPACK_SOURCE_IGNORE_FILES "/build*;/\\\\.;/makestats.sh$") +set(CPACK_SOURCE_IGNORE_FILES "${CPACK_SOURCE_IGNORE_FILES}" "/gtsam_unstable/") +set(CPACK_SOURCE_IGNORE_FILES "${CPACK_SOURCE_IGNORE_FILES}" "/package_scripts/") +set(CPACK_SOURCE_PACKAGE_FILE_NAME "gtsam-${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}") +#set(CPACK_SOURCE_PACKAGE_FILE_NAME "gtsam-aspn${GTSAM_VERSION_PATCH}") # Used for creating ASPN tarballs + +# Deb-package specific cpack +set(CPACK_DEBIAN_PACKAGE_NAME "libgtsam-dev") +set(CPACK_DEBIAN_PACKAGE_DEPENDS "libboost-dev (>= 1.58)") #Example: "libc6 (>= 2.3.1-6), libgcc1 (>= 1:3.4.2-12)") diff --git a/cmake/HandleEigen.cmake b/cmake/HandleEigen.cmake new file mode 100644 index 000000000..fda441907 --- /dev/null +++ b/cmake/HandleEigen.cmake @@ -0,0 +1,77 @@ +############################################################################### +# Option for using system Eigen or GTSAM-bundled Eigen + +option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', use the one bundled with GTSAM" OFF) + +if(NOT GTSAM_USE_SYSTEM_EIGEN) + # This option only makes sense if using the embedded copy of Eigen, it is + # used to decide whether to *install* the "unsupported" module: + option(GTSAM_WITH_EIGEN_UNSUPPORTED "Install Eigen's unsupported modules" OFF) +endif() + +# Switch for using system Eigen or GTSAM-bundled Eigen +if(GTSAM_USE_SYSTEM_EIGEN) + find_package(Eigen3 REQUIRED) + + # Use generic Eigen include paths e.g. + set(GTSAM_EIGEN_INCLUDE_FOR_INSTALL "${EIGEN3_INCLUDE_DIR}") + + # check if MKL is also enabled - can have one or the other, but not both! + # Note: Eigen >= v3.2.5 includes our patches + if(EIGEN_USE_MKL_ALL AND (EIGEN3_VERSION VERSION_LESS 3.2.5)) + message(FATAL_ERROR "MKL requires at least Eigen 3.2.5, and your system appears to have an older version. Disable GTSAM_USE_SYSTEM_EIGEN to use GTSAM's copy of Eigen, or disable GTSAM_WITH_EIGEN_MKL") + endif() + + # Check for Eigen version which doesn't work with MKL + # See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1527 for details. + if(EIGEN_USE_MKL_ALL AND (EIGEN3_VERSION VERSION_EQUAL 3.3.4)) + message(FATAL_ERROR "MKL does not work with Eigen 3.3.4 because of a bug in Eigen. See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1527. Disable GTSAM_USE_SYSTEM_EIGEN to use GTSAM's copy of Eigen, disable GTSAM_WITH_EIGEN_MKL, or upgrade/patch your installation of Eigen.") + endif() + + # The actual include directory (for BUILD cmake target interface): + set(GTSAM_EIGEN_INCLUDE_FOR_BUILD "${EIGEN3_INCLUDE_DIR}") +else() + # Use bundled Eigen include path. + # Clear any variables set by FindEigen3 + if(EIGEN3_INCLUDE_DIR) + set(EIGEN3_INCLUDE_DIR NOTFOUND CACHE STRING "" FORCE) + endif() + + # set full path to be used by external projects + # this will be added to GTSAM_INCLUDE_DIR by gtsam_extra.cmake.in + set(GTSAM_EIGEN_INCLUDE_FOR_INSTALL "include/gtsam/3rdparty/Eigen/") + + # The actual include directory (for BUILD cmake target interface): + set(GTSAM_EIGEN_INCLUDE_FOR_BUILD "${CMAKE_SOURCE_DIR}/gtsam/3rdparty/Eigen/") +endif() + +# Detect Eigen version: +set(EIGEN_VER_H "${GTSAM_EIGEN_INCLUDE_FOR_BUILD}/Eigen/src/Core/util/Macros.h") +if (EXISTS ${EIGEN_VER_H}) + file(READ "${EIGEN_VER_H}" STR_EIGEN_VERSION) + + # Extract the Eigen version from the Macros.h file, lines "#define EIGEN_WORLD_VERSION XX", etc... + + string(REGEX MATCH "EIGEN_WORLD_VERSION[ ]+[0-9]+" GTSAM_EIGEN_VERSION_WORLD "${STR_EIGEN_VERSION}") + string(REGEX MATCH "[0-9]+" GTSAM_EIGEN_VERSION_WORLD "${GTSAM_EIGEN_VERSION_WORLD}") + + string(REGEX MATCH "EIGEN_MAJOR_VERSION[ ]+[0-9]+" GTSAM_EIGEN_VERSION_MAJOR "${STR_EIGEN_VERSION}") + string(REGEX MATCH "[0-9]+" GTSAM_EIGEN_VERSION_MAJOR "${GTSAM_EIGEN_VERSION_MAJOR}") + + string(REGEX MATCH "EIGEN_MINOR_VERSION[ ]+[0-9]+" GTSAM_EIGEN_VERSION_MINOR "${STR_EIGEN_VERSION}") + string(REGEX MATCH "[0-9]+" GTSAM_EIGEN_VERSION_MINOR "${GTSAM_EIGEN_VERSION_MINOR}") + + set(GTSAM_EIGEN_VERSION "${GTSAM_EIGEN_VERSION_WORLD}.${GTSAM_EIGEN_VERSION_MAJOR}.${GTSAM_EIGEN_VERSION_MINOR}") + + message(STATUS "Found Eigen version: ${GTSAM_EIGEN_VERSION}") +else() + message(WARNING "Cannot determine Eigen version, missing file: `${EIGEN_VER_H}`") +endif () + +if (MSVC) + if (BUILD_SHARED_LIBS) + # mute eigen static assert to avoid errors in shared lib + list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC EIGEN_NO_STATIC_ASSERT) + endif() + list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE "/wd4244") # Disable loss of precision which is thrown all over our Eigen +endif() diff --git a/cmake/HandleFinalChecks.cmake b/cmake/HandleFinalChecks.cmake new file mode 100644 index 000000000..f91fc7fdb --- /dev/null +++ b/cmake/HandleFinalChecks.cmake @@ -0,0 +1,10 @@ +# Print warnings at the end +if(GTSAM_WITH_TBB AND NOT TBB_FOUND) + message(WARNING "TBB 4.4 or newer was not found - this is ok, but note that GTSAM parallelization will be disabled. Set GTSAM_WITH_TBB to 'Off' to avoid this warning.") +endif() +if(GTSAM_WITH_EIGEN_MKL AND NOT MKL_FOUND) + message(WARNING "MKL was not found - this is ok, but note that MKL will be disabled. Set GTSAM_WITH_EIGEN_MKL to 'Off' to disable this warning. See INSTALL.md for notes on performance.") +endif() +if(GTSAM_WITH_EIGEN_MKL_OPENMP AND NOT OPENMP_FOUND AND MKL_FOUND) + message(WARNING "Your compiler does not support OpenMP. Set GTSAM_WITH_EIGEN_MKL_OPENMP to 'Off' to avoid this warning. See INSTALL.md for notes on performance.") +endif() diff --git a/cmake/HandleGeneralOptions.cmake b/cmake/HandleGeneralOptions.cmake new file mode 100644 index 000000000..27d73bd86 --- /dev/null +++ b/cmake/HandleGeneralOptions.cmake @@ -0,0 +1,46 @@ +############################################################################### +# Set up options + +# See whether gtsam_unstable is available (it will be present only if we're using a git checkout) +if(EXISTS "${PROJECT_SOURCE_DIR}/gtsam_unstable" AND IS_DIRECTORY "${PROJECT_SOURCE_DIR}/gtsam_unstable") + set(GTSAM_UNSTABLE_AVAILABLE 1) +else() + set(GTSAM_UNSTABLE_AVAILABLE 0) +endif() + +# Configurable Options +if(GTSAM_UNSTABLE_AVAILABLE) + option(GTSAM_BUILD_UNSTABLE "Enable/Disable libgtsam_unstable" ON) + option(GTSAM_UNSTABLE_BUILD_PYTHON "Enable/Disable Python wrapper for libgtsam_unstable" ON) + option(GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX "Enable/Disable MATLAB wrapper for libgtsam_unstable" OFF) +endif() +option(BUILD_SHARED_LIBS "Build shared gtsam library, instead of static" ON) +option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF) +option(GTSAM_POSE3_EXPMAP "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." ON) +option(GTSAM_ROT3_EXPMAP "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." ON) +option(GTSAM_ENABLE_CONSISTENCY_CHECKS "Enable/Disable expensive consistency checks" OFF) +option(GTSAM_WITH_TBB "Use Intel Threaded Building Blocks (TBB) if available" ON) +option(GTSAM_WITH_EIGEN_MKL "Eigen will use Intel MKL if available" OFF) +option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" OFF) +option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON) +option(GTSAM_BUILD_PYTHON "Enable/Disable building & installation of Python module with pybind11" OFF) +option(GTSAM_ALLOW_DEPRECATED_SINCE_V41 "Allow use of methods/functions deprecated in GTSAM 4.1" ON) +option(GTSAM_SUPPORT_NESTED_DISSECTION "Support Metis-based nested dissection" ON) +option(GTSAM_TANGENT_PREINTEGRATION "Use new ImuFactor with integration on tangent space" ON) +if(NOT MSVC AND NOT XCODE_VERSION) + option(GTSAM_BUILD_WITH_CCACHE "Use ccache compiler cache" ON) +endif() + +# Options relating to MATLAB wrapper +# TODO: Check for matlab mex binary before handling building of binaries +option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" OFF) +set(GTSAM_PYTHON_VERSION "Default" CACHE STRING "The version of Python to build the wrappers against.") + +# Check / set dependent variables for MATLAB wrapper +if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_BUILD_TYPE_POSTFIXES) + set(CURRENT_POSTFIX ${CMAKE_${CMAKE_BUILD_TYPE_UPPER}_POSTFIX}) +endif() + +if(GTSAM_INSTALL_MATLAB_TOOLBOX AND NOT BUILD_SHARED_LIBS) + message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and BUILD_SHARED_LIBS=OFF. The MATLAB wrapper cannot be compiled with a static GTSAM library because mex modules are themselves shared libraries. If you want a self-contained mex module, enable GTSAM_MEX_BUILD_STATIC_MODULE instead of BUILD_SHARED_LIBS=OFF.") +endif() diff --git a/cmake/HandleGlobalBuildFlags.cmake b/cmake/HandleGlobalBuildFlags.cmake new file mode 100644 index 000000000..f33e12b94 --- /dev/null +++ b/cmake/HandleGlobalBuildFlags.cmake @@ -0,0 +1,52 @@ +# JLBC: These should ideally be ported to "modern cmake" via target properties. +# + +if (CMAKE_GENERATOR STREQUAL "Ninja" AND + ((CMAKE_CXX_COMPILER_ID STREQUAL "GNU" AND NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 4.9) OR + (CMAKE_CXX_COMPILER_ID STREQUAL "Clang" AND NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 3.5))) + # Force colored warnings in Ninja's output, if the compiler has -fdiagnostics-color support. + # Rationale in https://github.com/ninja-build/ninja/issues/814 + add_compile_options(-fdiagnostics-color=always) +endif() + + +# If building DLLs in MSVC, we need to avoid EIGEN_STATIC_ASSERT() +# or explicit instantiation will generate build errors. +# See: https://bitbucket.org/gtborg/gtsam/issues/417/fail-to-build-on-msvc-2017 +# +if(MSVC AND BUILD_SHARED_LIBS) + list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC EIGEN_NO_STATIC_ASSERT) +endif() + +if (APPLE AND BUILD_SHARED_LIBS) + # Set the default install directory on macOS + set(CMAKE_INSTALL_NAME_DIR "${CMAKE_INSTALL_PREFIX}/lib") +endif() + +############################################################################### +# Global compile options + +if(MSVC) + list_append_cache(GTSAM_COMPILE_DEFINITIONS_PRIVATE _CRT_SECURE_NO_WARNINGS _SCL_SECURE_NO_WARNINGS) + list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE /wd4251 /wd4275 /wd4251 /wd4661 /wd4344 /wd4503) # Disable non-DLL-exported base class and other warnings + list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE /bigobj) # Allow large object files for template-based code +endif() + +# GCC 4.8+ complains about local typedefs which we use for shared_ptr etc. +if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") + if (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 4.8) + list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE -Wno-unused-local-typedefs) + endif() +endif() + +# As of XCode 7, clang also complains about this +if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang") + if (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 7.0) + list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE -Wno-unused-local-typedefs) + endif() +endif() + +if(GTSAM_ENABLE_CONSISTENCY_CHECKS) + # This should be made PUBLIC if GTSAM_EXTRA_CONSISTENCY_CHECKS is someday used in a public .h + list_append_cache(GTSAM_COMPILE_DEFINITIONS_PRIVATE GTSAM_EXTRA_CONSISTENCY_CHECKS) +endif() diff --git a/cmake/HandleMKL.cmake b/cmake/HandleMKL.cmake new file mode 100644 index 000000000..5d7ec365b --- /dev/null +++ b/cmake/HandleMKL.cmake @@ -0,0 +1,17 @@ +############################################################################### +# Find MKL +find_package(MKL) + +if(MKL_FOUND AND GTSAM_WITH_EIGEN_MKL) + set(GTSAM_USE_EIGEN_MKL 1) # This will go into config.h + set(EIGEN_USE_MKL_ALL 1) # This will go into config.h - it makes Eigen use MKL + list(APPEND GTSAM_ADDITIONAL_LIBRARIES ${MKL_LIBRARIES}) + + # --no-as-needed is required with gcc according to the MKL link advisor + if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") + set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,--no-as-needed") + endif() +else() + set(GTSAM_USE_EIGEN_MKL 0) + set(EIGEN_USE_MKL_ALL 0) +endif() diff --git a/cmake/HandleOpenMP.cmake b/cmake/HandleOpenMP.cmake new file mode 100644 index 000000000..4f27aa633 --- /dev/null +++ b/cmake/HandleOpenMP.cmake @@ -0,0 +1,11 @@ + +############################################################################### +# Find OpenMP (if we're also using MKL) +find_package(OpenMP) # do this here to generate correct message if disabled + +if(GTSAM_WITH_EIGEN_MKL AND GTSAM_WITH_EIGEN_MKL_OPENMP AND GTSAM_USE_EIGEN_MKL) + if(OPENMP_FOUND AND GTSAM_USE_EIGEN_MKL AND GTSAM_WITH_EIGEN_MKL_OPENMP) + set(GTSAM_USE_EIGEN_MKL_OPENMP 1) # This will go into config.h + list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC ${OpenMP_CXX_FLAGS}) + endif() +endif() diff --git a/cmake/HandlePerfTools.cmake b/cmake/HandlePerfTools.cmake new file mode 100644 index 000000000..499caf80a --- /dev/null +++ b/cmake/HandlePerfTools.cmake @@ -0,0 +1,4 @@ + +############################################################################### +# Find Google perftools +find_package(GooglePerfTools) diff --git a/cmake/HandlePrintConfiguration.cmake b/cmake/HandlePrintConfiguration.cmake new file mode 100644 index 000000000..4ffd00e54 --- /dev/null +++ b/cmake/HandlePrintConfiguration.cmake @@ -0,0 +1,104 @@ +############################################################################### +# Print configuration variables +message(STATUS "===============================================================") +message(STATUS "================ Configuration Options ======================") +print_config("CMAKE_CXX_COMPILER_ID type" "${CMAKE_CXX_COMPILER_ID}") +print_config("CMAKE_CXX_COMPILER_VERSION" "${CMAKE_CXX_COMPILER_VERSION}") +print_config("CMake version" "${CMAKE_VERSION}") +print_config("CMake generator" "${CMAKE_GENERATOR}") +print_config("CMake build tool" "${CMAKE_BUILD_TOOL}") +message(STATUS "Build flags ") +print_enabled_config(${GTSAM_BUILD_TESTS} "Build Tests") +print_enabled_config(${GTSAM_BUILD_EXAMPLES_ALWAYS} "Build examples with 'make all'") +print_enabled_config(${GTSAM_BUILD_TIMING_ALWAYS} "Build timing scripts with 'make all'") +if (DOXYGEN_FOUND) + print_enabled_config(${GTSAM_BUILD_DOCS} "Build Docs") +endif() +print_enabled_config(${BUILD_SHARED_LIBS} "Build shared GTSAM libraries") +print_enabled_config(${GTSAM_BUILD_TYPE_POSTFIXES} "Put build type in library name") +if(GTSAM_UNSTABLE_AVAILABLE) + print_enabled_config(${GTSAM_BUILD_UNSTABLE} "Build libgtsam_unstable ") + print_enabled_config(${GTSAM_UNSTABLE_BUILD_PYTHON} "Build GTSAM unstable Python ") + print_enabled_config(${GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX} "Build MATLAB Toolbox for unstable") +endif() + +if(NOT MSVC AND NOT XCODE_VERSION) + print_enabled_config(${GTSAM_BUILD_WITH_MARCH_NATIVE} "Build for native architecture ") + print_config("Build type" "${CMAKE_BUILD_TYPE}") + print_config("C compilation flags" "${CMAKE_C_FLAGS} ${CMAKE_C_FLAGS_${CMAKE_BUILD_TYPE_UPPER}}") + print_config("C++ compilation flags" "${CMAKE_CXX_FLAGS} ${CMAKE_CXX_FLAGS_${CMAKE_BUILD_TYPE_UPPER}}") +endif() + +print_build_options_for_target(gtsam) + +print_config("Use System Eigen" "${GTSAM_USE_SYSTEM_EIGEN} (Using version: ${GTSAM_EIGEN_VERSION})") + +if(GTSAM_USE_TBB) + print_config("Use Intel TBB" "Yes (Version: ${TBB_VERSION})") +elseif(TBB_FOUND) + print_config("Use Intel TBB" "TBB (Version: ${TBB_VERSION}) found but GTSAM_WITH_TBB is disabled") +else() + print_config("Use Intel TBB" "TBB not found") +endif() +if(GTSAM_USE_EIGEN_MKL) + print_config("Eigen will use MKL" "Yes") +elseif(MKL_FOUND) + print_config("Eigen will use MKL" "MKL found but GTSAM_WITH_EIGEN_MKL is disabled") +else() + print_config("Eigen will use MKL" "MKL not found") +endif() +if(GTSAM_USE_EIGEN_MKL_OPENMP) + print_config("Eigen will use MKL and OpenMP" "Yes") +elseif(OPENMP_FOUND AND NOT GTSAM_WITH_EIGEN_MKL) + print_config("Eigen will use MKL and OpenMP" "OpenMP found but GTSAM_WITH_EIGEN_MKL is disabled") +elseif(OPENMP_FOUND AND NOT MKL_FOUND) + print_config("Eigen will use MKL and OpenMP" "OpenMP found but MKL not found") +elseif(OPENMP_FOUND) + print_config("Eigen will use MKL and OpenMP" "OpenMP found but GTSAM_WITH_EIGEN_MKL_OPENMP is disabled") +else() + print_config("Eigen will use MKL and OpenMP" "OpenMP not found") +endif() +print_config("Default allocator" "${GTSAM_DEFAULT_ALLOCATOR}") + +if(GTSAM_THROW_CHEIRALITY_EXCEPTION) + print_config("Cheirality exceptions enabled" "YES") +else() + print_config("Cheirality exceptions enabled" "NO") +endif() + +if(NOT MSVC AND NOT XCODE_VERSION) + if(CCACHE_FOUND AND GTSAM_BUILD_WITH_CCACHE) + print_config("Build with ccache" "Yes") + elseif(CCACHE_FOUND) + print_config("Build with ccache" "ccache found but GTSAM_BUILD_WITH_CCACHE is disabled") + else() + print_config("Build with ccache" "No") + endif() +endif() + +message(STATUS "Packaging flags") +print_config("CPack Source Generator" "${CPACK_SOURCE_GENERATOR}") +print_config("CPack Generator" "${CPACK_GENERATOR}") + +message(STATUS "GTSAM flags ") +print_enabled_config(${GTSAM_USE_QUATERNIONS} "Quaternions as default Rot3 ") +print_enabled_config(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency checking ") +print_enabled_config(${GTSAM_ROT3_EXPMAP} "Rot3 retract is full ExpMap ") +print_enabled_config(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ") +print_enabled_config(${GTSAM_ALLOW_DEPRECATED_SINCE_V41} "Allow features deprecated in GTSAM 4.1") +print_enabled_config(${GTSAM_SUPPORT_NESTED_DISSECTION} "Metis-based Nested Dissection ") +print_enabled_config(${GTSAM_TANGENT_PREINTEGRATION} "Use tangent-space preintegration") + +message(STATUS "MATLAB toolbox flags") +print_enabled_config(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install MATLAB toolbox ") +if (${GTSAM_INSTALL_MATLAB_TOOLBOX}) + print_config("MATLAB root" "${MATLAB_ROOT}") + print_config("MEX binary" "${MEX_COMMAND}") +endif() +message(STATUS "Python toolbox flags ") +print_enabled_config(${GTSAM_BUILD_PYTHON} "Build Python module with pybind ") +if(GTSAM_BUILD_PYTHON) + print_config("Python version" ${GTSAM_PYTHON_VERSION}) +endif() + +message(STATUS "===============================================================") diff --git a/cmake/HandlePython.cmake b/cmake/HandlePython.cmake new file mode 100644 index 000000000..ac7401906 --- /dev/null +++ b/cmake/HandlePython.cmake @@ -0,0 +1,26 @@ +if(GTSAM_BUILD_PYTHON) + if(${GTSAM_PYTHON_VERSION} STREQUAL "Default") + # Get info about the Python3 interpreter + # https://cmake.org/cmake/help/latest/module/FindPython3.html#module:FindPython3 + find_package(Python3 COMPONENTS Interpreter Development) + + if(NOT ${Python3_FOUND}) + message(FATAL_ERROR "Cannot find Python3 interpreter. Please install Python >= 3.6.") + endif() + + set(GTSAM_PYTHON_VERSION "${Python3_VERSION_MAJOR}.${Python3_VERSION_MINOR}" + CACHE + STRING + "The version of Python to build the wrappers against." + FORCE) + endif() + + if(GTSAM_UNSTABLE_BUILD_PYTHON) + if (NOT GTSAM_BUILD_UNSTABLE) + message(WARNING "GTSAM_UNSTABLE_BUILD_PYTHON requires the unstable module to be enabled.") + set(GTSAM_UNSTABLE_BUILD_PYTHON OFF) + endif() + endif() + + set(GTSAM_PY_INSTALL_PATH "${CMAKE_INSTALL_PREFIX}/python") +endif() diff --git a/cmake/HandleTBB.cmake b/cmake/HandleTBB.cmake new file mode 100644 index 000000000..cedee55ea --- /dev/null +++ b/cmake/HandleTBB.cmake @@ -0,0 +1,24 @@ +############################################################################### +# Find TBB +find_package(TBB 4.4 COMPONENTS tbb tbbmalloc) + +# Set up variables if we're using TBB +if(TBB_FOUND AND GTSAM_WITH_TBB) + set(GTSAM_USE_TBB 1) # This will go into config.h + if ((${TBB_VERSION_MAJOR} GREATER 2020) OR (${TBB_VERSION_MAJOR} EQUAL 2020)) + set(TBB_GREATER_EQUAL_2020 1) + else() + set(TBB_GREATER_EQUAL_2020 0) + endif() + # all definitions and link requisites will go via imported targets: + # tbb & tbbmalloc + list(APPEND GTSAM_ADDITIONAL_LIBRARIES tbb tbbmalloc) +else() + set(GTSAM_USE_TBB 0) # This will go into config.h +endif() + +############################################################################### +# Prohibit Timing build mode in combination with TBB +if(GTSAM_USE_TBB AND (CMAKE_BUILD_TYPE STREQUAL "Timing")) + message(FATAL_ERROR "Timing build mode cannot be used together with TBB. Use a sampling profiler such as Instruments or Intel VTune Amplifier instead.") +endif() diff --git a/cmake/HandleUninstall.cmake b/cmake/HandleUninstall.cmake new file mode 100644 index 000000000..1859b0273 --- /dev/null +++ b/cmake/HandleUninstall.cmake @@ -0,0 +1,10 @@ +# ---------------------------------------------------------------------------- +# Uninstall target, for "make uninstall" +# ---------------------------------------------------------------------------- +configure_file( + "${CMAKE_CURRENT_SOURCE_DIR}/cmake/cmake_uninstall.cmake.in" + "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake" + IMMEDIATE @ONLY) + +add_custom_target(uninstall + "${CMAKE_COMMAND}" -P "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake") diff --git a/docker/ubuntu-gtsam-python/Dockerfile b/docker/ubuntu-gtsam-python/Dockerfile index c733ceb19..ce5d8fdca 100644 --- a/docker/ubuntu-gtsam-python/Dockerfile +++ b/docker/ubuntu-gtsam-python/Dockerfile @@ -7,9 +7,9 @@ FROM dellaert/ubuntu-gtsam:bionic RUN apt-get install -y python3-pip python3-dev # Install python wrapper requirements -RUN python3 -m pip install -U -r /usr/src/gtsam/cython/requirements.txt +RUN python3 -m pip install -U -r /usr/src/gtsam/python/requirements.txt -# Run cmake again, now with cython toolbox on +# Run cmake again, now with python toolbox on WORKDIR /usr/src/gtsam/build RUN cmake \ -DCMAKE_BUILD_TYPE=Release \ @@ -17,7 +17,7 @@ RUN cmake \ -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \ -DGTSAM_BUILD_TIMING_ALWAYS=OFF \ -DGTSAM_BUILD_TESTS=OFF \ - -DGTSAM_INSTALL_CYTHON_TOOLBOX=ON \ + -DGTSAM_BUILD_PYTHON=ON \ -DGTSAM_PYTHON_VERSION=3\ .. @@ -25,7 +25,7 @@ RUN cmake \ RUN make -j4 install && make clean # Needed to run python wrapper: -RUN echo 'export PYTHONPATH=/usr/local/cython/:$PYTHONPATH' >> /root/.bashrc +RUN echo 'export PYTHONPATH=/usr/local/python/:$PYTHONPATH' >> /root/.bashrc # Run bash CMD ["bash"] diff --git a/docker/ubuntu-gtsam/Dockerfile b/docker/ubuntu-gtsam/Dockerfile index 187c76314..f2b476f15 100644 --- a/docker/ubuntu-gtsam/Dockerfile +++ b/docker/ubuntu-gtsam/Dockerfile @@ -23,7 +23,6 @@ RUN cmake \ -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \ -DGTSAM_BUILD_TIMING_ALWAYS=OFF \ -DGTSAM_BUILD_TESTS=OFF \ - -DGTSAM_INSTALL_CYTHON_TOOLBOX=OFF \ .. # Build diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index 34ff0327e..fc5531cdc 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -109,7 +109,7 @@ typename Eigen::Matrix numericalGradient( * @param delta increment for numerical derivative * Class Y is the output argument * Class X is the input argument - * int N is the dimension of the X input value if variable dimension type but known at test time + * @tparam int N is the dimension of the X input value if variable dimension type but known at test time * @return m*n Jacobian computed via central differencing */ @@ -167,15 +167,16 @@ typename internal::FixedSizeMatrix::type numericalDerivative11(Y (*h)(const * @param x2 second argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X1 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative21(const boost::function& h, const X1& x1, const X2& x2, double delta = 1e-5) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, _1, boost::cref(x2)), x1, delta); + return numericalDerivative11(boost::bind(h, _1, boost::cref(x2)), x1, delta); } /** use a raw C++ function pointer */ @@ -192,15 +193,16 @@ typename internal::FixedSizeMatrix::type numericalDerivative21(Y (*h)(cons * @param x2 n-dimensional second argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X2 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative22(boost::function h, const X1& x1, const X2& x2, double delta = 1e-5) { // BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), // "Template argument X1 must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X2 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), _1), x2, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), _1), x2, delta); } /** use a raw C++ function pointer */ @@ -219,8 +221,9 @@ typename internal::FixedSizeMatrix::type numericalDerivative22(Y (*h)(cons * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing * All classes Y,X1,X2,X3 need dim, expmap, logmap + * @tparam int N is the dimension of the X1 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative31( boost::function h, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { @@ -228,7 +231,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative31( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, _1, boost::cref(x2), boost::cref(x3)), x1, delta); + return numericalDerivative11(boost::bind(h, _1, boost::cref(x2), boost::cref(x3)), x1, delta); } template @@ -247,8 +250,9 @@ typename internal::FixedSizeMatrix::type numericalDerivative31(Y (*h)(cons * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing * All classes Y,X1,X2,X3 need dim, expmap, logmap + * @tparam int N is the dimension of the X2 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative32( boost::function h, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { @@ -256,7 +260,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative32( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X2 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), _1, boost::cref(x3)), x2, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), _1, boost::cref(x3)), x2, delta); } template @@ -275,8 +279,9 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative32(Y (* * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing * All classes Y,X1,X2,X3 need dim, expmap, logmap + * @tparam int N is the dimension of the X3 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative33( boost::function h, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { @@ -284,7 +289,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative33( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X3 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), _1), x3, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), _1), x3, delta); } template @@ -303,8 +308,9 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative33(Y (* * @param x4 fourth argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X1 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative41( boost::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { @@ -312,7 +318,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative41( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4)), x1, delta); + return numericalDerivative11(boost::bind(h, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4)), x1, delta); } template @@ -330,8 +336,9 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative41(Y (* * @param x4 fourth argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X2 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative42( boost::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { @@ -339,7 +346,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative42( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X2 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4)), x2, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4)), x2, delta); } template @@ -357,8 +364,9 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative42(Y (* * @param x4 fourth argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X3 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative43( boost::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { @@ -366,7 +374,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative43( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X3 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4)), x3, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4)), x3, delta); } template @@ -384,8 +392,9 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative43(Y (* * @param x4 n-dimensional fourth argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X4 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative44( boost::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { @@ -393,7 +402,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative44( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X4 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1), x4, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1), x4, delta); } template @@ -412,8 +421,9 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative44(Y (* * @param x5 fifth argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X1 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative51( boost::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { @@ -421,7 +431,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative51( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5)), x1, delta); + return numericalDerivative11(boost::bind(h, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5)), x1, delta); } template @@ -440,8 +450,9 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative51(Y (* * @param x5 fifth argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X2 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative52( boost::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { @@ -449,7 +460,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative52( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4), boost::cref(x5)), x2, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4), boost::cref(x5)), x2, delta); } template @@ -468,8 +479,9 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative52(Y (* * @param x5 fifth argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X3 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative53( boost::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { @@ -477,7 +489,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative53( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4), boost::cref(x5)), x3, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4), boost::cref(x5)), x3, delta); } template @@ -496,8 +508,9 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative53(Y (* * @param x5 fifth argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X4 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative54( boost::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { @@ -505,7 +518,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative54( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1, boost::cref(x5)), x4, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1, boost::cref(x5)), x4, delta); } template @@ -524,8 +537,9 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative54(Y (* * @param x5 fifth argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X5 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative55( boost::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { @@ -533,7 +547,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative55( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), _1), x5, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), _1), x5, delta); } template @@ -553,8 +567,9 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative55(Y (* * @param x6 sixth argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X1 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative61( boost::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { @@ -562,7 +577,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative61( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5), boost::cref(x6)), x1, delta); + return numericalDerivative11(boost::bind(h, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5), boost::cref(x6)), x1, delta); } template @@ -582,8 +597,9 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative61(Y (* * @param x6 sixth argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X2 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative62( boost::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { @@ -591,7 +607,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative62( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4), boost::cref(x5), boost::cref(x6)), x2, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4), boost::cref(x5), boost::cref(x6)), x2, delta); } template @@ -608,11 +624,12 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative62(Y (* * @param x3 third argument value * @param x4 fourth argument value * @param x5 fifth argument value - * @param x6 sixth argument value + * @param x6 sixth argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X3 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative63( boost::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { @@ -620,7 +637,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative63( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4), boost::cref(x5), boost::cref(x6)), x3, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4), boost::cref(x5), boost::cref(x6)), x3, delta); } template @@ -640,8 +657,9 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative63(Y (* * @param x6 sixth argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X4 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative64( boost::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { @@ -649,7 +667,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative64( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1, boost::cref(x5), boost::cref(x6)), x4, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1, boost::cref(x5), boost::cref(x6)), x4, delta); } template @@ -669,8 +687,9 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative64(Y (* * @param x6 sixth argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X5 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative65( boost::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { @@ -678,7 +697,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative65( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), _1, boost::cref(x6)), x5, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), _1, boost::cref(x6)), x5, delta); } template @@ -698,8 +717,9 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative65(Y (* * @param x6 sixth argument value * @param delta increment for numerical derivative * @return m*n Jacobian computed via central differencing + * @tparam int N is the dimension of the X6 input value if variable dimension type but known at test time */ -template +template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative66( boost::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, @@ -708,7 +728,7 @@ typename internal::FixedSizeMatrix::type numericalDerivative66( "Template argument Y must be a manifold type."); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); - return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5), _1), x6, delta); + return numericalDerivative11(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5), _1), x6, delta); } template diff --git a/gtsam/base/tests/testNumericalDerivative.cpp b/gtsam/base/tests/testNumericalDerivative.cpp index d27e43040..6d6f41fdd 100644 --- a/gtsam/base/tests/testNumericalDerivative.cpp +++ b/gtsam/base/tests/testNumericalDerivative.cpp @@ -143,6 +143,13 @@ Vector6 f6(const double x1, const double x2, const double x3, const double x4, return result; } +Vector g6(const double x1, const double x2, const double x3, const double x4, + const double x5, const double x6) { + Vector result(6); + result << sin(x1), cos(x2), x3 * x3, x4 * x4 * x4, sqrt(x5), sin(x6) - cos(x6); + return result; +} + /* ************************************************************************* */ // TEST(testNumericalDerivative, numeriDerivative61) { @@ -153,6 +160,14 @@ TEST(testNumericalDerivative, numeriDerivative61) { double, double, double, double>(f6, x1, x2, x3, x4, x5, x6); EXPECT(assert_equal(expected61, actual61, 1e-5)); + + Matrix expected61Dynamic = Matrix::Zero(6, 1); + expected61Dynamic(0, 0) = cos(x1); + Matrix actual61Dynamic = + numericalDerivative61(g6, x1, x2, x3, x4, x5, x6); + + EXPECT(assert_equal(expected61Dynamic, actual61Dynamic, 1e-5)); } /* ************************************************************************* */ @@ -165,6 +180,13 @@ TEST(testNumericalDerivative, numeriDerivative62) { double, double, double>(f6, x1, x2, x3, x4, x5, x6); EXPECT(assert_equal(expected62, actual62, 1e-5)); + + Matrix expected62Dynamic = Matrix::Zero(6, 1); + expected62Dynamic(1, 0) = -sin(x2); + Matrix61 actual62Dynamic = numericalDerivative62(f6, x1, x2, x3, x4, x5, x6); + + EXPECT(assert_equal(expected62Dynamic, actual62Dynamic, 1e-5)); } /* ************************************************************************* */ @@ -177,6 +199,14 @@ TEST(testNumericalDerivative, numeriDerivative63) { double, double, double>(f6, x1, x2, x3, x4, x5, x6); EXPECT(assert_equal(expected63, actual63, 1e-5)); + + Matrix expected63Dynamic = Matrix::Zero(6, 1); + expected63Dynamic(2, 0) = 2 * x3; + Matrix61 actual63Dynamic = + numericalDerivative63(f6, x1, x2, x3, x4, x5, x6); + + EXPECT(assert_equal(expected63Dynamic, actual63Dynamic, 1e-5)); } /* ************************************************************************* */ @@ -189,6 +219,14 @@ TEST(testNumericalDerivative, numeriDerivative64) { double, double, double>(f6, x1, x2, x3, x4, x5, x6); EXPECT(assert_equal(expected64, actual64, 1e-5)); + + Matrix expected64Dynamic = Matrix::Zero(6, 1); + expected64Dynamic(3, 0) = 3 * x4 * x4; + Matrix61 actual64Dynamic = + numericalDerivative64(f6, x1, x2, x3, x4, x5, x6); + + EXPECT(assert_equal(expected64Dynamic, actual64Dynamic, 1e-5)); } /* ************************************************************************* */ @@ -201,6 +239,14 @@ TEST(testNumericalDerivative, numeriDerivative65) { double, double, double>(f6, x1, x2, x3, x4, x5, x6); EXPECT(assert_equal(expected65, actual65, 1e-5)); + + Matrix expected65Dynamic = Matrix::Zero(6, 1); + expected65Dynamic(4, 0) = 0.5 / sqrt(x5); + Matrix61 actual65Dynamic = + numericalDerivative65(f6, x1, x2, x3, x4, x5, x6); + + EXPECT(assert_equal(expected65Dynamic, actual65Dynamic, 1e-5)); } /* ************************************************************************* */ @@ -213,6 +259,14 @@ TEST(testNumericalDerivative, numeriDerivative66) { double, double, double>(f6, x1, x2, x3, x4, x5, x6); EXPECT(assert_equal(expected66, actual66, 1e-5)); + + Matrix expected66Dynamic = Matrix::Zero(6, 1); + expected66Dynamic(5, 0) = cos(x6) + sin(x6); + Matrix61 actual66Dynamic = + numericalDerivative66(f6, x1, x2, x3, x4, x5, x6); + + EXPECT(assert_equal(expected66Dynamic, actual66Dynamic, 1e-5)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Cal3Bundler.cpp b/gtsam/geometry/Cal3Bundler.cpp index 0d64d0184..b198643b0 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -25,13 +25,13 @@ namespace gtsam { /* ************************************************************************* */ Cal3Bundler::Cal3Bundler() : - f_(1), k1_(0), k2_(0), u0_(0), v0_(0) { + f_(1), k1_(0), k2_(0), u0_(0), v0_(0), tol_(1e-5) { } /* ************************************************************************* */ -Cal3Bundler::Cal3Bundler(double f, double k1, double k2, double u0, double v0) : - f_(f), k1_(k1), k2_(k2), u0_(u0), v0_(v0) { -} +Cal3Bundler::Cal3Bundler(double f, double k1, double k2, double u0, double v0, + double tol) + : f_(f), k1_(k1), k2_(k2), u0_(u0), v0_(v0), tol_(tol) {} /* ************************************************************************* */ Matrix3 Cal3Bundler::K() const { @@ -94,21 +94,24 @@ Point2 Cal3Bundler::uncalibrate(const Point2& p, // } /* ************************************************************************* */ -Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const { +Point2 Cal3Bundler::calibrate(const Point2& pi, + OptionalJacobian<2, 3> Dcal, + OptionalJacobian<2, 2> Dp) const { // Copied from Cal3DS2 :-( // but specialized with k1,k2 non-zero only and fx=fy and s=0 - const Point2 invKPi((pi.x() - u0_)/f_, (pi.y() - v0_)/f_); + double x = (pi.x() - u0_)/f_, y = (pi.y() - v0_)/f_; + const Point2 invKPi(x, y); // initialize by ignoring the distortion at all, might be problematic for pixels around boundary - Point2 pn = invKPi; + Point2 pn(x, y); // iterate until the uncalibrate is close to the actual pixel coordinate const int maxIterations = 10; int iteration; for (iteration = 0; iteration < maxIterations; ++iteration) { - if (distance2(uncalibrate(pn), pi) <= tol) + if (distance2(uncalibrate(pn), pi) <= tol_) break; - const double x = pn.x(), y = pn.y(), xx = x * x, yy = y * y; + const double px = pn.x(), py = pn.y(), xx = px * px, yy = py * py; const double rr = xx + yy; const double g = (1 + k1_ * rr + k2_ * rr * rr); pn = invKPi / g; @@ -118,6 +121,25 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const { throw std::runtime_error( "Cal3Bundler::calibrate fails to converge. need a better initialization"); + // We make use of the Implicit Function Theorem to compute the Jacobians from uncalibrate + // Given f(pi, pn) = uncalibrate(pn) - pi, and g(pi) = calibrate, we can easily compute the Jacobians + // df/pi = -I (pn and pi are independent args) + // Dcal = -inv(H_uncal_pn) * df/pi = -inv(H_uncal_pn) * (-I) = inv(H_uncal_pn) + // Dp = -inv(H_uncal_pn) * df/K = -inv(H_uncal_pn) * H_uncal_K + Matrix23 H_uncal_K; + Matrix22 H_uncal_pn, H_uncal_pn_inv; + + if (Dcal || Dp) { + // Compute uncalibrate Jacobians + uncalibrate(pn, Dcal ? &H_uncal_K : nullptr, H_uncal_pn); + + H_uncal_pn_inv = H_uncal_pn.inverse(); + + if (Dp) *Dp = H_uncal_pn_inv; + if (Dcal) *Dcal = -H_uncal_pn_inv * H_uncal_K; + + } + return pn; } diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index 4787f565b..2e3fab002 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -33,6 +33,7 @@ private: double f_; ///< focal length double k1_, k2_; ///< radial distortion double u0_, v0_; ///< image center, not a parameter to be optimized but a constant + double tol_; ///< tolerance value when calibrating public: @@ -51,8 +52,10 @@ public: * @param k2 second radial distortion coefficient (quartic) * @param u0 optional image center (default 0), considered a constant * @param v0 optional image center (default 0), considered a constant + * @param tol optional calibration tolerance value */ - Cal3Bundler(double f, double k1, double k2, double u0 = 0, double v0 = 0); + Cal3Bundler(double f, double k1, double k2, double u0 = 0, double v0 = 0, + double tol = 1e-5); virtual ~Cal3Bundler() {} @@ -95,6 +98,17 @@ public: return k2_; } + /// image center in x + inline double px() const { + return u0_; + } + + /// image center in y + inline double py() const { + return v0_; + } + +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 /// get parameter u0 inline double u0() const { return u0_; @@ -104,6 +118,7 @@ public: inline double v0() const { return v0_; } +#endif /** @@ -117,8 +132,17 @@ public: Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal = boost::none, OptionalJacobian<2, 2> Dp = boost::none) const; - /// Convert a pixel coordinate to ideal coordinate - Point2 calibrate(const Point2& pi, const double tol = 1e-5) const; + /** + * Convert a pixel coordinate to ideal coordinate xy + * @param p point in image coordinates + * @param tol optional tolerance threshold value for iterative minimization + * @param Dcal optional 2*3 Jacobian wrpt Cal3Bundler parameters + * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates + * @return point in intrinsic coordinates + */ + Point2 calibrate(const Point2& pi, + OptionalJacobian<2, 3> Dcal = boost::none, + OptionalJacobian<2, 2> Dp = boost::none) const; /// @deprecated might be removed in next release, use uncalibrate Matrix2 D2d_intrinsic(const Point2& p) const; @@ -164,6 +188,7 @@ private: ar & BOOST_SERIALIZATION_NVP(k2_); ar & BOOST_SERIALIZATION_NVP(u0_); ar & BOOST_SERIALIZATION_NVP(v0_); + ar & BOOST_SERIALIZATION_NVP(tol_); } /// @} diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index 7a46f5988..a565ac140 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -75,6 +75,18 @@ double dot(const Point3 &p, const Point3 &q, OptionalJacobian<1, 3> H1, return p.x() * q.x() + p.y() * q.y() + p.z() * q.z(); } +Point3Pair means(const std::vector &abPointPairs) { + const size_t n = abPointPairs.size(); + if (n == 0) throw std::invalid_argument("Point3::mean input Point3Pair vector is empty"); + Point3 aSum(0, 0, 0), bSum(0, 0, 0); + for (const Point3Pair &abPair : abPointPairs) { + aSum += abPair.first; + bSum += abPair.second; + } + const double f = 1.0 / n; + return {aSum * f, bSum * f}; +} + /* ************************************************************************* */ ostream &operator<<(ostream &os, const gtsam::Point3Pair &p) { os << p.first << " <-> " << p.second; diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 19e328022..57188fc5e 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -26,6 +26,7 @@ #include #include #include +#include namespace gtsam { @@ -58,6 +59,18 @@ GTSAM_EXPORT double dot(const Point3& p, const Point3& q, OptionalJacobian<1, 3> H_p = boost::none, OptionalJacobian<1, 3> H_q = boost::none); +/// mean +template +GTSAM_EXPORT Point3 mean(const CONTAINER& points) { + if (points.size() == 0) throw std::invalid_argument("Point3::mean input container is empty"); + Point3 sum(0, 0, 0); + sum = std::accumulate(points.begin(), points.end(), sum); + return sum / points.size(); +} + +/// Calculate the two means of a set of Point3 pairs +GTSAM_EXPORT Point3Pair means(const std::vector &abPointPairs); + template struct Range; diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 0a56e2ef5..22849d4f5 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -24,10 +24,11 @@ #include #include -using namespace std; - namespace gtsam { +using std::vector; +using Point3Pairs = vector; + /** instantiate concept checks */ GTSAM_CONCEPT_POSE_INST(Pose3); @@ -190,15 +191,7 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) { } /* ************************************************************************* */ -/** - * Compute the 3x3 bottom-left block Q of the SE3 Expmap derivative matrix - * J(xi) = [J_(w) Z_3x3; - * Q J_(w)] - * where J_(w) is the SO3 Expmap derivative. - * (see Chirikjian11book2, pg 44, eq 10.95. - * The closed-form formula is similar to formula 102 in Barfoot14tro) - */ -static Matrix3 computeQforExpmapDerivative(const Vector6& xi) { +Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi, double nearZeroThreshold) { const auto w = xi.head<3>(); const auto v = xi.tail<3>(); const Matrix3 V = skewSymmetric(v); @@ -220,18 +213,20 @@ static Matrix3 computeQforExpmapDerivative(const Vector6& xi) { #else // The closed-form formula in Barfoot14tro eq. (102) double phi = w.norm(); - if (std::abs(phi)>1e-5) { - const double sinPhi = sin(phi), cosPhi = cos(phi); - const double phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi, phi5 = phi4 * phi; + const Matrix3 WVW = W * V * W; + if (std::abs(phi) > nearZeroThreshold) { + const double s = sin(phi), c = cos(phi); + const double phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi, + phi5 = phi4 * phi; // Invert the sign of odd-order terms to have the right Jacobian - Q = -0.5*V + (phi-sinPhi)/phi3*(W*V + V*W - W*V*W) - + (1-phi2/2-cosPhi)/phi4*(W*W*V + V*W*W - 3*W*V*W) - - 0.5*((1-phi2/2-cosPhi)/phi4 - 3*(phi-sinPhi-phi3/6.)/phi5)*(W*V*W*W + W*W*V*W); - } - else { - Q = -0.5*V + 1./6.*(W*V + V*W - W*V*W) - + 1./24.*(W*W*V + V*W*W - 3*W*V*W) - - 0.5*(1./24. + 3./120.)*(W*V*W*W + W*W*V*W); + Q = -0.5 * V + (phi - s) / phi3 * (W * V + V * W - WVW) + + (1 - phi2 / 2 - c) / phi4 * (W * W * V + V * W * W - 3 * WVW) - + 0.5 * ((1 - phi2 / 2 - c) / phi4 - 3 * (phi - s - phi3 / 6.) / phi5) * + (WVW * W + W * WVW); + } else { + Q = -0.5 * V + 1. / 6. * (W * V + V * W - WVW) - + 1. / 24. * (W * W * V + V * W * W - 3 * WVW) + + 1. / 120. * (WVW * W + W * WVW); } #endif @@ -242,7 +237,7 @@ static Matrix3 computeQforExpmapDerivative(const Vector6& xi) { Matrix6 Pose3::ExpmapDerivative(const Vector6& xi) { const Vector3 w = xi.head<3>(); const Matrix3 Jw = Rot3::ExpmapDerivative(w); - const Matrix3 Q = computeQforExpmapDerivative(xi); + const Matrix3 Q = ComputeQforExpmapDerivative(xi); Matrix6 J; J << Jw, Z_3x3, Q, Jw; return J; @@ -253,7 +248,7 @@ Matrix6 Pose3::LogmapDerivative(const Pose3& pose) { const Vector6 xi = Logmap(pose); const Vector3 w = xi.head<3>(); const Matrix3 Jw = Rot3::LogmapDerivative(w); - const Matrix3 Q = computeQforExpmapDerivative(xi); + const Matrix3 Q = ComputeQforExpmapDerivative(xi); const Matrix3 Q2 = -Jw*Q*Jw; Matrix6 J; J << Jw, Z_3x3, Q2, Jw; @@ -389,39 +384,33 @@ Unit3 Pose3::bearing(const Pose3& pose, OptionalJacobian<2, 6> Hself, } /* ************************************************************************* */ -boost::optional Pose3::Align(const std::vector& abPointPairs) { +boost::optional Pose3::Align(const Point3Pairs &abPointPairs) { const size_t n = abPointPairs.size(); - if (n < 3) - return boost::none; // we need at least three pairs + if (n < 3) { + return boost::none; // we need at least three pairs + } // calculate centroids - Point3 aCentroid(0,0,0), bCentroid(0,0,0); - for(const Point3Pair& abPair: abPointPairs) { - aCentroid += abPair.first; - bCentroid += abPair.second; - } - double f = 1.0 / n; - aCentroid *= f; - bCentroid *= f; + const auto centroids = means(abPointPairs); // Add to form H matrix Matrix3 H = Z_3x3; - for(const Point3Pair& abPair: abPointPairs) { - Point3 da = abPair.first - aCentroid; - Point3 db = abPair.second - bCentroid; + for (const Point3Pair &abPair : abPointPairs) { + const Point3 da = abPair.first - centroids.first; + const Point3 db = abPair.second - centroids.second; H += da * db.transpose(); - } + } // ClosestTo finds rotation matrix closest to H in Frobenius sense - Rot3 aRb = Rot3::ClosestTo(H); - Point3 aTb = Point3(aCentroid) - aRb * Point3(bCentroid); + const Rot3 aRb = Rot3::ClosestTo(H); + const Point3 aTb = centroids.first - aRb * centroids.second; return Pose3(aRb, aTb); } -boost::optional align(const vector& baPointPairs) { - vector abPointPairs; - for (const Point3Pair& baPair: baPointPairs) { - abPointPairs.push_back(make_pair(baPair.second, baPair.first)); +boost::optional align(const Point3Pairs &baPointPairs) { + Point3Pairs abPointPairs; + for (const Point3Pair &baPair : baPointPairs) { + abPointPairs.emplace_back(baPair.second, baPair.first); } return Pose3::Align(abPointPairs); } diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 159fd2927..990ffdfe2 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -181,6 +181,18 @@ public: static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = boost::none); }; + /** + * Compute the 3x3 bottom-left block Q of SE3 Expmap right derivative matrix + * J_r(xi) = [J_(w) Z_3x3; + * Q_r J_(w)] + * where J_(w) is the SO3 Expmap right derivative. + * (see Chirikjian11book2, pg 44, eq 10.95. + * The closed-form formula is identical to formula 102 in Barfoot14tro where + * Q_l of the SE3 Expmap left derivative matrix is given. + */ + static Matrix3 ComputeQforExpmapDerivative( + const Vector6& xi, double nearZeroThreshold = 1e-5); + using LieGroup::inverse; // version with derivative /** @@ -356,6 +368,9 @@ inline Matrix wedge(const Vector& xi) { return Pose3::wedge(xi(0), xi(1), xi(2), xi(3), xi(4), xi(5)); } +// Convenience typedef +using Pose3Pair = std::pair; + // For MATLAB wrapper typedef std::vector Pose3Vector; diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index de9d2b420..abd74e063 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -262,9 +262,29 @@ namespace gtsam { static Rot3 AlignTwoPairs(const Unit3& a_p, const Unit3& b_p, // const Unit3& a_q, const Unit3& b_q); - /// Static, named constructor that finds Rot3 element closest to M in Frobenius norm. + /** + * Static, named constructor that finds Rot3 element closest to M in Frobenius norm. + * + * Uses Full SVD to compute the orthogonal matrix, thus is highly accurate and robust. + * + * N. J. Higham. Matrix nearness problems and applications. + * In M. J. C. Gover and S. Barnett, editors, Applications of Matrix Theory, pages 1–27. + * Oxford University Press, 1989. + */ static Rot3 ClosestTo(const Matrix3& M) { return Rot3(SO3::ClosestTo(M)); } + /** + * Normalize rotation so that its determinant is 1. + * This means either re-orthogonalizing the Matrix representation or + * normalizing the quaternion representation. + * + * This method is akin to `ClosestTo` but uses a computationally cheaper + * algorithm. + * + * Ref: https://drive.google.com/file/d/0B9rLLz1XQKmaZTlQdV81QjNoZTA/view + */ + Rot3 normalized() const; + /// @} /// @name Testable /// @{ @@ -506,7 +526,7 @@ namespace gtsam { /** * @brief Spherical Linear intERPolation between *this and other - * @param s a value between 0 and 1 + * @param t a value between 0 and 1 * @param other final point of iterpolation geodesic on manifold */ Rot3 slerp(double t, const Rot3& other) const; diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 500941a16..02e5b771f 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -108,6 +108,33 @@ Rot3 Rot3::RzRyRx(double x, double y, double z, OptionalJacobian<3, 1> Hx, ); } +/* ************************************************************************* */ +Rot3 Rot3::normalized() const { + /// Implementation from here: https://stackoverflow.com/a/23082112/1236990 + + /// Essentially, this computes the orthogonalization error, distributes the + /// error to the x and y rows, and then performs a Taylor expansion to + /// orthogonalize. + + Matrix3 rot = rot_.matrix(), rot_orth; + + // Check if determinant is already 1. + // If yes, then return the current Rot3. + if (std::fabs(rot.determinant()-1) < 1e-12) return Rot3(rot_); + + Vector3 x = rot.block<1, 3>(0, 0), y = rot.block<1, 3>(1, 0); + double error = x.dot(y); + + Vector3 x_ort = x - (error / 2) * y, y_ort = y - (error / 2) * x; + Vector3 z_ort = x_ort.cross(y_ort); + + rot_orth.block<1, 3>(0, 0) = 0.5 * (3 - x_ort.dot(x_ort)) * x_ort; + rot_orth.block<1, 3>(1, 0) = 0.5 * (3 - y_ort.dot(y_ort)) * y_ort; + rot_orth.block<1, 3>(2, 0) = 0.5 * (3 - z_ort.dot(z_ort)) * z_ort; + + return Rot3(rot_orth); +} + /* ************************************************************************* */ Rot3 Rot3::operator*(const Rot3& R2) const { return Rot3(rot_*R2.rot_); diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index 6e1871c64..523255d87 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -86,6 +86,10 @@ namespace gtsam { gtsam::Quaternion(Eigen::AngleAxisd(x, Eigen::Vector3d::UnitX()))); } + /* ************************************************************************* */ + Rot3 Rot3::normalized() const { + return Rot3(quaternion_.normalized()); + } /* ************************************************************************* */ Rot3 Rot3::operator*(const Rot3& R2) const { return Rot3(quaternion_ * R2.quaternion_); diff --git a/gtsam/geometry/tests/testCal3Bundler.cpp b/gtsam/geometry/tests/testCal3Bundler.cpp index 8de049fa4..448600266 100644 --- a/gtsam/geometry/tests/testCal3Bundler.cpp +++ b/gtsam/geometry/tests/testCal3Bundler.cpp @@ -52,12 +52,14 @@ TEST( Cal3Bundler, calibrate ) Point2 pn(0.5, 0.5); Point2 pi = K.uncalibrate(pn); Point2 pn_hat = K.calibrate(pi); - CHECK( traits::Equals(pn, pn_hat, 1e-5)); + CHECK(traits::Equals(pn, pn_hat, 1e-5)); } /* ************************************************************************* */ Point2 uncalibrate_(const Cal3Bundler& k, const Point2& pt) { return k.uncalibrate(pt); } +Point2 calibrate_(const Cal3Bundler& k, const Point2& pt) { return k.calibrate(pt); } + /* ************************************************************************* */ TEST( Cal3Bundler, Duncalibrate) { @@ -69,11 +71,20 @@ TEST( Cal3Bundler, Duncalibrate) Matrix numerical2 = numericalDerivative22(uncalibrate_, K, p); CHECK(assert_equal(numerical1,Dcal,1e-7)); CHECK(assert_equal(numerical2,Dp,1e-7)); - CHECK(assert_equal(numerical1,K.D2d_calibration(p),1e-7)); - CHECK(assert_equal(numerical2,K.D2d_intrinsic(p),1e-7)); - Matrix Dcombined(2,5); - Dcombined << Dp, Dcal; - CHECK(assert_equal(Dcombined,K.D2d_intrinsic_calibration(p),1e-7)); +} + +/* ************************************************************************* */ +TEST( Cal3Bundler, Dcalibrate) +{ + Matrix Dcal, Dp; + Point2 pn(0.5, 0.5); + Point2 pi = K.uncalibrate(pn); + Point2 actual = K.calibrate(pi, Dcal, Dp); + CHECK(assert_equal(pn, actual, 1e-7)); + Matrix numerical1 = numericalDerivative21(calibrate_, K, pi); + Matrix numerical2 = numericalDerivative22(calibrate_, K, pi); + CHECK(assert_equal(numerical1,Dcal,1e-5)); + CHECK(assert_equal(numerical2,Dp,1e-5)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index a9b68bdec..ad6f4e921 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -336,6 +336,15 @@ TEST( PinholeCamera, range3) { EXPECT(assert_equal(Hexpected2, D2, 1e-7)); } +/* ************************************************************************* */ +TEST( PinholeCamera, Cal3Bundler) { + Cal3Bundler calibration; + Pose3 wTc; + PinholeCamera camera(wTc, calibration); + Point2 p(50, 100); + camera.backproject(p, 10); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index a7c2ac50c..a655011a0 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -164,6 +164,26 @@ TEST (Point3, normalize) { EXPECT(assert_equal(expectedH, actualH, 1e-8)); } +//************************************************************************* +TEST(Point3, mean) { + Point3 expected(2, 2, 2); + Point3 a1(0, 0, 0), a2(1, 2, 3), a3(5, 4, 3); + std::vector a_points{a1, a2, a3}; + Point3 actual = mean(a_points); + EXPECT(assert_equal(expected, actual)); +} + +TEST(Point3, mean_pair) { + Point3 a_mean(2, 2, 2), b_mean(-1, 1, 0); + Point3Pair expected = std::make_pair(a_mean, b_mean); + Point3 a1(0, 0, 0), a2(1, 2, 3), a3(5, 4, 3); + Point3 b1(-1, 0, 0), b2(-2, 4, 0), b3(0, -1, 0); + std::vector point_pairs{{a1, b1}, {a2, b2}, {a3, b3}}; + Point3Pair actual = means(point_pairs); + EXPECT(assert_equal(expected.first, actual.first)); + EXPECT(assert_equal(expected.second, actual.second)); +} + //************************************************************************* double norm_proxy(const Point3& point) { return double(point.norm()); diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 8586f35a0..9639ffbcf 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -807,6 +807,17 @@ TEST(Pose3, ExpmapDerivative2) { } } +TEST( Pose3, ExpmapDerivativeQr) { + Vector6 w = Vector6::Random(); + w.head<3>().normalize(); + w.head<3>() = w.head<3>() * 0.9e-2; + Matrix3 actualQr = Pose3::ComputeQforExpmapDerivative(w, 0.01); + Matrix expectedH = numericalDerivative21 >(&Pose3::Expmap, w, boost::none); + Matrix3 expectedQr = expectedH.bottomLeftCorner<3, 3>(); + EXPECT(assert_equal(expectedQr, actualQr, 1e-6)); +} + /* ************************************************************************* */ TEST( Pose3, LogmapDerivative) { Matrix6 actualH; diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index a7c6f5a77..7b792f8bd 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -910,6 +910,26 @@ TEST(Rot3, yaw_derivative) { CHECK(assert_equal(num, calc)); } +/* ************************************************************************* */ +TEST(Rot3, determinant) { + size_t degree = 1; + Rot3 R_w0; // Zero rotation + Rot3 R_w1 = Rot3::Ry(degree * M_PI / 180); + + Rot3 R_01, R_w2; + double actual, expected = 1.0; + + for (size_t i = 2; i < 360; ++i) { + R_01 = R_w0.between(R_w1); + R_w2 = R_w1 * R_01; + R_w0 = R_w1; + R_w1 = R_w2.normalized(); + actual = R_w2.matrix().determinant(); + + EXPECT_DOUBLES_EQUAL(expected, actual, 1e-7); + } +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/geometry/tests/testSerializationGeometry.cpp b/gtsam/geometry/tests/testSerializationGeometry.cpp index f7ff881eb..aa111c3ae 100644 --- a/gtsam/geometry/tests/testSerializationGeometry.cpp +++ b/gtsam/geometry/tests/testSerializationGeometry.cpp @@ -57,7 +57,7 @@ static StereoCamera cam2(pose3, cal4ptr); static StereoPoint2 spt(1.0, 2.0, 3.0); /* ************************************************************************* */ -TEST_DISABLED (Serialization, text_geometry) { +TEST (Serialization, text_geometry) { EXPECT(equalsObj(Point2(1.0, 2.0))); EXPECT(equalsObj(Pose2(1.0, 2.0, 0.3))); EXPECT(equalsObj(Rot2::fromDegrees(30.0))); @@ -82,7 +82,7 @@ TEST_DISABLED (Serialization, text_geometry) { } /* ************************************************************************* */ -TEST_DISABLED (Serialization, xml_geometry) { +TEST (Serialization, xml_geometry) { EXPECT(equalsXML(Point2(1.0, 2.0))); EXPECT(equalsXML(Pose2(1.0, 2.0, 0.3))); EXPECT(equalsXML(Rot2::fromDegrees(30.0))); @@ -106,7 +106,7 @@ TEST_DISABLED (Serialization, xml_geometry) { } /* ************************************************************************* */ -TEST_DISABLED (Serialization, binary_geometry) { +TEST (Serialization, binary_geometry) { EXPECT(equalsBinary(Point2(1.0, 2.0))); EXPECT(equalsBinary(Pose2(1.0, 2.0, 0.3))); EXPECT(equalsBinary(Rot2::fromDegrees(30.0))); diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 085d06245..1b4d976da 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -93,9 +93,9 @@ * - Add "void serializable()" to a class if you only want the class to be serialized as a * part of a container (such as noisemodel). This version does not require a publicly * accessible default constructor. - * Forward declarations and class definitions for Cython: - * - Need to specify the base class (both this forward class and base class are declared in an external cython header) - * This is so Cython can generate proper inheritance. + * Forward declarations and class definitions for Pybind: + * - Need to specify the base class (both this forward class and base class are declared in an external Pybind header) + * This is so Pybind can generate proper inheritance. * Example when wrapping a gtsam-based project: * // forward declarations * virtual class gtsam::NonlinearFactor @@ -104,7 +104,7 @@ * #include * virtual class MyFactor : gtsam::NoiseModelFactor {...}; * - *DO NOT* re-define overriden function already declared in the external (forward-declared) base class - * - This will cause an ambiguity problem in Cython pxd header file + * - This will cause an ambiguity problem in Pybind header file */ /** @@ -334,99 +334,6 @@ virtual class GenericValue : gtsam::Value { void serializable() const; }; -#include -class LieScalar { - // Standard constructors - LieScalar(); - LieScalar(double d); - - // Standard interface - double value() const; - - // Testable - void print(string s) const; - bool equals(const gtsam::LieScalar& expected, double tol) const; - - // Group - static gtsam::LieScalar identity(); - gtsam::LieScalar inverse() const; - gtsam::LieScalar compose(const gtsam::LieScalar& p) const; - gtsam::LieScalar between(const gtsam::LieScalar& l2) const; - - // Manifold - size_t dim() const; - gtsam::LieScalar retract(Vector v) const; - Vector localCoordinates(const gtsam::LieScalar& t2) const; - - // Lie group - static gtsam::LieScalar Expmap(Vector v); - static Vector Logmap(const gtsam::LieScalar& p); -}; - -#include -class LieVector { - // Standard constructors - LieVector(); - LieVector(Vector v); - - // Standard interface - Vector vector() const; - - // Testable - void print(string s) const; - bool equals(const gtsam::LieVector& expected, double tol) const; - - // Group - static gtsam::LieVector identity(); - gtsam::LieVector inverse() const; - gtsam::LieVector compose(const gtsam::LieVector& p) const; - gtsam::LieVector between(const gtsam::LieVector& l2) const; - - // Manifold - size_t dim() const; - gtsam::LieVector retract(Vector v) const; - Vector localCoordinates(const gtsam::LieVector& t2) const; - - // Lie group - static gtsam::LieVector Expmap(Vector v); - static Vector Logmap(const gtsam::LieVector& p); - - // enabling serialization functionality - void serialize() const; -}; - -#include -class LieMatrix { - // Standard constructors - LieMatrix(); - LieMatrix(Matrix v); - - // Standard interface - Matrix matrix() const; - - // Testable - void print(string s) const; - bool equals(const gtsam::LieMatrix& expected, double tol) const; - - // Group - static gtsam::LieMatrix identity(); - gtsam::LieMatrix inverse() const; - gtsam::LieMatrix compose(const gtsam::LieMatrix& p) const; - gtsam::LieMatrix between(const gtsam::LieMatrix& l2) const; - - // Manifold - size_t dim() const; - gtsam::LieMatrix retract(Vector v) const; - Vector localCoordinates(const gtsam::LieMatrix & t2) const; - - // Lie group - static gtsam::LieMatrix Expmap(Vector v); - static Vector Logmap(const gtsam::LieMatrix& p); - - // enabling serialization functionality - void serialize() const; -}; - //************************************************************************* // geometry //************************************************************************* @@ -690,6 +597,7 @@ class Rot3 { Rot3(double R11, double R12, double R13, double R21, double R22, double R23, double R31, double R32, double R33); + Rot3(double w, double x, double y, double z); static gtsam::Rot3 Rx(double t); static gtsam::Rot3 Ry(double t); @@ -1052,6 +960,7 @@ class Cal3Bundler { // Standard Constructors Cal3Bundler(); Cal3Bundler(double fx, double k1, double k2, double u0, double v0); + Cal3Bundler(double fx, double k1, double k2, double u0, double v0, double tol); // Testable void print(string s) const; @@ -1064,7 +973,7 @@ class Cal3Bundler { Vector localCoordinates(const gtsam::Cal3Bundler& c) const; // Action on Point2 - gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const; + gtsam::Point2 calibrate(const gtsam::Point2& p) const; gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; // Standard Interface @@ -1072,8 +981,8 @@ class Cal3Bundler { double fy() const; double k1() const; double k2() const; - double u0() const; - double v0() const; + double px() const; + double py() const; Vector vector() const; Vector k() const; Matrix K() const; @@ -1198,7 +1107,7 @@ typedef gtsam::PinholeCamera PinholeCameraCal3_S2; //TODO (Issue 237) due to lack of jacobians of Cal3DS2_Base::calibrate, PinholeCamera does not apply to Cal3DS2/Unified //typedef gtsam::PinholeCamera PinholeCameraCal3DS2; //typedef gtsam::PinholeCamera PinholeCameraCal3Unified; -//typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; +typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; #include class StereoCamera { @@ -2160,7 +2069,7 @@ class NonlinearFactorGraph { gtsam::KeySet keys() const; gtsam::KeyVector keyVector() const; - template + template, gtsam::imuBias::ConstantBias}> void addPrior(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); // NonlinearFactorGraph @@ -2249,12 +2158,13 @@ class Values { void insert(size_t j, const gtsam::SOn& P); void insert(size_t j, const gtsam::Rot3& rot3); void insert(size_t j, const gtsam::Pose3& pose3); + void insert(size_t j, const gtsam::Unit3& unit3); void insert(size_t j, const gtsam::Cal3_S2& cal3_s2); void insert(size_t j, const gtsam::Cal3DS2& cal3ds2); void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler); void insert(size_t j, const gtsam::EssentialMatrix& essential_matrix); void insert(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera); - // void insert(size_t j, const gtsam::PinholeCameraCal3Bundler& camera); + void insert(size_t j, const gtsam::PinholeCamera& camera); void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); void insert(size_t j, const gtsam::NavState& nav_state); @@ -2267,18 +2177,19 @@ class Values { void update(size_t j, const gtsam::SOn& P); void update(size_t j, const gtsam::Rot3& rot3); void update(size_t j, const gtsam::Pose3& pose3); + void update(size_t j, const gtsam::Unit3& unit3); void update(size_t j, const gtsam::Cal3_S2& cal3_s2); void update(size_t j, const gtsam::Cal3DS2& cal3ds2); void update(size_t j, const gtsam::Cal3Bundler& cal3bundler); void update(size_t j, const gtsam::EssentialMatrix& essential_matrix); void update(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera); - // void update(size_t j, const gtsam::PinholeCameraCal3Bundler& camera); + void update(size_t j, const gtsam::PinholeCamera& camera); void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); void update(size_t j, const gtsam::NavState& nav_state); void update(size_t j, Vector vector); void update(size_t j, Matrix matrix); - template + template, gtsam::imuBias::ConstantBias, gtsam::NavState, Vector, Matrix}> T at(size_t j); /// version for double @@ -2555,6 +2466,8 @@ class ISAM2Result { size_t getVariablesRelinearized() const; size_t getVariablesReeliminated() const; size_t getCliques() const; + double getErrorBefore() const; + double getErrorAfter() const; }; class ISAM2 { @@ -2571,16 +2484,17 @@ class ISAM2 { gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices); gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, const gtsam::KeyGroupMap& constrainedKeys); - // TODO: wrap the full version of update - //void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, FastMap& constrainedKeys); - //void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, FastMap& constrainedKeys, bool force_relinearize); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, gtsam::KeyGroupMap& constrainedKeys, const gtsam::KeyList& noRelinKeys); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, gtsam::KeyGroupMap& constrainedKeys, const gtsam::KeyList& noRelinKeys, const gtsam::KeyList& extraReelimKeys); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, gtsam::KeyGroupMap& constrainedKeys, const gtsam::KeyList& noRelinKeys, const gtsam::KeyList& extraReelimKeys, bool force_relinearize); gtsam::Values getLinearizationPoint() const; gtsam::Values calculateEstimate() const; template + gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::PinholeCamera, + Vector, Matrix}> VALUE calculateEstimate(size_t key) const; gtsam::Values calculateBestEstimate() const; Matrix marginalCovariance(size_t key) const; @@ -2618,7 +2532,7 @@ class NonlinearISAM { #include #include -template +template}> virtual class PriorFactor : gtsam::NoiseModelFactor { PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); T prior() const; @@ -2705,8 +2619,7 @@ class BearingRange { BearingRange(const BEARING& b, const RANGE& r); BEARING bearing() const; RANGE range() const; - // TODO(frank): can't class instance itself? - // static gtsam::BearingRange Measure(const POSE& pose, const POINT& point); + static This Measure(const POSE& pose, const POINT& point); static BEARING MeasureBearing(const POSE& pose, const POINT& point); static RANGE MeasureRange(const POSE& pose, const POINT& point); void print(string s) const; @@ -2764,6 +2677,7 @@ virtual class GeneralSFMFactor : gtsam::NoiseModelFactor { typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; //TODO (Issue 237) due to lack of jacobians of Cal3DS2_Base::calibrate, GeneralSFMFactor does not apply to Cal3DS2 //typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; +typedef gtsam::GeneralSFMFactor, gtsam::Point3> GeneralSFMFactorCal3Bundler; template virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { @@ -2850,22 +2764,36 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { }; #include + class SfmTrack { - Point3 point3() const; + SfmTrack(); + SfmTrack(const gtsam::Point3& pt); + const Point3& point3() const; + + double r; + double g; + double b; + // TODO Need to close wrap#10 to allow this to work. + // std::vector> measurements; + size_t number_measurements() const; pair measurement(size_t idx) const; pair siftIndex(size_t idx) const; + void add_measurement(size_t idx, const gtsam::Point2& m); }; class SfmData { + SfmData(); size_t number_cameras() const; size_t number_tracks() const; - //TODO(Varun) Need to fix issue #237 first before this can work - // gtsam::PinholeCamera camera(size_t idx) const; + gtsam::PinholeCamera camera(size_t idx) const; gtsam::SfmTrack track(size_t idx) const; + void add_track(const gtsam::SfmTrack& t) ; + void add_camera(const gtsam::SfmCamera& cam); }; gtsam::SfmData readBal(string filename); +bool writeBAL(string filename, gtsam::SfmData& data); gtsam::Values initialCamerasEstimate(const gtsam::SfmData& db); gtsam::Values initialCamerasAndPointsEstimate(const gtsam::SfmData& db); @@ -2977,6 +2905,7 @@ class BinaryMeasurement { size_t key1() const; size_t key2() const; T measured() const; + gtsam::noiseModel::Base* noiseModel() const; }; typedef gtsam::BinaryMeasurement BinaryMeasurementUnit3; @@ -3110,6 +3039,26 @@ class ShonanAveraging3 { pair run(const gtsam::Values& initial, size_t min_p, size_t max_p) const; }; +#include + +class KeyPairDoubleMap { + KeyPairDoubleMap(); + KeyPairDoubleMap(const gtsam::KeyPairDoubleMap& other); + + size_t size() const; + bool empty() const; + void clear(); + size_t at(const pair& keypair) const; +}; + +class MFAS { + MFAS(const gtsam::BinaryMeasurementsUnit3& relativeTranslations, + const gtsam::Unit3& projectionDirection); + + gtsam::KeyPairDoubleMap computeOutlierWeights() const; + gtsam::KeyVector computeOrdering() const; +}; + #include class TranslationRecovery { TranslationRecovery(const gtsam::BinaryMeasurementsUnit3 &relativeTranslations, diff --git a/gtsam/inference/Symbol.h b/gtsam/inference/Symbol.h index 5be195db3..469082f16 100644 --- a/gtsam/inference/Symbol.h +++ b/gtsam/inference/Symbol.h @@ -164,8 +164,16 @@ inline Key Y(std::uint64_t j) { return Symbol('y', j); } inline Key Z(std::uint64_t j) { return Symbol('z', j); } } +/** Generates symbol shorthands with alternative names different than the + * one-letter predefined ones. */ +class SymbolGenerator { + const char c_; +public: + SymbolGenerator(const char c) : c_(c) {} + Symbol operator()(const std::uint64_t j) const { return Symbol(c_, j); } +}; + /// traits template<> struct traits : public Testable {}; } // \ namespace gtsam - diff --git a/gtsam/inference/tests/testKey.cpp b/gtsam/inference/tests/testKey.cpp index a60258581..64674c36f 100644 --- a/gtsam/inference/tests/testKey.cpp +++ b/gtsam/inference/tests/testKey.cpp @@ -40,6 +40,25 @@ TEST(Key, KeySymbolConversion) { EXPECT(assert_equal(original, actual)) } +/* ************************************************************************* */ +TEST(Key, SymbolGenerator) { + const auto x1 = gtsam::symbol_shorthand::X(1); + const auto v1 = gtsam::symbol_shorthand::V(1); + const auto a1 = gtsam::symbol_shorthand::A(1); + + const auto Z = gtsam::SymbolGenerator('x'); + const auto DZ = gtsam::SymbolGenerator('v'); + const auto DDZ = gtsam::SymbolGenerator('a'); + + const auto z1 = Z(1); + const auto dz1 = DZ(1); + const auto ddz1 = DDZ(1); + + EXPECT(assert_equal(x1, z1)); + EXPECT(assert_equal(v1, dz1)); + EXPECT(assert_equal(a1, ddz1)); +} + /* ************************************************************************* */ template Key KeyTestValue(); @@ -106,4 +125,3 @@ int main() { return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index ec1a07f65..34626fcf6 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -42,7 +42,7 @@ class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation public: - /// Default constructor, only for serialization and Cython wrapper + /// Default constructor, only for serialization and wrappers PreintegratedAhrsMeasurements() {} /** diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 387353136..efca25bff 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -145,7 +145,7 @@ public: /// @name Constructors /// @{ - /// Default constructor only for serialization and Cython wrapper + /// Default constructor only for serialization and wrappers PreintegratedCombinedMeasurements() { preintMeasCov_.setZero(); } diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 51df3f24a..cd9c591f1 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -80,7 +80,7 @@ protected: public: - /// Default constructor for serialization and Cython wrapper + /// Default constructor for serialization and wrappers PreintegratedImuMeasurements() { preintMeasCov_.setZero(); } diff --git a/gtsam/nonlinear/ISAM2Result.h b/gtsam/nonlinear/ISAM2Result.h index e45b17e4a..b249af5c5 100644 --- a/gtsam/nonlinear/ISAM2Result.h +++ b/gtsam/nonlinear/ISAM2Result.h @@ -176,6 +176,8 @@ struct ISAM2Result { size_t getVariablesRelinearized() const { return variablesRelinearized; } size_t getVariablesReeliminated() const { return variablesReeliminated; } size_t getCliques() const { return cliques; } + double getErrorBefore() const { return errorBefore ? *errorBefore : std::nan(""); } + double getErrorAfter() const { return errorAfter ? *errorAfter : std::nan(""); } }; } // namespace gtsam diff --git a/gtsam/nonlinear/Marginals.h b/gtsam/nonlinear/Marginals.h index 4e201cc38..9935bafdd 100644 --- a/gtsam/nonlinear/Marginals.h +++ b/gtsam/nonlinear/Marginals.h @@ -48,7 +48,7 @@ protected: public: - /// Default constructor only for Cython wrapper + /// Default constructor only for wrappers Marginals(){} /** Construct a marginals class from a nonlinear factor graph. @@ -156,7 +156,7 @@ protected: FastMap indices_; public: - /// Default constructor only for Cython wrapper + /// Default constructor only for wrappers JointMarginal() {} /** Access a block, corresponding to a pair of variables, of the joint diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index 9a9c487b6..fd9961742 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -88,20 +88,24 @@ void NonlinearOptimizer::defaultOptimize() { } // Iterative loop + double newError = currentError; // used to avoid repeated calls to error() do { // Do next iteration - currentError = error(); // TODO(frank): don't do this twice at first !? Computed above! + currentError = newError; iterate(); tictoc_finishedIteration(); + // Update newError for either printouts or conditional-end checks: + newError = error(); + // Maybe show output if (params.verbosity >= NonlinearOptimizerParams::VALUES) values().print("newValues"); if (params.verbosity >= NonlinearOptimizerParams::ERROR) - cout << "newError: " << error() << endl; + cout << "newError: " << newError << endl; } while (iterations() < params.maxIterations && !checkConvergence(params.relativeErrorTol, params.absoluteErrorTol, params.errorTol, - currentError, error(), params.verbosity) && std::isfinite(currentError)); + currentError, newError, params.verbosity) && std::isfinite(currentError)); // Printing if verbose if (params.verbosity >= NonlinearOptimizerParams::TERMINATION) { diff --git a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp index 2f4f21286..1423b473e 100644 --- a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp +++ b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp @@ -41,7 +41,7 @@ struct Cal3Bundler0 : public Cal3Bundler { double v0 = 0) : Cal3Bundler(f, k1, k2, u0, v0) {} Cal3Bundler0 retract(const Vector& d) const { - return Cal3Bundler0(fx() + d(0), k1() + d(1), k2() + d(2), u0(), v0()); + return Cal3Bundler0(fx() + d(0), k1() + d(1), k2() + d(2), px(), py()); } Vector3 localCoordinates(const Cal3Bundler0& T2) const { return T2.vector() - vector(); diff --git a/gtsam/sfm/MFAS.cpp b/gtsam/sfm/MFAS.cpp index 6dccc2dee..4cd983ecd 100644 --- a/gtsam/sfm/MFAS.cpp +++ b/gtsam/sfm/MFAS.cpp @@ -111,10 +111,8 @@ void removeNodeFromGraph(const Key node, graph.erase(node); } -MFAS::MFAS(const std::shared_ptr>& nodes, - const TranslationEdges& relativeTranslations, - const Unit3& projectionDirection) - : nodes_(nodes) { +MFAS::MFAS(const TranslationEdges& relativeTranslations, + const Unit3& projectionDirection) { // Iterate over edges, obtain weights by projecting // their relativeTranslations along the projection direction for (const auto& measurement : relativeTranslations) { diff --git a/gtsam/sfm/MFAS.h b/gtsam/sfm/MFAS.h index 929aa5ff0..3b01122a9 100644 --- a/gtsam/sfm/MFAS.h +++ b/gtsam/sfm/MFAS.h @@ -56,40 +56,28 @@ class MFAS { using TranslationEdges = std::vector>; private: - // pointer to nodes in the graph - const std::shared_ptr> nodes_; - // edges with a direction such that all weights are positive // i.e, edges that originally had negative weights are flipped std::map edgeWeights_; public: /** - * @brief Construct from the nodes in a graph and weighted directed edges + * @brief Construct from the weighted directed edges * between the nodes. Each node is identified by a Key. - * A shared pointer to the nodes is used as input parameter - * because, MFAS ordering is usually used to compute the ordering of a - * large graph that is already stored in memory. It is unnecessary to make a - * copy of the nodes in this class. - * @param nodes: Nodes in the graph * @param edgeWeights: weights of edges in the graph */ - MFAS(const std::shared_ptr> &nodes, - const std::map &edgeWeights) - : nodes_(nodes), edgeWeights_(edgeWeights) {} + MFAS(const std::map &edgeWeights) + : edgeWeights_(edgeWeights) {} /** * @brief Constructor to be used in the context of translation averaging. * Here, the nodes of the graph are cameras in 3D and the edges have a unit * translation direction between them. The weights of the edges is computed by * projecting them along a projection direction. - * @param nodes cameras in the epipolar graph (each camera is identified by a - * Key) * @param relativeTranslations translation directions between the cameras * @param projectionDirection direction in which edges are to be projected */ - MFAS(const std::shared_ptr> &nodes, - const TranslationEdges &relativeTranslations, + MFAS(const TranslationEdges &relativeTranslations, const Unit3 &projectionDirection); /** @@ -99,7 +87,7 @@ class MFAS { std::vector computeOrdering() const; /** - * @brief Computes the "outlier weights" of the graph. We define the outlier + * @brief Computes the outlier weights of the graph. We define the outlier * weight of a edge to be zero if the edge is an inlier and the magnitude of * its edgeWeight if it is an outlier. This function internally calls * computeOrdering and uses the obtained ordering to identify outlier edges. @@ -108,4 +96,6 @@ class MFAS { std::map computeOutlierWeights() const; }; +typedef std::map, double> KeyPairDoubleMap; + } // namespace gtsam diff --git a/gtsam/sfm/tests/testMFAS.cpp b/gtsam/sfm/tests/testMFAS.cpp index 58ea4cc84..362027d5d 100644 --- a/gtsam/sfm/tests/testMFAS.cpp +++ b/gtsam/sfm/tests/testMFAS.cpp @@ -6,7 +6,7 @@ */ #include -#include + #include using namespace std; @@ -39,14 +39,13 @@ map getEdgeWeights(const vector &edges, for (size_t i = 0; i < edges.size(); i++) { edgeWeights[edges[i]] = weights[i]; } - cout << "returning edge weights " << edgeWeights.size() << endl; return edgeWeights; } // test the ordering and the outlierWeights function using weights2 - outlier // edge is rejected when projected in a direction that gives weights2 TEST(MFAS, OrderingWeights2) { - MFAS mfas_obj(make_shared>(nodes), getEdgeWeights(edges, weights2)); + MFAS mfas_obj(getEdgeWeights(edges, weights2)); vector ordered_nodes = mfas_obj.computeOrdering(); @@ -76,7 +75,7 @@ TEST(MFAS, OrderingWeights2) { // weights1 (outlier edge is accepted when projected in a direction that // produces weights1) TEST(MFAS, OrderingWeights1) { - MFAS mfas_obj(make_shared>(nodes), getEdgeWeights(edges, weights1)); + MFAS mfas_obj(getEdgeWeights(edges, weights1)); vector ordered_nodes = mfas_obj.computeOrdering(); diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 270dbeb95..74e074c9e 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -1164,8 +1164,8 @@ bool writeBAL(const string &filename, SfmData &data) { for (size_t k = 0; k < track.number_measurements(); k++) { // for each observation of the 3D point j size_t i = track.measurements[k].first; // camera id - double u0 = data.cameras[i].calibration().u0(); - double v0 = data.cameras[i].calibration().v0(); + double u0 = data.cameras[i].calibration().px(); + double v0 = data.cameras[i].calibration().py(); if (u0 != 0 || v0 != 0) { cout << "writeBAL has not been tested for calibration with nonzero " diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index a8fddcfe4..93bd2b2ee 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -211,16 +211,18 @@ GTSAM_EXPORT GraphAndValues load3D(const std::string& filename); /// A measurement with its camera index typedef std::pair SfmMeasurement; -/// SfmTrack +/// Sift index for SfmTrack typedef std::pair SiftIndex; /// Define the structure for the 3D points struct SfmTrack { SfmTrack(): p(0,0,0) {} + SfmTrack(const gtsam::Point3& pt) : p(pt) {} Point3 p; ///< 3D position of the point float r, g, b; ///< RGB color of the 3D point std::vector measurements; ///< The 2D image projections (id,(u,v)) std::vector siftIndices; + /// Total number of measurements in this track size_t number_measurements() const { return measurements.size(); @@ -233,11 +235,17 @@ struct SfmTrack { SiftIndex siftIndex(size_t idx) const { return siftIndices[idx]; } - Point3 point3() const { + /// Get 3D point + const Point3& point3() const { return p; } + /// Add measurement (camera_idx, Point2) to track + void add_measurement(size_t idx, const gtsam::Point2& m) { + measurements.emplace_back(idx, m); + } }; + /// Define the structure for the camera poses typedef PinholeCamera SfmCamera; @@ -260,6 +268,14 @@ struct SfmData { SfmTrack track(size_t idx) const { return tracks[idx]; } + /// Add a track to SfmData + void add_track(const SfmTrack& t) { + tracks.push_back(t); + } + /// Add a camera to SfmData + void add_camera(const SfmCamera& cam){ + cameras.push_back(cam); + } }; /** diff --git a/gtsam_extra.cmake.in b/gtsam_extra.cmake.in index 01ac00b37..44ba36bd6 100644 --- a/gtsam_extra.cmake.in +++ b/gtsam_extra.cmake.in @@ -7,8 +7,3 @@ set (GTSAM_VERSION_STRING "@GTSAM_VERSION_STRING@") set (GTSAM_USE_TBB @GTSAM_USE_TBB@) set (GTSAM_DEFAULT_ALLOCATOR @GTSAM_DEFAULT_ALLOCATOR@) - -if("@GTSAM_INSTALL_CYTHON_TOOLBOX@") - list(APPEND GTSAM_CYTHON_INSTALL_PATH "@GTSAM_CYTHON_INSTALL_PATH@") - list(APPEND GTSAM_EIGENCY_INSTALL_PATH "@GTSAM_EIGENCY_INSTALL_PATH@") -endif() diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index d40fb0b59..819c51fee 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -19,9 +19,68 @@ #include #include +#include namespace gtsam { +using std::vector; +using PointPairs = vector; + +namespace { +/// Subtract centroids from point pairs. +static PointPairs subtractCentroids(const PointPairs &abPointPairs, + const Point3Pair ¢roids) { + PointPairs d_abPointPairs; + for (const Point3Pair& abPair : abPointPairs) { + Point3 da = abPair.first - centroids.first; + Point3 db = abPair.second - centroids.second; + d_abPointPairs.emplace_back(da, db); + } + return d_abPointPairs; +} + +/// Form inner products x and y and calculate scale. +static const double calculateScale(const PointPairs &d_abPointPairs, + const Rot3 &aRb) { + double x = 0, y = 0; + Point3 da, db; + for (const Point3Pair& d_abPair : d_abPointPairs) { + std::tie(da, db) = d_abPair; + const Vector3 da_prime = aRb * db; + y += da.transpose() * da_prime; + x += da_prime.transpose() * da_prime; + } + const double s = y / x; + return s; +} + +/// Form outer product H. +static Matrix3 calculateH(const PointPairs &d_abPointPairs) { + Matrix3 H = Z_3x3; + for (const Point3Pair& d_abPair : d_abPointPairs) { + H += d_abPair.first * d_abPair.second.transpose(); + } + return H; +} + +/// This method estimates the similarity transform from differences point pairs, given a known or estimated rotation and point centroids. +static Similarity3 align(const PointPairs &d_abPointPairs, const Rot3 &aRb, + const Point3Pair ¢roids) { + const double s = calculateScale(d_abPointPairs, aRb); + const Point3 aTb = (centroids.first - s * (aRb * centroids.second)) / s; + return Similarity3(aRb, aTb, s); +} + +/// This method estimates the similarity transform from point pairs, given a known or estimated rotation. +// Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3 +static Similarity3 alignGivenR(const PointPairs &abPointPairs, + const Rot3 &aRb) { + auto centroids = means(abPointPairs); + auto d_abPointPairs = subtractCentroids(abPointPairs, centroids); + return align(d_abPointPairs, aRb, centroids); +} +} // namespace + Similarity3::Similarity3() : t_(0,0,0), s_(1) { } @@ -54,15 +113,15 @@ bool Similarity3::operator==(const Similarity3& other) const { void Similarity3::print(const std::string& s) const { std::cout << std::endl; std::cout << s; - rotation().print("R:\n"); - std::cout << "t: " << translation().transpose() << "s: " << scale() << std::endl; + rotation().print("\nR:\n"); + std::cout << "t: " << translation().transpose() << " s: " << scale() << std::endl; } Similarity3 Similarity3::identity() { return Similarity3(); } -Similarity3 Similarity3::operator*(const Similarity3& T) const { - return Similarity3(R_ * T.R_, ((1.0 / T.s_) * t_) + R_ * T.t_, s_ * T.s_); +Similarity3 Similarity3::operator*(const Similarity3& S) const { + return Similarity3(R_ * S.R_, ((1.0 / S.s_) * t_) + R_ * S.t_, s_ * S.s_); } Similarity3 Similarity3::inverse() const { @@ -85,11 +144,51 @@ Point3 Similarity3::transformFrom(const Point3& p, // return s_ * q; } +Pose3 Similarity3::transformFrom(const Pose3& T) const { + Rot3 R = R_.compose(T.rotation()); + Point3 t = Point3(s_ * (R_ * T.translation() + t_)); + return Pose3(R, t); +} + Point3 Similarity3::operator*(const Point3& p) const { return transformFrom(p); } -Matrix4 Similarity3::wedge(const Vector7& xi) { +Similarity3 Similarity3::Align(const PointPairs &abPointPairs) { + // Refer to Chapter 3 of + // http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf + if (abPointPairs.size() < 3) + throw std::runtime_error("input should have at least 3 pairs of points"); + auto centroids = means(abPointPairs); + auto d_abPointPairs = subtractCentroids(abPointPairs, centroids); + Matrix3 H = calculateH(d_abPointPairs); + // ClosestTo finds rotation matrix closest to H in Frobenius sense + Rot3 aRb = Rot3::ClosestTo(H); + return align(d_abPointPairs, aRb, centroids); +} + +Similarity3 Similarity3::Align(const vector &abPosePairs) { + const size_t n = abPosePairs.size(); + if (n < 2) + throw std::runtime_error("input should have at least 2 pairs of poses"); + + // calculate rotation + vector rotations; + PointPairs abPointPairs; + rotations.reserve(n); + abPointPairs.reserve(n); + Pose3 wTa, wTb; + for (const Pose3Pair &abPair : abPosePairs) { + std::tie(wTa, wTb) = abPair; + rotations.emplace_back(wTa.rotation().compose(wTb.rotation().inverse())); + abPointPairs.emplace_back(wTa.translation(), wTb.translation()); + } + const Rot3 aRb = FindKarcherMean(rotations); + + return alignGivenR(abPointPairs, aRb); +} + +Matrix4 Similarity3::wedge(const Vector7 &xi) { // http://www.ethaneade.org/latex2html/lie/node29.html const auto w = xi.head<3>(); const auto u = xi.segment<3>(3); @@ -128,12 +227,13 @@ Matrix3 Similarity3::GetV(Vector3 w, double lambda) { W = 1.0 / 24.0 - theta2 / 720.0; } const double lambda2 = lambda * lambda, lambda3 = lambda2 * lambda; + const double expMinLambda = exp(-lambda); double A, alpha = 0.0, beta, mu; if (lambda2 > 1e-9) { - A = (1.0 - exp(-lambda)) / lambda; + A = (1.0 - expMinLambda) / lambda; alpha = 1.0 / (1.0 + theta2 / lambda2); - beta = (exp(-lambda) - 1 + lambda) / lambda2; - mu = (1 - lambda + (0.5 * lambda2) - exp(-lambda)) / lambda3; + beta = (expMinLambda - 1 + lambda) / lambda2; + mu = (1 - lambda + (0.5 * lambda2) - expMinLambda) / lambda3; } else { A = 1.0 - lambda / 2.0 + lambda2 / 6.0; beta = 0.5 - lambda / 6.0 + lambda2 / 24.0 - lambda3 / 120.0; diff --git a/gtsam_unstable/geometry/Similarity3.h b/gtsam_unstable/geometry/Similarity3.h index bf4937ed4..b82862ddb 100644 --- a/gtsam_unstable/geometry/Similarity3.h +++ b/gtsam_unstable/geometry/Similarity3.h @@ -19,10 +19,12 @@ #include #include +#include #include #include #include + namespace gtsam { // Forward declarations @@ -87,7 +89,7 @@ public: GTSAM_UNSTABLE_EXPORT static Similarity3 identity(); /// Composition - GTSAM_UNSTABLE_EXPORT Similarity3 operator*(const Similarity3& T) const; + GTSAM_UNSTABLE_EXPORT Similarity3 operator*(const Similarity3& S) const; /// Return the inverse GTSAM_UNSTABLE_EXPORT Similarity3 inverse() const; @@ -101,9 +103,32 @@ public: OptionalJacobian<3, 7> H1 = boost::none, // OptionalJacobian<3, 3> H2 = boost::none) const; + /** + * Action on a pose T. + * |Rs ts| |R t| |Rs*R Rs*t+ts| + * |0 1/s| * |0 1| = | 0 1/s |, the result is still a Sim3 object. + * To retrieve a Pose3, we normalized the scale value into 1. + * |Rs*R Rs*t+ts| |Rs*R s(Rs*t+ts)| + * | 0 1/s | = | 0 1 | + * + * This group action satisfies the compatibility condition. + * For more details, refer to: https://en.wikipedia.org/wiki/Group_action + */ + GTSAM_UNSTABLE_EXPORT Pose3 transformFrom(const Pose3& T) const; + /** syntactic sugar for transformFrom */ GTSAM_UNSTABLE_EXPORT Point3 operator*(const Point3& p) const; + /** + * Create Similarity3 by aligning at least three point pairs + */ + GTSAM_UNSTABLE_EXPORT static Similarity3 Align(const std::vector& abPointPairs); + + /** + * Create Similarity3 by aligning at least two pose pairs + */ + GTSAM_UNSTABLE_EXPORT static Similarity3 Align(const std::vector& abPosePairs); + /// @} /// @name Lie Group /// @{ @@ -182,8 +207,8 @@ public: /// @name Helper functions /// @{ - /// Calculate expmap and logmap coefficients. private: + /// Calculate expmap and logmap coefficients. static Matrix3 GetV(Vector3 w, double lambda); /// @} diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index d23346896..b985eb374 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -51,6 +51,8 @@ static const Similarity3 T4(R, P, s); static const Similarity3 T5(R, P, 10); static const Similarity3 T6(Rot3(), Point3(1, 1, 0), 2); // Simpler transform +const double degree = M_PI / 180; + //****************************************************************************** TEST(Similarity3, Concepts) { BOOST_CONCEPT_ASSERT((IsGroup)); @@ -255,6 +257,114 @@ TEST(Similarity3, GroupAction) { } } +//****************************************************************************** +// Group action on Pose3 +TEST(Similarity3, GroupActionPose3) { + Similarity3 bSa(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0); + + // Create source poses + Pose3 Ta1(Rot3(), Point3(0, 0, 0)); + Pose3 Ta2(Rot3(-1, 0, 0, 0, -1, 0, 0, 0, 1), Point3(4, 0, 0)); + + // Create destination poses + Pose3 expected_Tb1(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10)); + Pose3 expected_Tb2(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(-4, 6, 10)); + + EXPECT(assert_equal(expected_Tb1, bSa.transformFrom(Ta1))); + EXPECT(assert_equal(expected_Tb2, bSa.transformFrom(Ta2))); +} + +// Test left group action compatibility. +// cSa*Ta = cSb*bSa*Ta +TEST(Similarity3, GroupActionPose3_Compatibility) { + Similarity3 bSa(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0); + Similarity3 cSb(Rot3::Ry(90 * degree), Point3(-10, -4, 0), 3.0); + Similarity3 cSa(Rot3::Ry(270 * degree), Point3(0, 1, -2), 6.0); + + // Create poses + Pose3 Ta1(Rot3(), Point3(0, 0, 0)); + Pose3 Ta2(Rot3(-1, 0, 0, 0, -1, 0, 0, 0, 1), Point3(4, 0, 0)); + Pose3 Tb1(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10)); + Pose3 Tb2(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(-4, 6, 10)); + Pose3 Tc1(Rot3(0, 0, -1, 0, 1, 0, 1, 0, 0), Point3(0, 6, -12)); + Pose3 Tc2(Rot3(0, 0, -1, 0, -1, 0, -1, 0, 0), Point3(0, 6, 12)); + + EXPECT(assert_equal(Tc1, cSb.transformFrom(Tb1))); + EXPECT(assert_equal(Tc2, cSb.transformFrom(Tb2))); + + EXPECT(assert_equal(cSa.transformFrom(Ta1), cSb.transformFrom(Tb1))); + EXPECT(assert_equal(cSa.transformFrom(Ta2), cSb.transformFrom(Tb2))); +} + +//****************************************************************************** +// Align with Point3 Pairs +TEST(Similarity3, AlignPoint3_1) { + Similarity3 expected_aSb(Rot3::Rz(-90 * degree), Point3(3, 4, 5), 2.0); + + Point3 b1(0, 0, 0), b2(3, 0, 0), b3(3, 0, 4); + + Point3Pair ab1(make_pair(expected_aSb.transformFrom(b1), b1)); + Point3Pair ab2(make_pair(expected_aSb.transformFrom(b2), b2)); + Point3Pair ab3(make_pair(expected_aSb.transformFrom(b3), b3)); + + vector correspondences{ab1, ab2, ab3}; + + Similarity3 actual_aSb = Similarity3::Align(correspondences); + EXPECT(assert_equal(expected_aSb, actual_aSb)); +} + +TEST(Similarity3, AlignPoint3_2) { + Similarity3 expected_aSb(Rot3(), Point3(10, 10, 0), 1.0); + + Point3 b1(0, 0, 0), b2(20, 10, 0), b3(10, 20, 0); + + Point3Pair ab1(make_pair(expected_aSb.transformFrom(b1), b1)); + Point3Pair ab2(make_pair(expected_aSb.transformFrom(b2), b2)); + Point3Pair ab3(make_pair(expected_aSb.transformFrom(b3), b3)); + + vector correspondences{ab1, ab2, ab3}; + + Similarity3 actual_aSb = Similarity3::Align(correspondences); + EXPECT(assert_equal(expected_aSb, actual_aSb)); +} + +TEST(Similarity3, AlignPoint3_3) { + Similarity3 expected_aSb(Rot3::RzRyRx(0.3, 0.2, 0.1), Point3(20, 10, 5), 1.0); + + Point3 b1(0, 0, 1), b2(10, 0, 2), b3(20, -10, 30); + + Point3Pair ab1(make_pair(expected_aSb.transformFrom(b1), b1)); + Point3Pair ab2(make_pair(expected_aSb.transformFrom(b2), b2)); + Point3Pair ab3(make_pair(expected_aSb.transformFrom(b3), b3)); + + vector correspondences{ab1, ab2, ab3}; + + Similarity3 actual_aSb = Similarity3::Align(correspondences); + EXPECT(assert_equal(expected_aSb, actual_aSb)); +} + +//****************************************************************************** +// Align with Pose3 Pairs +TEST(Similarity3, AlignPose3) { + Similarity3 expected_aSb(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0); + + // Create source poses + Pose3 Ta1(Rot3(), Point3(0, 0, 0)); + Pose3 Ta2(Rot3(-1, 0, 0, 0, -1, 0, 0, 0, 1), Point3(4, 0, 0)); + + // Create destination poses + Pose3 Tb1(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10)); + Pose3 Tb2(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(-4, 6, 10)); + + Pose3Pair bTa1(make_pair(Tb1, Ta1)); + Pose3Pair bTa2(make_pair(Tb2, Ta2)); + + vector correspondences{bTa1, bTa2}; + + Similarity3 actual_aSb = Similarity3::Align(correspondences); + EXPECT(assert_equal(expected_aSb, actual_aSb)); +} + //****************************************************************************** // Test very simple prior optimization example TEST(Similarity3, Optimization) { diff --git a/gtsam_unstable/gtsam_unstable.i b/gtsam_unstable/gtsam_unstable.i index 05b30bb0b..e18d32b59 100644 --- a/gtsam_unstable/gtsam_unstable.i +++ b/gtsam_unstable/gtsam_unstable.i @@ -5,8 +5,6 @@ // specify the classes from gtsam we are using virtual class gtsam::Value; class gtsam::Vector6; -class gtsam::LieScalar; -class gtsam::LieVector; class gtsam::Point2; class gtsam::Point2Vector; class gtsam::Rot2; @@ -476,14 +474,12 @@ virtual class DGroundConstraint : gtsam::NonlinearFactor { DGroundConstraint(size_t key, Vector constraint, const gtsam::noiseModel::Base* model); }; -#include - #include virtual class VelocityConstraint3 : gtsam::NonlinearFactor { /** Standard constructor */ VelocityConstraint3(size_t key1, size_t key2, size_t velKey, double dt); - Vector evaluateError(const gtsam::LieScalar& x1, const gtsam::LieScalar& x2, const gtsam::LieScalar& v) const; + Vector evaluateError(const double& x1, const double& x2, const double& v) const; }; #include @@ -491,7 +487,7 @@ virtual class PendulumFactor1 : gtsam::NonlinearFactor { /** Standard constructor */ PendulumFactor1(size_t k1, size_t k, size_t velKey, double dt); - Vector evaluateError(const gtsam::LieScalar& qk1, const gtsam::LieScalar& qk, const gtsam::LieScalar& v) const; + Vector evaluateError(const double& qk1, const double& qk, const double& v) const; }; #include @@ -499,21 +495,21 @@ virtual class PendulumFactor2 : gtsam::NonlinearFactor { /** Standard constructor */ PendulumFactor2(size_t vk1, size_t vk, size_t qKey, double dt, double L, double g); - Vector evaluateError(const gtsam::LieScalar& vk1, const gtsam::LieScalar& vk, const gtsam::LieScalar& q) const; + Vector evaluateError(const double& vk1, const double& vk, const double& q) const; }; virtual class PendulumFactorPk : gtsam::NonlinearFactor { /** Standard constructor */ PendulumFactorPk(size_t pk, size_t qk, size_t qk1, double h, double m, double r, double g, double alpha); - Vector evaluateError(const gtsam::LieScalar& pk, const gtsam::LieScalar& qk, const gtsam::LieScalar& qk1) const; + Vector evaluateError(const double& pk, const double& qk, const double& qk1) const; }; virtual class PendulumFactorPk1 : gtsam::NonlinearFactor { /** Standard constructor */ PendulumFactorPk1(size_t pk1, size_t qk, size_t qk1, double h, double m, double r, double g, double alpha); - Vector evaluateError(const gtsam::LieScalar& pk1, const gtsam::LieScalar& qk, const gtsam::LieScalar& qk1) const; + Vector evaluateError(const double& pk1, const double& qk, const double& qk1) const; }; #include diff --git a/gtsam_unstable/linear/ActiveSetSolver-inl.h b/gtsam_unstable/linear/ActiveSetSolver-inl.h index 602012090..12374ac76 100644 --- a/gtsam_unstable/linear/ActiveSetSolver-inl.h +++ b/gtsam_unstable/linear/ActiveSetSolver-inl.h @@ -149,7 +149,7 @@ Template JacobianFactor::shared_ptr This::createDualFactor( // to compute the least-square approximation of dual variables return boost::make_shared(Aterms, b); } else { - return boost::make_shared(); + return nullptr; } } @@ -165,14 +165,13 @@ Template JacobianFactor::shared_ptr This::createDualFactor( * if lambda = 0 you are on the constraint * if lambda > 0 you are violating the constraint. */ -Template GaussianFactorGraph::shared_ptr This::buildDualGraph( +Template GaussianFactorGraph This::buildDualGraph( const InequalityFactorGraph& workingSet, const VectorValues& delta) const { - GaussianFactorGraph::shared_ptr dualGraph(new GaussianFactorGraph()); + GaussianFactorGraph dualGraph; for (Key key : constrainedKeys_) { // Each constrained key becomes a factor in the dual graph - JacobianFactor::shared_ptr dualFactor = - createDualFactor(key, workingSet, delta); - if (!dualFactor->empty()) dualGraph->push_back(dualFactor); + auto dualFactor = createDualFactor(key, workingSet, delta); + if (dualFactor) dualGraph.push_back(dualFactor); } return dualGraph; } @@ -193,19 +192,16 @@ This::buildWorkingGraph(const InequalityFactorGraph& workingSet, Template typename This::State This::iterate( const typename This::State& state) const { // Algorithm 16.3 from Nocedal06book. - // Solve with the current working set eqn 16.39, but instead of solving for p - // solve for x - GaussianFactorGraph workingGraph = - buildWorkingGraph(state.workingSet, state.values); + // Solve with the current working set eqn 16.39, but solve for x not p + auto workingGraph = buildWorkingGraph(state.workingSet, state.values); VectorValues newValues = workingGraph.optimize(); // If we CAN'T move further // if p_k = 0 is the original condition, modified by Duy to say that the state // update is zero. if (newValues.equals(state.values, 1e-7)) { // Compute lambda from the dual graph - GaussianFactorGraph::shared_ptr dualGraph = buildDualGraph(state.workingSet, - newValues); - VectorValues duals = dualGraph->optimize(); + auto dualGraph = buildDualGraph(state.workingSet, newValues); + VectorValues duals = dualGraph.optimize(); int leavingFactor = identifyLeavingConstraint(state.workingSet, duals); // If all inequality constraints are satisfied: We have the solution!! if (leavingFactor < 0) { diff --git a/gtsam_unstable/linear/ActiveSetSolver.h b/gtsam_unstable/linear/ActiveSetSolver.h index 8c3c5a7e5..318912cf3 100644 --- a/gtsam_unstable/linear/ActiveSetSolver.h +++ b/gtsam_unstable/linear/ActiveSetSolver.h @@ -154,8 +154,8 @@ protected: public: /// Just for testing... /// Builds a dual graph from the current working set. - GaussianFactorGraph::shared_ptr buildDualGraph( - const InequalityFactorGraph& workingSet, const VectorValues& delta) const; + GaussianFactorGraph buildDualGraph(const InequalityFactorGraph &workingSet, + const VectorValues &delta) const; /** * Build a working graph of cost, equality and active inequality constraints diff --git a/gtsam_unstable/linear/EqualityFactorGraph.h b/gtsam_unstable/linear/EqualityFactorGraph.h index 43befdbe0..fb3f4c076 100644 --- a/gtsam_unstable/linear/EqualityFactorGraph.h +++ b/gtsam_unstable/linear/EqualityFactorGraph.h @@ -31,6 +31,11 @@ class EqualityFactorGraph: public FactorGraph { public: typedef boost::shared_ptr shared_ptr; + /// Add a linear inequality, forwards arguments to LinearInequality. + template void add(Args &&... args) { + emplace_shared(std::forward(args)...); + } + /// Compute error of a guess. double error(const VectorValues& x) const { double total_error = 0.; diff --git a/gtsam_unstable/linear/InequalityFactorGraph.h b/gtsam_unstable/linear/InequalityFactorGraph.h index c87645697..d042b0436 100644 --- a/gtsam_unstable/linear/InequalityFactorGraph.h +++ b/gtsam_unstable/linear/InequalityFactorGraph.h @@ -47,6 +47,11 @@ public: return Base::equals(other, tol); } + /// Add a linear inequality, forwards arguments to LinearInequality. + template void add(Args &&... args) { + emplace_shared(std::forward(args)...); + } + /** * Compute error of a guess. * Infinity error if it violates an inequality; zero otherwise. */ diff --git a/gtsam_unstable/linear/LPInitSolver.h b/gtsam_unstable/linear/LPInitSolver.h index 4eb672fbc..14e5fb000 100644 --- a/gtsam_unstable/linear/LPInitSolver.h +++ b/gtsam_unstable/linear/LPInitSolver.h @@ -21,7 +21,6 @@ #include #include -#include namespace gtsam { /** @@ -83,7 +82,7 @@ private: const InequalityFactorGraph& inequalities) const; // friend class for unit-testing private methods - FRIEND_TEST(LPInitSolver, initialization); + friend class LPInitSolverInitializationTest; }; } diff --git a/gtsam_unstable/linear/tests/testLPSolver.cpp b/gtsam_unstable/linear/tests/testLPSolver.cpp index a105a39f0..53c8c7618 100644 --- a/gtsam_unstable/linear/tests/testLPSolver.cpp +++ b/gtsam_unstable/linear/tests/testLPSolver.cpp @@ -16,21 +16,23 @@ * @author Duy-Nguyen Ta */ +#include +#include + #include -#include #include -#include +#include #include +#include #include #include #include + #include + #include #include -#include -#include - using namespace std; using namespace gtsam; using namespace gtsam::symbol_shorthand; @@ -47,37 +49,27 @@ static const Vector kOne = Vector::Ones(1), kZero = Vector::Zero(1); */ LP simpleLP1() { LP lp; - lp.cost = LinearCost(1, Vector2(-1., -1.)); // min -x1-x2 (max x1+x2) - lp.inequalities.push_back( - LinearInequality(1, Vector2(-1, 0), 0, 1)); // x1 >= 0 - lp.inequalities.push_back( - LinearInequality(1, Vector2(0, -1), 0, 2)); // x2 >= 0 - lp.inequalities.push_back( - LinearInequality(1, Vector2(1, 2), 4, 3)); // x1 + 2*x2 <= 4 - lp.inequalities.push_back( - LinearInequality(1, Vector2(4, 2), 12, 4)); // 4x1 + 2x2 <= 12 - lp.inequalities.push_back( - LinearInequality(1, Vector2(-1, 1), 1, 5)); // -x1 + x2 <= 1 + lp.cost = LinearCost(1, Vector2(-1., -1.)); // min -x1-x2 (max x1+x2) + lp.inequalities.add(1, Vector2(-1, 0), 0, 1); // x1 >= 0 + lp.inequalities.add(1, Vector2(0, -1), 0, 2); // x2 >= 0 + lp.inequalities.add(1, Vector2(1, 2), 4, 3); // x1 + 2*x2 <= 4 + lp.inequalities.add(1, Vector2(4, 2), 12, 4); // 4x1 + 2x2 <= 12 + lp.inequalities.add(1, Vector2(-1, 1), 1, 5); // -x1 + x2 <= 1 return lp; } /* ************************************************************************* */ namespace gtsam { -TEST(LPInitSolver, infinite_loop_single_var) { - LP initchecker; - initchecker.cost = LinearCost(1, Vector3(0, 0, 1)); // min alpha - initchecker.inequalities.push_back( - LinearInequality(1, Vector3(-2, -1, -1), -2, 1)); //-2x-y-alpha <= -2 - initchecker.inequalities.push_back( - LinearInequality(1, Vector3(-1, 2, -1), 6, 2)); // -x+2y-alpha <= 6 - initchecker.inequalities.push_back( - LinearInequality(1, Vector3(-1, 0, -1), 0, 3)); // -x - alpha <= 0 - initchecker.inequalities.push_back( - LinearInequality(1, Vector3(1, 0, -1), 20, 4)); // x - alpha <= 20 - initchecker.inequalities.push_back( - LinearInequality(1, Vector3(0, -1, -1), 0, 5)); // -y - alpha <= 0 - LPSolver solver(initchecker); +TEST(LPInitSolver, InfiniteLoopSingleVar) { + LP lp; + lp.cost = LinearCost(1, Vector3(0, 0, 1)); // min alpha + lp.inequalities.add(1, Vector3(-2, -1, -1), -2, 1); //-2x-y-a <= -2 + lp.inequalities.add(1, Vector3(-1, 2, -1), 6, 2); // -x+2y-a <= 6 + lp.inequalities.add(1, Vector3(-1, 0, -1), 0, 3); // -x - a <= 0 + lp.inequalities.add(1, Vector3(1, 0, -1), 20, 4); // x - a <= 20 + lp.inequalities.add(1, Vector3(0, -1, -1), 0, 5); // -y - a <= 0 + LPSolver solver(lp); VectorValues starter; starter.insert(1, Vector3(0, 0, 2)); VectorValues results, duals; @@ -87,25 +79,23 @@ TEST(LPInitSolver, infinite_loop_single_var) { CHECK(assert_equal(results, expected, 1e-7)); } -TEST(LPInitSolver, infinite_loop_multi_var) { - LP initchecker; +TEST(LPInitSolver, InfiniteLoopMultiVar) { + LP lp; Key X = symbol('X', 1); Key Y = symbol('Y', 1); Key Z = symbol('Z', 1); - initchecker.cost = LinearCost(Z, kOne); // min alpha - initchecker.inequalities.push_back( - LinearInequality(X, -2.0 * kOne, Y, -1.0 * kOne, Z, -1.0 * kOne, -2, - 1)); //-2x-y-alpha <= -2 - initchecker.inequalities.push_back( - LinearInequality(X, -1.0 * kOne, Y, 2.0 * kOne, Z, -1.0 * kOne, 6, - 2)); // -x+2y-alpha <= 6 - initchecker.inequalities.push_back(LinearInequality( - X, -1.0 * kOne, Z, -1.0 * kOne, 0, 3)); // -x - alpha <= 0 - initchecker.inequalities.push_back(LinearInequality( - X, 1.0 * kOne, Z, -1.0 * kOne, 20, 4)); // x - alpha <= 20 - initchecker.inequalities.push_back(LinearInequality( - Y, -1.0 * kOne, Z, -1.0 * kOne, 0, 5)); // -y - alpha <= 0 - LPSolver solver(initchecker); + lp.cost = LinearCost(Z, kOne); // min alpha + lp.inequalities.add(X, -2.0 * kOne, Y, -1.0 * kOne, Z, -1.0 * kOne, -2, + 1); //-2x-y-alpha <= -2 + lp.inequalities.add(X, -1.0 * kOne, Y, 2.0 * kOne, Z, -1.0 * kOne, 6, + 2); // -x+2y-alpha <= 6 + lp.inequalities.add(X, -1.0 * kOne, Z, -1.0 * kOne, 0, + 3); // -x - alpha <= 0 + lp.inequalities.add(X, 1.0 * kOne, Z, -1.0 * kOne, 20, + 4); // x - alpha <= 20 + lp.inequalities.add(Y, -1.0 * kOne, Z, -1.0 * kOne, 0, + 5); // -y - alpha <= 0 + LPSolver solver(lp); VectorValues starter; starter.insert(X, kZero); starter.insert(Y, kZero); @@ -119,7 +109,7 @@ TEST(LPInitSolver, infinite_loop_multi_var) { CHECK(assert_equal(results, expected, 1e-7)); } -TEST(LPInitSolver, initialization) { +TEST(LPInitSolver, Initialization) { LP lp = simpleLP1(); LPInitSolver initSolver(lp); @@ -138,19 +128,19 @@ TEST(LPInitSolver, initialization) { LP::shared_ptr initLP = initSolver.buildInitialLP(yKey); LP expectedInitLP; expectedInitLP.cost = LinearCost(yKey, kOne); - expectedInitLP.inequalities.push_back(LinearInequality( - 1, Vector2(-1, 0), 2, Vector::Constant(1, -1), 0, 1)); // -x1 - y <= 0 - expectedInitLP.inequalities.push_back(LinearInequality( - 1, Vector2(0, -1), 2, Vector::Constant(1, -1), 0, 2)); // -x2 - y <= 0 - expectedInitLP.inequalities.push_back( - LinearInequality(1, Vector2(1, 2), 2, Vector::Constant(1, -1), 4, - 3)); // x1 + 2*x2 - y <= 4 - expectedInitLP.inequalities.push_back( - LinearInequality(1, Vector2(4, 2), 2, Vector::Constant(1, -1), 12, - 4)); // 4x1 + 2x2 - y <= 12 - expectedInitLP.inequalities.push_back( - LinearInequality(1, Vector2(-1, 1), 2, Vector::Constant(1, -1), 1, - 5)); // -x1 + x2 - y <= 1 + expectedInitLP.inequalities.add(1, Vector2(-1, 0), 2, Vector::Constant(1, -1), + 0, 1); // -x1 - y <= 0 + expectedInitLP.inequalities.add(1, Vector2(0, -1), 2, Vector::Constant(1, -1), + 0, 2); // -x2 - y <= 0 + expectedInitLP.inequalities.add(1, Vector2(1, 2), 2, Vector::Constant(1, -1), + 4, + 3); // x1 + 2*x2 - y <= 4 + expectedInitLP.inequalities.add(1, Vector2(4, 2), 2, Vector::Constant(1, -1), + 12, + 4); // 4x1 + 2x2 - y <= 12 + expectedInitLP.inequalities.add(1, Vector2(-1, 1), 2, Vector::Constant(1, -1), + 1, + 5); // -x1 + x2 - y <= 1 CHECK(assert_equal(expectedInitLP, *initLP, 1e-10)); LPSolver lpSolveInit(*initLP); VectorValues xy0(x0); @@ -164,7 +154,7 @@ TEST(LPInitSolver, initialization) { VectorValues x = initSolver.solve(); CHECK(lp.isFeasible(x)); } -} +} // namespace gtsam /* ************************************************************************* */ /** @@ -173,28 +163,24 @@ TEST(LPInitSolver, initialization) { * x - y = 5 * x + 2y = 6 */ -TEST(LPSolver, overConstrainedLinearSystem) { +TEST(LPSolver, OverConstrainedLinearSystem) { GaussianFactorGraph graph; Matrix A1 = Vector3(1, 1, 1); Matrix A2 = Vector3(1, -1, 2); Vector b = Vector3(1, 5, 6); - JacobianFactor factor(1, A1, 2, A2, b, noiseModel::Constrained::All(3)); - graph.push_back(factor); + graph.add(1, A1, 2, A2, b, noiseModel::Constrained::All(3)); VectorValues x = graph.optimize(); // This check confirms that gtsam linear constraint solver can't handle // over-constrained system - CHECK(factor.error(x) != 0.0); + CHECK(graph[0]->error(x) != 0.0); } TEST(LPSolver, overConstrainedLinearSystem2) { GaussianFactorGraph graph; - graph.emplace_shared(1, I_1x1, 2, I_1x1, kOne, - noiseModel::Constrained::All(1)); - graph.emplace_shared(1, I_1x1, 2, -I_1x1, 5 * kOne, - noiseModel::Constrained::All(1)); - graph.emplace_shared(1, I_1x1, 2, 2 * I_1x1, 6 * kOne, - noiseModel::Constrained::All(1)); + graph.add(1, I_1x1, 2, I_1x1, kOne, noiseModel::Constrained::All(1)); + graph.add(1, I_1x1, 2, -I_1x1, 5 * kOne, noiseModel::Constrained::All(1)); + graph.add(1, I_1x1, 2, 2 * I_1x1, 6 * kOne, noiseModel::Constrained::All(1)); VectorValues x = graph.optimize(); // This check confirms that gtsam linear constraint solver can't handle // over-constrained system @@ -202,7 +188,7 @@ TEST(LPSolver, overConstrainedLinearSystem2) { } /* ************************************************************************* */ -TEST(LPSolver, simpleTest1) { +TEST(LPSolver, SimpleTest1) { LP lp = simpleLP1(); LPSolver lpSolver(lp); VectorValues init; @@ -222,7 +208,7 @@ TEST(LPSolver, simpleTest1) { } /* ************************************************************************* */ -TEST(LPSolver, testWithoutInitialValues) { +TEST(LPSolver, TestWithoutInitialValues) { LP lp = simpleLP1(); LPSolver lpSolver(lp); VectorValues result, duals, expectedResult; diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp index 2292c63d7..67a0c971e 100644 --- a/gtsam_unstable/linear/tests/testQPSolver.cpp +++ b/gtsam_unstable/linear/tests/testQPSolver.cpp @@ -17,10 +17,12 @@ * @author Ivan Dario Jimenez */ +#include +#include + #include #include -#include -#include + #include using namespace std; @@ -40,15 +42,15 @@ QP createTestCase() { // Hence, we have G11=2, G12 = -1, g1 = +3, G22 = 2, g2 = 0, f = 10 //TODO: THIS TEST MIGHT BE WRONG : the last parameter might be 5 instead of 10 because the form of the equation // Should be 0.5x'Gx + gx + f : Nocedal 449 - qp.cost.push_back( - HessianFactor(X(1), X(2), 2.0 * I_1x1, -I_1x1, 3.0 * I_1x1, 2.0 * I_1x1, - Z_1x1, 10.0)); + qp.cost.push_back(HessianFactor(X(1), X(2), 2.0 * I_1x1, -I_1x1, 3.0 * I_1x1, + 2.0 * I_1x1, Z_1x1, 10.0)); // Inequality constraints - qp.inequalities.push_back(LinearInequality(X(1), I_1x1, X(2), I_1x1, 2, 0)); // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2 - qp.inequalities.push_back(LinearInequality(X(1), -I_1x1, 0, 1)); // -x1 <= 0 - qp.inequalities.push_back(LinearInequality(X(2), -I_1x1, 0, 2)); // -x2 <= 0 - qp.inequalities.push_back(LinearInequality(X(1), I_1x1, 1.5, 3)); // x1 <= 3/2 + qp.inequalities.add(X(1), I_1x1, X(2), I_1x1, 2, + 0); // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2 + qp.inequalities.add(X(1), -I_1x1, 0, 1); // -x1 <= 0 + qp.inequalities.add(X(2), -I_1x1, 0, 2); // -x2 <= 0 + qp.inequalities.add(X(1), I_1x1, 1.5, 3); // x1 <= 3/2 return qp; } @@ -94,16 +96,15 @@ QP createEqualityConstrainedTest() { // Note the Hessian encodes: // 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f // Hence, we have G11=2, G12 = 0, g1 = 0, G22 = 2, g2 = 0, f = 0 - qp.cost.push_back( - HessianFactor(X(1), X(2), 2.0 * I_1x1, Z_1x1, Z_1x1, 2.0 * I_1x1, Z_1x1, - 0.0)); + qp.cost.push_back(HessianFactor(X(1), X(2), 2.0 * I_1x1, Z_1x1, Z_1x1, + 2.0 * I_1x1, Z_1x1, 0.0)); // Equality constraints // x1 + x2 = 1 --> x1 + x2 -1 = 0, hence we negate the b vector Matrix A1 = I_1x1; Matrix A2 = I_1x1; Vector b = -kOne; - qp.equalities.push_back(LinearEquality(X(1), A1, X(2), A2, b, 0)); + qp.equalities.add(X(1), A1, X(2), A2, b, 0); return qp; } @@ -118,9 +119,8 @@ TEST(QPSolver, dual) { QPSolver solver(qp); - GaussianFactorGraph::shared_ptr dualGraph = solver.buildDualGraph( - qp.inequalities, initialValues); - VectorValues dual = dualGraph->optimize(); + auto dualGraph = solver.buildDualGraph(qp.inequalities, initialValues); + VectorValues dual = dualGraph.optimize(); VectorValues expectedDual; expectedDual.insert(0, (Vector(1) << 2.0).finished()); CHECK(assert_equal(expectedDual, dual, 1e-10)); @@ -135,19 +135,19 @@ TEST(QPSolver, indentifyActiveConstraints) { currentSolution.insert(X(1), Z_1x1); currentSolution.insert(X(2), Z_1x1); - InequalityFactorGraph workingSet = solver.identifyActiveConstraints( - qp.inequalities, currentSolution); + auto workingSet = + solver.identifyActiveConstraints(qp.inequalities, currentSolution); CHECK(!workingSet.at(0)->active()); // inactive - CHECK(workingSet.at(1)->active());// active - CHECK(workingSet.at(2)->active());// active - CHECK(!workingSet.at(3)->active());// inactive + CHECK(workingSet.at(1)->active()); // active + CHECK(workingSet.at(2)->active()); // active + CHECK(!workingSet.at(3)->active()); // inactive VectorValues solution = solver.buildWorkingGraph(workingSet).optimize(); - VectorValues expectedSolution; - expectedSolution.insert(X(1), kZero); - expectedSolution.insert(X(2), kZero); - CHECK(assert_equal(expectedSolution, solution, 1e-100)); + VectorValues expected; + expected.insert(X(1), kZero); + expected.insert(X(2), kZero); + CHECK(assert_equal(expected, solution, 1e-100)); } /* ************************************************************************* */ @@ -159,24 +159,24 @@ TEST(QPSolver, iterate) { currentSolution.insert(X(1), Z_1x1); currentSolution.insert(X(2), Z_1x1); - std::vector expectedSolutions(4), expectedDuals(4); - expectedSolutions[0].insert(X(1), kZero); - expectedSolutions[0].insert(X(2), kZero); + std::vector expected(4), expectedDuals(4); + expected[0].insert(X(1), kZero); + expected[0].insert(X(2), kZero); expectedDuals[0].insert(1, (Vector(1) << 3).finished()); expectedDuals[0].insert(2, kZero); - expectedSolutions[1].insert(X(1), (Vector(1) << 1.5).finished()); - expectedSolutions[1].insert(X(2), kZero); + expected[1].insert(X(1), (Vector(1) << 1.5).finished()); + expected[1].insert(X(2), kZero); expectedDuals[1].insert(3, (Vector(1) << 1.5).finished()); - expectedSolutions[2].insert(X(1), (Vector(1) << 1.5).finished()); - expectedSolutions[2].insert(X(2), (Vector(1) << 0.75).finished()); + expected[2].insert(X(1), (Vector(1) << 1.5).finished()); + expected[2].insert(X(2), (Vector(1) << 0.75).finished()); - expectedSolutions[3].insert(X(1), (Vector(1) << 1.5).finished()); - expectedSolutions[3].insert(X(2), (Vector(1) << 0.5).finished()); + expected[3].insert(X(1), (Vector(1) << 1.5).finished()); + expected[3].insert(X(2), (Vector(1) << 0.5).finished()); - InequalityFactorGraph workingSet = solver.identifyActiveConstraints( - qp.inequalities, currentSolution); + auto workingSet = + solver.identifyActiveConstraints(qp.inequalities, currentSolution); QPSolver::State state(currentSolution, VectorValues(), workingSet, false, 100); @@ -188,12 +188,12 @@ TEST(QPSolver, iterate) { // Forst10book do not follow exactly what we implemented from Nocedal06book. // Specifically, we do not re-identify active constraints and // do not recompute dual variables after every step!!! -// CHECK(assert_equal(expectedSolutions[it], state.values, 1e-10)); -// CHECK(assert_equal(expectedDuals[it], state.duals, 1e-10)); + // CHECK(assert_equal(expected[it], state.values, 1e-10)); + // CHECK(assert_equal(expectedDuals[it], state.duals, 1e-10)); it++; } - CHECK(assert_equal(expectedSolutions[3], state.values, 1e-10)); + CHECK(assert_equal(expected[3], state.values, 1e-10)); } /* ************************************************************************* */ @@ -204,182 +204,161 @@ TEST(QPSolver, optimizeForst10book_pg171Ex5) { VectorValues initialValues; initialValues.insert(X(1), Z_1x1); initialValues.insert(X(2), Z_1x1); - VectorValues solution; - boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues); - VectorValues expectedSolution; - expectedSolution.insert(X(1), (Vector(1) << 1.5).finished()); - expectedSolution.insert(X(2), (Vector(1) << 0.5).finished()); - CHECK(assert_equal(expectedSolution, solution, 1e-100)); + VectorValues solution = solver.optimize(initialValues).first; + VectorValues expected; + expected.insert(X(1), (Vector(1) << 1.5).finished()); + expected.insert(X(2), (Vector(1) << 0.5).finished()); + CHECK(assert_equal(expected, solution, 1e-100)); } pair testParser(QPSParser parser) { QP exampleqp = parser.Parse(); - QP expectedqp; + QP expected; Key X1(Symbol('X', 1)), X2(Symbol('X', 2)); // min f(x,y) = 4 + 1.5x -y + 0.58x^2 + 2xy + 2yx + 10y^2 - expectedqp.cost.push_back( - HessianFactor(X1, X2, 8.0 * I_1x1, 2.0 * I_1x1, -1.5 * kOne, 10.0 * I_1x1, - 2.0 * kOne, 8.0)); - // 2x + y >= 2 - // -x + 2y <= 6 - expectedqp.inequalities.push_back( - LinearInequality(X1, -2.0 * I_1x1, X2, -I_1x1, -2, 0)); - expectedqp.inequalities.push_back( - LinearInequality(X1, -I_1x1, X2, 2.0 * I_1x1, 6, 1)); - // x<= 20 - expectedqp.inequalities.push_back(LinearInequality(X1, I_1x1, 20, 4)); - //x >= 0 - expectedqp.inequalities.push_back(LinearInequality(X1, -I_1x1, 0, 2)); - // y > = 0 - expectedqp.inequalities.push_back(LinearInequality(X2, -I_1x1, 0, 3)); - return std::make_pair(expectedqp, exampleqp); -} -; + expected.cost.push_back(HessianFactor(X1, X2, 8.0 * I_1x1, 2.0 * I_1x1, + -1.5 * kOne, 10.0 * I_1x1, 2.0 * kOne, + 8.0)); + + expected.inequalities.add(X1, -2.0 * I_1x1, X2, -I_1x1, -2, 0); // 2x + y >= 2 + expected.inequalities.add(X1, -I_1x1, X2, 2.0 * I_1x1, 6, 1); // -x + 2y <= 6 + expected.inequalities.add(X1, I_1x1, 20, 4); // x<= 20 + expected.inequalities.add(X1, -I_1x1, 0, 2); // x >= 0 + expected.inequalities.add(X2, -I_1x1, 0, 3); // y > = 0 + return {expected, exampleqp}; +}; TEST(QPSolver, ParserSyntaticTest) { - auto expectedActual = testParser(QPSParser("QPExample.QPS")); - CHECK(assert_equal(expectedActual.first.cost, expectedActual.second.cost, + auto result = testParser(QPSParser("QPExample.QPS")); + CHECK(assert_equal(result.first.cost, result.second.cost, 1e-7)); + CHECK(assert_equal(result.first.inequalities, result.second.inequalities, 1e-7)); - CHECK(assert_equal(expectedActual.first.inequalities, - expectedActual.second.inequalities, 1e-7)); - CHECK(assert_equal(expectedActual.first.equalities, - expectedActual.second.equalities, 1e-7)); + CHECK(assert_equal(result.first.equalities, result.second.equalities, 1e-7)); } TEST(QPSolver, ParserSemanticTest) { - auto expected_actual = testParser(QPSParser("QPExample.QPS")); - VectorValues actualSolution, expectedSolution; - boost::tie(expectedSolution, boost::tuples::ignore) = - QPSolver(expected_actual.first).optimize(); - boost::tie(actualSolution, boost::tuples::ignore) = - QPSolver(expected_actual.second).optimize(); - CHECK(assert_equal(actualSolution, expectedSolution, 1e-7)); + auto result = testParser(QPSParser("QPExample.QPS")); + VectorValues expected = QPSolver(result.first).optimize().first; + VectorValues actual = QPSolver(result.second).optimize().first; + CHECK(assert_equal(actual, expected, 1e-7)); } -TEST(QPSolver, QPExampleTest){ +TEST(QPSolver, QPExampleTest) { QP problem = QPSParser("QPExample.QPS").Parse(); - VectorValues actualSolution; auto solver = QPSolver(problem); - boost::tie(actualSolution, boost::tuples::ignore) = solver.optimize(); - VectorValues expectedSolution; - expectedSolution.insert(Symbol('X',1),0.7625*I_1x1); - expectedSolution.insert(Symbol('X',2),0.4750*I_1x1); - double error_expected = problem.cost.error(expectedSolution); - double error_actual = problem.cost.error(actualSolution); - CHECK(assert_equal(expectedSolution, actualSolution, 1e-7)) + VectorValues actual = solver.optimize().first; + VectorValues expected; + expected.insert(Symbol('X', 1), 0.7625 * I_1x1); + expected.insert(Symbol('X', 2), 0.4750 * I_1x1); + double error_expected = problem.cost.error(expected); + double error_actual = problem.cost.error(actual); + CHECK(assert_equal(expected, actual, 1e-7)) CHECK(assert_equal(error_expected, error_actual)) } TEST(QPSolver, HS21) { QP problem = QPSParser("HS21.QPS").Parse(); - VectorValues actualSolution; - VectorValues expectedSolution; - expectedSolution.insert(Symbol('X',1), 2.0*I_1x1); - expectedSolution.insert(Symbol('X',2), 0.0*I_1x1); - boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize(); - double error_actual = problem.cost.error(actualSolution); + VectorValues expected; + expected.insert(Symbol('X', 1), 2.0 * I_1x1); + expected.insert(Symbol('X', 2), 0.0 * I_1x1); + VectorValues actual = QPSolver(problem).optimize().first; + double error_actual = problem.cost.error(actual); CHECK(assert_equal(-99.9599999, error_actual, 1e-7)) - CHECK(assert_equal(expectedSolution, actualSolution)) + CHECK(assert_equal(expected, actual)) } TEST(QPSolver, HS35) { QP problem = QPSParser("HS35.QPS").Parse(); - VectorValues actualSolution; - boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize(); - double error_actual = problem.cost.error(actualSolution); - CHECK(assert_equal(1.11111111e-01,error_actual, 1e-7)) + VectorValues actual = QPSolver(problem).optimize().first; + double error_actual = problem.cost.error(actual); + CHECK(assert_equal(1.11111111e-01, error_actual, 1e-7)) } TEST(QPSolver, HS35MOD) { QP problem = QPSParser("HS35MOD.QPS").Parse(); - VectorValues actualSolution; - boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize(); - double error_actual = problem.cost.error(actualSolution); - CHECK(assert_equal(2.50000001e-01,error_actual, 1e-7)) + VectorValues actual = QPSolver(problem).optimize().first; + double error_actual = problem.cost.error(actual); + CHECK(assert_equal(2.50000001e-01, error_actual, 1e-7)) } TEST(QPSolver, HS51) { QP problem = QPSParser("HS51.QPS").Parse(); - VectorValues actualSolution; - boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize(); - double error_actual = problem.cost.error(actualSolution); - CHECK(assert_equal(8.88178420e-16,error_actual, 1e-7)) + VectorValues actual = QPSolver(problem).optimize().first; + double error_actual = problem.cost.error(actual); + CHECK(assert_equal(8.88178420e-16, error_actual, 1e-7)) } TEST(QPSolver, HS52) { QP problem = QPSParser("HS52.QPS").Parse(); - VectorValues actualSolution; - boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize(); - double error_actual = problem.cost.error(actualSolution); - CHECK(assert_equal(5.32664756,error_actual, 1e-7)) + VectorValues actual = QPSolver(problem).optimize().first; + double error_actual = problem.cost.error(actual); + CHECK(assert_equal(5.32664756, error_actual, 1e-7)) } -TEST(QPSolver, HS268) { // This test needs an extra order of magnitude of tolerance than the rest +TEST(QPSolver, HS268) { // This test needs an extra order of magnitude of + // tolerance than the rest QP problem = QPSParser("HS268.QPS").Parse(); - VectorValues actualSolution; - boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize(); - double error_actual = problem.cost.error(actualSolution); - CHECK(assert_equal(5.73107049e-07,error_actual, 1e-6)) + VectorValues actual = QPSolver(problem).optimize().first; + double error_actual = problem.cost.error(actual); + CHECK(assert_equal(5.73107049e-07, error_actual, 1e-6)) } TEST(QPSolver, QPTEST) { // REQUIRES Jacobian Fix QP problem = QPSParser("QPTEST.QPS").Parse(); - VectorValues actualSolution; - boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize(); - double error_actual = problem.cost.error(actualSolution); - CHECK(assert_equal(0.437187500e01,error_actual, 1e-7)) + VectorValues actual = QPSolver(problem).optimize().first; + double error_actual = problem.cost.error(actual); + CHECK(assert_equal(0.437187500e01, error_actual, 1e-7)) } /* ************************************************************************* */ -// Create Matlab's test graph as in http://www.mathworks.com/help/optim/ug/quadprog.html +// Create Matlab's test graph as in +// http://www.mathworks.com/help/optim/ug/quadprog.html QP createTestMatlabQPEx() { QP qp; // Objective functions 0.5*x1^2 + x2^2 - x1*x2 - 2*x1 -6*x2 // Note the Hessian encodes: - // 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f + // 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + + // 0.5*f // Hence, we have G11=1, G12 = -1, g1 = +2, G22 = 2, g2 = +6, f = 0 - qp.cost.push_back( - HessianFactor(X(1), X(2), 1.0 * I_1x1, -I_1x1, 2.0 * I_1x1, 2.0 * I_1x1, - 6 * I_1x1, 1000.0)); + qp.cost.push_back(HessianFactor(X(1), X(2), 1.0 * I_1x1, -I_1x1, 2.0 * I_1x1, + 2.0 * I_1x1, 6 * I_1x1, 1000.0)); // Inequality constraints - qp.inequalities.push_back(LinearInequality(X(1), I_1x1, X(2), I_1x1, 2, 0)); // x1 + x2 <= 2 - qp.inequalities.push_back( - LinearInequality(X(1), -I_1x1, X(2), 2 * I_1x1, 2, 1)); //-x1 + 2*x2 <=2 - qp.inequalities.push_back( - LinearInequality(X(1), 2 * I_1x1, X(2), I_1x1, 3, 2)); // 2*x1 + x2 <=3 - qp.inequalities.push_back(LinearInequality(X(1), -I_1x1, 0, 3)); // -x1 <= 0 - qp.inequalities.push_back(LinearInequality(X(2), -I_1x1, 0, 4)); // -x2 <= 0 + qp.inequalities.add(X(1), I_1x1, X(2), I_1x1, 2, 0); // x1 + x2 <= 2 + qp.inequalities.add(X(1), -I_1x1, X(2), 2 * I_1x1, 2, 1); //-x1 + 2*x2 <=2 + qp.inequalities.add(X(1), 2 * I_1x1, X(2), I_1x1, 3, 2); // 2*x1 + x2 <=3 + qp.inequalities.add(X(1), -I_1x1, 0, 3); // -x1 <= 0 + qp.inequalities.add(X(2), -I_1x1, 0, 4); // -x2 <= 0 return qp; } -///* ************************************************************************* */ +///* ************************************************************************* +///*/ TEST(QPSolver, optimizeMatlabEx) { QP qp = createTestMatlabQPEx(); QPSolver solver(qp); VectorValues initialValues; initialValues.insert(X(1), Z_1x1); initialValues.insert(X(2), Z_1x1); - VectorValues solution; - boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues); - VectorValues expectedSolution; - expectedSolution.insert(X(1), (Vector(1) << 2.0 / 3.0).finished()); - expectedSolution.insert(X(2), (Vector(1) << 4.0 / 3.0).finished()); - CHECK(assert_equal(expectedSolution, solution, 1e-7)); + VectorValues solution = solver.optimize(initialValues).first; + VectorValues expected; + expected.insert(X(1), (Vector(1) << 2.0 / 3.0).finished()); + expected.insert(X(2), (Vector(1) << 4.0 / 3.0).finished()); + CHECK(assert_equal(expected, solution, 1e-7)); } -///* ************************************************************************* */ +///* ************************************************************************* +///*/ TEST(QPSolver, optimizeMatlabExNoinitials) { QP qp = createTestMatlabQPEx(); QPSolver solver(qp); - VectorValues solution; - boost::tie(solution, boost::tuples::ignore) = solver.optimize(); - VectorValues expectedSolution; - expectedSolution.insert(X(1), (Vector(1) << 2.0 / 3.0).finished()); - expectedSolution.insert(X(2), (Vector(1) << 4.0 / 3.0).finished()); - CHECK(assert_equal(expectedSolution, solution, 1e-7)); + VectorValues solution = solver.optimize().first; + VectorValues expected; + expected.insert(X(1), (Vector(1) << 2.0 / 3.0).finished()); + expected.insert(X(2), (Vector(1) << 4.0 / 3.0).finished()); + CHECK(assert_equal(expected, solution, 1e-7)); } /* ************************************************************************* */ @@ -387,18 +366,15 @@ TEST(QPSolver, optimizeMatlabExNoinitials) { QP createTestNocedal06bookEx16_4() { QP qp; - qp.cost.push_back(JacobianFactor(X(1), I_1x1, I_1x1)); - qp.cost.push_back(JacobianFactor(X(2), I_1x1, 2.5 * I_1x1)); + qp.cost.add(X(1), I_1x1, I_1x1); + qp.cost.add(X(2), I_1x1, 2.5 * I_1x1); // Inequality constraints - qp.inequalities.push_back( - LinearInequality(X(1), -I_1x1, X(2), 2 * I_1x1, 2, 0)); - qp.inequalities.push_back( - LinearInequality(X(1), I_1x1, X(2), 2 * I_1x1, 6, 1)); - qp.inequalities.push_back( - LinearInequality(X(1), I_1x1, X(2), -2 * I_1x1, 2, 2)); - qp.inequalities.push_back(LinearInequality(X(1), -I_1x1, 0.0, 3)); - qp.inequalities.push_back(LinearInequality(X(2), -I_1x1, 0.0, 4)); + qp.inequalities.add(X(1), -I_1x1, X(2), 2 * I_1x1, 2, 0); + qp.inequalities.add(X(1), I_1x1, X(2), 2 * I_1x1, 6, 1); + qp.inequalities.add(X(1), I_1x1, X(2), -2 * I_1x1, 2, 2); + qp.inequalities.add(X(1), -I_1x1, 0.0, 3); + qp.inequalities.add(X(2), -I_1x1, 0.0, 4); return qp; } @@ -410,21 +386,19 @@ TEST(QPSolver, optimizeNocedal06bookEx16_4) { initialValues.insert(X(1), (Vector(1) << 2.0).finished()); initialValues.insert(X(2), Z_1x1); - VectorValues solution; - boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues); - VectorValues expectedSolution; - expectedSolution.insert(X(1), (Vector(1) << 1.4).finished()); - expectedSolution.insert(X(2), (Vector(1) << 1.7).finished()); - CHECK(assert_equal(expectedSolution, solution, 1e-7)); + VectorValues solution = solver.optimize(initialValues).first; + VectorValues expected; + expected.insert(X(1), (Vector(1) << 1.4).finished()); + expected.insert(X(2), (Vector(1) << 1.7).finished()); + CHECK(assert_equal(expected, solution, 1e-7)); } /* ************************************************************************* */ TEST(QPSolver, failedSubproblem) { QP qp; - qp.cost.push_back(JacobianFactor(X(1), I_2x2, Z_2x1)); + qp.cost.add(X(1), I_2x2, Z_2x1); qp.cost.push_back(HessianFactor(X(1), Z_2x2, Z_2x1, 100.0)); - qp.inequalities.push_back( - LinearInequality(X(1), (Matrix(1, 2) << -1.0, 0.0).finished(), -1.0, 0)); + qp.inequalities.add(X(1), (Matrix(1, 2) << -1.0, 0.0).finished(), -1.0, 0); VectorValues expected; expected.insert(X(1), (Vector(2) << 1.0, 0.0).finished()); @@ -433,8 +407,7 @@ TEST(QPSolver, failedSubproblem) { initialValues.insert(X(1), (Vector(2) << 10.0, 100.0).finished()); QPSolver solver(qp); - VectorValues solution; - boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues); + VectorValues solution = solver.optimize(initialValues).first; CHECK(assert_equal(expected, solution, 1e-7)); } @@ -442,10 +415,9 @@ TEST(QPSolver, failedSubproblem) { /* ************************************************************************* */ TEST(QPSolver, infeasibleInitial) { QP qp; - qp.cost.push_back(JacobianFactor(X(1), I_2x2, Vector::Zero(2))); + qp.cost.add(X(1), I_2x2, Vector::Zero(2)); qp.cost.push_back(HessianFactor(X(1), Z_2x2, Vector::Zero(2), 100.0)); - qp.inequalities.push_back( - LinearInequality(X(1), (Matrix(1, 2) << -1.0, 0.0).finished(), -1.0, 0)); + qp.inequalities.add(X(1), (Matrix(1, 2) << -1.0, 0.0).finished(), -1.0, 0); VectorValues expected; expected.insert(X(1), (Vector(2) << 1.0, 0.0).finished()); @@ -464,4 +436,3 @@ int main() { return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - diff --git a/matlab/CMakeLists.txt b/matlab/CMakeLists.txt index 9abd4e31d..52d56a2b5 100644 --- a/matlab/CMakeLists.txt +++ b/matlab/CMakeLists.txt @@ -2,6 +2,10 @@ include(GtsamMatlabWrap) +# Record the root dir for gtsam - needed during external builds, e.g., ROS +set(GTSAM_SOURCE_ROOT_DIR ${GTSAM_SOURCE_DIR}) +message(STATUS "GTSAM_SOURCE_ROOT_DIR: [${GTSAM_SOURCE_ROOT_DIR}]") + # Tests #message(STATUS "Installing Matlab Toolbox") install_matlab_scripts("${GTSAM_SOURCE_ROOT_DIR}/matlab/" "*.m;*.fig") @@ -21,7 +25,7 @@ install_matlab_scripts("${GTSAM_SOURCE_ROOT_DIR}/matlab/" "README-gtsam-toolbox. file(GLOB matlab_examples_data_graph "${GTSAM_SOURCE_ROOT_DIR}/examples/Data/*.graph") file(GLOB matlab_examples_data_mat "${GTSAM_SOURCE_ROOT_DIR}/examples/Data/*.mat") file(GLOB matlab_examples_data_txt "${GTSAM_SOURCE_ROOT_DIR}/examples/Data/*.txt") -set(matlab_examples_data ${matlab_examples_data_graph} ${matlab_examples_data_mat} ${matlab_examples_data_txt}) +set(matlab_examples_data ${matlab_examples_data_graph} ${matlab_examples_data_mat} ${matlab_examples_data_txt}) if(GTSAM_BUILD_TYPE_POSTFIXES) foreach(build_type ${CMAKE_CONFIGURATION_TYPES}) string(TOUPPER "${build_type}" build_type_upper) @@ -38,4 +42,3 @@ if(GTSAM_BUILD_TYPE_POSTFIXES) else() install(FILES ${matlab_examples_data} DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam_examples/Data) endif() - diff --git a/matlab/README.md b/matlab/README.md index 86e7d9fe0..39053364d 100644 --- a/matlab/README.md +++ b/matlab/README.md @@ -4,16 +4,16 @@ http://borg.cc.gatech.edu/projects/gtsam This is the GTSAM MATLAB toolbox, a MATLAB wrapper around the GTSAM C++ library. To build it, enable `GTSAM_INSTALL_MATLAB_TOOLBOX=ON` in CMake. -The interface to the toolbox is generated automatically by the wrap -tool which directly parses C++ header files. The tool generates matlab proxy objects together with all the native functions for wrapping and unwrapping scalar and non scalar types and objects. The interface generated by the wrap tool also redirects the standard output stream (cout) to matlab's console. +The interface to the toolbox is generated automatically by the wrap tool which directly parses C++ header files. +The tool generates matlab proxy objects together with all the native functions for wrapping and unwrapping scalar and non scalar types and objects. +The interface generated by the wrap tool also redirects the standard output stream (cout) to matlab's console. ## Ubuntu -If you have a newer Ubuntu system (later than 10.04), you must make a small modification to your MATLAB installation, due to MATLAB being distributed with an old version of the C++ standard library. Delete or rename all files starting with `libstdc++` in your MATLAB installation directory, in paths: - - /usr/local/MATLAB/[version]/sys/os/[system]/ - /usr/local/MATLAB/[version]/bin/[system]/ +If you have a newer Ubuntu system (later than 10.04), you must make a small modification to your MATLAB installation, due to MATLAB being distributed with an old version of the C++ standard library. Delete or rename all files starting with `libstdc++` in your MATLAB installation directory, in paths: + /usr/local/MATLAB/[version]/sys/os/[system]/ + /usr/local/MATLAB/[version]/bin/[system]/ ## Adding the toolbox to your MATLAB path @@ -37,25 +37,28 @@ export LD_LIBRARY_PATH=/gtsam:$LD_LIBRARY_PATH ### Linker issues -If you compile the MATLAB toolbox and everything compiles smoothly, but when you run any MATLAB script, you get following error messages in MATLAB +If you compile the MATLAB toolbox and everything compiles smoothly, but when you run any MATLAB script, you get any of the following error messages in MATLAB + ``` Invalid MEX-file '/usr/local/gtsam_toolbox/gtsam_wrapper.mexa64': Missing symbol 'mexAtExit' required by '/usr/local/gtsam_toolbox/gtsam_wrapper.mexa64' Missing symbol 'mexCallMATLABWithObject' required by '/usr/local/gtsam_toolbox/gtsam_wrapper.mexa64' ... ``` + run following shell line + ```sh export LD_PRELOAD=/usr/lib/x86_64-linux-gnu/libstdc++.so.6 ``` -before you run MATLAB from the same shell. + +before you run MATLAB from the same shell. This mainly happens if you have GCC >= 5 and newer version MATLAB like R2017a. - ## Trying out the examples -The examples are located in the 'gtsam_examples' subfolder. You may either run them individually at the MATLAB command line, or open the GTSAM example GUI by running 'gtsamExamples'. Example: +The examples are located in the 'gtsam_examples' subfolder. You may either run them individually at the MATLAB command line, or open the GTSAM example GUI by running 'gtsamExamples'. Example: ```matlab >> cd /Users/yourname/toolbox % Change to wherever you installed the toolbox @@ -65,7 +68,7 @@ The examples are located in the 'gtsam_examples' subfolder. You may either run ## Running the unit tests -The GTSAM MATLAB toolbox also has a small set of unit tests located in the gtsam_tests directory. Example: +The GTSAM MATLAB toolbox also has a small set of unit tests located in the gtsam_tests directory. Example: ```matlab >> cd /Users/yourname/toolbox % Change to wherever you installed the toolbox @@ -86,4 +89,4 @@ Tests complete! ## Writing your own code -Coding for the GTSAM MATLAB toolbox is straightforward and very fast once you understand a few basic concepts! Please see the manual to get started. +Coding for the GTSAM MATLAB toolbox is straightforward and very fast once you understand a few basic concepts! Please see the manual to get started. diff --git a/matlab/gtsam_tests/testSerialization.m b/matlab/gtsam_tests/testSerialization.m index f8b21b7ad..e55822c1c 100644 --- a/matlab/gtsam_tests/testSerialization.m +++ b/matlab/gtsam_tests/testSerialization.m @@ -28,10 +28,6 @@ serialized_pose1 = pose1.string_serialize(); pose1ds = Pose2.string_deserialize(serialized_pose1); CHECK('pose1ds.equals(pose1, 1e-9)', pose1ds.equals(pose1, 1e-9)); -serialized_landmark1 = landmark1.string_serialize(); -landmark1ds = Point2.string_deserialize(serialized_landmark1); -CHECK('landmark1ds.equals(landmark1, 1e-9)', landmark1ds.equals(landmark1, 1e-9)); - %% Create and serialize Values values = Values; values.insert(i1, pose1); diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index bec02fb64..00b537340 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -37,7 +37,8 @@ set(ignore gtsam::Point2Vector gtsam::Pose3Vector gtsam::KeyVector - gtsam::BinaryMeasurementsUnit3) + gtsam::BinaryMeasurementsUnit3 + gtsam::KeyPairDoubleMap) pybind_wrap(gtsam_py # target ${PROJECT_SOURCE_DIR}/gtsam/gtsam.i # interface_header @@ -80,7 +81,9 @@ set(ignore gtsam::Pose3Vector gtsam::KeyVector gtsam::FixedLagSmootherKeyTimestampMapValue - gtsam::BinaryMeasurementsUnit3) + gtsam::BinaryMeasurementsUnit3 + gtsam::KeyPairDoubleMap) + pybind_wrap(gtsam_unstable_py # target ${PROJECT_SOURCE_DIR}/gtsam_unstable/gtsam_unstable.i # interface_header "gtsam_unstable.cpp" # generated_cpp diff --git a/python/gtsam/examples/ImuFactorExample.py b/python/gtsam/examples/ImuFactorExample.py index eec7c5ebd..bb707a8f5 100644 --- a/python/gtsam/examples/ImuFactorExample.py +++ b/python/gtsam/examples/ImuFactorExample.py @@ -104,7 +104,7 @@ class ImuFactorExample(PreintegrationExample): if verbose: print(factor) - print(pim.predict(actual_state_i, self.actualBias)) + print(pim.predict(initial_state_i, self.actualBias)) pim.resetIntegration() @@ -125,7 +125,7 @@ class ImuFactorExample(PreintegrationExample): i += 1 # add priors on end - # self.addPrior(num_poses - 1, graph) + self.addPrior(num_poses - 1, graph) initial.print_("Initial values:") diff --git a/python/gtsam/examples/README.md b/python/gtsam/examples/README.md index e05a0c7e1..2a2c9f2ef 100644 --- a/python/gtsam/examples/README.md +++ b/python/gtsam/examples/README.md @@ -39,7 +39,7 @@ | SelfCalibrationExample | | | SFMdata | | | SFMExample_bal_COLAMD_METIS | | -| SFMExample_bal | | +| SFMExample_bal | :heavy_check_mark: | | SFMExample | :heavy_check_mark: | | SFMExampleExpressions_bal | | | SFMExampleExpressions | | diff --git a/python/gtsam/examples/SFMExample_bal.py b/python/gtsam/examples/SFMExample_bal.py new file mode 100644 index 000000000..dfe8b523c --- /dev/null +++ b/python/gtsam/examples/SFMExample_bal.py @@ -0,0 +1,118 @@ +""" + GTSAM Copyright 2010, Georgia Tech Research Corporation, + Atlanta, Georgia 30332-0415 + All Rights Reserved + Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + See LICENSE for the license information + + Solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file + Author: Frank Dellaert (Python: Akshay Krishnan, John Lambert) +""" + +import argparse +import logging +import sys + +import matplotlib.pyplot as plt +import numpy as np + +import gtsam +from gtsam import ( + GeneralSFMFactorCal3Bundler, + PinholeCameraCal3Bundler, + PriorFactorPinholeCameraCal3Bundler, + readBal, + symbol_shorthand +) + +C = symbol_shorthand.C +P = symbol_shorthand.P + + +logging.basicConfig(stream=sys.stdout, level=logging.DEBUG) + +def run(args): + """ Run LM optimization with BAL input data and report resulting error """ + input_file = gtsam.findExampleDataFile(args.input_file) + + # Load the SfM data from file + scene_data = readBal(input_file) + logging.info(f"read {scene_data.number_tracks()} tracks on {scene_data.number_cameras()} cameras\n") + + # Create a factor graph + graph = gtsam.NonlinearFactorGraph() + + # We share *one* noiseModel between all projection factors + noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v + + # Add measurements to the factor graph + j = 0 + for t_idx in range(scene_data.number_tracks()): + track = scene_data.track(t_idx) # SfmTrack + # retrieve the SfmMeasurement objects + for m_idx in range(track.number_measurements()): + # i represents the camera index, and uv is the 2d measurement + i, uv = track.measurement(m_idx) + # note use of shorthand symbols C and P + graph.add(GeneralSFMFactorCal3Bundler(uv, noise, C(i), P(j))) + j += 1 + + # Add a prior on pose x1. This indirectly specifies where the origin is. + graph.push_back( + gtsam.PriorFactorPinholeCameraCal3Bundler( + C(0), scene_data.camera(0), gtsam.noiseModel.Isotropic.Sigma(9, 0.1) + ) + ) + # Also add a prior on the position of the first landmark to fix the scale + graph.push_back( + gtsam.PriorFactorPoint3( + P(0), scene_data.track(0).point3(), gtsam.noiseModel.Isotropic.Sigma(3, 0.1) + ) + ) + + # Create initial estimate + initial = gtsam.Values() + + i = 0 + # add each PinholeCameraCal3Bundler + for cam_idx in range(scene_data.number_cameras()): + camera = scene_data.camera(cam_idx) + initial.insert(C(i), camera) + i += 1 + + j = 0 + # add each SfmTrack + for t_idx in range(scene_data.number_tracks()): + track = scene_data.track(t_idx) + initial.insert(P(j), track.point3()) + j += 1 + + # Optimize the graph and print results + try: + params = gtsam.LevenbergMarquardtParams() + params.setVerbosityLM("ERROR") + lm = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) + result = lm.optimize() + except Exception as e: + logging.exception("LM Optimization failed") + return + # Error drops from ~2764.22 to ~0.046 + logging.info(f"final error: {graph.error(result)}") + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument( + '-i', + '--input_file', + type=str, + default="dubrovnik-3-7-pre", + help='Read SFM data from the specified BAL file' + 'The data format is described here: https://grail.cs.washington.edu/projects/bal/.' + 'BAL files contain (nrPoses, nrPoints, nrObservations), followed by (i,j,u,v) tuples, ' + 'then (wx,wy,wz,tx,ty,tz,f,k1,k1) as Bundler camera calibrations w/ Rodrigues vector' + 'and (x,y,z) 3d point initializations.' + ) + run(parser.parse_args()) + diff --git a/python/gtsam/examples/TranslationAveragingExample.py b/python/gtsam/examples/TranslationAveragingExample.py new file mode 100644 index 000000000..054b61126 --- /dev/null +++ b/python/gtsam/examples/TranslationAveragingExample.py @@ -0,0 +1,144 @@ +""" +GTSAM Copyright 2010-2018, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved +Authors: Frank Dellaert, et al. (see THANKS for the full author list) + +See LICENSE for the license information + +This example shows how 1dsfm uses outlier rejection (MFAS) and optimization (translation recovery) +together for estimating global translations from relative translation directions and global rotations. +The purpose of this example is to illustrate the connection between these two classes using a small SfM dataset. + +Author: Akshay Krishnan +Date: September 2020 +""" + +from collections import defaultdict +from typing import List, Tuple + +import numpy as np + +import gtsam +from gtsam.examples import SFMdata + +# Hyperparameters for 1dsfm, values used from Kyle Wilson's code. +MAX_1DSFM_PROJECTION_DIRECTIONS = 48 +OUTLIER_WEIGHT_THRESHOLD = 0.1 + + +def get_data() -> Tuple[gtsam.Values, List[gtsam.BinaryMeasurementUnit3]]: + """"Returns global rotations and unit translation directions between 8 cameras + that lie on a circle and face the center. The poses of 8 cameras are obtained from SFMdata + and the unit translations directions between some camera pairs are computed from their + global translations. """ + fx, fy, s, u0, v0 = 50.0, 50.0, 0.0, 50.0, 50.0 + wTc_list = SFMdata.createPoses(gtsam.Cal3_S2(fx, fy, s, u0, v0)) + # Rotations of the cameras in the world frame. + wRc_values = gtsam.Values() + # Normalized translation directions from camera i to camera j + # in the coordinate frame of camera i. + i_iZj_list = [] + for i in range(0, len(wTc_list) - 2): + # Add the rotation. + wRi = wTc_list[i].rotation() + wRc_values.insert(i, wRi) + # Create unit translation measurements with next two poses. + for j in range(i + 1, i + 3): + # Compute the translation from pose i to pose j, in the world reference frame. + w_itj = wTc_list[j].translation() - wTc_list[i].translation() + # Obtain the translation in the camera i's reference frame. + i_itj = wRi.unrotate(w_itj) + # Compute the normalized unit translation direction. + i_iZj = gtsam.Unit3(i_itj) + i_iZj_list.append(gtsam.BinaryMeasurementUnit3( + i, j, i_iZj, gtsam.noiseModel.Isotropic.Sigma(3, 0.01))) + # Add the last two rotations. + wRc_values.insert(len(wTc_list) - 1, wTc_list[-1].rotation()) + wRc_values.insert(len(wTc_list) - 2, wTc_list[-2].rotation()) + return wRc_values, i_iZj_list + + +def filter_outliers(w_iZj_list: gtsam.BinaryMeasurementsUnit3) -> gtsam.BinaryMeasurementsUnit3: + """Removes outliers from a list of Unit3 measurements that are the + translation directions from camera i to camera j in the world frame.""" + + # Indices of measurements that are to be used as projection directions. + # These are randomly chosen. All sampled directions must be unique. + num_directions_to_sample = min( + MAX_1DSFM_PROJECTION_DIRECTIONS, len(w_iZj_list)) + sampled_indices = np.random.choice( + len(w_iZj_list), num_directions_to_sample, replace=False) + + # Sample projection directions from the measurements. + projection_directions = [w_iZj_list[idx].measured() + for idx in sampled_indices] + + outlier_weights = [] + # Find the outlier weights for each direction using MFAS. + for direction in projection_directions: + algorithm = gtsam.MFAS(w_iZj_list, direction) + outlier_weights.append(algorithm.computeOutlierWeights()) + + # Compute average of outlier weights. Each outlier weight is a map from a pair of Keys + # (camera IDs) to a weight, where weights are proportional to the probability of the edge + # being an outlier. + avg_outlier_weights = defaultdict(float) + for outlier_weight_dict in outlier_weights: + for keypair, weight in outlier_weight_dict.items(): + avg_outlier_weights[keypair] += weight / len(outlier_weights) + + # Remove w_iZj that have weight greater than threshold, these are outliers. + w_iZj_inliers = gtsam.BinaryMeasurementsUnit3() + [w_iZj_inliers.append(w_iZj) for w_iZj in w_iZj_list if avg_outlier_weights[( + w_iZj.key1(), w_iZj.key2())] < OUTLIER_WEIGHT_THRESHOLD] + + return w_iZj_inliers + + +def estimate_poses(i_iZj_list: gtsam.BinaryMeasurementsUnit3, + wRc_values: gtsam.Values) -> gtsam.Values: + """Estimate poses given rotations and normalized translation directions between cameras. + + Args: + i_iZj_list: List of normalized translation direction measurements between camera pairs, + Z here refers to measurements. The measurements are of camera j with reference + to camera i (iZj), in camera i's coordinate frame (i_). iZj represents a unit + vector to j in i's frame and is not a transformation. + wRc_values: Rotations of the cameras in the world frame. + + Returns: + gtsam.Values: Estimated poses. + """ + + # Convert the translation direction measurements to world frame using the rotations. + w_iZj_list = gtsam.BinaryMeasurementsUnit3() + for i_iZj in i_iZj_list: + w_iZj = gtsam.Unit3(wRc_values.atRot3(i_iZj.key1()) + .rotate(i_iZj.measured().point3())) + w_iZj_list.append(gtsam.BinaryMeasurementUnit3( + i_iZj.key1(), i_iZj.key2(), w_iZj, i_iZj.noiseModel())) + + # Remove the outliers in the unit translation directions. + w_iZj_inliers = filter_outliers(w_iZj_list) + + # Run the optimizer to obtain translations for normalized directions. + wtc_values = gtsam.TranslationRecovery(w_iZj_inliers).run() + + wTc_values = gtsam.Values() + for key in wRc_values.keys(): + wTc_values.insert(key, gtsam.Pose3( + wRc_values.atRot3(key), wtc_values.atPoint3(key))) + return wTc_values + + +def main(): + wRc_values, i_iZj_list = get_data() + wTc_values = estimate_poses(i_iZj_list, wRc_values) + print("**** Translation averaging output ****") + print(wTc_values) + print("**************************************") + + +if __name__ == '__main__': + main() diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h index 3f6b8fa38..cacad874c 100644 --- a/python/gtsam/specializations.h +++ b/python/gtsam/specializations.h @@ -12,3 +12,4 @@ py::bind_vector py::bind_vector > >(m_, "BinaryMeasurementsUnit3"); py::bind_map(m_, "IndexPairSetMap"); py::bind_vector(m_, "IndexPairVector"); +py::bind_map(m_, "KeyPairDoubleMap"); diff --git a/python/gtsam/tests/test_JacobianFactor.py b/python/gtsam/tests/test_JacobianFactor.py index 6e049ed47..79f512f60 100644 --- a/python/gtsam/tests/test_JacobianFactor.py +++ b/python/gtsam/tests/test_JacobianFactor.py @@ -19,7 +19,7 @@ from gtsam.utils.test_case import GtsamTestCase class TestJacobianFactor(GtsamTestCase): def test_eliminate(self): - # Recommended way to specify a matrix (see cython/README) + # Recommended way to specify a matrix (see python/README) Ax2 = np.array( [[-5., 0.], [+0., -5.], diff --git a/python/gtsam/tests/test_Pose3.py b/python/gtsam/tests/test_Pose3.py index 138f1ff13..e07b904a9 100644 --- a/python/gtsam/tests/test_Pose3.py +++ b/python/gtsam/tests/test_Pose3.py @@ -65,6 +65,14 @@ class TestPose3(GtsamTestCase): actual = Pose3.adjoint_(xi, xi) np.testing.assert_array_equal(actual, expected) + def test_serialization(self): + """Test if serialization is working normally""" + expected = Pose3(Rot3.Ypr(0.0, 1.0, 0.0), Point3(1, 1, 0)) + actual = Pose3() + serialized = expected.serialize() + actual.deserialize(serialized) + self.gtsamAssertEquals(expected, actual, 1e-10) + if __name__ == "__main__": unittest.main() diff --git a/python/gtsam/tests/test_SfmData.py b/python/gtsam/tests/test_SfmData.py new file mode 100644 index 000000000..9c965ddc5 --- /dev/null +++ b/python/gtsam/tests/test_SfmData.py @@ -0,0 +1,79 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Unit tests for testing dataset access. +Author: Frank Dellaert (Python: Sushmita Warrier) +""" +# pylint: disable=invalid-name, no-name-in-module, no-member + +from __future__ import print_function + +import unittest + +import numpy as np + +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestSfmData(GtsamTestCase): + """Tests for SfmData and SfmTrack modules.""" + + def setUp(self): + """Initialize SfmData and SfmTrack""" + self.data = gtsam.SfmData() + # initialize SfmTrack with 3D point + self.tracks = gtsam.SfmTrack() + + def test_tracks(self): + """Test functions in SfmTrack""" + # measurement is of format (camera_idx, imgPoint) + # create arbitrary camera indices for two cameras + i1, i2 = 4,5 + # create arbitrary image measurements for cameras i1 and i2 + uv_i1 = gtsam.Point2(12.6, 82) + # translating point uv_i1 along X-axis + uv_i2 = gtsam.Point2(24.88, 82) + # add measurements to the track + self.tracks.add_measurement(i1, uv_i1) + self.tracks.add_measurement(i2, uv_i2) + # Number of measurements in the track is 2 + self.assertEqual(self.tracks.number_measurements(), 2) + # camera_idx in the first measurement of the track corresponds to i1 + cam_idx, img_measurement = self.tracks.measurement(0) + self.assertEqual(cam_idx, i1) + np.testing.assert_array_almost_equal( + gtsam.Point3(0.,0.,0.), + self.tracks.point3() + ) + + + def test_data(self): + """Test functions in SfmData""" + # Create new track with 3 measurements + i1, i2, i3 = 3,5,6 + uv_i1 = gtsam.Point2(21.23, 45.64) + # translating along X-axis + uv_i2 = gtsam.Point2(45.7, 45.64) + uv_i3 = gtsam.Point2(68.35, 45.64) + # add measurements and arbitrary point to the track + measurements = [(i1, uv_i1), (i2, uv_i2), (i3, uv_i3)] + pt = gtsam.Point3(1.0, 6.0, 2.0) + track2 = gtsam.SfmTrack(pt) + track2.add_measurement(i1, uv_i1) + track2.add_measurement(i2, uv_i2) + track2.add_measurement(i3, uv_i3) + self.data.add_track(self.tracks) + self.data.add_track(track2) + # Number of tracks in SfmData is 2 + self.assertEqual(self.data.number_tracks(), 2) + # camera idx of first measurement of second track corresponds to i1 + cam_idx, img_measurement = self.data.track(1).measurement(0) + self.assertEqual(cam_idx, i1) + +if __name__ == '__main__': + unittest.main() diff --git a/wrap/cmake/PybindWrap.cmake b/wrap/cmake/PybindWrap.cmake index 85f956d50..ff69b5aed 100644 --- a/wrap/cmake/PybindWrap.cmake +++ b/wrap/cmake/PybindWrap.cmake @@ -146,7 +146,7 @@ function(install_python_scripts else() set(build_type_tag "") endif() - # Split up filename to strip trailing '/' in WRAP_CYTHON_INSTALL_PATH if + # Split up filename to strip trailing '/' in GTSAM_PY_INSTALL_PATH if # there is one get_filename_component(location "${dest_directory}" PATH) get_filename_component(name "${dest_directory}" NAME) diff --git a/wrap/matlab_wrapper.py b/wrap/matlab_wrapper.py index 23f56c8b0..1b6b75a49 100755 --- a/wrap/matlab_wrapper.py +++ b/wrap/matlab_wrapper.py @@ -210,18 +210,26 @@ class MatlabWrapper(object): else: formatted_type_name += name - if len(type_name.instantiations) == 1: - if separator == "::": # C++ - formatted_type_name += '<{}>'.format(cls._format_type_name(type_name.instantiations[0], - include_namespace=include_namespace, - constructor=constructor, method=method)) - else: + if separator == "::": # C++ + templates = [] + for idx in range(len(type_name.instantiations)): + template = '{}'.format(cls._format_type_name(type_name.instantiations[idx], + include_namespace=include_namespace, + constructor=constructor, method=method)) + templates.append(template) + + if len(templates) > 0: # If there are no templates + formatted_type_name += '<{}>'.format(','.join(templates)) + + else: + for idx in range(len(type_name.instantiations)): formatted_type_name += '{}'.format(cls._format_type_name( - type_name.instantiations[0], + type_name.instantiations[idx], separator=separator, include_namespace=False, constructor=constructor, method=method )) + return formatted_type_name @classmethod diff --git a/wrap/pybind_wrapper.py b/wrap/pybind_wrapper.py index 3624d06df..326d9be52 100755 --- a/wrap/pybind_wrapper.py +++ b/wrap/pybind_wrapper.py @@ -69,13 +69,13 @@ class PybindWrapper(object): return textwrap.dedent(''' .def("serialize", []({class_inst} self){{ - return gtsam::serialize(self); + return gtsam::serialize(*self); }} ) .def("deserialize", []({class_inst} self, string serialized){{ - return gtsam::deserialize(serialized, self); - }}) + gtsam::deserialize(serialized, *self); + }}, py::arg("serialized")) '''.format(class_inst=cpp_class + '*')) is_method = isinstance(method, instantiator.InstantiatedMethod) diff --git a/wrap/tests/expected-python/geometry_pybind.cpp b/wrap/tests/expected-python/geometry_pybind.cpp index 6e18f83d7..3eee55bf4 100644 --- a/wrap/tests/expected-python/geometry_pybind.cpp +++ b/wrap/tests/expected-python/geometry_pybind.cpp @@ -40,13 +40,13 @@ PYBIND11_MODULE(geometry_py, m_) { .def("vectorConfusion",[](gtsam::Point2* self){return self->vectorConfusion();}) .def("serialize", [](gtsam::Point2* self){ - return gtsam::serialize(self); + return gtsam::serialize(*self); } ) .def("deserialize", [](gtsam::Point2* self, string serialized){ - return gtsam::deserialize(serialized, self); - }) + gtsam::deserialize(serialized, *self); + }, py::arg("serialized")) ; py::class_>(m_gtsam, "Point3") @@ -54,13 +54,13 @@ PYBIND11_MODULE(geometry_py, m_) { .def("norm",[](gtsam::Point3* self){return self->norm();}) .def("serialize", [](gtsam::Point3* self){ - return gtsam::serialize(self); + return gtsam::serialize(*self); } ) .def("deserialize", [](gtsam::Point3* self, string serialized){ - return gtsam::deserialize(serialized, self); - }) + gtsam::deserialize(serialized, *self); + }, py::arg("serialized")) .def_static("staticFunction",[](){return gtsam::Point3::staticFunction();}) .def_static("StaticFunctionRet",[]( double z){return gtsam::Point3::StaticFunctionRet(z);}, py::arg("z"));