diff --git a/.github/ISSUE_TEMPLATE/bug-report.md b/.github/ISSUE_TEMPLATE/bug-report.md new file mode 100644 index 000000000..9f15b2b7c --- /dev/null +++ b/.github/ISSUE_TEMPLATE/bug-report.md @@ -0,0 +1,35 @@ +--- +name: "Bug Report" +about: Submit a bug report to help us improve GTSAM +--- + + + + + + + +## Description + + + +## Steps to reproduce + +1. +2. + + + +## Expected behavior + + + +## Environment + + + + + +## Additional information + + \ No newline at end of file diff --git a/.github/ISSUE_TEMPLATE/feature-request.md b/.github/ISSUE_TEMPLATE/feature-request.md new file mode 100644 index 000000000..e1e13650a --- /dev/null +++ b/.github/ISSUE_TEMPLATE/feature-request.md @@ -0,0 +1,24 @@ +--- +name: "Feature Request" +about: Submit a proposal/request for a new GTSAM feature +--- + +## Feature + + + +## Motivation + + + +## Pitch + + + +## Alternatives + + + +## Additional context + + \ No newline at end of file diff --git a/.github/ISSUE_TEMPLATE/questions-help-support.md b/.github/ISSUE_TEMPLATE/questions-help-support.md new file mode 100644 index 000000000..49dcefbd0 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/questions-help-support.md @@ -0,0 +1,5 @@ +--- +name: "Questions/Help/Support" +--- + +Please post questions and support requests in the [GTSAM Google group](https://groups.google.com/forum/#!forum/gtsam-users) and not on Github. diff --git a/.travis.sh b/.travis.sh index c2feb71f2..3cec20f53 100755 --- a/.travis.sh +++ b/.travis.sh @@ -51,7 +51,7 @@ function build () -DGTSAM_ALLOW_DEPRECATED_SINCE_V4=$GTSAM_ALLOW_DEPRECATED_SINCE_V4 # Actual build: - make -j2 + VERBOSE=1 make -j2 finish } diff --git a/.travis.yml b/.travis.yml index b2d44a9cc..1e2d6760a 100644 --- a/.travis.yml +++ b/.travis.yml @@ -101,9 +101,13 @@ env: script: - bash .travis.sh -t -# Exclude clang on Linux/clang in release until issue #57 is solved matrix: exclude: + # Exclude g++ debug on Linux as it consistently times out + - os: linux + compiler: gcc + env : CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF + # Exclude clang on Linux/clang in release until issue #57 is solved - os: linux compiler: clang env : CMAKE_BUILD_TYPE=Release diff --git a/CMakeLists.txt b/CMakeLists.txt index 85bb70036..202c19842 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,4 +1,3 @@ - project(GTSAM CXX C) cmake_minimum_required(VERSION 3.0) @@ -134,11 +133,11 @@ if(MSVC) # 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 GTSAM_COMPILE_DEFINITIONS_PUBLIC BOOST_ALL_NO_LIB BOOST_ALL_DYN_LINK) + 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 GTSAM_COMPILE_OPTIONS_PRIVATE -Zm295) + list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE -Zm295) endif() endif() @@ -147,7 +146,7 @@ endif() # See: https://bitbucket.org/gtborg/gtsam/issues/417/fail-to-build-on-msvc-2017 # if(MSVC AND BUILD_SHARED_LIBS) - list(APPEND GTSAM_COMPILE_DEFINITIONS_PUBLIC EIGEN_NO_STATIC_ASSERT) + 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. @@ -188,7 +187,7 @@ set(GTSAM_BOOST_LIBRARIES message(STATUS "GTSAM_BOOST_LIBRARIES: ${GTSAM_BOOST_LIBRARIES}") if (GTSAM_DISABLE_NEW_TIMERS) message("WARNING: GTSAM timing instrumentation manually disabled") - list(APPEND GTSAM_COMPILE_DEFINITIONS_PUBLIC DGTSAM_DISABLE_NEW_TIMERS) + list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC DGTSAM_DISABLE_NEW_TIMERS) else() if(Boost_TIMER_LIBRARY) list(APPEND GTSAM_BOOST_LIBRARIES @@ -208,7 +207,7 @@ endif() if(NOT (${Boost_VERSION} LESS 105600)) message("Ignoring Boost restriction on optional lvalue assignment from rvalues") - list(APPEND GTSAM_COMPILE_DEFINITIONS_PUBLIC BOOST_OPTIONAL_ALLOW_BINDING_TO_RVALUES BOOST_OPTIONAL_CONFIG_ALLOW_BINDING_TO_RVALUES) + list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC BOOST_OPTIONAL_ALLOW_BINDING_TO_RVALUES BOOST_OPTIONAL_CONFIG_ALLOW_BINDING_TO_RVALUES) endif() ############################################################################### @@ -276,7 +275,7 @@ 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 - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") + list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC ${OpenMP_CXX_FLAGS}) endif() endif() @@ -351,9 +350,9 @@ endif () if (MSVC) if (BUILD_SHARED_LIBS) # mute eigen static assert to avoid errors in shared lib - list(APPEND GTSAM_COMPILE_DEFINITIONS_PUBLIC DEIGEN_NO_STATIC_ASSERT) + list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC EIGEN_NO_STATIC_ASSERT) endif() - list(APPEND GTSAM_COMPILE_OPTIONS_PRIVATE "/wd4244") # Disable loss of precision which is thrown all over our Eigen + list_append_cache(GTSAM_COMPILE_OPTIONS_PRIVATE "/wd4244") # Disable loss of precision which is thrown all over our Eigen endif() ############################################################################### @@ -395,28 +394,28 @@ elseif("${GTSAM_DEFAULT_ALLOCATOR}" STREQUAL "tcmalloc") endif() if(MSVC) - list(APPEND GTSAM_COMPILE_DEFINITIONS_PRIVATE _CRT_SECURE_NO_WARNINGS _SCL_SECURE_NO_WARNINGS) - list(APPEND GTSAM_COMPILE_OPTIONS_PRIVATE /wd4251 /wd4275 /wd4251 /wd4661 /wd4344 /wd4503) # Disable non-DLL-exported base class and other warnings - list(APPEND GTSAM_COMPILE_OPTIONS_PRIVATE /bigobj) # Allow large object files for template-based code + 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 GTSAM_COMPILE_OPTIONS_PRIVATE -Wno-unused-local-typedefs) + 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 GTSAM_COMPILE_OPTIONS_PRIVATE -Wno-unused-local-typedefs) + 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 GTSAM_COMPILE_DEFINITIONS_PRIVATE GTSAM_EXTRA_CONSISTENCY_CHECKS) + list_append_cache(GTSAM_COMPILE_DEFINITIONS_PRIVATE GTSAM_EXTRA_CONSISTENCY_CHECKS) endif() ############################################################################### @@ -514,6 +513,9 @@ message(STATUS "===============================================================" message(STATUS "================ Configuration Options ======================") message(STATUS " CMAKE_CXX_COMPILER_ID type : ${CMAKE_CXX_COMPILER_ID}") message(STATUS " CMAKE_CXX_COMPILER_VERSION : ${CMAKE_CXX_COMPILER_VERSION}") +message(STATUS " CMake version : ${CMAKE_VERSION}") +message(STATUS " CMake generator : ${CMAKE_GENERATOR}") +message(STATUS " CMake build tool : ${CMAKE_BUILD_TOOL}") message(STATUS "Build flags ") print_config_flag(${GTSAM_BUILD_TESTS} "Build Tests ") print_config_flag(${GTSAM_BUILD_EXAMPLES_ALWAYS} "Build examples with 'make all' ") @@ -527,13 +529,16 @@ if(GTSAM_UNSTABLE_AVAILABLE) print_config_flag(${GTSAM_BUILD_UNSTABLE} "Build libgtsam_unstable ") endif() string(TOUPPER "${CMAKE_BUILD_TYPE}" cmake_build_type_toupper) -print_config_flag(${GTSAM_BUILD_WITH_MARCH_NATIVE} "Build for native architecture ") if(NOT MSVC AND NOT XCODE_VERSION) + print_config_flag(${GTSAM_BUILD_WITH_MARCH_NATIVE} "Build for native architecture ") message(STATUS " Build type : ${CMAKE_BUILD_TYPE}") - message(STATUS " C compilation flags : ${CMAKE_C_FLAGS} ${CMAKE_C_FLAGS_${cmake_build_type_toupper}}") message(STATUS " C++ compilation flags : ${CMAKE_CXX_FLAGS} ${CMAKE_CXX_FLAGS_${cmake_build_type_toupper}}") endif() + +print_build_options_for_target(gtsam) + message(STATUS " Use System Eigen : ${GTSAM_USE_SYSTEM_EIGEN} (Using version: ${GTSAM_EIGEN_VERSION})") + if(GTSAM_USE_TBB) message(STATUS " Use Intel TBB : Yes") elseif(TBB_FOUND) diff --git a/DEVELOP b/DEVELOP deleted file mode 100644 index 483197bc8..000000000 --- a/DEVELOP +++ /dev/null @@ -1,19 +0,0 @@ -Information for developers - -Coding Conventions: - -* Classes are Uppercase, methods and functions lowerMixedCase -* We use a modified K&R Style, with 2-space tabs, inserting spaces for tabs -* Use meaningful variable names, e.g., measurement not msm - - -Windows: - -On Windows it is necessary to explicitly export all functions from the library -which should be externally accessible. To do this, include the macro -GTSAM_EXPORT in your class or function definition. - -For example: -class GTSAM_EXPORT MyClass { ... }; - -GTSAM_EXPORT myFunction(); \ No newline at end of file diff --git a/DEVELOP.md b/DEVELOP.md new file mode 100644 index 000000000..133f3ea11 --- /dev/null +++ b/DEVELOP.md @@ -0,0 +1,19 @@ +# Information for Developers + +### Coding Conventions + +* Classes are Uppercase, methods and functions lowerMixedCase. +* We use a modified K&R Style, with 2-space tabs, inserting spaces for tabs. +* Use meaningful variable names, e.g. `measurement` not `msm`. + + +### Windows + +On Windows it is necessary to explicitly export all functions from the library which should be externally accessible. To do this, include the macro `GTSAM_EXPORT` in your class or function definition. + +For example: +```cpp +class GTSAM_EXPORT MyClass { ... }; + +GTSAM_EXPORT myFunction(); +``` diff --git a/THANKS b/THANKS.md similarity index 100% rename from THANKS rename to THANKS.md diff --git a/USAGE.md b/USAGE.md index 0493db680..f8e462550 100644 --- a/USAGE.md +++ b/USAGE.md @@ -1,48 +1,42 @@ -USAGE - Georgia Tech Smoothing and Mapping library -=================================== -What is this file? +# GTSAM USAGE - This file explains how to make use of the library for common SLAM tasks, - using a visual SLAM implementation as an example. +This file explains how to make use of the library for common SLAM tasks, using a visual SLAM implementation as an example. + +## Getting Started + +### Install - -Getting Started ---------------------------------------------------- -Install: - Follow the installation instructions in the README file to build and - install gtsam, as well as running tests to ensure the library is working - properly. +Follow the installation instructions in the README file to build and install gtsam, as well as running tests to ensure the library is working properly. -Compiling/Linking with gtsam: - The installation creates a binary "libgtsam" at the installation prefix, - and an include folder "gtsam". These are the only required includes, but - the library has also been designed to make use of XML serialization through - the Boost.serialization library, which requires the the Boost.serialization - headers and binaries to be linked. - - If you use CMake for your project, you can use the CMake scripts in the - cmake folder for finding GTSAM, CppUnitLite, and Wrap. +### Compiling/Linking with GTSAM -Examples: - To see how the library works, examine the unit tests provided. +The installation creates a binary `libgtsam` at the installation prefix, and an include folder `gtsam`. These are the only required includes, but the library has also been designed to make use of XML serialization through the `Boost.serialization` library, which requires the the Boost.serialization headers and binaries to be linked. + +If you use CMake for your project, you can use the CMake scripts in the cmake folder for finding `GTSAM`, `CppUnitLite`, and `Wrap`. + +### Examples + +To see how the library works, examine the unit tests provided. +## Overview -Overview ---------------------------------------------------- -The GTSAM library has three primary components necessary for the construction -of factor graph representation and optimization which users will need to -adapt to their particular problem. +The GTSAM library has three primary components necessary for the construction of factor graph representation and optimization which users will need to adapt to their particular problem. + +* FactorGraph + + A factor graph contains a set of variables to solve for (i.e., robot poses, landmark poses, etc.) and a set of constraints between these variables, which make up factors. + +* Values: + + Values is a single object containing labeled values for all of the variables. Currently, all variables are labeled with strings, but the type or organization of the variables can change. + +* Factors -* FactorGraph: - A factor graph contains a set of variables to solve for (i.e., robot poses, landmark poses, etc.) and a set of constraints between these variables, which make up factors. -* Values: - Values is a single object containing labeled values for all of the variables. Currently, all variables are labeled with strings, but the type or organization of the variables can change -* Factors: A nonlinear factor expresses a constraint between variables, which in the SLAM example, is a measurement such as a visual reading on a landmark or odometry. The library is organized according to the following directory structure: - 3rdparty local copies of third party libraries - Eigen3 and CCOLAMD + 3rdparty local copies of third party libraries e.g. Eigen3 and CCOLAMD base provides some base Math and data structures, as well as test-related utilities geometry points, poses, tensors, etc inference core graphical model inference such as factor graphs, junction trees, Bayes nets, Bayes trees diff --git a/bitbucket-pipelines.yml b/bitbucket-pipelines.yml deleted file mode 100644 index 9a94aa86c..000000000 --- a/bitbucket-pipelines.yml +++ /dev/null @@ -1,15 +0,0 @@ -# Built from sample configuration for C++ – Make. -# Check https://confluence.atlassian.com/x/5Q4SMw for more examples. -# ----- -# Our custom docker image from Docker Hub as the build environment. -image: dellaert/ubuntu-boost-tbb-eigen3:bionic - -pipelines: - default: - - step: - script: # Modify the commands below to build your repository. - - mkdir build - - cd build - - cmake -DGTSAM_USE_SYSTEM_EIGEN=OFF -DGTSAM_USE_EIGEN_MKL=OFF .. - - make -j2 - - make -j2 check \ No newline at end of file diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index 0c9a34da1..56dd7dc08 100644 --- a/cmake/GtsamBuildTypes.cmake +++ b/cmake/GtsamBuildTypes.cmake @@ -1,7 +1,45 @@ +# function: list_append_cache(var [new_values ...]) +# Like "list(APPEND ...)" but working for CACHE variables. +# ----------------------------------------------------------- +function(list_append_cache var) + set(cur_value ${${var}}) + list(APPEND cur_value ${ARGN}) + get_property(MYVAR_DOCSTRING CACHE ${var} PROPERTY HELPSTRING) + set(${var} "${cur_value}" CACHE STRING "${MYVAR_DOCSTRING}" FORCE) +endfunction() + +# function: append_config_if_not_empty(TARGET_VARIABLE build_type) +# Auxiliary function used to merge configuration-specific flags into the +# global variables that will actually be send to cmake targets. +# ----------------------------------------------------------- +function(append_config_if_not_empty TARGET_VARIABLE_ build_type) + string(TOUPPER "${build_type}" build_type_toupper) + set(flags_variable_name "${TARGET_VARIABLE_}_${build_type_toupper}") + set(flags_ ${${flags_variable_name}}) + if (NOT "${flags_}" STREQUAL "") + if (${build_type_toupper} STREQUAL "COMMON") + # Special "COMMON" configuration type, just append without CMake expression: + list_append_cache(${TARGET_VARIABLE_} "${flags_}") + else() + # Regular configuration type: + list_append_cache(${TARGET_VARIABLE_} "$<$:${flags_}>") + endif() + endif() +endfunction() + + # Add install prefix to search path list(APPEND CMAKE_PREFIX_PATH "${CMAKE_INSTALL_PREFIX}") + +# Set up build types for MSVC and XCode +set(GTSAM_CMAKE_CONFIGURATION_TYPES Debug Release Timing Profiling RelWithDebInfo MinSizeRel + CACHE STRING "Build types available to MSVC and XCode") +mark_as_advanced(FORCE GTSAM_CMAKE_CONFIGURATION_TYPES) +set(CMAKE_CONFIGURATION_TYPES ${GTSAM_CMAKE_CONFIGURATION_TYPES} CACHE STRING "Build configurations" FORCE) + + # Default to Release mode if(NOT CMAKE_BUILD_TYPE AND NOT MSVC AND NOT XCODE_VERSION) set(GTSAM_CMAKE_BUILD_TYPE "Release" CACHE STRING @@ -13,39 +51,76 @@ endif() # Add option for using build type postfixes to allow installing multiple build modes option(GTSAM_BUILD_TYPE_POSTFIXES "Enable/Disable appending the build type to the name of compiled libraries" ON) -# Set custom compilation flags. -# NOTE: We set all the CACHE variables with a GTSAM prefix, and then set a normal local variable below -# so that we don't "pollute" the global variable namespace in the cmake cache. - # Set all CMAKE_BUILD_TYPE flags: - # (see https://cmake.org/Wiki/CMake_Useful_Variables#Compilers_and_Tools) +# Define all cache variables, to be populated below depending on the OS/compiler: +set(GTSAM_COMPILE_OPTIONS_PRIVATE "" CACHE STRING "(Do not edit) Private compiler flags for all build configurations." FORCE) +set(GTSAM_COMPILE_OPTIONS_PUBLIC "" CACHE STRING "(Do not edit) Public compiler flags (exported to user projects) for all build configurations." FORCE) +set(GTSAM_COMPILE_DEFINITIONS_PRIVATE "" CACHE STRING "(Do not edit) Private preprocessor macros for all build configurations." FORCE) +set(GTSAM_COMPILE_DEFINITIONS_PUBLIC "" CACHE STRING "(Do not edit) Public preprocessor macros for all build configurations." FORCE) +mark_as_advanced(GTSAM_COMPILE_OPTIONS_PRIVATE) +mark_as_advanced(GTSAM_COMPILE_OPTIONS_PUBLIC) +mark_as_advanced(GTSAM_COMPILE_DEFINITIONS_PRIVATE) +mark_as_advanced(GTSAM_COMPILE_DEFINITIONS_PUBLIC) + +foreach(build_type ${GTSAM_CMAKE_CONFIGURATION_TYPES}) + string(TOUPPER "${build_type}" build_type_toupper) + + # Define empty cache variables for "public". "private" are creaed below. + set(GTSAM_COMPILE_OPTIONS_PUBLIC_${build_type_toupper} "" CACHE STRING "(User editable) Public compiler flags (exported to user projects) for `${build_type_toupper}` configuration.") + set(GTSAM_COMPILE_DEFINITIONS_PUBLIC_${build_type_toupper} "" CACHE STRING "(User editable) Public preprocessor macros for `${build_type_toupper}` configuration.") +endforeach() + +# Common preprocessor macros for each configuration: +set(GTSAM_COMPILE_DEFINITIONS_PRIVATE_DEBUG "_DEBUG;EIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "(User editable) Private preprocessor macros for Debug configuration.") +set(GTSAM_COMPILE_DEFINITIONS_PRIVATE_RELWITHDEBINFO "NDEBUG" CACHE STRING "(User editable) Private preprocessor macros for RelWithDebInfo configuration.") +set(GTSAM_COMPILE_DEFINITIONS_PRIVATE_RELEASE "NDEBUG" CACHE STRING "(User editable) Private preprocessor macros for Release configuration.") +set(GTSAM_COMPILE_DEFINITIONS_PRIVATE_PROFILING "NDEBUG" CACHE STRING "(User editable) Private preprocessor macros for Profiling configuration.") +set(GTSAM_COMPILE_DEFINITIONS_PRIVATE_TIMING "NDEBUG;ENABLE_TIMING" CACHE STRING "(User editable) Private preprocessor macros for Timing configuration.") if(MSVC) - set(GTSAM_CMAKE_C_FLAGS "/W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler for all builds.") - set(GTSAM_CMAKE_CXX_FLAGS "/W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler for all builds.") - set(GTSAM_CMAKE_C_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Extra flags used by the compiler during debug builds.") - set(GTSAM_CMAKE_CXX_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Extra flags used by the compiler during debug builds.") - set(GTSAM_CMAKE_C_FLAGS_RELWITHDEBINFO "/MD /O2 /DNDEBUG /Zi /d2Zi+" CACHE STRING "Extra flags used by the compiler during relwithdebinfo builds.") - set(GTSAM_CMAKE_CXX_FLAGS_RELWITHDEBINFO "/MD /O2 /DNDEBUG /Zi /d2Zi+" CACHE STRING "Extra flags used by the compiler during relwithdebinfo builds.") - set(GTSAM_CMAKE_C_FLAGS_RELEASE "/MD /O2 /DNDEBUG" CACHE STRING "Extra flags used by the compiler during release builds.") - set(GTSAM_CMAKE_CXX_FLAGS_RELEASE "/MD /O2 /DNDEBUG" CACHE STRING "Extra flags used by the compiler during release builds.") - set(GTSAM_CMAKE_C_FLAGS_PROFILING "${GTSAM_CMAKE_C_FLAGS_RELEASE} /Zi" CACHE STRING "Extra flags used by the compiler during profiling builds.") - set(GTSAM_CMAKE_CXX_FLAGS_PROFILING "${GTSAM_CMAKE_CXX_FLAGS_RELEASE} /Zi" CACHE STRING "Extra flags used by the compiler during profiling builds.") - set(GTSAM_CMAKE_C_FLAGS_TIMING "${GTSAM_CMAKE_C_FLAGS_RELEASE} /DENABLE_TIMING" CACHE STRING "Extra flags used by the compiler during timing builds.") - set(GTSAM_CMAKE_CXX_FLAGS_TIMING "${GTSAM_CMAKE_CXX_FLAGS_RELEASE} /DENABLE_TIMING" CACHE STRING "Extra flags used by the compiler during timing builds.") -else() - set(GTSAM_CMAKE_C_FLAGS "-std=c11 -Wall" CACHE STRING "Flags used by the compiler for all builds.") - set(GTSAM_CMAKE_CXX_FLAGS "-std=c++11 -Wall" CACHE STRING "Flags used by the compiler for all builds.") - set(GTSAM_CMAKE_C_FLAGS_DEBUG "-g -fno-inline -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Extra flags used by the compiler during debug builds.") - set(GTSAM_CMAKE_CXX_FLAGS_DEBUG "-g -fno-inline -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Extra flags used by the compiler during debug builds.") - set(GTSAM_CMAKE_C_FLAGS_RELWITHDEBINFO "-g -O3 -DNDEBUG" CACHE STRING "Extra flags used by the compiler during relwithdebinfo builds.") - set(GTSAM_CMAKE_CXX_FLAGS_RELWITHDEBINFO "-g -O3 -DNDEBUG" CACHE STRING "Extra flags used by the compiler during relwithdebinfo builds.") - set(GTSAM_CMAKE_C_FLAGS_RELEASE " -O3 -DNDEBUG" CACHE STRING "Extra flags used by the compiler during release builds.") - set(GTSAM_CMAKE_CXX_FLAGS_RELEASE " -O3 -DNDEBUG" CACHE STRING "Extra flags used by the compiler during release builds.") - set(GTSAM_CMAKE_C_FLAGS_PROFILING "${GTSAM_CMAKE_C_FLAGS_RELEASE}" CACHE STRING "Extra flags used by the compiler during profiling builds.") - set(GTSAM_CMAKE_CXX_FLAGS_PROFILING "${GTSAM_CMAKE_CXX_FLAGS_RELEASE}" CACHE STRING "Extra flags used by the compiler during profiling builds.") - set(GTSAM_CMAKE_C_FLAGS_TIMING "${GTSAM_CMAKE_C_FLAGS_RELEASE} -DENABLE_TIMING" CACHE STRING "Extra flags used by the compiler during timing builds.") - set(GTSAM_CMAKE_CXX_FLAGS_TIMING "${GTSAM_CMAKE_CXX_FLAGS_RELEASE} -DENABLE_TIMING" CACHE STRING "Extra flags used by the compiler during timing builds.") + # Common to all configurations: + list_append_cache(GTSAM_COMPILE_DEFINITIONS_PRIVATE WINDOWS_LEAN_AND_MEAN) endif() +# Other (non-preprocessor macros) compiler flags: +if(MSVC) + # Common to all configurations, next for each configuration: + set(GTSAM_COMPILE_OPTIONS_PRIVATE_COMMON /W3 /GR /EHsc /MP CACHE STRING "(User editable) Private compiler flags for all configurations.") + set(GTSAM_COMPILE_OPTIONS_PRIVATE_DEBUG /MDd /Zi /Ob0 /Od /RTC1 CACHE STRING "(User editable) Private compiler flags for Debug configuration.") + set(GTSAM_COMPILE_OPTIONS_PRIVATE_RELWITHDEBINFO /MD /O2 /D /Zi /d2Zi+ CACHE STRING "(User editable) Private compiler flags for RelWithDebInfo configuration.") + set(GTSAM_COMPILE_OPTIONS_PRIVATE_RELEASE /MD /O2 CACHE STRING "(User editable) Private compiler flags for Release configuration.") + set(GTSAM_COMPILE_OPTIONS_PRIVATE_PROFILING /MD /O2 /Zi CACHE STRING "(User editable) Private compiler flags for Profiling configuration.") + set(GTSAM_COMPILE_OPTIONS_PRIVATE_TIMING /MD /O2 CACHE STRING "(User editable) Private compiler flags for Timing configuration.") +else() + # Common to all configurations, next for each configuration: + set(GTSAM_COMPILE_OPTIONS_PRIVATE_COMMON -Wall CACHE STRING "(User editable) Private compiler flags for all configurations.") + set(GTSAM_COMPILE_OPTIONS_PRIVATE_DEBUG -g -fno-inline CACHE STRING "(User editable) Private compiler flags for Debug configuration.") + set(GTSAM_COMPILE_OPTIONS_PRIVATE_RELWITHDEBINFO -g -O3 CACHE STRING "(User editable) Private compiler flags for RelWithDebInfo configuration.") + set(GTSAM_COMPILE_OPTIONS_PRIVATE_RELEASE -O3 CACHE STRING "(User editable) Private compiler flags for Release configuration.") + set(GTSAM_COMPILE_OPTIONS_PRIVATE_PROFILING -O3 CACHE STRING "(User editable) Private compiler flags for Profiling configuration.") + set(GTSAM_COMPILE_OPTIONS_PRIVATE_TIMING -g -O3 CACHE STRING "(User editable) Private compiler flags for Timing configuration.") +endif() + +# Enable C++11: +if (NOT CMAKE_VERSION VERSION_LESS 3.8) + set(GTSAM_COMPILE_FEATURES_PUBLIC "cxx_std_11" CACHE STRING "CMake compile features property for all gtsam targets.") + # See: https://cmake.org/cmake/help/latest/prop_tgt/CXX_EXTENSIONS.html + # This is to enable -std=c++11 instead of -std=g++11 + set(CMAKE_CXX_EXTENSIONS OFF) +else() + # Old cmake versions: + if (NOT MSVC) + list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC $<$:-std=c++11>) + endif() +endif() + +# Merge all user-defined flags into the variables that are to be actually used by CMake: +foreach(build_type "common" ${GTSAM_CMAKE_CONFIGURATION_TYPES}) + append_config_if_not_empty(GTSAM_COMPILE_OPTIONS_PRIVATE ${build_type}) + append_config_if_not_empty(GTSAM_COMPILE_OPTIONS_PUBLIC ${build_type}) + append_config_if_not_empty(GTSAM_COMPILE_DEFINITIONS_PRIVATE ${build_type}) + append_config_if_not_empty(GTSAM_COMPILE_DEFINITIONS_PUBLIC ${build_type}) +endforeach() + +# Linker flags: set(GTSAM_CMAKE_SHARED_LINKER_FLAGS_TIMING "${CMAKE_SHARED_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds.") set(GTSAM_CMAKE_MODULE_LINKER_FLAGS_TIMING "${CMAKE_MODULE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds.") set(GTSAM_CMAKE_EXE_LINKER_FLAGS_TIMING "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds.") @@ -54,26 +129,11 @@ set(GTSAM_CMAKE_SHARED_LINKER_FLAGS_PROFILING "${CMAKE_SHARED_LINKER_FLAGS_RELEA set(GTSAM_CMAKE_MODULE_LINKER_FLAGS_PROFILING "${CMAKE_MODULE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds.") set(GTSAM_CMAKE_EXE_LINKER_FLAGS_PROFILING "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds.") -mark_as_advanced(GTSAM_CMAKE_C_FLAGS_TIMING GTSAM_CMAKE_CXX_FLAGS_TIMING GTSAM_CMAKE_EXE_LINKER_FLAGS_TIMING +mark_as_advanced(GTSAM_CMAKE_EXE_LINKER_FLAGS_TIMING GTSAM_CMAKE_SHARED_LINKER_FLAGS_TIMING GTSAM_CMAKE_MODULE_LINKER_FLAGS_TIMING - GTSAM_CMAKE_C_FLAGS_PROFILING GTSAM_CMAKE_CXX_FLAGS_PROFILING GTSAM_CMAKE_EXE_LINKER_FLAGS_PROFILING + GTSAM_CMAKE_C_FLAGS_PROFILING GTSAM_ GTSAM_CMAKE_EXE_LINKER_FLAGS_PROFILING GTSAM_CMAKE_SHARED_LINKER_FLAGS_PROFILING GTSAM_CMAKE_MODULE_LINKER_FLAGS_PROFILING) -# Apply the gtsam specific build flags as normal variables. This makes it so that they only -# apply to the gtsam part of the build if gtsam is built as a subproject -set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${GTSAM_CMAKE_C_FLAGS}") -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GTSAM_CMAKE_CXX_FLAGS}") -set(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS_DEBUG} ${GTSAM_CMAKE_C_FLAGS_DEBUG}") -set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} ${GTSAM_CMAKE_CXX_FLAGS_DEBUG}") -set(CMAKE_C_FLAGS_RELWITHDEBINFO "${CMAKE_C_FLAGS_RELWITHDEBINFO} ${GTSAM_CMAKE_C_FLAGS_RELWITHDEBINFO}") -set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "${CMAKE_CXX_FLAGS_RELWITHDEBINFO} ${GTSAM_CMAKE_CXX_FLAGS_RELWITHDEBINFO}") -set(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} ${GTSAM_CMAKE_C_FLAGS_RELEASE}") -set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} ${GTSAM_CMAKE_CXX_FLAGS_RELEASE}") -set(CMAKE_C_FLAGS_PROFILING "${CMAKE_C_FLAGS_PROFILING} ${GTSAM_CMAKE_C_FLAGS_PROFILING}") -set(CMAKE_CXX_FLAGS_PROFILING "${CMAKE_CXX_FLAGS_PROFILING} ${GTSAM_CMAKE_CXX_FLAGS_PROFILING}") -set(CMAKE_C_FLAGS_TIMING "${CMAKE_C_FLAGS_TIMING} ${GTSAM_CMAKE_C_FLAGS_TIMING}") -set(CMAKE_CXX_FLAGS_TIMING "${CMAKE_CXX_FLAGS_TIMING} ${GTSAM_CMAKE_CXX_FLAGS_TIMING}") - set(CMAKE_SHARED_LINKER_FLAGS_TIMING ${GTSAM_CMAKE_SHARED_LINKER_FLAGS_TIMING}) set(CMAKE_MODULE_LINKER_FLAGS_TIMING ${GTSAM_CMAKE_MODULE_LINKER_FLAGS_TIMING}) set(CMAKE_EXE_LINKER_FLAGS_TIMING ${GTSAM_CMAKE_EXE_LINKER_FLAGS_TIMING}) @@ -86,15 +146,16 @@ set(CMAKE_EXE_LINKER_FLAGS_PROFILING ${GTSAM_CMAKE_EXE_LINKER_FLAGS_PROFILING}) if(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang") # Apple Clang before 5.0 does not support -ftemplate-depth. if(NOT (APPLE AND "${CMAKE_CXX_COMPILER_VERSION}" VERSION_LESS "5.0")) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -ftemplate-depth=1024") + list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC "-ftemplate-depth=1024") endif() endif() if (NOT MSVC) option(GTSAM_BUILD_WITH_MARCH_NATIVE "Enable/Disable building with all instructions supported by native architecture (binary may not be portable!)" ON) if(GTSAM_BUILD_WITH_MARCH_NATIVE) - set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -march=native") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=native") + # Add as public flag so all dependant projects also use it, as required + # by Eigen to avid crashes due to SIMD vectorization: + list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC "-march=native") endif() endif() @@ -120,12 +181,6 @@ if(NOT "${CMAKE_BUILD_TYPE}" STREQUAL "") endif() endif() -# Set up build types for MSVC and XCode -set(GTSAM_CMAKE_CONFIGURATION_TYPES Debug Release Timing Profiling RelWithDebInfo MinSizeRel - CACHE STRING "Build types available to MSVC and XCode") -mark_as_advanced(FORCE GTSAM_CMAKE_CONFIGURATION_TYPES) -set(CMAKE_CONFIGURATION_TYPES ${GTSAM_CMAKE_CONFIGURATION_TYPES}) - # Check build types string(TOLOWER "${CMAKE_BUILD_TYPE}" cmake_build_type_tolower) if( NOT cmake_build_type_tolower STREQUAL "" @@ -161,3 +216,20 @@ function(gtsam_assign_all_source_folders) gtsam_assign_source_folders("${all_c_srcs};${all_cpp_srcs};${all_headers}") endfunction() +# Applies the per-config build flags to the given target (e.g. gtsam, wrap_lib) +function(gtsam_apply_build_flags target_name_) + # To enable C++11: the use of target_compile_features() is preferred since + # it will be not in conflict with a more modern C++ standard, if used in a + # client program. + if (NOT "${GTSAM_COMPILE_FEATURES_PUBLIC}" STREQUAL "") + target_compile_features(${target_name_} PUBLIC ${GTSAM_COMPILE_FEATURES_PUBLIC}) + endif() + + target_compile_definitions(${target_name_} PRIVATE ${GTSAM_COMPILE_DEFINITIONS_PRIVATE}) + target_compile_definitions(${target_name_} PUBLIC ${GTSAM_COMPILE_DEFINITIONS_PUBLIC}) + if (NOT "${GTSAM_COMPILE_OPTIONS_PUBLIC}" STREQUAL "") + target_compile_options(${target_name_} PUBLIC ${GTSAM_COMPILE_OPTIONS_PUBLIC}) + endif() + target_compile_options(${target_name_} PRIVATE ${GTSAM_COMPILE_OPTIONS_PRIVATE}) + +endfunction(gtsam_apply_build_flags) diff --git a/cmake/GtsamPrinting.cmake b/cmake/GtsamPrinting.cmake index 674fd4086..e53f9c54f 100644 --- a/cmake/GtsamPrinting.cmake +++ b/cmake/GtsamPrinting.cmake @@ -7,4 +7,45 @@ function(print_config_flag flag msg) else () message(STATUS " ${msg}: Disabled") endif () -endfunction(print_config_flag) +endfunction() + +# Based on https://github.com/jimbraun/XCDF/blob/master/cmake/CMakePadString.cmake +function(string_pad RESULT_NAME DESIRED_LENGTH VALUE) + string(LENGTH "${VALUE}" VALUE_LENGTH) + math(EXPR REQUIRED_PADS "${DESIRED_LENGTH} - ${VALUE_LENGTH}") + set(PAD ${VALUE}) + if(REQUIRED_PADS GREATER 0) + math(EXPR REQUIRED_MINUS_ONE "${REQUIRED_PADS} - 1") + foreach(FOO RANGE ${REQUIRED_MINUS_ONE}) + set(PAD "${PAD} ") + endforeach() + endif() + set(${RESULT_NAME} "${PAD}" PARENT_SCOPE) +endfunction() + +set(GTSAM_PRINT_SUMMARY_PADDING_LENGTH 50 CACHE STRING "Padding of cmake summary report lines after configuring.") +mark_as_advanced(GTSAM_PRINT_SUMMARY_PADDING_LENGTH) + +# Print " var: ${var}" padding with spaces as needed +function(print_padded variable_name) + string_pad(padded_prop ${GTSAM_PRINT_SUMMARY_PADDING_LENGTH} " ${variable_name}") + message(STATUS "${padded_prop}: ${${variable_name}}") +endfunction() + + +# Prints all the relevant CMake build options for a given target: +function(print_build_options_for_target target_name_) + print_padded(GTSAM_COMPILE_FEATURES_PUBLIC) + print_padded(GTSAM_COMPILE_OPTIONS_PRIVATE) + print_padded(GTSAM_COMPILE_OPTIONS_PUBLIC) + print_padded(GTSAM_COMPILE_DEFINITIONS_PRIVATE) + print_padded(GTSAM_COMPILE_DEFINITIONS_PUBLIC) + + foreach(build_type ${GTSAM_CMAKE_CONFIGURATION_TYPES}) + string(TOUPPER "${build_type}" build_type_toupper) + print_padded(GTSAM_COMPILE_OPTIONS_PRIVATE_${build_type_toupper}) + print_padded(GTSAM_COMPILE_OPTIONS_PUBLIC_${build_type_toupper}) + print_padded(GTSAM_COMPILE_DEFINITIONS_PRIVATE_${build_type_toupper}) + print_padded(GTSAM_COMPILE_DEFINITIONS_PUBLIC_${build_type_toupper}) + endforeach() +endfunction() diff --git a/cmake/GtsamTesting.cmake b/cmake/GtsamTesting.cmake index d115f395a..3b42ffa21 100644 --- a/cmake/GtsamTesting.cmake +++ b/cmake/GtsamTesting.cmake @@ -169,6 +169,9 @@ macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries) add_executable(${script_name} ${script_src} ${script_headers}) target_link_libraries(${script_name} CppUnitLite ${linkLibraries}) + # Apply user build flags from CMake cache variables: + gtsam_apply_build_flags(${script_name}) + # Add target dependencies add_test(NAME ${script_name} COMMAND ${script_name}) add_dependencies(check.${groupName} ${script_name}) @@ -191,7 +194,7 @@ macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries) endif() # Add TOPSRCDIR - set_property(SOURCE ${script_src} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"") + set_property(SOURCE ${script_src} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${GTSAM_SOURCE_DIR}\"") # Exclude from 'make all' and 'make install' set_target_properties(${script_name} PROPERTIES EXCLUDE_FROM_ALL ON) @@ -213,6 +216,9 @@ macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries) add_executable(${target_name} "${script_srcs}" ${script_headers}) target_link_libraries(${target_name} CppUnitLite ${linkLibraries}) + # Apply user build flags from CMake cache variables: + gtsam_apply_build_flags(${target_name}) + set_property(TARGET check_${groupName}_program PROPERTY FOLDER "Unit tests") # Only have a main function in one script - use preprocessor @@ -229,7 +235,7 @@ macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries) endif() # Add TOPSRCDIR - set_property(SOURCE ${script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"") + set_property(SOURCE ${script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${GTSAM_SOURCE_DIR}\"") # Exclude from 'make all' and 'make install' set_target_properties(${target_name} PROPERTIES EXCLUDE_FROM_ALL ON) @@ -280,6 +286,9 @@ macro(gtsamAddExesGlob_impl globPatterns excludedFiles linkLibraries groupName b add_executable(${script_name} ${script_src} ${script_headers}) target_link_libraries(${script_name} ${linkLibraries}) + # Apply user build flags from CMake cache variables: + gtsam_apply_build_flags(${script_name}) + # Add target dependencies add_dependencies(${groupName} ${script_name}) if(NOT MSVC AND NOT XCODE_VERSION) @@ -287,7 +296,7 @@ macro(gtsamAddExesGlob_impl globPatterns excludedFiles linkLibraries groupName b endif() # Add TOPSRCDIR - set_property(SOURCE ${script_src} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"") + set_property(SOURCE ${script_src} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${GTSAM_SOURCE_DIR}\"") # Exclude from all or not - note weird variable assignment because we're in a macro set(buildWithAll_on ${buildWithAll}) diff --git a/cmake/README.md b/cmake/README.md index 34d1ffb52..7f38bbcf2 100644 --- a/cmake/README.md +++ b/cmake/README.md @@ -1,5 +1,4 @@ -GTSAMCMakeTools -=============== +# GTSAMCMakeTools This is the collection of GTSAM CMake tools that may be useful in external projects. The way to use this collection is by first making a find_package call: @@ -7,8 +6,7 @@ This is the collection of GTSAM CMake tools that may be useful in external proje which will add a directory containing the GTSAM CMake tools to the CMAKE_MODULE_PATH variable. After that, you may include the files you would like to use. These files and the functions they define are explained below. -GtsamBuildTypes ---------------- +## GtsamBuildTypes include(GtsamBuildTypes) @@ -17,8 +15,8 @@ Including this file immediately sets up the following build types and a drop-dow * `Debug` * `Release` * `RelWithDebInfo` -* `Profiling`: All optimizations enabled and minimal debug symbols -* `Timing`: Defines the symbol GTSAM_ENABLE_TIMING for using GTSAM timing instrumentation +* `Profiling`: All optimizations enabled and minimal debug symbols +* `Timing`: Defines the symbol GTSAM_ENABLE_TIMING for using GTSAM timing instrumentation It also configures several minor details, as follows: @@ -30,8 +28,7 @@ It defines the following functions: * `gtsam_assign_source_folders( [files] )` Organizes files in the IDE into folders to reflect the actual directory structure of those files. Folders will be determined relative to the current source folder when this function is called. * `gtsam_assign_all_source_folders()` Calls `gtsam_assign_source_folders` on all cpp, c, and h files recursively in the current source folder. -GtsamTesting ------------- +## GtsamTesting include(GtsamTesting) @@ -70,8 +67,7 @@ Defines two useful functions for creating CTest unit tests. Also immediately cr an empty string "" if nothing needs to be excluded. linkLibraries: The list of libraries to link to. -GtsamMatlabWrap ---------------- +## GtsamMatlabWrap include(GtsamMatlabWrap) @@ -97,8 +93,7 @@ Defines functions for generating MATLAB wrappers. Also immediately creates seve extraMexFlags: Any *additional* flags to pass to the compiler when building the wrap code. Normally, leave this empty. -GtsamMakeConfigFile -------------------- +## GtsamMakeConfigFile include(GtsamMakeConfigFile) diff --git a/cmake/obsolete/GtsamTestingObsolete.cmake b/cmake/obsolete/GtsamTestingObsolete.cmake index f56d138e6..c90abfa6c 100644 --- a/cmake/obsolete/GtsamTestingObsolete.cmake +++ b/cmake/obsolete/GtsamTestingObsolete.cmake @@ -1,22 +1,22 @@ -# Macro for adding categorized tests in a "tests" folder, with +# Macro for adding categorized tests in a "tests" folder, with # optional exclusion of tests and convenience library linking options -# +# # By default, all tests are linked with CppUnitLite and boost -# Arguments: +# Arguments: # - subdir The name of the category for this test # - local_libs A list of convenience libraries to use (if GTSAM_BUILD_CONVENIENCE_LIBRARIES is true) # - full_libs The main library to link against if not using convenience libraries -# - excluded_tests A list of test files that should not be compiled - use for debugging +# - excluded_tests A list of test files that should not be compiled - use for debugging function(gtsam_add_subdir_tests subdir local_libs full_libs excluded_tests) # Subdirectory target for tests add_custom_target(check.${subdir} COMMAND ${CMAKE_CTEST_COMMAND} -C $ --output-on-failure) set(is_test TRUE) # Put check target in Visual Studio solution folder - file(RELATIVE_PATH relative_path "${PROJECT_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}") + file(RELATIVE_PATH relative_path "${GTSAM_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}") set_property(TARGET check.${subdir} PROPERTY FOLDER "${relative_path}") - + # Link with CppUnitLite - pulled from gtsam installation list(APPEND local_libs CppUnitLite) list(APPEND full_libs CppUnitLite) @@ -29,15 +29,15 @@ function(gtsam_add_subdir_tests subdir local_libs full_libs excluded_tests) ${is_test}) # Set all as tests endfunction() -# Macro for adding categorized timing scripts in a "tests" folder, with +# Macro for adding categorized timing scripts in a "tests" folder, with # optional exclusion of tests and convenience library linking options -# +# # By default, all tests are linked with boost -# Arguments: +# Arguments: # - subdir The name of the category for this timing script # - local_libs A list of convenience libraries to use (if GTSAM_BUILD_CONVENIENCE_LIBRARIES is true) # - full_libs The main library to link against if not using convenience libraries -# - excluded_srcs A list of timing files that should not be compiled - use for debugging +# - excluded_srcs A list of timing files that should not be compiled - use for debugging macro(gtsam_add_subdir_timing subdir local_libs full_libs excluded_srcs) # Subdirectory target for timing - does not actually execute the scripts add_custom_target(timing.${subdir}) @@ -60,11 +60,11 @@ endmacro() # - excluded_srcs A list of timing files that should not be compiled - use for debugging function(gtsam_add_executables pattern local_libs full_libs excluded_srcs) set(is_test FALSE) - + if(NOT excluded_srcs) set(excluded_srcs "") endif() - + # Build executables gtsam_add_grouped_scripts("" "${pattern}" "" "Executable" "${local_libs}" "${full_libs}" "${excluded_srcs}" ${is_test}) endfunction() @@ -73,7 +73,7 @@ endfunction() macro(gtsam_add_grouped_scripts group pattern target_prefix pretty_prefix_name local_libs full_libs excluded_srcs is_test) # Print warning about using this obsolete function message(AUTHOR_WARNING "Warning: Please see GtsamTesting.cmake - obsolete cmake cmake macro for creating unit tests, examples, and scripts was called. This will be removed in the future. The new macros are much easier anyway!!") - + # Get all script files set(script_files "") foreach(one_pattern ${pattern}) @@ -102,20 +102,20 @@ macro(gtsam_add_grouped_scripts group pattern target_prefix pretty_prefix_name l list(APPEND script_srcs ${script_file}) endif() endforeach() - - + + # Add targets and dependencies for each script if(NOT "${group}" STREQUAL "") message(STATUS "Adding ${pretty_prefix_name}s in ${group}") endif() - + # Create exe's for each script, unless we're in SINGLE_TEST_EXE mode if(NOT is_test OR NOT GTSAM_SINGLE_TEST_EXE) foreach(script_src ${script_srcs}) get_filename_component(script_base ${script_src} NAME_WE) if (script_base) # Check for null filenames and headers set( script_bin ${script_base} ) - message(STATUS "Adding ${pretty_prefix_name} ${script_bin}") + message(STATUS "Adding ${pretty_prefix_name} ${script_bin}") add_executable(${script_bin} ${script_src} ${script_headers}) if(NOT "${target_prefix}" STREQUAL "") if(NOT "${group}" STREQUAL "") @@ -123,37 +123,37 @@ macro(gtsam_add_grouped_scripts group pattern target_prefix pretty_prefix_name l endif() add_dependencies(${target_prefix} ${script_bin}) endif() - + # Add TOPSRCDIR - set_property(SOURCE ${script_src} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"") + set_property(SOURCE ${script_src} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${GTSAM_SOURCE_DIR}\"") # Disable building during make all/install if (GTSAM_DISABLE_TESTS_ON_INSTALL) set_target_properties(${script_bin} PROPERTIES EXCLUDE_FROM_ALL ON) endif() - + if (is_test) add_test(NAME ${script_base} COMMAND ${script_bin}) endif() - + # Linking and dependendencies if (GTSAM_BUILD_CONVENIENCE_LIBRARIES) target_link_libraries(${script_bin} ${local_libs} ${GTSAM_BOOST_LIBRARIES}) else() target_link_libraries(${script_bin} ${full_libs} ${GTSAM_BOOST_LIBRARIES}) endif() - + # Add .run target if(NOT MSVC AND NOT XCODE_VERSION) add_custom_target(${script_bin}.run ${EXECUTABLE_OUTPUT_PATH}${script_bin} ${ARGN}) endif() - + # Set up Visual Studio folders - file(RELATIVE_PATH relative_path "${PROJECT_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}") + file(RELATIVE_PATH relative_path "${GTSAM_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}") set_property(TARGET ${script_bin} PROPERTY FOLDER "${relative_path}") endif() endforeach(script_src) - + if(MSVC) source_group("" FILES ${script_srcs} ${script_headers}) endif() @@ -166,28 +166,28 @@ macro(gtsam_add_grouped_scripts group pattern target_prefix pretty_prefix_name l else() target_link_libraries(${script_bin} ${Boost_LIBRARIES} ${full_libs}) endif() - + # Only have a main function in one script set(rest_script_srcs ${script_srcs}) list(REMOVE_AT rest_script_srcs 0) set_property(SOURCE ${rest_script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "main=static no_main") - + # Add TOPSRCDIR - set_property(SOURCE ${script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"") - + set_property(SOURCE ${script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${GTSAM_SOURCE_DIR}\"") + # Add test add_dependencies(${target_prefix}.${group} ${script_bin}) add_dependencies(${target_prefix} ${script_bin}) add_test(NAME ${target_prefix}.${group} COMMAND ${script_bin}) - + # Disable building during make all/install if (GTSAM_DISABLE_TESTS_ON_INSTALL) set_target_properties(${script_bin} PROPERTIES EXCLUDE_FROM_ALL ON) endif() - + # Set up Visual Studio folders if(MSVC) - file(RELATIVE_PATH relative_path "${PROJECT_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}") + file(RELATIVE_PATH relative_path "${GTSAM_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}") set_property(TARGET ${script_bin} PROPERTY FOLDER "${relative_path}") source_group("" FILES ${script_srcs} ${script_headers}) endif() diff --git a/cython/README.md b/cython/README.md index cb23b0d4a..6dcdd7c1c 100644 --- a/cython/README.md +++ b/cython/README.md @@ -1,7 +1,7 @@ This is the Cython/Python wrapper around the GTSAM C++ library. -INSTALL -======= +# INSTALL + - if you want to build the gtsam python library for a specific python version (eg 2.7), use the `-DGTSAM_PYTHON_VERSION=2.7` option when running `cmake` otherwise the default interpreter will be used. - If the interpreter is inside an environment (such as an anaconda environment or virtualenv environment) then the environment should be active while building gtsam. - This wrapper needs Cython(>=0.25.2), backports_abc>=0.5, and numpy. These can be installed as follows: @@ -27,8 +27,8 @@ export PYTHONPATH=$PYTHONPATH: - if you run `setup.py` from the build directory rather than the installation directory, the script will warn you with the message: `setup.py is being run from an unexpected location`. Before `make install` is run, not all the components of the package have been copied across, so running `setup.py` from the build directory would result in an incomplete package. -UNIT TESTS -========== +# UNIT TESTS + The Cython toolbox also has a small set of unit tests located in the test directory. To run them: @@ -37,8 +37,8 @@ test directory. To run them: python -m unittest discover ``` -WRITING YOUR OWN SCRIPTS -======================== +# WRITING YOUR OWN SCRIPTS + See the tests for examples. ## Some important notes: @@ -66,8 +66,8 @@ Examples: noiseGaussian = dynamic_cast_noiseModel_Gaussian_noiseModel_Base(noiseBase) ``` -WRAPPING YOUR OWN PROJECT THAT USES GTSAM -========================================= +# WRAPPING YOUR OWN PROJECT THAT USES GTSAM + - Set PYTHONPATH to include ${GTSAM_CYTHON_INSTALL_PATH} + so that it can find gtsam Cython header: gtsam/gtsam.pxd @@ -99,63 +99,55 @@ KNOWN ISSUES - support these constructors by default and declare "delete" for special classes? -TODO -===== -☐ allow duplication of parent' functions in child classes. Not allowed for now due to conflicts in Cython. -☐ a common header for boost shared_ptr? (Or wait until everything is switched to std::shared_ptr in gtsam?) -☐ inner namespaces ==> inner packages? -☐ Wrap fixed-size Matrices/Vectors? +# TODO + +- [ ] allow duplication of parent' functions in child classes. Not allowed for now due to conflicts in Cython. +- [ ] a common header for boost shared_ptr? (Or wait until everything is switched to std::shared_ptr in gtsam?) +- [ ] inner namespaces ==> inner packages? +- [ ] Wrap fixed-size Matrices/Vectors? -Completed/Cancelled: -===== -✔ Fix Python tests: don't use " import * ": Bad style!!! @done (18-03-17 19:50) -✔ Unit tests for cython wrappers @done (18-03-17 18:45) -- simply compare generated files -✔ Wrap unstable @done (18-03-17 15:30) -✔ Unify cython/gtsam.h and the original gtsam.h @done (18-03-17 15:30) - ✔ 18-03-17: manage to unify the two versions by removing std container stubs from the matlab version,and keeping KeyList/KeyVector/KeySet as in the matlab version. Probably Cython 0.25 fixes the casting problem. - ✔ 06-03-17: manage to remove the requirements for default and copy constructors - ✘ 25-11-16: - Try to unify but failed. Main reasons are: Key/size_t, std containers, KeyVector/KeyList/KeySet. - Matlab doesn't need to know about Key, but I can't make Cython to ignore Key as it couldn't cast KeyVector, i.e. FastVector, to FastVector. -✘ Marginal and JointMarginal: revert changes @failed (17-03-17 11:00) -- Cython does need a default constructor! It produces cpp code like this: ```gtsam::JointMarginal __pyx_t_1;``` Users don't have to wrap this constructor, however. -✔ Convert input numpy Matrix/Vector to float dtype and storage order 'F' automatically, cannot crash! @done (15-03-17 13:00) -✔ Remove requirements.txt - Frank: don't bother with only 2 packages and a special case for eigency! @done (08-03-17 10:30) -✔ CMake install script @done (25-11-16 02:30) -✘ [REFACTOR] better name for uninstantiateClass: very vague!! @cancelled (25-11-16 02:30) -- lazy -✘ forward declaration? @cancelled (23-11-16 13:00) - nothing to do, seem to work? -✔ wrap VariableIndex: why is it in inference? If need to, shouldn't have constructors to specific FactorGraphs @done (23-11-16 13:00) -✔ Global functions @done (22-11-16 21:00) -✔ [REFACTOR] typesEqual --> isSameSignature @done (22-11-16 21:00) -✔ Proper overloads (constructors, static methods, methods) @done (20-11-16 21:00) -✔ Allow overloading methods. The current solution is annoying!!! @done (20-11-16 21:00) -✔ Casting from parent and grandparents @done (16-11-16 17:00) -✔ Allow overloading constructors. The current solution is annoying!!! @done (16-11-16 17:00) -✔ Support "print obj" @done (16-11-16 17:00) -✔ methods for FastVector: at, [], ... @done (16-11-16 17:00) -✔ Cython: Key and size_t: traits doesn't exist @done (16-09-12 18:34) -✔ KeyVector, KeyList, KeySet... @done (16-09-13 17:19) -✔ [Nice to have] parse typedef @done (16-09-13 17:19) -✔ ctypedef at correct places @done (16-09-12 18:34) -✔ expand template variable type in constructor/static methods? @done (16-09-12 18:34) -✔ NonlinearOptimizer: copy constructor deleted!!! @done (16-09-13 17:20) -✔ Value: no default constructor @done (16-09-13 17:20) -✔ ctypedef PriorFactor[Vector] PriorFactorVector @done (16-09-19 12:25) -✔ Delete duplicate methods in derived class @done (16-09-12 13:38) -✔ Fix return properly @done (16-09-11 17:14) - ✔ handle pair @done (16-09-11 17:14) -✔ Eigency: ambiguous call: A(const T&) A(const Vector& v) and Eigency A(Map[Vector]& v) @done (16-09-11 07:59) -✔ Eigency: Constructor: ambiguous construct from Vector/Matrix @done (16-09-11 07:59) -✔ Eigency: Fix method template of Vector/Matrix: template argument is [Vector] while arugment is Map[Vector] @done (16-09-11 08:22) -✔ Robust noise: copy assignment operator is deleted because of shared_ptr of the abstract Base class @done (16-09-10 09:05) -✘ Cython: Constructor: generate default constructor? (hack: if it's serializable?) @cancelled (16-09-13 17:20) -✘ Eigency: Map[] to Block @created(16-09-10 07:59) @cancelled (16-09-11 08:28) +# Completed/Cancelled: + +- [x] Fix Python tests: don't use " import * ": Bad style!!! (18-03-17 19:50) +- [x] Unit tests for cython wrappers @done (18-03-17 18:45) -- simply compare generated files +- [x] Wrap unstable @done (18-03-17 15:30) +- [x] Unify cython/gtsam.h and the original gtsam.h @done (18-03-17 15:30) +- [x] 18-03-17: manage to unify the two versions by removing std container stubs from the matlab version,and keeping KeyList/KeyVector/KeySet as in the matlab version. Probably Cython 0.25 fixes the casting problem. +- [x] 06-03-17: manage to remove the requirements for default and copy constructors +- [ ] 25-11-16: Try to unify but failed. Main reasons are: Key/size_t, std containers, KeyVector/KeyList/KeySet. Matlab doesn't need to know about Key, but I can't make Cython to ignore Key as it couldn't cast KeyVector, i.e. FastVector, to FastVector. +- [ ] Marginal and JointMarginal: revert changes @failed (17-03-17 11:00) -- Cython does need a default constructor! It produces cpp code like this: ```gtsam::JointMarginal __pyx_t_1;``` Users don't have to wrap this constructor, however. +- [x] Convert input numpy Matrix/Vector to float dtype and storage order 'F' automatically, cannot crash! @done (15-03-17 13:00) +- [x] Remove requirements.txt - Frank: don't bother with only 2 packages and a special case for eigency! @done (08-03-17 10:30) +- [x] CMake install script @done (25-11-16 02:30) +- [ ] [REFACTOR] better name for uninstantiateClass: very vague!! @cancelled (25-11-16 02:30) -- lazy +- [ ] forward declaration? @cancelled (23-11-16 13:00) - nothing to do, seem to work? +- [x] wrap VariableIndex: why is it in inference? If need to, shouldn't have constructors to specific FactorGraphs @done (23-11-16 13:00) +- [x] Global functions @done (22-11-16 21:00) +- [x] [REFACTOR] typesEqual --> isSameSignature @done (22-11-16 21:00) +- [x] Proper overloads (constructors, static methods, methods) @done (20-11-16 21:00) +- [x] Allow overloading methods. The current solution is annoying!!! @done (20-11-16 21:00) +- [x] Casting from parent and grandparents @done (16-11-16 17:00) +- [x] Allow overloading constructors. The current solution is annoying!!! @done (16-11-16 17:00) +- [x] Support "print obj" @done (16-11-16 17:00) +- [x] methods for FastVector: at, [], ... @done (16-11-16 17:00) +- [x] Cython: Key and size_t: traits doesn't exist @done (16-09-12 18:34) +- [x] KeyVector, KeyList, KeySet... @done (16-09-13 17:19) +- [x] [Nice to have] parse typedef @done (16-09-13 17:19) +- [x] ctypedef at correct places @done (16-09-12 18:34) +- [x] expand template variable type in constructor/static methods? @done (16-09-12 18:34) +- [x] NonlinearOptimizer: copy constructor deleted!!! @done (16-09-13 17:20) +- [x] Value: no default constructor @done (16-09-13 17:20) +- [x] ctypedef PriorFactor[Vector] PriorFactorVector @done (16-09-19 12:25) +- [x] Delete duplicate methods in derived class @done (16-09-12 13:38) +- [x] Fix return properly @done (16-09-11 17:14) +- [x] handle pair @done (16-09-11 17:14) +- [x] Eigency: ambiguous call: A(const T&) A(const Vector& v) and Eigency A(Map[Vector]& v) @done (16-09-11 07:59) +- [x] Eigency: Constructor: ambiguous construct from Vector/Matrix @done (16-09-11 07:59) +- [x] Eigency: Fix method template of Vector/Matrix: template argument is [Vector] while arugment is Map[Vector] @done (16-09-11 08:22) +- [x] Robust noise: copy assignment operator is deleted because of shared_ptr of the abstract Base class @done (16-09-10 09:05) +- [ ] Cython: Constructor: generate default constructor? (hack: if it's serializable?) @cancelled (16-09-13 17:20) +- [ ] Eigency: Map[] to Block @created(16-09-10 07:59) @cancelled (16-09-11 08:28) - inference before symbolic/linear - what's the purpose of "virtual" ?? - -Installation: - ☐ Prerequisite: - - Users create venv and pip install requirements before compiling - - Wrap cython script in gtsam/cython folder - ☐ Install built module into venv? diff --git a/gtsam.h b/gtsam.h index ba5aa5f4f..97eb2d8c3 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1870,7 +1870,6 @@ class NonlinearFactorGraph { // FactorGraph void print(string s) const; - void printErrors(const gtsam::Values& values); bool equals(const gtsam::NonlinearFactorGraph& fg, double tol) const; size_t size() const; bool empty() const; @@ -1887,6 +1886,7 @@ class NonlinearFactorGraph { gtsam::KeyVector keyVector() const; // NonlinearFactorGraph + void printErrors(const gtsam::Values& values) const; double error(const gtsam::Values& values) const; double probPrime(const gtsam::Values& values) const; gtsam::Ordering orderingCOLAMD() const; diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 60915eead..b4a33943e 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -1,3 +1,5 @@ +project(gtsam LANGUAGES CXX) + # We split the library in to separate subfolders, each containing # tests, timing, and an optional convenience library. # The following variable is the master list of subdirs to add @@ -82,9 +84,9 @@ ENDIF(MSVC) # Generate and install config and dllexport files configure_file(config.h.in config.h) set(library_name GTSAM) # For substitution in dllexport.h.in -configure_file("${PROJECT_SOURCE_DIR}/cmake/dllexport.h.in" "dllexport.h") -list(APPEND gtsam_srcs "${PROJECT_BINARY_DIR}/gtsam/config.h" "${PROJECT_BINARY_DIR}/gtsam/dllexport.h") -install(FILES "${PROJECT_BINARY_DIR}/gtsam/config.h" "${PROJECT_BINARY_DIR}/gtsam/dllexport.h" DESTINATION include/gtsam) +configure_file("${GTSAM_SOURCE_DIR}/cmake/dllexport.h.in" "dllexport.h") +list(APPEND gtsam_srcs "${PROJECT_BINARY_DIR}/config.h" "${PROJECT_BINARY_DIR}/dllexport.h") +install(FILES "${PROJECT_BINARY_DIR}/config.h" "${PROJECT_BINARY_DIR}/dllexport.h" DESTINATION include/gtsam) if(GTSAM_SUPPORT_NESTED_DISSECTION) list(APPEND GTSAM_ADDITIONAL_LIBRARIES metis) @@ -101,17 +103,16 @@ message(STATUS "Building GTSAM - shared: ${BUILD_SHARED_LIBS}") # BUILD_SHARED_LIBS automatically defines static/shared libs: add_library(gtsam ${gtsam_srcs}) + # Boost: target_link_libraries(gtsam PUBLIC ${GTSAM_BOOST_LIBRARIES}) target_include_directories(gtsam PUBLIC ${Boost_INCLUDE_DIR}) # Other deps: target_link_libraries(gtsam PUBLIC ${GTSAM_ADDITIONAL_LIBRARIES}) -target_compile_definitions(gtsam PRIVATE ${GTSAM_COMPILE_DEFINITIONS_PRIVATE}) -target_compile_definitions(gtsam PUBLIC ${GTSAM_COMPILE_DEFINITIONS_PUBLIC}) -if (NOT "${GTSAM_COMPILE_OPTIONS_PUBLIC}" STREQUAL "") - target_compile_options(gtsam PUBLIC ${GTSAM_COMPILE_OPTIONS_PUBLIC}) -endif() -target_compile_options(gtsam PRIVATE ${GTSAM_COMPILE_OPTIONS_PRIVATE}) + +# Apply build flags: +gtsam_apply_build_flags(gtsam) + set_target_properties(gtsam PROPERTIES OUTPUT_NAME gtsam CLEAN_DIRECT_OUTPUT 1 @@ -171,7 +172,7 @@ if(WIN32) # Add 'lib' prefix to static library to avoid filename collision with set_target_properties(gtsam PROPERTIES PREFIX "" DEFINE_SYMBOL GTSAM_EXPORTS - RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/bin") + RUNTIME_OUTPUT_DIRECTORY "${GTSAM_BINARY_DIR}/bin") endif() endif() diff --git a/gtsam/base/cholesky.cpp b/gtsam/base/cholesky.cpp index 31e4b8244..17cb291f0 100644 --- a/gtsam/base/cholesky.cpp +++ b/gtsam/base/cholesky.cpp @@ -111,7 +111,7 @@ bool choleskyPartial(Matrix& ABC, size_t nFrontal, size_t topleft) { return true; assert(ABC.cols() == ABC.rows()); - assert(ABC.rows() >= topleft); + assert(size_t(ABC.rows()) >= topleft); const size_t n = static_cast(ABC.rows() - topleft); assert(nFrontal <= size_t(n)); diff --git a/gtsam/config.h.in b/gtsam/config.h.in index 92380f8eb..4b8bd180d 100644 --- a/gtsam/config.h.in +++ b/gtsam/config.h.in @@ -25,7 +25,7 @@ #define GTSAM_VERSION_STRING "@GTSAM_VERSION_STRING@" // Paths to example datasets distributed with GTSAM -#define GTSAM_SOURCE_TREE_DATASET_DIR "@PROJECT_SOURCE_DIR@/examples/Data" +#define GTSAM_SOURCE_TREE_DATASET_DIR "@GTSAM_SOURCE_DIR@/examples/Data" #define GTSAM_INSTALLED_DATASET_DIR "@GTSAM_TOOLBOX_INSTALL_PATH@/gtsam_examples/Data" // Whether GTSAM is compiled to use quaternions for Rot3 (otherwise uses rotation matrices) diff --git a/gtsam/inference/Key.cpp b/gtsam/inference/Key.cpp index f25727441..dd433ff09 100644 --- a/gtsam/inference/Key.cpp +++ b/gtsam/inference/Key.cpp @@ -56,7 +56,7 @@ string _multirobotKeyFormatter(Key key) { /* ************************************************************************* */ template -static void Print(const CONTAINER& keys, const string& s, +void Print(const CONTAINER& keys, const string& s, const KeyFormatter& keyFormatter) { cout << s << " "; if (keys.empty()) @@ -83,6 +83,44 @@ void PrintKeySet(const KeySet& keys, const string& s, const KeyFormatter& keyFormatter) { Print(keys, s, keyFormatter); } + +/* ************************************************************************* */ +// Access to custom stream property. +void *&key_formatter::property(ios_base &s) { + static int kUniqueIndex = ios_base::xalloc(); + return s.pword(kUniqueIndex); +} + +/* ************************************************************************* */ +// Store pointer to formatter in property. +void key_formatter::set_property(ios_base &s, const KeyFormatter &f) { + property(s) = (void *)(&f); +} + +/* ************************************************************************* */ +// Get pointer to formatter from property. +KeyFormatter *key_formatter::get_property(ios_base &s) { + return (KeyFormatter *)(property(s)); +} + +/* ************************************************************************* */ +// Stream operator that will take a key_formatter and set the stream property. +ostream &operator<<(ostream &os, const key_formatter &m) { + key_formatter::set_property(os, m.formatter_); + return os; +} + +/* ************************************************************************* */ +// Stream operator that takes a StreamedKey and properly formats it +ostream &operator<<(ostream &os, const StreamedKey &streamedKey) { + const KeyFormatter *formatter = key_formatter::get_property(os); + if (formatter == nullptr) { + formatter = &DefaultKeyFormatter; + } + os << (*formatter)(streamedKey.key_); + return (os); +} + /* ************************************************************************* */ } // \namespace gtsam diff --git a/gtsam/inference/Key.h b/gtsam/inference/Key.h index d400a33c0..ae3f3844b 100644 --- a/gtsam/inference/Key.h +++ b/gtsam/inference/Key.h @@ -27,6 +27,8 @@ #include +#include + namespace gtsam { /// Typedef for a function to format a key, i.e. to convert it to a string @@ -52,6 +54,34 @@ GTSAM_EXPORT std::string _multirobotKeyFormatter(gtsam::Key key); static const gtsam::KeyFormatter MultiRobotKeyFormatter = &_multirobotKeyFormatter; +/// To use the key_formatter on Keys, they must be wrapped in a StreamedKey. +struct StreamedKey { + const Key &key_; + explicit StreamedKey(const Key &key) : key_(key) {} + friend std::ostream &operator<<(std::ostream &, const StreamedKey &); +}; + +/** + * Output stream manipulator that will format gtsam::Keys according to the given + * KeyFormatter, as long as Key values are wrapped in a gtsam::StreamedKey. + * LabeledSymbol and Symbol values do not have to be wrapped. + * usage: + * Key key = LabeledSymbol('x', 'A', 5); // cast to key type + * cout << key_formatter(MultiRobotKeyFormatter) << StreamedKey(key); + */ +class key_formatter { + public: + explicit key_formatter(KeyFormatter v) : formatter_(v) {} + friend std::ostream &operator<<(std::ostream &, const key_formatter &); + friend std::ostream &operator<<(std::ostream &, const StreamedKey &); + + private: + KeyFormatter formatter_; + static void *&property(std::ios_base &s); + static void set_property(std::ios_base &s, const KeyFormatter &f); + static KeyFormatter *get_property(std::ios_base &s); +}; + /// Define collection type once and for all - also used in wrappers typedef FastVector KeyVector; diff --git a/gtsam/inference/LabeledSymbol.cpp b/gtsam/inference/LabeledSymbol.cpp index 0d9c35d2c..6d34883fa 100644 --- a/gtsam/inference/LabeledSymbol.cpp +++ b/gtsam/inference/LabeledSymbol.cpp @@ -121,6 +121,13 @@ boost::function LabeledSymbol::TypeLabelTest(unsigned char c, return boost::bind(&LabeledSymbol::chr, boost::bind(make, _1)) == c && boost::bind(&LabeledSymbol::label, boost::bind(make, _1)) == label; } + +/* ************************************************************************* */ +std::ostream &operator<<(std::ostream &os, const LabeledSymbol &symbol) { + os << StreamedKey(symbol); + return os; +} + /* ************************************************************************* */ } // \namespace gtsam diff --git a/gtsam/inference/LabeledSymbol.h b/gtsam/inference/LabeledSymbol.h index 8c521b067..5b3ec8766 100644 --- a/gtsam/inference/LabeledSymbol.h +++ b/gtsam/inference/LabeledSymbol.h @@ -100,10 +100,15 @@ public: LabeledSymbol upper() const { return LabeledSymbol(c_, toupper(label_), j_); } LabeledSymbol lower() const { return LabeledSymbol(c_, tolower(label_), j_); } - // Create a new symbol with a different value + // Create a new symbol with a different character. LabeledSymbol newChr(unsigned char c) const { return LabeledSymbol(c, label_, j_); } + + // Create a new symbol with a different label. LabeledSymbol newLabel(unsigned char label) const { return LabeledSymbol(c_, label, j_); } + /// Output stream operator that can be used with key_formatter (see Key.h). + friend std::ostream &operator<<(std::ostream &, const LabeledSymbol &); + private: /** Serialization function */ diff --git a/gtsam/inference/Symbol.cpp b/gtsam/inference/Symbol.cpp index ccabcb07e..5e41b3eac 100644 --- a/gtsam/inference/Symbol.cpp +++ b/gtsam/inference/Symbol.cpp @@ -66,5 +66,10 @@ boost::function Symbol::ChrTest(unsigned char c) { return bind(&Symbol::chr, bind(make, _1)) == c; } +std::ostream &operator<<(std::ostream &os, const Symbol &symbol) { + os << StreamedKey(symbol); + return os; +} + } // namespace gtsam diff --git a/gtsam/inference/Symbol.h b/gtsam/inference/Symbol.h index 8e22202ed..86574f70d 100644 --- a/gtsam/inference/Symbol.h +++ b/gtsam/inference/Symbol.h @@ -112,6 +112,9 @@ public: */ static boost::function ChrTest(unsigned char c); + /// Output stream operator that can be used with key_formatter (see Key.h). + friend std::ostream &operator<<(std::ostream &, const Symbol &); + private: /** Serialization function */ diff --git a/gtsam/inference/tests/testKey.cpp b/gtsam/inference/tests/testKey.cpp index 93a161ccd..fcdb5709b 100644 --- a/gtsam/inference/tests/testKey.cpp +++ b/gtsam/inference/tests/testKey.cpp @@ -22,6 +22,9 @@ #include #include // for operator += + +#include + using namespace boost::assign; using namespace std; using namespace gtsam; @@ -41,17 +44,15 @@ TEST(Key, KeySymbolConversion) { template Key KeyTestValue(); -template<> -Key KeyTestValue<8>() -{ +template <> +Key KeyTestValue<8>() { return 0x6100000000000005; -}; +} -template<> -Key KeyTestValue<4>() -{ +template <> +Key KeyTestValue<4>() { return 0x61000005; -}; +} /* ************************************************************************* */ TEST(Key, KeySymbolEncoding) { @@ -68,12 +69,41 @@ TEST(Key, KeySymbolEncoding) { /* ************************************************************************* */ TEST(Key, ChrTest) { - Key key = Symbol('c',3); + Symbol key('c', 3); EXPECT(Symbol::ChrTest('c')(key)); EXPECT(!Symbol::ChrTest('d')(key)); } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +// A custom (nonsensical) formatter. +string myFormatter(Key key) { + return "special"; +} + +TEST(Key, Formatting) { + Symbol key('c', 3); + EXPECT("c3" == DefaultKeyFormatter(key)); + + // Try streaming keys, should be default-formatted. + stringstream ss; + ss << StreamedKey(key); + EXPECT("c3" == ss.str()); + + // use key_formatter with a function pointer + stringstream ss2; + ss2 << key_formatter(myFormatter) << StreamedKey(key); + EXPECT("special" == ss2.str()); + + // use key_formatter with a function object. + stringstream ss3; + ss3 << key_formatter(DefaultKeyFormatter) << StreamedKey(key); + EXPECT("c3" == ss3.str()); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/gtsam/inference/tests/testLabeledSymbol.cpp b/gtsam/inference/tests/testLabeledSymbol.cpp index b463f4131..2a56b39c2 100644 --- a/gtsam/inference/tests/testLabeledSymbol.cpp +++ b/gtsam/inference/tests/testLabeledSymbol.cpp @@ -80,6 +80,29 @@ TEST(LabeledSymbol, ChrTest) { } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +// A custom (nonsensical) formatter. +string myFormatter(Key key) { + return "special"; +} + +TEST(LabeledSymbol, Formatting) { + LabeledSymbol symbol('c', 'A', 3); + + // use key_formatter with a function pointer + stringstream ss2; + ss2 << key_formatter(myFormatter) << symbol; + EXPECT("special" == ss2.str()); + + // use key_formatter with a function object. + stringstream ss3; + ss3 << key_formatter(MultiRobotKeyFormatter) << symbol; + EXPECT("cA3" == ss3.str()); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/gtsam/inference/tests/testSymbol.cpp b/gtsam/inference/tests/testSymbol.cpp new file mode 100644 index 000000000..43a0e219a --- /dev/null +++ b/gtsam/inference/tests/testSymbol.cpp @@ -0,0 +1,50 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/* + * @file testSymbol.cpp + * @author Frank Dellaert + */ + +#include + +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +// A custom (nonsensical) formatter. +string myFormatter(Key key) { + return "special"; +} + +TEST(Symbol, Formatting) { + Symbol symbol('c', 3); + + // use key_formatter with a function pointer + stringstream ss2; + ss2 << key_formatter(myFormatter) << symbol; + EXPECT("special" == ss2.str()); + + // use key_formatter with a function object. + stringstream ss3; + ss3 << key_formatter(MultiRobotKeyFormatter) << symbol; + EXPECT("c3" == ss3.str()); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index d16373c78..c208259b8 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -505,7 +505,7 @@ VectorValues HessianFactor::solve() { // Do Cholesky Factorization const size_t n = size(); - assert(info_.nBlocks() == n + 1); + assert(size_t(info_.nBlocks()) == n + 1); info_.choleskyPartial(n); auto R = info_.triangularView(0, n); auto eta = linearTerm(); diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index a7802120c..2ad71cb3c 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -64,7 +64,7 @@ typedef ManifoldPreintegration PreintegrationType; * * @addtogroup SLAM */ -class PreintegratedCombinedMeasurements : public PreintegrationType { +class GTSAM_EXPORT PreintegratedCombinedMeasurements : public PreintegrationType { public: @@ -222,7 +222,7 @@ public: * * @addtogroup SLAM */ -class CombinedImuFactor: public NoiseModelFactor6 { public: diff --git a/gtsam/nonlinear/DoglegOptimizer.h b/gtsam/nonlinear/DoglegOptimizer.h index 1fa25fd60..6b3097476 100644 --- a/gtsam/nonlinear/DoglegOptimizer.h +++ b/gtsam/nonlinear/DoglegOptimizer.h @@ -41,7 +41,7 @@ public: VerbosityDL verbosityDL; ///< The verbosity level for Dogleg (default: SILENT), see also NonlinearOptimizerParams::verbosity DoglegParams() : - deltaInitial(10.0), verbosityDL(SILENT) {} + deltaInitial(1.0), verbosityDL(SILENT) {} virtual ~DoglegParams() {} diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 7e14578c3..ff220044a 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -145,13 +145,13 @@ namespace gtsam { boost::make_filter_iterator(filter, ((const Values&) values).begin(), ((const Values&) values).end()), - &ValuesCastHelper::cast)), constEnd_( boost::make_transform_iterator( boost::make_filter_iterator(filter, ((const Values&) values).end(), ((const Values&) values).end()), - &ValuesCastHelper::cast)) { } diff --git a/gtsam_unstable/CMakeLists.txt b/gtsam_unstable/CMakeLists.txt index 327326201..65ba4848d 100644 --- a/gtsam_unstable/CMakeLists.txt +++ b/gtsam_unstable/CMakeLists.txt @@ -1,3 +1,5 @@ +project(gtsam_unstable LANGUAGES CXX) + # Build full gtsam_unstable library as a single library # and also build tests set (gtsam_unstable_subdirs @@ -65,6 +67,10 @@ message(STATUS "Install prefix: ${CMAKE_INSTALL_PREFIX}") # BUILD_SHARED_LIBS automatically defines static/shared libs: add_library(gtsam_unstable ${gtsam_unstable_srcs}) + +# Apply build flags: +gtsam_apply_build_flags(gtsam_unstable) + set_target_properties(gtsam_unstable PROPERTIES OUTPUT_NAME gtsam_unstable CLEAN_DIRECT_OUTPUT 1 diff --git a/gtsam_unstable/examples/README b/gtsam_unstable/examples/README.md similarity index 100% rename from gtsam_unstable/examples/README rename to gtsam_unstable/examples/README.md diff --git a/gtsampy.h b/gtsampy.h deleted file mode 100644 index cc5ccb1b4..000000000 --- a/gtsampy.h +++ /dev/null @@ -1,2658 +0,0 @@ -/** - - * GTSAM Wrap Module Definition - * - * These are the current classes available through the matlab toolbox interface, - * add more functions/classes as they are available. - * - * Requirements: - * Classes must start with an uppercase letter - * - Can wrap a typedef - * Only one Method/Constructor per line, though methods/constructors can extend across multiple lines - * Methods can return - * - Eigen types: Matrix, Vector - * - C/C++ basic types: string, bool, size_t, int, double, char, unsigned char - * - void - * - Any class with which be copied with boost::make_shared() - * - boost::shared_ptr of any object type - * Constructors - * - Overloads are supported - * - A class with no constructors can be returned from other functions but not allocated directly in MATLAB - * Methods - * - Constness has no effect - * - Specify by-value (not reference) return types, even if C++ method returns reference - * - Must start with a letter (upper or lowercase) - * - Overloads are supported - * Static methods - * - Must start with a letter (upper or lowercase) and use the "static" keyword - * - The first letter will be made uppercase in the generated MATLAB interface - * - Overloads are supported - * Arguments to functions any of - * - Eigen types: Matrix, Vector - * - Eigen types and classes as an optionally const reference - * - C/C++ basic types: string, bool, size_t, size_t, double, char, unsigned char - * - Any class with which be copied with boost::make_shared() (except Eigen) - * - boost::shared_ptr of any object type (except Eigen) - * Comments can use either C++ or C style, with multiple lines - * Namespace definitions - * - Names of namespaces must start with a lowercase letter - * - start a namespace with "namespace {" - * - end a namespace with exactly "}" - * - Namespaces can be nested - * Namespace usage - * - Namespaces can be specified for classes in arguments and return values - * - In each case, the namespace must be fully specified, e.g., "namespace1::namespace2::ClassName" - * Includes in C++ wrappers - * - All includes will be collected and added in a single file - * - All namespaces must have angle brackets: - * - No default includes will be added - * Global/Namespace functions - * - Functions specified outside of a class are global - * - Can be overloaded with different arguments - * - Can have multiple functions of the same name in different namespaces - * Using classes defined in other modules - * - If you are using a class 'OtherClass' not wrapped in this definition file, add "class OtherClass;" to avoid a dependency error - * Virtual inheritance - * - Specify fully-qualified base classes, i.e. "virtual class Derived : ns::Base {" where "ns" is the namespace - * - Mark with 'virtual' keyword, e.g. "virtual class Base {", and also "virtual class Derived : ns::Base {" - * - Forward declarations must also be marked virtual, e.g. "virtual class ns::Base;" and - * also "virtual class ns::Derived;" - * - Pure virtual (abstract) classes should list no constructors in this interface file - * - Virtual classes must have a clone() function in C++ (though it does not have to be included - * in the MATLAB interface). clone() will be called whenever an object copy is needed, instead - * of using the copy constructor (which is used for non-virtual objects). - * - Signature of clone function - will be called virtually, so must appear at least at the top of the inheritance tree - * virtual boost::shared_ptr clone() const; - * Class Templates - * - Basic templates are supported either with an explicit list of types to instantiate, - * e.g. template class Class1 { ... }; - * or with typedefs, e.g. - * template class Class2 { ... }; - * typedef Class2 MyInstantiatedClass; - * - In the class definition, appearances of the template argument(s) will be replaced with their - * instantiated types, e.g. 'void setValue(const T& value);'. - * - To refer to the instantiation of the template class itself, use 'This', i.e. 'static This Create();' - * - To create new instantiations in other modules, you must copy-and-paste the whole class definition - * into the new module, but use only your new instantiation types. - * - When forward-declaring template instantiations, use the generated/typedefed name, e.g. - * class gtsam::Class1Pose2; - * class gtsam::MyInstantiatedClass; - * Boost.serialization within Matlab: - * - you need to mark classes as being serializable in the markup file (see this file for an example). - * - There are two options currently, depending on the class. To "mark" a class as serializable, - * add a function with a particular signature so that wrap will catch it. - * - Add "void serialize()" to a class to create serialization functions for a class. - * Adding this flag subsumes the serializable() flag below. Requirements: - * - A default constructor must be publicly accessible - * - Must not be an abstract base class - * - The class must have an actual boost.serialization serialize() function. - * - 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. - */ - -/** - * Status: - * - TODO: default values for arguments - * - WORKAROUND: make multiple versions of the same function for different configurations of default arguments - * - TODO: Handle gtsam::Rot3M conversions to quaternions - * - TODO: Parse return of const ref arguments - * - TODO: Parse std::string variants and convert directly to special string - * - TODO: Add enum support - * - TODO: Add generalized serialization support via boost.serialization with hooks to matlab save/load - */ - -namespace std { - #include - template - class vector - { - //Do we need these? - //Capacity - /*size_t size() const; - size_t max_size() const; - //void resize(size_t sz); - size_t capacity() const; - bool empty() const; - void reserve(size_t n); - - //Element access - T* at(size_t n); - T* front(); - T* back(); - - //Modifiers - void assign(size_t n, const T& u); - void push_back(const T& x); - void pop_back();*/ - }; - //typedef std::vector - - #include - template - class list - { - - - }; - -} - -namespace gtsam { - -//************************************************************************* -// base -//************************************************************************* - -/** gtsam namespace functions */ -bool linear_independent(Matrix A, Matrix B, double tol); - -virtual class Value { - // No constructors because this is an abstract class - - // Testable - void print(string s) const; - - // Manifold - size_t dim() 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 -//************************************************************************* - -class Point2 { - // Standard Constructors - Point2(); - Point2(double x, double y); - Point2(Vector v); - - // Testable - void print(string s) const; - bool equals(const gtsam::Point2& pose, double tol) const; - - // Group - static gtsam::Point2 identity(); - gtsam::Point2 inverse() const; - gtsam::Point2 compose(const gtsam::Point2& p2) const; - gtsam::Point2 between(const gtsam::Point2& p2) const; - - // Manifold - gtsam::Point2 retract(Vector v) const; - Vector localCoordinates(const gtsam::Point2& p) const; - - // Lie Group - static gtsam::Point2 Expmap(Vector v); - static Vector Logmap(const gtsam::Point2& p); - - // Standard Interface - double x() const; - double y() const; - Vector vector() const; - double dist(const gtsam::Point2& p2) const; - double norm() const; - - // enabling serialization functionality - void serialize() const; -}; - -// std::vector -class Point2Vector -{ - // Constructors - Point2Vector(); - Point2Vector(const gtsam::Point2Vector& v); - - //Capacity - size_t size() const; - size_t max_size() const; - void resize(size_t sz); - size_t capacity() const; - bool empty() const; - void reserve(size_t n); - - //Element access - gtsam::Point2 at(size_t n) const; - gtsam::Point2 front() const; - gtsam::Point2 back() const; - - //Modifiers - void assign(size_t n, const gtsam::Point2& u); - void push_back(const gtsam::Point2& x); - void pop_back(); -}; - -class StereoPoint2 { - // Standard Constructors - StereoPoint2(); - StereoPoint2(double uL, double uR, double v); - - // Testable - void print(string s) const; - bool equals(const gtsam::StereoPoint2& point, double tol) const; - - // Group - static gtsam::StereoPoint2 identity(); - gtsam::StereoPoint2 inverse() const; - gtsam::StereoPoint2 compose(const gtsam::StereoPoint2& p2) const; - gtsam::StereoPoint2 between(const gtsam::StereoPoint2& p2) const; - - // Manifold - gtsam::StereoPoint2 retract(Vector v) const; - Vector localCoordinates(const gtsam::StereoPoint2& p) const; - - // Lie Group - static gtsam::StereoPoint2 Expmap(Vector v); - static Vector Logmap(const gtsam::StereoPoint2& p); - - // Standard Interface - Vector vector() const; - double uL() const; - double uR() const; - double v() const; - - // enabling serialization functionality - void serialize() const; -}; - -class Point3 { - // Standard Constructors - Point3(); - Point3(double x, double y, double z); - Point3(Vector v); - - // Testable - void print(string s) const; - bool equals(const gtsam::Point3& p, double tol) const; - - // Group - static gtsam::Point3 identity(); - - // Standard Interface - Vector vector() const; - double x() const; - double y() const; - double z() const; - - // enabling serialization functionality - void serialize() const; -}; - -class Rot2 { - // Standard Constructors and Named Constructors - Rot2(); - Rot2(double theta); - static gtsam::Rot2 fromAngle(double theta); - static gtsam::Rot2 fromDegrees(double theta); - static gtsam::Rot2 fromCosSin(double c, double s); - - // Testable - void print(string s) const; - bool equals(const gtsam::Rot2& rot, double tol) const; - - // Group - static gtsam::Rot2 identity(); - gtsam::Rot2 inverse(); - gtsam::Rot2 compose(const gtsam::Rot2& p2) const; - gtsam::Rot2 between(const gtsam::Rot2& p2) const; - - // Manifold - gtsam::Rot2 retract(Vector v) const; - Vector localCoordinates(const gtsam::Rot2& p) const; - - // Lie Group - static gtsam::Rot2 Expmap(Vector v); - static Vector Logmap(const gtsam::Rot2& p); - - // Group Action on Point2 - gtsam::Point2 rotate(const gtsam::Point2& point) const; - gtsam::Point2 unrotate(const gtsam::Point2& point) const; - - // Standard Interface - static gtsam::Rot2 relativeBearing(const gtsam::Point2& d); // Ignoring derivative - static gtsam::Rot2 atan2(double y, double x); - double theta() const; - double degrees() const; - double c() const; - double s() const; - Matrix matrix() const; - - // enabling serialization functionality - void serialize() const; -}; - -class Rot3 { - // Standard Constructors and Named Constructors - Rot3(); - Rot3(Matrix R); - static gtsam::Rot3 Rx(double t); - static gtsam::Rot3 Ry(double t); - static gtsam::Rot3 Rz(double t); - static gtsam::Rot3 RzRyRx(double x, double y, double z); - static gtsam::Rot3 RzRyRx(Vector xyz); - static gtsam::Rot3 yaw(double t); // positive yaw is to right (as in aircraft heading) - static gtsam::Rot3 pitch(double t); // positive pitch is up (increasing aircraft altitude) - static gtsam::Rot3 roll(double t); // positive roll is to right (increasing yaw in aircraft) - static gtsam::Rot3 ypr(double y, double p, double r); - static gtsam::Rot3 quaternion(double w, double x, double y, double z); - static gtsam::Rot3 Rodrigues(Vector v); - - // Testable - void print(string s) const; - bool equals(const gtsam::Rot3& rot, double tol) const; - - // Group - static gtsam::Rot3 identity(); - gtsam::Rot3 inverse() const; - gtsam::Rot3 compose(const gtsam::Rot3& p2) const; - gtsam::Rot3 between(const gtsam::Rot3& p2) const; - - // Manifold - //gtsam::Rot3 retractCayley(Vector v) const; // FIXME, does not exist in both Matrix and Quaternion options - gtsam::Rot3 retract(Vector v) const; - Vector localCoordinates(const gtsam::Rot3& p) const; - - // Group Action on Point3 - gtsam::Point3 rotate(const gtsam::Point3& p) const; - gtsam::Point3 unrotate(const gtsam::Point3& p) const; - - // Standard Interface - static gtsam::Rot3 Expmap(Vector v); - static Vector Logmap(const gtsam::Rot3& p); - Matrix matrix() const; - Matrix transpose() const; - gtsam::Point3 column(size_t index) const; - Vector xyz() const; - Vector ypr() const; - Vector rpy() const; - double roll() const; - double pitch() const; - double yaw() const; -// Vector toQuaternion() const; // FIXME: Can't cast to Vector properly - Vector quaternion() const; - - // enabling serialization functionality - void serialize() const; -}; - -class Pose2 { - // Standard Constructor - Pose2(); - Pose2(const gtsam::Pose2& pose); - Pose2(double x, double y, double theta); - Pose2(double theta, const gtsam::Point2& t); - Pose2(const gtsam::Rot2& r, const gtsam::Point2& t); - Pose2(Vector v); - - // Testable - void print(string s) const; - bool equals(const gtsam::Pose2& pose, double tol) const; - - // Group - static gtsam::Pose2 identity(); - gtsam::Pose2 inverse() const; - gtsam::Pose2 compose(const gtsam::Pose2& p2) const; - gtsam::Pose2 between(const gtsam::Pose2& p2) const; - - // Manifold - gtsam::Pose2 retract(Vector v) const; - Vector localCoordinates(const gtsam::Pose2& p) const; - - // Lie Group - static gtsam::Pose2 Expmap(Vector v); - static Vector Logmap(const gtsam::Pose2& p); - Matrix AdjointMap() const; - Vector Adjoint(const Vector& xi) const; - static Matrix wedge(double vx, double vy, double w); - - // Group Actions on Point2 - gtsam::Point2 transformFrom(const gtsam::Point2& p) const; - gtsam::Point2 transformTo(const gtsam::Point2& p) const; - - // Standard Interface - double x() const; - double y() const; - double theta() const; - gtsam::Rot2 bearing(const gtsam::Point2& point) const; - double range(const gtsam::Point2& point) const; - gtsam::Point2 translation() const; - gtsam::Rot2 rotation() const; - Matrix matrix() const; - - // enabling serialization functionality - void serialize() const; -}; - -class Pose3 { - // Standard Constructors - Pose3(); - Pose3(const gtsam::Pose3& pose); - Pose3(const gtsam::Rot3& r, const gtsam::Point3& t); - Pose3(const gtsam::Pose2& pose2); // FIXME: shadows Pose3(Pose3 pose) - Pose3(Matrix t); - - // Testable - void print(string s) const; - bool equals(const gtsam::Pose3& pose, double tol) const; - - // Group - static gtsam::Pose3 identity(); - gtsam::Pose3 inverse() const; - gtsam::Pose3 compose(const gtsam::Pose3& p2) const; - gtsam::Pose3 between(const gtsam::Pose3& p2) const; - - // Manifold - gtsam::Pose3 retract(Vector v) const; - Vector localCoordinates(const gtsam::Pose3& T2) const; - - // Lie Group - static gtsam::Pose3 Expmap(Vector v); - static Vector Logmap(const gtsam::Pose3& p); - Matrix AdjointMap() const; - Vector Adjoint(Vector xi) const; - static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz); - - // Group Action on Point3 - gtsam::Point3 transformFrom(const gtsam::Point3& p) const; - gtsam::Point3 transformTo(const gtsam::Point3& p) const; - - // Standard Interface - gtsam::Rot3 rotation() const; - gtsam::Point3 translation() const; - double x() const; - double y() const; - double z() const; - Matrix matrix() const; - gtsam::Pose3 transformTo(const gtsam::Pose3& pose) const; // FIXME: shadows other transformTo() - double range(const gtsam::Point3& point); - double range(const gtsam::Pose3& pose); - - // enabling serialization functionality - void serialize() const; -}; - -// std::vector -class Pose3Vector -{ - Pose3Vector(); - size_t size() const; - bool empty() const; - gtsam::Pose3 at(size_t n) const; - void push_back(const gtsam::Pose3& x); -}; - -#include -class Unit3 { - // Standard Constructors - Unit3(); - Unit3(const gtsam::Point3& pose); - - // Testable - void print(string s) const; - bool equals(const gtsam::Unit3& pose, double tol) const; - - // Other functionality - Matrix basis() const; - Matrix skew() const; - - // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::Unit3 retract(Vector v) const; - Vector localCoordinates(const gtsam::Unit3& s) const; -}; - -#include -class EssentialMatrix { - // Standard Constructors - EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb); - - // Testable - void print(string s) const; - bool equals(const gtsam::EssentialMatrix& pose, double tol) const; - - // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::EssentialMatrix retract(Vector v) const; - Vector localCoordinates(const gtsam::EssentialMatrix& s) const; - - // Other methods: - gtsam::Rot3 rotation() const; - gtsam::Unit3 direction() const; - Matrix matrix() const; - double error(Vector vA, Vector vB); -}; - -class Cal3_S2 { - // Standard Constructors - Cal3_S2(); - Cal3_S2(double fx, double fy, double s, double u0, double v0); - Cal3_S2(Vector v); - Cal3_S2(double fov, int w, int h); - - // Testable - void print(string s) const; - bool equals(const gtsam::Cal3_S2& rhs, double tol) const; - - // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::Cal3_S2 retract(Vector v) const; - Vector localCoordinates(const gtsam::Cal3_S2& c) const; - - // Action on Point2 - gtsam::Point2 calibrate(const gtsam::Point2& p) const; - gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; - - // Standard Interface - double fx() const; - double fy() const; - double skew() const; - double px() const; - double py() const; - gtsam::Point2 principalPoint() const; - Vector vector() const; - Matrix matrix() const; - Matrix matrix_inverse() const; - - // enabling serialization functionality - void serialize() const; -}; - -#include -virtual class Cal3DS2_Base { - // Standard Constructors - Cal3DS2_Base(); - - // Testable - void print(string s) const; - - // Standard Interface - double fx() const; - double fy() const; - double skew() const; - double px() const; - double py() const; - double k1() const; - double k2() const; - - // Action on Point2 - gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; - gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const; - gtsam::Point2 calibrate(const gtsam::Point2& p) const; - - // enabling serialization functionality - void serialize() const; -}; - -#include -virtual class Cal3DS2 : gtsam::Cal3DS2_Base { - // Standard Constructors - Cal3DS2(); - Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2); - Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2); - Cal3DS2(Vector v); - - // Testable - bool equals(const gtsam::Cal3DS2& rhs, double tol) const; - - // Manifold - size_t dim() const; - static size_t Dim(); - gtsam::Cal3DS2 retract(Vector v) const; - Vector localCoordinates(const gtsam::Cal3DS2& c) const; - - // enabling serialization functionality - void serialize() const; -}; - -#include -virtual class Cal3Unified : gtsam::Cal3DS2_Base { - // Standard Constructors - Cal3Unified(); - Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2); - Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2, double xi); - Cal3Unified(Vector v); - - // Testable - bool equals(const gtsam::Cal3Unified& rhs, double tol) const; - - // Standard Interface - double xi() const; - gtsam::Point2 spaceToNPlane(const gtsam::Point2& p) const; - gtsam::Point2 nPlaneToSpace(const gtsam::Point2& p) const; - - // Manifold - size_t dim() const; - static size_t Dim(); - gtsam::Cal3Unified retract(Vector v) const; - Vector localCoordinates(const gtsam::Cal3Unified& c) const; - - // enabling serialization functionality - void serialize() const; -}; - -#include -class Cal3_S2Stereo { - // Standard Constructors - Cal3_S2Stereo(); - Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b); - Cal3_S2Stereo(Vector v); - - // Testable - void print(string s) const; - bool equals(const gtsam::Cal3_S2Stereo& K, double tol) const; - - // Standard Interface - double fx() const; - double fy() const; - double skew() const; - double px() const; - double py() const; - gtsam::Point2 principalPoint() const; - double baseline() const; -}; - -#include -class Cal3Bundler { - // Standard Constructors - Cal3Bundler(); - Cal3Bundler(double fx, double k1, double k2, double u0, double v0); - - // Testable - void print(string s) const; - bool equals(const gtsam::Cal3Bundler& rhs, double tol) const; - - // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::Cal3Bundler retract(Vector v) const; - 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 - double fx() const; - double fy() const; - double k1() const; - double k2() const; - double u0() const; - double v0() const; - Vector vector() const; - Vector k() const; - //Matrix K() const; //FIXME: Uppercase - - // enabling serialization functionality - void serialize() const; -}; - -class CalibratedCamera { - // Standard Constructors and Named Constructors - CalibratedCamera(); - CalibratedCamera(const gtsam::Pose3& pose); - CalibratedCamera(const Vector& v); - static gtsam::CalibratedCamera Level(const gtsam::Pose2& pose2, double height); - - // Testable - void print(string s) const; - bool equals(const gtsam::CalibratedCamera& camera, double tol) const; - - // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::CalibratedCamera retract(const Vector& d) const; - Vector localCoordinates(const gtsam::CalibratedCamera& T2) const; - - // Action on Point3 - gtsam::Point2 project(const gtsam::Point3& point) const; - static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); - - // Standard Interface - gtsam::Pose3 pose() const; - double range(const gtsam::Point3& p) const; // TODO: Other overloaded range methods - - // enabling serialization functionality - void serialize() const; -}; - -template -class PinholeCamera { - // Standard Constructors and Named Constructors - PinholeCamera(); - PinholeCamera(const gtsam::Pose3& pose); - PinholeCamera(const gtsam::Pose3& pose, const CALIBRATION& K); - static This Level(const CALIBRATION& K, const gtsam::Pose2& pose, double height); - static This Level(const gtsam::Pose2& pose, double height); - static This Lookat(const gtsam::Point3& eye, const gtsam::Point3& target, - const gtsam::Point3& upVector, const CALIBRATION& K); - - // Testable - void print(string s) const; - bool equals(const This& camera, double tol) const; - - // Standard Interface - gtsam::Pose3 pose() const; - CALIBRATION calibration() const; - - // Manifold - This retract(const Vector& d) const; - Vector localCoordinates(const This& T2) const; - size_t dim() const; - static size_t Dim(); - - // Transformations and measurement functions - static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); - pair projectSafe(const gtsam::Point3& pw) const; - gtsam::Point2 project(const gtsam::Point3& point); - gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; - double range(const gtsam::Point3& point); - double range(const gtsam::Pose3& point); - - // enabling serialization functionality - void serialize() const; -}; - -virtual class SimpleCamera { - // Standard Constructors and Named Constructors - SimpleCamera(); - SimpleCamera(const gtsam::Pose3& pose); - SimpleCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2& K); - static gtsam::SimpleCamera Level(const gtsam::Cal3_S2& K, const gtsam::Pose2& pose, double height); - static gtsam::SimpleCamera Level(const gtsam::Pose2& pose, double height); - static gtsam::SimpleCamera Lookat(const gtsam::Point3& eye, const gtsam::Point3& target, - const gtsam::Point3& upVector, const gtsam::Cal3_S2& K); - - // Testable - void print(string s) const; - bool equals(const gtsam::SimpleCamera& camera, double tol) const; - - // Standard Interface - gtsam::Pose3 pose() const; - gtsam::Cal3_S2 calibration() const; - - // Manifold - gtsam::SimpleCamera retract(const Vector& d) const; - Vector localCoordinates(const gtsam::SimpleCamera& T2) const; - size_t dim() const; - static size_t Dim(); - - // Transformations and measurement functions - static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); - pair projectSafe(const gtsam::Point3& pw) const; - gtsam::Point2 project(const gtsam::Point3& point); - gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; - double range(const gtsam::Point3& point); - double range(const gtsam::Pose3& point); - - // enabling serialization functionality - void serialize() const; - -}; - -// Some typedefs for common camera types -// PinholeCameraCal3_S2 is the same as SimpleCamera above -typedef gtsam::PinholeCamera PinholeCameraCal3_S2; -typedef gtsam::PinholeCamera PinholeCameraCal3DS2; -typedef gtsam::PinholeCamera PinholeCameraCal3Unified; -typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; - -class StereoCamera { - // Standard Constructors and Named Constructors - StereoCamera(); - StereoCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2Stereo* K); - - // Testable - void print(string s) const; - bool equals(const gtsam::StereoCamera& camera, double tol) const; - - // Standard Interface - gtsam::Pose3 pose() const; - double baseline() const; - gtsam::Cal3_S2Stereo calibration() const; - - // Manifold - gtsam::StereoCamera retract(const Vector& d) const; - Vector localCoordinates(const gtsam::StereoCamera& T2) const; - size_t dim() const; - static size_t Dim(); - - // Transformations and measurement functions - gtsam::StereoPoint2 project(const gtsam::Point3& point); - gtsam::Point3 backproject(const gtsam::StereoPoint2& p) const; - - // enabling serialization functionality - void serialize() const; -}; - -#include - -// Templates appear not yet supported for free functions -gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, - gtsam::Cal3_S2* sharedCal, const gtsam::Point2Vector& measurements, - double rank_tol, bool optimize); -gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, - gtsam::Cal3Bundler* sharedCal, const gtsam::Point2Vector& measurements, - double rank_tol, bool optimize); - -//************************************************************************* -// Symbolic -//************************************************************************* - -#include -virtual class SymbolicFactor { - // Standard Constructors and Named Constructors - SymbolicFactor(const gtsam::SymbolicFactor& f); - SymbolicFactor(); - SymbolicFactor(size_t j); - SymbolicFactor(size_t j1, size_t j2); - SymbolicFactor(size_t j1, size_t j2, size_t j3); - SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4); - SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5); - SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5, size_t j6); - static gtsam::SymbolicFactor FromKeys(const gtsam::KeyVector& js); - - // From Factor - size_t size() const; - void print(string s) const; - bool equals(const gtsam::SymbolicFactor& other, double tol) const; - gtsam::KeyVector keys(); -}; - -#include -virtual class SymbolicFactorGraph { - SymbolicFactorGraph(); - SymbolicFactorGraph(const gtsam::SymbolicBayesNet& bayesNet); - SymbolicFactorGraph(const gtsam::SymbolicBayesTree& bayesTree); - - // From FactorGraph - void push_back(gtsam::SymbolicFactor* factor); - void print(string s) const; - bool equals(const gtsam::SymbolicFactorGraph& rhs, double tol) const; - size_t size() const; - bool exists(size_t idx) const; - - // Standard interface - gtsam::KeySet keys() const; - void push_back(gtsam::SymbolicFactor* factor); - void push_back(const gtsam::SymbolicFactorGraph& graph); - void push_back(const gtsam::SymbolicBayesNet& bayesNet); - void push_back(const gtsam::SymbolicBayesTree& bayesTree); - - //Advanced Interface - void push_factor(size_t key); - void push_factor(size_t key1, size_t key2); - void push_factor(size_t key1, size_t key2, size_t key3); - void push_factor(size_t key1, size_t key2, size_t key3, size_t key4); - - gtsam::SymbolicBayesNet* eliminateSequential(); - gtsam::SymbolicBayesNet* eliminateSequential(const gtsam::Ordering& ordering); - gtsam::SymbolicBayesTree* eliminateMultifrontal(); - gtsam::SymbolicBayesTree* eliminateMultifrontal(const gtsam::Ordering& ordering); - pair eliminatePartialSequential( - const gtsam::Ordering& ordering); - pair eliminatePartialSequential( - const gtsam::KeyVector& keys); - pair eliminatePartialMultifrontal( - const gtsam::Ordering& ordering); - pair eliminatePartialMultifrontal( - const gtsam::KeyVector& keys); - gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& variables); - gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& variables); - gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& variables, - const gtsam::Ordering& marginalizedVariableOrdering); - gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& variables, - const gtsam::Ordering& marginalizedVariableOrdering); - gtsam::SymbolicFactorGraph* marginal(const gtsam::KeyVector& variables); -}; - -#include -virtual class SymbolicConditional : gtsam::SymbolicFactor { - // Standard Constructors and Named Constructors - SymbolicConditional(); - SymbolicConditional(const gtsam::SymbolicConditional& other); - SymbolicConditional(size_t key); - SymbolicConditional(size_t key, size_t parent); - SymbolicConditional(size_t key, size_t parent1, size_t parent2); - SymbolicConditional(size_t key, size_t parent1, size_t parent2, size_t parent3); - static gtsam::SymbolicConditional FromKeys(const gtsam::KeyVector& keys, size_t nrFrontals); - - // Testable - void print(string s) const; - bool equals(const gtsam::SymbolicConditional& other, double tol) const; - - // Standard interface - size_t nrFrontals() const; - size_t nrParents() const; -}; - -#include -class SymbolicBayesNet { - SymbolicBayesNet(); - SymbolicBayesNet(const gtsam::SymbolicBayesNet& other); - // Testable - void print(string s) const; - bool equals(const gtsam::SymbolicBayesNet& other, double tol) const; - - // Standard interface - size_t size() const; - void saveGraph(string s) const; - gtsam::SymbolicConditional* at(size_t idx) const; - gtsam::SymbolicConditional* front() const; - gtsam::SymbolicConditional* back() const; - void push_back(gtsam::SymbolicConditional* conditional); - void push_back(const gtsam::SymbolicBayesNet& bayesNet); -}; - -#include -class SymbolicBayesTree { - - //Constructors - SymbolicBayesTree(); - SymbolicBayesTree(const gtsam::SymbolicBayesTree& other); - - // Testable - void print(string s); - bool equals(const gtsam::SymbolicBayesTree& other, double tol) const; - - //Standard Interface - //size_t findParentClique(const gtsam::IndexVector& parents) const; - size_t size(); - void saveGraph(string s) const; - void clear(); - void deleteCachedShortcuts(); - size_t numCachedSeparatorMarginals() const; - - gtsam::SymbolicConditional* marginalFactor(size_t key) const; - gtsam::SymbolicFactorGraph* joint(size_t key1, size_t key2) const; - gtsam::SymbolicBayesNet* jointBayesNet(size_t key1, size_t key2) const; -}; - -// class SymbolicBayesTreeClique { -// BayesTreeClique(); -// BayesTreeClique(CONDITIONAL* conditional); -// // BayesTreeClique(const std::pair& result) : Base(result) {} -// -// bool equals(const This& other, double tol) const; -// void print(string s) const; -// void printTree() const; // Default indent of "" -// void printTree(string indent) const; -// size_t numCachedSeparatorMarginals() const; -// -// CONDITIONAL* conditional() const; -// bool isRoot() const; -// size_t treeSize() const; -// // const std::list& children() const { return children_; } -// // derived_ptr parent() const { return parent_.lock(); } -// -// // FIXME: need wrapped versions graphs, BayesNet -// // BayesNet shortcut(derived_ptr root, Eliminate function) const; -// // FactorGraph marginal(derived_ptr root, Eliminate function) const; -// // FactorGraph joint(derived_ptr C2, derived_ptr root, Eliminate function) const; -// -// void deleteCachedShortcuts(); -// }; - -#include -class VariableIndex { - // Standard Constructors and Named Constructors - VariableIndex(); - // TODO: Templetize constructor when wrap supports it - //template - //VariableIndex(const T& factorGraph, size_t nVariables); - //VariableIndex(const T& factorGraph); - VariableIndex(const gtsam::SymbolicFactorGraph& factorGraph); - VariableIndex(const gtsam::GaussianFactorGraph& factorGraph); - VariableIndex(const gtsam::NonlinearFactorGraph& factorGraph); - VariableIndex(const gtsam::VariableIndex& other); - - // Testable - bool equals(const gtsam::VariableIndex& other, double tol) const; - void print(string s) const; - - // Standard interface - size_t size() const; - size_t nFactors() const; - size_t nEntries() const; -}; - -//************************************************************************* -// linear -//************************************************************************* - -namespace noiseModel { -#include -virtual class Base { -}; - -virtual class Gaussian : gtsam::noiseModel::Base { - static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R); - static gtsam::noiseModel::Gaussian* Covariance(Matrix R); - Matrix R() const; - bool equals(gtsam::noiseModel::Base& expected, double tol); - void print(string s) const; - - // enabling serialization functionality - void serializable() const; -}; - -virtual class Diagonal : gtsam::noiseModel::Gaussian { - static gtsam::noiseModel::Diagonal* Sigmas(Vector sigmas); - static gtsam::noiseModel::Diagonal* Variances(Vector variances); - static gtsam::noiseModel::Diagonal* Precisions(Vector precisions); - Matrix R() const; - void print(string s) const; - - // enabling serialization functionality - void serializable() const; -}; - -virtual class Constrained : gtsam::noiseModel::Diagonal { - static gtsam::noiseModel::Constrained* MixedSigmas(const Vector& mu, const Vector& sigmas); - static gtsam::noiseModel::Constrained* MixedSigmas(double m, const Vector& sigmas); - static gtsam::noiseModel::Constrained* MixedVariances(const Vector& mu, const Vector& variances); - static gtsam::noiseModel::Constrained* MixedVariances(const Vector& variances); - static gtsam::noiseModel::Constrained* MixedPrecisions(const Vector& mu, const Vector& precisions); - static gtsam::noiseModel::Constrained* MixedPrecisions(const Vector& precisions); - - static gtsam::noiseModel::Constrained* All(size_t dim); - static gtsam::noiseModel::Constrained* All(size_t dim, double mu); - - gtsam::noiseModel::Constrained* unit() const; - - // enabling serialization functionality - void serializable() const; -}; - -virtual class Isotropic : gtsam::noiseModel::Diagonal { - static gtsam::noiseModel::Isotropic* Sigma(size_t dim, double sigma); - static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace); - static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision); - void print(string s) const; - - // enabling serialization functionality - void serializable() const; -}; - -virtual class Unit : gtsam::noiseModel::Isotropic { - static gtsam::noiseModel::Unit* Create(size_t dim); - void print(string s) const; - - // enabling serialization functionality - void serializable() const; -}; - -namespace mEstimator { -virtual class Base { -}; - -virtual class Null: gtsam::noiseModel::mEstimator::Base { - Null(); - void print(string s) const; - static gtsam::noiseModel::mEstimator::Null* Create(); - - // enabling serialization functionality - void serializable() const; -}; - -virtual class Fair: gtsam::noiseModel::mEstimator::Base { - Fair(double c); - void print(string s) const; - static gtsam::noiseModel::mEstimator::Fair* Create(double c); - - // enabling serialization functionality - void serializable() const; -}; - -virtual class Huber: gtsam::noiseModel::mEstimator::Base { - Huber(double k); - void print(string s) const; - static gtsam::noiseModel::mEstimator::Huber* Create(double k); - - // enabling serialization functionality - void serializable() const; -}; - -virtual class Tukey: gtsam::noiseModel::mEstimator::Base { - Tukey(double k); - void print(string s) const; - static gtsam::noiseModel::mEstimator::Tukey* Create(double k); - - // enabling serialization functionality - void serializable() const; -}; - -}///\namespace mEstimator - -virtual class Robust : gtsam::noiseModel::Base { - Robust(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise); - static gtsam::noiseModel::Robust* Create(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise); - void print(string s) const; - - // enabling serialization functionality - void serializable() const; -}; - -}///\namespace noiseModel - -#include -class Sampler { - //Constructors - Sampler(gtsam::noiseModel::Diagonal* model, int seed); - Sampler(Vector sigmas, int seed); - Sampler(int seed); - - //Standard Interface - size_t dim() const; - Vector sigmas() const; - gtsam::noiseModel::Diagonal* model() const; - Vector sample(); - Vector sampleNewModel(gtsam::noiseModel::Diagonal* model); -}; - -#include -class VectorValues { - //Constructors - VectorValues(); - VectorValues(const gtsam::VectorValues& other); - - //Named Constructors - static gtsam::VectorValues Zero(const gtsam::VectorValues& model); - - //Standard Interface - size_t size() const; - size_t dim(size_t j) const; - bool exists(size_t j) const; - void print(string s) const; - bool equals(const gtsam::VectorValues& expected, double tol) const; - void insert(size_t j, Vector value); - Vector vector() const; - Vector at(size_t j) const; - void update(const gtsam::VectorValues& values); - - //Advanced Interface - void setZero(); - - gtsam::VectorValues add(const gtsam::VectorValues& c) const; - void addInPlace(const gtsam::VectorValues& c); - gtsam::VectorValues subtract(const gtsam::VectorValues& c) const; - gtsam::VectorValues scale(double a) const; - void scaleInPlace(double a); - - bool hasSameStructure(const gtsam::VectorValues& other) const; - double dot(const gtsam::VectorValues& V) const; - double norm() const; - double squaredNorm() const; - - // enabling serialization functionality - void serialize() const; -}; - -#include -virtual class GaussianFactor { - gtsam::KeyVector keys() const; - void print(string s) const; - bool equals(const gtsam::GaussianFactor& lf, double tol) const; - double error(const gtsam::VectorValues& c) const; - gtsam::GaussianFactor* clone() const; - gtsam::GaussianFactor* negate() const; - Matrix augmentedInformation() const; - Matrix information() const; - Matrix augmentedJacobian() const; - pair jacobian() const; - size_t size() const; - bool empty() const; -}; - -#include -virtual class JacobianFactor : gtsam::GaussianFactor { - //Constructors - JacobianFactor(); - JacobianFactor(const gtsam::GaussianFactor& factor); - JacobianFactor(Vector b_in); - JacobianFactor(size_t i1, Matrix A1, Vector b, - const gtsam::noiseModel::Diagonal* model); - JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, Vector b, - const gtsam::noiseModel::Diagonal* model); - JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3, - Vector b, const gtsam::noiseModel::Diagonal* model); - JacobianFactor(const gtsam::GaussianFactorGraph& graph); - - //Testable - void print(string s) const; - void printKeys(string s) const; - bool equals(const gtsam::GaussianFactor& lf, double tol) const; - size_t size() const; - Vector unweighted_error(const gtsam::VectorValues& c) const; - Vector error_vector(const gtsam::VectorValues& c) const; - double error(const gtsam::VectorValues& c) const; - - //Standard Interface - Matrix getA() const; - Vector getb() const; - size_t rows() const; - size_t cols() const; - bool isConstrained() const; - pair jacobianUnweighted() const; - Matrix augmentedJacobianUnweighted() const; - - void transposeMultiplyAdd(double alpha, const Vector& e, gtsam::VectorValues& x) const; - gtsam::JacobianFactor whiten() const; - - pair eliminate(const gtsam::Ordering& keys) const; - - void setModel(bool anyConstrained, const Vector& sigmas); - - gtsam::noiseModel::Diagonal* get_model() const; - - // enabling serialization functionality - void serialize() const; -}; - -#include -virtual class HessianFactor : gtsam::GaussianFactor { - //Constructors - HessianFactor(); - HessianFactor(const gtsam::GaussianFactor& factor); - HessianFactor(size_t j, Matrix G, Vector g, double f); - HessianFactor(size_t j, Vector mu, Matrix Sigma); - HessianFactor(size_t j1, size_t j2, Matrix G11, Matrix G12, Vector g1, Matrix G22, - Vector g2, double f); - HessianFactor(size_t j1, size_t j2, size_t j3, Matrix G11, Matrix G12, Matrix G13, - Vector g1, Matrix G22, Matrix G23, Vector g2, Matrix G33, Vector g3, - double f); - HessianFactor(const gtsam::GaussianFactorGraph& factors); - - //Testable - size_t size() const; - void print(string s) const; - void printKeys(string s) const; - bool equals(const gtsam::GaussianFactor& lf, double tol) const; - double error(const gtsam::VectorValues& c) const; - - //Standard Interface - size_t rows() const; - Matrix info() const; - double constantTerm() const; - Vector linearTerm() const; - - // enabling serialization functionality - void serialize() const; -}; - -#include -class GaussianFactorGraph { - GaussianFactorGraph(); - GaussianFactorGraph(const gtsam::GaussianBayesNet& bayesNet); - GaussianFactorGraph(const gtsam::GaussianBayesTree& bayesTree); - - // From FactorGraph - void print(string s) const; - bool equals(const gtsam::GaussianFactorGraph& lfgraph, double tol) const; - size_t size() const; - gtsam::GaussianFactor* at(size_t idx) const; - gtsam::KeySet keys() const; - bool exists(size_t idx) const; - - // Building the graph - void push_back(const gtsam::GaussianFactor* factor); - void push_back(const gtsam::GaussianConditional* factor); - void push_back(const gtsam::GaussianFactorGraph& graph); - void push_back(const gtsam::GaussianBayesNet& bayesNet); - void push_back(const gtsam::GaussianBayesTree& bayesTree); - void add(const gtsam::GaussianFactor& factor); - void add(Vector b); - void add(size_t key1, Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model); - void add(size_t key1, Matrix A1, size_t key2, Matrix A2, Vector b, - const gtsam::noiseModel::Diagonal* model); - void add(size_t key1, Matrix A1, size_t key2, Matrix A2, size_t key3, Matrix A3, - Vector b, const gtsam::noiseModel::Diagonal* model); - - // error and probability - double error(const gtsam::VectorValues& c) const; - double probPrime(const gtsam::VectorValues& c) const; - - gtsam::GaussianFactorGraph clone() const; - gtsam::GaussianFactorGraph negate() const; - - // Optimizing and linear algebra - gtsam::VectorValues optimize() const; - gtsam::VectorValues optimize(const gtsam::Ordering& ordering) const; - gtsam::VectorValues optimizeGradientSearch() const; - gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; - gtsam::VectorValues gradientAtZero() const; - - // Elimination and marginals - gtsam::GaussianBayesNet* eliminateSequential(); - gtsam::GaussianBayesNet* eliminateSequential(const gtsam::Ordering& ordering); - gtsam::GaussianBayesTree* eliminateMultifrontal(); - gtsam::GaussianBayesTree* eliminateMultifrontal(const gtsam::Ordering& ordering); - pair eliminatePartialSequential( - const gtsam::Ordering& ordering); - pair eliminatePartialSequential( - const gtsam::KeyVector& keys); - pair eliminatePartialMultifrontal( - const gtsam::Ordering& ordering); - pair eliminatePartialMultifrontal( - const gtsam::KeyVector& keys); - gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& variables); - gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& variables); - gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& variables, - const gtsam::Ordering& marginalizedVariableOrdering); - gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& variables, - const gtsam::Ordering& marginalizedVariableOrdering); - gtsam::GaussianFactorGraph* marginal(const gtsam::KeyVector& variables); - - // Conversion to matrices - Matrix sparseJacobian_() const; - Matrix augmentedJacobian() const; - Matrix augmentedJacobian(const gtsam::Ordering& ordering) const; - pair jacobian() const; - pair jacobian(const gtsam::Ordering& ordering) const; - Matrix augmentedHessian() const; - Matrix augmentedHessian(const gtsam::Ordering& ordering) const; - pair hessian() const; - pair hessian(const gtsam::Ordering& ordering) const; - - // enabling serialization functionality - void serialize() const; -}; - -#include -virtual class GaussianConditional : gtsam::GaussianFactor { - //Constructors - GaussianConditional(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas); - GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, - const gtsam::noiseModel::Diagonal* sigmas); - GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, - size_t name2, Matrix T, const gtsam::noiseModel::Diagonal* sigmas); - - //Constructors with no noise model - GaussianConditional(size_t key, Vector d, Matrix R); - GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S); - GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, - size_t name2, Matrix T); - - //Standard Interface - void print(string s) const; - bool equals(const gtsam::GaussianConditional &cg, double tol) const; - - //Advanced Interface - gtsam::VectorValues solve(const gtsam::VectorValues& parents) const; - gtsam::VectorValues solveOtherRHS(const gtsam::VectorValues& parents, const gtsam::VectorValues& rhs) const; - void solveTransposeInPlace(gtsam::VectorValues& gy) const; - void scaleFrontalsBySigma(gtsam::VectorValues& gy) const; - - // enabling serialization functionality - void serialize() const; -}; - -#include -virtual class GaussianDensity : gtsam::GaussianConditional { - //Constructors - GaussianDensity(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas); - - //Standard Interface - void print(string s) const; - bool equals(const gtsam::GaussianDensity &cg, double tol) const; - Vector mean() const; - Matrix covariance() const; -}; - -#include -virtual class GaussianBayesNet { - //Constructors - GaussianBayesNet(); - GaussianBayesNet(const gtsam::GaussianConditional* conditional); - - // Testable - void print(string s) const; - bool equals(const gtsam::GaussianBayesNet& other, double tol) const; - size_t size() const; - - // FactorGraph derived interface - size_t size() const; - gtsam::GaussianConditional* at(size_t idx) const; - gtsam::KeySet keys() const; - bool exists(size_t idx) const; - - gtsam::GaussianConditional* front() const; - gtsam::GaussianConditional* back() const; - void push_back(gtsam::GaussianConditional* conditional); - void push_back(const gtsam::GaussianBayesNet& bayesNet); - - gtsam::VectorValues optimize() const; - gtsam::VectorValues optimize(gtsam::VectorValues& solutionForMissing) const; - gtsam::VectorValues optimizeGradientSearch() const; - gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; - gtsam::VectorValues gradientAtZero() const; - double error(const gtsam::VectorValues& x) const; - double determinant() const; - double logDeterminant() const; - gtsam::VectorValues backSubstitute(const gtsam::VectorValues& gx) const; - gtsam::VectorValues backSubstituteTranspose(const gtsam::VectorValues& gx) const; -}; - -#include -virtual class GaussianBayesTree { - // Standard Constructors and Named Constructors - GaussianBayesTree(); - GaussianBayesTree(const gtsam::GaussianBayesTree& other); - bool equals(const gtsam::GaussianBayesTree& other, double tol) const; - void print(string s); - size_t size() const; - bool empty() const; - size_t numCachedSeparatorMarginals() const; - void saveGraph(string s) const; - - gtsam::VectorValues optimize() const; - gtsam::VectorValues optimizeGradientSearch() const; - gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; - gtsam::VectorValues gradientAtZero() const; - double error(const gtsam::VectorValues& x) const; - double determinant() const; - double logDeterminant() const; - Matrix marginalCovariance(size_t key) const; - gtsam::GaussianConditional* marginalFactor(size_t key) const; - gtsam::GaussianFactorGraph* joint(size_t key1, size_t key2) const; - gtsam::GaussianBayesNet* jointBayesNet(size_t key1, size_t key2) const; -}; - -class Errors { - //Constructors - Errors(); - Errors(const gtsam::VectorValues& V); - - //Testable - void print(string s); - bool equals(const gtsam::Errors& expected, double tol) const; -}; - -class GaussianISAM { - //Constructor - GaussianISAM(); - - //Standard Interface - void update(const gtsam::GaussianFactorGraph& newFactors); - void saveGraph(string s) const; - void clear(); -}; - -#include -virtual class IterativeOptimizationParameters { - string getVerbosity() const; - void setVerbosity(string s) ; - void print() const; -}; - -//virtual class IterativeSolver { -// IterativeSolver(); -// gtsam::VectorValues optimize (); -//}; - -#include -virtual class ConjugateGradientParameters : gtsam::IterativeOptimizationParameters { - ConjugateGradientParameters(); - int getMinIterations() const ; - int getMaxIterations() const ; - int getReset() const; - double getEpsilon_rel() const; - double getEpsilon_abs() const; - - void setMinIterations(int value); - void setMaxIterations(int value); - void setReset(int value); - void setEpsilon_rel(double value); - void setEpsilon_abs(double value); - void print(); -}; - -#include -virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters { - SubgraphSolverParameters(); - void print() const; -}; - -virtual class SubgraphSolver { - SubgraphSolver(const gtsam::GaussianFactorGraph &A, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering& ordering); - SubgraphSolver(const gtsam::GaussianFactorGraph &Ab1, const gtsam::GaussianFactorGraph &Ab2, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering& ordering); - gtsam::VectorValues optimize() const; -}; - -#include -class KalmanFilter { - KalmanFilter(size_t n); - // gtsam::GaussianDensity* init(Vector x0, const gtsam::SharedDiagonal& P0); - gtsam::GaussianDensity* init(Vector x0, Matrix P0); - void print(string s) const; - static size_t step(gtsam::GaussianDensity* p); - gtsam::GaussianDensity* predict(gtsam::GaussianDensity* p, Matrix F, - Matrix B, Vector u, const gtsam::noiseModel::Diagonal* modelQ); - gtsam::GaussianDensity* predictQ(gtsam::GaussianDensity* p, Matrix F, - Matrix B, Vector u, Matrix Q); - gtsam::GaussianDensity* predict2(gtsam::GaussianDensity* p, Matrix A0, - Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model); - gtsam::GaussianDensity* update(gtsam::GaussianDensity* p, Matrix H, - Vector z, const gtsam::noiseModel::Diagonal* model); - gtsam::GaussianDensity* updateQ(gtsam::GaussianDensity* p, Matrix H, - Vector z, Matrix Q); -}; - -//************************************************************************* -// nonlinear -//************************************************************************* - -#include -size_t symbol(char chr, size_t index); -char symbolChr(size_t key); -size_t symbolIndex(size_t key); - -// Default keyformatter -void PrintKeyList (const gtsam::KeyList& keys); -void PrintKeyList (const gtsam::KeyList& keys, string s); -void PrintKeyVector(const gtsam::KeyVector& keys); -void PrintKeyVector(const gtsam::KeyVector& keys, string s); -void PrintKeySet (const gtsam::KeySet& keys); -void PrintKeySet (const gtsam::KeySet& keys, string s); - -#include -class LabeledSymbol { - LabeledSymbol(size_t full_key); - LabeledSymbol(const gtsam::LabeledSymbol& key); - LabeledSymbol(unsigned char valType, unsigned char label, size_t j); - - size_t key() const; - unsigned char label() const; - unsigned char chr() const; - size_t index() const; - - gtsam::LabeledSymbol upper() const; - gtsam::LabeledSymbol lower() const; - gtsam::LabeledSymbol newChr(unsigned char c) const; - gtsam::LabeledSymbol newLabel(unsigned char label) const; - - void print(string s) const; -}; - -size_t mrsymbol(unsigned char c, unsigned char label, size_t j); -unsigned char mrsymbolChr(size_t key); -unsigned char mrsymbolLabel(size_t key); -size_t mrsymbolIndex(size_t key); - -#include -class Ordering { - // Standard Constructors and Named Constructors - Ordering(); - Ordering(const gtsam::Ordering& other); - - // Testable - void print(string s) const; - bool equals(const gtsam::Ordering& ord, double tol) const; - - // Standard interface - size_t size() const; - size_t at(size_t key) const; - void push_back(size_t key); - - // enabling serialization functionality - void serialize() const; -}; - -class NonlinearFactorGraph { - NonlinearFactorGraph(); - NonlinearFactorGraph(const gtsam::NonlinearFactorGraph& graph); - - // FactorGraph - void print(string s) const; - bool equals(const gtsam::NonlinearFactorGraph& fg, double tol) const; - size_t size() const; - bool empty() const; - void remove(size_t i); - size_t nrFactors() const; - gtsam::NonlinearFactor* at(size_t idx) const; - void push_back(const gtsam::NonlinearFactorGraph& factors); - void push_back(gtsam::NonlinearFactor* factor); - void add(gtsam::NonlinearFactor* factor); - bool exists(size_t idx) const; - gtsam::KeySet keys() const; - - // NonlinearFactorGraph - double error(const gtsam::Values& values) const; - double probPrime(const gtsam::Values& values) const; - gtsam::Ordering orderingCOLAMD() const; - // Ordering* orderingCOLAMDConstrained(const gtsam::Values& c, const std::map& constraints) const; - gtsam::GaussianFactorGraph* linearize(const gtsam::Values& values) const; - gtsam::NonlinearFactorGraph clone() const; - - // enabling serialization functionality - void serialize() const; -}; - -virtual class NonlinearFactor { - // Factor base class - size_t size() const; - gtsam::KeyVector keys() const; - void print(string s) const; - void printKeys(string s) const; - // NonlinearFactor - void equals(const gtsam::NonlinearFactor& other, double tol) const; - double error(const gtsam::Values& c) const; - size_t dim() const; - bool active(const gtsam::Values& c) const; - gtsam::GaussianFactor* linearize(const gtsam::Values& c) const; - gtsam::NonlinearFactor* clone() const; - // gtsam::NonlinearFactor* rekey(const gtsam::KeyVector& newKeys) const; //FIXME: Conversion from KeyVector to std::vector does not happen -}; - -virtual class NoiseModelFactor: gtsam::NonlinearFactor { - void equals(const gtsam::NoiseModelFactor& other, double tol) const; - gtsam::noiseModel::Base* noiseModel() const; - Vector unwhitenedError(const gtsam::Values& x) const; - Vector whitenedError(const gtsam::Values& x) const; -}; - -#include -class Values { - Values(); - Values(const gtsam::Values& other); - - size_t size() const; - bool empty() const; - void clear(); - size_t dim() const; - - void print(string s) const; - bool equals(const gtsam::Values& other, double tol) const; - - void insert(const gtsam::Values& values); - void update(const gtsam::Values& values); - void erase(size_t j); - void swap(gtsam::Values& values); - - bool exists(size_t j) const; - gtsam::KeyVector keys() const; - - gtsam::VectorValues zeroVectors() const; - - gtsam::Values retract(const gtsam::VectorValues& delta) const; - gtsam::VectorValues localCoordinates(const gtsam::Values& cp) const; - - // enabling serialization functionality - void serialize() const; - - // New in 4.0, we have to specialize every insert/update/at to generate wrappers - // Instead of the old: - // void insert(size_t j, const gtsam::Value& value); - // void update(size_t j, const gtsam::Value& val); - // gtsam::Value at(size_t j) const; - - void insert(size_t j, const gtsam::Point2& t); - void insert(size_t j, const gtsam::Point3& t); - void insert(size_t j, const gtsam::Rot2& t); - void insert(size_t j, const gtsam::Pose2& t); - void insert(size_t j, const gtsam::Rot3& t); - void insert(size_t j, const gtsam::Pose3& t); - void insert(size_t j, const gtsam::Cal3_S2& t); - void insert(size_t j, const gtsam::Cal3DS2& t); - void insert(size_t j, const gtsam::Cal3Bundler& t); - void insert(size_t j, const gtsam::EssentialMatrix& t); - void insert(size_t j, const gtsam::SimpleCamera& t); - void insert(size_t j, const gtsam::imuBias::ConstantBias& t); - void insert(size_t j, Vector t); - void insert(size_t j, Matrix t); - - // Fixed size version - void insertFixed(size_t j, Vector t, size_t n); - - void update(size_t j, const gtsam::Point2& t); - void update(size_t j, const gtsam::Point3& t); - void update(size_t j, const gtsam::Rot2& t); - void update(size_t j, const gtsam::Pose2& t); - void update(size_t j, const gtsam::Rot3& t); - void update(size_t j, const gtsam::Pose3& t); - void update(size_t j, const gtsam::Cal3_S2& t); - void update(size_t j, const gtsam::Cal3DS2& t); - void update(size_t j, const gtsam::Cal3Bundler& t); - void update(size_t j, const gtsam::EssentialMatrix& t); - void update(size_t j, const gtsam::imuBias::ConstantBias& t); - void update(size_t j, Vector t); - void update(size_t j, Matrix t); - - template - T at(size_t j); - - /// Fixed size versions, for n in 1..9 - void insertFixed(size_t j, Vector t, size_t n); - - /// Fixed size versions, for n in 1..9 - Vector atFixed(size_t j, size_t n); - - /// version for double - void insertDouble(size_t j, double c); - double atDouble(size_t j) const; -}; - -// Actually a FastList -#include -class KeyList { - KeyList(); - KeyList(const gtsam::KeyList& other); - - // Note: no print function - - // common STL methods - size_t size() const; - bool empty() const; - void clear(); - - // structure specific methods - size_t front() const; - size_t back() const; - void push_back(size_t key); - void push_front(size_t key); - void pop_back(); - void pop_front(); - void sort(); - void remove(size_t key); - - void serialize() const; -}; - -// Actually a FastSet -class KeySet { - KeySet(); - KeySet(const gtsam::KeySet& other); - KeySet(const gtsam::KeyVector& other); - KeySet(const gtsam::KeyList& other); - - // Testable - void print(string s) const; - bool equals(const gtsam::KeySet& other) const; - - // common STL methods - size_t size() const; - bool empty() const; - void clear(); - - // structure specific methods - void insert(size_t key); - void merge(gtsam::KeySet& other); - bool erase(size_t key); // returns true if value was removed - bool count(size_t key) const; // returns true if value exists - - void serialize() const; -}; - -// Actually a vector -class KeyVector { - KeyVector(); - KeyVector(const gtsam::KeyVector& other); - - // Note: no print function - - // common STL methods - size_t size() const; - bool empty() const; - void clear(); - - // structure specific methods - size_t at(size_t i) const; - size_t front() const; - size_t back() const; - void push_back(size_t key) const; - - void serialize() const; -}; - -// Actually a FastMap -class KeyGroupMap { - KeyGroupMap(); - - // Note: no print function - - // common STL methods - size_t size() const; - bool empty() const; - void clear(); - - // structure specific methods - size_t at(size_t key) const; - int erase(size_t key); - bool insert2(size_t key, int val); -}; - -#include -class Marginals { - Marginals(const gtsam::NonlinearFactorGraph& graph, - const gtsam::Values& solution); - - void print(string s) const; - Matrix marginalCovariance(size_t variable) const; - Matrix marginalInformation(size_t variable) const; - gtsam::JointMarginal jointMarginalCovariance(const gtsam::KeyVector& variables) const; - gtsam::JointMarginal jointMarginalInformation(const gtsam::KeyVector& variables) const; -}; - -class JointMarginal { - Matrix at(size_t iVariable, size_t jVariable) const; - Matrix fullMatrix() const; - void print(string s) const; - void print() const; -}; - -#include -virtual class LinearContainerFactor : gtsam::NonlinearFactor { - - LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::Values& linearizationPoint); - LinearContainerFactor(gtsam::GaussianFactor* factor); - - gtsam::GaussianFactor* factor() const; -// const boost::optional& linearizationPoint() const; - - bool isJacobian() const; - gtsam::JacobianFactor* toJacobian() const; - gtsam::HessianFactor* toHessian() const; - - static gtsam::NonlinearFactorGraph convertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph, - const gtsam::Values& linearizationPoint); - - static gtsam::NonlinearFactorGraph convertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph); - - // enabling serialization functionality - void serializable() const; -}; // \class LinearContainerFactor - -// Summarization functionality -//#include -// -//// Uses partial QR approach by default -//gtsam::GaussianFactorGraph summarize( -// const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values, -// const gtsam::KeySet& saved_keys); -// -//gtsam::NonlinearFactorGraph summarizeAsNonlinearContainer( -// const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values, -// const gtsam::KeySet& saved_keys); - -//************************************************************************* -// Nonlinear optimizers -//************************************************************************* - -#include -#include -virtual class NonlinearOptimizerParams { - NonlinearOptimizerParams(); - void print(string s) const; - - int getMaxIterations() const; - double getRelativeErrorTol() const; - double getAbsoluteErrorTol() const; - double getErrorTol() const; - string getVerbosity() const; - - void setMaxIterations(int value); - void setRelativeErrorTol(double value); - void setAbsoluteErrorTol(double value); - void setErrorTol(double value); - void setVerbosity(string s); - - string getLinearSolverType() const; - - void setLinearSolverType(string solver); - void setOrdering(const gtsam::Ordering& ordering); - void setIterativeParams(gtsam::IterativeOptimizationParameters* params); - - bool isMultifrontal() const; - bool isSequential() const; - bool isCholmod() const; - bool isIterative() const; -}; - -bool checkConvergence(double relativeErrorTreshold, - double absoluteErrorTreshold, double errorThreshold, - double currentError, double newError); - -#include -virtual class GaussNewtonParams : gtsam::NonlinearOptimizerParams { - GaussNewtonParams(); -}; - -#include -virtual class LevenbergMarquardtParams : gtsam::NonlinearOptimizerParams { - LevenbergMarquardtParams(); - - double getlambdaInitial() const; - double getlambdaFactor() const; - double getlambdaUpperBound() const; - string getVerbosityLM() const; - - void setlambdaInitial(double value); - void setlambdaFactor(double value); - void setlambdaUpperBound(double value); - void setVerbosityLM(string s); -}; - -#include -virtual class DoglegParams : gtsam::NonlinearOptimizerParams { - DoglegParams(); - - double getDeltaInitial() const; - string getVerbosityDL() const; - - void setDeltaInitial(double deltaInitial) const; - void setVerbosityDL(string verbosityDL) const; -}; - -virtual class NonlinearOptimizer { - gtsam::Values optimize(); - gtsam::Values optimizeSafely(); - double error() const; - int iterations() const; - gtsam::Values values() const; - gtsam::GaussianFactorGraph* iterate() const; -}; - -virtual class GaussNewtonOptimizer : gtsam::NonlinearOptimizer { - GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); - GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::GaussNewtonParams& params); -}; - -virtual class DoglegOptimizer : gtsam::NonlinearOptimizer { - DoglegOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); - DoglegOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::DoglegParams& params); - double getDelta() const; -}; - -virtual class LevenbergMarquardtOptimizer : gtsam::NonlinearOptimizer { - LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); - LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::LevenbergMarquardtParams& params); - double lambda() const; - void print(string str) const; -}; - -#include -class ISAM2GaussNewtonParams { - ISAM2GaussNewtonParams(); - - void print(string str) const; - - /** Getters and Setters for all properties */ - double getWildfireThreshold() const; - void setWildfireThreshold(double wildfireThreshold); -}; - -class ISAM2DoglegParams { - ISAM2DoglegParams(); - - void print(string str) const; - - /** Getters and Setters for all properties */ - double getWildfireThreshold() const; - void setWildfireThreshold(double wildfireThreshold); - double getInitialDelta() const; - void setInitialDelta(double initialDelta); - string getAdaptationMode() const; - void setAdaptationMode(string adaptationMode); - bool isVerbose() const; - void setVerbose(bool verbose); -}; - -class ISAM2ThresholdMapValue { - ISAM2ThresholdMapValue(char c, Vector thresholds); - ISAM2ThresholdMapValue(const gtsam::ISAM2ThresholdMapValue& other); -}; - -class ISAM2ThresholdMap { - ISAM2ThresholdMap(); - ISAM2ThresholdMap(const gtsam::ISAM2ThresholdMap& other); - - // Note: no print function - - // common STL methods - size_t size() const; - bool empty() const; - void clear(); - - // structure specific methods - void insert(const gtsam::ISAM2ThresholdMapValue& value) const; -}; - -class ISAM2Params { - ISAM2Params(); - - void print(string str) const; - - /** Getters and Setters for all properties */ - void setOptimizationParams(const gtsam::ISAM2GaussNewtonParams& params); - void setOptimizationParams(const gtsam::ISAM2DoglegParams& params); - void setRelinearizeThreshold(double relinearizeThreshold); - void setRelinearizeThreshold(const gtsam::ISAM2ThresholdMap& relinearizeThreshold); - int getRelinearizeSkip() const; - void setRelinearizeSkip(int relinearizeSkip); - bool isEnableRelinearization() const; - void setEnableRelinearization(bool enableRelinearization); - bool isEvaluateNonlinearError() const; - void setEvaluateNonlinearError(bool evaluateNonlinearError); - string getFactorization() const; - void setFactorization(string factorization); - bool isCacheLinearizedFactors() const; - void setCacheLinearizedFactors(bool cacheLinearizedFactors); - bool isEnableDetailedResults() const; - void setEnableDetailedResults(bool enableDetailedResults); - bool isEnablePartialRelinearizationCheck() const; - void setEnablePartialRelinearizationCheck(bool enablePartialRelinearizationCheck); -}; - -class ISAM2Clique { - - //Constructors - ISAM2Clique(); - - //Standard Interface - Vector gradientContribution() const; - void print(string s); -}; - -class ISAM2Result { - ISAM2Result(); - - void print(string str) const; - - /** Getters and Setters for all properties */ - size_t getVariablesRelinearized() const; - size_t getVariablesReeliminated() const; - size_t getCliques() const; -}; - -class ISAM2 { - ISAM2(); - ISAM2(const gtsam::ISAM2Params& params); - ISAM2(const gtsam::ISAM2& other); - - bool equals(const gtsam::ISAM2& other, double tol) const; - void print(string s) const; - void printStats() const; - void saveGraph(string s) const; - - gtsam::ISAM2Result update(); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& 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::Values getLinearizationPoint() const; - gtsam::Values calculateEstimate() const; - gtsam::Value calculateEstimate(size_t key) const; - gtsam::Values calculateBestEstimate() const; - Matrix marginalCovariance(size_t key) const; - gtsam::VectorValues getDelta() const; - gtsam::NonlinearFactorGraph getFactorsUnsafe() const; - gtsam::VariableIndex getVariableIndex() const; - gtsam::ISAM2Params params() const; -}; - -#include -class NonlinearISAM { - NonlinearISAM(); - NonlinearISAM(int reorderInterval); - void print(string s) const; - void printStats() const; - void saveGraph(string s) const; - gtsam::Values estimate() const; - Matrix marginalCovariance(size_t key) const; - int reorderInterval() const; - int reorderCounter() const; - void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& initialValues); - void reorder_relinearize(); - - // These might be expensive as instead of a reference the wrapper will make a copy - gtsam::GaussianISAM bayesTree() const; - gtsam::Values getLinearizationPoint() const; - gtsam::NonlinearFactorGraph getFactorsUnsafe() const; -}; - -//************************************************************************* -// Nonlinear factor types -//************************************************************************* -#include -#include -#include - -#include -template -virtual class PriorFactor : gtsam::NoiseModelFactor { - PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); - T prior() const; - - // enabling serialization functionality - void serialize() const; -}; - - -#include -template -virtual class BetweenFactor : gtsam::NoiseModelFactor { - BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel); - T measured() const; - - // enabling serialization functionality - void serialize() const; -}; - - - -#include -template -virtual class NonlinearEquality : gtsam::NoiseModelFactor { - // Constructor - forces exact evaluation - NonlinearEquality(size_t j, const T& feasible); - // Constructor - allows inexact evaluation - NonlinearEquality(size_t j, const T& feasible, double error_gain); - - // enabling serialization functionality - void serialize() const; -}; - - -#include -template -virtual class RangeFactor : gtsam::NoiseModelFactor { - RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel); -}; - -typedef gtsam::RangeFactor RangeFactor2D; -typedef gtsam::RangeFactor RangeFactor3D; -typedef gtsam::RangeFactor RangeFactorPose2; -typedef gtsam::RangeFactor RangeFactorPose3; -typedef gtsam::RangeFactor RangeFactorCalibratedCameraPoint; -typedef gtsam::RangeFactor RangeFactorSimpleCameraPoint; -typedef gtsam::RangeFactor RangeFactorCalibratedCamera; -typedef gtsam::RangeFactor RangeFactorSimpleCamera; - - -#include -template -virtual class BearingFactor : gtsam::NoiseModelFactor { - BearingFactor(size_t key1, size_t key2, const BEARING& measured, const gtsam::noiseModel::Base* noiseModel); - - // enabling serialization functionality - void serialize() const; -}; - -typedef gtsam::BearingFactor BearingFactor2D; - -#include -template -virtual class BearingRangeFactor : gtsam::NoiseModelFactor { - BearingRangeFactor(size_t poseKey, size_t pointKey, - const BEARING& measuredBearing, const RANGE& measuredRange, - const gtsam::noiseModel::Base* noiseModel); - - // enabling serialization functionality - void serialize() const; -}; - -typedef gtsam::BearingRangeFactor BearingRangeFactor2D; - - -#include -template -virtual class GenericProjectionFactor : gtsam::NoiseModelFactor { - GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, - size_t poseKey, size_t pointKey, const CALIBRATION* k); - GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, - size_t poseKey, size_t pointKey, const CALIBRATION* k, const POSE& body_P_sensor); - - GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, - size_t poseKey, size_t pointKey, const CALIBRATION* k, bool throwCheirality, bool verboseCheirality); - GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, - size_t poseKey, size_t pointKey, const CALIBRATION* k, bool throwCheirality, bool verboseCheirality, - const POSE& body_P_sensor); - - gtsam::Point2 measured() const; - CALIBRATION* calibration() const; - bool verboseCheirality() const; - bool throwCheirality() const; - - // enabling serialization functionality - void serialize() const; -}; -typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3_S2; -typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3DS2; - - -#include -template -virtual class GeneralSFMFactor : gtsam::NoiseModelFactor { - GeneralSFMFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t cameraKey, size_t landmarkKey); - gtsam::Point2 measured() const; -}; -typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; -typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; - -template -virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { - GeneralSFMFactor2(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t poseKey, size_t landmarkKey, size_t calibKey); - gtsam::Point2 measured() const; - - // enabling serialization functionality - void serialize() const; -}; - -#include -class SmartProjectionParams { - SmartProjectionParams(); - // TODO(frank): make these work: - // void setLinearizationMode(LinearizationMode linMode); - // void setDegeneracyMode(DegeneracyMode degMode); - void setRankTolerance(double rankTol); - void setEnableEPI(bool enableEPI); - void setLandmarkDistanceThreshold(bool landmarkDistanceThreshold); - void setDynamicOutlierRejectionThreshold(bool dynOutRejectionThreshold); -}; - -#include -template -virtual class SmartProjectionPoseFactor: gtsam::NonlinearFactor { - - SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, - const CALIBRATION* K); - SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, - const CALIBRATION* K, - const gtsam::Pose3& body_P_sensor); - SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, - const CALIBRATION* K, - const gtsam::Pose3& body_P_sensor, - const gtsam::SmartProjectionParams& params); - - void add(const gtsam::Point2& measured_i, size_t poseKey_i); - - // enabling serialization functionality - //void serialize() const; -}; - -typedef gtsam::SmartProjectionPoseFactor SmartProjectionPose3Factor; - - -#include -template -virtual class GenericStereoFactor : gtsam::NoiseModelFactor { - GenericStereoFactor(const gtsam::StereoPoint2& measured, const gtsam::noiseModel::Base* noiseModel, - size_t poseKey, size_t landmarkKey, const gtsam::Cal3_S2Stereo* K); - gtsam::StereoPoint2 measured() const; - gtsam::Cal3_S2Stereo* calibration() const; - - // enabling serialization functionality - void serialize() const; -}; -typedef gtsam::GenericStereoFactor GenericStereoFactor3D; - -#include -template -virtual class PoseTranslationPrior : gtsam::NoiseModelFactor { - PoseTranslationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel); -}; - -typedef gtsam::PoseTranslationPrior PoseTranslationPrior2D; -typedef gtsam::PoseTranslationPrior PoseTranslationPrior3D; - -#include -template -virtual class PoseRotationPrior : gtsam::NoiseModelFactor { - PoseRotationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel); -}; - -typedef gtsam::PoseRotationPrior PoseRotationPrior2D; -typedef gtsam::PoseRotationPrior PoseRotationPrior3D; - -#include -virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { - EssentialMatrixFactor(size_t key, const gtsam::Point2& pA, const gtsam::Point2& pB, - const gtsam::noiseModel::Base* noiseModel); -}; - -#include -pair load2D(string filename, - gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise, bool smart); -pair load2D(string filename, - gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise); -pair load2D(string filename, - gtsam::noiseModel::Diagonal* model, int maxID); -pair load2D(string filename, - gtsam::noiseModel::Diagonal* model); -pair load2D(string filename); -pair load2D_robust(string filename, - gtsam::noiseModel::Base* model); -void save2D(const gtsam::NonlinearFactorGraph& graph, - const gtsam::Values& config, gtsam::noiseModel::Diagonal* model, - string filename); - -pair readG2o(string filename); -void writeG2o(const gtsam::NonlinearFactorGraph& graph, - const gtsam::Values& estimate, string filename); - -//************************************************************************* -// Navigation -//************************************************************************* -namespace imuBias { -#include - -class ConstantBias { - // Standard Constructor - ConstantBias(); - ConstantBias(Vector biasAcc, Vector biasGyro); - - // Testable - void print(string s) const; - bool equals(const gtsam::imuBias::ConstantBias& expected, double tol) const; - - // Group - static gtsam::imuBias::ConstantBias identity(); - gtsam::imuBias::ConstantBias inverse() const; - gtsam::imuBias::ConstantBias compose(const gtsam::imuBias::ConstantBias& b) const; - gtsam::imuBias::ConstantBias between(const gtsam::imuBias::ConstantBias& b) const; - - // Manifold - gtsam::imuBias::ConstantBias retract(Vector v) const; - Vector localCoordinates(const gtsam::imuBias::ConstantBias& b) const; - - // Lie Group - static gtsam::imuBias::ConstantBias Expmap(Vector v); - static Vector Logmap(const gtsam::imuBias::ConstantBias& b); - - // Standard Interface - Vector vector() const; - Vector accelerometer() const; - Vector gyroscope() const; - Vector correctAccelerometer(Vector measurement) const; - Vector correctGyroscope(Vector measurement) const; -}; - -}///\namespace imuBias - -#include -class PoseVelocityBias{ - PoseVelocityBias(const gtsam::Pose3& pose, Vector velocity, const gtsam::imuBias::ConstantBias& bias); - }; -class PreintegratedImuMeasurements { - // Standard Constructor - PreintegratedImuMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance, bool use2ndOrderIntegration); - PreintegratedImuMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance); - // PreintegratedImuMeasurements(const gtsam::PreintegratedImuMeasurements& rhs); - - // Testable - void print(string s) const; - bool equals(const gtsam::PreintegratedImuMeasurements& expected, double tol); - - double deltaTij() const; - gtsam::Rot3 deltaRij() const; - Vector deltaPij() const; - Vector deltaVij() const; - Vector biasHatVector() const; - Matrix delPdelBiasAcc() const; - Matrix delPdelBiasOmega() const; - Matrix delVdelBiasAcc() const; - Matrix delVdelBiasOmega() const; - Matrix delRdelBiasOmega() const; - Matrix preintMeasCov() const; - - // Standard Interface - void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT); - gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias, - Vector gravity, Vector omegaCoriolis) const; -}; - -virtual class ImuFactor : gtsam::NonlinearFactor { - ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias, - const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis); - ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias, - const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis, - const gtsam::Pose3& body_P_sensor); - // Standard Interface - gtsam::PreintegratedImuMeasurements preintegratedMeasurements() const; -}; - -#include -class PreintegratedCombinedMeasurements { - // Standard Constructor - PreintegratedCombinedMeasurements( - const gtsam::imuBias::ConstantBias& bias, - Matrix measuredAccCovariance, - Matrix measuredOmegaCovariance, - Matrix integrationErrorCovariance, - Matrix biasAccCovariance, - Matrix biasOmegaCovariance, - Matrix biasAccOmegaInit); - PreintegratedCombinedMeasurements( - const gtsam::imuBias::ConstantBias& bias, - Matrix measuredAccCovariance, - Matrix measuredOmegaCovariance, - Matrix integrationErrorCovariance, - Matrix biasAccCovariance, - Matrix biasOmegaCovariance, - Matrix biasAccOmegaInit, - bool use2ndOrderIntegration); - // PreintegratedCombinedMeasurements(const gtsam::PreintegratedCombinedMeasurements& rhs); - - // Testable - void print(string s) const; - bool equals(const gtsam::PreintegratedCombinedMeasurements& expected, double tol); - - double deltaTij() const; - gtsam::Rot3 deltaRij() const; - Vector deltaPij() const; - Vector deltaVij() const; - Vector biasHatVector() const; - Matrix delPdelBiasAcc() const; - Matrix delPdelBiasOmega() const; - Matrix delVdelBiasAcc() const; - Matrix delVdelBiasOmega() const; - Matrix delRdelBiasOmega() const; - Matrix preintMeasCov() const; - - // Standard Interface - void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT); - gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias, - Vector gravity, Vector omegaCoriolis) const; -}; - -virtual class CombinedImuFactor : gtsam::NonlinearFactor { - CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias_i, size_t bias_j, - const gtsam::PreintegratedCombinedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis); - // Standard Interface - gtsam::PreintegratedCombinedMeasurements preintegratedMeasurements() const; -}; - -#include -class PreintegratedAhrsMeasurements { - // Standard Constructor - PreintegratedAhrsMeasurements(Vector bias, Matrix measuredOmegaCovariance); - PreintegratedAhrsMeasurements(Vector bias, Matrix measuredOmegaCovariance); - PreintegratedAhrsMeasurements(const gtsam::PreintegratedAhrsMeasurements& rhs); - - // Testable - void print(string s) const; - bool equals(const gtsam::PreintegratedAhrsMeasurements& expected, double tol); - - // get Data - gtsam::Rot3 deltaRij() const; - double deltaTij() const; - Vector biasHat() const; - - // Standard Interface - void integrateMeasurement(Vector measuredOmega, double deltaT); - void resetIntegration() ; -}; - -virtual class AHRSFactor : gtsam::NonlinearFactor { - AHRSFactor(size_t rot_i, size_t rot_j,size_t bias, - const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis); - AHRSFactor(size_t rot_i, size_t rot_j, size_t bias, - const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis, - const gtsam::Pose3& body_P_sensor); - - // Standard Interface - gtsam::PreintegratedAhrsMeasurements preintegratedMeasurements() const; - Vector evaluateError(const gtsam::Rot3& rot_i, const gtsam::Rot3& rot_j, - Vector bias) const; - gtsam::Rot3 predict(const gtsam::Rot3& rot_i, Vector bias, - const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, - Vector omegaCoriolis) const; -}; - -#include -//virtual class AttitudeFactor : gtsam::NonlinearFactor { -// AttitudeFactor(const Unit3& nZ, const Unit3& bRef); -// AttitudeFactor(); -//}; -virtual class Rot3AttitudeFactor : gtsam::NonlinearFactor{ - Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model, - const gtsam::Unit3& bRef); - Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model); - Rot3AttitudeFactor(); - void print(string s) const; - bool equals(const gtsam::NonlinearFactor& expected, double tol) const; - gtsam::Unit3 nZ() const; - gtsam::Unit3 bRef() const; -}; - -virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor{ - Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model, - const gtsam::Unit3& bRef); - Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model); - Pose3AttitudeFactor(); - void print(string s) const; - bool equals(const gtsam::NonlinearFactor& expected, double tol) const; - gtsam::Unit3 nZ() const; - gtsam::Unit3 bRef() const; -}; - -//************************************************************************* -// Utilities -//************************************************************************* - -namespace utilities { - - #include - gtsam::KeyList createKeyList(Vector I); - gtsam::KeyList createKeyList(string s, Vector I); - gtsam::KeyVector createKeyVector(Vector I); - gtsam::KeyVector createKeyVector(string s, Vector I); - gtsam::KeySet createKeySet(Vector I); - gtsam::KeySet createKeySet(string s, Vector I); - Matrix extractPoint2(const gtsam::Values& values); - Matrix extractPoint3(const gtsam::Values& values); - Matrix extractPose2(const gtsam::Values& values); - gtsam::Values allPose3s(gtsam::Values& values); - Matrix extractPose3(const gtsam::Values& values); - void perturbPoint2(gtsam::Values& values, double sigma, int seed); - void perturbPose2 (gtsam::Values& values, double sigmaT, double sigmaR, int seed); - void perturbPoint3(gtsam::Values& values, double sigma, int seed); - void insertBackprojections(gtsam::Values& values, const gtsam::SimpleCamera& c, Vector J, Matrix Z, double depth); - void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K); - void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K, const gtsam::Pose3& body_P_sensor); - Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values); - gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base); - gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base, const gtsam::KeyVector& keys); - -} //\namespace utilities - -} diff --git a/wrap/CMakeLists.txt b/wrap/CMakeLists.txt index 3daa87c7c..6e046e3ab 100644 --- a/wrap/CMakeLists.txt +++ b/wrap/CMakeLists.txt @@ -26,6 +26,9 @@ if (NOT GTSAM_WRAP_SERIALIZATION) target_compile_definitions(wrap_lib PUBLIC -DWRAP_DISABLE_SERIALIZE) endif() +# Apply build flags: +gtsam_apply_build_flags(wrap_lib) + target_link_libraries(wrap_lib ${WRAP_BOOST_LIBRARIES}) target_include_directories(wrap_lib PUBLIC ${Boost_INCLUDE_DIR}) diff --git a/wrap/Class.cpp b/wrap/Class.cpp index 5c1e6187e..65ce9eab7 100644 --- a/wrap/Class.cpp +++ b/wrap/Class.cpp @@ -342,17 +342,21 @@ vector Class::expandTemplate(Str templateArg, /* ************************************************************************* */ void Class::addMethod(bool verbose, bool is_const, Str methodName, - const ArgumentList& argumentList, const ReturnValue& returnValue, - const Template& tmplate) { + const ArgumentList& argumentList, + const ReturnValue& returnValue, const Template& tmplate) { // Check if templated if (tmplate.valid()) { - templateMethods_[methodName].addOverload(methodName, argumentList, - returnValue, is_const, - tmplate.argName(), verbose); + try { + templateMethods_[methodName].addOverload(methodName, argumentList, + returnValue, is_const, + tmplate.argName(), verbose); + } catch (const std::runtime_error& e) { + throw std::runtime_error("Class::addMethod: error adding " + name_ + + "::" + methodName + "\n" + e.what()); + } // Create method to expand // For all values of the template argument, create a new method - for(const Qualified& instName: tmplate.argValues()) { - + for (const Qualified& instName : tmplate.argValues()) { const TemplateSubstitution ts(tmplate.argName(), instName, *this); // substitute template in arguments ArgumentList expandedArgs = argumentList.expandTemplate(ts); @@ -361,15 +365,27 @@ void Class::addMethod(bool verbose, bool is_const, Str methodName, // Now stick in new overload stack with expandedMethodName key // but note we use the same, unexpanded methodName in overload string expandedMethodName = methodName + instName.name(); - methods_[expandedMethodName].addOverload(methodName, expandedArgs, - expandedRetVal, is_const, instName, verbose); + try { + methods_[expandedMethodName].addOverload(methodName, expandedArgs, + expandedRetVal, is_const, + instName, verbose); + } catch (const std::runtime_error& e) { + throw std::runtime_error("Class::addMethod: error adding " + name_ + + "::" + expandedMethodName + "\n" + e.what()); + } } } else { - // just add overload - methods_[methodName].addOverload(methodName, argumentList, returnValue, - is_const, boost::none, verbose); - nontemplateMethods_[methodName].addOverload(methodName, argumentList, returnValue, - is_const, boost::none, verbose); + try { + // just add overload + methods_[methodName].addOverload(methodName, argumentList, returnValue, + is_const, boost::none, verbose); + nontemplateMethods_[methodName].addOverload(methodName, argumentList, + returnValue, is_const, + boost::none, verbose); + } catch (const std::runtime_error& e) { + throw std::runtime_error("Class::addMethod: error adding " + name_ + + "::" + methodName + "\n" + e.what()); + } } } diff --git a/wrap/FileWriter.cpp b/wrap/FileWriter.cpp index 2c3843b37..c07de0eb0 100644 --- a/wrap/FileWriter.cpp +++ b/wrap/FileWriter.cpp @@ -29,7 +29,7 @@ void FileWriter::emit(bool add_header, bool force_overwrite) const { bool file_exists = true; try { existing_contents = file_contents(filename_.c_str(), add_header); - } catch (const CantOpenFile& e) { + } catch (const CantOpenFile& ) { file_exists = false; } diff --git a/wrap/Method.cpp b/wrap/Method.cpp index f7247341c..2a4b0b3af 100644 --- a/wrap/Method.cpp +++ b/wrap/Method.cpp @@ -38,12 +38,12 @@ bool Method::addOverload(Str name, const ArgumentList& args, is_const_ = is_const; else if (is_const && !is_const_) throw std::runtime_error( - "Method::addOverload now designated as const whereas before it was " - "not"); + "Method::addOverload: " + name + + " now designated as const whereas before it was not"); else if (!is_const && is_const_) throw std::runtime_error( - "Method::addOverload now designated as non-const whereas before it " - "was"); + "Method::addOverload: " + name + + " now designated as non-const whereas before it was"); return first; } diff --git a/wrap/README b/wrap/README.md similarity index 70% rename from wrap/README rename to wrap/README.md index 92fd1967c..014577b5a 100644 --- a/wrap/README +++ b/wrap/README.md @@ -1,25 +1,21 @@ -Frank Dellaert -October 2011 +# WRAP README The wrap library wraps the GTSAM library into a MATLAB toolbox. -It was designed to be more general than just wrapping GTSAM, but a small amount of -GTSAM specific code exists in matlab.h, the include file that is included by the -mex files. The GTSAM-specific functionality consists primarily of handling of -Eigen Matrix and Vector classes. +It was designed to be more general than just wrapping GTSAM, but a small amount of GTSAM specific code exists in matlab.h, the include file that is included by the mex files. The GTSAM-specific functionality consists primarily of handling of Eigen Matrix and Vector classes. -For notes on creating a wrap interface, see gtsam.h for what features can be -wrapped into a toolbox, as well as the current state of the toolbox for gtsam. -For more technical details on the interface, please read comments in matlab.h +For notes on creating a wrap interface, see gtsam.h for what features can be wrapped into a toolbox, as well as the current state of the toolbox for gtsam. For more technical details on the interface, please read comments in matlab.h Some good things to know: OBJECT CREATION + - Classes are created by special constructors, e.g., new_GaussianFactorGraph_.cpp. These constructors are called from the MATLAB class @GaussianFactorGraph. new_GaussianFactorGraph_ calls wrap_constructed in matlab.h, see documentation there METHOD (AND CONSTRUCTOR) ARGUMENTS + - Simple argument types of methods, such as "double", will be converted in the mex wrappers by calling unwrap, defined in matlab.h - Vector and Matrix arguments are normally passed by reference in GTSAM, but