diff --git a/.gitignore b/.gitignore index ad0e08aa1..03ce578e7 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,5 @@ /build* +/debug* .idea *.pyc *.DS_Store diff --git a/CMakeLists.txt b/CMakeLists.txt index 673f9a45f..ce97fc689 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -61,8 +61,8 @@ option(GTSAM_POSE3_EXPMAP "Enable/Disable using Pose3::EXPMAP as the defau option(GTSAM_ROT3_EXPMAP "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." OFF) option(GTSAM_ENABLE_CONSISTENCY_CHECKS "Enable/Disable expensive consistency checks" OFF) option(GTSAM_WITH_TBB "Use Intel Threaded Building Blocks (TBB) if available" ON) -option(GTSAM_WITH_EIGEN_MKL "Eigen will use Intel MKL if available" ON) -option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" ON) +option(GTSAM_WITH_EIGEN_MKL "Eigen will use Intel MKL if available" OFF) +option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" OFF) option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON) option(GTSAM_BUILD_PYTHON "Enable/Disable building & installation of Python module" OFF) option(GTSAM_ALLOW_DEPRECATED_SINCE_V4 "Allow use of methods/functions deprecated in GTSAM 4" ON) @@ -78,6 +78,7 @@ endif() option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" OFF) option(GTSAM_INSTALL_CYTHON_TOOLBOX "Enable/Disable installation of Cython toolbox" OFF) option(GTSAM_BUILD_WRAP "Enable/Disable building of matlab/cython wrap utility (necessary for matlab/cython interface)" ON) +set(GTSAM_PYTHON_VERSION "Default" CACHE STRING "The version of python to build the cython wrapper or python module for (or Default)") # Check / set dependent variables for MATLAB wrapper if((GTSAM_INSTALL_MATLAB_TOOLBOX OR GTSAM_INSTALL_CYTHON_TOOLBOX) AND NOT GTSAM_BUILD_WRAP) @@ -121,15 +122,27 @@ 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 BOOST_ALL_NO_LIB BOOST_ALL_DYN_LINK) + list(APPEND 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 -Zm295) + list(APPEND GTSAM_COMPILE_OPTIONS_PRIVATE -Zm295) endif() endif() -find_package(Boost 1.43 COMPONENTS serialization system filesystem thread program_options date_time timer chrono regex) +# If building DLLs in MSVC, we need to avoid EIGEN_STATIC_ASSERT() +# or explicit instantiation will generate build errors. +# See: https://bitbucket.org/gtborg/gtsam/issues/417/fail-to-build-on-msvc-2017 +# +if(MSVC AND BUILD_SHARED_LIBS) + list(APPEND GTSAM_COMPILE_DEFINITIONS_PUBLIC EIGEN_NO_STATIC_ASSERT) +endif() + +# Store these in variables so they are automatically replicated in GTSAMConfig.cmake and such. +set(BOOST_FIND_MINIMUM_VERSION 1.43) +set(BOOST_FIND_MINIMUM_COMPONENTS serialization system filesystem thread program_options date_time timer chrono regex) + +find_package(Boost ${BOOST_FIND_MINIMUM_VERSION} COMPONENTS ${BOOST_FIND_MINIMUM_COMPONENTS}) # Required components if(NOT Boost_SERIALIZATION_LIBRARY OR NOT Boost_SYSTEM_LIBRARY OR NOT Boost_FILESYSTEM_LIBRARY OR @@ -137,22 +150,43 @@ if(NOT Boost_SERIALIZATION_LIBRARY OR NOT Boost_SYSTEM_LIBRARY OR NOT Boost_FILE message(FATAL_ERROR "Missing required Boost components >= v1.43, please install/upgrade Boost or configure your search paths.") endif() -option(GTSAM_DISABLE_NEW_TIMERS "Disables using Boost.chrono for timing" OFF) # Allow for not using the timer libraries on boost < 1.48 (GTSAM timing code falls back to old timer library) +option(GTSAM_DISABLE_NEW_TIMERS "Disables using Boost.chrono for timing" OFF) + +# JLBC: This was once updated to target-based names (Boost::xxx), but it caused +# problems with Boost versions newer than FindBoost.cmake was prepared to handle, +# so we downgraded this to classic filenames-based variables, and manually adding +# the target_include_directories(xxx ${Boost_INCLUDE_DIR}) set(GTSAM_BOOST_LIBRARIES - Boost::serialization - Boost::system - Boost::filesystem - Boost::thread - Boost::date_time - Boost::regex + optimized + ${Boost_SERIALIZATION_LIBRARY_RELEASE} + ${Boost_SYSTEM_LIBRARY_RELEASE} + ${Boost_FILESYSTEM_LIBRARY_RELEASE} + ${Boost_THREAD_LIBRARY_RELEASE} + ${Boost_DATE_TIME_LIBRARY_RELEASE} + ${Boost_REGEX_LIBRARY_RELEASE} + debug + ${Boost_SERIALIZATION_LIBRARY_DEBUG} + ${Boost_SYSTEM_LIBRARY_DEBUG} + ${Boost_FILESYSTEM_LIBRARY_DEBUG} + ${Boost_THREAD_LIBRARY_DEBUG} + ${Boost_DATE_TIME_LIBRARY_DEBUG} + ${Boost_REGEX_LIBRARY_DEBUG} ) +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 DGTSAM_DISABLE_NEW_TIMERS) + list(APPEND GTSAM_COMPILE_DEFINITIONS_PUBLIC DGTSAM_DISABLE_NEW_TIMERS) else() if(Boost_TIMER_LIBRARY) - list(APPEND GTSAM_BOOST_LIBRARIES Boost::timer Boost::chrono) + list(APPEND GTSAM_BOOST_LIBRARIES + optimized + ${Boost_TIMER_LIBRARY_RELEASE} + ${Boost_CHRONO_LIBRARY_RELEASE} + debug + ${Boost_TIMER_LIBRARY_DEBUG} + ${Boost_CHRONO_LIBRARY_DEBUG} + ) else() list(APPEND GTSAM_BOOST_LIBRARIES rt) # When using the header-only boost timer library, need -lrt message("WARNING: GTSAM timing instrumentation will use the older, less accurate, Boost timer library because boost older than 1.48 was found.") @@ -162,7 +196,7 @@ endif() if(NOT (${Boost_VERSION} LESS 105600)) message("Ignoring Boost restriction on optional lvalue assignment from rvalues") - list(APPEND GTSAM_COMPILE_DEFINITIONS BOOST_OPTIONAL_ALLOW_BINDING_TO_RVALUES BOOST_OPTIONAL_CONFIG_ALLOW_BINDING_TO_RVALUES) + list(APPEND GTSAM_COMPILE_DEFINITIONS_PUBLIC BOOST_OPTIONAL_ALLOW_BINDING_TO_RVALUES BOOST_OPTIONAL_CONFIG_ALLOW_BINDING_TO_RVALUES) endif() ############################################################################### @@ -212,7 +246,6 @@ find_package(MKL) if(MKL_FOUND AND GTSAM_WITH_EIGEN_MKL) set(GTSAM_USE_EIGEN_MKL 1) # This will go into config.h set(EIGEN_USE_MKL_ALL 1) # This will go into config.h - it makes Eigen use MKL - include_directories(${MKL_INCLUDE_DIR}) list(APPEND GTSAM_ADDITIONAL_LIBRARIES ${MKL_LIBRARIES}) # --no-as-needed is required with gcc according to the MKL link advisor @@ -247,10 +280,9 @@ option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', us # Switch for using system Eigen or GTSAM-bundled Eigen if(GTSAM_USE_SYSTEM_EIGEN) find_package(Eigen3 REQUIRED) - include_directories(AFTER "${EIGEN3_INCLUDE_DIR}") # Use generic Eigen include paths e.g. - set(GTSAM_EIGEN_INCLUDE_PREFIX "${EIGEN3_INCLUDE_DIR}") + set(GTSAM_EIGEN_INCLUDE_FOR_INSTALL "${EIGEN3_INCLUDE_DIR}") # check if MKL is also enabled - can have one or the other, but not both! # Note: Eigen >= v3.2.5 includes our patches @@ -264,28 +296,29 @@ if(GTSAM_USE_SYSTEM_EIGEN) message(FATAL_ERROR "MKL does not work with Eigen 3.3.4 because of a bug in Eigen. See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1527. Disable GTSAM_USE_SYSTEM_EIGEN to use GTSAM's copy of Eigen, disable GTSAM_WITH_EIGEN_MKL, or upgrade/patch your installation of Eigen.") endif() + # The actual include directory (for BUILD cmake target interface): + set(GTSAM_EIGEN_INCLUDE_FOR_BUILD "${EIGEN3_INCLUDE_DIR}") else() # Use bundled Eigen include path. # Clear any variables set by FindEigen3 if(EIGEN3_INCLUDE_DIR) set(EIGEN3_INCLUDE_DIR NOTFOUND CACHE STRING "" FORCE) endif() - # Add the bundled version of eigen to the include path so that it can still be included - # with #include - include_directories(BEFORE "gtsam/3rdparty/Eigen/") # set full path to be used by external projects # this will be added to GTSAM_INCLUDE_DIR by gtsam_extra.cmake.in - set(GTSAM_EIGEN_INCLUDE_PREFIX "${CMAKE_INSTALL_PREFIX}/include/gtsam/3rdparty/Eigen/") + set(GTSAM_EIGEN_INCLUDE_FOR_INSTALL "include/gtsam/3rdparty/Eigen/") + # The actual include directory (for BUILD cmake target interface): + set(GTSAM_EIGEN_INCLUDE_FOR_BUILD "${CMAKE_SOURCE_DIR}/gtsam/3rdparty/Eigen/") endif() if (MSVC) if (BUILD_SHARED_LIBS) # mute eigen static assert to avoid errors in shared lib - list(APPEND GTSAM_COMPILE_DEFINITIONS DEIGEN_NO_STATIC_ASSERT) + list(APPEND GTSAM_COMPILE_DEFINITIONS_PUBLIC DEIGEN_NO_STATIC_ASSERT) endif() - list(APPEND GTSAM_COMPILE_OPTIONS "/wd4244") # Disable loss of precision which is thrown all over our Eigen + list(APPEND GTSAM_COMPILE_OPTIONS_PRIVATE "/wd4244") # Disable loss of precision which is thrown all over our Eigen endif() ############################################################################### @@ -326,52 +359,29 @@ elseif("${GTSAM_DEFAULT_ALLOCATOR}" STREQUAL "tcmalloc") list(APPEND GTSAM_ADDITIONAL_LIBRARIES "tcmalloc") endif() -# Include boost - use 'BEFORE' so that a specific boost specified to CMake -# takes precedence over a system-installed one. -include_directories(BEFORE SYSTEM ${Boost_INCLUDE_DIR}) - -if(GTSAM_SUPPORT_NESTED_DISSECTION) - set(METIS_INCLUDE_DIRECTORIES - gtsam/3rdparty/metis/include - gtsam/3rdparty/metis/libmetis - gtsam/3rdparty/metis/GKlib) -else() - set(METIS_INCLUDE_DIRECTORIES) -endif() - -# Add includes for source directories 'BEFORE' boost and any system include -# paths so that the compiler uses GTSAM headers in our source directory instead -# of any previously installed GTSAM headers. -include_directories(BEFORE - gtsam/3rdparty/SuiteSparse_config - gtsam/3rdparty/CCOLAMD/Include - ${METIS_INCLUDE_DIRECTORIES} - ${PROJECT_SOURCE_DIR} - ${PROJECT_BINARY_DIR} # So we can include generated config header files - CppUnitLite) - if(MSVC) - list(APPEND GTSAM_COMPILE_DEFINITIONS _CRT_SECURE_NO_WARNINGS _SCL_SECURE_NO_WARNINGS) - list(APPEND GTSAM_COMPILE_OPTIONS /wd4251 /wd4275 /wd4251 /wd4661 /wd4344 /wd4503) # Disable non-DLL-exported base class and other warnings - list(APPEND GTSAM_COMPILE_OPTIONS /bigobj) # Allow large object files for template-based code + 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 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 -Wno-unused-local-typedefs) + list(APPEND 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 -Wno-unused-local-typedefs) + list(APPEND GTSAM_COMPILE_OPTIONS_PRIVATE -Wno-unused-local-typedefs) endif() endif() if(GTSAM_ENABLE_CONSISTENCY_CHECKS) - list(APPEND GTSAM_COMPILE_DEFINITIONS GTSAM_EXTRA_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) endif() ############################################################################### @@ -574,6 +584,7 @@ endif() message(STATUS "Cython toolbox flags ") print_config_flag(${GTSAM_INSTALL_CYTHON_TOOLBOX} "Install Cython toolbox ") +message(STATUS " Python version : ${GTSAM_PYTHON_VERSION}") print_config_flag(${GTSAM_BUILD_WRAP} "Build Wrap ") message(STATUS "===============================================================") @@ -582,10 +593,10 @@ if(GTSAM_WITH_TBB AND NOT TBB_FOUND) message(WARNING "TBB was not found - this is ok, but note that GTSAM parallelization will be disabled. Set GTSAM_WITH_TBB to 'Off' to avoid this warning.") endif() if(GTSAM_WITH_EIGEN_MKL AND NOT MKL_FOUND) - message(WARNING "MKL was not found - this is ok, but note that MKL yields better performance. Set GTSAM_WITH_EIGEN_MKL to 'Off' to disable this warning.") + message(WARNING "MKL was not found - this is ok, but note that MKL will be disabled. Set GTSAM_WITH_EIGEN_MKL to 'Off' to disable this warning. See INSTALL.md for notes on performance.") endif() if(GTSAM_WITH_EIGEN_MKL_OPENMP AND NOT OPENMP_FOUND AND MKL_FOUND) - message(WARNING "Your compiler does not support OpenMP - this is ok, but performance may be improved with OpenMP. Set GTSAM_WITH_EIGEN_MKL_OPENMP to 'Off' to avoid this warning.") + message(WARNING "Your compiler does not support OpenMP. Set GTSAM_WITH_EIGEN_MKL_OPENMP to 'Off' to avoid this warning. See INSTALL.md for notes on performance.") endif() if(GTSAM_BUILD_PYTHON AND GTSAM_PYTHON_WARNINGS) message(WARNING "${GTSAM_PYTHON_WARNINGS}") diff --git a/CppUnitLite/CMakeLists.txt b/CppUnitLite/CMakeLists.txt index 35ecd71f8..72651aca9 100644 --- a/CppUnitLite/CMakeLists.txt +++ b/CppUnitLite/CMakeLists.txt @@ -6,6 +6,7 @@ file(GLOB cppunitlite_src "*.cpp") add_library(CppUnitLite STATIC ${cppunitlite_src} ${cppunitlite_headers}) list(APPEND GTSAM_EXPORTED_TARGETS CppUnitLite) set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE) +target_include_directories(CppUnitLite PUBLIC ${Boost_INCLUDE_DIR}) # boost/lexical_cast.h gtsam_assign_source_folders("${cppunitlite_headers};${cppunitlite_src}") # MSVC project structure diff --git a/GTSAM-Concepts.md b/GTSAM-Concepts.md index 366a58a09..a6cfee984 100644 --- a/GTSAM-Concepts.md +++ b/GTSAM-Concepts.md @@ -97,12 +97,24 @@ Note that in the Lie group case, the usual valid expressions for Retract and Loc For Lie groups, the `exponential map` above is the most obvious mapping: it associates straight lines in the tangent space with geodesics on the manifold -(and it's inverse, the log map). However, there are two cases in which we deviate from this: +(and it's inverse, the log map). However, there are several cases in which we deviate from this: However, the exponential map is unnecessarily expensive for use in optimization. Hence, in GTSAM there is the option to provide a cheaper chart by means of the `ChartAtOrigin` struct in a class. This is done for *SE(2)*, *SO(3)* and *SE(3)* (see `Pose2`, `Rot3`, `Pose3`) Most Lie groups we care about are *Matrix groups*, continuous sub-groups of *GL(n)*, the group of *n x n* invertible matrices. In this case, a lot of the derivatives calculations needed can be standardized, and this is done by the `LieGroup` superclass. You only need to provide an `AdjointMap` method. +A CRTP helper class `LieGroup` is available that can take a class and create some of the Lie group methods automatically. The class needs: + +* operator* : implements group operator +* inverse: implements group inverse +* AdjointMap: maps tangent vectors according to group element +* Expmap/Logmap: exponential map and its inverse +* ChartAtOrigin: struct where you define Retract/Local at origin + +To use, simply derive, but also say `using LieGroup::inverse` so you get an inverse with a derivative. + +Finally, to create the traits automatically you can use `internal::LieGroupTraits` + Vector Space ------------ diff --git a/INSTALL b/INSTALL deleted file mode 100644 index c71dcd4f9..000000000 --- a/INSTALL +++ /dev/null @@ -1,146 +0,0 @@ -Quickstart - -In the root library folder execute: - -$] mkdir build -$] cd build -$] cmake .. -$] make check (optional, runs unit tests) -$] make install - -Important Installation Notes ----------------------------- - -1) -GTSAM requires the following libraries to be installed on your system: - - BOOST version 1.43 or greater (install through Linux repositories or MacPorts) - - Cmake version 2.6 or higher - - Support for XCode 4.3 command line tools on Mac requires CMake 2.8.8 or higher - -Optional dependent libraries: - - If TBB is installed and detectable by CMake GTSAM will use it automatically. - Ensure that CMake prints "Use Intel TBB : Yes". To disable the use of TBB, - disable the CMake flag GTSAM_WITH_TBB (enabled by default). On Ubuntu, TBB - may be installed from the Ubuntu repositories, and for other platforms it - may be downloaded from https://www.threadingbuildingblocks.org/ - -Tested compilers: - -- GCC 4.2-4.7 -- OSX Clang 2.9-5.0 -- OSX GCC 4.2 -- MSVC 2010, 2012 - -Tested systems: - -- Ubuntu 11.04 - 13.10 -- MacOS 10.6 - 10.9 -- Windows 7, 8, 8.1 - -Known issues: - -- MSVC 2013 is not yet supported because it cannot build the serialization module - of Boost 1.55 (or earlier). - -2) -GTSAM makes extensive use of debug assertions, and we highly recommend you work -in Debug mode while developing (enabled by default). Likewise, it is imperative -that you switch to release mode when running finished code and for timing. GTSAM -will run up to 10x faster in Release mode! See the end of this document for -additional debugging tips. - -3) -GTSAM has Doxygen documentation. To generate, run 'make doc' from your -build directory. - -4) -The instructions below install the library to the default system install path and -build all components. From a terminal, starting in the root library folder, -execute commands as follows for an out-of-source build: - -$] mkdir build -$] cd build -$] cmake .. -$] make check (optional, runs unit tests) -$] make install - -This will build the library and unit tests, run all of the unit tests, -and then install the library itself. - -- CMake Configuration Options and Details - -GTSAM has a number of options that can be configured, which is best done with -one of the following: - - ccmake the curses GUI for cmake - cmake-gui a real GUI for cmake - -Important Options: - -CMAKE_BUILD_TYPE: We support several build configurations for GTSAM (case insensitive) - Debug (default) All error checking options on, no optimization. Use for development. - Release Optimizations turned on, no debug symbols. - Timing Adds ENABLE_TIMING flag to provide statistics on operation - Profiling Standard configuration for use during profiling - RelWithDebInfo Same as Release, but with the -g flag for debug symbols - -CMAKE_INSTALL_PREFIX: The install folder. The default is typically /usr/local/ -To configure to install to your home directory, you could execute: -$] cmake -DCMAKE_INSTALL_PREFIX:PATH=$HOME .. - -GTSAM_TOOLBOX_INSTALL_PATH: The Matlab toolbox will be installed in a subdirectory -of this folder, called 'gtsam'. -$] cmake -DGTSAM_TOOLBOX_INSTALL_PATH:PATH=$HOME/toolbox .. - -GTSAM_BUILD_CONVENIENCE_LIBRARIES: This is a build option to allow for tests in -subfolders to be linked against convenience libraries rather than the full libgtsam. -Set with the command line as follows: -$] cmake -DGTSAM_BUILD_CONVENIENCE_LIBRARIES:OPTION=ON .. - ON (Default) This builds convenience libraries and links tests against them. This - option is suggested for gtsam developers, as it is possible to build - and run tests without first building the rest of the library, and - speeds up compilation for a single test. The downside of this option - is that it will build the entire library again to build the full - libgtsam library, so build/install will be slower. - OFF This will build all of libgtsam before any of the tests, and then - link all of the tests at once. This option is best for users of GTSAM, - as it avoids rebuilding the entirety of gtsam an extra time. - -GTSAM_BUILD_UNSTABLE: Enable build and install for libgtsam_unstable library. -Set with the command line as follows: -$] cmake -DGTSAM_BUILD_UNSTABLE:OPTION=ON .. - ON When enabled, libgtsam_unstable will be built and installed with the - same options as libgtsam. In addition, if tests are enabled, the - unit tests will be built as well. The Matlab toolbox will also - be generated if the matlab toolbox is enabled, installing into a - folder called "gtsam_unstable". - OFF (Default) If disabled, no gtsam_unstable code will be included in build or install. - -Check - -"make check" will build and run all of the tests. Note that the tests will only be -built when using the "check" targets, to prevent "make install" from building the tests -unnecessarily. You can also run "make timing" to build all of the timing scripts. -To run check on a particular module only, run "make check.[subfolder]", so to run -just the geometry tests, run "make check.geometry". Individual tests can be run by -appending ".run" to the name of the test, for example, to run testMatrix, run -"make testMatrix.run". - -MEX_COMMAND: Path to the mex compiler. Defaults to assume the path is included in your -shell's PATH environment variable. mex is installed with matlab at -$MATLABROOT/bin/mex - -$MATLABROOT can be found by executing the command 'matlabroot' in MATLAB - -Debugging tips: - -Another useful debugging symbol is _GLIBCXX_DEBUG, which enables debug checks -and safe containers in the standard C++ library and makes problems much easier -to find. - -NOTE: The native Snow Leopard g++ compiler/library contains a bug that makes -it impossible to use _GLIBCXX_DEBUG. MacPorts g++ compilers do work with it though. - -NOTE: If _GLIBCXX_DEBUG is used to compile gtsam, anything that links against -gtsam will need to be compiled with _GLIBCXX_DEBUG as well, due to the use of -header-only Eigen. diff --git a/INSTALL.md b/INSTALL.md new file mode 100644 index 000000000..3437d074d --- /dev/null +++ b/INSTALL.md @@ -0,0 +1,168 @@ +# Quickstart + +In the root library folder execute: + +```sh +$ mkdir build +$ cd build +$ cmake .. +$ make check # (optional, runs unit tests) +$ make install +``` + +## Important Installation Notes + +1. GTSAM requires the following libraries to be installed on your system: + - BOOST version 1.43 or greater (install through Linux repositories or MacPorts) + - Cmake version 3.0 or higher + - Support for XCode 4.3 command line tools on Mac requires CMake 2.8.8 or higher + + Optional dependent libraries: + - If TBB is installed and detectable by CMake GTSAM will use it automatically. + Ensure that CMake prints "Use Intel TBB : Yes". To disable the use of TBB, + disable the CMake flag GTSAM_WITH_TBB (enabled by default). On Ubuntu, TBB + may be installed from the Ubuntu repositories, and for other platforms it + may be downloaded from https://www.threadingbuildingblocks.org/ + - GTSAM may be configured to use MKL by toggling `GTSAM_WITH_EIGEN_MKL` and + `GTSAM_WITH_EIGEN_MKL_OPENMP` to `ON`; however, best performance is usually + achieved with MKL disabled. We therefore advise you to benchmark your problem + before using MKL. + + Tested compilers: + + - GCC 4.2-7.3 + - OS X Clang 2.9-10.0 + - OS X GCC 4.2 + - MSVC 2010, 2012, 2017 + + Tested systems: + + - Ubuntu 16.04 - 18.04 + - MacOS 10.6 - 10.14 + - Windows 7, 8, 8.1, 10 + + Known issues: + + - MSVC 2013 is not yet supported because it cannot build the serialization module + of Boost 1.55 (or earlier). + +2. GTSAM makes extensive use of debug assertions, and we highly recommend you work +in Debug mode while developing (enabled by default). Likewise, it is imperative +that you switch to release mode when running finished code and for timing. GTSAM +will run up to 10x faster in Release mode! See the end of this document for +additional debugging tips. + +3. GTSAM has Doxygen documentation. To generate, run 'make doc' from your +build directory. + +4. The instructions below install the library to the default system install path and +build all components. From a terminal, starting in the root library folder, +execute commands as follows for an out-of-source build: + + ```sh + $ mkdir build + $ cd build + $ cmake .. + $ make check (optional, runs unit tests) + $ make install + ``` + + This will build the library and unit tests, run all of the unit tests, + and then install the library itself. + +## CMake Configuration Options and Details + +GTSAM has a number of options that can be configured, which is best done with +one of the following: + + - ccmake the curses GUI for cmake + - cmake-gui a real GUI for cmake + +### Important Options: + +#### CMAKE_BUILD_TYPE +We support several build configurations for GTSAM (case insensitive) + +```cmake -DCMAKE_BUILD_TYPE=[Option] ..``` + + - Debug (default) All error checking options on, no optimization. Use for development. + - Release Optimizations turned on, no debug symbols. + - Timing Adds ENABLE_TIMING flag to provide statistics on operation + - Profiling Standard configuration for use during profiling + - RelWithDebInfo Same as Release, but with the -g flag for debug symbols + +#### CMAKE_INSTALL_PREFIX + +The install folder. The default is typically `/usr/local/`. +To configure to install to your home directory, you could execute: + +```cmake -DCMAKE_INSTALL_PREFIX:PATH=$HOME ..``` + +#### GTSAM_TOOLBOX_INSTALL_PATH + +The Matlab toolbox will be installed in a subdirectory +of this folder, called 'gtsam'. + +```cmake -DGTSAM_TOOLBOX_INSTALL_PATH:PATH=$HOME/toolbox ..``` + +#### GTSAM_BUILD_CONVENIENCE_LIBRARIES + +This is a build option to allow for tests in subfolders to be linked against convenience libraries rather than the full libgtsam. +Set with the command line as follows: + +```cmake -DGTSAM_BUILD_CONVENIENCE_LIBRARIES:OPTION=ON ..``` + - ON (Default): This builds convenience libraries and links tests against them. This option is suggested for gtsam developers, as it is possible to build and run tests without first building the rest of the library, and speeds up compilation for a single test. The downside of this option is that it will build the entire library again to build the full libgtsam library, so build/install will be slower. + - OFF: This will build all of libgtsam before any of the tests, and then link all of the tests at once. This option is best for users of GTSAM, as it avoids rebuilding the entirety of gtsam an extra time. + +#### GTSAM_BUILD_UNSTABLE + +Enable build and install for libgtsam_unstable library. +Set with the command line as follows: + +```cmake -DGTSAM_BUILD_UNSTABLE:OPTION=ON ..``` + + ON: When enabled, libgtsam_unstable will be built and installed with the same options as libgtsam. In addition, if tests are enabled, the unit tests will be built as well. The Matlab toolbox will also be generated if the matlab toolbox is enabled, installing into a folder called `gtsam_unstable`. + OFF (Default) If disabled, no `gtsam_unstable` code will be included in build or install. + +## Check + +`make check` will build and run all of the tests. Note that the tests will only be +built when using the "check" targets, to prevent `make install` from building the tests +unnecessarily. You can also run `make timing` to build all of the timing scripts. +To run check on a particular module only, run `make check.[subfolder]`, so to run +just the geometry tests, run `make check.geometry`. Individual tests can be run by +appending `.run` to the name of the test, for example, to run testMatrix, run +`make testMatrix.run`. + +MEX_COMMAND: Path to the mex compiler. Defaults to assume the path is included in your shell's PATH environment variable. mex is installed with matlab at `$MATLABROOT/bin/mex` + +$MATLABROOT can be found by executing the command `matlabroot` in MATLAB + +## Performance + +Here are some tips to get the best possible performance out of GTSAM. + +1. Build in `Release` mode. GTSAM will run up to 10x faster compared to `Debug` mode. +2. Enable TBB. On modern processors with multiple cores, this can easily speed up + optimization by 30-50%. Please note that this may not be true for very small + problems where the overhead of dispatching work to multiple threads outweighs + the benefit. We recommend that you benchmark your problem with/without TBB. +3. Add `-march=native` to `GTSAM_CMAKE_CXX_FLAGS`. A performance gain of + 25-30% can be expected on modern processors. Note that this affects the portability + of your executable. It may not run when copied to another system with older/different + processor architecture. + Also note that all dependent projects *must* be compiled with the same flag, or + seg-faults and other undefined behavior may result. +4. Possibly enable MKL. Please note that our benchmarks have shown that this helps only + in very limited cases, and actually hurts performance in the usual case. We therefore + recommend that you do *not* enable MKL, unless you have benchmarked it on + your problem and have verified that it improves performance. + + +## Debugging tips + +Another useful debugging symbol is _GLIBCXX_DEBUG, which enables debug checks and safe containers in the standard C++ library and makes problems much easier to find. + +NOTE: The native Snow Leopard g++ compiler/library contains a bug that makes it impossible to use _GLIBCXX_DEBUG. MacPorts g++ compilers do work with it though. + +NOTE: If _GLIBCXX_DEBUG is used to compile gtsam, anything that links against gtsam will need to be compiled with _GLIBCXX_DEBUG as well, due to the use of header-only Eigen. diff --git a/README.md b/README.md index 02ed5149c..7e03c81f2 100644 --- a/README.md +++ b/README.md @@ -30,7 +30,7 @@ $ make install Prerequisites: - [Boost](http://www.boost.org/users/download/) >= 1.43 (Ubuntu: `sudo apt-get install libboost-all-dev`) -- [CMake](http://www.cmake.org/cmake/resources/software.html) >= 2.6 (Ubuntu: `sudo apt-get install cmake`) +- [CMake](http://www.cmake.org/cmake/resources/software.html) >= 3.0 (Ubuntu: `sudo apt-get install cmake`) - A modern compiler, i.e., at least gcc 4.7.3 on Linux. Optional prerequisites - used automatically if findable by CMake: @@ -54,7 +54,7 @@ GTSAM includes a state of the art IMU handling scheme based on Our implementation improves on this using integration on the manifold, as detailed in -- Luca Carlone, Zsolt Kira, Chris Beall, Vadim Indelman, and Frank Dellaert, "Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors", Int. Conf. on Robotics and Automation (ICRA), 2014. +- Luca Carlone, Zsolt Kira, Chris Beall, Vadim Indelman, and Frank Dellaert, "Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors", Int. Conf. on Robotics and Automation (ICRA), 2014. - Christian Forster, Luca Carlone, Frank Dellaert, and Davide Scaramuzza, "IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation", Robotics: Science and Systems (RSS), 2015. If you are using the factor in academic work, please cite the publications above. @@ -67,14 +67,14 @@ Additional Information There is a [`GTSAM users Google group`](https://groups.google.com/forum/#!forum/gtsam-users) for general discussion. -Read about important [`GTSAM-Concepts`](GTSAM-Concepts.md) here. A primer on GTSAM Expressions, -which support (superfast) automatic differentiation, +Read about important [`GTSAM-Concepts`](GTSAM-Concepts.md) here. A primer on GTSAM Expressions, +which support (superfast) automatic differentiation, can be found on the [GTSAM wiki on BitBucket](https://bitbucket.org/gtborg/gtsam/wiki/Home). -See the [`INSTALL`](INSTALL) file for more detailed installation instructions. +See the [`INSTALL`](INSTALL.md) file for more detailed installation instructions. GTSAM is open source under the BSD license, see the [`LICENSE`](LICENSE) and [`LICENSE.BSD`](LICENSE.BSD) files. Please see the [`examples/`](examples) directory and the [`USAGE`](USAGE.md) file for examples on how to use GTSAM. -GTSAM was developed in the lab of [Frank Dellaert](http://www.cc.gatech.edu/~dellaert) at the [Georgia Institute of Technology](http://www.gatech.edu), with the help of many contributors over the years, see [THANKS](THANKS). \ No newline at end of file +GTSAM was developed in the lab of [Frank Dellaert](http://www.cc.gatech.edu/~dellaert) at the [Georgia Institute of Technology](http://www.gatech.edu), with the help of many contributors over the years, see [THANKS](THANKS). diff --git a/THANKS b/THANKS index f84cfa185..7db7daaf3 100644 --- a/THANKS +++ b/THANKS @@ -1,5 +1,6 @@ GTSAM was made possible by the efforts of many collaborators at Georgia Tech, listed below with their current afffiliation, if they left Tech: +* Jeremy Aguilon, Facebook * Sungtae An * Doru Balcan, Bank of America * Chris Beall @@ -26,6 +27,7 @@ GTSAM was made possible by the efforts of many collaborators at Georgia Tech, li * Natesh Srinivasan * Alex Trevor * Stephen Williams, BossaNova +* Matthew Broadway at ETH, Zurich diff --git a/cmake/Config.cmake.in b/cmake/Config.cmake.in index 5f3956f07..4323f760b 100644 --- a/cmake/Config.cmake.in +++ b/cmake/Config.cmake.in @@ -1,7 +1,7 @@ # - Config file for @CMAKE_PROJECT_NAME@ # It defines the following variables # @PACKAGE_NAME@_INCLUDE_DIR - include directories for @CMAKE_PROJECT_NAME@ - + # Compute paths get_filename_component(OUR_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH) if(EXISTS "${OUR_CMAKE_DIR}/CMakeCache.txt") @@ -11,7 +11,11 @@ else() # Find installed library set(@PACKAGE_NAME@_INCLUDE_DIR "${OUR_CMAKE_DIR}/@CONF_REL_INCLUDE_DIR@" CACHE PATH "@PACKAGE_NAME@ include directory") endif() - + +# Find dependencies, required by cmake exported targets: +include(CMakeFindDependencyMacro) +find_dependency(Boost @BOOST_FIND_MINIMUM_VERSION@ COMPONENTS @BOOST_FIND_MINIMUM_COMPONENTS@) + # Load exports include(${OUR_CMAKE_DIR}/@PACKAGE_NAME@-exports.cmake) diff --git a/cmake/FindCython.cmake b/cmake/FindCython.cmake index 23afb00e6..e5a32c30d 100644 --- a/cmake/FindCython.cmake +++ b/cmake/FindCython.cmake @@ -29,10 +29,15 @@ # Use the Cython executable that lives next to the Python executable # if it is a local installation. -find_package( PythonInterp ) +if(GTSAM_PYTHON_VERSION STREQUAL "Default") + find_package(PythonInterp) +else() + find_package(PythonInterp ${GTSAM_PYTHON_VERSION} EXACT) +endif() + if ( PYTHONINTERP_FOUND ) execute_process( COMMAND "${PYTHON_EXECUTABLE}" "-c" - "import Cython; print Cython.__path__" + "import Cython; print(Cython.__path__[0])" RESULT_VARIABLE RESULT OUTPUT_VARIABLE CYTHON_PATH OUTPUT_STRIP_TRAILING_WHITESPACE @@ -51,7 +56,7 @@ endif () # RESULT=0 means ok if ( NOT RESULT ) execute_process( COMMAND "${PYTHON_EXECUTABLE}" "-c" - "import Cython; print Cython.__version__" + "import Cython; print(Cython.__version__)" RESULT_VARIABLE RESULT OUTPUT_VARIABLE CYTHON_VAR_OUTPUT ERROR_VARIABLE CYTHON_VAR_OUTPUT diff --git a/cmake/FindMKL.cmake b/cmake/FindMKL.cmake index cbe46a908..32e183baa 100644 --- a/cmake/FindMKL.cmake +++ b/cmake/FindMKL.cmake @@ -206,6 +206,15 @@ ELSEIF(MKL_ROOT_DIR) # UNIX and macOS ) ENDIF() + IF(NOT MKL_LAPACK_LIBRARY) + FIND_LIBRARY(MKL_LAPACK_LIBRARY + mkl_intel_lp64 + PATHS + ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR} + ${MKL_ROOT_DIR}/lib/ + ) + ENDIF() + # iomp5 IF("${MKL_ARCH_DIR}" STREQUAL "32") IF(UNIX AND NOT APPLE) diff --git a/cmake/FindNumPy.cmake b/cmake/FindNumPy.cmake index eafed165e..4f5743aa6 100644 --- a/cmake/FindNumPy.cmake +++ b/cmake/FindNumPy.cmake @@ -40,9 +40,17 @@ # Finding NumPy involves calling the Python interpreter if(NumPy_FIND_REQUIRED) + if(GTSAM_PYTHON_VERSION STREQUAL "Default") find_package(PythonInterp REQUIRED) + else() + find_package(PythonInterp ${GTSAM_PYTHON_VERSION} EXACT REQUIRED) + endif() else() + if(GTSAM_PYTHON_VERSION STREQUAL "Default") find_package(PythonInterp) + else() + find_package(PythonInterp ${GTSAM_PYTHON_VERSION} EXACT) + endif() endif() if(NOT PYTHONINTERP_FOUND) diff --git a/cmake/GtsamCythonWrap.cmake b/cmake/GtsamCythonWrap.cmake index 73e2b63e0..6366c1508 100644 --- a/cmake/GtsamCythonWrap.cmake +++ b/cmake/GtsamCythonWrap.cmake @@ -3,8 +3,23 @@ # in the current environment are different from the cached! unset(PYTHON_EXECUTABLE CACHE) unset(CYTHON_EXECUTABLE CACHE) +unset(PYTHON_INCLUDE_DIR CACHE) +unset(PYTHON_MAJOR_VERSION CACHE) + +if(GTSAM_PYTHON_VERSION STREQUAL "Default") + find_package(PythonInterp REQUIRED) + find_package(PythonLibs REQUIRED) +else() + find_package(PythonInterp ${GTSAM_PYTHON_VERSION} EXACT REQUIRED) + find_package(PythonLibs ${GTSAM_PYTHON_VERSION} EXACT REQUIRED) +endif() find_package(Cython 0.25.2 REQUIRED) +execute_process(COMMAND "${PYTHON_EXECUTABLE}" "-c" + "from __future__ import print_function;import sys;print(sys.version[0], end='')" + OUTPUT_VARIABLE PYTHON_MAJOR_VERSION +) + # User-friendly Cython wrapping and installing function. # Builds a Cython module from the provided interface_header. # For example, for the interface header gtsam.h, @@ -29,12 +44,12 @@ endfunction() function(set_up_required_cython_packages) # Set up building of cython module - find_package(PythonLibs 2.7 REQUIRED) include_directories(${PYTHON_INCLUDE_DIRS}) find_package(NumPy REQUIRED) include_directories(${NUMPY_INCLUDE_DIRS}) endfunction() + # Convert pyx to cpp by executing cython # This is the first step to compile cython from the command line # as described at: http://cython.readthedocs.io/en/latest/src/reference/compilation.html @@ -52,7 +67,7 @@ function(pyx_to_cpp target pyx_file generated_cpp include_dirs) add_custom_command( OUTPUT ${generated_cpp} COMMAND - ${CYTHON_EXECUTABLE} -X boundscheck=False -v --fast-fail --cplus ${includes_for_cython} ${pyx_file} -o ${generated_cpp} + ${CYTHON_EXECUTABLE} -X boundscheck=False -v --fast-fail --cplus -${PYTHON_MAJOR_VERSION} ${includes_for_cython} ${pyx_file} -o ${generated_cpp} VERBATIM) add_custom_target(${target} ALL DEPENDS ${generated_cpp}) endfunction() diff --git a/cmake/example_project/CMakeLists.txt b/cmake/example_project/CMakeLists.txt index e287087d1..2afc6edc2 100644 --- a/cmake/example_project/CMakeLists.txt +++ b/cmake/example_project/CMakeLists.txt @@ -22,7 +22,10 @@ include_directories(BEFORE "${PROJECT_SOURCE_DIR}") ################################################################################### # Find GTSAM components find_package(GTSAM REQUIRED) # Uses installed package -include_directories(${GTSAM_INCLUDE_DIR}) +# Note: Since Jan-2019, GTSAMConfig.cmake defines exported CMake targets +# that automatically do include the include_directories() without the need +# to call include_directories(), just target_link_libraries(NAME gtsam) +#include_directories(${GTSAM_INCLUDE_DIR}) ################################################################################### # Build static library from common sources diff --git a/cython/CMakeLists.txt b/cython/CMakeLists.txt index bc21b91d1..a351ec52b 100644 --- a/cython/CMakeLists.txt +++ b/cython/CMakeLists.txt @@ -19,22 +19,25 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX) # wrap gtsam_unstable if(GTSAM_BUILD_UNSTABLE) add_custom_target(gtsam_unstable_header DEPENDS "../gtsam_unstable/gtsam_unstable.h") - set(GTSAM_UNSTABLE_IMPORT "from gtsam_unstable import *") wrap_and_install_library_cython("../gtsam_unstable/gtsam_unstable.h" # interface_header "from gtsam.gtsam cimport *" # extra imports - "${GTSAM_CYTHON_INSTALL_PATH}/gtsam" # install path + "${GTSAM_CYTHON_INSTALL_PATH}/gtsam_unstable" # install path gtsam_unstable # library to link with "gtsam_unstable;gtsam_unstable_header;cythonize_gtsam" # dependencies to be built before wrapping ) - # for some reasons cython gtsam_unstable can't find gtsam/gtsam.pxd without doing this - file(WRITE ${PROJECT_BINARY_DIR}/cython/gtsam_unstable/__init__.py "") endif() + file(READ "${PROJECT_SOURCE_DIR}/cython/requirements.txt" CYTHON_INSTALL_REQUIREMENTS) + file(READ "${PROJECT_SOURCE_DIR}/README.md" README_CONTENTS) + # Install the custom-generated __init__.py # This is to make the build/cython/gtsam folder a python package, so gtsam can be found while wrapping gtsam_unstable - configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam/__init__.py.in ${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py) - install_cython_files("${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py" "${GTSAM_CYTHON_INSTALL_PATH}/gtsam") + configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam/__init__.py ${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py COPYONLY) + configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam_unstable/__init__.py ${PROJECT_BINARY_DIR}/cython/gtsam_unstable/__init__.py COPYONLY) + configure_file(${PROJECT_SOURCE_DIR}/cython/setup.py.in ${PROJECT_BINARY_DIR}/cython/setup.py) + install_cython_files("${PROJECT_BINARY_DIR}/cython/setup.py" "${GTSAM_CYTHON_INSTALL_PATH}") # install scripts and tests install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam" "${GTSAM_CYTHON_INSTALL_PATH}" "*.py") + install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam_unstable" "${GTSAM_CYTHON_INSTALL_PATH}" "*.py") endif () diff --git a/cython/README.md b/cython/README.md index 368d2a76d..b9ce2f012 100644 --- a/cython/README.md +++ b/cython/README.md @@ -2,23 +2,30 @@ This is the Cython/Python wrapper around the GTSAM C++ library. 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: ```bash pip install -r /cython/requirements.txt ``` -- For compatiblity with gtsam's Eigen version, it contains its own cloned version of [Eigency](https://github.com/wouterboomsma/eigency.git), +- For compatibility with gtsam's Eigen version, it contains its own cloned version of [Eigency](https://github.com/wouterboomsma/eigency.git), named **gtsam_eigency**, to interface between C++'s Eigen and Python's numpy. -- Build and install gtsam using cmake with GTSAM_INSTALL_CYTHON_TOOLBOX enabled. -The wrapped module will be installed to GTSAM_CYTHON_INSTALL_PATH, which is -by default: /cython +- Build and install gtsam using cmake with `GTSAM_INSTALL_CYTHON_TOOLBOX` enabled. +The wrapped module will be installed to `GTSAM_CYTHON_INSTALL_PATH`, which is +by default: `/cython` -- Modify your PYTHONPATH to include the GTSAM_CYTHON_INSTALL_PATH: +- To use the library without installing system-wide: modify your `PYTHONPATH` to include the `GTSAM_CYTHON_INSTALL_PATH`: ```bash export PYTHONPATH=$PYTHONPATH: ``` +- To install system-wide: run `make install` then navigate to `GTSAM_CYTHON_INSTALL_PATH` and run `python setup.py install` + - (the same command can be used to install into a virtual environment if it is active) + - note: if you don't want gtsam to install to a system directory such as `/usr/local`, pass `-DCMAKE_INSTALL_PREFIX="./install"` to cmake to install gtsam to a subdirectory of the build directory. + - 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 ========== diff --git a/cython/gtsam/__init__.py b/cython/gtsam/__init__.py new file mode 100644 index 000000000..d40ee4502 --- /dev/null +++ b/cython/gtsam/__init__.py @@ -0,0 +1,26 @@ +from .gtsam import * + +try: + import gtsam_unstable + + + def _deprecated_wrapper(item, name): + def wrapper(*args, **kwargs): + from warnings import warn + message = ('importing the unstable item "{}" directly from gtsam is deprecated. '.format(name) + + 'Please import it from gtsam_unstable.') + warn(message) + return item(*args, **kwargs) + return wrapper + + + for name in dir(gtsam_unstable): + if not name.startswith('__'): + item = getattr(gtsam_unstable, name) + if callable(item): + item = _deprecated_wrapper(item, name) + globals()[name] = item + +except ImportError: + pass + diff --git a/cython/gtsam/__init__.py.in b/cython/gtsam/__init__.py.in deleted file mode 100644 index 7d456023f..000000000 --- a/cython/gtsam/__init__.py.in +++ /dev/null @@ -1,2 +0,0 @@ -from gtsam import * -${GTSAM_UNSTABLE_IMPORT} diff --git a/cython/gtsam/examples/OdometryExample.py b/cython/gtsam/examples/OdometryExample.py index 5d2190d56..e778e3f85 100644 --- a/cython/gtsam/examples/OdometryExample.py +++ b/cython/gtsam/examples/OdometryExample.py @@ -17,6 +17,9 @@ import numpy as np import gtsam +import matplotlib.pyplot as plt +import gtsam.utils.plot as gtsam_plot + # Create noise models ODOMETRY_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) @@ -50,3 +53,17 @@ params = gtsam.LevenbergMarquardtParams() optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) result = optimizer.optimize() print("\nFinal Result:\n{}".format(result)) + +# 5. Calculate and print marginal covariances for all variables +marginals = gtsam.Marginals(graph, result) +for i in range(1, 4): + print("X{} covariance:\n{}\n".format(i, marginals.marginalCovariance(i))) + +fig = plt.figure(0) +for i in range(1, 4): + gtsam_plot.plot_pose2(0, result.atPose2(i), 0.5, marginals.marginalCovariance(i)) +plt.axis('equal') +plt.show() + + + diff --git a/cython/gtsam/examples/PlanarManipulatorExample.py b/cython/gtsam/examples/PlanarManipulatorExample.py index af21e6ca5..e42ae09d7 100644 --- a/cython/gtsam/examples/PlanarManipulatorExample.py +++ b/cython/gtsam/examples/PlanarManipulatorExample.py @@ -24,6 +24,7 @@ from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611 import gtsam import gtsam.utils.plot as gtsam_plot from gtsam import Pose2 +from gtsam.utils.test_case import GtsamTestCase def vector3(x, y, z): @@ -114,7 +115,7 @@ class ThreeLinkArm(object): l1Zl1 = expmap(0.0, 0.0, q[0]) l2Zl2 = expmap(0.0, self.L1, q[1]) - l3Zl3 = expmap(0.0, 7.0, q[2]) + l3Zl3 = expmap(0.0, self.L1+self.L2, q[2]) return compose(l1Zl1, l2Zl2, l3Zl3, self.sXt0) def ik(self, sTt_desired, e=1e-9): @@ -202,7 +203,7 @@ Q1 = np.radians(vector3(-30, -45, -90)) Q2 = np.radians(vector3(-90, 90, 0)) -class TestPose2SLAMExample(unittest.TestCase): +class TestPose2SLAMExample(GtsamTestCase): """Unit tests for functions used below.""" def setUp(self): @@ -296,12 +297,18 @@ def run_example(): """ Use trajectory interpolation and then trajectory tracking a la Murray to move a 3-link arm on a straight line. """ + # Create arm arm = ThreeLinkArm() + + # Get initial pose using forward kinematics q = np.radians(vector3(30, -30, 45)) sTt_initial = arm.fk(q) + + # Create interpolated trajectory in task space to desired goal pose sTt_goal = Pose2(2.4, 4.3, math.radians(0)) poses = trajectory(sTt_initial, sTt_goal, 50) + # Setup figure and plot initial pose fignum = 0 fig = plt.figure(fignum) axes = fig.gca() @@ -309,6 +316,8 @@ def run_example(): axes.set_ylim(0, 10) gtsam_plot.plot_pose2(fignum, arm.fk(q)) + # For all poses in interpolated trajectory, calculate dq to move to next pose. + # We do this by calculating the local Jacobian J and doing dq = inv(J)*delta(sTt, pose). for pose in poses: sTt = arm.fk(q) error = delta(sTt, pose) diff --git a/cython/gtsam/examples/Pose2SLAMExample.py b/cython/gtsam/examples/Pose2SLAMExample.py index b15b90864..680f2209f 100644 --- a/cython/gtsam/examples/Pose2SLAMExample.py +++ b/cython/gtsam/examples/Pose2SLAMExample.py @@ -19,6 +19,9 @@ import numpy as np import gtsam +import matplotlib.pyplot as plt +import gtsam.utils.plot as gtsam_plot + def vector3(x, y, z): """Create 3d double numpy array.""" @@ -85,3 +88,10 @@ print("Final Result:\n{}".format(result)) marginals = gtsam.Marginals(graph, result) for i in range(1, 6): print("X{} covariance:\n{}\n".format(i, marginals.marginalCovariance(i))) + +fig = plt.figure(0) +for i in range(1, 6): + gtsam_plot.plot_pose2(0, result.atPose2(i), 0.5, marginals.marginalCovariance(i)) + +plt.axis('equal') +plt.show() diff --git a/cython/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py b/cython/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py new file mode 100644 index 000000000..02c696905 --- /dev/null +++ b/cython/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py @@ -0,0 +1,35 @@ +""" +GTSAM Copyright 2010-2018, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved +Authors: Frank Dellaert, et al. (see THANKS for the full author list) + +See LICENSE for the license information + +Initialize PoseSLAM with Chordal init +Author: Luca Carlone, Frank Dellaert (python port) +""" +# pylint: disable=invalid-name, E1101 + +from __future__ import print_function + +import numpy as np + +import gtsam + +# Read graph from file +g2oFile = gtsam.findExampleDataFile("pose3example.txt") + +is3D = True +graph, initial = gtsam.readG2o(g2oFile, is3D) + +# Add prior on the first key. TODO: assumes first key ios z +priorModel = gtsam.noiseModel_Diagonal.Variances( + np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4])) +firstKey = initial.keys().at(0) +graph.add(gtsam.PriorFactorPose3(0, gtsam.Pose3(), priorModel)) + +# Initializing Pose3 - chordal relaxation" +initialization = gtsam.InitializePose3.initialize(graph) + +print(initialization) diff --git a/cython/gtsam/examples/README.md b/cython/gtsam/examples/README.md index 067929a20..35ec4f66d 100644 --- a/cython/gtsam/examples/README.md +++ b/cython/gtsam/examples/README.md @@ -1,4 +1,57 @@ These examples are almost identical to the old handwritten python wrapper examples. However, there are just some slight name changes, for example -noiseModel.Diagonal becomes noiseModel_Diagonal etc... -Also, annoyingly, instead of gtsam.Symbol('b',0) we now need to say gtsam.symbol(ord('b'), 0)) +`noiseModel.Diagonal` becomes `noiseModel_Diagonal` etc... +Also, annoyingly, instead of `gtsam.Symbol('b', 0)` we now need to say `gtsam.symbol(ord('b'), 0))` + +# Porting Progress + +| C++ Example Name | Ported | +|-------------------------------------------------------|--------| +| CameraResectioning | | +| CreateSFMExampleData | | +| DiscreteBayesNet_FG | none of the required discrete functionality is exposed through cython | +| easyPoint2KalmanFilter | ExtendedKalmanFilter not exposed through cython | +| elaboratePoint2KalmanFilter | GaussianSequentialSolver not exposed through cython | +| ImuFactorExample2 | X | +| ImuFactorsExample | | +| ISAM2Example_SmartFactor | | +| ISAM2_SmartFactorStereo_IMU | | +| LocalizationExample | impossible? | +| METISOrderingExample | | +| OdometryExample | X | +| PlanarSLAMExample | X | +| Pose2SLAMExample | X | +| Pose2SLAMExampleExpressions | | +| Pose2SLAMExample_g2o | X | +| Pose2SLAMExample_graph | | +| Pose2SLAMExample_graphviz | | +| Pose2SLAMExample_lago | lago not exposed through cython | +| Pose2SLAMStressTest | | +| Pose2SLAMwSPCG | | +| Pose3SLAMExample_changeKeys | | +| Pose3SLAMExampleExpressions_BearingRangeWithTransform | | +| Pose3SLAMExample_g2o | X | +| Pose3SLAMExample_initializePose3Chordal | | +| Pose3SLAMExample_initializePose3Gradient | | +| RangeISAMExample_plaza2 | | +| SelfCalibrationExample | | +| SFMExample_bal_COLAMD_METIS | | +| SFMExample_bal | | +| SFMExample | X | +| SFMExampleExpressions_bal | | +| SFMExampleExpressions | | +| SFMExample_SmartFactor | | +| SFMExample_SmartFactorPCG | | +| SimpleRotation | X | +| SolverComparer | | +| StereoVOExample | | +| StereoVOExample_large | | +| TimeTBB | | +| UGM_chain | discrete functionality not exposed | +| UGM_small | discrete functionality not exposed | +| VisualISAM2Example | X | +| VisualISAMExample | X | + +Extra Examples (with no C++ equivalent) +- PlanarManipulatorExample +- SFMData diff --git a/cython/gtsam/examples/SFMExample.py b/cython/gtsam/examples/SFMExample.py new file mode 100644 index 000000000..bf46f09d5 --- /dev/null +++ b/cython/gtsam/examples/SFMExample.py @@ -0,0 +1,118 @@ +""" +GTSAM Copyright 2010, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved +Authors: Frank Dellaert, et al. (see THANKS for the full author list) + +See LICENSE for the license information + +A structure-from-motion problem on a simulated dataset +""" +from __future__ import print_function + +import gtsam +import numpy as np +from gtsam.examples import SFMdata +from gtsam.gtsam import (Cal3_S2, DoglegOptimizer, + GenericProjectionFactorCal3_S2, NonlinearFactorGraph, + Point3, Pose3, PriorFactorPoint3, PriorFactorPose3, + Rot3, SimpleCamera, Values) + + +def symbol(name: str, index: int) -> int: + """ helper for creating a symbol without explicitly casting 'name' from str to int """ + return gtsam.symbol(ord(name), index) + + +def main(): + """ + Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). + + Each variable in the system (poses and landmarks) must be identified with a unique key. + We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). + Here we will use Symbols + + In GTSAM, measurement functions are represented as 'factors'. Several common factors + have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. + Here we will use Projection factors to model the camera's landmark observations. + Also, we will initialize the robot at some location using a Prior factor. + + When the factors are created, we will add them to a Factor Graph. As the factors we are using + are nonlinear factors, we will need a Nonlinear Factor Graph. + + Finally, once all of the factors have been added to our factor graph, we will want to + solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values. + GTSAM includes several nonlinear optimizers to perform this step. Here we will use a + trust-region method known as Powell's Degleg + + The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the + nonlinear functions around an initial linearization point, then solve the linear system + to update the linearization point. This happens repeatedly until the solver converges + to a consistent set of variable values. This requires us to specify an initial guess + for each variable, held in a Values container. + """ + + # Define the camera calibration parameters + K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0) + + # Define the camera observation noise model + measurement_noise = gtsam.noiseModel_Isotropic.Sigma(2, 1.0) # one pixel in u and v + + # Create the set of ground-truth landmarks + points = SFMdata.createPoints() + + # Create the set of ground-truth poses + poses = SFMdata.createPoses(K) + + # Create a factor graph + graph = NonlinearFactorGraph() + + # Add a prior on pose x1. This indirectly specifies where the origin is. + # 0.3 rad std on roll,pitch,yaw and 0.1m on x,y,z + pose_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) + factor = PriorFactorPose3(symbol('x', 0), poses[0], pose_noise) + graph.push_back(factor) + + # Simulated measurements from each camera pose, adding them to the factor graph + for i, pose in enumerate(poses): + camera = SimpleCamera(pose, K) + for j, point in enumerate(points): + measurement = camera.project(point) + factor = GenericProjectionFactorCal3_S2( + measurement, measurement_noise, symbol('x', i), symbol('l', j), K) + graph.push_back(factor) + + # Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained + # Here we add a prior on the position of the first landmark. This fixes the scale by indicating the distance + # between the first camera and the first landmark. All other landmark positions are interpreted using this scale. + point_noise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1) + factor = PriorFactorPoint3(symbol('l', 0), points[0], point_noise) + graph.push_back(factor) + graph.print_('Factor Graph:\n') + + # Create the data structure to hold the initial estimate to the solution + # Intentionally initialize the variables off from the ground truth + initial_estimate = Values() + for i, pose in enumerate(poses): + r = Rot3.Rodrigues(-0.1, 0.2, 0.25) + t = Point3(0.05, -0.10, 0.20) + transformed_pose = pose.compose(Pose3(r, t)) + initial_estimate.insert(symbol('x', i), transformed_pose) + for j, point in enumerate(points): + transformed_point = Point3(point.vector() + np.array([-0.25, 0.20, 0.15])) + initial_estimate.insert(symbol('l', j), transformed_point) + initial_estimate.print_('Initial Estimates:\n') + + # Optimize the graph and print results + params = gtsam.DoglegParams() + params.setVerbosity('TERMINATION') + optimizer = DoglegOptimizer(graph, initial_estimate, params) + print('Optimizing:') + result = optimizer.optimize() + result.print_('Final results:\n') + print('initial error = {}'.format(graph.error(initial_estimate))) + print('final error = {}'.format(graph.error(result))) + + +if __name__ == '__main__': + main() diff --git a/cython/gtsam/examples/SimpleRotation.py b/cython/gtsam/examples/SimpleRotation.py new file mode 100644 index 000000000..6e9b1320b --- /dev/null +++ b/cython/gtsam/examples/SimpleRotation.py @@ -0,0 +1,84 @@ +""" +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 + +This example will perform a relatively trivial optimization on +a single variable with a single factor. +""" + +import numpy as np +import gtsam + + +def main(): + """ + Step 1: Create a factor to express a unary constraint + + The "prior" in this case is the measurement from a sensor, + with a model of the noise on the measurement. + + The "Key" created here is a label used to associate parts of the + state (stored in "RotValues") with particular factors. They require + an index to allow for lookup, and should be unique. + + In general, creating a factor requires: + - A key or set of keys labeling the variables that are acted upon + - A measurement value + - A measurement model with the correct dimensionality for the factor + """ + prior = gtsam.Rot2.fromAngle(np.deg2rad(30)) + prior.print_('goal angle') + model = gtsam.noiseModel_Isotropic.Sigma(dim=1, sigma=np.deg2rad(1)) + key = gtsam.symbol(ord('x'), 1) + factor = gtsam.PriorFactorRot2(key, prior, model) + + """ + Step 2: Create a graph container and add the factor to it + + Before optimizing, all factors need to be added to a Graph container, + which provides the necessary top-level functionality for defining a + system of constraints. + + In this case, there is only one factor, but in a practical scenario, + many more factors would be added. + """ + graph = gtsam.NonlinearFactorGraph() + graph.push_back(factor) + graph.print_('full graph') + + """ + Step 3: Create an initial estimate + + An initial estimate of the solution for the system is necessary to + start optimization. This system state is the "Values" instance, + which is similar in structure to a dictionary, in that it maps + keys (the label created in step 1) to specific values. + + The initial estimate provided to optimization will be used as + a linearization point for optimization, so it is important that + all of the variables in the graph have a corresponding value in + this structure. + """ + initial = gtsam.Values() + initial.insert(key, gtsam.Rot2.fromAngle(np.deg2rad(20))) + initial.print_('initial estimate') + + """ + Step 4: Optimize + + After formulating the problem with a graph of constraints + and an initial estimate, executing optimization is as simple + as calling a general optimization function with the graph and + initial estimate. This will yield a new RotValues structure + with the final state of the optimization. + """ + result = gtsam.LevenbergMarquardtOptimizer(graph, initial).optimize() + result.print_('final result') + + +if __name__ == '__main__': + main() diff --git a/cython/gtsam/examples/VisualISAMExample.py b/cython/gtsam/examples/VisualISAMExample.py new file mode 100644 index 000000000..f4d81eaf7 --- /dev/null +++ b/cython/gtsam/examples/VisualISAMExample.py @@ -0,0 +1,106 @@ +""" +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 + +A visualSLAM example for the structure-from-motion problem on a simulated dataset +This version uses iSAM to solve the problem incrementally +""" + +from __future__ import print_function + +import numpy as np +import gtsam +from gtsam.examples import SFMdata +from gtsam.gtsam import (Cal3_S2, GenericProjectionFactorCal3_S2, + NonlinearFactorGraph, NonlinearISAM, Point3, Pose3, + PriorFactorPoint3, PriorFactorPose3, Rot3, + SimpleCamera, Values) + + +def symbol(name: str, index: int) -> int: + """ helper for creating a symbol without explicitly casting 'name' from str to int """ + return gtsam.symbol(ord(name), index) + + +def main(): + """ + A structure-from-motion example with landmarks + - The landmarks form a 10 meter cube + - The robot rotates around the landmarks, always facing towards the cube + """ + + # Define the camera calibration parameters + K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0) + + # Define the camera observation noise model + camera_noise = gtsam.noiseModel_Isotropic.Sigma(2, 1.0) # one pixel in u and v + + # Create the set of ground-truth landmarks + points = SFMdata.createPoints() + # Create the set of ground-truth poses + poses = SFMdata.createPoses(K) + + # Create a NonlinearISAM object which will relinearize and reorder the variables + # every "reorderInterval" updates + isam = NonlinearISAM(reorderInterval=3) + + # Create a Factor Graph and Values to hold the new data + graph = NonlinearFactorGraph() + initial_estimate = Values() + + # Loop over the different poses, adding the observations to iSAM incrementally + for i, pose in enumerate(poses): + camera = SimpleCamera(pose, K) + # Add factors for each landmark observation + for j, point in enumerate(points): + measurement = camera.project(point) + factor = GenericProjectionFactorCal3_S2(measurement, camera_noise, symbol('x', i), symbol('l', j), K) + graph.push_back(factor) + + # Intentionally initialize the variables off from the ground truth + noise = Pose3(r=Rot3.Rodrigues(-0.1, 0.2, 0.25), t=Point3(0.05, -0.10, 0.20)) + initial_xi = pose.compose(noise) + + # Add an initial guess for the current pose + initial_estimate.insert(symbol('x', i), initial_xi) + + # If this is the first iteration, add a prior on the first pose to set the coordinate frame + # and a prior on the first landmark to set the scale + # Also, as iSAM solves incrementally, we must wait until each is observed at least twice before + # adding it to iSAM. + if i == 0: + # Add a prior on pose x0, with 0.3 rad std on roll,pitch,yaw and 0.1m x,y,z + pose_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) + factor = PriorFactorPose3(symbol('x', 0), poses[0], pose_noise) + graph.push_back(factor) + + # Add a prior on landmark l0 + point_noise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1) + factor = PriorFactorPoint3(symbol('l', 0), points[0], point_noise) + graph.push_back(factor) + + # Add initial guesses to all observed landmarks + noise = np.array([-0.25, 0.20, 0.15]) + for j, point in enumerate(points): + # Intentionally initialize the variables off from the ground truth + initial_lj = points[j].vector() + noise + initial_estimate.insert(symbol('l', j), Point3(initial_lj)) + else: + # Update iSAM with the new factors + isam.update(graph, initial_estimate) + current_estimate = isam.estimate() + print('*' * 50) + print('Frame {}:'.format(i)) + current_estimate.print_('Current estimate: ') + + # Clear the factor graph and values for the next iteration + graph.resize(0) + initial_estimate.clear() + + +if __name__ == '__main__': + main() diff --git a/cython/gtsam/tests/experiments.py b/cython/gtsam/tests/experiments.py deleted file mode 100644 index 425180173..000000000 --- a/cython/gtsam/tests/experiments.py +++ /dev/null @@ -1,112 +0,0 @@ -""" -This file is not a real python unittest. It contains small experiments -to test the wrapper with gtsam_test, a short version of gtsam.h. -Its name convention is different from other tests so it won't be discovered. -""" -import gtsam -import numpy as np - -r = gtsam.Rot3() -print(r) -print(r.pitch()) -r2 = gtsam.Rot3() -r3 = r.compose(r2) -print("r3 pitch:", r3.pitch()) - -v = np.array([1, 1, 1]) -print("v = ", v) -r4 = r3.retract(v) -print("r4 pitch:", r4.pitch()) -r4.print_(b'r4: ') -r3.print_(b"r3: ") - -v = r3.localCoordinates(r4) -print("localCoordinates:", v) - -Rmat = np.array([ - [0.990074, -0.0942928, 0.104218], - [0.104218, 0.990074, -0.0942928], - [-0.0942928, 0.104218, 0.990074] - ]) -r5 = gtsam.Rot3(Rmat) -r5.print_(b"r5: ") - -l = gtsam.Rot3.Logmap(r5) -print("l = ", l) - - -noise = gtsam.noiseModel_Gaussian.Covariance(Rmat) -noise.print_(b"noise:") - -D = np.array([1.,2.,3.]) -diag = gtsam.noiseModel_Diagonal.Variances(D) -print("diag:", diag) -diag.print_(b"diag:") -print("diag R:", diag.R()) - -p = gtsam.Point3() -p.print_("p:") -factor = gtsam.BetweenFactorPoint3(1,2,p, noise) -factor.print_(b"factor:") - -vv = gtsam.VectorValues() -vv.print_(b"vv:") -vv.insert(1, np.array([1.,2.,3.])) -vv.insert(2, np.array([3.,4.])) -vv.insert(3, np.array([5.,6.,7.,8.])) -vv.print_(b"vv:") - -vv2 = gtsam.VectorValues(vv) -vv2.insert(4, np.array([4.,2.,1])) -vv2.print_(b"vv2:") -vv.print_(b"vv:") - -vv.insert(4, np.array([1.,2.,4.])) -vv.print_(b"vv:") -vv3 = vv.add(vv2) - -vv3.print_(b"vv3:") - -values = gtsam.Values() -values.insert(1, gtsam.Point3()) -values.insert(2, gtsam.Rot3()) -values.print_(b"values:") - -factor = gtsam.PriorFactorVector(1, np.array([1.,2.,3.]), diag) -print "Prior factor vector: ", factor - - - -keys = gtsam.KeyVector() - -keys.push_back(1) -keys.push_back(2) -print 'size: ', keys.size() -print keys.at(0) -print keys.at(1) - -noise = gtsam.noiseModel_Isotropic.Precision(2, 3.0) -noise.print_('noise:') -print 'noise print:', noise -f = gtsam.JacobianFactor(7, np.ones([2,2]), model=noise, b=np.ones(2)) -print 'JacobianFactor(7):\n', f -print "A = ", f.getA() -print "b = ", f.getb() - -f = gtsam.JacobianFactor(np.ones(2)) -f.print_('jacoboian b_in:') - - -print "JacobianFactor initalized with b_in:", f - -diag = gtsam.noiseModel_Diagonal.Sigmas(np.array([1.,2.,3.])) -fv = gtsam.PriorFactorVector(1, np.array([4.,5.,6.]), diag) -print "priorfactorvector: ", fv - -print "base noise: ", fv.get_noiseModel() -print "casted to gaussian2: ", gtsam.dynamic_cast_noiseModel_Diagonal_noiseModel_Base(fv.get_noiseModel()) - -X = gtsam.symbol(65, 19) -print X -print gtsam.symbolChr(X) -print gtsam.symbolIndex(X) diff --git a/cython/gtsam/tests/gtsam_test.h b/cython/gtsam/tests/gtsam_test.h deleted file mode 100644 index 659a7cd0c..000000000 --- a/cython/gtsam/tests/gtsam_test.h +++ /dev/null @@ -1,772 +0,0 @@ -namespace gtsam { - -#include -typedef size_t Key; - -#include -template class FastVector { - FastVector(); - FastVector(const This& f); - void push_back(const T& e); - //T& operator[](int); - T at(int i); - size_t size() const; -}; - -typedef gtsam::FastVector KeyVector; - -//************************************************************************* -// geometry -//************************************************************************* - -#include -class Point2 { - // Standard Constructors - Point2(); - Point2(double x, double y); - Point2(Vector v); - //Point2(const gtsam::Point2& l); - - // Testable - void print(string s) const; - bool equals(const gtsam::Point2& pose, double tol) const; - - // Group - static gtsam::Point2 identity(); - - // Standard Interface - double x() const; - double y() const; - Vector vector() const; - double distance(const gtsam::Point2& p2) const; - double norm() const; - - // enabling serialization functionality - void serialize() const; -}; - -#include -class Point3 { - // Standard Constructors - Point3(); - Point3(double x, double y, double z); - Point3(Vector v); - //Point3(const gtsam::Point3& l); - - // 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; -}; - -#include -class Rot2 { - // Standard Constructors and Named Constructors - Rot2(); - Rot2(double theta); - //Rot2(const gtsam::Rot2& l); - - 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; -}; - -#include -class Rot3 { - // Standard Constructors and Named Constructors - Rot3(); - Rot3(Matrix R); - //Rot3(const gtsam::Rot3& l); - - 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; -}; - -#include -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 transform_from(const gtsam::Point2& p) const; - gtsam::Point2 transform_to(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; -}; - - -#include -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 transform_from(const gtsam::Point3& p) const; - gtsam::Point3 transform_to(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 transform_to(const gtsam::Pose3& pose) const; // FIXME: shadows other transform_to() - double range(const gtsam::Point3& point); - double range(const gtsam::Pose3& pose); - - // enabling serialization functionality - void serialize() const; -}; - -//************************************************************************* -// noise -//************************************************************************* - -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(); - //Null(const gtsam::noiseModel::mEstimator::Null& other); - 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); - //Fair(const gtsam::noiseModel::mEstimator::Fair& other); - 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); - //Huber(const gtsam::noiseModel::mEstimator::Huber& other); - - 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); - //Tukey(const gtsam::noiseModel::mEstimator::Tukey& other); - - 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); - //Robust(const gtsam::noiseModel::Robust& other); - - 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); - //Sampler(const gtsam::Sampler& other); - - - //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); - //JacobianFactor(const gtsam::JacobianFactor& other); - - //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); - //HessianFactor(const gtsam::HessianFactor& other); - - //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 information() const; - double constantTerm() const; - Vector linearTerm() const; - - // enabling serialization functionality - void serialize() 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; - - // template - // void insert(size_t j, const T& t); - - // template - // void update(size_t j, const T& t); - 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, Vector t); - void insert(size_t j, Matrix t); - - 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, Vector t); - void update(size_t j, Matrix t); - - template - T at(size_t j); - - /// version for double - void insertDouble(size_t j, double c); - double atDouble(size_t j) const; -}; - -#include -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 - bool 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 -}; - -#include -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; -}; - -#include -virtual class NoiseModelFactor: gtsam::NonlinearFactor { - void equals(const gtsam::NoiseModelFactor& other, double tol) const; - gtsam::noiseModel::Base* get_noiseModel() const; // deprecated by below - gtsam::noiseModel::Base* noiseModel() const; - Vector unwhitenedError(const gtsam::Values& x) const; - Vector whitenedError(const gtsam::Values& x) const; -}; - -#include -template -virtual class PriorFactor : gtsam::NoiseModelFactor { - PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); - //PriorFactor(const This& other); - 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); - //BetweenFactor(const This& other); - T measured() const; - - // enabling serialization functionality - void serialize() const; -}; - -#include -size_t symbol(char chr, size_t index); -char symbolChr(size_t key); -size_t symbolIndex(size_t key); - -#include -// Default keyformatter -void PrintKeyVector(const gtsam::KeyVector& keys); -void PrintKeyVector(const gtsam::KeyVector& keys, string s); - -#include -bool checkConvergence(double relativeErrorTreshold, - double absoluteErrorTreshold, double errorThreshold, - double currentError, double newError); - -#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); - -//************************************************************************* -// 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 - -#include -class RedirectCout { - RedirectCout(); - string str(); -}; - -} //\namespace gtsam diff --git a/cython/gtsam/tests/test_Cal3Unified.py b/cython/gtsam/tests/test_Cal3Unified.py index 3225d2ff9..fbf5f3565 100644 --- a/cython/gtsam/tests/test_Cal3Unified.py +++ b/cython/gtsam/tests/test_Cal3Unified.py @@ -1,9 +1,22 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Cal3Unified unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import unittest -import gtsam + import numpy as np +import gtsam +from gtsam.utils.test_case import GtsamTestCase -class TestCal3Unified(unittest.TestCase): + +class TestCal3Unified(GtsamTestCase): def test_Cal3Unified(self): K = gtsam.Cal3Unified() @@ -11,12 +24,15 @@ class TestCal3Unified(unittest.TestCase): self.assertEqual(K.fx(), 1.) def test_retract(self): - expected = gtsam.Cal3Unified(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1) - K = gtsam.Cal3Unified(100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0.1) + expected = gtsam.Cal3Unified(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, + 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1) + K = gtsam.Cal3Unified(100, 105, 0.0, 320, 240, + 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0.1) d = np.array([2, 3, 4, 5, 6, 7, 8, 9, 10, 1], order='F') actual = K.retract(d) - self.assertTrue(actual.equals(expected, 1e-9)) + self.gtsamAssertEquals(actual, expected) np.testing.assert_allclose(d, K.localCoordinates(actual)) + if __name__ == "__main__": unittest.main() diff --git a/cython/gtsam/tests/test_JacobianFactor.py b/cython/gtsam/tests/test_JacobianFactor.py index bf63c839b..04433492b 100644 --- a/cython/gtsam/tests/test_JacobianFactor.py +++ b/cython/gtsam/tests/test_JacobianFactor.py @@ -1,8 +1,22 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +JacobianFactor unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import unittest -import gtsam + import numpy as np -class TestJacobianFactor(unittest.TestCase): +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestJacobianFactor(GtsamTestCase): def test_eliminate(self): # Recommended way to specify a matrix (see cython/README) @@ -54,7 +68,7 @@ class TestJacobianFactor(unittest.TestCase): expectedCG = gtsam.GaussianConditional( x2, d, R11, l1, S12, x1, S13, gtsam.noiseModel_Unit.Create(2)) # check if the result matches - self.assertTrue(actualCG.equals(expectedCG, 1e-4)) + self.gtsamAssertEquals(actualCG, expectedCG, 1e-4) # the expected linear factor Bl1 = np.array([[4.47214, 0.00], @@ -72,7 +86,7 @@ class TestJacobianFactor(unittest.TestCase): expectedLF = gtsam.JacobianFactor(l1, Bl1, x1, Bx1, b1, model2) # check if the result matches the combined (reduced) factor - self.assertTrue(lf.equals(expectedLF, 1e-4)) + self.gtsamAssertEquals(lf, expectedLF, 1e-4) if __name__ == "__main__": unittest.main() diff --git a/cython/gtsam/tests/test_KalmanFilter.py b/cython/gtsam/tests/test_KalmanFilter.py index 56f9e2573..94c41df72 100644 --- a/cython/gtsam/tests/test_KalmanFilter.py +++ b/cython/gtsam/tests/test_KalmanFilter.py @@ -1,8 +1,22 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +KalmanFilter unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import unittest -import gtsam + import numpy as np -class TestKalmanFilter(unittest.TestCase): +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestKalmanFilter(GtsamTestCase): def test_KalmanFilter(self): F = np.eye(2) diff --git a/cython/gtsam/tests/test_LocalizationExample.py b/cython/gtsam/tests/test_LocalizationExample.py index c373f162c..6ce65f087 100644 --- a/cython/gtsam/tests/test_LocalizationExample.py +++ b/cython/gtsam/tests/test_LocalizationExample.py @@ -1,8 +1,22 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Localization unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import unittest -import gtsam + import numpy as np -class TestLocalizationExample(unittest.TestCase): +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestLocalizationExample(GtsamTestCase): def test_LocalizationExample(self): # Create the graph (defined in pose2SLAM.h, derived from @@ -43,7 +57,7 @@ class TestLocalizationExample(unittest.TestCase): P = [None] * result.size() for i in range(0, result.size()): pose_i = result.atPose2(i) - self.assertTrue(pose_i.equals(groundTruth.atPose2(i), 1e-4)) + self.gtsamAssertEquals(pose_i, groundTruth.atPose2(i), 1e-4) P[i] = marginals.marginalCovariance(i) if __name__ == "__main__": diff --git a/cython/gtsam/tests/test_NonlinearOptimizer.py b/cython/gtsam/tests/test_NonlinearOptimizer.py new file mode 100644 index 000000000..efefb218a --- /dev/null +++ b/cython/gtsam/tests/test_NonlinearOptimizer.py @@ -0,0 +1,72 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Unit tests for IMU testing scenarios. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" +# pylint: disable=invalid-name, no-name-in-module + +from __future__ import print_function + +import unittest + +import gtsam +from gtsam import (DoglegOptimizer, DoglegParams, GaussNewtonOptimizer, + GaussNewtonParams, LevenbergMarquardtOptimizer, + LevenbergMarquardtParams, NonlinearFactorGraph, Ordering, + Point2, PriorFactorPoint2, Values) +from gtsam.utils.test_case import GtsamTestCase + +KEY1 = 1 +KEY2 = 2 + + +class TestScenario(GtsamTestCase): + def test_optimize(self): + """Do trivial test with three optimizer variants.""" + fg = NonlinearFactorGraph() + model = gtsam.noiseModel_Unit.Create(2) + fg.add(PriorFactorPoint2(KEY1, Point2(0, 0), model)) + + # test error at minimum + xstar = Point2(0, 0) + optimal_values = Values() + optimal_values.insert(KEY1, xstar) + self.assertEqual(0.0, fg.error(optimal_values), 0.0) + + # test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 = + x0 = Point2(3, 3) + initial_values = Values() + initial_values.insert(KEY1, x0) + self.assertEqual(9.0, fg.error(initial_values), 1e-3) + + # optimize parameters + ordering = Ordering() + ordering.push_back(KEY1) + + # Gauss-Newton + gnParams = GaussNewtonParams() + gnParams.setOrdering(ordering) + actual1 = GaussNewtonOptimizer(fg, initial_values, gnParams).optimize() + self.assertAlmostEqual(0, fg.error(actual1)) + + # Levenberg-Marquardt + lmParams = LevenbergMarquardtParams.CeresDefaults() + lmParams.setOrdering(ordering) + actual2 = LevenbergMarquardtOptimizer( + fg, initial_values, lmParams).optimize() + self.assertAlmostEqual(0, fg.error(actual2)) + + # Dogleg + dlParams = DoglegParams() + dlParams.setOrdering(ordering) + actual3 = DoglegOptimizer(fg, initial_values, dlParams).optimize() + self.assertAlmostEqual(0, fg.error(actual3)) + + +if __name__ == "__main__": + unittest.main() diff --git a/cython/gtsam/tests/test_OdometryExample.py b/cython/gtsam/tests/test_OdometryExample.py index 1100e8334..c8ea95588 100644 --- a/cython/gtsam/tests/test_OdometryExample.py +++ b/cython/gtsam/tests/test_OdometryExample.py @@ -1,8 +1,22 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Odometry unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import unittest -import gtsam + import numpy as np -class TestOdometryExample(unittest.TestCase): +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestOdometryExample(GtsamTestCase): def test_OdometryExample(self): # Create the graph (defined in pose2SLAM.h, derived from @@ -39,7 +53,7 @@ class TestOdometryExample(unittest.TestCase): # Check first pose equality pose_1 = result.atPose2(1) - self.assertTrue(pose_1.equals(gtsam.Pose2(), 1e-4)) + self.gtsamAssertEquals(pose_1, gtsam.Pose2(), 1e-4) if __name__ == "__main__": unittest.main() diff --git a/cython/gtsam/tests/test_PlanarSLAMExample.py b/cython/gtsam/tests/test_PlanarSLAMExample.py index 046a93f35..ae813d35c 100644 --- a/cython/gtsam/tests/test_PlanarSLAMExample.py +++ b/cython/gtsam/tests/test_PlanarSLAMExample.py @@ -1,11 +1,25 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +PlanarSLAM unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import unittest -import gtsam from math import pi + import numpy as np -class TestPose2SLAMExample(unittest.TestCase): +import gtsam +from gtsam.utils.test_case import GtsamTestCase - def test_Pose2SLAMExample(self): + +class TestPlanarSLAM(GtsamTestCase): + + def test_PlanarSLAM(self): # Assumptions # - All values are axis aligned # - Robot poses are facing along the X axis (horizontal, to the right in images) @@ -56,7 +70,7 @@ class TestPose2SLAMExample(unittest.TestCase): P = marginals.marginalCovariance(1) pose_1 = result.atPose2(1) - self.assertTrue(pose_1.equals(gtsam.Pose2(), 1e-4)) + self.gtsamAssertEquals(pose_1, gtsam.Pose2(), 1e-4) diff --git a/cython/gtsam/tests/test_Pose2.py b/cython/gtsam/tests/test_Pose2.py new file mode 100644 index 000000000..9652b594a --- /dev/null +++ b/cython/gtsam/tests/test_Pose2.py @@ -0,0 +1,32 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Pose2 unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" +import unittest + +import numpy as np + +import gtsam +from gtsam import Pose2 +from gtsam.utils.test_case import GtsamTestCase + + +class TestPose2(GtsamTestCase): + """Test selected Pose2 methods.""" + + def test_adjoint(self): + """Test adjoint method.""" + xi = np.array([1, 2, 3]) + expected = np.dot(Pose2.adjointMap_(xi), xi) + actual = Pose2.adjoint_(xi, xi) + np.testing.assert_array_equal(actual, expected) + + +if __name__ == "__main__": + unittest.main() diff --git a/cython/gtsam/tests/test_Pose2SLAMExample.py b/cython/gtsam/tests/test_Pose2SLAMExample.py index bcaa7be4f..a79b6b18c 100644 --- a/cython/gtsam/tests/test_Pose2SLAMExample.py +++ b/cython/gtsam/tests/test_Pose2SLAMExample.py @@ -1,9 +1,23 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Pose2SLAM unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import unittest -import gtsam from math import pi + import numpy as np -class TestPose2SLAMExample(unittest.TestCase): +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestPose2SLAMExample(GtsamTestCase): def test_Pose2SLAMExample(self): # Assumptions @@ -56,7 +70,7 @@ class TestPose2SLAMExample(unittest.TestCase): P = marginals.marginalCovariance(1) pose_1 = result.atPose2(1) - self.assertTrue(pose_1.equals(gtsam.Pose2(), 1e-4)) + self.gtsamAssertEquals(pose_1, gtsam.Pose2(), 1e-4) if __name__ == "__main__": unittest.main() diff --git a/cython/gtsam/tests/test_Pose3.py b/cython/gtsam/tests/test_Pose3.py index 4752a4b02..3eb4067c9 100644 --- a/cython/gtsam/tests/test_Pose3.py +++ b/cython/gtsam/tests/test_Pose3.py @@ -1,13 +1,24 @@ -"""Pose3 unit tests.""" +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Pose3 unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import math import unittest import numpy as np +import gtsam from gtsam import Point3, Pose3, Rot3 +from gtsam.utils.test_case import GtsamTestCase -class TestPose3(unittest.TestCase): +class TestPose3(GtsamTestCase): """Test selected Pose3 methods.""" def test_between(self): @@ -16,14 +27,14 @@ class TestPose3(unittest.TestCase): T3 = Pose3(Rot3.Rodrigues(-90, 0, 0), Point3(1, 2, 3)) expected = T2.inverse().compose(T3) actual = T2.between(T3) - self.assertTrue(actual.equals(expected, 1e-6)) + self.gtsamAssertEquals(actual, expected, 1e-6) def test_transform_to(self): """Test transform_to method.""" transform = Pose3(Rot3.Rodrigues(0, 0, -1.570796), Point3(2, 4, 0)) actual = transform.transform_to(Point3(3, 2, 10)) expected = Point3(2, 1, 10) - self.assertTrue(actual.equals(expected, 1e-6)) + self.gtsamAssertEquals(actual, expected, 1e-6) def test_range(self): """Test range method.""" @@ -49,8 +60,8 @@ class TestPose3(unittest.TestCase): def test_adjoint(self): """Test adjoint method.""" xi = np.array([1, 2, 3, 4, 5, 6]) - expected = np.dot(Pose3.adjointMap(xi), xi) - actual = Pose3.adjoint(xi, xi) + expected = np.dot(Pose3.adjointMap_(xi), xi) + actual = Pose3.adjoint_(xi, xi) np.testing.assert_array_equal(actual, expected) diff --git a/cython/gtsam/tests/test_Pose3SLAMExample.py b/cython/gtsam/tests/test_Pose3SLAMExample.py index e33db2145..1e9eaac67 100644 --- a/cython/gtsam/tests/test_Pose3SLAMExample.py +++ b/cython/gtsam/tests/test_Pose3SLAMExample.py @@ -1,10 +1,24 @@ -import unittest -import numpy as np -import gtsam -from math import pi -from gtsam.utils.circlePose3 import * +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved -class TestPose3SLAMExample(unittest.TestCase): +See LICENSE for the license information + +PoseSLAM unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" +import unittest +from math import pi + +import numpy as np + +import gtsam +from gtsam.utils.test_case import GtsamTestCase +from gtsam.utils.circlePose3 import * + + +class TestPose3SLAMExample(GtsamTestCase): def test_Pose3SLAMExample(self): # Create a hexagon of poses @@ -40,7 +54,7 @@ class TestPose3SLAMExample(unittest.TestCase): result = optimizer.optimizeSafely() pose_1 = result.atPose3(1) - self.assertTrue(pose_1.equals(p1, 1e-4)) + self.gtsamAssertEquals(pose_1, p1, 1e-4) if __name__ == "__main__": unittest.main() diff --git a/cython/gtsam/tests/test_PriorFactor.py b/cython/gtsam/tests/test_PriorFactor.py index 95ec2ae94..0acf50c2c 100644 --- a/cython/gtsam/tests/test_PriorFactor.py +++ b/cython/gtsam/tests/test_PriorFactor.py @@ -1,8 +1,22 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +PriorFactor unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import unittest -import gtsam + import numpy as np -class TestPriorFactor(unittest.TestCase): +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestPriorFactor(GtsamTestCase): def test_PriorFactor(self): values = gtsam.Values() diff --git a/cython/gtsam/tests/test_SFMExample.py b/cython/gtsam/tests/test_SFMExample.py index 606b26a43..e8fa46186 100644 --- a/cython/gtsam/tests/test_SFMExample.py +++ b/cython/gtsam/tests/test_SFMExample.py @@ -1,11 +1,24 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +SFM unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import unittest -import gtsam -from gtsam import symbol + import numpy as np + +import gtsam import gtsam.utils.visual_data_generator as generator +from gtsam import symbol +from gtsam.utils.test_case import GtsamTestCase -class TestSFMExample(unittest.TestCase): +class TestSFMExample(GtsamTestCase): def test_SFMExample(self): options = generator.Options() @@ -59,11 +72,11 @@ class TestSFMExample(unittest.TestCase): # Check optimized results, should be equal to ground truth for i in range(len(truth.cameras)): pose_i = result.atPose3(symbol(ord('x'), i)) - self.assertTrue(pose_i.equals(truth.cameras[i].pose(), 1e-5)) + self.gtsamAssertEquals(pose_i, truth.cameras[i].pose(), 1e-5) for j in range(len(truth.points)): point_j = result.atPoint3(symbol(ord('p'), j)) - self.assertTrue(point_j.equals(truth.points[j], 1e-5)) + self.gtsamAssertEquals(point_j, truth.points[j], 1e-5) if __name__ == "__main__": unittest.main() diff --git a/cython/gtsam/tests/test_Scenario.py b/cython/gtsam/tests/test_Scenario.py index 4cca1400b..09601fba5 100644 --- a/cython/gtsam/tests/test_Scenario.py +++ b/cython/gtsam/tests/test_Scenario.py @@ -1,11 +1,27 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Scenario unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" +from __future__ import print_function + import math import unittest + import numpy as np import gtsam +from gtsam.utils.test_case import GtsamTestCase + +# pylint: disable=invalid-name, E1101 -class TestScenario(unittest.TestCase): +class TestScenario(GtsamTestCase): def setUp(self): pass @@ -29,7 +45,8 @@ class TestScenario(unittest.TestCase): T30 = scenario.pose(T) np.testing.assert_almost_equal( np.array([math.pi, 0, math.pi]), T30.rotation().xyz()) - self.assert_(gtsam.Point3(0, 0, 2 * R).equals(T30.translation(), 1e-9)) + self.gtsamAssertEquals(gtsam.Point3( + 0, 0, 2.0 * R), T30.translation(), 1e-9) if __name__ == '__main__': diff --git a/cython/gtsam/tests/test_SimpleCamera.py b/cython/gtsam/tests/test_SimpleCamera.py index 7924a9b1c..efdfec561 100644 --- a/cython/gtsam/tests/test_SimpleCamera.py +++ b/cython/gtsam/tests/test_SimpleCamera.py @@ -1,18 +1,31 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +SimpleCamera unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import math -import numpy as np import unittest -from gtsam import Pose2, Point3, Rot3, Pose3, Cal3_S2, SimpleCamera +import numpy as np + +import gtsam +from gtsam import Cal3_S2, Point3, Pose2, Pose3, Rot3, SimpleCamera +from gtsam.utils.test_case import GtsamTestCase K = Cal3_S2(625, 625, 0, 0, 0) -class TestSimpleCamera(unittest.TestCase): +class TestSimpleCamera(GtsamTestCase): def test_constructor(self): pose1 = Pose3(Rot3(np.diag([1, -1, -1])), Point3(0, 0, 0.5)) camera = SimpleCamera(pose1, K) - self.assertTrue(camera.calibration().equals(K, 1e-9)) - self.assertTrue(camera.pose().equals(pose1, 1e-9)) + self.gtsamAssertEquals(camera.calibration(), K, 1e-9) + self.gtsamAssertEquals(camera.pose(), pose1, 1e-9) def test_level2(self): # Create a level camera, looking in Y-direction @@ -25,7 +38,7 @@ class TestSimpleCamera(unittest.TestCase): z = Point3(0,1,0) wRc = Rot3(x,y,z) expected = Pose3(wRc,Point3(0.4,0.3,0.1)) - self.assertTrue(camera.pose().equals(expected, 1e-9)) + self.gtsamAssertEquals(camera.pose(), expected, 1e-9) if __name__ == "__main__": diff --git a/cython/gtsam/tests/test_StereoVOExample.py b/cython/gtsam/tests/test_StereoVOExample.py index dacd4a116..3f5f57522 100644 --- a/cython/gtsam/tests/test_StereoVOExample.py +++ b/cython/gtsam/tests/test_StereoVOExample.py @@ -1,10 +1,23 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Stereo VO unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import unittest -import gtsam -from gtsam import symbol + import numpy as np +import gtsam +from gtsam import symbol +from gtsam.utils.test_case import GtsamTestCase -class TestStereoVOExample(unittest.TestCase): + +class TestStereoVOExample(GtsamTestCase): def test_StereoVOExample(self): ## Assumptions @@ -60,10 +73,10 @@ class TestStereoVOExample(unittest.TestCase): ## check equality for the first pose and point pose_x1 = result.atPose3(x1) - self.assertTrue(pose_x1.equals(first_pose,1e-4)) + self.gtsamAssertEquals(pose_x1, first_pose,1e-4) point_l1 = result.atPoint3(l1) - self.assertTrue(point_l1.equals(expected_l1,1e-4)) + self.gtsamAssertEquals(point_l1, expected_l1,1e-4) if __name__ == "__main__": unittest.main() diff --git a/cython/gtsam/tests/test_Values.py b/cython/gtsam/tests/test_Values.py index 08e133840..20634a21c 100644 --- a/cython/gtsam/tests/test_Values.py +++ b/cython/gtsam/tests/test_Values.py @@ -1,19 +1,34 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Values unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" +# pylint: disable=invalid-name, E1101, E0611 import unittest + import numpy as np -from gtsam import Point2, Point3, Unit3, Rot2, Pose2, Rot3, Pose3 -from gtsam import Values, Cal3_S2, Cal3DS2, Cal3Bundler, EssentialMatrix, imuBias_ConstantBias +import gtsam +from gtsam import (Cal3_S2, Cal3Bundler, Cal3DS2, EssentialMatrix, Point2, + Point3, Pose2, Pose3, Rot2, Rot3, Unit3, Values, + imuBias_ConstantBias) +from gtsam.utils.test_case import GtsamTestCase -class TestValues(unittest.TestCase): +class TestValues(GtsamTestCase): def test_values(self): values = Values() E = EssentialMatrix(Rot3(), Unit3()) tol = 1e-9 - values.insert(0, Point2(0,0)) - values.insert(1, Point3(0,0,0)) + values.insert(0, Point2(0, 0)) + values.insert(1, Point3(0, 0, 0)) values.insert(2, Rot2()) values.insert(3, Pose2()) values.insert(4, Rot3()) @@ -34,36 +49,38 @@ class TestValues(unittest.TestCase): # The wrapper will automatically fix the type and storage order for you, # but for performance reasons, it's recommended to specify the correct # type and storage order. - vec = np.array([1., 2., 3.]) # for vectors, the order is not important, but dtype still is + # for vectors, the order is not important, but dtype still is + vec = np.array([1., 2., 3.]) values.insert(11, vec) mat = np.array([[1., 2.], [3., 4.]], order='F') values.insert(12, mat) # Test with dtype int and the default order='C' # This still works as the wrapper converts to the correct type and order for you # but is nornally not recommended! - mat2 = np.array([[1,2,],[3,5]]) + mat2 = np.array([[1, 2, ], [3, 5]]) values.insert(13, mat2) - self.assertTrue(values.atPoint2(0).equals(Point2(), tol)) - self.assertTrue(values.atPoint3(1).equals(Point3(), tol)) - self.assertTrue(values.atRot2(2).equals(Rot2(), tol)) - self.assertTrue(values.atPose2(3).equals(Pose2(), tol)) - self.assertTrue(values.atRot3(4).equals(Rot3(), tol)) - self.assertTrue(values.atPose3(5).equals(Pose3(), tol)) - self.assertTrue(values.atCal3_S2(6).equals(Cal3_S2(), tol)) - self.assertTrue(values.atCal3DS2(7).equals(Cal3DS2(), tol)) - self.assertTrue(values.atCal3Bundler(8).equals(Cal3Bundler(), tol)) - self.assertTrue(values.atEssentialMatrix(9).equals(E, tol)) - self.assertTrue(values.atimuBias_ConstantBias( - 10).equals(imuBias_ConstantBias(), tol)) + self.gtsamAssertEquals(values.atPoint2(0), Point2(0,0), tol) + self.gtsamAssertEquals(values.atPoint3(1), Point3(0,0,0), tol) + self.gtsamAssertEquals(values.atRot2(2), Rot2(), tol) + self.gtsamAssertEquals(values.atPose2(3), Pose2(), tol) + self.gtsamAssertEquals(values.atRot3(4), Rot3(), tol) + self.gtsamAssertEquals(values.atPose3(5), Pose3(), tol) + self.gtsamAssertEquals(values.atCal3_S2(6), Cal3_S2(), tol) + self.gtsamAssertEquals(values.atCal3DS2(7), Cal3DS2(), tol) + self.gtsamAssertEquals(values.atCal3Bundler(8), Cal3Bundler(), tol) + self.gtsamAssertEquals(values.atEssentialMatrix(9), E, tol) + self.gtsamAssertEquals(values.atimuBias_ConstantBias( + 10), imuBias_ConstantBias(), tol) # special cases for Vector and Matrix: actualVector = values.atVector(11) - self.assertTrue(np.allclose(vec, actualVector, tol)) + np.testing.assert_allclose(vec, actualVector, tol) actualMatrix = values.atMatrix(12) - self.assertTrue(np.allclose(mat, actualMatrix, tol)) + np.testing.assert_allclose(mat, actualMatrix, tol) actualMatrix2 = values.atMatrix(13) - self.assertTrue(np.allclose(mat2, actualMatrix2, tol)) + np.testing.assert_allclose(mat2, actualMatrix2, tol) + if __name__ == "__main__": unittest.main() diff --git a/cython/gtsam/tests/test_VisualISAMExample.py b/cython/gtsam/tests/test_VisualISAMExample.py index 39bfa6eb4..99d7e6160 100644 --- a/cython/gtsam/tests/test_VisualISAMExample.py +++ b/cython/gtsam/tests/test_VisualISAMExample.py @@ -1,10 +1,25 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +visual_isam unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" import unittest + import numpy as np -from gtsam import symbol + +import gtsam import gtsam.utils.visual_data_generator as generator import gtsam.utils.visual_isam as visual_isam +from gtsam import symbol +from gtsam.utils.test_case import GtsamTestCase -class TestVisualISAMExample(unittest.TestCase): + +class TestVisualISAMExample(GtsamTestCase): def test_VisualISAMExample(self): # Data Options @@ -32,11 +47,11 @@ class TestVisualISAMExample(unittest.TestCase): for i in range(len(truth.cameras)): pose_i = result.atPose3(symbol(ord('x'), i)) - self.assertTrue(pose_i.equals(truth.cameras[i].pose(), 1e-5)) + self.gtsamAssertEquals(pose_i, truth.cameras[i].pose(), 1e-5) for j in range(len(truth.points)): point_j = result.atPoint3(symbol(ord('l'), j)) - self.assertTrue(point_j.equals(truth.points[j], 1e-5)) + self.gtsamAssertEquals(point_j, truth.points[j], 1e-5) if __name__ == "__main__": unittest.main() diff --git a/cython/gtsam/tests/test_dataset.py b/cython/gtsam/tests/test_dataset.py new file mode 100644 index 000000000..60fb9450d --- /dev/null +++ b/cython/gtsam/tests/test_dataset.py @@ -0,0 +1,45 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Unit tests for testing dataset access. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" +# pylint: disable=invalid-name, no-name-in-module, no-member + +from __future__ import print_function + +import unittest + +import gtsam +from gtsam import BetweenFactorPose3, BetweenFactorPose3s +from gtsam.utils.test_case import GtsamTestCase + + +class TestDataset(GtsamTestCase): + """Tests for datasets.h wrapper.""" + + def setUp(self): + """Get some common paths.""" + self.pose3_example_g2o_file = gtsam.findExampleDataFile( + "pose3example.txt") + + def test_readG2o3D(self): + """Test reading directly into factor graph.""" + is3D = True + graph, initial = gtsam.readG2o(self.pose3_example_g2o_file, is3D) + self.assertEqual(graph.size(), 6) + self.assertEqual(initial.size(), 5) + + def test_parse3Dfactors(self): + """Test parsing into data structure.""" + factors = gtsam.parse3DFactors(self.pose3_example_g2o_file) + self.assertEqual(factors.size(), 6) + self.assertIsInstance(factors.at(0), BetweenFactorPose3) + + +if __name__ == '__main__': + unittest.main() diff --git a/cython/gtsam/tests/test_dsf_map.py b/cython/gtsam/tests/test_dsf_map.py new file mode 100644 index 000000000..6eb551034 --- /dev/null +++ b/cython/gtsam/tests/test_dsf_map.py @@ -0,0 +1,38 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Unit tests for Disjoint Set Forest. +Author: Frank Dellaert & Varun Agrawal +""" +# pylint: disable=invalid-name, no-name-in-module, no-member + +from __future__ import print_function + +import unittest + +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestDSFMap(GtsamTestCase): + """Tests for DSFMap.""" + + def test_all(self): + """Test everything in DFSMap.""" + def key(index_pair): + return index_pair.i(), index_pair.j() + + dsf = gtsam.DSFMapIndexPair() + pair1 = gtsam.IndexPair(1, 18) + self.assertEqual(key(dsf.find(pair1)), key(pair1)) + pair2 = gtsam.IndexPair(2, 2) + dsf.merge(pair1, pair2) + self.assertTrue(dsf.find(pair1), dsf.find(pair1)) + + +if __name__ == '__main__': + unittest.main() diff --git a/cython/gtsam/tests/test_initialize_pose3.py b/cython/gtsam/tests/test_initialize_pose3.py new file mode 100644 index 000000000..3aa7e3470 --- /dev/null +++ b/cython/gtsam/tests/test_initialize_pose3.py @@ -0,0 +1,89 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Unit tests for 3D SLAM initialization, using rotation relaxation. +Author: Luca Carlone and Frank Dellaert (Python) +""" +# pylint: disable=invalid-name, E1101, E0611 +import unittest + +import numpy as np + +import gtsam +from gtsam import NonlinearFactorGraph, Point3, Pose3, Rot3, Values +from gtsam.utils.test_case import GtsamTestCase + +x0, x1, x2, x3 = 0, 1, 2, 3 + + +class TestValues(GtsamTestCase): + + def setUp(self): + + model = gtsam.noiseModel_Isotropic.Sigma(6, 0.1) + + # We consider a small graph: + # symbolic FG + # x2 0 1 + # / | \ 1 2 + # / | \ 2 3 + # x3 | x1 2 0 + # \ | / 0 3 + # \ | / + # x0 + # + p0 = Point3(0, 0, 0) + self.R0 = Rot3.Expmap(np.array([0.0, 0.0, 0.0])) + p1 = Point3(1, 2, 0) + self.R1 = Rot3.Expmap(np.array([0.0, 0.0, 1.570796])) + p2 = Point3(0, 2, 0) + self.R2 = Rot3.Expmap(np.array([0.0, 0.0, 3.141593])) + p3 = Point3(-1, 1, 0) + self.R3 = Rot3.Expmap(np.array([0.0, 0.0, 4.712389])) + + pose0 = Pose3(self.R0, p0) + pose1 = Pose3(self.R1, p1) + pose2 = Pose3(self.R2, p2) + pose3 = Pose3(self.R3, p3) + + g = NonlinearFactorGraph() + g.add(gtsam.BetweenFactorPose3(x0, x1, pose0.between(pose1), model)) + g.add(gtsam.BetweenFactorPose3(x1, x2, pose1.between(pose2), model)) + g.add(gtsam.BetweenFactorPose3(x2, x3, pose2.between(pose3), model)) + g.add(gtsam.BetweenFactorPose3(x2, x0, pose2.between(pose0), model)) + g.add(gtsam.BetweenFactorPose3(x0, x3, pose0.between(pose3), model)) + g.add(gtsam.PriorFactorPose3(x0, pose0, model)) + self.graph = g + + def test_buildPose3graph(self): + pose3graph = gtsam.InitializePose3.buildPose3graph(self.graph) + + def test_orientations(self): + pose3Graph = gtsam.InitializePose3.buildPose3graph(self.graph) + + initial = gtsam.InitializePose3.computeOrientationsChordal(pose3Graph) + + # comparison is up to M_PI, that's why we add some multiples of 2*M_PI + self.gtsamAssertEquals(initial.atRot3(x0), self.R0, 1e-6) + self.gtsamAssertEquals(initial.atRot3(x1), self.R1, 1e-6) + self.gtsamAssertEquals(initial.atRot3(x2), self.R2, 1e-6) + self.gtsamAssertEquals(initial.atRot3(x3), self.R3, 1e-6) + + def test_initializePoses(self): + g2oFile = gtsam.findExampleDataFile("pose3example-grid") + is3D = True + inputGraph, expectedValues = gtsam.readG2o(g2oFile, is3D) + priorModel = gtsam.noiseModel_Unit.Create(6) + inputGraph.add(gtsam.PriorFactorPose3(0, Pose3(), priorModel)) + + initial = gtsam.InitializePose3.initialize(inputGraph) + # TODO(frank): very loose !! + self.gtsamAssertEquals(initial, expectedValues, 0.1) + + +if __name__ == "__main__": + unittest.main() diff --git a/cython/gtsam/utils/plot.py b/cython/gtsam/utils/plot.py index 19402a080..3871fa18c 100644 --- a/cython/gtsam/utils/plot.py +++ b/cython/gtsam/utils/plot.py @@ -2,9 +2,10 @@ import numpy as np import matplotlib.pyplot as plt +from matplotlib import patches -def plot_pose2_on_axes(axes, pose, axis_length=0.1): +def plot_pose2_on_axes(axes, pose, axis_length=0.1, covariance=None): """Plot a 2D pose on given axis 'axes' with given 'axis_length'.""" # get rotation and translation (center) gRp = pose.rotation().matrix() # rotation from pose to global @@ -20,13 +21,26 @@ def plot_pose2_on_axes(axes, pose, axis_length=0.1): line = np.append(origin[np.newaxis], y_axis[np.newaxis], axis=0) axes.plot(line[:, 0], line[:, 1], 'g-') + if covariance is not None: + pPp = covariance[0:2, 0:2] + gPp = np.matmul(np.matmul(gRp, pPp), gRp.T) -def plot_pose2(fignum, pose, axis_length=0.1): + w, v = np.linalg.eig(gPp) + + # k = 2.296 + k = 5.0 + + angle = np.arctan2(v[1, 0], v[0, 0]) + e1 = patches.Ellipse(origin, np.sqrt(w[0]*k), np.sqrt(w[1]*k), + np.rad2deg(angle), fill=False) + axes.add_patch(e1) + +def plot_pose2(fignum, pose, axis_length=0.1, covariance=None): """Plot a 2D pose on given figure with given 'axis_length'.""" # get figure object fig = plt.figure(fignum) axes = fig.gca() - plot_pose2_on_axes(axes, pose, axis_length) + plot_pose2_on_axes(axes, pose, axis_length, covariance) def plot_point3_on_axes(axes, point, linespec): diff --git a/cython/gtsam/utils/test_case.py b/cython/gtsam/utils/test_case.py new file mode 100644 index 000000000..7df1e6ee9 --- /dev/null +++ b/cython/gtsam/utils/test_case.py @@ -0,0 +1,27 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +TestCase class with GTSAM assert utils. +Author: Frank Dellaert +""" +import unittest + + +class GtsamTestCase(unittest.TestCase): + """TestCase class with GTSAM assert utils.""" + + def gtsamAssertEquals(self, actual, expected, tol=1e-9): + """ AssertEqual function that prints out actual and expected if not equal. + Usage: + self.gtsamAssertEqual(actual,expected) + Keyword Arguments: + tol {float} -- tolerance passed to 'equals', default 1e-9 + """ + equal = actual.equals(expected, tol) + if not equal: + raise self.failureException( + "Values are not equal:\n{}!={}".format(actual, expected)) diff --git a/cython/gtsam_eigency/CMakeLists.txt b/cython/gtsam_eigency/CMakeLists.txt index 54b7de9aa..4bff567eb 100644 --- a/cython/gtsam_eigency/CMakeLists.txt +++ b/cython/gtsam_eigency/CMakeLists.txt @@ -22,6 +22,17 @@ cythonize(cythonize_eigency_conversions "../gtsam_eigency/conversions.pyx" "conv "${OUTPUT_DIR}" "${EIGENCY_INCLUDE_DIR}" "" "" "") cythonize(cythonize_eigency_core "../gtsam_eigency/core.pyx" "core" ${OUTPUT_DIR} "${EIGENCY_INCLUDE_DIR}" "" "" "") + +# Include Eigen headers: +target_include_directories(cythonize_eigency_conversions PUBLIC + $ + $ +) +target_include_directories(cythonize_eigency_core PUBLIC + $ + $ +) + add_dependencies(cythonize_eigency_core cythonize_eigency_conversions) add_custom_target(cythonize_eigency) add_dependencies(cythonize_eigency cythonize_eigency_conversions cythonize_eigency_core) diff --git a/cython/gtsam_eigency/__init__.py.in b/cython/gtsam_eigency/__init__.py.in index dd278d128..a59d51eab 100644 --- a/cython/gtsam_eigency/__init__.py.in +++ b/cython/gtsam_eigency/__init__.py.in @@ -1,7 +1,7 @@ import os import numpy as np -__eigen_dir__ = "${GTSAM_EIGEN_INCLUDE_PREFIX}" +__eigen_dir__ = "${GTSAM_EIGEN_INCLUDE_FOR_INSTALL}" def get_includes(include_eigen=True): root = os.path.dirname(__file__) diff --git a/cython/gtsam_unstable/__init__.py b/cython/gtsam_unstable/__init__.py new file mode 100644 index 000000000..3195e6da4 --- /dev/null +++ b/cython/gtsam_unstable/__init__.py @@ -0,0 +1 @@ +from .gtsam_unstable import * diff --git a/cython/gtsam_unstable/examples/FixedLagSmootherExample.py b/cython/gtsam_unstable/examples/FixedLagSmootherExample.py new file mode 100644 index 000000000..786701e0f --- /dev/null +++ b/cython/gtsam_unstable/examples/FixedLagSmootherExample.py @@ -0,0 +1,102 @@ +""" +GTSAM Copyright 2010-2018, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved +Authors: Frank Dellaert, et al. (see THANKS for the full author list) + +See LICENSE for the license information + +Demonstration of the fixed-lag smoothers using a planar robot example +and multiple odometry-like sensors +Author: Frank Dellaert (C++), Jeremy Aguilon (Python) +""" + +import numpy as np + +import gtsam +import gtsam_unstable + + +def _timestamp_key_value(key, value): + """ + + """ + return gtsam_unstable.FixedLagSmootherKeyTimestampMapValue( + key, value + ) + + +def BatchFixedLagSmootherExample(): + """ + Runs a batch fixed smoother on an agent with two odometry + sensors that is simply moving to the + """ + + # Define a batch fixed lag smoother, which uses + # Levenberg-Marquardt to perform the nonlinear optimization + lag = 2.0 + smoother_batch = gtsam_unstable.BatchFixedLagSmoother(lag) + + # Create containers to store the factors and linearization points + # that will be sent to the smoothers + new_factors = gtsam.NonlinearFactorGraph() + new_values = gtsam.Values() + new_timestamps = gtsam_unstable.FixedLagSmootherKeyTimestampMap() + + # Create a prior on the first pose, placing it at the origin + prior_mean = gtsam.Pose2(0, 0, 0) + prior_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) + X1 = 0 + new_factors.push_back(gtsam.PriorFactorPose2(X1, prior_mean, prior_noise)) + new_values.insert(X1, prior_mean) + new_timestamps.insert(_timestamp_key_value(X1, 0.0)) + + delta_time = 0.25 + time = 0.25 + + while time <= 3.0: + previous_key = 1000 * (time - delta_time) + current_key = 1000 * time + + # assign current key to the current timestamp + new_timestamps.insert(_timestamp_key_value(current_key, time)) + + # Add a guess for this pose to the new values + # Assume that the robot moves at 2 m/s. Position is time[s] * 2[m/s] + current_pose = gtsam.Pose2(time * 2, 0, 0) + new_values.insert(current_key, current_pose) + + # Add odometry factors from two different sources with different error + # stats + odometry_measurement_1 = gtsam.Pose2(0.61, -0.08, 0.02) + odometry_noise_1 = gtsam.noiseModel_Diagonal.Sigmas( + np.array([0.1, 0.1, 0.05])) + new_factors.push_back(gtsam.BetweenFactorPose2( + previous_key, current_key, odometry_measurement_1, odometry_noise_1 + )) + + odometry_measurement_2 = gtsam.Pose2(0.47, 0.03, 0.01) + odometry_noise_2 = gtsam.noiseModel_Diagonal.Sigmas( + np.array([0.05, 0.05, 0.05])) + new_factors.push_back(gtsam.BetweenFactorPose2( + previous_key, current_key, odometry_measurement_2, odometry_noise_2 + )) + + # Update the smoothers with the new factors. In this case, + # one iteration must pass for Levenberg-Marquardt to accurately + # estimate + if time >= 0.50: + smoother_batch.update(new_factors, new_values, new_timestamps) + print("Timestamp = " + str(time) + ", Key = " + str(current_key)) + print(smoother_batch.calculateEstimatePose2(current_key)) + + new_timestamps.clear() + new_values.clear() + new_factors.resize(0) + + time += delta_time + + +if __name__ == '__main__': + BatchFixedLagSmootherExample() + print("Example complete") diff --git a/cython/gtsam_unstable/examples/__init__.py b/cython/gtsam_unstable/examples/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/cython/gtsam_unstable/tests/__init__.py b/cython/gtsam_unstable/tests/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py b/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py new file mode 100644 index 000000000..8d3af311f --- /dev/null +++ b/cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py @@ -0,0 +1,135 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Cal3Unified unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" +import unittest + +import numpy as np + +import gtsam +import gtsam_unstable +from gtsam.utils.test_case import GtsamTestCase + + +def _timestamp_key_value(key, value): + return gtsam_unstable.FixedLagSmootherKeyTimestampMapValue( + key, value + ) + + +class TestFixedLagSmootherExample(GtsamTestCase): + ''' + Tests the fixed lag smoother wrapper + ''' + + def test_FixedLagSmootherExample(self): + ''' + Simple test that checks for equality between C++ example + file and the Python implementation. See + gtsam_unstable/examples/FixedLagSmootherExample.cpp + ''' + # Define a batch fixed lag smoother, which uses + # Levenberg-Marquardt to perform the nonlinear optimization + lag = 2.0 + smoother_batch = gtsam_unstable.BatchFixedLagSmoother(lag) + + # Create containers to store the factors and linearization points + # that will be sent to the smoothers + new_factors = gtsam.NonlinearFactorGraph() + new_values = gtsam.Values() + new_timestamps = gtsam_unstable.FixedLagSmootherKeyTimestampMap() + + # Create a prior on the first pose, placing it at the origin + prior_mean = gtsam.Pose2(0, 0, 0) + prior_noise = gtsam.noiseModel_Diagonal.Sigmas( + np.array([0.3, 0.3, 0.1])) + X1 = 0 + new_factors.push_back( + gtsam.PriorFactorPose2( + X1, prior_mean, prior_noise)) + new_values.insert(X1, prior_mean) + new_timestamps.insert(_timestamp_key_value(X1, 0.0)) + + delta_time = 0.25 + time = 0.25 + + i = 0 + + ground_truth = [ + gtsam.Pose2(0.995821, 0.0231012, 0.0300001), + gtsam.Pose2(1.49284, 0.0457247, 0.045), + gtsam.Pose2(1.98981, 0.0758879, 0.06), + gtsam.Pose2(2.48627, 0.113502, 0.075), + gtsam.Pose2(2.98211, 0.158558, 0.09), + gtsam.Pose2(3.47722, 0.211047, 0.105), + gtsam.Pose2(3.97149, 0.270956, 0.12), + gtsam.Pose2(4.4648, 0.338272, 0.135), + gtsam.Pose2(4.95705, 0.41298, 0.15), + gtsam.Pose2(5.44812, 0.495063, 0.165), + gtsam.Pose2(5.9379, 0.584503, 0.18), + ] + + # Iterates from 0.25s to 3.0s, adding 0.25s each loop + # In each iteration, the agent moves at a constant speed + # and its two odometers measure the change. The smoothed + # result is then compared to the ground truth + while time <= 3.0: + previous_key = 1000 * (time - delta_time) + current_key = 1000 * time + + # assign current key to the current timestamp + new_timestamps.insert(_timestamp_key_value(current_key, time)) + + # Add a guess for this pose to the new values + # Assume that the robot moves at 2 m/s. Position is time[s] * + # 2[m/s] + current_pose = gtsam.Pose2(time * 2, 0, 0) + new_values.insert(current_key, current_pose) + + # Add odometry factors from two different sources with different + # error stats + odometry_measurement_1 = gtsam.Pose2(0.61, -0.08, 0.02) + odometry_noise_1 = gtsam.noiseModel_Diagonal.Sigmas( + np.array([0.1, 0.1, 0.05])) + new_factors.push_back( + gtsam.BetweenFactorPose2( + previous_key, + current_key, + odometry_measurement_1, + odometry_noise_1)) + + odometry_measurement_2 = gtsam.Pose2(0.47, 0.03, 0.01) + odometry_noise_2 = gtsam.noiseModel_Diagonal.Sigmas( + np.array([0.05, 0.05, 0.05])) + new_factors.push_back( + gtsam.BetweenFactorPose2( + previous_key, + current_key, + odometry_measurement_2, + odometry_noise_2)) + + # Update the smoothers with the new factors. In this case, + # one iteration must pass for Levenberg-Marquardt to accurately + # estimate + if time >= 0.50: + smoother_batch.update(new_factors, new_values, new_timestamps) + + estimate = smoother_batch.calculateEstimatePose2(current_key) + self.assertTrue(estimate.equals(ground_truth[i], 1e-4)) + i += 1 + + new_timestamps.clear() + new_values.clear() + new_factors.resize(0) + + time += delta_time + + +if __name__ == "__main__": + unittest.main() diff --git a/cython/setup.py.in b/cython/setup.py.in new file mode 100644 index 000000000..d6af28762 --- /dev/null +++ b/cython/setup.py.in @@ -0,0 +1,49 @@ +import os +import sys +try: + from setuptools import setup, find_packages +except ImportError: + from distutils.core import setup, find_packages + +if 'SETUP_PY_NO_CHECK' not in os.environ: + script_path = os.path.abspath(os.path.realpath(__file__)) + install_path = os.path.abspath(os.path.realpath(os.path.join('${GTSAM_CYTHON_INSTALL_PATH}', 'setup.py'))) + if script_path != install_path: + print('setup.py is being run from an unexpected location: "{}"'.format(script_path)) + print('please run `make install` and run the script from there') + sys.exit(1) + + +packages = find_packages() + +setup( + name='gtsam', + description='Georgia Tech Smoothing And Mapping library', + url='https://bitbucket.org/gtborg/gtsam', + version='${GTSAM_VERSION_STRING}', # https://www.python.org/dev/peps/pep-0440/ + license='Simplified BSD license', + keywords='slam sam robotics localization mapping optimization', + long_description='''${README_CONTENTS}''', + # https://pypi.org/pypi?%3Aaction=list_classifiers + classifiers=[ + 'Development Status :: 5 - Production/Stable', + 'Intended Audience :: Education', + 'Intended Audience :: Developers', + 'Intended Audience :: Science/Research', + 'Operating System :: MacOS', + 'Operating System :: Microsoft :: Windows', + 'Operating System :: POSIX', + 'License :: OSI Approved :: BSD License', + 'Programming Language :: Python :: 2', + 'Programming Language :: Python :: 3', + ], + + packages=packages, + package_data={package: + [f for f in os.listdir(package.replace('.', os.path.sep)) if os.path.splitext(f)[1] in ('.so', '.dll')] + for package in packages + }, + install_requires=[line.strip() for line in ''' +${CYTHON_INSTALL_REQUIREMENTS} +'''.splitlines() if len(line.strip()) > 0 and not line.strip().startswith('#')] +) diff --git a/examples/Data/HS21.QPS b/examples/Data/HS21.QPS new file mode 100644 index 000000000..c71305c6e --- /dev/null +++ b/examples/Data/HS21.QPS @@ -0,0 +1,20 @@ +NAME HS21 +ROWS + N OBJ.FUNC + G R------1 +COLUMNS + C------1 R------1 0.100000e+02 + C------2 R------1 -.100000e+01 +RHS + RHS OBJ.FUNC 0.100000e+03 + RHS R------1 0.100000e+02 +RANGES +BOUNDS + LO BOUNDS C------1 0.200000e+01 + UP BOUNDS C------1 0.500000e+02 + LO BOUNDS C------2 -.500000e+02 + UP BOUNDS C------2 0.500000e+02 +QUADOBJ + C------1 C------1 0.200000e-01 + C------2 C------2 0.200000e+01 +ENDATA diff --git a/examples/Data/HS268.QPS b/examples/Data/HS268.QPS new file mode 100644 index 000000000..53de89421 --- /dev/null +++ b/examples/Data/HS268.QPS @@ -0,0 +1,55 @@ +NAME HS268 +ROWS + N OBJ.FUNC + G R------1 + G R------2 + G R------3 + G R------4 + G R------5 +COLUMNS + C------1 OBJ.FUNC 0.183400e+05 R------1 -.100000e+01 + C------1 R------2 0.100000e+02 R------3 -.800000e+01 + C------1 R------4 0.800000e+01 R------5 -.400000e+01 + C------2 OBJ.FUNC -.341980e+05 R------1 -.100000e+01 + C------2 R------2 0.100000e+02 R------3 0.100000e+01 + C------2 R------4 -.100000e+01 R------5 -.200000e+01 + C------3 OBJ.FUNC 0.454200e+04 R------1 -.100000e+01 + C------3 R------2 -.300000e+01 R------3 -.200000e+01 + C------3 R------4 0.200000e+01 R------5 0.300000e+01 + C------4 OBJ.FUNC 0.867200e+04 R------1 -.100000e+01 + C------4 R------2 0.500000e+01 R------3 -.500000e+01 + C------4 R------4 0.500000e+01 R------5 -.500000e+01 + C------5 OBJ.FUNC 0.860000e+02 R------1 -.100000e+01 + C------5 R------2 0.400000e+01 R------3 0.300000e+01 + C------5 R------4 -.300000e+01 R------5 0.100000e+01 +RHS + RHS OBJ.FUNC -.144630e+05 + RHS R------1 -.500000e+01 + RHS R------2 0.200000e+02 + RHS R------3 -.400000e+02 + RHS R------4 0.110000e+02 + RHS R------5 -.300000e+02 +RANGES +BOUNDS + FR BOUNDS C------1 + FR BOUNDS C------2 + FR BOUNDS C------3 + FR BOUNDS C------4 + FR BOUNDS C------5 +QUADOBJ + C------1 C------1 0.203940e+05 + C------1 C------2 -.249080e+05 + C------1 C------3 -.202600e+04 + C------1 C------4 0.389600e+04 + C------1 C------5 0.658000e+03 + C------2 C------2 0.418180e+05 + C------2 C------3 -.346600e+04 + C------2 C------4 -.982800e+04 + C------2 C------5 -.372000e+03 + C------3 C------3 0.351000e+04 + C------3 C------4 0.217800e+04 + C------3 C------5 -.348000e+03 + C------4 C------4 0.303000e+04 + C------4 C------5 -.440000e+02 + C------5 C------5 0.540000e+02 +ENDATA diff --git a/examples/Data/HS35.QPS b/examples/Data/HS35.QPS new file mode 100644 index 000000000..1ee230e39 --- /dev/null +++ b/examples/Data/HS35.QPS @@ -0,0 +1,20 @@ +NAME HS35 +ROWS + N OBJ.FUNC + G R------1 +COLUMNS + C------1 OBJ.FUNC -.800000e+01 R------1 -.100000e+01 + C------2 OBJ.FUNC -.600000e+01 R------1 -.100000e+01 + C------3 OBJ.FUNC -.400000e+01 R------1 -.200000e+01 +RHS + RHS OBJ.FUNC -.900000e+01 + RHS R------1 -.300000e+01 +RANGES +BOUNDS +QUADOBJ + C------1 C------1 0.400000e+01 + C------1 C------2 0.200000e+01 + C------1 C------3 0.200000e+01 + C------2 C------2 0.400000e+01 + C------3 C------3 0.200000e+01 +ENDATA diff --git a/examples/Data/HS35MOD.QPS b/examples/Data/HS35MOD.QPS new file mode 100644 index 000000000..2fe75ef96 --- /dev/null +++ b/examples/Data/HS35MOD.QPS @@ -0,0 +1,21 @@ +NAME HS35MOD +ROWS + N OBJ.FUNC + G R------1 +COLUMNS + C------1 OBJ.FUNC -.800000e+01 R------1 -.100000e+01 + C------2 OBJ.FUNC -.600000e+01 R------1 -.100000e+01 + C------3 OBJ.FUNC -.400000e+01 R------1 -.200000e+01 +RHS + RHS OBJ.FUNC -.900000e+01 + RHS R------1 -.300000e+01 +RANGES +BOUNDS + FX BOUNDS C------2 0.500000e+00 +QUADOBJ + C------1 C------1 0.400000e+01 + C------1 C------2 0.200000e+01 + C------1 C------3 0.200000e+01 + C------2 C------2 0.400000e+01 + C------3 C------3 0.200000e+01 +ENDATA diff --git a/examples/Data/HS51.QPS b/examples/Data/HS51.QPS new file mode 100644 index 000000000..46416af03 --- /dev/null +++ b/examples/Data/HS51.QPS @@ -0,0 +1,33 @@ +NAME HS51 +ROWS + N OBJ.FUNC + E R------1 + E R------2 + E R------3 +COLUMNS + C------1 R------1 0.100000e+01 + C------2 OBJ.FUNC -.400000e+01 R------1 0.300000e+01 + C------2 R------3 0.100000e+01 + C------3 OBJ.FUNC -.400000e+01 R------2 0.100000e+01 + C------4 OBJ.FUNC -.200000e+01 R------2 0.100000e+01 + C------5 OBJ.FUNC -.200000e+01 R------2 -.200000e+01 + C------5 R------3 -.100000e+01 +RHS + RHS OBJ.FUNC -.600000e+01 + RHS R------1 0.400000e+01 +RANGES +BOUNDS + FR BOUNDS C------1 + FR BOUNDS C------2 + FR BOUNDS C------3 + FR BOUNDS C------4 + FR BOUNDS C------5 +QUADOBJ + C------1 C------1 0.200000e+01 + C------1 C------2 -.200000e+01 + C------2 C------2 0.400000e+01 + C------2 C------3 0.200000e+01 + C------3 C------3 0.200000e+01 + C------4 C------4 0.200000e+01 + C------5 C------5 0.200000e+01 +ENDATA diff --git a/examples/Data/HS52.QPS b/examples/Data/HS52.QPS new file mode 100644 index 000000000..06add5ac3 --- /dev/null +++ b/examples/Data/HS52.QPS @@ -0,0 +1,32 @@ +NAME HS52 +ROWS + N OBJ.FUNC + E R------1 + E R------2 + E R------3 +COLUMNS + C------1 R------1 0.100000e+01 + C------2 OBJ.FUNC -.400000e+01 R------1 0.300000e+01 + C------2 R------3 0.100000e+01 + C------3 OBJ.FUNC -.400000e+01 R------2 0.100000e+01 + C------4 OBJ.FUNC -.200000e+01 R------2 0.100000e+01 + C------5 OBJ.FUNC -.200000e+01 R------2 -.200000e+01 + C------5 R------3 -.100000e+01 +RHS + RHS OBJ.FUNC -.600000e+01 +RANGES +BOUNDS + FR BOUNDS C------1 + FR BOUNDS C------2 + FR BOUNDS C------3 + FR BOUNDS C------4 + FR BOUNDS C------5 +QUADOBJ + C------1 C------1 0.320000e+02 + C------1 C------2 -.800000e+01 + C------2 C------2 0.400000e+01 + C------2 C------3 0.200000e+01 + C------3 C------3 0.200000e+01 + C------4 C------4 0.200000e+01 + C------5 C------5 0.200000e+01 +ENDATA diff --git a/examples/Data/QPTEST.QPS b/examples/Data/QPTEST.QPS new file mode 100644 index 000000000..8f4f17f05 --- /dev/null +++ b/examples/Data/QPTEST.QPS @@ -0,0 +1,19 @@ +NAME QP example +ROWS + N obj + G r1 + L r2 +COLUMNS + c1 r1 2.0 r2 -1.0 + c1 obj 1.5 + c2 r1 1.0 r2 2.0 + c2 obj -2.0 +RHS + rhs1 r1 2.0 r2 6.0 +BOUNDS + UP bnd1 c1 20.0 +QUADOBJ + c1 c1 8.0 + c1 c2 2.0 + c2 c2 10.0 +ENDATA diff --git a/examples/Data/randomGrid3D.xml b/examples/Data/randomGrid3D.xml new file mode 100644 index 000000000..6a82ce31c --- /dev/null +++ b/examples/Data/randomGrid3D.xml @@ -0,0 +1,3414 @@ + + + + + + + 32 + 1 + + + + + + 2 + 0 + 0 + 1 + + + + + + 9 + 7 + + 3.20776022311033415e+01 + -3.21030367555546334e+01 + -4.10921918858809718e+01 + -2.97297166857905353e+01 + 5.12273135695288744e+01 + 5.32024088136580744e+01 + 7.91786461660617107e+01 + 6.02804880595302208e+01 + -3.20503372222358784e+00 + -4.49785699465720157e+00 + 9.06604158034029126e+01 + 5.65784169718906416e+00 + 3.12765298180226239e+01 + 2.69796747523965372e+01 + 6.45574939874757661e+01 + -3.41086208590283135e+01 + 3.16243899688174857e+01 + -6.07548743284260979e+01 + 5.22759578856884062e+01 + 2.73875651690404389e+01 + -6.70630095670253041e+01 + 8.09592862312814248e+01 + -2.90562646005293850e+01 + 1.97217242898365228e+01 + 1.92063557124931243e+01 + -3.42017680808024593e+01 + 5.18983240742203904e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -0.00000000000000000e+00 + -8.81008786560076906e+00 + 9.93170954106413291e+01 + 7.64832733308121160e+00 + 9.74088247837876793e+01 + 6.98423466470551624e+00 + 2.15114230210294117e+01 + 8.81008786560076906e+00 + -9.93170954106413291e+01 + -7.64832733308121160e+00 + -0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 2.08303433971152465e+01 + 9.34532104006540187e+00 + -9.73589326595991906e+01 + -9.74088247837876793e+01 + -6.98423466470551624e+00 + -2.15114230210294117e+01 + -2.08303433971152465e+01 + -9.34532104006540187e+00 + 9.73589326595991906e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -9.96847734946016431e+01 + -9.41956956915240795e+00 + 3.58611803229205250e+01 + 1.37202268725982691e+02 + 8.31655615217660085e+01 + -2.96050013164699770e+01 + 5.56978428429618404e+01 + -1.64096733189184050e+02 + -6.76911341847846018e+01 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 1 + 2 + + + + + + 9 + 7 + + -5.23677913323444741e+01 + 2.23054746283748031e+01 + -9.06323879014932388e+00 + 5.52805622243718773e+01 + 7.94539068269289146e+01 + 1.94541508392102571e+01 + -6.13821741431047201e+01 + 5.56974973159738553e+01 + -7.78665862090978944e+00 + -3.50785425178010168e+00 + -8.59643527732278869e+01 + 4.37949256925254957e+01 + 1.46448346762102268e+01 + -9.84525875940883211e+00 + -6.31187688571179351e+01 + -1.68743787040301001e+01 + 4.96429783126596220e+01 + 6.02933235638437850e+01 + -8.50324227920073668e+01 + -8.14408668091479448e+00 + 7.80614049299559465e+00 + -3.12168328896096590e+01 + -8.16862254520412101e+00 + 7.01147846710003364e+01 + 4.14413157080187986e+01 + -1.74987783157454646e+00 + 7.04591203724149580e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -0.00000000000000000e+00 + -2.60374020501451531e+00 + -5.42127786775135831e+01 + 8.39892562474970532e+01 + -9.97226081614345645e+01 + 7.26909685044996312e+00 + 1.60051631037066500e+00 + 2.60374020501451531e+00 + 5.42127786775135831e+01 + -8.39892562474970532e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -6.97294474564191535e+00 + -8.37146036187337330e+01 + -5.42516652512392241e+01 + 9.97226081614345645e+01 + -7.26909685044996312e+00 + -1.60051631037066500e+00 + 6.97294474564191535e+00 + 8.37146036187337330e+01 + 5.42516652512392241e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.08200476037924709e+01 + 1.28950357649989712e+02 + 1.43352339174516089e+02 + -2.38602247867744630e+01 + -5.20893463997723600e+01 + 2.84608516229487805e+01 + 6.76429765595175212e+01 + 1.20773294728176111e+02 + -1.20589752280510609e+02 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 2 + 3 + + + + + + 9 + 7 + + -1.96993691143317093e+01 + -2.72709220997507202e+01 + 4.46131383257576886e+01 + 9.30649268771392428e+01 + 3.22308546546604946e+00 + -1.69350730671129561e+01 + -3.00372292195983448e+01 + 4.73051200126789553e+01 + -6.91348627543176519e+01 + 8.77985890195871921e-01 + 2.95766095687473936e+01 + -7.96244285285575017e+01 + 7.31430977105361357e+00 + 9.52212779955403192e+01 + 2.32608409272044945e+01 + -1.06369613385613393e+00 + -2.30114188850419632e+00 + -5.58240668503478901e+01 + 8.63825257313862096e+01 + 3.81456041339674954e+01 + 2.72998827723357493e+01 + 3.09037953339841458e+01 + -1.77447464898715950e+01 + -1.04957062263738177e+01 + 3.97018822008379715e+01 + -7.27393475106917862e+01 + -4.57204758371371796e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -4.59114589009974594e+01 + 8.09444187944725400e+01 + 3.66079090854293696e+01 + -7.62990082722604797e+01 + -1.48208463505055157e+01 + -6.29190261377608593e+01 + 4.59114589009974594e+01 + -8.09444187944725400e+01 + -3.66079090854293696e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -4.55038380606685564e+01 + -5.68185144075392898e+01 + 6.85642555729200751e+01 + 7.62990082722604797e+01 + 1.48208463505055157e+01 + 6.29190261377608593e+01 + 4.55038380606685564e+01 + 5.68185144075392898e+01 + -6.85642555729200751e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -8.55922050845758786e-01 + 1.40043738778271234e+02 + -3.81613152068798769e+01 + -5.81227180496655009e+01 + -3.94713178138164338e+01 + 3.22753989340650662e+01 + 1.32631681079235051e+02 + -3.12863815406522541e+01 + -4.03072840384836510e+01 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 3 + 4 + + + + + + 9 + 7 + + 8.66048172555662319e+01 + -1.52328612569721304e+01 + 4.48534445123967629e+01 + 2.02981055828854124e+01 + -4.93746572823205057e+01 + -2.74451016941264996e+01 + 4.12224960002507945e+00 + -6.40449795031895519e+01 + -5.03376341440287618e+01 + 4.92248012617162161e+01 + 3.93527186553859920e+01 + -6.89623533878909143e+01 + -4.08605085624496382e+01 + 5.95700970246627293e+01 + 3.55176671176527492e+01 + 9.26276585311693879e+00 + -6.84332455006064038e+01 + 4.88721436512163976e+00 + -4.03054194818879719e+00 + -1.78980733327891492e+01 + 3.45198923956961394e+01 + -8.80228615455582002e+01 + -4.55655125601066899e+01 + -9.64228954249413128e+00 + 1.13078887631363152e+01 + -3.24259783798813004e+01 + 8.58793556515557270e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -0.00000000000000000e+00 + -8.08548316492842929e+01 + 5.81668844249688917e+01 + 8.89436648998715995e+00 + 4.55868302729299728e+01 + 5.23625800953892124e+01 + 7.19722245829668879e+01 + 8.08548316492842929e+01 + -5.81668844249688917e+01 + -8.89436648998715995e+00 + -0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 3.72066809139564114e+01 + 6.22476807764455131e+01 + -6.88541148612105047e+01 + -4.55868302729299728e+01 + -5.23625800953892124e+01 + -7.19722245829668879e+01 + -3.72066809139564114e+01 + -6.22476807764455131e+01 + 6.88541148612105047e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -4.49706199136485765e+01 + 2.66285359051983370e+01 + 1.14028652024075640e+02 + 5.86398529753103119e+01 + 8.34746189227890056e+00 + 1.60810773504427289e+02 + 1.79694799386287997e+02 + -4.53728709677613509e+01 + -1.70780906392258842e+01 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 4 + 5 + + + + + + 9 + 7 + + 6.43003424386522084e+01 + 1.19796180071396279e+01 + 4.55761542738693137e+01 + -6.52039759607597773e+01 + 5.89636417155482349e+01 + 1.80719252805888360e+01 + -1.51516354244859457e+01 + -5.00500232613797280e+01 + -5.34352949392747476e+01 + 7.42583468150113362e+01 + 7.45315542640838657e+00 + -5.24573072801267415e+01 + 4.09274245687232323e+01 + -1.68412174720425938e+01 + -1.36705786744805522e+01 + 2.70644906205976383e+01 + -8.31804320954470597e+01 + 4.33744783589924054e+01 + -1.22788002717957223e+01 + -6.11064170046755617e+00 + -7.16592171005377452e+01 + -5.30800076837930419e+01 + -7.82610172559931669e+01 + 2.92797087670289500e+01 + -2.22315273959740871e+01 + -2.13187310855278938e+01 + -6.26781275987031279e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 6.86610695050494400e+01 + 1.26119199040840346e+01 + 7.16002584545316836e+01 + -7.26277336162735736e+01 + 1.63646703530006263e+01 + 6.67638365734117087e+01 + -6.86610695050494400e+01 + -1.26119199040840346e+01 + -7.16002584545316836e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -3.29694467444833483e+00 + -9.78424092128285139e+01 + 2.03959092771819215e+01 + 7.26277336162735736e+01 + -1.63646703530006263e+01 + -6.67638365734117087e+01 + 3.29694467444833483e+00 + 9.78424092128285139e+01 + -2.03959092771819215e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.74531092467603024e+01 + -9.68528064078233442e-01 + -1.43955992851650620e+01 + -3.71890171910416072e+01 + 2.70827825121624883e+01 + 1.59657441575989878e+02 + -1.61093211332558155e+02 + -2.36356649401689936e+01 + -3.50656643781457547e+01 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 5 + 6 + + + + + + 9 + 7 + + -6.40637543398504619e+01 + -1.35919926214779476e+01 + -7.55586787939104454e+01 + 7.49591051894634859e+01 + -1.51203828101076443e+01 + -6.04179319959996945e+01 + 1.63126149967003826e+01 + -3.67352169146467711e+00 + -1.49855747686980401e+01 + -7.64033153697704415e-01 + 5.83204655642280656e+00 + -2.26064773215979020e+00 + -1.21801830224353154e+01 + 9.23282518419687364e+01 + -3.58808093806811215e+01 + 6.76481543691339766e+01 + 3.42590164955414807e+01 + 6.51923982648987277e+01 + -2.26801968652302843e+00 + -9.78783400225560598e+01 + 1.94155506146273105e+01 + 1.64832141235940597e+01 + 2.49817026599325320e+00 + -1.62466150125265507e+01 + -7.07730069025061255e+01 + 1.59540577889256383e+01 + 6.50575730811547004e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -0.00000000000000000e+00 + -2.60428350122129437e+01 + -6.97694761102178944e+01 + 6.67382270354284231e+01 + -9.26426416957655618e+01 + 3.75218214997053465e+01 + 3.07471152699694139e+00 + 2.60428350122129437e+01 + 6.97694761102178944e+01 + -6.67382270354284231e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -2.71866085445878198e+01 + -6.10273144964611944e+01 + -7.44080318325472234e+01 + 9.26426416957655618e+01 + -3.75218214997053465e+01 + -3.07471152699694139e+00 + 2.71866085445878198e+01 + 6.10273144964611944e+01 + 7.44080318325472234e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.03933661487596595e+02 + 4.68471191704998802e+01 + 1.18875204735324314e+01 + -2.97055153316788463e+01 + 7.27443939906840598e+01 + 7.23444094058463492e+01 + 3.82415703375670404e+01 + -2.27415768204949451e+01 + -3.07813391103898937e+01 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 6 + 7 + + + + + + 9 + 7 + + 1.85630374271583953e+01 + -5.29313735130414926e+01 + 3.66303674355102942e+01 + 1.04645716047590440e+01 + 5.73485858047936077e+01 + -5.08591115829850366e+01 + 9.38445241664948213e+01 + -1.36042785531324792e+01 + -2.31303222840427054e+01 + -9.56612084104992455e+00 + -2.98701442261074703e+01 + 7.62840716168778670e+01 + -1.92910486636706899e+01 + -5.04699434611332478e+01 + 3.46538201809487560e+01 + 3.08818026153629788e+01 + 7.17821170583860777e+01 + 5.45013702935509130e+01 + -5.34080385938858839e+01 + 6.23631140795577465e+01 + 4.43545654716690905e+01 + -7.95485560661077074e+01 + -2.30201810552558968e+01 + -5.51074857191607634e+01 + 1.18903119380668851e+01 + -2.67045605220385767e+01 + -2.32776001751549053e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.79872266803328138e+01 + 8.64209977399809475e+01 + -4.69879859748907549e+01 + 9.82455450157955568e+01 + -1.33902787243874393e+01 + 1.29812680517931049e+01 + -1.79872266803328138e+01 + -8.64209977399809475e+01 + 4.69879859748907549e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 4.92671908064704667e+00 + -4.84985730234346519e+01 + -8.73133200250165942e+01 + -9.82455450157955568e+01 + 1.33902787243874393e+01 + -1.29812680517931049e+01 + -4.92671908064704667e+00 + 4.84985730234346519e+01 + 8.73133200250165942e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -8.68503697620646733e+01 + -6.63265206223623283e-01 + 5.77899517416623070e+01 + 1.54728954524297080e+02 + -7.36729123490597857e+01 + -4.33714485898333280e+01 + -8.08065933920789270e+00 + -1.49263004123200204e+02 + 1.24142002075972883e+02 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 7 + 8 + + + + + + 9 + 7 + + 4.74062871911341759e+01 + 5.71222009314007693e+01 + -2.90538462259461063e+01 + 4.09276699085499303e+01 + 3.99974137866047528e+01 + -1.99073676332006535e+01 + -7.78029399881401531e+01 + 5.27745236895511312e+01 + -3.37039333639074385e+01 + -9.12848490867447815e+00 + -6.92290536396862421e+01 + -2.32360574632871630e+00 + 1.58993825041621264e+01 + 5.31010116114126660e+01 + -6.55564733827918502e+01 + -3.41955054720470475e+00 + 4.69927897084473187e+01 + 7.43534572740371971e+01 + -6.28440645552295862e+01 + 4.27829075558255525e+01 + 5.46299457034644860e+01 + 7.56316908352858093e+01 + 1.42185369226067397e+01 + 5.51031175481571864e+01 + 2.63284733063378651e+00 + 2.23353419791525312e+01 + 4.20873574259948739e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.80585028832817862e+01 + -9.52829540657210430e+01 + -2.43936290478516966e+01 + -9.74947022342857252e+01 + -2.06160114466111395e+01 + 8.35243127964282905e+00 + -1.80585028832817862e+01 + 9.52829540657210430e+01 + 2.43936290478516966e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -1.29874366163019737e+01 + 2.22741719608807998e+01 + -9.66187753679099757e+01 + 9.74947022342857252e+01 + 2.06160114466111395e+01 + -8.35243127964282905e+00 + 1.29874366163019737e+01 + -2.22741719608807998e+01 + 9.66187753679099757e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 7.39787443001634273e+01 + -3.29520218737291373e+01 + 1.75142945360573606e+02 + -4.89974435425370416e+01 + -9.39530685963477481e+01 + -3.92888663032043226e+01 + 4.46158732311312889e+01 + 1.62422063134678211e+02 + -1.51571321993876627e+01 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 8 + 9 + + + + + + 9 + 7 + + 9.41130157063151671e+01 + 1.30096002577236085e+01 + -9.65142385029295902e+00 + -2.97023439769390656e+01 + 3.10157410489098062e-01 + 4.06407354840771795e+00 + -9.58311576289570155e+00 + 9.66152903465384156e+01 + 2.35615250878309794e+01 + 1.77277266667175297e+01 + -4.94856229862016690e+01 + 8.47852845508831336e+01 + 1.34310978254269742e+01 + -8.43883523083151488e+01 + -5.15167787166412623e+01 + 3.41073399110957187e-01 + 2.18139939555895035e+00 + 9.36548913434189423e+00 + -2.80240621096084510e+01 + -7.66577929709818839e+00 + 9.19688865464188510e+00 + -9.38736839860108603e+01 + -1.81862826844923511e+01 + 1.54239226370280336e+00 + -8.73344590429244683e+00 + 2.30576115914954443e+01 + -9.65297250180462356e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -9.99929178544290664e+01 + 6.62091181731783163e-01 + -9.88946016968861308e-01 + -6.23564336177379053e-01 + 4.16306145486879302e+01 + 9.09203118110426516e+01 + 9.99929178544290664e+01 + -6.62091181731783163e-01 + 9.88946016968861308e-01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.01367967132287018e+00 + 9.09200394168724984e+01 + -4.16235376434812636e+01 + 6.23564336177379053e-01 + -4.16306145486879302e+01 + -9.09203118110426516e+01 + -1.01367967132287018e+00 + -9.09200394168724984e+01 + 4.16235376434812636e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 5.55282235936336566e+00 + -1.76495493086246057e+02 + -9.69626169859593645e+00 + -1.18086675146736617e+01 + 9.21060305839002780e+01 + 5.32168712759193419e+00 + 1.99148216329777483e+02 + 1.06989143861811957e+01 + -5.26829527913709494e+00 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 9 + 10 + + + + + + 9 + 7 + + -8.63087793249897430e+01 + -1.92823330270961684e+01 + -4.42211315553459627e+01 + 2.21505354932965801e+01 + -2.63320996942306031e+01 + -5.69788664986630025e+01 + -4.53775288609686314e+01 + 2.58526272351369890e+01 + 5.53658756361871127e+01 + -1.54315640909267664e+01 + -4.32654755327350813e+01 + 1.97045712499729326e+01 + 7.39853470327932428e+01 + -5.76879953686396973e+01 + 2.69216116215563304e+01 + 6.54801163688819514e+01 + 5.53812651216026666e+01 + -2.49089008945160231e+01 + -9.71189061792836705e-01 + -7.25245368327360609e+01 + 4.96429238183381116e+01 + 6.85930292060070612e-01 + -1.12841816293779988e+01 + -7.69094846357958062e+01 + -5.16757922033419734e-02 + -6.79147932987382035e+01 + -4.02434226181160639e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 7.83449748282976799e+01 + 6.13201754047114846e+01 + 1.00946028891096997e+01 + -1.98027611608243852e+01 + 9.23644092983748521e+00 + 9.75834966034573341e+01 + -7.83449748282976799e+01 + -6.13201754047114846e+01 + -1.00946028891096997e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 5.89059892503364324e+01 + -7.84507759508154265e+01 + 1.93793752003053505e+01 + 1.98027611608243852e+01 + -9.23644092983748521e+00 + -9.75834966034573341e+01 + -5.89059892503364324e+01 + 7.84507759508154265e+01 + -1.93793752003053505e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -1.06986312175636783e+02 + 1.28415573927117009e+02 + 5.26747761738054123e+01 + -8.33248123208498015e+01 + -6.72578872464476092e+01 + 1.08240234163658471e+02 + -1.79030185090507175e+01 + -1.01966657528716965e+02 + 5.84227812301104876e+01 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 10 + 11 + + + + + + 9 + 7 + + 1.17302291731448682e+00 + 2.23736436785092039e+01 + 8.70065012231049764e+01 + 7.67707833816357947e+01 + 5.73162887867323292e+01 + -1.32881532104113664e+00 + -2.51982778355239070e+01 + -7.72901736529863381e+00 + 4.53049731782644329e+01 + 8.54937155952362531e+01 + -4.11223504877864485e+01 + 2.12416634370425328e+01 + -2.90259802009985357e+01 + -8.76138896821732693e+00 + -5.06100435935214055e+00 + -3.81597643193563130e+01 + -9.02600433698408864e+01 + 7.99447271884073940e-01 + -2.24916749275050378e+01 + 2.24299023473424732e+01 + 3.83080111263447165e+01 + -5.03856743318980023e+01 + 7.20086537445330919e+01 + -4.63731853719149143e+01 + 2.85914723565250490e+01 + -2.34837711903294171e+01 + -7.92472197577816644e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -2.63903093070078647e+01 + -2.67183596992454575e+01 + 9.26805310173727577e+01 + -8.93547271763020774e+01 + 4.29554589851731663e+01 + -1.30599109729635039e+01 + 2.63903093070078647e+01 + 2.67183596992454575e+01 + -9.26805310173727577e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -3.63219534982506076e+01 + -8.62609865371063478e+01 + -3.52101959056759028e+01 + 8.93547271763020774e+01 + -4.29554589851731663e+01 + 1.30599109729635039e+01 + 3.63219534982506076e+01 + 8.62609865371063478e+01 + 3.52101959056759028e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -1.04067559180773088e+01 + 7.90108728350369383e-01 + 5.78189525430708571e+01 + -1.16280923178694167e+02 + 8.10714898076704600e+01 + 7.53829806570444134e+01 + -5.78201683295110769e+01 + 6.19589188064910701e+01 + -1.33505720595716895e+02 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 11 + 12 + + + + + + 9 + 7 + + 3.68344254692879192e+00 + 4.86691203808377892e+00 + -1.57231292941544805e+01 + 3.72290369400325147e+01 + 2.06331817836064921e+01 + -8.89534944072540839e+01 + -8.53296212948073389e+01 + 4.59306709423131920e+01 + -2.45010409318042690e+01 + 2.57025147685637876e+01 + 2.56000347344376031e+01 + -9.16529948262070491e+01 + 2.63288801817711970e-01 + 6.59382843634221683e+00 + 1.98864164108950128e+01 + -3.68109153857383404e+01 + -8.63480489179755466e+01 + -3.21511205713796500e+01 + 8.19562719339757138e+01 + -5.68704775649636858e+01 + 6.94175446640136773e+00 + -5.03482257069523769e+01 + -7.72158009035328661e+01 + -3.65965187643956114e+01 + -7.19547325119217618e+00 + -9.46558534641098959e+00 + -4.77765582369805042e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -7.29991262018860425e+01 + 6.43134564832916169e+01 + -2.31280541536218962e+01 + -5.24720785637083011e+01 + -7.44213220616246360e+01 + -4.13297446617319864e+01 + 7.29991262018860425e+01 + -6.43134564832916169e+01 + 2.31280541536218962e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -4.37927910159324512e+01 + -1.80345817187895037e+01 + 8.80735222258265793e+01 + 5.24720785637083011e+01 + 7.44213220616246360e+01 + 4.13297446617319864e+01 + 4.37927910159324512e+01 + 1.80345817187895037e+01 + -8.80735222258265793e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 9.48709591330575535e+01 + 9.60518679589807789e+01 + -5.19581436253384297e+01 + 2.54957713470891250e+01 + -1.34158356561916207e+02 + -2.25547332522903439e+01 + 1.09221138500000791e+02 + -4.57450052559468858e+01 + -6.82129290695598485e+01 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 12 + 13 + + + + + + 9 + 7 + + 1.93941690967042213e+01 + -3.90998618366849584e+01 + 1.63698620253740224e+00 + 1.66123034210985914e+01 + 8.85555525420248557e+01 + 2.63934202043697148e+01 + -8.61979615512473032e+01 + 1.74318161103396001e+01 + -3.92907393347052789e+01 + -7.36585675537442341e+01 + 5.45643959900256945e+01 + -4.73596788713465688e+00 + 1.23230795012408407e+01 + 3.75671217770436385e+01 + -8.32914041112130690e+01 + -4.08563087068878872e+01 + 8.53843323058891990e+00 + 3.64960868565984242e+01 + -6.45808132649689668e+01 + -7.41043072657268738e+01 + -2.22937919750649804e+00 + -1.10797951271104100e+00 + -1.86389067743133623e+01 + -4.83331185591061043e+01 + 2.23431972515867372e+01 + -4.99220528272985842e+00 + -8.44039814228390526e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -0.00000000000000000e+00 + 5.32207681440972067e+01 + -3.06680603945050478e+01 + 7.89114688102511224e+01 + -5.56684880542022569e+01 + -8.29014582098623265e+01 + 5.32613034365356164e+00 + -5.32207681440972067e+01 + 3.06680603945050478e+01 + -7.89114688102511224e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 6.37853374680371630e+01 + -4.67634290692784660e+01 + -6.11932383991170568e+01 + 5.56684880542022569e+01 + 8.29014582098623265e+01 + -5.32613034365356164e+00 + -6.37853374680371630e+01 + 4.67634290692784660e+01 + 6.11932383991170568e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -6.90389218165145877e+01 + 4.83379444724982577e+01 + 1.61042728694415416e+02 + -1.53499694264126390e+02 + -6.29210660700233788e+01 + -1.36328982272427429e-01 + -7.32570951739669738e+01 + -6.73028752573054874e+01 + -7.84207879822722163e+01 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 13 + 14 + + + + + + 9 + 7 + + 6.06804300720567937e+01 + 6.78223389316968621e+01 + 1.14214673416742691e+01 + 2.85045083366314387e+01 + 2.67345752695483867e+01 + 9.28156907783291274e+00 + -3.79052729172203868e+01 + 4.99857449838935750e+01 + -7.77096553357260120e+01 + -7.82277784247720263e+01 + 5.40246939742984793e+01 + 2.32633737948870305e+01 + 2.47956201341556586e+01 + -1.05317240685613065e+01 + 9.52360095902563302e+01 + -1.29078256939381077e+01 + 9.88755369559818398e+00 + 1.89881719971782701e+01 + 9.09983932556493968e+00 + 4.26215664457260388e+01 + 1.04271324229186870e+01 + -6.00988012459201499e+01 + -6.92347566083080466e+01 + 1.36255057217382767e+01 + 5.89311254734215737e+01 + -4.94916274785966195e+01 + -5.89796648826666967e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -2.89030917873941853e+01 + 8.70749520313458305e+01 + -3.97814531392241904e+01 + -9.56630827209552450e+01 + -2.78467078053438328e+01 + 8.55192777842975183e+00 + 2.89030917873941853e+01 + -8.70749520313458305e+01 + 3.97814531392241904e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -3.63123800557649812e+00 + 4.05279359595652906e+01 + 9.13471429077244466e+01 + 9.56630827209552450e+01 + 2.78467078053438328e+01 + -8.55192777842975183e+00 + 3.63123800557649812e+00 + -4.05279359595652906e+01 + -9.13471429077244466e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.43780450624784670e+01 + -6.63129258801445189e+01 + 4.67182022743824987e+00 + -2.52301003257763057e+01 + -9.40350333946463337e+01 + -1.71054540098886392e+01 + 9.90721820600476235e+01 + -1.66885836907954435e+01 + 5.08294565998758543e+01 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 14 + 15 + + + + + + 9 + 7 + + -2.76355843137293995e+01 + -7.18186699571359952e+01 + 3.07651775764515065e+01 + -6.40640399324160086e+01 + -2.68919032866484322e+01 + 9.38443754617159698e+00 + -7.15464009647350991e+01 + 4.97633461308791567e+01 + -2.49225935590679519e+01 + 1.43932895637872207e+01 + -2.88438285303894943e+01 + 6.70843814351506182e+01 + -2.31361564910724091e+01 + 9.02877315480068603e+01 + 3.51897296575981642e+01 + 1.03017517985169054e+01 + -1.55151545070516619e+01 + 6.47165615401468699e+01 + 5.11107200952757168e+01 + 4.26513944039867638e+01 + 5.62224381230135037e+01 + -6.85270728206629229e+01 + -1.63576632701424352e+01 + 1.39839419419296167e+01 + 4.30853333607984084e+01 + -1.81999495152501289e+01 + -7.11401633803200042e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -8.15059339771809164e+01 + -5.54598536753216109e+01 + -1.67626774955357369e+01 + -1.04808841952049807e+01 + 4.25686446848321935e+01 + -8.98780371179923492e+01 + 8.15059339771809164e+01 + 5.54598536753216109e+01 + 1.67626774955357369e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 5.69818724946287603e+01 + -7.14990567770542356e+01 + -4.05086544703406517e+01 + 1.04808841952049807e+01 + -4.25686446848321935e+01 + 8.98780371179923492e+01 + -5.69818724946287603e+01 + 7.14990567770542356e+01 + 4.05086544703406517e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -0.00000000000000000e+00 + 2.31230716791378725e+01 + 2.46912491883496621e+01 + 3.19609408881894597e+00 + 1.52981265769411010e+01 + 1.32876273309615431e+01 + 2.19836964146123970e+00 + 2.74814898754146313e+01 + -2.79167095337952915e+01 + 5.37372498127196874e+00 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 15 + 16 + + + + + + 9 + 7 + + -5.10421334268497020e+01 + -3.97471595810313332e+01 + -1.64389363316277826e+00 + 2.54089618298921245e+01 + -3.15327493370988705e+01 + 9.13980507822707153e+01 + -5.91789668556331776e+01 + -4.81012757100621684e+01 + 1.65563384387473489e+00 + -6.78890977332195860e+01 + -3.50650650222022904e+01 + -8.69105008709039062e+00 + -5.80472933502369273e+01 + 6.88255140605843536e+01 + 3.93666342323419940e+01 + 4.37233513120340049e+01 + 4.71317258377345070e+01 + 1.72241668652535651e+01 + -4.44141172746378885e+01 + 7.81566561420669927e+01 + -4.26269969902096406e+01 + 1.02994128500864690e+01 + -1.21754201166700025e+01 + -9.79654030213998794e+00 + -3.57503904294827279e+01 + 2.58360247551576450e+01 + 8.83514468333149239e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -8.11187605223852870e+01 + -5.83599337897738124e+01 + 3.72354929672284474e+00 + 4.13366904132922599e+01 + -5.27200270517053724e+01 + 7.42420148793328991e+01 + 8.11187605223852870e+01 + 5.83599337897738124e+01 + -3.72354929672284474e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -4.13645345312568224e+01 + 6.17633943021323262e+01 + 6.68898976474907556e+01 + -4.13366904132922599e+01 + 5.27200270517053724e+01 + -7.42420148793328991e+01 + 4.13645345312568224e+01 + -6.17633943021323262e+01 + -6.68898976474907556e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 6.98804353612945732e+01 + -9.46593060564561881e+01 + -1.56916014852066439e+02 + -3.53367495425767473e+01 + -1.16911080827186709e+02 + 7.34112761651981316e+01 + 2.36033204137547230e+01 + 1.27622949564988858e+02 + -4.72506285651327431e+01 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 16 + 17 + + + + + + 9 + 7 + + -5.85435867217505290e+01 + 2.33746012476837990e+01 + -5.77863483723881544e+01 + -5.63329646716383934e+01 + -7.47638541726440593e+01 + 3.41977282497739097e+01 + -4.10879444630993547e+01 + 7.01966859196727366e+00 + -3.18903501380708931e+01 + 7.49027535040175820e+01 + -1.88037812404022127e+01 + -5.50573390958028739e+01 + -1.05105126900153234e+01 + 3.04370847103721403e+01 + 2.74657650627104459e+01 + -5.06989372188159564e+01 + -7.70752893724180126e+01 + -2.65036593071036783e+01 + -8.31624785461406368e+00 + 7.75118867469858053e+00 + -5.96862946091213260e+01 + 3.40815941189988152e+01 + -5.16283223754141716e+01 + -6.67021661737265248e+01 + 4.67870803895238083e+01 + -6.21976850537821235e+01 + 4.44353424269327562e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -9.98522291990096846e+01 + -2.17344032867269910e+00 + 4.98081129196222250e+00 + -1.69510675143138556e+00 + -7.46243396331473150e+01 + -6.65457327513788073e+01 + 9.98522291990096846e+01 + 2.17344032867269910e+00 + -4.98081129196222250e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 5.16322932762926534e+00 + -6.65318276575533361e+01 + 7.44772245149880234e+01 + 1.69510675143138556e+00 + 7.46243396331473150e+01 + 6.65457327513788073e+01 + -5.16322932762926534e+00 + 6.65318276575533361e+01 + -7.44772245149880234e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -3.50474015754750923e+01 + -2.85468905697825797e+01 + -8.26608268609589487e+01 + 7.28338697761986396e+01 + -1.03233140690458100e+02 + -6.32144076335772187e+00 + 4.02504937228882511e+01 + 1.40718088483059116e+01 + 7.44299004116042227e+01 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 17 + 18 + + + + + + 9 + 7 + + -8.82393493416751795e+01 + -6.12481728289156635e+00 + 6.45898243198292366e-01 + 3.86841248560220095e+01 + -4.52281770219861770e+01 + -4.30849606407744687e+01 + -2.62811256127213326e+01 + -5.90797411700203270e+01 + -5.09550094510600857e+01 + 4.68685179819303457e+01 + -2.73625956906887646e+00 + 1.87456681275038339e+00 + 7.09242852066529537e+01 + -3.96201491012311280e+01 + 4.26233675831482230e+01 + -5.26328438238840803e+01 + -5.34225198350244597e+01 + 6.12486839444391720e+01 + -3.76709521788862034e+00 + 9.35400534400023815e+01 + -3.47030072986988287e+01 + 3.20432748703922687e+00 + 2.41487264929615755e+01 + 7.47759826419143678e+01 + -2.25246616168526836e+00 + -2.57366654666450962e+01 + -5.63865719112063317e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 9.03046090553630876e+01 + 2.33581936525576914e+01 + -3.60481951371722431e+01 + 1.31196185024049097e+01 + -9.49105504326207523e+01 + -2.86332503730943557e+01 + -9.03046090553630876e+01 + -2.33581936525576914e+01 + 3.60481951371722431e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -4.09017504968844747e+01 + 2.11277591302666750e+01 + -8.87731074167399186e+01 + -1.31196185024049097e+01 + 9.49105504326207523e+01 + 2.86332503730943557e+01 + 4.09017504968844747e+01 + -2.11277591302666750e+01 + 8.87731074167399186e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -0.00000000000000000e+00 + 4.26251548517658989e+01 + -5.58446106332349572e+01 + -4.99134201995332649e+00 + 7.19670254876823492e+01 + -1.87430011171921045e+01 + -5.57531304221817550e+01 + -9.47135556308831283e+00 + -7.80687492596315167e+01 + 5.77908380638228465e+01 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 18 + 19 + + + + + + 9 + 7 + + 6.21423423624779545e+01 + 1.02374321697767794e+01 + -2.61952819865811541e+01 + 1.64810134236028816e+01 + -9.33026901138422886e+01 + -2.97993293115291316e+01 + 6.47591857034336442e+01 + 2.72657426895055011e+01 + -2.33482872492159856e+01 + -6.84639855508772825e+01 + -1.58264297171629540e+01 + 2.10899487969025330e+01 + -3.01026938649666285e+01 + 2.36828763275138563e+01 + -9.22944706954510821e+01 + 6.50721593326321113e+01 + 1.34399733570446998e+01 + -1.47336875153178202e+01 + 1.81996160352434408e+01 + 5.50471142576061965e+01 + 8.12609439867173649e+01 + 3.09138639218085887e+00 + -1.18364488818499112e+01 + 7.45772273447166401e-02 + 3.87852402909780736e+01 + -7.92723675773540748e+01 + 4.57949875755143552e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -7.14353163221378225e+01 + -6.97621833385175307e+01 + 5.49848686450887225e+00 + 4.54649938592219129e+01 + -4.02948505639650421e+01 + 7.94308463470498509e+01 + 7.14353163221378225e+01 + 6.97621833385175307e+01 + -5.49848686450887225e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -5.31970855906319784e+01 + 5.92415630606653920e+01 + 6.05021263328172836e+01 + -4.54649938592219129e+01 + 4.02948505639650421e+01 + -7.94308463470498509e+01 + 5.31970855906319784e+01 + -5.92415630606653920e+01 + -6.05021263328172836e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 8.66602846584249278e+01 + -1.40571348898168708e+02 + -1.29029667681538402e+01 + -4.84107069228954998e+01 + -6.46593270163667029e+01 + 1.03797317950936275e+02 + 6.32154043923236131e+01 + 1.69243370521808174e+01 + -9.00005277714220426e+01 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 19 + 20 + + + + + + 9 + 7 + + 5.40325063921370585e+01 + 5.12983919091620777e+00 + 4.24856501928409855e+01 + -6.01366381168498805e+01 + -6.66348312341534665e+01 + 1.23707663998909716e+01 + 2.51821654645419386e+01 + -4.49879933093015580e+01 + 6.61923323512717872e+01 + -8.16765559697753787e+01 + -1.72226454079742588e+01 + 3.80134460532522880e+01 + -3.51922609512735107e+01 + -2.39876166901732191e+01 + 7.97471671594631992e+00 + -4.83518374187914013e+00 + 8.66273522162050824e+01 + 4.67131848907273408e+01 + 1.17456223558336159e+01 + -5.59162876565777989e+00 + 8.16530126090797523e+01 + 6.71811935630704937e+01 + -7.04411581080473894e+01 + -2.09183319463410733e+01 + -1.56755267575532145e+01 + 1.18327312907135145e+01 + -5.35254607305620027e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -0.00000000000000000e+00 + 1.72947781738215802e+01 + -7.37458142645349284e+01 + 6.52874071041194242e+01 + -9.78660995310737718e+01 + -5.39934004805816503e+00 + 1.98260860892763304e+01 + -1.72947781738215802e+01 + 7.37458142645349284e+01 + -6.52874071041194242e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -1.10958195052130399e+01 + -6.73231164274660614e+01 + -7.31059558722925402e+01 + 9.78660995310737718e+01 + 5.39934004805816503e+00 + -1.98260860892763304e+01 + 1.10958195052130399e+01 + 6.73231164274660614e+01 + 7.31059558722925402e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -5.38048032045895752e+00 + 1.65536142881929180e+02 + 8.22017033006773943e+01 + -7.27309788858385105e+01 + -1.01358249880792499e+01 + 1.16499706864703938e+02 + 7.80821322304104655e+01 + 9.19602704161340796e+01 + -8.91929751904364565e+01 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 20 + 21 + + + + + + 9 + 7 + + 6.79396504738739537e+01 + 4.82037868237773779e+01 + -5.47024548652807283e+01 + -7.00832895591236564e+01 + 5.14309272143008727e+01 + -3.67255818969308834e+01 + 1.86928047352814559e+01 + -2.23350198922307115e+01 + 1.77311293144151279e+01 + 7.44105813075135192e+00 + 5.99087969415584212e+01 + 5.30460372996668283e+01 + -1.22158131494639797e+01 + -1.50072994727732638e+01 + -6.43870652455934760e+01 + -1.47523300546991756e+01 + 7.84637907473050689e+01 + -5.14524303561770466e+01 + 1.48130048314395335e+00 + -3.96193494527523526e+01 + -4.51446929817367320e+01 + 2.41623483145540554e+01 + -4.80504534286710054e+01 + -6.06763607845337773e+01 + 9.54718801387472098e+01 + 2.61345793187026096e+01 + 4.22977195343479639e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -4.31638784413171734e+01 + 8.58566719453584426e+01 + 2.76678781327787284e+01 + -6.66070179567136051e+01 + -9.65155343163612223e+00 + -7.39618325575448523e+01 + 4.31638784413171734e+01 + -8.58566719453584426e+01 + -2.76678781327787284e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -6.08307879023215037e+01 + -5.03535440542508113e+01 + 6.13525536906117850e+01 + 6.66070179567136051e+01 + 9.65155343163612223e+00 + 7.39618325575448523e+01 + 6.08307879023215037e+01 + 5.03535440542508113e+01 + -6.13525536906117850e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.33814642652391399e+02 + 1.77445244940366287e-01 + -1.49228346303260491e+01 + -6.12411970511728132e-01 + 5.97803936488639280e+01 + -1.02665793382055853e+02 + 2.53293581625552058e+01 + -3.42663008134674527e+01 + 5.61197444293587395e+01 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 21 + 22 + + + + + + 9 + 7 + + -6.74405298173737435e+01 + 7.32759993494661614e+01 + -6.72758148030300518e+00 + 2.84952832715093294e+01 + 3.71106482095568992e+01 + 5.87105666468560798e+01 + 3.06497832963929113e+01 + 2.67945705145493136e+01 + 5.23800862467740629e+01 + -2.34193589858697777e+01 + -2.67958038635302600e+01 + 2.45872057837964846e+01 + 1.16252649553510334e+01 + 8.99424898505579051e+01 + -2.22061693363508326e+01 + 6.98562239082597500e+01 + -3.31539117547968303e+01 + -5.85832970823880999e+01 + 5.89049685210982901e+01 + 4.59874265404548481e+01 + -5.08083954437895216e+01 + -2.22191873368612889e+01 + 1.16539331220696187e+01 + -7.08270509506904347e+01 + 6.46018011815524602e+01 + 1.94415681363770645e+01 + 4.04539860386468320e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 9.78145711723120286e+01 + 1.76232674770143696e+01 + -1.10331368980137903e+01 + -1.40616464377447681e+01 + 1.69815554662718142e+01 + -9.75392068524539155e+01 + -9.78145711723120286e+01 + -1.76232674770143696e+01 + 1.10331368980137903e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -1.53159970565603700e+01 + 9.69589976091932186e+01 + 1.90885572211179095e+01 + 1.40616464377447681e+01 + -1.69815554662718142e+01 + 9.75392068524539155e+01 + 1.53159970565603700e+01 + -9.69589976091932186e+01 + -1.90885572211179095e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -2.25457773322750015e+01 + -1.39359117049240723e+02 + -1.01360767749937452e+02 + -1.06577486560693373e+02 + 3.69148207609075527e+01 + -6.52361857149522422e+01 + -9.51115531141346224e+01 + 7.07220482565039248e+01 + -3.57407782543718326e+01 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 22 + 23 + + + + + + 9 + 7 + + 9.41929589378244003e+01 + 8.94097901372627391e+00 + 3.01620867599012286e+01 + 2.46922319059140740e+01 + 1.35465056881596215e+01 + -4.89963411381141611e+01 + -1.68344538935716344e+01 + -1.83167626535859007e+01 + 7.95313381974681306e+01 + -5.18828216803281350e+00 + -4.19655627938833504e+01 + -6.55823195034947481e+00 + 7.35562337314085681e+01 + 5.97403143502788865e+01 + -2.03387857576245445e-01 + 6.60668686562018337e+01 + -6.61937508072900442e+01 + -2.10487850516630495e+01 + 1.54947760148805180e+01 + -8.96286058833042887e+01 + -5.79472767879112993e+00 + -6.70795355915261116e+00 + -1.69646265262306706e+01 + -8.65671984840189168e+01 + -1.21336256996353509e+01 + 3.69909891189522924e+01 + -4.84851501469951671e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -0.00000000000000000e+00 + 8.51395966446430350e+01 + -5.19584580090174413e+01 + 7.18106708732509347e+00 + -5.24346283807042397e+01 + -8.39535649250525182e+01 + 1.42270405549584336e+01 + -8.51395966446430350e+01 + 5.19584580090174413e+01 + -7.18106708732509347e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -1.36338907320492919e+00 + -1.58782107839693651e+01 + -9.87219509153607788e+01 + 5.24346283807042397e+01 + 8.39535649250525182e+01 + -1.42270405549584336e+01 + 1.36338907320492919e+00 + 1.58782107839693651e+01 + 9.87219509153607788e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 3.07007154914777551e+01 + 2.70881185233511914e+01 + 3.78166527462940882e+00 + -1.15162420127751133e+02 + -6.75390943713141123e+00 + 3.95885323960980529e+00 + -1.29970649317129876e+01 + 1.14524822858325365e+02 + 2.24989739182521227e+01 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 23 + 24 + + + + + + 9 + 7 + + -1.83421298069983294e+01 + -8.40178887379047552e+01 + 1.37665553988889080e+01 + 3.46271545199608042e+01 + 4.05922653756383482e+01 + -7.00698095538007770e+00 + 9.19927590625682683e+01 + -3.22667773518094236e+01 + 3.91925773350330608e+00 + 4.07207141973007651e+01 + -3.85719388369921674e+01 + -7.75337815930943464e+01 + 7.24865255322073381e+01 + -3.53413936296266158e+01 + 5.86044610642822335e+01 + -1.83884977004002117e+01 + 1.46625501311897892e+01 + 1.87515674799128185e+01 + -2.53722481762123628e+01 + -3.66484706705800178e+01 + 3.56472023092901580e+01 + -4.40875208799477321e+01 + -6.95087704456724254e+01 + 1.97777311636326232e+01 + 1.27979930474377834e+01 + 3.35520358632383733e+01 + 9.10304934695741821e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -2.81762066375282245e+01 + 9.59328037613137070e+01 + -1.73162987170684679e+00 + -8.31278175295505264e+01 + -2.53085961788951153e+01 + -4.94898061445724977e+01 + 2.81762066375282245e+01 + -9.59328037613137070e+01 + 1.73162987170684679e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -4.79152098220707003e+01 + -1.25048839237672293e+01 + 8.68778484181097070e+01 + 8.31278175295505264e+01 + 2.53085961788951153e+01 + 4.94898061445724977e+01 + 4.79152098220707003e+01 + 1.25048839237672293e+01 + -8.68778484181097070e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -3.78847831737257366e+01 + 2.29972416224487830e+01 + -1.37159770597291157e+02 + -4.30909041970757869e+01 + -7.29718405033518991e+01 + -1.27753851859623239e+02 + -4.00153271264082644e+00 + -1.83214678716897197e+02 + 3.84258732608477231e+01 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 24 + 25 + + + + + + 9 + 7 + + -1.42022591781322873e+01 + -3.56662961537267762e+01 + -1.29665633112741379e+01 + -4.42547548664715933e+01 + 8.53484085500931968e+01 + -1.21228131477824768e+01 + 7.44576769541145751e+01 + 3.58775048496875613e+01 + 4.62292669635554603e+01 + 1.27026107710752729e+01 + -9.11117710856913021e+01 + 2.52571349253896464e+01 + -3.19164772916096373e+00 + -2.63470878340458832e+01 + 1.88346366483448016e+01 + 5.40205809637968812e+01 + -1.90775864349462978e+01 + -8.09820522299282857e+01 + 6.16280452912953720e+01 + 1.93876135715390099e+01 + 7.12990641830701719e+01 + 6.58414093447228908e+01 + 1.80759831933341069e+01 + -6.99233759491107918e+01 + 3.27566373625739402e+01 + 9.70961890249048842e+00 + 4.91628253265810322e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 9.34197183252336458e+01 + 2.76233972145661149e+01 + 2.25766284985226449e+01 + -2.39170403241009595e+01 + 1.53702168542746431e+00 + 9.70855949483436831e+01 + -9.34197183252336458e+01 + -2.76233972145661149e+01 + -2.25766284985226449e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 2.64713318548450189e+01 + -9.60967506769340645e+01 + 8.04258037981846208e+00 + 2.39170403241009595e+01 + -1.53702168542746431e+00 + -9.70855949483436831e+01 + -2.64713318548450189e+01 + 9.60967506769340645e+01 + -8.04258037981846208e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 4.99415394471549448e+01 + 8.89774612805008616e+01 + -7.21549206200566857e+01 + -8.47137695131298614e+01 + -3.96302269514171712e+01 + 2.91958404364136328e+01 + -1.14979727399682560e+02 + 6.32312875848932592e+01 + -5.83619937525406982e+01 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 25 + 26 + + + + + + 9 + 7 + + 4.75279810851286726e+01 + 1.27698906927141866e+01 + -3.85248964418578721e+00 + 8.38265009034030300e+01 + 2.34263445942633055e+01 + 4.00325151654156208e+00 + -3.67133537961906153e+00 + 7.31784749045807636e+00 + 9.95211070535517166e+01 + 7.72966987607180016e+01 + 3.83523138848579492e+01 + 1.84348658933893006e+01 + -5.00282378767150675e+01 + 1.62934700342947103e+00 + -1.23502536597363957e+01 + 3.08280910111897235e+01 + -9.23259671614170685e+01 + 9.05617058795025009e+00 + -3.32567607414421573e+01 + 4.65581163028938505e+01 + 8.06472888898161528e+01 + -1.09449242551094059e+01 + 8.15866317932763252e+01 + -5.45355209481987444e+01 + -6.85221032691171050e+00 + 2.03202505385987919e+01 + 3.49109728776780415e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -0.00000000000000000e+00 + 5.75486223700384372e+01 + -3.56976774925786202e+01 + 7.35787461495951334e+01 + -7.77278254550152923e+01 + 4.09823526552186745e+00 + 6.27820803871861983e+01 + -5.75486223700384372e+01 + 3.56976774925786202e+01 + -7.35787461495951334e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -2.54271747023806824e+01 + -9.33213817372218273e+01 + -2.53885505161348775e+01 + 7.77278254550152923e+01 + -4.09823526552186745e+00 + -6.27820803871861983e+01 + 2.54271747023806824e+01 + 9.33213817372218273e+01 + 2.53885505161348775e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 5.11234651369603128e+01 + 1.45916426283882128e+01 + 8.14361101436695947e+01 + -9.64495575814352151e+01 + 5.69392664765279974e+01 + 1.45591038998347216e+02 + -1.52360008121073719e+02 + 3.92577375478538881e+00 + -7.47401324352360774e+01 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 7 + 10 + + + + + + 9 + 7 + + -5.40766118135344485e+01 + -6.55945677789347137e+00 + 5.92166444746720977e-01 + 8.33981325801565987e+01 + -1.15536936327200639e+01 + 1.11233523720273890e+01 + -9.80808079024424018e+00 + -8.64371084278765380e+01 + 4.74584491239462309e+01 + 1.25135196935038806e+01 + 9.77748374151099995e+01 + 6.14999725352584115e+00 + 1.36807989121553284e+01 + 4.43989849701706429e+00 + -9.89599830977766572e+01 + -2.01505456436379626e+00 + -1.55263904030224698e+01 + -7.65145679760131925e-01 + 8.31600037168701789e+01 + -1.88305696684158264e+01 + -2.79941305069657931e+00 + 5.18811705043763141e+01 + -7.17174238817171439e+00 + 6.66998670151083850e+00 + -8.31970807941664781e+00 + -4.61019176373218968e+01 + -8.79761519740697651e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 7.83449748282976799e+01 + 6.13201754047114846e+01 + 1.00946028891096997e+01 + -1.98027611608243852e+01 + 9.23644092983748521e+00 + 9.75834966034573341e+01 + -7.83449748282976799e+01 + -6.13201754047114846e+01 + -1.00946028891096997e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 5.89059892503364324e+01 + -7.84507759508154265e+01 + 1.93793752003053505e+01 + 1.98027611608243852e+01 + -9.23644092983748521e+00 + -9.75834966034573341e+01 + -5.89059892503364324e+01 + 7.84507759508154265e+01 + -1.93793752003053505e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -6.07889604414481113e+01 + 8.49672366649234476e+01 + -1.19149060367207383e+02 + -3.26841768394553398e+01 + -8.97350957633814517e+01 + 9.13622785379021281e+01 + -1.77493968880864855e+02 + -4.85855715361152676e+01 + -7.39158595946127228e+00 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 9 + 14 + + + + + + 9 + 7 + + 4.43615564152903019e+01 + -3.09502672537490433e+01 + -6.65256070847414378e+01 + 4.96695435786911474e+01 + 2.78172662190901541e+01 + 6.19720498005987466e+01 + -7.45919697090238145e+01 + 1.35179597700267684e+00 + 1.13620760877569249e+00 + 5.07131897166802617e+01 + -6.73003419642306540e+01 + 3.11633063652204427e+01 + 5.47056695452382584e+01 + 6.11187671649742512e+01 + -2.76098847773204135e+01 + 6.65963623398869657e+01 + 1.43297991149407444e+00 + -1.99391285797603790e-01 + 7.65967691022320984e-01 + 1.84943157433388130e+01 + -6.50656556956834748e+01 + 8.44674932355512387e-01 + 2.30555803308049860e+01 + -6.99700625824593629e+01 + -3.40876610014427706e-01 + -9.55299072188111609e+01 + -2.94902469873441930e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -2.89030917873941853e+01 + 8.70749520313458305e+01 + -3.97814531392241904e+01 + -9.56630827209552450e+01 + -2.78467078053438328e+01 + 8.55192777842975183e+00 + 2.89030917873941853e+01 + -8.70749520313458305e+01 + 3.97814531392241904e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -3.63123800557649812e+00 + 4.05279359595652906e+01 + 9.13471429077244466e+01 + 9.56630827209552450e+01 + 2.78467078053438328e+01 + -8.55192777842975183e+00 + 3.63123800557649812e+00 + -4.05279359595652906e+01 + -9.13471429077244466e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -7.02581401004128878e+01 + -1.05108950320020767e+02 + -1.10573546354685661e+02 + -2.82871715200345584e+01 + -9.82678476821147342e+01 + -1.38389269150812240e+01 + 2.98238876860104902e+01 + -1.16573384649075678e+02 + 1.35327233223631623e+02 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 10 + 25 + + + + + + 9 + 7 + + 3.95766223652825175e+01 + 2.36448299245490254e+01 + -2.45799893899839930e+01 + -7.00228044208398188e+01 + -4.93985238797199173e+01 + 1.28698697829822883e+01 + 7.78345939301136092e+00 + 2.90892349948341078e+01 + 9.40990441114148695e+01 + -8.42616415682074660e+01 + -2.90334966132265606e+01 + -1.43512687624874875e+01 + -2.62231810185785967e+01 + -4.77560189769582593e+01 + -8.01321381020614343e-01 + 4.26634609899314583e+01 + -8.24081707666526171e+01 + 1.64578293010562966e+01 + -8.45857606147821528e+00 + 2.58771125144124774e+01 + -9.15441805767733570e+01 + 5.92769857321209486e+01 + -7.03919737513036381e+01 + -3.24535577243972782e+01 + -1.66359962010244793e+01 + 2.48028458121687763e+01 + -2.15529183777999371e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 9.34197183252336458e+01 + 2.76233972145661149e+01 + 2.25766284985226449e+01 + -2.39170403241009595e+01 + 1.53702168542746431e+00 + 9.70855949483436831e+01 + -9.34197183252336458e+01 + -2.76233972145661149e+01 + -2.25766284985226449e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 2.64713318548450189e+01 + -9.60967506769340645e+01 + 8.04258037981846208e+00 + 2.39170403241009595e+01 + -1.53702168542746431e+00 + -9.70855949483436831e+01 + -2.64713318548450189e+01 + 9.60967506769340645e+01 + -8.04258037981846208e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -6.19976450903283478e+01 + 1.85138049290959714e+02 + 2.04096238885501400e+01 + -5.38401179783636792e+01 + 1.95403129638118465e+01 + 3.38132128740523674e+00 + -1.81977553628765463e+02 + -6.94268461743989320e+01 + -2.32863678513998718e+00 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 13 + 22 + + + + + + 9 + 7 + + 1.61359876726555882e+01 + 1.08711432986265653e+01 + 8.51184696478907732e+00 + 6.31122752697449414e+01 + -1.20771880436756209e+01 + 7.50149628126125521e+01 + -4.10840851091186110e+01 + -8.68858010361213076e+01 + 2.35731234098279678e+01 + 4.08177879353251924e+01 + -2.17654374458011439e+01 + 8.78457730201184290e+01 + -2.17842465512247614e+01 + 1.46672234096332783e+01 + 6.07972286227193015e-01 + 6.89954693463132855e+01 + -4.93810369540159471e+01 + -4.74803457645439977e+01 + -6.20341282977744939e+01 + -7.54126088063309794e+01 + 1.25298075512294904e+01 + -5.28542109714301915e+01 + 5.80638264069757923e+01 + 5.82126617561848008e+01 + -2.29292298928228675e+01 + -1.00045379878072604e+00 + 1.50754924253946800e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 9.78145711723120286e+01 + 1.76232674770143696e+01 + -1.10331368980137903e+01 + -1.40616464377447681e+01 + 1.69815554662718142e+01 + -9.75392068524539155e+01 + -9.78145711723120286e+01 + -1.76232674770143696e+01 + 1.10331368980137903e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -1.53159970565603700e+01 + 9.69589976091932186e+01 + 1.90885572211179095e+01 + 1.40616464377447681e+01 + -1.69815554662718142e+01 + 9.75392068524539155e+01 + 1.53159970565603700e+01 + -9.69589976091932186e+01 + -1.90885572211179095e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 8.03183434009733901e+01 + -1.57959181406014523e+02 + -6.44060214101481705e+01 + -6.64898917378079801e+01 + -6.21866664385735888e+01 + -6.61756460634393022e+01 + -1.52823432141161049e+02 + -1.42511660560011357e+01 + -7.24093903591090111e+01 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 20 + 15 + + + + + + 9 + 7 + + 4.71343070271561331e+01 + -6.72091990499062604e+01 + 5.47388912655424136e+01 + 8.35500845890857278e+01 + 2.23841738875626213e+01 + -3.32945376880477681e+01 + -2.59734415135038539e+01 + -2.12006909898996589e+01 + 2.34658474313030787e+01 + 6.32652845860905977e+00 + 9.30238234940692976e+00 + 3.37625583972403689e+01 + 3.44554933454442613e+00 + 8.79955033997688929e+01 + 4.09722885299349642e+01 + -1.92439510927881194e+01 + 4.62720459594682580e+01 + -8.23910435376807584e+01 + -5.55999617765192156e+00 + 6.20368695338309522e+01 + 7.15470037167418553e+01 + 3.41059120969163914e+01 + -1.39920442875337869e+01 + -2.48395357584865195e+01 + 9.22321060162172301e+01 + 2.27421056426905714e+01 + 1.25600461547882558e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -8.15059339771809164e+01 + -5.54598536753216109e+01 + -1.67626774955357369e+01 + -1.04808841952049807e+01 + 4.25686446848321935e+01 + -8.98780371179923492e+01 + 8.15059339771809164e+01 + 5.54598536753216109e+01 + 1.67626774955357369e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 5.69818724946287603e+01 + -7.14990567770542356e+01 + -4.05086544703406517e+01 + 1.04808841952049807e+01 + -4.25686446848321935e+01 + 8.98780371179923492e+01 + -5.69818724946287603e+01 + 7.14990567770542356e+01 + 4.05086544703406517e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -0.00000000000000000e+00 + -1.44773765128431023e+02 + 3.21567907629271019e+01 + 6.77990836285083986e+01 + 3.24643872643349098e+01 + 3.07230481384375675e+00 + -8.66387073171390654e+00 + 6.03331707496851593e+01 + 1.38479259212844596e+02 + 6.83329111783498746e+01 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 27 + 0 + 0 + 1 + 2 + 3 + 4 + 5 + 6 + 7 + 8 + 9 + 10 + 11 + 12 + 13 + 14 + 15 + 16 + 17 + 18 + 19 + 20 + 21 + 22 + 23 + 24 + 25 + 26 + + + + + + 3 + 82 + + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + + + + 29 + 0 + 0 + 3 + 6 + 9 + 12 + 15 + 18 + 21 + 24 + 27 + 30 + 33 + 36 + 39 + 42 + 45 + 48 + 51 + 54 + 57 + 60 + 63 + 66 + 69 + 72 + 75 + 78 + 81 + 82 + + 0 + 3 + 0 + + + + + + + + + + + diff --git a/examples/Data/toy3D.xml b/examples/Data/toy3D.xml new file mode 100644 index 000000000..13dbcbe6c --- /dev/null +++ b/examples/Data/toy3D.xml @@ -0,0 +1,169 @@ + + + + + + + 2 + 1 + + + + + + 2 + 0 + 0 + 1 + + + + + + 9 + 7 + + 1.22427709071730959e+01 + 1.51514613104920048e+01 + 3.60934366857813060e+00 + -1.18407259026383116e+01 + 7.84826117220921216e+00 + 1.23509165494819051e+01 + -6.09875015991639735e+00 + 6.16547190708139126e-01 + 3.94972084922329048e+00 + -4.89208482920378174e+00 + 3.02091647632478866e+00 + -8.95328692238917512e+00 + 7.89831607220345955e+00 + -2.36793602009719084e+00 + 1.48517612051941725e+01 + -3.97284286249233731e-01 + -1.95744531643153863e+01 + -3.85954855417462017e+00 + 4.79268277145419042e+00 + -9.01707953629520453e+00 + 1.37848069005841385e+01 + 1.04829326688375950e+01 + -5.00630568442241675e+00 + 4.70463561852773182e+00 + -1.59179134598689274e+01 + -2.04767784956723942e+00 + 9.54135497908261954e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + -0.00000000000000000e+00 + -1.76201757312015372e+00 + 1.98634190821282672e+01 + 1.52966546661624236e+00 + 1.94817649567575373e+01 + 1.39684693294110307e+00 + 4.30228460420588288e+00 + 1.76201757312015372e+00 + -1.98634190821282672e+01 + -1.52966546661624236e+00 + -0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 4.16606867942304948e+00 + 1.86906420801308037e+00 + -1.94717865319198360e+01 + -1.94817649567575373e+01 + -1.39684693294110307e+00 + -4.30228460420588288e+00 + -4.16606867942304948e+00 + -1.86906420801308037e+00 + 1.94717865319198360e+01 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00891462904330531e+01 + -1.08132497987816869e+01 + 8.66487736568128497e+00 + 2.88370015604634311e+01 + 1.89391698948574643e+01 + 2.12398960190661290e+00 + 1.22150946112124039e+01 + -2.33658532501596561e+01 + 1.51576204760307363e+01 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 9 + 0 + + + + + + + + + + + + 2 + 0 + 0 + 1 + + + + + + 3 + 7 + + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 1.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + 0.00000000000000000e+00 + + + + 4 + 0 + 0 + 3 + 6 + 7 + + 0 + 3 + 0 + + + + + + + + + + + diff --git a/examples/InverseKinematicsExampleExpressions.cpp b/examples/InverseKinematicsExampleExpressions.cpp new file mode 100644 index 000000000..9e86886e7 --- /dev/null +++ b/examples/InverseKinematicsExampleExpressions.cpp @@ -0,0 +1,92 @@ +/* ---------------------------------------------------------------------------- + + * 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 InverseKinematicsExampleExpressions.cpp + * @brief Implement inverse kinematics on a three-link arm using expressions. + * @date April 15, 2019 + * @author Frank Dellaert + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +// Scalar multiplication of a vector, with derivatives. +inline Vector3 scalarMultiply(const double& s, const Vector3& v, + OptionalJacobian<3, 1> Hs, + OptionalJacobian<3, 3> Hv) { + if (Hs) *Hs = v; + if (Hv) *Hv = s * I_3x3; + return s * v; +} + +// Expression version of scalar product, using above function. +inline Vector3_ operator*(const Double_& s, const Vector3_& v) { + return Vector3_(&scalarMultiply, s, v); +} + +// Expression version of Pose2::Expmap +inline Pose2_ Expmap(const Vector3_& xi) { return Pose2_(&Pose2::Expmap, xi); } + +// Main function +int main(int argc, char** argv) { + // Three-link planar manipulator specification. + const double L1 = 3.5, L2 = 3.5, L3 = 2.5; // link lengths + const Pose2 sXt0(0, L1 + L2 + L3, M_PI / 2); // end-effector pose at rest + const Vector3 xi1(0, 0, 1), xi2(L1, 0, 1), + xi3(L1 + L2, 0, 1); // screw axes at rest + + // Create Expressions for unknowns + using symbol_shorthand::Q; + Double_ q1(Q(1)), q2(Q(2)), q3(Q(3)); + + // Forward kinematics expression as product of exponentials + Pose2_ l1Zl1 = Expmap(q1 * Vector3_(xi1)); + Pose2_ l2Zl2 = Expmap(q2 * Vector3_(xi2)); + Pose2_ l3Zl3 = Expmap(q3 * Vector3_(xi3)); + Pose2_ forward = compose(compose(l1Zl1, l2Zl2), compose(l3Zl3, Pose2_(sXt0))); + + // Create a factor graph with a a single expression factor. + ExpressionFactorGraph graph; + Pose2 desiredEndEffectorPose(3, 2, 0); + auto model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); + graph.addExpressionFactor(forward, desiredEndEffectorPose, model); + + // Create initial estimate + Values initial; + initial.insert(Q(1), 0.1); + initial.insert(Q(2), 0.2); + initial.insert(Q(3), 0.3); + initial.print("\nInitial Estimate:\n"); // print + GTSAM_PRINT(forward.value(initial)); + + // Optimize the initial values using a Levenberg-Marquardt nonlinear optimizer + LevenbergMarquardtParams params; + params.setlambdaInitial(1e6); + LevenbergMarquardtOptimizer optimizer(graph, initial, params); + Values result = optimizer.optimize(); + result.print("Final Result:\n"); + + GTSAM_PRINT(forward.value(result)); + + return 0; +} diff --git a/gtsam.h b/gtsam.h index 3d4ee5c29..aefe4e028 100644 --- a/gtsam.h +++ b/gtsam.h @@ -209,11 +209,60 @@ class KeyGroupMap { bool insert2(size_t key, int val); }; +// Actually a FastSet +class FactorIndexSet { + FactorIndexSet(); + FactorIndexSet(const gtsam::FactorIndexSet& set); + + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + void insert(size_t factorIndex); + bool erase(size_t factorIndex); // returns true if value was removed + bool count(size_t factorIndex) const; // returns true if value exists +}; + +// Actually a vector +class FactorIndices { + FactorIndices(); + FactorIndices(const gtsam::FactorIndices& other); + + // 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 factorIndex) const; +}; + //************************************************************************* // base //************************************************************************* /** gtsam namespace functions */ + +#include +class IndexPair { + IndexPair(); + IndexPair(size_t i, size_t j); + size_t i() const; + size_t j() const; +}; + +template +class DSFMap { + DSFMap(); + KEY find(const KEY& key) const; + void merge(const KEY& x, const KEY& y); +}; + #include bool linear_independent(Matrix A, Matrix B, double tol); @@ -577,9 +626,9 @@ class Pose2 { static Matrix LogmapDerivative(const gtsam::Pose2& v); Matrix AdjointMap() const; Vector Adjoint(Vector xi) const; - static Matrix adjointMap(Vector v); - Vector adjoint(Vector xi, Vector y); - Vector adjointTranspose(Vector xi, Vector y); + static Matrix adjointMap_(Vector v); + static Vector adjoint_(Vector xi, Vector y); + static Vector adjointTranspose(Vector xi, Vector y); static Matrix wedge(double vx, double vy, double w); // Group Actions on Point2 @@ -628,8 +677,8 @@ class Pose3 { static Vector Logmap(const gtsam::Pose3& pose); Matrix AdjointMap() const; Vector Adjoint(Vector xi) const; - static Matrix adjointMap(Vector xi); - static Vector adjoint(Vector xi, Vector y); + static Matrix adjointMap_(Vector xi); + static Vector adjoint_(Vector xi, Vector y); static Vector adjointTranspose(Vector xi, Vector y); static Matrix ExpmapDerivative(Vector xi); static Matrix LogmapDerivative(const gtsam::Pose3& xi); @@ -1218,14 +1267,30 @@ class VariableIndex { namespace noiseModel { #include virtual class Base { + void print(string s) const; + // Methods below are available for all noise models. However, can't add them + // because wrap (incorrectly) thinks robust classes derive from this Base as well. + // bool isConstrained() const; + // bool isUnit() const; + // size_t dim() const; + // Vector sigmas() const; }; 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; + + // access to noise model + Matrix R() const; + Matrix information() const; + Matrix covariance() const; + + // Whitening operations + Vector whiten(Vector v) const; + Vector unwhiten(Vector v) const; + Matrix Whiten(Matrix H) const; // enabling serialization functionality void serializable() const; @@ -1236,7 +1301,11 @@ virtual class Diagonal : gtsam::noiseModel::Gaussian { static gtsam::noiseModel::Diagonal* Variances(Vector variances); static gtsam::noiseModel::Diagonal* Precisions(Vector precisions); Matrix R() const; - void print(string s) const; + + // access to noise model + Vector sigmas() const; + Vector invsigmas() const; + Vector precisions() const; // enabling serialization functionality void serializable() const; @@ -1263,7 +1332,9 @@ 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; + + // access to noise model + double sigma() const; // enabling serialization functionality void serializable() const; @@ -1271,7 +1342,6 @@ virtual class Isotropic : gtsam::noiseModel::Diagonal { 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; @@ -1279,11 +1349,11 @@ virtual class Unit : gtsam::noiseModel::Isotropic { namespace mEstimator { virtual class Base { + void print(string s) const; }; virtual class Null: gtsam::noiseModel::mEstimator::Base { Null(); - void print(string s) const; static gtsam::noiseModel::mEstimator::Null* Create(); // enabling serialization functionality @@ -1292,7 +1362,6 @@ virtual class Null: gtsam::noiseModel::mEstimator::Base { 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 @@ -1301,7 +1370,6 @@ virtual class Fair: gtsam::noiseModel::mEstimator::Base { 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 @@ -1310,7 +1378,6 @@ virtual class Huber: gtsam::noiseModel::mEstimator::Base { 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 @@ -1322,7 +1389,6 @@ virtual class Tukey: gtsam::noiseModel::mEstimator::Base { 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; @@ -1713,7 +1779,7 @@ virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters { 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); + SubgraphSolver(const gtsam::GaussianFactorGraph &Ab1, const gtsam::GaussianFactorGraph* Ab2, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering& ordering); gtsam::VectorValues optimize() const; }; @@ -2002,10 +2068,12 @@ virtual class NonlinearOptimizerParams { void setVerbosity(string s); string getLinearSolverType() const; - void setLinearSolverType(string solver); - void setOrdering(const gtsam::Ordering& ordering); + void setIterativeParams(gtsam::IterativeOptimizationParameters* params); + void setOrdering(const gtsam::Ordering& ordering); + string getOrderingType() const; + void setOrderingType(string ordering); bool isMultifrontal() const; bool isSequential() const; @@ -2026,15 +2094,32 @@ virtual class GaussNewtonParams : gtsam::NonlinearOptimizerParams { virtual class LevenbergMarquardtParams : gtsam::NonlinearOptimizerParams { LevenbergMarquardtParams(); - double getlambdaInitial() const; + bool getDiagonalDamping() const; double getlambdaFactor() const; + double getlambdaInitial() const; + double getlambdaLowerBound() const; double getlambdaUpperBound() const; + bool getUseFixedLambdaFactor(); + string getLogFile() const; string getVerbosityLM() const; - void setlambdaInitial(double value); + void setDiagonalDamping(bool flag); void setlambdaFactor(double value); + void setlambdaInitial(double value); + void setlambdaLowerBound(double value); void setlambdaUpperBound(double value); + void setUseFixedLambdaFactor(bool flag); + void setLogFile(string s); void setVerbosityLM(string s); + + static gtsam::LevenbergMarquardtParams LegacyDefaults(); + static gtsam::LevenbergMarquardtParams CeresDefaults(); + + static gtsam::LevenbergMarquardtParams EnsureHasOrdering( + gtsam::LevenbergMarquardtParams params, + const gtsam::NonlinearFactorGraph& graph); + static gtsam::LevenbergMarquardtParams ReplaceOrdering( + gtsam::LevenbergMarquardtParams params, const gtsam::Ordering& ordering); }; #include @@ -2173,8 +2258,6 @@ class ISAM2Result { size_t getCliques() const; }; -class FactorIndices {}; - class ISAM2 { ISAM2(); ISAM2(const gtsam::ISAM2Params& params); @@ -2480,6 +2563,36 @@ void save2D(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& config, gtsam::noiseModel::Diagonal* model, string filename); +// std::vector::shared_ptr> +class BetweenFactorPose3s +{ + size_t size() const; + gtsam::BetweenFactorPose3* at(size_t i) const; +}; + +#include +class InitializePose3 { + static gtsam::Values computeOrientationsChordal( + const gtsam::NonlinearFactorGraph& pose3Graph); + static gtsam::Values computeOrientationsGradient( + const gtsam::NonlinearFactorGraph& pose3Graph, + const gtsam::Values& givenGuess, size_t maxIter, const bool setRefFrame); + static gtsam::Values computeOrientationsGradient( + const gtsam::NonlinearFactorGraph& pose3Graph, + const gtsam::Values& givenGuess); + static gtsam::NonlinearFactorGraph buildPose3graph( + const gtsam::NonlinearFactorGraph& graph); + static gtsam::Values initializeOrientations( + const gtsam::NonlinearFactorGraph& graph); + static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& givenGuess, + bool useGradient); + static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph); +}; + +gtsam::BetweenFactorPose3s parse3DFactors(string filename); +pair load3D(string filename); + pair readG2o(string filename); pair readG2o(string filename, bool is3D); void writeG2o(const gtsam::NonlinearFactorGraph& graph, diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index d369009c0..60915eead 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -101,16 +101,67 @@ 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 PUBLIC ${GTSAM_COMPILE_DEFINITIONS}) -target_compile_options(gtsam PUBLIC ${GTSAM_COMPILE_OPTIONS}) +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}) set_target_properties(gtsam PROPERTIES OUTPUT_NAME gtsam CLEAN_DIRECT_OUTPUT 1 VERSION ${gtsam_version} SOVERSION ${gtsam_soversion}) +# Append Eigen include path, set in top-level CMakeLists.txt to either +# system-eigen, or GTSAM eigen path +target_include_directories(gtsam PUBLIC + $ + $ +) +# MKL include dir: +if (GTSAM_USE_EIGEN_MKL) + target_include_directories(gtsam PUBLIC ${MKL_INCLUDE_DIR}) +endif() + +if(GTSAM_USE_TBB) + target_include_directories(gtsam PUBLIC ${TBB_INCLUDE_DIRS}) +endif() + +# Add includes for source directories 'BEFORE' boost and any system include +# paths so that the compiler uses GTSAM headers in our source directory instead +# of any previously installed GTSAM headers. +target_include_directories(gtsam BEFORE PUBLIC + # SuiteSparse_config + $ + $ + # CCOLAMD + $ + $ + # main gtsam includes: + $ + $ + # config.h + $ + # unit tests: + $ +) +if(GTSAM_SUPPORT_NESTED_DISSECTION) + target_include_directories(gtsam BEFORE PUBLIC + $ + $ + $ + $ + ) +endif() + + + if(WIN32) # Add 'lib' prefix to static library to avoid filename collision with shared library if (NOT BUILD_SHARED_LIBS) set_target_properties(gtsam PROPERTIES diff --git a/gtsam_unstable/base/DSFMap.h b/gtsam/base/DSFMap.h similarity index 82% rename from gtsam_unstable/base/DSFMap.h rename to gtsam/base/DSFMap.h index f5a5ca2b1..dfce185dc 100644 --- a/gtsam_unstable/base/DSFMap.h +++ b/gtsam/base/DSFMap.h @@ -18,9 +18,9 @@ #pragma once +#include // Provides size_t #include #include -#include // Provides size_t namespace gtsam { @@ -29,11 +29,9 @@ namespace gtsam { * Uses rank compression and union by rank, iterator version * @addtogroup base */ -template +template class DSFMap { - -protected: - + protected: /// We store the forest in an STL map, but parents are done with pointers struct Entry { typename std::map::iterator parent_; @@ -41,7 +39,7 @@ protected: Entry() {} }; - typedef typename std::map Map; + typedef typename std::map Map; typedef typename Map::iterator iterator; mutable Map entries_; @@ -62,8 +60,7 @@ protected: iterator find_(const iterator& it) const { // follow parent pointers until we reach set representative iterator& parent = it->second.parent_; - if (parent != it) - parent = find_(parent); // not yet, recurse! + if (parent != it) parent = find_(parent); // not yet, recurse! return parent; } @@ -73,13 +70,11 @@ protected: return find_(initial); } -public: - + public: typedef std::set Set; /// constructor - DSFMap() { - } + DSFMap() {} /// Given key, find the representative key for the set in which it lives inline KEY find(const KEY& key) const { @@ -89,12 +84,10 @@ public: /// Merge two sets void merge(const KEY& x, const KEY& y) { - // straight from http://en.wikipedia.org/wiki/Disjoint-set_data_structure iterator xRoot = find_(x); iterator yRoot = find_(y); - if (xRoot == yRoot) - return; + if (xRoot == yRoot) return; // Merge sets if (xRoot->second.rank_ < yRoot->second.rank_) @@ -117,7 +110,14 @@ public: } return sets; } - }; -} +/// Small utility class for representing a wrappable pairs of ints. +class IndexPair : public std::pair { + public: + IndexPair(): std::pair(0,0) {} + IndexPair(size_t i, size_t j) : std::pair(i,j) {} + inline size_t i() const { return first; }; + inline size_t j() const { return second; }; +}; +} // namespace gtsam diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index cc1cbdb51..a9a088108 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -67,28 +67,29 @@ struct FixedSizeMatrix { } /** - * Numerically compute gradient of scalar function + * @brief Numerically compute gradient of scalar function + * @return n-dimensional gradient computed via central differencing * Class X is the input argument * The class X needs to have dim, expmap, logmap + * int N is the dimension of the X input value if variable dimension type but known at test time */ -template -typename internal::FixedSizeMatrix::type numericalGradient(boost::function h, const X& x, - double delta = 1e-5) { + +template ::dimension> +typename Eigen::Matrix numericalGradient( + boost::function h, const X& x, double delta = 1e-5) { double factor = 1.0 / (2.0 * delta); BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X must be a manifold type."); - static const int N = traits::dimension; - BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type."); - - typedef typename traits::TangentVector TangentX; + BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type or N must be specified."); // Prepare a tangent vector to perturb x with, only works for fixed size - TangentX d; + typename traits::TangentVector d; d.setZero(); - Eigen::Matrix g; g.setZero(); // Can be fixed size + Eigen::Matrix g; + g.setZero(); for (int j = 0; j < N; j++) { d(j) = delta; double hxplus = h(traits::Retract(x, d)); @@ -108,37 +109,34 @@ typename internal::FixedSizeMatrix::type numericalGradient(boost::function +template ::dimension> // TODO Should compute fixed-size matrix -typename internal::FixedSizeMatrix::type numericalDerivative11(boost::function h, const X& x, - double delta = 1e-5) { - +typename internal::FixedSizeMatrix::type numericalDerivative11( + boost::function h, const X& x, double delta = 1e-5) { typedef typename internal::FixedSizeMatrix::type Matrix; BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); typedef traits TraitsY; - typedef typename TraitsY::TangentVector TangentY; BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X must be a manifold type."); - static const int N = traits::dimension; - BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type."); + BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type or N must be specified."); typedef traits TraitsX; - typedef typename TraitsX::TangentVector TangentX; // get value at x, and corresponding chart const Y hx = h(x); // Bit of a hack for now to find number of rows - const TangentY zeroY = TraitsY::Local(hx, hx); + const typename TraitsY::TangentVector zeroY = TraitsY::Local(hx, hx); const size_t m = zeroY.size(); // Prepare a tangent vector to perturb x with, only works for fixed size - TangentX dx; + Eigen::Matrix dx; dx.setZero(); // Fill in Jacobian H @@ -146,9 +144,9 @@ typename internal::FixedSizeMatrix::type numericalDerivative11(boost::funct const double factor = 1.0 / (2.0 * delta); for (int j = 0; j < N; j++) { dx(j) = delta; - const TangentY dy1 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx))); + const auto dy1 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx))); dx(j) = -delta; - const TangentY dy2 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx))); + const auto dy2 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx))); dx(j) = 0; H.col(j) << (dy1 - dy2) * factor; } diff --git a/gtsam_unstable/base/tests/testDSFMap.cpp b/gtsam/base/tests/testDSFMap.cpp similarity index 93% rename from gtsam_unstable/base/tests/testDSFMap.cpp rename to gtsam/base/tests/testDSFMap.cpp index 94955e1d9..816498ce8 100644 --- a/gtsam_unstable/base/tests/testDSFMap.cpp +++ b/gtsam/base/tests/testDSFMap.cpp @@ -16,7 +16,7 @@ * @brief unit tests for DSFMap */ -#include +#include #include #include @@ -115,7 +115,6 @@ TEST(DSFMap, mergePairwiseMatches2) { TEST(DSFMap, sets){ // Create some "matches" typedef pair Match; - typedef pair > key_pair; list matches; matches += Match(1,2), Match(2,3), Match(4,5), Match(4,6); @@ -140,6 +139,12 @@ TEST(DSFMap, sets){ EXPECT(s2 == sets[4]); } +/* ************************************************************************* */ +TEST(DSFMap, findIndexPair) { + DSFMap dsf; + EXPECT(dsf.find(IndexPair(1,2))==IndexPair(1,2)); + EXPECT(dsf.find(IndexPair(1,2)) != dsf.find(IndexPair(1,3))); +} /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/gtsam/base/types.h b/gtsam/base/types.h index c4775b672..3b6c9f337 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -56,6 +56,9 @@ namespace gtsam { /// Integer nonlinear key type typedef std::uint64_t Key; + /// Integer nonlinear factor index type + typedef std::uint64_t FactorIndex; + /// The index type for Eigen objects typedef ptrdiff_t DenseIndex; diff --git a/gtsam/geometry/Cal3_S2.cpp b/gtsam/geometry/Cal3_S2.cpp index 96e47fff0..ca4e88eb6 100644 --- a/gtsam/geometry/Cal3_S2.cpp +++ b/gtsam/geometry/Cal3_S2.cpp @@ -44,8 +44,7 @@ Cal3_S2::Cal3_S2(const std::string &path) : if (infile) infile >> fx_ >> fy_ >> s_ >> u0_ >> v0_; else { - printf("Unable to load the calibration\n"); - exit(0); + throw std::runtime_error("Cal3_S2: Unable to load the calibration"); } infile.close(); diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 603c3a421..6937ff78d 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -146,17 +146,21 @@ public: /** * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives */ - Vector3 adjoint(const Vector3& xi, const Vector3& y) { + static Vector3 adjoint(const Vector3& xi, const Vector3& y) { return adjointMap(xi) * y; } /** * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space. */ - Vector3 adjointTranspose(const Vector3& xi, const Vector3& y) { + static Vector3 adjointTranspose(const Vector3& xi, const Vector3& y) { return adjointMap(xi).transpose() * y; } + // temporary fix for wrappers until case issue is resolved + static Matrix3 adjointMap_(const Vector3 &xi) { return adjointMap(xi);} + static Vector3 adjoint_(const Vector3 &xi, const Vector3 &y) { return adjoint(xi, y);} + /** * wedge for SE(2): * @param xi 3-dim twist (v,omega) where diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index ca0fdff10..757c4fdd4 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -160,6 +160,10 @@ public: static Vector6 adjoint(const Vector6 &xi, const Vector6 &y, OptionalJacobian<6, 6> = boost::none); + // temporary fix for wrappers until case issue is resolved + static Matrix6 adjointMap_(const Vector6 &xi) { return adjointMap(xi);} + static Vector6 adjoint_(const Vector6 &xi, const Vector6 &y) { return adjoint(xi, y);} + /** * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space. */ diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 07c03ab49..034956e99 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -31,7 +31,6 @@ namespace so3 { void ExpmapFunctor::init(bool nearZeroApprox) { nearZero = nearZeroApprox || (theta2 <= std::numeric_limits::epsilon()); if (!nearZero) { - theta = std::sqrt(theta2); // rotation angle sin_theta = std::sin(theta); const double s2 = std::sin(theta / 2.0); one_minus_cos = 2.0 * s2 * s2; // numerically better than [1 - cos(theta)] @@ -39,7 +38,7 @@ void ExpmapFunctor::init(bool nearZeroApprox) { } ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox) - : theta2(omega.dot(omega)) { + : theta2(omega.dot(omega)), theta(std::sqrt(theta2)) { const double wx = omega.x(), wy = omega.y(), wz = omega.z(); W << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0; init(nearZeroApprox); @@ -50,7 +49,7 @@ ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox) } ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox) - : theta2(angle * angle) { + : theta2(angle * angle), theta(angle) { const double ax = axis.x(), ay = axis.y(), az = axis.z(); K << 0.0, -az, +ay, +az, 0.0, -ax, -ay, +ax, 0.0; W = K * angle; diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 6f0af3828..e468eaf55 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -103,6 +103,20 @@ Rot3 slow_but_correct_Rodrigues(const Vector& w) { return Rot3(R); } +/* ************************************************************************* */ +TEST( Rot3, AxisAngle) +{ + Vector axis = Vector3(0., 1., 0.); // rotation around Y + double angle = 3.14 / 4.0; + Rot3 expected(0.707388, 0, 0.706825, + 0, 1, 0, + -0.706825, 0, 0.707388); + Rot3 actual = Rot3::AxisAngle(axis, angle); + CHECK(assert_equal(expected,actual,1e-5)); + Rot3 actual2 = Rot3::AxisAngle(axis, angle-2*M_PI); + CHECK(assert_equal(expected,actual2,1e-5)); +} + /* ************************************************************************* */ TEST( Rot3, Rodrigues) { diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index 68e87d5ba..56751fa06 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -82,6 +82,34 @@ TEST(SO3, ChartDerivatives) { CHECK_CHART_DERIVATIVES(R2, R1); } +/* ************************************************************************* */ +TEST(SO3, Expmap) { + Vector axis = Vector3(0., 1., 0.); // rotation around Y + double angle = 3.14 / 4.0; + Matrix expected(3,3); + expected << 0.707388, 0, 0.706825, 0, 1, 0, -0.706825, 0, 0.707388; + + // axis angle version + so3::ExpmapFunctor f1(axis, angle); + SO3 actual1 = f1.expmap(); + CHECK(assert_equal(expected, actual1.matrix(), 1e-5)); + + // axis angle version, negative angle + so3::ExpmapFunctor f2(axis, angle - 2*M_PI); + SO3 actual2 = f2.expmap(); + CHECK(assert_equal(expected, actual2.matrix(), 1e-5)); + + // omega version + so3::ExpmapFunctor f3(axis * angle); + SO3 actual3 = f3.expmap(); + CHECK(assert_equal(expected, actual3.matrix(), 1e-5)); + + // omega version, negative angle + so3::ExpmapFunctor f4(axis * (angle - 2*M_PI)); + SO3 actual4 = f4.expmap(); + CHECK(assert_equal(expected, actual4.matrix(), 1e-5)); +} + /* ************************************************************************* */ namespace exmap_derivative { static const Vector3 w(0.1, 0.27, -0.2); diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h index b53743f06..1aaaff0e4 100644 --- a/gtsam/inference/Factor.h +++ b/gtsam/inference/Factor.h @@ -28,6 +28,9 @@ #include namespace gtsam { +/// Define collection types: +typedef FastVector FactorIndices; +typedef FastSet FactorIndexSet; /** * This is the base class for all factor types. It is templated on a KEY type, diff --git a/gtsam/inference/VariableIndex-inl.h b/gtsam/inference/VariableIndex-inl.h index 303043f13..bc8100e4a 100644 --- a/gtsam/inference/VariableIndex-inl.h +++ b/gtsam/inference/VariableIndex-inl.h @@ -25,7 +25,7 @@ namespace gtsam { /* ************************************************************************* */ template void VariableIndex::augment(const FG& factors, - boost::optional&> newFactorIndices) { + boost::optional newFactorIndices) { gttic(VariableIndex_augment); // Augment index for each factor diff --git a/gtsam/inference/VariableIndex.cpp b/gtsam/inference/VariableIndex.cpp index 72c56be5b..b71c81988 100644 --- a/gtsam/inference/VariableIndex.cpp +++ b/gtsam/inference/VariableIndex.cpp @@ -35,8 +35,8 @@ void VariableIndex::print(const string& str, const KeyFormatter& keyFormatter) c cout << "nEntries = " << nEntries() << ", nFactors = " << nFactors() << "\n"; for(KeyMap::value_type key_factors: index_) { cout << "var " << keyFormatter(key_factors.first) << ":"; - for(const size_t factor: key_factors.second) - cout << " " << factor; + for(const auto index: key_factors.second) + cout << " " << index; cout << "\n"; } cout.flush(); @@ -48,8 +48,8 @@ void VariableIndex::outputMetisFormat(ostream& os) const { // run over variables, which will be hyper-edges. for(KeyMap::value_type key_factors: index_) { // every variable is a hyper-edge covering its factors - for(const size_t factor: key_factors.second) - os << (factor+1) << " "; // base 1 + for(const auto index: key_factors.second) + os << (index+1) << " "; // base 1 os << "\n"; } os << flush; diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h index ad8d6720b..5f9e05cb0 100644 --- a/gtsam/inference/VariableIndex.h +++ b/gtsam/inference/VariableIndex.h @@ -17,6 +17,7 @@ #pragma once +#include #include #include #include @@ -43,7 +44,7 @@ class GTSAM_EXPORT VariableIndex { public: typedef boost::shared_ptr shared_ptr; - typedef FastVector Factors; + typedef FactorIndices Factors; typedef Factors::iterator Factor_iterator; typedef Factors::const_iterator Factor_const_iterator; @@ -63,7 +64,7 @@ public: /// @name Standard Constructors /// @{ - /** Default constructor, creates an empty VariableIndex */ + /// Default constructor, creates an empty VariableIndex VariableIndex() : nFactors_(0), nEntries_(0) {} /** @@ -77,19 +78,16 @@ public: /// @name Standard Interface /// @{ - /** - * The number of variable entries. This is one greater than the variable - * with the highest index. - */ + /// The number of variable entries. This is equal to the number of unique variable Keys. size_t size() const { return index_.size(); } - /** The number of factors in the original factor graph */ + /// The number of factors in the original factor graph size_t nFactors() const { return nFactors_; } - /** The number of nonzero blocks, i.e. the number of variable-factor entries */ + /// The number of nonzero blocks, i.e. the number of variable-factor entries size_t nEntries() const { return nEntries_; } - /** Access a list of factors by variable */ + /// Access a list of factors by variable const Factors& operator[](Key variable) const { KeyMap::const_iterator item = index_.find(variable); if(item == index_.end()) @@ -102,10 +100,10 @@ public: /// @name Testable /// @{ - /** Test for equality (for unit tests and debug assertions). */ + /// Test for equality (for unit tests and debug assertions). bool equals(const VariableIndex& other, double tol=0.0) const; - /** Print the variable index (for unit tests and debugging). */ + /// Print the variable index (for unit tests and debugging). void print(const std::string& str = "VariableIndex: ", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; @@ -125,7 +123,7 @@ public: * solving problems incrementally. */ template - void augment(const FG& factors, boost::optional&> newFactorIndices = boost::none); + void augment(const FG& factors, boost::optional newFactorIndices = boost::none); /** * Remove entries corresponding to the specified factors. NOTE: We intentionally do not decrement @@ -140,17 +138,17 @@ public: template void remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG& factors); - /** Remove unused empty variables (in debug mode verifies they are empty). */ + /// Remove unused empty variables (in debug mode verifies they are empty). template void removeUnusedVariables(ITERATOR firstKey, ITERATOR lastKey); - /** Iterator to the first variable entry */ + /// Iterator to the first variable entry const_iterator begin() const { return index_.begin(); } - /** Iterator to the first variable entry */ + /// Iterator to the first variable entry const_iterator end() const { return index_.end(); } - /** Find the iterator for the requested variable entry */ + /// Find the iterator for the requested variable entry const_iterator find(Key key) const { return index_.find(key); } protected: diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index 25672beaf..46d7b2264 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -138,23 +138,34 @@ namespace gtsam { //} /* ************************************************************************* */ - pair GaussianBayesNet::matrix() const { + Ordering GaussianBayesNet::ordering() const { GaussianFactorGraph factorGraph(*this); - KeySet keys = factorGraph.keys(); + auto keys = factorGraph.keys(); // add frontal keys in order Ordering ordering; - for (const sharedConditional& cg: *this) + for (const sharedConditional& cg : *this) if (cg) { - for (Key key: cg->frontals()) { + for (Key key : cg->frontals()) { ordering.push_back(key); keys.erase(key); } } // add remaining keys in case Bayes net is incomplete - for (Key key: keys) - ordering.push_back(key); - // return matrix and RHS - return factorGraph.jacobian(ordering); + for (Key key : keys) ordering.push_back(key); + return ordering; + } + + /* ************************************************************************* */ + pair GaussianBayesNet::matrix(boost::optional ordering) const { + if (ordering) { + // Convert to a GaussianFactorGraph and use its machinery + GaussianFactorGraph factorGraph(*this); + return factorGraph.jacobian(ordering); + } else { + // recursively call with default ordering + const auto defaultOrdering = this->ordering(); + return matrix(defaultOrdering); + } } ///* ************************************************************************* */ diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index 7cbb1a227..669c8a964 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -74,6 +74,14 @@ namespace gtsam { /// Version of optimize for incomplete BayesNet, needs solution for missing variables VectorValues optimize(const VectorValues& solutionForMissing) const; + /** + * Return ordering corresponding to a topological sort. + * There are many topological sorts of a Bayes net. This one + * corresponds to the one that makes 'matrix' below upper-triangular. + * In case Bayes net is incomplete any non-frontal are added to the end. + */ + Ordering ordering() const; + ///@} ///@name Linear Algebra @@ -81,8 +89,10 @@ namespace gtsam { /** * Return (dense) upper-triangular matrix representation + * Will return upper-triangular matrix only when using 'ordering' above. + * In case Bayes net is incomplete zero columns are added to the end. */ - std::pair matrix() const; + std::pair matrix(boost::optional ordering = boost::none) const; /** * Optimize along the gradient direction, with a closed-form computation to perform the line diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 93217c027..85569de14 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -134,7 +134,7 @@ namespace gtsam { VectorValues result; DenseIndex vectorPosition = 0; for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) { - result.insert(*frontal, solution.segment(vectorPosition, getDim(frontal))); + result.emplace(*frontal, solution.segment(vectorPosition, getDim(frontal))); vectorPosition += getDim(frontal); } @@ -162,7 +162,7 @@ namespace gtsam { VectorValues result; DenseIndex vectorPosition = 0; for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) { - result.insert(*frontal, soln.segment(vectorPosition, getDim(frontal))); + result.emplace(*frontal, soln.segment(vectorPosition, getDim(frontal))); vectorPosition += getDim(frontal); } @@ -172,13 +172,13 @@ namespace gtsam { /* ************************************************************************* */ void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const { Vector frontalVec = gy.vector(KeyVector(beginFrontals(), endFrontals())); - frontalVec = gtsam::backSubstituteUpper(frontalVec, Matrix(get_R())); + frontalVec = get_R().transpose().triangularView().solve(frontalVec); // Check for indeterminant solution if (frontalVec.hasNaN()) throw IndeterminantLinearSystemException(this->keys().front()); for (const_iterator it = beginParents(); it!= endParents(); it++) - gy[*it] += -1.0 * Matrix(getA(it)).transpose() * frontalVec; + gy[*it].noalias() += -1.0 * getA(it).transpose() * frontalVec; // Scale by sigmas if (model_) diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index a4df04cf9..9281c7e7a 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -61,7 +61,7 @@ namespace gtsam { for (GaussianFactor::const_iterator it = gf->begin(); it != gf->end(); it++) { map::iterator it2 = spec.find(*it); if ( it2 == spec.end() ) { - spec.insert(make_pair(*it, gf->getDim(it))); + spec.emplace(*it, gf->getDim(it)); } } } @@ -246,7 +246,7 @@ namespace gtsam { if (blocks.count(j)) blocks[j] += Bj; else - blocks.insert(make_pair(j,Bj)); + blocks.emplace(j,Bj); } } return blocks; diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index cded6d2ee..da7d7bd7b 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -305,7 +305,7 @@ Matrix HessianFactor::information() const { VectorValues HessianFactor::hessianDiagonal() const { VectorValues d; for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) { - d.insert(keys_[j], info_.diagonal(j)); + d.emplace(keys_[j], info_.diagonal(j)); } return d; } @@ -436,7 +436,7 @@ VectorValues HessianFactor::gradientAtZero() const { VectorValues g; size_t n = size(); for (size_t j = 0; j < n; ++j) - g.insert(keys_[j], -info_.aboveDiagonalBlock(j, n)); + g.emplace(keys_[j], -info_.aboveDiagonalBlock(j, n)); return g; } @@ -513,7 +513,7 @@ VectorValues HessianFactor::solve() { std::size_t offset = 0; for (DenseIndex j = 0; j < (DenseIndex)n; ++j) { const DenseIndex dim = info_.getDim(j); - delta.insert(keys_[j], solution.segment(offset, dim)); + delta.emplace(keys_[j], solution.segment(offset, dim)); offset += dim; } diff --git a/gtsam/linear/IterativeSolver.cpp b/gtsam/linear/IterativeSolver.cpp index 411feb7a9..c7d4e5405 100644 --- a/gtsam/linear/IterativeSolver.cpp +++ b/gtsam/linear/IterativeSolver.cpp @@ -117,7 +117,7 @@ void KeyInfo::initialize(const GaussianFactorGraph &fg) { for (size_t i = 0; i < n; ++i) { const size_t key = ordering_[i]; const size_t dim = colspec.find(key)->second; - insert(make_pair(key, KeyInfoEntry(i, dim, start))); + this->emplace(key, KeyInfoEntry(i, dim, start)); start += dim; } numCols_ = start; @@ -126,8 +126,8 @@ void KeyInfo::initialize(const GaussianFactorGraph &fg) { /****************************************************************************/ vector KeyInfo::colSpec() const { std::vector result(size(), 0); - for ( const KeyInfo::value_type &item: *this ) { - result[item.second.index()] = item.second.dim(); + for ( const auto &item: *this ) { + result[item.second.index] = item.second.dim; } return result; } @@ -135,8 +135,8 @@ vector KeyInfo::colSpec() const { /****************************************************************************/ VectorValues KeyInfo::x0() const { VectorValues result; - for ( const KeyInfo::value_type &item: *this ) { - result.insert(item.first, Vector::Zero(item.second.dim())); + for ( const auto &item: *this ) { + result.emplace(item.first, Vector::Zero(item.second.dim)); } return result; } diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index 92dcbfa07..f0fbfbfd2 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -32,8 +32,8 @@ namespace gtsam { // Forward declarations +struct KeyInfoEntry; class KeyInfo; -class KeyInfoEntry; class GaussianFactorGraph; class Values; class VectorValues; @@ -109,27 +109,14 @@ public: /** * Handy data structure for iterative solvers - * key to (index, dimension, colstart) + * key to (index, dimension, start) */ -class GTSAM_EXPORT KeyInfoEntry: public boost::tuple { - -public: - - typedef boost::tuple Base; - +struct GTSAM_EXPORT KeyInfoEntry { + size_t index, dim, start; KeyInfoEntry() { } KeyInfoEntry(size_t idx, size_t d, Key start) : - Base(idx, d, start) { - } - size_t index() const { - return this->get<0>(); - } - size_t dim() const { - return this->get<1>(); - } - size_t colstart() const { - return this->get<2>(); + index(idx), dim(d), start(start) { } }; diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 1f5e5557c..06f6b3bed 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -102,10 +102,9 @@ JacobianFactor::JacobianFactor(const Key i1, const Matrix& A1, Key i2, } /* ************************************************************************* */ -JacobianFactor::JacobianFactor(const HessianFactor& factor) : - Base(factor), Ab_( - VerticalBlockMatrix::LikeActiveViewOf(factor.info(), - factor.rows())) { +JacobianFactor::JacobianFactor(const HessianFactor& factor) + : Base(factor), + Ab_(VerticalBlockMatrix::LikeActiveViewOf(factor.info(), factor.rows())) { // Copy Hessian into our matrix and then do in-place Cholesky Ab_.full() = factor.info().selfadjointView(); @@ -114,16 +113,19 @@ JacobianFactor::JacobianFactor(const HessianFactor& factor) : bool success; boost::tie(maxrank, success) = choleskyCareful(Ab_.matrix()); - // Check for indefinite system - if (!success) + // Check that Cholesky succeeded OR it managed to factor the full Hessian. + // THe latter case occurs with non-positive definite matrices arising from QP. + if (success || maxrank == factor.rows() - 1) { + // Zero out lower triangle + Ab_.matrix().topRows(maxrank).triangularView() = + Matrix::Zero(maxrank, Ab_.matrix().cols()); + // FIXME: replace with triangular system + Ab_.rowEnd() = maxrank; + model_ = SharedDiagonal(); // is equivalent to Unit::Create(maxrank) + } else { + // indefinite system throw IndeterminantLinearSystemException(factor.keys().front()); - - // Zero out lower triangle - Ab_.matrix().topRows(maxrank).triangularView() = - Matrix::Zero(maxrank, Ab_.matrix().cols()); - // FIXME: replace with triangular system - Ab_.rowEnd() = maxrank; - model_ = SharedDiagonal(); // should be same as Unit::Create(maxrank); + } } /* ************************************************************************* */ @@ -164,9 +166,10 @@ boost::tuple, DenseIndex, DenseIndex> _countDims( n += vardim; } else { if(!(varDims[jointVarpos] == vardim)) { - cout << "Factor " << sourceFactorI << " variable " << DefaultKeyFormatter(sourceFactor.keys()[sourceVarpos]) << - " has different dimensionality of " << vardim << " instead of " << varDims[jointVarpos] << endl; - exit(1); + std::stringstream ss; + ss << "Factor " << sourceFactorI << " variable " << DefaultKeyFormatter(sourceFactor.keys()[sourceVarpos]) << + " has different dimensionality of " << vardim << " instead of " << varDims[jointVarpos]; + throw std::runtime_error(ss.str()); } } #else @@ -429,10 +432,9 @@ Vector JacobianFactor::unweighted_error(const VectorValues& c) const { /* ************************************************************************* */ Vector JacobianFactor::error_vector(const VectorValues& c) const { - if (model_) - return model_->whiten(unweighted_error(c)); - else - return unweighted_error(c); + Vector e = unweighted_error(c); + if (model_) model_->whitenInPlace(e); + return e; } /* ************************************************************************* */ @@ -473,10 +475,10 @@ VectorValues JacobianFactor::hessianDiagonal() const { for (size_t k = 0; k < nj; ++k) { Vector column_k = Ab_(pos).col(k); if (model_) - column_k = model_->whiten(column_k); + model_->whitenInPlace(column_k); dj(k) = dot(column_k, column_k); } - d.insert(j, dj); + d.emplace(j, dj); } return d; } @@ -495,7 +497,7 @@ map JacobianFactor::hessianBlockDiagonal() const { Matrix Aj = Ab_(pos); if (model_) Aj = model_->Whiten(Aj); - blocks.insert(make_pair(j, Aj.transpose() * Aj)); + blocks.emplace(j, Aj.transpose() * Aj); } return blocks; } @@ -540,29 +542,38 @@ void JacobianFactor::updateHessian(const KeyVector& infoKeys, /* ************************************************************************* */ Vector JacobianFactor::operator*(const VectorValues& x) const { - Vector Ax = Vector::Zero(Ab_.rows()); + Vector Ax(Ab_.rows()); + Ax.setZero(); if (empty()) return Ax; // Just iterate over all A matrices and multiply in correct config part - for (size_t pos = 0; pos < size(); ++pos) - Ax += Ab_(pos) * x[keys_[pos]]; + for (size_t pos = 0; pos < size(); ++pos) { + // http://eigen.tuxfamily.org/dox/TopicWritingEfficientProductExpression.html + Ax.noalias() += Ab_(pos) * x[keys_[pos]]; + } - return model_ ? model_->whiten(Ax) : Ax; + if (model_) model_->whitenInPlace(Ax); + return Ax; } /* ************************************************************************* */ void JacobianFactor::transposeMultiplyAdd(double alpha, const Vector& e, - VectorValues& x) const { - Vector E = alpha * (model_ ? model_->whiten(e) : e); + VectorValues& x) const { + Vector E(e.size()); + E.noalias() = alpha * e; + if (model_) model_->whitenInPlace(E); // Just iterate over all A matrices and insert Ai^e into VectorValues for (size_t pos = 0; pos < size(); ++pos) { - Key j = keys_[pos]; - // Create the value as a zero vector if it does not exist. - pair xi = x.tryInsert(j, Vector()); - if (xi.second) - xi.first->second = Vector::Zero(getDim(begin() + pos)); - xi.first->second += Ab_(pos).transpose()*E; + const Key j = keys_[pos]; + // To avoid another malloc if key exists, we explicitly check + auto it = x.find(j); + if (it != x.end()) { + // http://eigen.tuxfamily.org/dox/TopicWritingEfficientProductExpression.html + it->second.noalias() += Ab_(pos).transpose() * E; + } else { + x.emplace(j, Ab_(pos).transpose() * E); + } } } @@ -624,8 +635,8 @@ VectorValues JacobianFactor::gradientAtZero() const { Vector b = getb(); // Gradient is really -A'*b / sigma^2 // transposeMultiplyAdd will divide by sigma once, so we need one more - Vector b_sigma = model_ ? model_->whiten(b) : b; - this->transposeMultiplyAdd(-1.0, b_sigma, g); // g -= A'*b/sigma^2 + if (model_) model_->whitenInPlace(b); + this->transposeMultiplyAdd(-1.0, b, g); // g -= A'*b/sigma^2 return g; } diff --git a/gtsam/linear/PCGSolver.cpp b/gtsam/linear/PCGSolver.cpp index f9ef756f1..08307c5ab 100644 --- a/gtsam/linear/PCGSolver.cpp +++ b/gtsam/linear/PCGSolver.cpp @@ -89,7 +89,7 @@ void GaussianFactorGraphSystem::multiply(const Vector &x, Vector& AtAx) const { VectorValues vvX = buildVectorValues(x, keyInfo_); // VectorValues form of A'Ax for multiplyHessianAdd - VectorValues vvAtAx; + VectorValues vvAtAx = keyInfo_.x0(); // crucial for performance // vvAtAx += 1.0 * A'Ax for each factor gfg_.multiplyHessianAdd(1.0, vvX, vvAtAx); @@ -132,14 +132,14 @@ VectorValues buildVectorValues(const Vector &v, const Ordering &ordering, DenseIndex offset = 0; for (size_t i = 0; i < ordering.size(); ++i) { - const Key &key = ordering[i]; + const Key key = ordering[i]; map::const_iterator it = dimensions.find(key); if (it == dimensions.end()) { throw invalid_argument( "buildVectorValues: inconsistent ordering and dimensions"); } const size_t dim = it->second; - result.insert(key, v.segment(offset, dim)); + result.emplace(key, v.segment(offset, dim)); offset += dim; } @@ -150,8 +150,7 @@ VectorValues buildVectorValues(const Vector &v, const Ordering &ordering, VectorValues buildVectorValues(const Vector &v, const KeyInfo &keyInfo) { VectorValues result; for ( const KeyInfo::value_type &item: keyInfo ) { - result.insert(item.first, - v.segment(item.second.colstart(), item.second.dim())); + result.emplace(item.first, v.segment(item.second.start, item.second.dim)); } return result; } diff --git a/gtsam/linear/Preconditioner.h b/gtsam/linear/Preconditioner.h index 623b29863..31901db3f 100644 --- a/gtsam/linear/Preconditioner.h +++ b/gtsam/linear/Preconditioner.h @@ -70,21 +70,20 @@ public: Preconditioner() {} virtual ~Preconditioner() {} - /* Computation Interfaces */ + /* + * Abstract interface for raw vectors. VectorValues is a speed bottleneck + * and Yong-Dian has profiled preconditioners (outside GTSAM) with the the + * three methods below. In GTSAM, unfortunately, we are still using the + * VectorValues methods called in iterative-inl.h + */ - /* implement x = L^{-1} y */ + /// implement x = L^{-1} y virtual void solve(const Vector& y, Vector &x) const = 0; -// virtual void solve(const VectorValues& y, VectorValues &x) const = 0; - /* implement x = L^{-T} y */ + /// implement x = L^{-T} y virtual void transposeSolve(const Vector& y, Vector& x) const = 0; -// virtual void transposeSolve(const VectorValues& y, VectorValues &x) const = 0; -// /* implement x = L^{-1} L^{-T} y */ -// virtual void fullSolve(const Vector& y, Vector &x) const = 0; -// virtual void fullSolve(const VectorValues& y, VectorValues &x) const = 0; - - /* build/factorize the preconditioner */ + /// build/factorize the preconditioner virtual void build( const GaussianFactorGraph &gfg, const KeyInfo &info, @@ -113,14 +112,7 @@ public: /* Computation Interfaces for raw vector */ virtual void solve(const Vector& y, Vector &x) const { x = y; } -// virtual void solve(const VectorValues& y, VectorValues& x) const { x = y; } - virtual void transposeSolve(const Vector& y, Vector& x) const { x = y; } -// virtual void transposeSolve(const VectorValues& y, VectorValues& x) const { x = y; } - -// virtual void fullSolve(const Vector& y, Vector &x) const { x = y; } -// virtual void fullSolve(const VectorValues& y, VectorValues& x) const { x = y; } - virtual void build( const GaussianFactorGraph &gfg, const KeyInfo &info, @@ -145,8 +137,6 @@ public: /* Computation Interfaces for raw vector */ virtual void solve(const Vector& y, Vector &x) const; virtual void transposeSolve(const Vector& y, Vector& x) const ; -// virtual void fullSolve(const Vector& y, Vector &x) const ; - virtual void build( const GaussianFactorGraph &gfg, const KeyInfo &info, diff --git a/gtsam/linear/SubgraphBuilder.cpp b/gtsam/linear/SubgraphBuilder.cpp new file mode 100644 index 000000000..22ad89cd8 --- /dev/null +++ b/gtsam/linear/SubgraphBuilder.cpp @@ -0,0 +1,545 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010-2019, 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 SubgraphBuilder.cpp + * @date Dec 31, 2009 + * @author Frank Dellaert, Yong-Dian Jian + */ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include // accumulate +#include +#include +#include +#include + +using std::cout; +using std::endl; +using std::ostream; +using std::vector; + +namespace gtsam { + +/*****************************************************************************/ +/* sort the container and return permutation index with default comparator */ +template +static vector sort_idx(const Container &src) { + typedef typename Container::value_type T; + const size_t n = src.size(); + vector > tmp; + tmp.reserve(n); + for (size_t i = 0; i < n; i++) tmp.emplace_back(i, src[i]); + + /* sort */ + std::stable_sort(tmp.begin(), tmp.end()); + + /* copy back */ + vector idx; + idx.reserve(n); + for (size_t i = 0; i < n; i++) { + idx.push_back(tmp[i].first); + } + return idx; +} + +/*****************************************************************************/ +static vector iidSampler(const vector &weight, const size_t n) { + /* compute the sum of the weights */ + const double sum = std::accumulate(weight.begin(), weight.end(), 0.0); + if (sum == 0.0) { + throw std::runtime_error("Weight vector has no non-zero weights"); + } + + /* make a normalized and accumulated version of the weight vector */ + const size_t m = weight.size(); + vector cdf; + cdf.reserve(m); + for (size_t i = 0; i < m; ++i) { + cdf.push_back(weight[i] / sum); + } + + vector acc(m); + std::partial_sum(cdf.begin(), cdf.end(), acc.begin()); + + /* iid sample n times */ + vector samples; + samples.reserve(n); + const double denominator = (double)RAND_MAX; + for (size_t i = 0; i < n; ++i) { + const double value = rand() / denominator; + /* binary search the interval containing "value" */ + const auto it = std::lower_bound(acc.begin(), acc.end(), value); + const size_t index = it - acc.begin(); + samples.push_back(index); + } + return samples; +} + +/*****************************************************************************/ +static vector UniqueSampler(const vector &weight, + const size_t n) { + const size_t m = weight.size(); + if (n > m) throw std::invalid_argument("UniqueSampler: invalid input size"); + + vector samples; + + size_t count = 0; + vector touched(m, false); + while (count < n) { + vector localIndices; + localIndices.reserve(n - count); + vector localWeights; + localWeights.reserve(n - count); + + /* collect data */ + for (size_t i = 0; i < m; ++i) { + if (!touched[i]) { + localIndices.push_back(i); + localWeights.push_back(weight[i]); + } + } + + /* sampling and cache results */ + vector samples = iidSampler(localWeights, n - count); + for (const size_t &index : samples) { + if (touched[index] == false) { + touched[index] = true; + samples.push_back(index); + if (++count >= n) break; + } + } + } + return samples; +} + +/****************************************************************************/ +Subgraph::Subgraph(const vector &indices) { + edges_.reserve(indices.size()); + for (const size_t &index : indices) { + const Edge e {index,1.0}; + edges_.push_back(e); + } +} + +/****************************************************************************/ +vector Subgraph::edgeIndices() const { + vector eid; + eid.reserve(size()); + for (const Edge &edge : edges_) { + eid.push_back(edge.index); + } + return eid; +} + +/****************************************************************************/ +void Subgraph::save(const std::string &fn) const { + std::ofstream os(fn.c_str()); + boost::archive::text_oarchive oa(os); + oa << *this; + os.close(); +} + +/****************************************************************************/ +Subgraph Subgraph::load(const std::string &fn) { + std::ifstream is(fn.c_str()); + boost::archive::text_iarchive ia(is); + Subgraph subgraph; + ia >> subgraph; + is.close(); + return subgraph; +} + +/****************************************************************************/ +ostream &operator<<(ostream &os, const Subgraph::Edge &edge) { + if (edge.weight != 1.0) + os << edge.index << "(" << std::setprecision(2) << edge.weight << ")"; + else + os << edge.index; + return os; +} + +/****************************************************************************/ +ostream &operator<<(ostream &os, const Subgraph &subgraph) { + os << "Subgraph" << endl; + for (const auto &e : subgraph.edges()) { + os << e << ", "; + } + return os; +} + +/*****************************************************************************/ +void SubgraphBuilderParameters::print() const { print(cout); } + +/***************************************************************************************/ +void SubgraphBuilderParameters::print(ostream &os) const { + os << "SubgraphBuilderParameters" << endl + << "skeleton: " << skeletonTranslator(skeletonType) << endl + << "skeleton weight: " << skeletonWeightTranslator(skeletonWeight) + << endl + << "augmentation weight: " + << augmentationWeightTranslator(augmentationWeight) << endl; +} + +/*****************************************************************************/ +ostream &operator<<(ostream &os, const SubgraphBuilderParameters &p) { + p.print(os); + return os; +} + +/*****************************************************************************/ +SubgraphBuilderParameters::Skeleton +SubgraphBuilderParameters::skeletonTranslator(const std::string &src) { + std::string s = src; + boost::algorithm::to_upper(s); + if (s == "NATURALCHAIN") + return NATURALCHAIN; + else if (s == "BFS") + return BFS; + else if (s == "KRUSKAL") + return KRUSKAL; + throw std::invalid_argument( + "SubgraphBuilderParameters::skeletonTranslator undefined string " + s); + return KRUSKAL; +} + +/****************************************************************/ +std::string SubgraphBuilderParameters::skeletonTranslator(Skeleton s) { + if (s == NATURALCHAIN) + return "NATURALCHAIN"; + else if (s == BFS) + return "BFS"; + else if (s == KRUSKAL) + return "KRUSKAL"; + else + return "UNKNOWN"; +} + +/****************************************************************/ +SubgraphBuilderParameters::SkeletonWeight +SubgraphBuilderParameters::skeletonWeightTranslator(const std::string &src) { + std::string s = src; + boost::algorithm::to_upper(s); + if (s == "EQUAL") + return EQUAL; + else if (s == "RHS") + return RHS_2NORM; + else if (s == "LHS") + return LHS_FNORM; + else if (s == "RANDOM") + return RANDOM; + throw std::invalid_argument( + "SubgraphBuilderParameters::skeletonWeightTranslator undefined string " + + s); + return EQUAL; +} + +/****************************************************************/ +std::string SubgraphBuilderParameters::skeletonWeightTranslator( + SkeletonWeight w) { + if (w == EQUAL) + return "EQUAL"; + else if (w == RHS_2NORM) + return "RHS"; + else if (w == LHS_FNORM) + return "LHS"; + else if (w == RANDOM) + return "RANDOM"; + else + return "UNKNOWN"; +} + +/****************************************************************/ +SubgraphBuilderParameters::AugmentationWeight +SubgraphBuilderParameters::augmentationWeightTranslator( + const std::string &src) { + std::string s = src; + boost::algorithm::to_upper(s); + if (s == "SKELETON") return SKELETON; + // else if (s == "STRETCH") return STRETCH; + // else if (s == "GENERALIZED_STRETCH") return GENERALIZED_STRETCH; + throw std::invalid_argument( + "SubgraphBuilder::Parameters::augmentationWeightTranslator undefined " + "string " + + s); + return SKELETON; +} + +/****************************************************************/ +std::string SubgraphBuilderParameters::augmentationWeightTranslator( + AugmentationWeight w) { + if (w == SKELETON) return "SKELETON"; + // else if ( w == STRETCH ) return "STRETCH"; + // else if ( w == GENERALIZED_STRETCH ) return "GENERALIZED_STRETCH"; + else + return "UNKNOWN"; +} + +/****************************************************************/ +vector SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg, + const FastMap &ordering, + const vector &weights) const { + const SubgraphBuilderParameters &p = parameters_; + switch (p.skeletonType) { + case SubgraphBuilderParameters::NATURALCHAIN: + return natural_chain(gfg); + break; + case SubgraphBuilderParameters::BFS: + return bfs(gfg); + break; + case SubgraphBuilderParameters::KRUSKAL: + return kruskal(gfg, ordering, weights); + break; + default: + std::cerr << "SubgraphBuilder::buildTree undefined skeleton type" << endl; + break; + } + return vector(); +} + +/****************************************************************/ +vector SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const { + vector unaryFactorIndices; + size_t index = 0; + for (const auto &factor : gfg) { + if (factor->size() == 1) { + unaryFactorIndices.push_back(index); + } + index++; + } + return unaryFactorIndices; +} + +/****************************************************************/ +vector SubgraphBuilder::natural_chain( + const GaussianFactorGraph &gfg) const { + vector chainFactorIndices; + size_t index = 0; + for (const GaussianFactor::shared_ptr &gf : gfg) { + if (gf->size() == 2) { + const Key k0 = gf->keys()[0], k1 = gf->keys()[1]; + if ((k1 - k0) == 1 || (k0 - k1) == 1) chainFactorIndices.push_back(index); + } + index++; + } + return chainFactorIndices; +} + +/****************************************************************/ +vector SubgraphBuilder::bfs(const GaussianFactorGraph &gfg) const { + const VariableIndex variableIndex(gfg); + /* start from the first key of the first factor */ + Key seed = gfg[0]->keys()[0]; + + const size_t n = variableIndex.size(); + + /* each vertex has self as the predecessor */ + vector bfsFactorIndices; + bfsFactorIndices.reserve(n - 1); + + /* Initialize */ + std::queue q; + q.push(seed); + + std::set flags; + flags.insert(seed); + + /* traversal */ + while (!q.empty()) { + const size_t head = q.front(); + q.pop(); + for (const size_t index : variableIndex[head]) { + const GaussianFactor &gf = *gfg[index]; + for (const size_t key : gf.keys()) { + if (flags.count(key) == 0) { + q.push(key); + flags.insert(key); + bfsFactorIndices.push_back(index); + } + } + } + } + return bfsFactorIndices; +} + +/****************************************************************/ +vector SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg, + const FastMap &ordering, + const vector &weights) const { + const VariableIndex variableIndex(gfg); + const size_t n = variableIndex.size(); + const vector sortedIndices = sort_idx(weights); + + /* initialize buffer */ + vector treeIndices; + treeIndices.reserve(n - 1); + + // container for acsendingly sorted edges + DSFVector dsf(n); + + size_t count = 0; + double sum = 0.0; + for (const size_t index : sortedIndices) { + const GaussianFactor &gf = *gfg[index]; + const auto keys = gf.keys(); + if (keys.size() != 2) continue; + const size_t u = ordering.find(keys[0])->second, + v = ordering.find(keys[1])->second; + if (dsf.find(u) != dsf.find(v)) { + dsf.merge(u, v); + treeIndices.push_back(index); + sum += weights[index]; + if (++count == n - 1) break; + } + } + return treeIndices; +} + +/****************************************************************/ +vector SubgraphBuilder::sample(const vector &weights, + const size_t t) const { + return UniqueSampler(weights, t); +} + +/****************************************************************/ +Subgraph SubgraphBuilder::operator()( + const GaussianFactorGraph &gfg) const { + const auto &p = parameters_; + const auto inverse_ordering = Ordering::Natural(gfg); + const FastMap forward_ordering = inverse_ordering.invert(); + const size_t n = inverse_ordering.size(), m = gfg.size(); + + // Make sure the subgraph preconditioner does not include more than half of + // the edges beyond the spanning tree, or we might as well solve the whole + // thing. + size_t numExtraEdges = n * p.augmentationFactor; + const size_t numRemaining = m - (n - 1); + numExtraEdges = std::min(numExtraEdges, numRemaining / 2); + + // Calculate weights + vector weights = this->weights(gfg); + + // Build spanning tree. + const vector tree = buildTree(gfg, forward_ordering, weights); + if (tree.size() != n - 1) { + throw std::runtime_error( + "SubgraphBuilder::operator() failure: tree.size() != n-1"); + } + + // Downweight the tree edges to zero. + for (const size_t index : tree) { + weights[index] = 0.0; + } + + /* decide how many edges to augment */ + vector offTree = sample(weights, numExtraEdges); + + vector subgraph = unary(gfg); + subgraph.insert(subgraph.end(), tree.begin(), tree.end()); + subgraph.insert(subgraph.end(), offTree.begin(), offTree.end()); + + return Subgraph(subgraph); +} + +/****************************************************************/ +SubgraphBuilder::Weights SubgraphBuilder::weights( + const GaussianFactorGraph &gfg) const { + const size_t m = gfg.size(); + Weights weight; + weight.reserve(m); + + for (const GaussianFactor::shared_ptr &gf : gfg) { + switch (parameters_.skeletonWeight) { + case SubgraphBuilderParameters::EQUAL: + weight.push_back(1.0); + break; + case SubgraphBuilderParameters::RHS_2NORM: { + if (JacobianFactor::shared_ptr jf = + boost::dynamic_pointer_cast(gf)) { + weight.push_back(jf->getb().norm()); + } else if (HessianFactor::shared_ptr hf = + boost::dynamic_pointer_cast(gf)) { + weight.push_back(hf->linearTerm().norm()); + } + } break; + case SubgraphBuilderParameters::LHS_FNORM: { + if (JacobianFactor::shared_ptr jf = + boost::dynamic_pointer_cast(gf)) { + weight.push_back(std::sqrt(jf->getA().squaredNorm())); + } else if (HessianFactor::shared_ptr hf = + boost::dynamic_pointer_cast(gf)) { + weight.push_back(std::sqrt(hf->information().squaredNorm())); + } + } break; + + case SubgraphBuilderParameters::RANDOM: + weight.push_back(std::rand() % 100 + 1.0); + break; + + default: + throw std::invalid_argument( + "SubgraphBuilder::weights: undefined weight scheme "); + break; + } + } + return weight; +} + +/*****************************************************************************/ +GaussianFactorGraph::shared_ptr buildFactorSubgraph( + const GaussianFactorGraph &gfg, const Subgraph &subgraph, + const bool clone) { + auto subgraphFactors = boost::make_shared(); + subgraphFactors->reserve(subgraph.size()); + for (const auto &e : subgraph) { + const auto factor = gfg[e.index]; + subgraphFactors->push_back(clone ? factor->clone(): factor); + } + return subgraphFactors; +} + +/**************************************************************************************************/ +std::pair // +splitFactorGraph(const GaussianFactorGraph &factorGraph, const Subgraph &subgraph) { + + // Get the subgraph by calling cheaper method + auto subgraphFactors = buildFactorSubgraph(factorGraph, subgraph, false); + + // Now, copy all factors then set subGraph factors to zero + auto remaining = boost::make_shared(factorGraph); + + for (const auto &e : subgraph) { + remaining->remove(e.index); + } + + return std::make_pair(subgraphFactors, remaining); +} + +/*****************************************************************************/ + +} // namespace gtsam diff --git a/gtsam/linear/SubgraphBuilder.h b/gtsam/linear/SubgraphBuilder.h new file mode 100644 index 000000000..5247ea2a2 --- /dev/null +++ b/gtsam/linear/SubgraphBuilder.h @@ -0,0 +1,182 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010-2019, 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 SubgraphBuilder.h + * @date Dec 31, 2009 + * @author Frank Dellaert, Yong-Dian Jian + */ + +#pragma once + +#include +#include +#include + +#include +#include + +#include + +namespace boost { +namespace serialization { +class access; +} /* namespace serialization */ +} /* namespace boost */ + +namespace gtsam { + +// Forward declarations +class GaussianFactorGraph; +struct PreconditionerParameters; + +/**************************************************************************/ +class GTSAM_EXPORT Subgraph { + public: + struct GTSAM_EXPORT Edge { + size_t index; /* edge id */ + double weight; /* edge weight */ + inline bool isUnitWeight() const { return (weight == 1.0); } + friend std::ostream &operator<<(std::ostream &os, const Edge &edge); + + private: + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int /*version*/) { + ar &BOOST_SERIALIZATION_NVP(index); + ar &BOOST_SERIALIZATION_NVP(weight); + } + }; + + typedef std::vector Edges; + typedef std::vector EdgeIndices; + typedef Edges::iterator iterator; + typedef Edges::const_iterator const_iterator; + + protected: + Edges edges_; /* index to the factors */ + + public: + Subgraph() {} + Subgraph(const Subgraph &subgraph) : edges_(subgraph.edges()) {} + Subgraph(const Edges &edges) : edges_(edges) {} + Subgraph(const std::vector &indices); + + inline const Edges &edges() const { return edges_; } + inline size_t size() const { return edges_.size(); } + EdgeIndices edgeIndices() const; + + iterator begin() { return edges_.begin(); } + const_iterator begin() const { return edges_.begin(); } + iterator end() { return edges_.end(); } + const_iterator end() const { return edges_.end(); } + + void save(const std::string &fn) const; + static Subgraph load(const std::string &fn); + friend std::ostream &operator<<(std::ostream &os, const Subgraph &subgraph); + + private: + friend class boost::serialization::access; + template + void serialize(Archive &ar, const unsigned int /*version*/) { + ar &BOOST_SERIALIZATION_NVP(edges_); + } +}; + +/****************************************************************************/ +struct GTSAM_EXPORT SubgraphBuilderParameters { + typedef boost::shared_ptr shared_ptr; + + enum Skeleton { + /* augmented tree */ + NATURALCHAIN = 0, /* natural ordering of the graph */ + BFS, /* breadth-first search tree */ + KRUSKAL, /* maximum weighted spanning tree */ + } skeletonType; + + enum SkeletonWeight { /* how to weigh the graph edges */ + EQUAL = 0, /* every block edge has equal weight */ + RHS_2NORM, /* use the 2-norm of the rhs */ + LHS_FNORM, /* use the frobenius norm of the lhs */ + RANDOM, /* bounded random edge weight */ + } skeletonWeight; + + enum AugmentationWeight { /* how to weigh the graph edges */ + SKELETON = 0, /* use the same weights in building + the skeleton */ + // STRETCH, /* stretch in the + // laplacian sense */ GENERALIZED_STRETCH /* + // the generalized stretch defined in + // jian2013iros */ + } augmentationWeight; + + /// factor multiplied with n, yields number of extra edges. + double augmentationFactor; + + SubgraphBuilderParameters() + : skeletonType(KRUSKAL), + skeletonWeight(RANDOM), + augmentationWeight(SKELETON), + augmentationFactor(1.0) {} + virtual ~SubgraphBuilderParameters() {} + + /* for serialization */ + void print() const; + virtual void print(std::ostream &os) const; + friend std::ostream &operator<<(std::ostream &os, + const PreconditionerParameters &p); + + static Skeleton skeletonTranslator(const std::string &s); + static std::string skeletonTranslator(Skeleton s); + static SkeletonWeight skeletonWeightTranslator(const std::string &s); + static std::string skeletonWeightTranslator(SkeletonWeight w); + static AugmentationWeight augmentationWeightTranslator(const std::string &s); + static std::string augmentationWeightTranslator(AugmentationWeight w); +}; + +/*****************************************************************************/ +class GTSAM_EXPORT SubgraphBuilder { + public: + typedef SubgraphBuilder Base; + typedef std::vector Weights; + + SubgraphBuilder( + const SubgraphBuilderParameters &p = SubgraphBuilderParameters()) + : parameters_(p) {} + virtual ~SubgraphBuilder() {} + virtual Subgraph operator()(const GaussianFactorGraph &jfg) const; + + private: + std::vector buildTree(const GaussianFactorGraph &gfg, + const FastMap &ordering, + const std::vector &weights) const; + std::vector unary(const GaussianFactorGraph &gfg) const; + std::vector natural_chain(const GaussianFactorGraph &gfg) const; + std::vector bfs(const GaussianFactorGraph &gfg) const; + std::vector kruskal(const GaussianFactorGraph &gfg, + const FastMap &ordering, + const std::vector &weights) const; + std::vector sample(const std::vector &weights, + const size_t t) const; + Weights weights(const GaussianFactorGraph &gfg) const; + SubgraphBuilderParameters parameters_; +}; + +/** Select the factors in a factor graph according to the subgraph. */ +boost::shared_ptr buildFactorSubgraph( + const GaussianFactorGraph &gfg, const Subgraph &subgraph, const bool clone); + +/** Split the graph into a subgraph and the remaining edges. + * Note that the remaining factorgraph has null factors. */ +std::pair, boost::shared_ptr > +splitFactorGraph(const GaussianFactorGraph &factorGraph, const Subgraph &subgraph); + +} // namespace gtsam diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index d796e28b7..d74669c07 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -12,451 +12,85 @@ /** * @file SubgraphPreconditioner.cpp * @date Dec 31, 2009 - * @author: Frank Dellaert + * @author Frank Dellaert, Yong-Dian Jian */ #include -#include -#include + +#include +#include +#include #include -#include -#include -#include -#include -#include -#include #include #include #include -#include -#include -#include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include // accumulate -#include -#include #include -#include -#include -#include -using namespace std; +using std::cout; +using std::endl; +using std::vector; +using std::ostream; namespace gtsam { /* ************************************************************************* */ -static GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) { - GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph()); - for(const GaussianFactor::shared_ptr &gf: gfg) { - JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(gf); - if( !jf ) { - jf = boost::make_shared(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) - } - result->push_back(jf); +static Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, + const KeyVector &keys) { + /* a cache of starting index and dim */ + vector > cache; + cache.reserve(3); + + /* figure out dimension by traversing the keys */ + size_t dim = 0; + for (const Key &key : keys) { + const KeyInfoEntry &entry = keyInfo.find(key)->second; + cache.emplace_back(entry.start, entry.dim); + dim += entry.dim; } + + /* use the cache to fill the result */ + Vector result(dim); + size_t index = 0; + for (const auto &p : cache) { + result.segment(index, p.second) = src.segment(p.first, p.second); + index += p.second; + } + return result; } /*****************************************************************************/ -static std::vector iidSampler(const vector &weight, const size_t n) { - - /* compute the sum of the weights */ - const double sum = std::accumulate(weight.begin(), weight.end(), 0.0); - - /* make a normalized and accumulated version of the weight vector */ - const size_t m = weight.size(); - vector w; w.reserve(m); - for ( size_t i = 0 ; i < m ; ++i ) { - w.push_back(weight[i]/sum); +static void setSubvector(const Vector &src, const KeyInfo &keyInfo, + const KeyVector &keys, Vector &dst) { + size_t index = 0; + for (const Key &key : keys) { + const KeyInfoEntry &entry = keyInfo.find(key)->second; + const size_t keyDim = entry.dim; + dst.segment(entry.start, keyDim) = src.segment(index, keyDim); + index += keyDim; } +} - vector acc(m); - std::partial_sum(w.begin(),w.end(),acc.begin()); - - /* iid sample n times */ - vector result; result.reserve(n); - const double denominator = (double)RAND_MAX; - for ( size_t i = 0 ; i < n ; ++i ) { - const double value = rand() / denominator; - /* binary search the interval containing "value" */ - vector::iterator it = std::lower_bound(acc.begin(), acc.end(), value); - size_t idx = it - acc.begin(); - result.push_back(idx); - } +/* ************************************************************************* */ +// Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with +// Cholesky) +static GaussianFactorGraph::shared_ptr convertToJacobianFactors( + const GaussianFactorGraph &gfg) { + auto result = boost::make_shared(); + for (const auto &factor : gfg) + if (factor) { + auto jf = boost::dynamic_pointer_cast(factor); + if (!jf) { + jf = boost::make_shared(*factor); + } + result->push_back(jf); + } return result; } -/*****************************************************************************/ -vector uniqueSampler(const vector &weight, const size_t n) { - - const size_t m = weight.size(); - if ( n > m ) throw std::invalid_argument("uniqueSampler: invalid input size"); - - vector result; - - size_t count = 0; - std::vector touched(m, false); - while ( count < n ) { - std::vector localIndices; localIndices.reserve(n-count); - std::vector localWeights; localWeights.reserve(n-count); - - /* collect data */ - for ( size_t i = 0 ; i < m ; ++i ) { - if ( !touched[i] ) { - localIndices.push_back(i); - localWeights.push_back(weight[i]); - } - } - - /* sampling and cache results */ - vector samples = iidSampler(localWeights, n-count); - for ( const size_t &id: samples ) { - if ( touched[id] == false ) { - touched[id] = true ; - result.push_back(id); - if ( ++count >= n ) break; - } - } - } - return result; -} - -/****************************************************************************/ -Subgraph::Subgraph(const std::vector &indices) { - edges_.reserve(indices.size()); - for ( const size_t &idx: indices ) { - edges_.push_back(SubgraphEdge(idx, 1.0)); - } -} - -/****************************************************************************/ -std::vector Subgraph::edgeIndices() const { - std::vector eid; eid.reserve(size()); - for ( const SubgraphEdge &edge: edges_ ) { - eid.push_back(edge.index_); - } - return eid; -} - -/****************************************************************************/ -void Subgraph::save(const std::string &fn) const { - std::ofstream os(fn.c_str()); - boost::archive::text_oarchive oa(os); - oa << *this; - os.close(); -} - -/****************************************************************************/ -Subgraph::shared_ptr Subgraph::load(const std::string &fn) { - std::ifstream is(fn.c_str()); - boost::archive::text_iarchive ia(is); - Subgraph::shared_ptr subgraph(new Subgraph()); - ia >> *subgraph; - is.close(); - return subgraph; -} - -/****************************************************************************/ -std::ostream &operator<<(std::ostream &os, const SubgraphEdge &edge) { - if ( edge.weight() != 1.0 ) - os << edge.index() << "(" << std::setprecision(2) << edge.weight() << ")"; - else - os << edge.index() ; - return os; -} - -/****************************************************************************/ -std::ostream &operator<<(std::ostream &os, const Subgraph &subgraph) { - os << "Subgraph" << endl; - for ( const SubgraphEdge &e: subgraph.edges() ) { - os << e << ", " ; - } - return os; -} - -/*****************************************************************************/ -void SubgraphBuilderParameters::print() const { - print(cout); -} - -/***************************************************************************************/ -void SubgraphBuilderParameters::print(ostream &os) const { - os << "SubgraphBuilderParameters" << endl - << "skeleton: " << skeletonTranslator(skeleton_) << endl - << "skeleton weight: " << skeletonWeightTranslator(skeletonWeight_) << endl - << "augmentation weight: " << augmentationWeightTranslator(augmentationWeight_) << endl - ; -} - -/*****************************************************************************/ -ostream& operator<<(ostream &os, const SubgraphBuilderParameters &p) { - p.print(os); - return os; -} - -/*****************************************************************************/ -SubgraphBuilderParameters::Skeleton SubgraphBuilderParameters::skeletonTranslator(const std::string &src){ - std::string s = src; boost::algorithm::to_upper(s); - if (s == "NATURALCHAIN") return NATURALCHAIN; - else if (s == "BFS") return BFS; - else if (s == "KRUSKAL") return KRUSKAL; - throw invalid_argument("SubgraphBuilderParameters::skeletonTranslator undefined string " + s); - return KRUSKAL; -} - -/****************************************************************/ -std::string SubgraphBuilderParameters::skeletonTranslator(Skeleton w) { - if ( w == NATURALCHAIN )return "NATURALCHAIN"; - else if ( w == BFS ) return "BFS"; - else if ( w == KRUSKAL )return "KRUSKAL"; - else return "UNKNOWN"; -} - -/****************************************************************/ -SubgraphBuilderParameters::SkeletonWeight SubgraphBuilderParameters::skeletonWeightTranslator(const std::string &src) { - std::string s = src; boost::algorithm::to_upper(s); - if (s == "EQUAL") return EQUAL; - else if (s == "RHS") return RHS_2NORM; - else if (s == "LHS") return LHS_FNORM; - else if (s == "RANDOM") return RANDOM; - throw invalid_argument("SubgraphBuilderParameters::skeletonWeightTranslator undefined string " + s); - return EQUAL; -} - -/****************************************************************/ -std::string SubgraphBuilderParameters::skeletonWeightTranslator(SkeletonWeight w) { - if ( w == EQUAL ) return "EQUAL"; - else if ( w == RHS_2NORM ) return "RHS"; - else if ( w == LHS_FNORM ) return "LHS"; - else if ( w == RANDOM ) return "RANDOM"; - else return "UNKNOWN"; -} - -/****************************************************************/ -SubgraphBuilderParameters::AugmentationWeight SubgraphBuilderParameters::augmentationWeightTranslator(const std::string &src) { - std::string s = src; boost::algorithm::to_upper(s); - if (s == "SKELETON") return SKELETON; -// else if (s == "STRETCH") return STRETCH; -// else if (s == "GENERALIZED_STRETCH") return GENERALIZED_STRETCH; - throw invalid_argument("SubgraphBuilder::Parameters::augmentationWeightTranslator undefined string " + s); - return SKELETON; -} - -/****************************************************************/ -std::string SubgraphBuilderParameters::augmentationWeightTranslator(AugmentationWeight w) { - if ( w == SKELETON ) return "SKELETON"; -// else if ( w == STRETCH ) return "STRETCH"; -// else if ( w == GENERALIZED_STRETCH ) return "GENERALIZED_STRETCH"; - else return "UNKNOWN"; -} - -/****************************************************************/ -std::vector SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg, const FastMap &ordering, const std::vector &w) const { - const SubgraphBuilderParameters &p = parameters_; - switch (p.skeleton_) { - case SubgraphBuilderParameters::NATURALCHAIN: - return natural_chain(gfg); - break; - case SubgraphBuilderParameters::BFS: - return bfs(gfg); - break; - case SubgraphBuilderParameters::KRUSKAL: - return kruskal(gfg, ordering, w); - break; - default: - cerr << "SubgraphBuilder::buildTree undefined skeleton type" << endl; - break; - } - return vector(); -} - -/****************************************************************/ -std::vector SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const { - std::vector result ; - size_t idx = 0; - for ( const GaussianFactor::shared_ptr &gf: gfg ) { - if ( gf->size() == 1 ) { - result.push_back(idx); - } - idx++; - } - return result; -} - -/****************************************************************/ -std::vector SubgraphBuilder::natural_chain(const GaussianFactorGraph &gfg) const { - std::vector result ; - size_t idx = 0; - for ( const GaussianFactor::shared_ptr &gf: gfg ) { - if ( gf->size() == 2 ) { - const Key k0 = gf->keys()[0], k1 = gf->keys()[1]; - if ( (k1-k0) == 1 || (k0-k1) == 1 ) - result.push_back(idx); - } - idx++; - } - return result; -} - -/****************************************************************/ -std::vector SubgraphBuilder::bfs(const GaussianFactorGraph &gfg) const { - const VariableIndex variableIndex(gfg); - /* start from the first key of the first factor */ - Key seed = gfg[0]->keys()[0]; - - const size_t n = variableIndex.size(); - - /* each vertex has self as the predecessor */ - std::vector result; - result.reserve(n-1); - - /* Initialize */ - std::queue q; - q.push(seed); - - std::set flags; - flags.insert(seed); - - /* traversal */ - while ( !q.empty() ) { - const size_t head = q.front(); q.pop(); - for ( const size_t id: variableIndex[head] ) { - const GaussianFactor &gf = *gfg[id]; - for ( const size_t key: gf.keys() ) { - if ( flags.count(key) == 0 ) { - q.push(key); - flags.insert(key); - result.push_back(id); - } - } - } - } - return result; -} - -/****************************************************************/ -std::vector SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg, const FastMap &ordering, const std::vector &w) const { - const VariableIndex variableIndex(gfg); - const size_t n = variableIndex.size(); - const vector idx = sort_idx(w) ; - - /* initialize buffer */ - vector result; - result.reserve(n-1); - - // container for acsendingly sorted edges - DSFVector D(n) ; - - size_t count = 0 ; double sum = 0.0 ; - for (const size_t id: idx) { - const GaussianFactor &gf = *gfg[id]; - if ( gf.keys().size() != 2 ) continue; - const size_t u = ordering.find(gf.keys()[0])->second, - u_root = D.find(u), - v = ordering.find(gf.keys()[1])->second, - v_root = D.find(v) ; - if ( u_root != v_root ) { - D.merge(u_root, v_root) ; - result.push_back(id) ; - sum += w[id] ; - if ( ++count == n-1 ) break ; - } - } - return result; -} - -/****************************************************************/ -std::vector SubgraphBuilder::sample(const std::vector &weights, const size_t t) const { - return uniqueSampler(weights, t); -} - -/****************************************************************/ -Subgraph::shared_ptr SubgraphBuilder::operator() (const GaussianFactorGraph &gfg) const { - - const SubgraphBuilderParameters &p = parameters_; - const Ordering inverse_ordering = Ordering::Natural(gfg); - const FastMap forward_ordering = inverse_ordering.invert(); - const size_t n = inverse_ordering.size(), t = n * p.complexity_ ; - - vector w = weights(gfg); - const vector tree = buildTree(gfg, forward_ordering, w); - - /* sanity check */ - if ( tree.size() != n-1 ) { - throw runtime_error("SubgraphBuilder::operator() tree.size() != n-1 failed "); - } - - /* down weight the tree edges to zero */ - for ( const size_t id: tree ) { - w[id] = 0.0; - } - - /* decide how many edges to augment */ - std::vector offTree = sample(w, t); - - vector subgraph = unary(gfg); - subgraph.insert(subgraph.end(), tree.begin(), tree.end()); - subgraph.insert(subgraph.end(), offTree.begin(), offTree.end()); - - return boost::make_shared(subgraph); -} - -/****************************************************************/ -SubgraphBuilder::Weights SubgraphBuilder::weights(const GaussianFactorGraph &gfg) const -{ - const size_t m = gfg.size() ; - Weights weight; weight.reserve(m); - - for(const GaussianFactor::shared_ptr &gf: gfg ) { - switch ( parameters_.skeletonWeight_ ) { - case SubgraphBuilderParameters::EQUAL: - weight.push_back(1.0); - break; - case SubgraphBuilderParameters::RHS_2NORM: - { - if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(gf) ) { - weight.push_back(jf->getb().norm()); - } - else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast(gf) ) { - weight.push_back(hf->linearTerm().norm()); - } - } - break; - case SubgraphBuilderParameters::LHS_FNORM: - { - if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(gf) ) { - weight.push_back(std::sqrt(jf->getA().squaredNorm())); - } - else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast(gf) ) { - weight.push_back(std::sqrt(hf->information().squaredNorm())); - } - } - break; - - case SubgraphBuilderParameters::RANDOM: - weight.push_back(std::rand()%100 + 1.0); - break; - - default: - throw invalid_argument("SubgraphBuilder::weights: undefined weight scheme "); - break; - } - } - return weight; -} - /* ************************************************************************* */ SubgraphPreconditioner::SubgraphPreconditioner(const SubgraphPreconditionerParameters &p) : parameters_(p) {} @@ -484,21 +118,20 @@ double SubgraphPreconditioner::error(const VectorValues& y) const { /* ************************************************************************* */ // gradient is y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar), -VectorValues SubgraphPreconditioner::gradient(const VectorValues& y) const { +VectorValues SubgraphPreconditioner::gradient(const VectorValues &y) const { VectorValues x = Rc1()->backSubstitute(y); /* inv(R1)*y */ - Errors e = (*Ab2()*x - *b2bar()); /* (A2*inv(R1)*y-b2bar) */ + Errors e = (*Ab2() * x - *b2bar()); /* (A2*inv(R1)*y-b2bar) */ VectorValues v = VectorValues::Zero(x); - Ab2()->transposeMultiplyAdd(1.0, e, v); /* A2'*(A2*inv(R1)*y-b2bar) */ + Ab2()->transposeMultiplyAdd(1.0, e, v); /* A2'*(A2*inv(R1)*y-b2bar) */ return y + Rc1()->backSubstituteTranspose(v); } /* ************************************************************************* */ // Apply operator A, A*y = [I;A2*inv(R1)]*y = [y; A2*inv(R1)*y] Errors SubgraphPreconditioner::operator*(const VectorValues& y) const { - Errors e(y); VectorValues x = Rc1()->backSubstitute(y); /* x=inv(R1)*y */ - Errors e2 = *Ab2() * x; /* A2*x */ + Errors e2 = *Ab2() * x; /* A2*x */ e.splice(e.end(), e2); return e; } @@ -508,8 +141,10 @@ Errors SubgraphPreconditioner::operator*(const VectorValues& y) const { void SubgraphPreconditioner::multiplyInPlace(const VectorValues& y, Errors& e) const { Errors::iterator ei = e.begin(); - for (size_t i = 0; i < y.size(); ++i, ++ei) - *ei = y[i]; + for(const auto& key_value: y) { + *ei = key_value.second; + ++ei; + } // Add A2 contribution VectorValues x = Rc1()->backSubstitute(y); // x=inv(R1)*y @@ -522,8 +157,10 @@ VectorValues SubgraphPreconditioner::operator^(const Errors& e) const { Errors::const_iterator it = e.begin(); VectorValues y = zero(); - for (size_t i = 0; i < y.size(); ++i, ++it) - y[i] = *it; + for(auto& key_value: y) { + key_value.second = *it; + ++it; + } transposeMultiplyAdd2(1.0, it, e.end(), y); return y; } @@ -534,9 +171,10 @@ void SubgraphPreconditioner::transposeMultiplyAdd (double alpha, const Errors& e, VectorValues& y) const { Errors::const_iterator it = e.begin(); - for (size_t i = 0; i < y.size(); ++i, ++it) { + for(auto& key_value: y) { const Vector& ei = *it; - axpy(alpha, ei, y[i]); + axpy(alpha, ei, key_value.second); + ++it; } transposeMultiplyAdd2(alpha, it, e.end(), y); } @@ -563,47 +201,51 @@ void SubgraphPreconditioner::print(const std::string& s) const { } /*****************************************************************************/ -void SubgraphPreconditioner::solve(const Vector& y, Vector &x) const -{ - /* copy first */ - std::copy(y.data(), y.data() + y.rows(), x.data()); +void SubgraphPreconditioner::solve(const Vector &y, Vector &x) const { + assert(x.size() == y.size()); - /* in place back substitute */ - for (auto cg: boost::adaptors::reverse(*Rc1_)) { + /* back substitute */ + for (const auto &cg : boost::adaptors::reverse(*Rc1_)) { /* collect a subvector of x that consists of the parents of cg (S) */ - const Vector xParent = getSubvector(x, keyInfo_, KeyVector(cg->beginParents(), cg->endParents())); - const Vector rhsFrontal = getSubvector(x, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals())); + const KeyVector parentKeys(cg->beginParents(), cg->endParents()); + const KeyVector frontalKeys(cg->beginFrontals(), cg->endFrontals()); + const Vector xParent = getSubvector(x, keyInfo_, parentKeys); + const Vector rhsFrontal = getSubvector(y, keyInfo_, frontalKeys); /* compute the solution for the current pivot */ - const Vector solFrontal = cg->get_R().triangularView().solve(rhsFrontal - cg->get_S() * xParent); + const Vector solFrontal = cg->get_R().triangularView().solve( + rhsFrontal - cg->get_S() * xParent); /* assign subvector of sol to the frontal variables */ - setSubvector(solFrontal, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals()), x); + setSubvector(solFrontal, keyInfo_, frontalKeys, x); } } /*****************************************************************************/ -void SubgraphPreconditioner::transposeSolve(const Vector& y, Vector& x) const -{ +void SubgraphPreconditioner::transposeSolve(const Vector &y, Vector &x) const { /* copy first */ + assert(x.size() == y.size()); std::copy(y.data(), y.data() + y.rows(), x.data()); /* in place back substitute */ - for(const boost::shared_ptr & cg: *Rc1_) { - const Vector rhsFrontal = getSubvector(x, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals())); -// const Vector solFrontal = cg->get_R().triangularView().transpose().solve(rhsFrontal); - const Vector solFrontal = cg->get_R().transpose().triangularView().solve(rhsFrontal); + for (const auto &cg : *Rc1_) { + const KeyVector frontalKeys(cg->beginFrontals(), cg->endFrontals()); + const Vector rhsFrontal = getSubvector(x, keyInfo_, frontalKeys); + const Vector solFrontal = + cg->get_R().transpose().triangularView().solve( + rhsFrontal); // Check for indeterminant solution - if ( solFrontal.hasNaN()) throw IndeterminantLinearSystemException(cg->keys().front()); + if (solFrontal.hasNaN()) + throw IndeterminantLinearSystemException(cg->keys().front()); /* assign subvector of sol to the frontal variables */ - setSubvector(solFrontal, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals()), x); + setSubvector(solFrontal, keyInfo_, frontalKeys, x); /* substract from parent variables */ - for (GaussianConditional::const_iterator it = cg->beginParents(); it != cg->endParents(); it++) { - KeyInfo::const_iterator it2 = keyInfo_.find(*it); - Eigen::Map rhsParent(x.data()+it2->second.colstart(), it2->second.dim(), 1); + for (auto it = cg->beginParents(); it != cg->endParents(); it++) { + const KeyInfoEntry &entry = keyInfo_.find(*it)->second; + Eigen::Map rhsParent(x.data() + entry.start, entry.dim, 1); rhsParent -= Matrix(cg->getA(it)).transpose() * solFrontal; } } @@ -613,67 +255,18 @@ void SubgraphPreconditioner::transposeSolve(const Vector& y, Vector& x) const void SubgraphPreconditioner::build(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map &lambda) { /* identify the subgraph structure */ - const SubgraphBuilder builder(parameters_.builderParams_); - Subgraph::shared_ptr subgraph = builder(gfg); + const SubgraphBuilder builder(parameters_.builderParams); + auto subgraph = builder(gfg); keyInfo_ = keyInfo; /* build factor subgraph */ - GaussianFactorGraph::shared_ptr gfg_subgraph = buildFactorSubgraph(gfg, *subgraph, true); + GaussianFactorGraph::shared_ptr gfg_subgraph = buildFactorSubgraph(gfg, subgraph, true); /* factorize and cache BayesNet */ Rc1_ = gfg_subgraph->eliminateSequential(); } /*****************************************************************************/ -Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys) { - - /* a cache of starting index and dim */ - typedef vector > Cache; - Cache cache; - - /* figure out dimension by traversing the keys */ - size_t d = 0; - for ( const Key &key: keys ) { - const KeyInfoEntry &entry = keyInfo.find(key)->second; - cache.push_back(make_pair(entry.colstart(), entry.dim())); - d += entry.dim(); - } - - /* use the cache to fill the result */ - Vector result = Vector::Zero(d, 1); - size_t idx = 0; - for ( const Cache::value_type &p: cache ) { - result.segment(idx, p.second) = src.segment(p.first, p.second) ; - idx += p.second; - } - - return result; -} - -/*****************************************************************************/ -void setSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys, Vector &dst) { - /* use the cache */ - size_t idx = 0; - for ( const Key &key: keys ) { - const KeyInfoEntry &entry = keyInfo.find(key)->second; - dst.segment(entry.colstart(), entry.dim()) = src.segment(idx, entry.dim()) ; - idx += entry.dim(); - } -} - -/*****************************************************************************/ -boost::shared_ptr -buildFactorSubgraph(const GaussianFactorGraph &gfg, const Subgraph &subgraph, const bool clone) { - - GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph()); - result->reserve(subgraph.size()); - for ( const SubgraphEdge &e: subgraph ) { - const size_t idx = e.index(); - if ( clone ) result->push_back(gfg[idx]->clone()); - else result->push_back(gfg[idx]); - } - return result; -} } // nsamespace gtsam diff --git a/gtsam/linear/SubgraphPreconditioner.h b/gtsam/linear/SubgraphPreconditioner.h index e440f32e4..8906337a9 100644 --- a/gtsam/linear/SubgraphPreconditioner.h +++ b/gtsam/linear/SubgraphPreconditioner.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -17,30 +17,16 @@ #pragma once +#include #include -#include -#include #include #include #include -#include -#include -#include -#include #include -#include #include #include -#include -#include - -namespace boost { -namespace serialization { -class access; -} /* namespace serialization */ -} /* namespace boost */ namespace gtsam { @@ -49,142 +35,11 @@ namespace gtsam { class GaussianFactorGraph; class VectorValues; - struct GTSAM_EXPORT SubgraphEdge { - size_t index_; /* edge id */ - double weight_; /* edge weight */ - SubgraphEdge() : index_(0), weight_(1.0) {} - SubgraphEdge(const SubgraphEdge &e) : index_(e.index()), weight_(e.weight()) {} - SubgraphEdge(const size_t index, const double weight = 1.0): index_(index), weight_(weight) {} - inline size_t index() const { return index_; } - inline double weight() const { return weight_; } - inline bool isUnitWeight() const { return (weight_ == 1.0); } - friend std::ostream &operator<<(std::ostream &os, const SubgraphEdge &edge); - private: - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_NVP(index_); - ar & BOOST_SERIALIZATION_NVP(weight_); - } - }; - - /**************************************************************************/ - class GTSAM_EXPORT Subgraph { - public: - typedef boost::shared_ptr shared_ptr; - typedef std::vector vector_shared_ptr; - typedef std::vector Edges; - typedef std::vector EdgeIndices; - typedef Edges::iterator iterator; - typedef Edges::const_iterator const_iterator; - - protected: - Edges edges_; /* index to the factors */ - - public: - Subgraph() {} - Subgraph(const Subgraph &subgraph) : edges_(subgraph.edges()) {} - Subgraph(const Edges &edges) : edges_(edges) {} - Subgraph(const std::vector &indices) ; - - inline const Edges& edges() const { return edges_; } - inline size_t size() const { return edges_.size(); } - EdgeIndices edgeIndices() const; - - iterator begin() { return edges_.begin(); } - const_iterator begin() const { return edges_.begin(); } - iterator end() { return edges_.end(); } - const_iterator end() const { return edges_.end(); } - - void save(const std::string &fn) const; - static shared_ptr load(const std::string &fn); - friend std::ostream &operator<<(std::ostream &os, const Subgraph &subgraph); - - private: - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_NVP(edges_); - } - }; - - /****************************************************************************/ - struct GTSAM_EXPORT SubgraphBuilderParameters { - public: - typedef boost::shared_ptr shared_ptr; - - enum Skeleton { - /* augmented tree */ - NATURALCHAIN = 0, /* natural ordering of the graph */ - BFS, /* breadth-first search tree */ - KRUSKAL, /* maximum weighted spanning tree */ - } skeleton_ ; - - enum SkeletonWeight { /* how to weigh the graph edges */ - EQUAL = 0, /* every block edge has equal weight */ - RHS_2NORM, /* use the 2-norm of the rhs */ - LHS_FNORM, /* use the frobenius norm of the lhs */ - RANDOM, /* bounded random edge weight */ - } skeletonWeight_ ; - - enum AugmentationWeight { /* how to weigh the graph edges */ - SKELETON = 0, /* use the same weights in building the skeleton */ -// STRETCH, /* stretch in the laplacian sense */ -// GENERALIZED_STRETCH /* the generalized stretch defined in jian2013iros */ - } augmentationWeight_ ; - - double complexity_; - - SubgraphBuilderParameters() - : skeleton_(KRUSKAL), skeletonWeight_(RANDOM), augmentationWeight_(SKELETON), complexity_(1.0) {} - virtual ~SubgraphBuilderParameters() {} - - /* for serialization */ - void print() const ; - virtual void print(std::ostream &os) const ; - friend std::ostream& operator<<(std::ostream &os, const PreconditionerParameters &p); - - static Skeleton skeletonTranslator(const std::string &s); - static std::string skeletonTranslator(Skeleton w); - static SkeletonWeight skeletonWeightTranslator(const std::string &s); - static std::string skeletonWeightTranslator(SkeletonWeight w); - static AugmentationWeight augmentationWeightTranslator(const std::string &s); - static std::string augmentationWeightTranslator(AugmentationWeight w); - }; - - /*****************************************************************************/ - class GTSAM_EXPORT SubgraphBuilder { - - public: - typedef SubgraphBuilder Base; - typedef boost::shared_ptr shared_ptr; - typedef std::vector Weights; - - SubgraphBuilder(const SubgraphBuilderParameters &p = SubgraphBuilderParameters()) - : parameters_(p) {} - virtual ~SubgraphBuilder() {} - virtual boost::shared_ptr operator() (const GaussianFactorGraph &jfg) const ; - - private: - std::vector buildTree(const GaussianFactorGraph &gfg, const FastMap &ordering, const std::vector &weights) const ; - std::vector unary(const GaussianFactorGraph &gfg) const ; - std::vector natural_chain(const GaussianFactorGraph &gfg) const ; - std::vector bfs(const GaussianFactorGraph &gfg) const ; - std::vector kruskal(const GaussianFactorGraph &gfg, const FastMap &ordering, const std::vector &w) const ; - std::vector sample(const std::vector &weights, const size_t t) const ; - Weights weights(const GaussianFactorGraph &gfg) const; - SubgraphBuilderParameters parameters_; - - }; - - /*******************************************************************************************/ struct GTSAM_EXPORT SubgraphPreconditionerParameters : public PreconditionerParameters { - typedef PreconditionerParameters Base; typedef boost::shared_ptr shared_ptr; SubgraphPreconditionerParameters(const SubgraphBuilderParameters &p = SubgraphBuilderParameters()) - : Base(), builderParams_(p) {} - virtual ~SubgraphPreconditionerParameters() {} - SubgraphBuilderParameters builderParams_; + : builderParams(p) {} + SubgraphBuilderParameters builderParams; }; /** @@ -249,8 +104,8 @@ namespace gtsam { /* A zero VectorValues with the structure of xbar */ VectorValues zero() const { - VectorValues V(VectorValues::Zero(*xbar_)); - return V ; + assert(xbar_); + return VectorValues::Zero(*xbar_); } /** @@ -285,50 +140,19 @@ namespace gtsam { /*****************************************************************************/ /* implement virtual functions of Preconditioner */ - /* Computation Interfaces for Vector */ - virtual void solve(const Vector& y, Vector &x) const; - virtual void transposeSolve(const Vector& y, Vector& x) const ; + /// implement x = R^{-1} y + void solve(const Vector& y, Vector &x) const override; - virtual void build( + /// implement x = R^{-T} y + void transposeSolve(const Vector& y, Vector& x) const override; + + /// build/factorize the preconditioner + void build( const GaussianFactorGraph &gfg, const KeyInfo &info, const std::map &lambda - ) ; + ) override; /*****************************************************************************/ }; - /* get subvectors */ - Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys); - - /* set subvectors */ - void setSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys, Vector &dst); - - - /* build a factor subgraph, which is defined as a set of weighted edges (factors) */ - boost::shared_ptr - buildFactorSubgraph(const GaussianFactorGraph &gfg, const Subgraph &subgraph, const bool clone); - - - /* sort the container and return permutation index with default comparator */ - template - std::vector sort_idx(const Container &src) - { - typedef typename Container::value_type T; - const size_t n = src.size() ; - std::vector > tmp; - tmp.reserve(n); - for ( size_t i = 0 ; i < n ; i++ ) - tmp.push_back(std::make_pair(i, src[i])); - - /* sort */ - std::stable_sort(tmp.begin(), tmp.end()) ; - - /* copy back */ - std::vector idx; idx.reserve(n); - for ( size_t i = 0 ; i < n ; i++ ) { - idx.push_back(tmp[i].first) ; - } - return idx; - } - } // namespace gtsam diff --git a/gtsam/linear/SubgraphSolver.cpp b/gtsam/linear/SubgraphSolver.cpp index 22d39a7f2..56b843e8d 100644 --- a/gtsam/linear/SubgraphSolver.cpp +++ b/gtsam/linear/SubgraphSolver.cpp @@ -18,153 +18,94 @@ */ #include + +#include #include #include #include #include -#include using namespace std; namespace gtsam { /**************************************************************************************************/ -SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &gfg, +// Just taking system [A|b] +SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab, const Parameters ¶meters, const Ordering& ordering) : - parameters_(parameters), ordering_(ordering) { - initialize(gfg); + parameters_(parameters) { + GaussianFactorGraph::shared_ptr Ab1,Ab2; + std::tie(Ab1, Ab2) = splitGraph(Ab); + if (parameters_.verbosity()) + cout << "Split A into (A1) " << Ab1->size() << " and (A2) " << Ab2->size() + << " factors" << endl; + + auto Rc1 = Ab1->eliminateSequential(ordering, EliminateQR); + auto xbar = boost::make_shared(Rc1->optimize()); + pc_ = boost::make_shared(Ab2, Rc1, xbar); } /**************************************************************************************************/ -SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &jfg, - const Parameters ¶meters, const Ordering& ordering) : - parameters_(parameters), ordering_(ordering) { - initialize(*jfg); +// Taking eliminated tree [R1|c] and constraint graph [A2|b2] +SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, + const GaussianFactorGraph::shared_ptr &Ab2, + const Parameters ¶meters) + : parameters_(parameters) { + auto xbar = boost::make_shared(Rc1->optimize()); + pc_ = boost::make_shared(Ab2, Rc1, xbar); } /**************************************************************************************************/ +// Taking subgraphs [A1|b1] and [A2|b2] +// delegate up SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1, - const GaussianFactorGraph &Ab2, const Parameters ¶meters, - const Ordering& ordering) : - parameters_(parameters), ordering_(ordering) { - - GaussianBayesNet::shared_ptr Rc1 = Ab1.eliminateSequential(ordering_, - EliminateQR); - initialize(Rc1, boost::make_shared(Ab2)); -} - -/**************************************************************************************************/ -SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &Ab1, - const GaussianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters, - const Ordering& ordering) : - parameters_(parameters), ordering_(ordering) { - - GaussianBayesNet::shared_ptr Rc1 = Ab1->eliminateSequential(ordering_, - EliminateQR); - initialize(Rc1, Ab2); -} + const GaussianFactorGraph::shared_ptr &Ab2, + const Parameters ¶meters, + const Ordering &ordering) + : SubgraphSolver(Ab1.eliminateSequential(ordering, EliminateQR), Ab2, + parameters) {} /**************************************************************************************************/ +// deprecated variants +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, - const GaussianFactorGraph &Ab2, const Parameters ¶meters, - const Ordering& ordering) : - parameters_(parameters), ordering_(ordering) { - initialize(Rc1, boost::make_shared(Ab2)); -} + const GaussianFactorGraph &Ab2, + const Parameters ¶meters) + : SubgraphSolver(Rc1, boost::make_shared(Ab2), + parameters) {} + +SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1, + const GaussianFactorGraph &Ab2, + const Parameters ¶meters, + const Ordering &ordering) + : SubgraphSolver(Ab1, boost::make_shared(Ab2), + parameters, ordering) {} +#endif /**************************************************************************************************/ -SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, - const GaussianFactorGraph::shared_ptr &Ab2, const Parameters ¶meters, - const Ordering& ordering) : - parameters_(parameters), ordering_(ordering) { - initialize(Rc1, Ab2); -} - -/**************************************************************************************************/ -VectorValues SubgraphSolver::optimize() { +VectorValues SubgraphSolver::optimize() const { VectorValues ybar = conjugateGradients(*pc_, pc_->zero(), parameters_); return pc_->x(ybar); } -/**************************************************************************************************/ -VectorValues SubgraphSolver::optimize(const VectorValues &initial) { - // the initial is ignored in this case ... - return optimize(); -} - -/**************************************************************************************************/ -void SubgraphSolver::initialize(const GaussianFactorGraph &jfg) { - GaussianFactorGraph::shared_ptr Ab1 = - boost::make_shared(), Ab2 = boost::make_shared< - GaussianFactorGraph>(); - - boost::tie(Ab1, Ab2) = splitGraph(jfg); - if (parameters_.verbosity()) - cout << "Split A into (A1) " << Ab1->size() << " and (A2) " << Ab2->size() - << " factors" << endl; - - GaussianBayesNet::shared_ptr Rc1 = Ab1->eliminateSequential(ordering_, - EliminateQR); - VectorValues::shared_ptr xbar = boost::make_shared( - Rc1->optimize()); - pc_ = boost::make_shared(Ab2, Rc1, xbar); -} - -/**************************************************************************************************/ -void SubgraphSolver::initialize(const GaussianBayesNet::shared_ptr &Rc1, - const GaussianFactorGraph::shared_ptr &Ab2) { - VectorValues::shared_ptr xbar = boost::make_shared( - Rc1->optimize()); - pc_ = boost::make_shared(Ab2, Rc1, xbar); -} - -/**************************************************************************************************/ -boost::tuple // -SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) { - - const VariableIndex index(jfg); - const size_t n = index.size(); - DSFVector D(n); - - GaussianFactorGraph::shared_ptr At(new GaussianFactorGraph()); - GaussianFactorGraph::shared_ptr Ac(new GaussianFactorGraph()); - - size_t t = 0; - for ( const GaussianFactor::shared_ptr &gf: jfg ) { - - if (gf->keys().size() > 2) { - throw runtime_error( - "SubgraphSolver::splitGraph the graph is not simple, sanity check failed "); - } - - bool augment = false; - - /* check whether this factor should be augmented to the "tree" graph */ - if (gf->keys().size() == 1) - augment = true; - else { - const Key u = gf->keys()[0], v = gf->keys()[1], u_root = D.find(u), - v_root = D.find(v); - if (u_root != v_root) { - t++; - augment = true; - D.merge(u_root, v_root); - } - } - if (augment) - At->push_back(gf); - else - Ac->push_back(gf); - } - - return boost::tie(At, Ac); -} - -/****************************************************************************/ VectorValues SubgraphSolver::optimize(const GaussianFactorGraph &gfg, - const KeyInfo &keyInfo, const std::map &lambda, + const KeyInfo &keyInfo, const map &lambda, const VectorValues &initial) { return VectorValues(); } +/**************************************************************************************************/ +pair // +SubgraphSolver::splitGraph(const GaussianFactorGraph &factorGraph) { + + /* identify the subgraph structure */ + const SubgraphBuilder builder(parameters_.builderParams); + auto subgraph = builder(factorGraph); + + /* build factor subgraph */ + return splitFactorGraph(factorGraph, subgraph); +} + +/****************************************************************************/ + } // \namespace gtsam diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h index 318c029f3..5ab1a8520 100644 --- a/gtsam/linear/SubgraphSolver.h +++ b/gtsam/linear/SubgraphSolver.h @@ -20,6 +20,10 @@ #pragma once #include +#include + +#include +#include // pair namespace gtsam { @@ -28,30 +32,36 @@ class GaussianFactorGraph; class GaussianBayesNet; class SubgraphPreconditioner; -class GTSAM_EXPORT SubgraphSolverParameters: public ConjugateGradientParameters { -public: - typedef ConjugateGradientParameters Base; - SubgraphSolverParameters() : - Base() { - } - void print() const { - Base::print(); - } +struct GTSAM_EXPORT SubgraphSolverParameters + : public ConjugateGradientParameters { + SubgraphBuilderParameters builderParams; + explicit SubgraphSolverParameters(const SubgraphBuilderParameters &p = SubgraphBuilderParameters()) + : builderParams(p) {} + void print() const { Base::print(); } virtual void print(std::ostream &os) const { Base::print(os); } }; /** - * This class implements the SPCG solver presented in Dellaert et al in IROS'10. + * This class implements the linear SPCG solver presented in Dellaert et al in + * IROS'10. * - * Given a linear least-squares problem \f$ f(x) = |A x - b|^2 \f$. We split the problem into - * \f$ f(x) = |A_t - b_t|^2 + |A_c - b_c|^2 \f$ where \f$ A_t \f$ denotes the "tree" part, and \f$ A_c \f$ denotes the "constraint" part. - * \f$ A_t \f$ is factorized into \f$ Q_t R_t \f$, and we compute \f$ c_t = Q_t^{-1} b_t \f$, and \f$ x_t = R_t^{-1} c_t \f$ accordingly. - * Then we solve a reparametrized problem \f$ f(y) = |y|^2 + |A_c R_t^{-1} y - \bar{b_y}|^2 \f$, where \f$ y = R_t(x - x_t) \f$, and \f$ \bar{b_y} = (b_c - A_c x_t) \f$ + * Given a linear least-squares problem \f$ f(x) = |A x - b|^2 \f$. We split the + * problem into \f$ f(x) = |A_t - b_t|^2 + |A_c - b_c|^2 \f$ where \f$ A_t \f$ + * denotes the "tree" part, and \f$ A_c \f$ denotes the "constraint" part. * - * In the matrix form, it is equivalent to solving \f$ [A_c R_t^{-1} ; I ] y = [\bar{b_y} ; 0] \f$. We can solve it - * with the least-squares variation of the conjugate gradient method. + * \f$A_t \f$ is factorized into \f$ Q_t R_t \f$, and we compute + * \f$ c_t = Q_t^{-1} b_t \f$, and \f$ x_t = R_t^{-1} c_t \f$ accordingly. + * + * Then we solve a reparametrized problem + * \f$ f(y) = |y|^2 + |A_c R_t^{-1} y -\bar{b_y}|^2 \f$, + * where \f$ y = R_t(x - x_t) \f$, and \f$ \bar{b_y} = (b_c - A_c x_t) \f$ + * + * In the matrix form, it is equivalent to solving + * \f$ [A_c R_t^{-1} ; I ] y = [\bar{b_y} ; 0] \f$. + * We can solve it with the least-squares variation of the conjugate gradient + * method. * * To use it in nonlinear optimization, please see the following example * @@ -63,72 +73,86 @@ public: * * \nosubgrouping */ -class GTSAM_EXPORT SubgraphSolver: public IterativeSolver { - -public: +class GTSAM_EXPORT SubgraphSolver : public IterativeSolver { + public: typedef SubgraphSolverParameters Parameters; -protected: + protected: Parameters parameters_; - Ordering ordering_; - boost::shared_ptr pc_; ///< preconditioner object + boost::shared_ptr pc_; ///< preconditioner object -public: - - /// Given a gaussian factor graph, split it into a spanning tree (A1) + others (A2) for SPCG + public: + /// @name Constructors + /// @{ + /** + * Given a gaussian factor graph, split it into a spanning tree (A1) + others + * (A2) for SPCG Will throw exception if there are ternary factors or higher + * arity, as we use Kruskal's algorithm to split the graph, treating binary + * factors as edges. + */ SubgraphSolver(const GaussianFactorGraph &A, const Parameters ¶meters, - const Ordering& ordering); - - /// Shared pointer version - SubgraphSolver(const boost::shared_ptr &A, - const Parameters ¶meters, const Ordering& ordering); + const Ordering &ordering); /** - * The user specify the subgraph part and the constraint part - * may throw exception if A1 is underdetermined + * The user specifies the subgraph part and the constraints part. + * May throw exception if A1 is underdetermined. An ordering is required to + * eliminate Ab1. We take Ab1 as a const reference, as it will be transformed + * into Rc1, but take Ab2 as a shared pointer as we need to keep it around. + */ + SubgraphSolver(const GaussianFactorGraph &Ab1, + const boost::shared_ptr &Ab2, + const Parameters ¶meters, const Ordering &ordering); + /** + * The same as above, but we assume A1 was solved by caller. + * We take two shared pointers as we keep both around. */ - SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, - const Parameters ¶meters, const Ordering& ordering); - - /// Shared pointer version - SubgraphSolver(const boost::shared_ptr &Ab1, - const boost::shared_ptr &Ab2, - const Parameters ¶meters, const Ordering& ordering); - - /* The same as above, but the A1 is solved before */ SubgraphSolver(const boost::shared_ptr &Rc1, - const GaussianFactorGraph &Ab2, const Parameters ¶meters, - const Ordering& ordering); - - /// Shared pointer version - SubgraphSolver(const boost::shared_ptr &Rc1, - const boost::shared_ptr &Ab2, - const Parameters ¶meters, const Ordering& ordering); + const boost::shared_ptr &Ab2, + const Parameters ¶meters); /// Destructor - virtual ~SubgraphSolver() { - } + virtual ~SubgraphSolver() {} + + /// @} + /// @name Implement interface + /// @{ /// Optimize from zero - VectorValues optimize(); + VectorValues optimize() const; - /// Optimize from given initial values - VectorValues optimize(const VectorValues &initial); + /// Interface that IterativeSolver subclasses have to implement + VectorValues optimize(const GaussianFactorGraph &gfg, + const KeyInfo &keyInfo, + const std::map &lambda, + const VectorValues &initial) override; - /** Interface that IterativeSolver subclasses have to implement */ - virtual VectorValues optimize(const GaussianFactorGraph &gfg, - const KeyInfo &keyInfo, const std::map &lambda, - const VectorValues &initial); + /// @} + /// @name Implement interface + /// @{ -protected: + /// Split graph using Kruskal algorithm, treating binary factors as edges. + std::pair < boost::shared_ptr, + boost::shared_ptr > splitGraph( + const GaussianFactorGraph &gfg); - void initialize(const GaussianFactorGraph &jfg); - void initialize(const boost::shared_ptr &Rc1, - const boost::shared_ptr &Ab2); + /// @} - boost::tuple, - boost::shared_ptr > - splitGraph(const GaussianFactorGraph &gfg); +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 + /// @name Deprecated + /// @{ + SubgraphSolver(const boost::shared_ptr &A, + const Parameters ¶meters, const Ordering &ordering) + : SubgraphSolver(*A, parameters, ordering) {} + SubgraphSolver(const GaussianFactorGraph &, const GaussianFactorGraph &, + const Parameters &, const Ordering &); + SubgraphSolver(const boost::shared_ptr &Ab1, + const boost::shared_ptr &Ab2, + const Parameters ¶meters, const Ordering &ordering) + : SubgraphSolver(*Ab1, Ab2, parameters, ordering) {} + SubgraphSolver(const boost::shared_ptr &, + const GaussianFactorGraph &, const Parameters &); + /// @} +#endif }; -} // namespace gtsam +} // namespace gtsam diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index b037aad92..e8304e6e7 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -51,7 +51,7 @@ namespace gtsam { Key key; size_t n; boost::tie(key, n) = v; - values_.insert(make_pair(key, x.segment(j, n))); + values_.emplace(key, x.segment(j, n)); j += n; } } @@ -60,7 +60,7 @@ namespace gtsam { VectorValues::VectorValues(const Vector& x, const Scatter& scatter) { size_t j = 0; for (const SlotEntry& v : scatter) { - values_.insert(make_pair(v.key, x.segment(j, v.dimension))); + values_.emplace(v.key, x.segment(j, v.dimension)); j += v.dimension; } } @@ -70,14 +70,12 @@ namespace gtsam { { VectorValues result; for(const KeyValuePair& v: other) - result.values_.insert(make_pair(v.first, Vector::Zero(v.second.size()))); + result.values_.emplace(v.first, Vector::Zero(v.second.size())); return result; } /* ************************************************************************* */ VectorValues::iterator VectorValues::insert(const std::pair& key_value) { - // Note that here we accept a pair with a reference to the Vector, but the Vector is copied as - // it is inserted into the values_ map. std::pair result = values_.insert(key_value); if(!result.second) throw std::invalid_argument( @@ -86,6 +84,16 @@ namespace gtsam { return result.first; } + /* ************************************************************************* */ + VectorValues::iterator VectorValues::emplace(Key j, const Vector& value) { + std::pair result = values_.emplace(j, value); + if(!result.second) + throw std::invalid_argument( + "Requested to emplace variable '" + DefaultKeyFormatter(j) + + "' already in this VectorValues."); + return result.first; + } + /* ************************************************************************* */ void VectorValues::update(const VectorValues& values) { @@ -132,8 +140,7 @@ namespace gtsam { bool VectorValues::equals(const VectorValues& x, double tol) const { if(this->size() != x.size()) return false; - typedef boost::tuple ValuePair; - for(const ValuePair& values: boost::combine(*this, x)) { + for(const auto& values: boost::combine(*this, x)) { if(values.get<0>().first != values.get<1>().first || !equal_with_abs_tol(values.get<0>().second, values.get<1>().second, tol)) return false; @@ -142,17 +149,15 @@ namespace gtsam { } /* ************************************************************************* */ - Vector VectorValues::vector() const - { + Vector VectorValues::vector() const { // Count dimensions DenseIndex totalDim = 0; - for(const Vector& v: *this | map_values) - totalDim += v.size(); + for (const Vector& v : *this | map_values) totalDim += v.size(); // Copy vectors Vector result(totalDim); DenseIndex pos = 0; - for(const Vector& v: *this | map_values) { + for (const Vector& v : *this | map_values) { result.segment(pos, v.size()) = v; pos += v.size(); } @@ -242,7 +247,7 @@ namespace gtsam { VectorValues result; // The result.end() hint here should result in constant-time inserts for(const_iterator j1 = begin(), j2 = c.begin(); j1 != end(); ++j1, ++j2) - result.values_.insert(result.end(), make_pair(j1->first, j1->second + j2->second)); + result.values_.emplace(j1->first, j1->second + j2->second); return result; } @@ -300,7 +305,7 @@ namespace gtsam { VectorValues result; // The result.end() hint here should result in constant-time inserts for(const_iterator j1 = begin(), j2 = c.begin(); j1 != end(); ++j1, ++j2) - result.values_.insert(result.end(), make_pair(j1->first, j1->second - j2->second)); + result.values_.emplace(j1->first, j1->second - j2->second); return result; } @@ -316,7 +321,7 @@ namespace gtsam { { VectorValues result; for(const VectorValues::KeyValuePair& key_v: v) - result.values_.insert(result.values_.end(), make_pair(key_v.first, a * key_v.second)); + result.values_.emplace(key_v.first, a * key_v.second); return result; } diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 90ca6d244..b1d9de585 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -50,9 +50,9 @@ namespace gtsam { * Example: * \code VectorValues values; - values.insert(3, Vector3(1.0, 2.0, 3.0)); - values.insert(4, Vector2(4.0, 5.0)); - values.insert(0, (Vector(4) << 6.0, 7.0, 8.0, 9.0).finished()); + values.emplace(3, Vector3(1.0, 2.0, 3.0)); + values.emplace(4, Vector2(4.0, 5.0)); + values.emplace(0, (Vector(4) << 6.0, 7.0, 8.0, 9.0).finished()); // Prints [ 3.0 4.0 ] gtsam::print(values[1]); @@ -64,18 +64,7 @@ namespace gtsam { * *

Advanced Interface and Performance Information

* - * For advanced usage, or where speed is important: - * - Allocate space ahead of time using a pre-allocating constructor - * (\ref AdvancedConstructors "Advanced Constructors"), Zero(), - * SameStructure(), resize(), or append(). Do not use - * insert(Key, const Vector&), which always has to re-allocate the - * internal vector. - * - The vector() function permits access to the underlying Vector, for - * doing mathematical or other operations that require all values. - * - operator[]() returns a SubVector view of the underlying Vector, - * without copying any data. - * - * Access is through the variable index j, and returns a SubVector, + * Access is through the variable Key j, and returns a SubVector, * which is a view on the underlying data structure. * * This class is additionally used in gradient descent and dog leg to store the gradient. @@ -183,15 +172,21 @@ namespace gtsam { * j is already used. * @param value The vector to be inserted. * @param j The index with which the value will be associated. */ - iterator insert(Key j, const Vector& value) { - return insert(std::make_pair(j, value)); - } + iterator insert(const std::pair& key_value); + + /** Emplace a vector \c value with key \c j. Throws an invalid_argument exception if the key \c + * j is already used. + * @param value The vector to be inserted. + * @param j The index with which the value will be associated. */ + iterator emplace(Key j, const Vector& value); /** Insert a vector \c value with key \c j. Throws an invalid_argument exception if the key \c * j is already used. * @param value The vector to be inserted. * @param j The index with which the value will be associated. */ - iterator insert(const std::pair& key_value); + iterator insert(Key j, const Vector& value) { + return insert(std::make_pair(j, value)); + } /** Insert all values from \c values. Throws an invalid_argument exception if any keys to be * inserted are already used. */ @@ -202,7 +197,8 @@ namespace gtsam { * exist, it is inserted and an iterator pointing to the new element, along with 'true', is * returned. */ std::pair tryInsert(Key j, const Vector& value) { - return values_.insert(std::make_pair(j, value)); } + return values_.emplace(j, value); + } /** Erase the vector with the given key, or throw std::out_of_range if it does not exist */ void erase(Key var) { diff --git a/gtsam/linear/linearAlgorithms-inst.h b/gtsam/linear/linearAlgorithms-inst.h index 35f3d4c72..6fa5d0584 100644 --- a/gtsam/linear/linearAlgorithms-inst.h +++ b/gtsam/linear/linearAlgorithms-inst.h @@ -56,7 +56,7 @@ namespace gtsam myData.parentData = parentData; // Take any ancestor results we'll need for(Key parent: clique->conditional_->parents()) - myData.cliqueResults.insert(std::make_pair(parent, myData.parentData->cliqueResults.at(parent))); + myData.cliqueResults.emplace(parent, myData.parentData->cliqueResults.at(parent)); // Solve and store in our results { @@ -99,8 +99,8 @@ namespace gtsam DenseIndex vectorPosition = 0; for(GaussianConditional::const_iterator frontal = c.beginFrontals(); frontal != c.endFrontals(); ++frontal) { VectorValues::const_iterator r = - collectedResult.insert(*frontal, solution.segment(vectorPosition, c.getDim(frontal))); - myData.cliqueResults.insert(make_pair(r->first, r)); + collectedResult.emplace(*frontal, solution.segment(vectorPosition, c.getDim(frontal))); + myData.cliqueResults.emplace(r->first, r); vectorPosition += c.getDim(frontal); } } diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp index 47565ed5a..488368c72 100644 --- a/gtsam/linear/tests/testGaussianBayesNet.cpp +++ b/gtsam/linear/tests/testGaussianBayesNet.cpp @@ -21,6 +21,8 @@ #include #include +#include +#include #include #include // for operator += using namespace boost::assign; @@ -28,13 +30,11 @@ using namespace boost::assign; // STL/C++ #include #include -#include -#include using namespace std; using namespace gtsam; -static const Key _x_=0, _y_=1; +static const Key _x_ = 11, _y_ = 22, _z_ = 33; static GaussianBayesNet smallBayesNet = list_of(GaussianConditional(_x_, Vector1::Constant(9), I_1x1, _y_, I_1x1))( @@ -42,9 +42,9 @@ static GaussianBayesNet smallBayesNet = static GaussianBayesNet noisyBayesNet = list_of(GaussianConditional(_x_, Vector1::Constant(9), I_1x1, _y_, I_1x1, - noiseModel::Diagonal::Sigmas(Vector1::Constant(2))))( + noiseModel::Isotropic::Sigma(1, 2.0)))( GaussianConditional(_y_, Vector1::Constant(5), I_1x1, - noiseModel::Diagonal::Sigmas(Vector1::Constant(3)))); + noiseModel::Isotropic::Sigma(1, 3.0))); /* ************************************************************************* */ TEST( GaussianBayesNet, Matrix ) @@ -136,6 +136,37 @@ TEST( GaussianBayesNet, optimize3 ) EXPECT(assert_equal(expected, actual)); } +/* ************************************************************************* */ +TEST(GaussianBayesNet, ordering) +{ + Ordering expected; + expected += _x_, _y_; + const auto actual = noisyBayesNet.ordering(); + EXPECT(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +TEST( GaussianBayesNet, MatrixStress ) +{ + GaussianBayesNet bn; + using GC = GaussianConditional; + bn.emplace_shared(_x_, Vector2(1, 2), 1 * I_2x2, _y_, 2 * I_2x2, _z_, 3 * I_2x2); + bn.emplace_shared(_y_, Vector2(3, 4), 4 * I_2x2, _z_, 5 * I_2x2); + bn.emplace_shared(_z_, Vector2(5, 6), 6 * I_2x2); + + const VectorValues expected = bn.optimize(); + for (const auto keys : + {KeyVector({_x_, _y_, _z_}), KeyVector({_x_, _z_, _y_}), + KeyVector({_y_, _x_, _z_}), KeyVector({_y_, _z_, _x_}), + KeyVector({_z_, _x_, _y_}), KeyVector({_z_, _y_, _x_})}) { + const Ordering ordering(keys); + Matrix R; + Vector d; + boost::tie(R, d) = bn.matrix(ordering); + EXPECT(assert_equal(expected.vector(ordering), R.inverse() * d)); + } +} + /* ************************************************************************* */ TEST( GaussianBayesNet, backSubstituteTranspose ) { @@ -152,6 +183,34 @@ TEST( GaussianBayesNet, backSubstituteTranspose ) VectorValues actual = smallBayesNet.backSubstituteTranspose(x); EXPECT(assert_equal(expected, actual)); + + const auto ordering = noisyBayesNet.ordering(); + const Matrix R = smallBayesNet.matrix(ordering).first; + const Vector expected_vector = R.transpose().inverse() * x.vector(ordering); + EXPECT(assert_equal(expected_vector, actual.vector(ordering))); +} + +/* ************************************************************************* */ +TEST( GaussianBayesNet, backSubstituteTransposeNoisy ) +{ + // x=R'*y, expected=inv(R')*x + // 2 = 1 2 + // 5 1 1 3 + VectorValues + x = map_list_of + (_x_, Vector1::Constant(2)) + (_y_, Vector1::Constant(5)), + expected = map_list_of + (_x_, Vector1::Constant(4)) + (_y_, Vector1::Constant(9)); + + VectorValues actual = noisyBayesNet.backSubstituteTranspose(x); + EXPECT(assert_equal(expected, actual)); + + const auto ordering = noisyBayesNet.ordering(); + const Matrix R = noisyBayesNet.matrix(ordering).first; + const Vector expected_vector = R.transpose().inverse() * x.vector(ordering); + EXPECT(assert_equal(expected_vector, actual.vector(ordering))); } /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testJacobianFactor.cpp b/gtsam/linear/tests/testJacobianFactor.cpp index 2ea1b15bd..110a1223a 100644 --- a/gtsam/linear/tests/testJacobianFactor.cpp +++ b/gtsam/linear/tests/testJacobianFactor.cpp @@ -146,7 +146,7 @@ TEST(JacobianFactor, constructors_and_accessors) /* ************************************************************************* */ TEST(JabobianFactor, Hessian_conversion) { - HessianFactor hessian(0, (Matrix(4,4) << + HessianFactor hessian(0, (Matrix(4, 4) << 1.57, 2.695, -1.1, -2.35, 2.695, 11.3125, -0.65, -10.225, -1.1, -0.65, 1, 0.5, @@ -154,7 +154,7 @@ TEST(JabobianFactor, Hessian_conversion) { (Vector(4) << -7.885, -28.5175, 2.75, 25.675).finished(), 73.1725); - JacobianFactor expected(0, (Matrix(2,4) << + JacobianFactor expected(0, (Matrix(2, 4) << 1.2530, 2.1508, -0.8779, -1.8755, 0, 2.5858, 0.4789, -2.3943).finished(), Vector2(-6.2929, -5.7941)); @@ -162,6 +162,27 @@ TEST(JabobianFactor, Hessian_conversion) { EXPECT(assert_equal(expected, JacobianFactor(hessian), 1e-3)); } +/* ************************************************************************* */ +TEST(JabobianFactor, Hessian_conversion2) { + JacobianFactor jf(0, (Matrix(3, 3) << + 1, 2, 3, + 0, 2, 3, + 0, 0, 3).finished(), + Vector3(1, 2, 2)); + HessianFactor hessian(jf); + EXPECT(assert_equal(jf, JacobianFactor(hessian), 1e-9)); +} + +/* ************************************************************************* */ +TEST(JabobianFactor, Hessian_conversion3) { + JacobianFactor jf(0, (Matrix(2, 4) << + 1, 2, 3, 0, + 0, 3, 2, 1).finished(), + Vector2(1, 2)); + HessianFactor hessian(jf); + EXPECT(assert_equal(jf, JacobianFactor(hessian), 1e-9)); +} + /* ************************************************************************* */ namespace simple_graph { @@ -322,27 +343,30 @@ TEST(JacobianFactor, matrices) /* ************************************************************************* */ TEST(JacobianFactor, operators ) { - SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1); + const double sigma = 0.1; + SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2, sigma); Matrix I = I_2x2; Vector b = Vector2(0.2,-0.1); JacobianFactor lf(1, -I, 2, I, b, sigma0_1); - VectorValues c; - c.insert(1, Vector2(10.,20.)); - c.insert(2, Vector2(30.,60.)); + VectorValues x; + Vector2 x1(10,20), x2(30,60); + x.insert(1, x1); + x.insert(2, x2); // test A*x - Vector expectedE = Vector2(200.,400.); - Vector actualE = lf * c; + Vector expectedE = (x2 - x1)/sigma; + Vector actualE = lf * x; EXPECT(assert_equal(expectedE, actualE)); // test A^e VectorValues expectedX; - expectedX.insert(1, Vector2(-2000.,-4000.)); - expectedX.insert(2, Vector2(2000., 4000.)); + const double alpha = 0.5; + expectedX.insert(1, - alpha * expectedE /sigma); + expectedX.insert(2, alpha * expectedE /sigma); VectorValues actualX = VectorValues::Zero(expectedX); - lf.transposeMultiplyAdd(1.0, actualE, actualX); + lf.transposeMultiplyAdd(alpha, actualE, actualX); EXPECT(assert_equal(expectedX, actualX)); // test gradient at zero diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 32a24b51d..c0b2d0757 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -96,7 +96,7 @@ bool ISAM2::equals(const ISAM2& other, double tol) const { } /* ************************************************************************* */ -KeySet ISAM2::getAffectedFactors(const KeyList& keys) const { +FactorIndexSet ISAM2::getAffectedFactors(const KeyList& keys) const { static const bool debug = false; if (debug) cout << "Getting affected factors for "; if (debug) { @@ -106,15 +106,14 @@ KeySet ISAM2::getAffectedFactors(const KeyList& keys) const { } if (debug) cout << endl; - NonlinearFactorGraph allAffected; - KeySet indices; + FactorIndexSet indices; for (const Key key : keys) { const VariableIndex::Factors& factors(variableIndex_[key]); indices.insert(factors.begin(), factors.end()); } if (debug) cout << "Affected factors are: "; if (debug) { - for (const size_t index : indices) { + for (const auto index : indices) { cout << index << " "; } } @@ -132,8 +131,6 @@ GaussianFactorGraph::shared_ptr ISAM2::relinearizeAffectedFactors( KeySet candidates = getAffectedFactors(affectedKeys); gttoc(getAffectedFactors); - NonlinearFactorGraph nonlinearAffectedFactors; - gttic(affectedKeysSet); // for fast lookup below KeySet affectedKeysSet; @@ -589,7 +586,7 @@ ISAM2Result ISAM2::update( // Remove the removed factors NonlinearFactorGraph removeFactors; removeFactors.reserve(removeFactorIndices.size()); - for (size_t index : removeFactorIndices) { + for (const auto index : removeFactorIndices) { removeFactors.push_back(nonlinearFactors_[index]); nonlinearFactors_.remove(index); if (params_.cacheLinearizedFactors) linearFactors_.remove(index); @@ -823,7 +820,7 @@ void ISAM2::marginalizeLeaves( KeySet leafKeysRemoved; // Keep track of factors that get summarized by removing cliques - KeySet factorIndicesToRemove; + FactorIndexSet factorIndicesToRemove; // Remove the subtree and throw away the cliques auto trackingRemoveSubtree = [&](const sharedClique& subtreeRoot) { @@ -937,8 +934,8 @@ void ISAM2::marginalizeLeaves( } } // Create factor graph from factor indices - for (size_t i : factorsFromMarginalizedInClique_step1) { - graph.push_back(nonlinearFactors_[i]->linearize(theta_)); + for (const auto index: factorsFromMarginalizedInClique_step1) { + graph.push_back(nonlinearFactors_[index]->linearize(theta_)); } // Reeliminate the linear graph to get the marginal and discard the @@ -1011,10 +1008,10 @@ void ISAM2::marginalizeLeaves( // Remove the factors to remove that have been summarized in the newly-added // marginal factors NonlinearFactorGraph removedFactors; - for (size_t i : factorIndicesToRemove) { - removedFactors.push_back(nonlinearFactors_[i]); - nonlinearFactors_.remove(i); - if (params_.cacheLinearizedFactors) linearFactors_.remove(i); + for (const auto index: factorIndicesToRemove) { + removedFactors.push_back(nonlinearFactors_[index]); + nonlinearFactors_.remove(index); + if (params_.cacheLinearizedFactors) linearFactors_.remove(index); } variableIndex_.remove(factorIndicesToRemove.begin(), factorIndicesToRemove.end(), removedFactors); diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 697fff0b3..4b8b1398b 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -292,7 +292,7 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { */ void expmapMasked(const KeySet& mask); - FastSet getAffectedFactors(const FastList& keys) const; + FactorIndexSet getAffectedFactors(const FastList& keys) const; GaussianFactorGraph::shared_ptr relinearizeAffectedFactors( const FastList& affectedKeys, const KeySet& relinKeys) const; GaussianFactorGraph getCachedBoundaryFactors(const Cliques& orphans); diff --git a/gtsam/nonlinear/ISAM2Result.h b/gtsam/nonlinear/ISAM2Result.h index 1539d90c4..763a5e198 100644 --- a/gtsam/nonlinear/ISAM2Result.h +++ b/gtsam/nonlinear/ISAM2Result.h @@ -31,8 +31,6 @@ namespace gtsam { -typedef FastVector FactorIndices; - /** * @addtogroup ISAM2 * This struct is returned from ISAM2::update() and contains information about diff --git a/gtsam/nonlinear/LevenbergMarquardtParams.h b/gtsam/nonlinear/LevenbergMarquardtParams.h index abb8c3c22..4962ad366 100644 --- a/gtsam/nonlinear/LevenbergMarquardtParams.h +++ b/gtsam/nonlinear/LevenbergMarquardtParams.h @@ -122,24 +122,35 @@ public: virtual ~LevenbergMarquardtParams() {} void print(const std::string& str = "") const override; - /// @name Getters/Setters, mainly for MATLAB. Use fields above in C++. + /// @name Getters/Setters, mainly for wrappers. Use fields above in C++. /// @{ bool getDiagonalDamping() const { return diagonalDamping; } double getlambdaFactor() const { return lambdaFactor; } double getlambdaInitial() const { return lambdaInitial; } double getlambdaLowerBound() const { return lambdaLowerBound; } double getlambdaUpperBound() const { return lambdaUpperBound; } + bool getUseFixedLambdaFactor() { return useFixedLambdaFactor; } std::string getLogFile() const { return logFile; } std::string getVerbosityLM() const { return verbosityLMTranslator(verbosityLM);} + void setDiagonalDamping(bool flag) { diagonalDamping = flag; } void setlambdaFactor(double value) { lambdaFactor = value; } void setlambdaInitial(double value) { lambdaInitial = value; } void setlambdaLowerBound(double value) { lambdaLowerBound = value; } void setlambdaUpperBound(double value) { lambdaUpperBound = value; } - void setLogFile(const std::string& s) { logFile = s; } void setUseFixedLambdaFactor(bool flag) { useFixedLambdaFactor = flag;} + void setLogFile(const std::string& s) { logFile = s; } void setVerbosityLM(const std::string& s) { verbosityLM = verbosityLMTranslator(s);} // @} + /// @name Clone + /// @{ + + /// @return a deep copy of this object + boost::shared_ptr clone() const { + return boost::shared_ptr(new LevenbergMarquardtParams(*this)); + } + + /// @} }; } diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index 3082b9097..3018a14b9 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -54,47 +54,24 @@ public: } virtual void print(const std::string& str = "") const; - size_t getMaxIterations() const { - return maxIterations; - } - double getRelativeErrorTol() const { - return relativeErrorTol; - } - double getAbsoluteErrorTol() const { - return absoluteErrorTol; - } - double getErrorTol() const { - return errorTol; - } - std::string getVerbosity() const { - return verbosityTranslator(verbosity); - } + size_t getMaxIterations() const { return maxIterations; } + double getRelativeErrorTol() const { return relativeErrorTol; } + double getAbsoluteErrorTol() const { return absoluteErrorTol; } + double getErrorTol() const { return errorTol; } + std::string getVerbosity() const { return verbosityTranslator(verbosity); } - void setMaxIterations(int value) { - maxIterations = value; - } - void setRelativeErrorTol(double value) { - relativeErrorTol = value; - } - void setAbsoluteErrorTol(double value) { - absoluteErrorTol = value; - } - void setErrorTol(double value) { - errorTol = value; - } - void setVerbosity(const std::string &src) { + void setMaxIterations(int value) { maxIterations = value; } + void setRelativeErrorTol(double value) { relativeErrorTol = value; } + void setAbsoluteErrorTol(double value) { absoluteErrorTol = value; } + void setErrorTol(double value) { errorTol = value; } + void setVerbosity(const std::string& src) { verbosity = verbosityTranslator(src); } static Verbosity verbosityTranslator(const std::string &s) ; static std::string verbosityTranslator(Verbosity value) ; - // Successive Linearization Parameters - -public: - /** See NonlinearOptimizerParams::linearSolverType */ - enum LinearSolverType { MULTIFRONTAL_CHOLESKY, MULTIFRONTAL_QR, @@ -168,13 +145,9 @@ public: private: std::string linearSolverTranslator(LinearSolverType linearSolverType) const; - LinearSolverType linearSolverTranslator(const std::string& linearSolverType) const; - std::string orderingTypeTranslator(Ordering::OrderingType type) const; - Ordering::OrderingType orderingTypeTranslator(const std::string& type) const; - }; // For backward compatibility: diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index ea407a4ca..9c412eb19 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -56,7 +56,7 @@ namespace gtsam { /** Constructor */ BetweenFactor(Key key1, Key key2, const VALUE& measured, - const SharedNoiseModel& model) : + const SharedNoiseModel& model = nullptr) : Base(model, key1, key2), measured_(measured) { } diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index 3979996da..a1baab5fa 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -16,7 +16,7 @@ * @date August, 2014 */ -#include +#include #include #include #include @@ -29,69 +29,69 @@ using namespace std; namespace gtsam { -namespace InitializePose3 { -static const Matrix I9 = I_9x9; -static const Vector zero9 = Vector::Zero(9); -static const Matrix zero33 = Z_3x3; - -static const Key keyAnchor = symbol('Z', 9999999); +static const Key kAnchorKey = symbol('Z', 9999999); /* ************************************************************************* */ -GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) { +GaussianFactorGraph InitializePose3::buildLinearOrientationGraph(const NonlinearFactorGraph& g) { GaussianFactorGraph linearGraph; - noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(9); - for(const boost::shared_ptr& factor: g) { + for(const auto& factor: g) { Matrix3 Rij; + double rotationPrecision = 1.0; - boost::shared_ptr > pose3Between = - boost::dynamic_pointer_cast >(factor); - if (pose3Between) + auto pose3Between = boost::dynamic_pointer_cast >(factor); + if (pose3Between){ Rij = pose3Between->measured().rotation().matrix(); - else - std::cout << "Error in buildLinearOrientationGraph" << std::endl; + Vector precisions = Vector::Zero(6); + precisions[0] = 1.0; // vector of all zeros except first entry equal to 1 + pose3Between->noiseModel()->whitenInPlace(precisions); // gets marginal precision of first variable + rotationPrecision = precisions[0]; // rotations first + }else{ + cout << "Error in buildLinearOrientationGraph" << endl; + } - const KeyVector& keys = factor->keys(); + const auto& keys = factor->keys(); Key key1 = keys[0], key2 = keys[1]; Matrix M9 = Z_9x9; M9.block(0,0,3,3) = Rij; M9.block(3,3,3,3) = Rij; M9.block(6,6,3,3) = Rij; - linearGraph.add(key1, -I9, key2, M9, zero9, model); + linearGraph.add(key1, -I_9x9, key2, M9, Z_9x1, noiseModel::Isotropic::Precision(9, rotationPrecision)); } // prior on the anchor orientation - linearGraph.add(keyAnchor, I9, (Vector(9) << 1.0, 0.0, 0.0,/* */ 0.0, 1.0, 0.0, /* */ 0.0, 0.0, 1.0).finished(), model); + linearGraph.add( + kAnchorKey, I_9x9, + (Vector(9) << 1.0, 0.0, 0.0, /* */ 0.0, 1.0, 0.0, /* */ 0.0, 0.0, 1.0) + .finished(), + noiseModel::Isotropic::Precision(9, 1)); return linearGraph; } /* ************************************************************************* */ // Transform VectorValues into valid Rot3 -Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) { +Values InitializePose3::normalizeRelaxedRotations( + const VectorValues& relaxedRot3) { gttic(InitializePose3_computeOrientationsChordal); Matrix ppm = Z_3x3; // plus plus minus ppm(0,0) = 1; ppm(1,1) = 1; ppm(2,2) = -1; Values validRot3; - for(const VectorValues::value_type& it: relaxedRot3) { + for(const auto& it: relaxedRot3) { Key key = it.first; - if (key != keyAnchor) { - const Vector& rotVector = it.second; - Matrix3 rotMat; - rotMat(0,0) = rotVector(0); rotMat(0,1) = rotVector(1); rotMat(0,2) = rotVector(2); - rotMat(1,0) = rotVector(3); rotMat(1,1) = rotVector(4); rotMat(1,2) = rotVector(5); - rotMat(2,0) = rotVector(6); rotMat(2,1) = rotVector(7); rotMat(2,2) = rotVector(8); + if (key != kAnchorKey) { + Matrix3 R; + R << Eigen::Map(it.second.data()); // Recover M from vectorized + + // ClosestTo finds rotation matrix closest to H in Frobenius sense + // Rot3 initRot = Rot3::ClosestTo(M.transpose()); Matrix U, V; Vector s; - svd(rotMat, U, s, V); + svd(R.transpose(), U, s, V); Matrix3 normalizedRotMat = U * V.transpose(); - // std::cout << "rotMat \n" << rotMat << std::endl; - // std::cout << "U V' \n" << U * V.transpose() << std::endl; - // std::cout << "V \n" << V << std::endl; - if(normalizedRotMat.determinant() < 0) normalizedRotMat = U * ppm * V.transpose(); @@ -103,32 +103,29 @@ Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) { } /* ************************************************************************* */ -// Select the subgraph of betweenFactors and transforms priors into between wrt a fictitious node -NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph) { +NonlinearFactorGraph InitializePose3::buildPose3graph(const NonlinearFactorGraph& graph) { gttic(InitializePose3_buildPose3graph); NonlinearFactorGraph pose3Graph; - for(const boost::shared_ptr& factor: graph) { + for(const auto& factor: graph) { // recast to a between on Pose3 - boost::shared_ptr > pose3Between = - boost::dynamic_pointer_cast >(factor); + const auto pose3Between = boost::dynamic_pointer_cast >(factor); if (pose3Between) pose3Graph.add(pose3Between); // recast PriorFactor to BetweenFactor - boost::shared_ptr > pose3Prior = - boost::dynamic_pointer_cast >(factor); + const auto pose3Prior = boost::dynamic_pointer_cast >(factor); if (pose3Prior) - pose3Graph.emplace_shared >(keyAnchor, pose3Prior->keys()[0], + pose3Graph.emplace_shared >(kAnchorKey, pose3Prior->keys()[0], pose3Prior->prior(), pose3Prior->noiseModel()); } return pose3Graph; } /* ************************************************************************* */ -// Return the orientations of a graph including only BetweenFactors -Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph) { +Values InitializePose3::computeOrientationsChordal( + const NonlinearFactorGraph& pose3Graph) { gttic(InitializePose3_computeOrientationsChordal); // regularize measurements and plug everything in a factor graph @@ -142,14 +139,15 @@ Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph) { } /* ************************************************************************* */ -// Return the orientations of a graph including only BetweenFactors -Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const Values& givenGuess, const size_t maxIter, const bool setRefFrame) { +Values InitializePose3::computeOrientationsGradient( + const NonlinearFactorGraph& pose3Graph, const Values& givenGuess, + const size_t maxIter, const bool setRefFrame) { gttic(InitializePose3_computeOrientationsGradient); // this works on the inverse rotations, according to Tron&Vidal,2011 Values inverseRot; - inverseRot.insert(keyAnchor, Rot3()); - for(const Values::ConstKeyValuePair& key_value: givenGuess) { + inverseRot.insert(kAnchorKey, Rot3()); + for(const auto& key_value: givenGuess) { Key key = key_value.key; const Pose3& pose = givenGuess.at(key); inverseRot.insert(key, pose.rotation().inverse()); @@ -164,9 +162,9 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const // calculate max node degree & allocate gradient size_t maxNodeDeg = 0; VectorValues grad; - for(const Values::ConstKeyValuePair& key_value: inverseRot) { + for(const auto& key_value: inverseRot) { Key key = key_value.key; - grad.insert(key,Vector3::Zero()); + grad.insert(key,Z_3x1); size_t currNodeDeg = (adjEdgesMap.at(key)).size(); if(currNodeDeg > maxNodeDeg) maxNodeDeg = currNodeDeg; @@ -180,42 +178,37 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const double mu_max = maxNodeDeg * rho; double stepsize = 2/mu_max; // = 1/(a b dG) - std::cout <<" b " << b <<" f0 " << f0 <<" a " << a <<" rho " << rho <<" stepsize " << stepsize << " maxNodeDeg "<< maxNodeDeg << std::endl; double maxGrad; // gradient iterations size_t it; - for(it=0; it < maxIter; it++){ + for (it = 0; it < maxIter; it++) { ////////////////////////////////////////////////////////////////////////// // compute the gradient at each node - //std::cout << "it " << it <<" b " << b <<" f0 " << f0 <<" a " << a - // <<" rho " << rho <<" stepsize " << stepsize << " maxNodeDeg "<< maxNodeDeg << std::endl; maxGrad = 0; - for(const Values::ConstKeyValuePair& key_value: inverseRot) { + for (const auto& key_value : inverseRot) { Key key = key_value.key; - //std::cout << "---------------------------key " << DefaultKeyFormatter(key) << std::endl; - Vector gradKey = Vector3::Zero(); + Vector gradKey = Z_3x1; // collect the gradient for each edge incident on key - for(const size_t& factorId: adjEdgesMap.at(key)){ + for (const size_t& factorId : adjEdgesMap.at(key)) { Rot3 Rij = factorId2RotMap.at(factorId); Rot3 Ri = inverseRot.at(key); - if( key == (pose3Graph.at(factorId))->keys()[0] ){ - Key key1 = (pose3Graph.at(factorId))->keys()[1]; + auto factor = pose3Graph.at(factorId); + const auto& keys = factor->keys(); + if (key == keys[0]) { + Key key1 = keys[1]; Rot3 Rj = inverseRot.at(key1); - gradKey = gradKey + gradientTron(Ri, Rij * Rj, a, b); - //std::cout << "key1 " << DefaultKeyFormatter(key1) << " gradientTron(Ri, Rij * Rj, a, b) \n " << gradientTron(Ri, Rij * Rj, a, b) << std::endl; - }else if( key == (pose3Graph.at(factorId))->keys()[1] ){ - Key key0 = (pose3Graph.at(factorId))->keys()[0]; + gradKey = gradKey + gradientTron(Ri, Rij * Rj, a, b); + } else if (key == keys[1]) { + Key key0 = keys[0]; Rot3 Rj = inverseRot.at(key0); gradKey = gradKey + gradientTron(Ri, Rij.between(Rj), a, b); - //std::cout << "key0 " << DefaultKeyFormatter(key0) << " gradientTron(Ri, Rij.inverse() * Rj, a, b) \n " << gradientTron(Ri, Rij.between(Rj), a, b) << std::endl; - }else{ - std::cout << "Error in gradient computation" << std::endl; + } else { + cout << "Error in gradient computation" << endl; } - } // end of i-th gradient computation - grad.at(key) = stepsize * gradKey; + } // end of i-th gradient computation + grad.at(key) = stepsize * gradKey; double normGradKey = (gradKey).norm(); - //std::cout << "key " << DefaultKeyFormatter(key) <<" \n grad \n" << grad.at(key) << std::endl; if(normGradKey>maxGrad) maxGrad = normGradKey; } // end of loop over nodes @@ -230,14 +223,12 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const break; } // enf of gradient iterations - std::cout << "nr of gradient iterations " << it << "maxGrad " << maxGrad << std::endl; - // Return correct rotations - const Rot3& Rref = inverseRot.at(keyAnchor); // This will be set to the identity as so far we included no prior + const Rot3& Rref = inverseRot.at(kAnchorKey); // This will be set to the identity as so far we included no prior Values estimateRot; - for(const Values::ConstKeyValuePair& key_value: inverseRot) { + for(const auto& key_value: inverseRot) { Key key = key_value.key; - if (key != keyAnchor) { + if (key != kAnchorKey) { const Rot3& R = inverseRot.at(key); if(setRefFrame) estimateRot.insert(key, Rref.compose(R.inverse())); @@ -249,11 +240,11 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const } /* ************************************************************************* */ -void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, const NonlinearFactorGraph& pose3Graph){ +void InitializePose3::createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, + const NonlinearFactorGraph& pose3Graph) { size_t factorId = 0; - for(const boost::shared_ptr& factor: pose3Graph) { - boost::shared_ptr > pose3Between = - boost::dynamic_pointer_cast >(factor); + for(const auto& factor: pose3Graph) { + auto pose3Between = boost::dynamic_pointer_cast >(factor); if (pose3Between){ Rot3 Rij = pose3Between->measured().rotation(); factorId2RotMap.insert(pair(factorId,Rij)); @@ -275,14 +266,14 @@ void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, adjEdgesMap.insert(pair >(key2, edge_id)); } }else{ - std::cout << "Error in computeOrientationsGradient" << std::endl; + cout << "Error in createSymbolicGraph" << endl; } factorId++; } } /* ************************************************************************* */ -Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const double b) { +Vector3 InitializePose3::gradientTron(const Rot3& R1, const Rot3& R2, const double a, const double b) { Vector3 logRot = Rot3::Logmap(R1.between(R2)); double th = logRot.norm(); @@ -292,10 +283,10 @@ Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const doubl th = logRot.norm(); } // exclude small or invalid rotations - if (th > 1e-5 && th == th){ // nonzero valid rotations + if (th > 1e-5 && th == th) { // nonzero valid rotations logRot = logRot / th; - }else{ - logRot = Vector3::Zero(); + } else { + logRot = Z_3x1; th = 0.0; } @@ -304,8 +295,7 @@ Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const doubl } /* ************************************************************************* */ -Values initializeOrientations(const NonlinearFactorGraph& graph) { - +Values InitializePose3::initializeOrientations(const NonlinearFactorGraph& graph) { // We "extract" the Pose3 subgraph of the original graph: this // is done to properly model priors and avoiding operating on a larger graph NonlinearFactorGraph pose3Graph = buildPose3graph(graph); @@ -315,29 +305,30 @@ Values initializeOrientations(const NonlinearFactorGraph& graph) { } ///* ************************************************************************* */ -Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) { +Values InitializePose3::computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) { gttic(InitializePose3_computePoses); // put into Values structure Values initialPose; - for(const Values::ConstKeyValuePair& key_value: initialRot){ + for (const auto& key_value : initialRot) { Key key = key_value.key; const Rot3& rot = initialRot.at(key); Pose3 initializedPose = Pose3(rot, Point3(0, 0, 0)); initialPose.insert(key, initializedPose); } + // add prior noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6); - initialPose.insert(keyAnchor, Pose3()); - pose3graph.emplace_shared >(keyAnchor, Pose3(), priorModel); + initialPose.insert(kAnchorKey, Pose3()); + pose3graph.emplace_shared >(kAnchorKey, Pose3(), priorModel); // Create optimizer GaussNewtonParams params; bool singleIter = true; - if(singleIter){ + if (singleIter) { params.maxIterations = 1; - }else{ - std::cout << " \n\n\n\n performing more than 1 GN iterations \n\n\n" <(key); estimate.insert(key, pose); } @@ -356,22 +347,9 @@ Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) { } /* ************************************************************************* */ -Values initialize(const NonlinearFactorGraph& graph) { +Values InitializePose3::initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, + bool useGradient) { gttic(InitializePose3_initialize); - - // We "extract" the Pose3 subgraph of the original graph: this - // is done to properly model priors and avoiding operating on a larger graph - NonlinearFactorGraph pose3Graph = buildPose3graph(graph); - - // Get orientations from relative orientation measurements - Values valueRot3 = computeOrientationsChordal(pose3Graph); - - // Compute the full poses (1 GN iteration on full poses) - return computePoses(pose3Graph, valueRot3); -} - -/* ************************************************************************* */ -Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, bool useGradient) { Values initialValues; // We "extract" the Pose3 subgraph of the original graph: this @@ -380,27 +358,18 @@ Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, b // Get orientations from relative orientation measurements Values orientations; - if(useGradient) + if (useGradient) orientations = computeOrientationsGradient(pose3Graph, givenGuess); else orientations = computeOrientationsChordal(pose3Graph); -// orientations.print("orientations\n"); - // Compute the full poses (1 GN iteration on full poses) return computePoses(pose3Graph, orientations); - - // for(const Values::ConstKeyValuePair& key_value: orientations) { - // Key key = key_value.key; - // if (key != keyAnchor) { - // const Point3& pos = givenGuess.at(key).translation(); - // const Rot3& rot = orientations.at(key); - // Pose3 initializedPoses = Pose3(rot, pos); - // initialValues.insert(key, initializedPoses); - // } - // } - // return initialValues; } -} // end of namespace lago -} // end of namespace gtsam +/* ************************************************************************* */ +Values InitializePose3::initialize(const NonlinearFactorGraph& graph) { + return initialize(graph, Values(), false); +} + +} // namespace gtsam diff --git a/gtsam/slam/InitializePose3.h b/gtsam/slam/InitializePose3.h index dba5ceac3..ce1b28854 100644 --- a/gtsam/slam/InitializePose3.h +++ b/gtsam/slam/InitializePose3.h @@ -20,40 +20,68 @@ #pragma once -#include +#include +#include #include #include -#include -#include +#include namespace gtsam { typedef std::map > KeyVectorMap; -typedef std::map KeyRotMap; +typedef std::map KeyRotMap; -namespace InitializePose3 { +struct GTSAM_EXPORT InitializePose3 { + static GaussianFactorGraph buildLinearOrientationGraph( + const NonlinearFactorGraph& g); -GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g); + static Values normalizeRelaxedRotations(const VectorValues& relaxedRot3); -GTSAM_EXPORT Values normalizeRelaxedRotations(const VectorValues& relaxedRot3); + /** + * Return the orientations of a graph including only BetweenFactors + */ + static Values computeOrientationsChordal( + const NonlinearFactorGraph& pose3Graph); -GTSAM_EXPORT Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph); + /** + * Return the orientations of a graph including only BetweenFactors + */ + static Values computeOrientationsGradient( + const NonlinearFactorGraph& pose3Graph, const Values& givenGuess, + size_t maxIter = 10000, const bool setRefFrame = true); -GTSAM_EXPORT Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, - const Values& givenGuess, size_t maxIter = 10000, const bool setRefFrame = true); + static void createSymbolicGraph(KeyVectorMap& adjEdgesMap, + KeyRotMap& factorId2RotMap, + const NonlinearFactorGraph& pose3Graph); -GTSAM_EXPORT void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, - const NonlinearFactorGraph& pose3Graph); + static Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, + const double b); -GTSAM_EXPORT Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const double b); + /** + * Select the subgraph of betweenFactors and transforms priors into between + * wrt a fictitious node + */ + static NonlinearFactorGraph buildPose3graph( + const NonlinearFactorGraph& graph); -GTSAM_EXPORT NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph); + static Values computePoses(NonlinearFactorGraph& pose3graph, + Values& initialRot); -GTSAM_EXPORT Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot); + /** + * "extract" the Pose3 subgraph of the original graph, get orientations from + * relative orientation measurements using chordal method. + */ + static Values initializeOrientations(const NonlinearFactorGraph& graph); -GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph); + /** + * "extract" the Pose3 subgraph of the original graph, get orientations from + * relative orientation measurements (using either gradient or chordal + * method), and finish up with 1 GN iteration on full poses. + */ + static Values initialize(const NonlinearFactorGraph& graph, + const Values& givenGuess, bool useGradient = false); -GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, bool useGradient = false); - -} // end of namespace lago -} // end of namespace gtsam + /// Calls initialize above using Chordal method. + static Values initialize(const NonlinearFactorGraph& graph); +}; +} // end of namespace gtsam diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h index e2fa04a93..8bb7a3ab8 100644 --- a/gtsam/slam/PriorFactor.h +++ b/gtsam/slam/PriorFactor.h @@ -53,7 +53,7 @@ namespace gtsam { virtual ~PriorFactor() {} /** Constructor */ - PriorFactor(Key key, const VALUE& prior, const SharedNoiseModel& model) : + PriorFactor(Key key, const VALUE& prior, const SharedNoiseModel& model = nullptr) : Base(model, key), prior_(prior) { } diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index e554e0c85..cbeed77c5 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -183,12 +183,9 @@ public: std::vector Gs(numKeys * (numKeys + 1) / 2); std::vector gs(numKeys); - if (this->measured_.size() != cameras.size()) { - std::cout - << "SmartProjectionHessianFactor: this->measured_.size() inconsistent with input" - << std::endl; - exit(1); - } + if (this->measured_.size() != cameras.size()) + throw std::runtime_error("SmartProjectionHessianFactor: this->measured_" + ".size() inconsistent with input"); triangulateSafe(cameras); diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 6efd01feb..b66f22ced 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -51,7 +51,6 @@ using namespace gtsam::symbol_shorthand; namespace gtsam { -#ifndef MATLAB_MEX_FILE /* ************************************************************************* */ string findExampleDataFile(const string& name) { // Search source tree and installed location @@ -67,6 +66,7 @@ string findExampleDataFile(const string& name) { namesToSearch.push_back(name + ".graph"); namesToSearch.push_back(name + ".txt"); namesToSearch.push_back(name + ".out"); + namesToSearch.push_back(name + ".xml"); // Find first name that exists for(const fs::path& root: rootsToSearch) { @@ -100,9 +100,6 @@ string createRewrittenFileName(const string& name) { return newpath.string(); } -/* ************************************************************************* */ -#endif - /* ************************************************************************* */ GraphAndValues load2D(pair dataset, int maxID, bool addNoise, bool smart, NoiseFormat noiseFormat, @@ -413,17 +410,17 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config, /* ************************************************************************* */ GraphAndValues readG2o(const string& g2oFile, const bool is3D, - KernelFunctionType kernelFunctionType) { - // just call load2D - int maxID = 0; - bool addNoise = false; - bool smart = true; - - if(is3D) + KernelFunctionType kernelFunctionType) { + if (is3D) { return load3D(g2oFile); - - return load2D(g2oFile, SharedNoiseModel(), maxID, addNoise, smart, - NoiseFormatG2O, kernelFunctionType); + } else { + // just call load2D + int maxID = 0; + bool addNoise = false; + bool smart = true; + return load2D(g2oFile, SharedNoiseModel(), maxID, addNoise, smart, + NoiseFormatG2O, kernelFunctionType); + } } /* ************************************************************************* */ @@ -514,15 +511,12 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, } /* ************************************************************************* */ -GraphAndValues load3D(const string& filename) { - +std::map parse3DPoses(const string& filename) { ifstream is(filename.c_str()); if (!is) - throw invalid_argument("load3D: can not find file " + filename); - - Values::shared_ptr initial(new Values); - NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph); + throw invalid_argument("parse3DPoses: can not find file " + filename); + std::map poses; while (!is.eof()) { char buf[LINESIZE]; is.getline(buf, LINESIZE); @@ -534,22 +528,24 @@ GraphAndValues load3D(const string& filename) { Key id; double x, y, z, roll, pitch, yaw; ls >> id >> x >> y >> z >> roll >> pitch >> yaw; - Rot3 R = Rot3::Ypr(yaw,pitch,roll); - Point3 t = Point3(x, y, z); - initial->insert(id, Pose3(R,t)); + poses.emplace(id, Pose3(Rot3::Ypr(yaw, pitch, roll), {x, y, z})); } if (tag == "VERTEX_SE3:QUAT") { Key id; double x, y, z, qx, qy, qz, qw; ls >> id >> x >> y >> z >> qx >> qy >> qz >> qw; - Rot3 R = Rot3::Quaternion(qw, qx, qy, qz); - Point3 t = Point3(x, y, z); - initial->insert(id, Pose3(R,t)); + poses.emplace(id, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z})); } } - is.clear(); /* clears the end-of-file and error flags */ - is.seekg(0, ios::beg); + return poses; +} +/* ************************************************************************* */ +BetweenFactorPose3s parse3DFactors(const string& filename) { + ifstream is(filename.c_str()); + if (!is) throw invalid_argument("parse3DFactors: can not find file " + filename); + + std::vector::shared_ptr> factors; while (!is.eof()) { char buf[LINESIZE]; is.getline(buf, LINESIZE); @@ -561,44 +557,55 @@ GraphAndValues load3D(const string& filename) { Key id1, id2; double x, y, z, roll, pitch, yaw; ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw; - Rot3 R = Rot3::Ypr(yaw,pitch,roll); - Point3 t = Point3(x, y, z); - Matrix m = I_6x6; - for (int i = 0; i < 6; i++) - for (int j = i; j < 6; j++) - ls >> m(i, j); + Matrix m(6, 6); + for (size_t i = 0; i < 6; i++) + for (size_t j = i; j < 6; j++) ls >> m(i, j); SharedNoiseModel model = noiseModel::Gaussian::Information(m); - NonlinearFactor::shared_ptr factor( - new BetweenFactor(id1, id2, Pose3(R,t), model)); - graph->push_back(factor); + factors.emplace_back(new BetweenFactor( + id1, id2, Pose3(Rot3::Ypr(yaw, pitch, roll), {x, y, z}), model)); } if (tag == "EDGE_SE3:QUAT") { - Matrix m = I_6x6; Key id1, id2; double x, y, z, qx, qy, qz, qw; ls >> id1 >> id2 >> x >> y >> z >> qx >> qy >> qz >> qw; - Rot3 R = Rot3::Quaternion(qw, qx, qy, qz); - Point3 t = Point3(x, y, z); - for (int i = 0; i < 6; i++){ - for (int j = i; j < 6; j++){ + Matrix m(6, 6); + for (size_t i = 0; i < 6; i++) { + for (size_t j = i; j < 6; j++) { double mij; ls >> mij; m(i, j) = mij; m(j, i) = mij; } } - Matrix mgtsam = I_6x6; + Matrix mgtsam(6, 6); - mgtsam.block<3,3>(0,0) = m.block<3,3>(3,3); // cov rotation - mgtsam.block<3,3>(3,3) = m.block<3,3>(0,0); // cov translation - mgtsam.block<3,3>(0,3) = m.block<3,3>(0,3); // off diagonal - mgtsam.block<3,3>(3,0) = m.block<3,3>(3,0); // off diagonal + mgtsam.block<3, 3>(0, 0) = m.block<3, 3>(3, 3); // cov rotation + mgtsam.block<3, 3>(3, 3) = m.block<3, 3>(0, 0); // cov translation + mgtsam.block<3, 3>(0, 3) = m.block<3, 3>(0, 3); // off diagonal + mgtsam.block<3, 3>(3, 0) = m.block<3, 3>(3, 0); // off diagonal SharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam); - NonlinearFactor::shared_ptr factor(new BetweenFactor(id1, id2, Pose3(R,t), model)); - graph->push_back(factor); + factors.emplace_back(new BetweenFactor( + id1, id2, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z}), model)); } } + return factors; +} + +/* ************************************************************************* */ +GraphAndValues load3D(const string& filename) { + const auto factors = parse3DFactors(filename); + NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph); + for (const auto& factor : factors) { + graph->push_back(factor); + } + + const auto poses = parse3DPoses(filename); + Values::shared_ptr initial(new Values); + for (const auto& key_pose : poses) { + initial->insert(key_pose.first, key_pose.second); + } + return make_pair(graph, initial); } diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 929ada2c1..2106df48d 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -18,6 +18,7 @@ #pragma once +#include #include #include #include @@ -34,10 +35,10 @@ #include // for pair #include #include +#include namespace gtsam { -#ifndef MATLAB_MEX_FILE /** * Find the full path to an example dataset distributed with gtsam. The name * may be specified with or without a file extension - if no extension is @@ -56,7 +57,6 @@ GTSAM_EXPORT std::string findExampleDataFile(const std::string& name); * for checking read-write oprations */ GTSAM_EXPORT std::string createRewrittenFileName(const std::string& name); -#endif /// Indicates how noise parameters are stored in file enum NoiseFormat { @@ -155,9 +155,14 @@ GTSAM_EXPORT GraphAndValues readG2o(const std::string& g2oFile, const bool is3D GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, const std::string& filename); -/** - * Load TORO 3D Graph - */ +/// Parse edges in 3D TORO graph file into a set of BetweenFactors. +using BetweenFactorPose3s = std::vector::shared_ptr>; +GTSAM_EXPORT BetweenFactorPose3s parse3DFactors(const std::string& filename); + +/// Parse vertices in 3D TORO graph file into a map of Pose3s. +GTSAM_EXPORT std::map parse3DPoses(const std::string& filename); + +/// Load TORO 3D Graph GTSAM_EXPORT GraphAndValues load3D(const std::string& filename); /// A measurement with its camera index diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 39c2d3722..9434280d4 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -16,8 +16,8 @@ */ -#include #include +#include #include #include #include @@ -162,65 +162,74 @@ TEST( dataSet, readG2o) } /* ************************************************************************* */ -TEST( dataSet, readG2o3D) -{ +TEST(dataSet, readG2o3D) { const string g2oFile = findExampleDataFile("pose3example"); + auto model = noiseModel::Isotropic::Precision(6, 10000); + + // Initialize 6 relative measurements with quaternion/point3 values: + const std::vector relative_poses = { + {{0.854230, 0.190253, 0.283162, -0.392318}, + {1.001367, 0.015390, 0.004948}}, + {{0.105373, 0.311512, 0.656877, -0.678505}, + {0.523923, 0.776654, 0.326659}}, + {{0.568551, 0.595795, -0.561677, 0.079353}, + {0.910927, 0.055169, -0.411761}}, + {{0.542221, -0.592077, 0.303380, -0.513226}, + {0.775288, 0.228798, -0.596923}}, + {{0.327419, -0.125250, -0.534379, 0.769122}, + {-0.577841, 0.628016, -0.543592}}, + {{0.083672, 0.104639, 0.627755, 0.766795}, + {-0.623267, 0.086928, 0.773222}}, + }; + + // Initialize 5 poses with quaternion/point3 values: + const std::vector poses = { + {{1.000000, 0.000000, 0.000000, 0.000000}, {0, 0, 0}}, + {{0.854230, 0.190253, 0.283162, -0.392318}, + {1.001367, 0.015390, 0.004948}}, + {{0.421446, -0.351729, -0.597838, 0.584174}, + {1.993500, 0.023275, 0.003793}}, + {{0.067024, 0.331798, -0.200659, 0.919323}, + {2.004291, 1.024305, 0.018047}}, + {{0.765488, -0.035697, -0.462490, 0.445933}, + {0.999908, 1.055073, 0.020212}}, + }; + + // Specify connectivity: + using KeyPair = pair; + std::vector edges = {{0, 1}, {1, 2}, {2, 3}, {3, 4}, {1, 4}, {3, 0}}; + + // Created expected factor graph + size_t i = 0; + NonlinearFactorGraph expectedGraph; + for (const auto& keys : edges) { + expectedGraph.emplace_shared>( + keys.first, keys.second, relative_poses[i++], model); + } + + // Check factor parsing + const auto actualFactors = parse3DFactors(g2oFile); + for (size_t i : {0, 1, 2, 3, 4, 5}) { + EXPECT(assert_equal( + *boost::dynamic_pointer_cast>(expectedGraph[i]), + *actualFactors[i], 1e-5)); + } + + // Check pose parsing + const auto actualPoses = parse3DPoses(g2oFile); + for (size_t j : {0, 1, 2, 3, 4}) { + EXPECT(assert_equal(poses[j], actualPoses.at(j), 1e-5)); + } + + // Check graph version NonlinearFactorGraph::shared_ptr actualGraph; Values::shared_ptr actualValues; bool is3D = true; boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D); - - Values expectedValues; - Rot3 R0 = Rot3::Quaternion(1.000000, 0.000000, 0.000000, 0.000000 ); - Point3 p0 = Point3(0.000000, 0.000000, 0.000000); - expectedValues.insert(0, Pose3(R0, p0)); - - Rot3 R1 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); - Point3 p1 = Point3(1.001367, 0.015390, 0.004948); - expectedValues.insert(1, Pose3(R1, p1)); - - Rot3 R2 = Rot3::Quaternion(0.421446, -0.351729, -0.597838, 0.584174 ); - Point3 p2 = Point3(1.993500, 0.023275, 0.003793); - expectedValues.insert(2, Pose3(R2, p2)); - - Rot3 R3 = Rot3::Quaternion(0.067024, 0.331798, -0.200659, 0.919323); - Point3 p3 = Point3(2.004291, 1.024305, 0.018047); - expectedValues.insert(3, Pose3(R3, p3)); - - Rot3 R4 = Rot3::Quaternion(0.765488, -0.035697, -0.462490, 0.445933); - Point3 p4 = Point3(0.999908, 1.055073, 0.020212); - expectedValues.insert(4, Pose3(R4, p4)); - - EXPECT(assert_equal(expectedValues,*actualValues,1e-5)); - - noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions((Vector(6) << 10000.0,10000.0,10000.0,10000.0,10000.0,10000.0).finished()); - NonlinearFactorGraph expectedGraph; - - Point3 p01 = Point3(1.001367, 0.015390, 0.004948); - Rot3 R01 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); - expectedGraph.emplace_shared >(0, 1, Pose3(R01,p01), model); - - Point3 p12 = Point3(0.523923, 0.776654, 0.326659); - Rot3 R12 = Rot3::Quaternion(0.105373 , 0.311512, 0.656877, -0.678505 ); - expectedGraph.emplace_shared >(1, 2, Pose3(R12,p12), model); - - Point3 p23 = Point3(0.910927, 0.055169, -0.411761); - Rot3 R23 = Rot3::Quaternion(0.568551 , 0.595795, -0.561677, 0.079353 ); - expectedGraph.emplace_shared >(2, 3, Pose3(R23,p23), model); - - Point3 p34 = Point3(0.775288, 0.228798, -0.596923); - Rot3 R34 = Rot3::Quaternion(0.542221 , -0.592077, 0.303380, -0.513226 ); - expectedGraph.emplace_shared >(3, 4, Pose3(R34,p34), model); - - Point3 p14 = Point3(-0.577841, 0.628016, -0.543592); - Rot3 R14 = Rot3::Quaternion(0.327419 , -0.125250, -0.534379, 0.769122 ); - expectedGraph.emplace_shared >(1, 4, Pose3(R14,p14), model); - - Point3 p30 = Point3(-0.623267, 0.086928, 0.773222); - Rot3 R30 = Rot3::Quaternion(0.083672 , 0.104639, 0.627755, 0.766795 ); - expectedGraph.emplace_shared >(3, 0, Pose3(R30,p30), model); - - EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5)); + EXPECT(assert_equal(expectedGraph, *actualGraph, 1e-5)); + for (size_t j : {0, 1, 2, 3, 4}) { + EXPECT(assert_equal(poses[j], actualValues->at(j), 1e-5)); + } } /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testInitializePose3.cpp b/gtsam/slam/tests/testInitializePose3.cpp index e6f08286a..6fe8b3d7c 100644 --- a/gtsam/slam/tests/testInitializePose3.cpp +++ b/gtsam/slam/tests/testInitializePose3.cpp @@ -70,6 +70,17 @@ NonlinearFactorGraph graph() { g.add(PriorFactor(x0, pose0, model)); return g; } + +NonlinearFactorGraph graph2() { + NonlinearFactorGraph g; + g.add(BetweenFactor(x0, x1, pose0.between(pose1), noiseModel::Isotropic::Precision(6, 1.0))); + g.add(BetweenFactor(x1, x2, pose1.between(pose2), noiseModel::Isotropic::Precision(6, 1.0))); + g.add(BetweenFactor(x2, x3, pose2.between(pose3), noiseModel::Isotropic::Precision(6, 1.0))); + g.add(BetweenFactor(x2, x0, Pose3(Rot3::Ypr(0.1,0,0.1), Point3()), noiseModel::Isotropic::Precision(6, 0.0))); // random pose, but zero information + g.add(BetweenFactor(x0, x3, Pose3(Rot3::Ypr(0.5,-0.2,0.2), Point3(10,20,30)), noiseModel::Isotropic::Precision(6, 0.0))); // random pose, but zero informatoin + g.add(PriorFactor(x0, pose0, model)); + return g; +} } /* *************************************************************************** */ @@ -91,6 +102,19 @@ TEST( InitializePose3, orientations ) { EXPECT(assert_equal(simple::R3, initial.at(x3), 1e-6)); } +/* *************************************************************************** */ +TEST( InitializePose3, orientationsPrecisions ) { + NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph2()); + + Values initial = InitializePose3::computeOrientationsChordal(pose3Graph); + + // comparison is up to M_PI, that's why we add some multiples of 2*M_PI + EXPECT(assert_equal(simple::R0, initial.at(x0), 1e-6)); + EXPECT(assert_equal(simple::R1, initial.at(x1), 1e-6)); + EXPECT(assert_equal(simple::R2, initial.at(x2), 1e-6)); + EXPECT(assert_equal(simple::R3, initial.at(x3), 1e-6)); +} + /* *************************************************************************** */ TEST( InitializePose3, orientationsGradientSymbolicGraph ) { NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph()); diff --git a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp index aaffbf0e6..ddb4d8adb 100644 --- a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp @@ -46,7 +46,6 @@ static double rankTol = 1.0; template PinholeCamera perturbCameraPoseAndCalibration( const PinholeCamera& camera) { - GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION) Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); Pose3 cameraPose = camera.pose(); diff --git a/gtsam/symbolic/tests/testVariableIndex.cpp b/gtsam/symbolic/tests/testVariableIndex.cpp index 3d42c0c1e..6afb47e26 100644 --- a/gtsam/symbolic/tests/testVariableIndex.cpp +++ b/gtsam/symbolic/tests/testVariableIndex.cpp @@ -11,60 +11,66 @@ /** * @file testVariableIndex.cpp - * @brief + * @brief Unit tests for VariableIndex class * @author Richard Roberts - * @date Sep 26, 2010 + * @date Sep 26, 2010 */ +#include +#include +#include + +#include + #include #include using namespace boost::assign; -#include -#include - -#include -#include - using namespace std; using namespace gtsam; /* ************************************************************************* */ -TEST(VariableIndex, augment) { +// 2 small symbolic graphs shared by all tests - SymbolicFactorGraph fg1, fg2; +SymbolicFactorGraph testGraph1() { + SymbolicFactorGraph fg1; fg1.push_factor(0, 1); fg1.push_factor(0, 2); fg1.push_factor(5, 9); fg1.push_factor(2, 3); + return fg1; +} + +SymbolicFactorGraph testGraph2() { + SymbolicFactorGraph fg2; fg2.push_factor(1, 3); fg2.push_factor(2, 4); fg2.push_factor(3, 5); fg2.push_factor(5, 6); + return fg2; +} - SymbolicFactorGraph fgCombined; fgCombined.push_back(fg1); fgCombined.push_back(fg2); +/* ************************************************************************* */ +TEST(VariableIndex, augment) { + auto fg1 = testGraph1(), fg2 = testGraph2(); + SymbolicFactorGraph fgCombined; + fgCombined.push_back(fg1); + fgCombined.push_back(fg2); VariableIndex expected(fgCombined); VariableIndex actual(fg1); actual.augment(fg2); - LONGS_EQUAL(16, (long)actual.nEntries()); - LONGS_EQUAL(8, (long)actual.nFactors()); + LONGS_EQUAL(8, actual.size()); + LONGS_EQUAL(16, actual.nEntries()); + LONGS_EQUAL(8, actual.nFactors()); EXPECT(assert_equal(expected, actual)); } /* ************************************************************************* */ TEST(VariableIndex, augment2) { - SymbolicFactorGraph fg1, fg2; - fg1.push_factor(0, 1); - fg1.push_factor(0, 2); - fg1.push_factor(5, 9); - fg1.push_factor(2, 3); - fg2.push_factor(1, 3); - fg2.push_factor(2, 4); - fg2.push_factor(3, 5); - fg2.push_factor(5, 6); + auto fg1 = testGraph1(), fg2 = testGraph2(); SymbolicFactorGraph fgCombined; fgCombined.push_back(fg1); @@ -73,27 +79,20 @@ TEST(VariableIndex, augment2) { VariableIndex expected(fgCombined); - FastVector newIndices = list_of(5)(6)(7)(8); + FactorIndices newIndices = list_of(5)(6)(7)(8); VariableIndex actual(fg1); actual.augment(fg2, newIndices); - LONGS_EQUAL(16, (long) actual.nEntries()); - LONGS_EQUAL(9, (long) actual.nFactors()); + LONGS_EQUAL(8, actual.size()); + LONGS_EQUAL(16, actual.nEntries()); + LONGS_EQUAL(9, actual.nFactors()); EXPECT(assert_equal(expected, actual)); } /* ************************************************************************* */ TEST(VariableIndex, remove) { - SymbolicFactorGraph fg1, fg2; - fg1.push_factor(0, 1); - fg1.push_factor(0, 2); - fg1.push_factor(5, 9); - fg1.push_factor(2, 3); - fg2.push_factor(1, 3); - fg2.push_factor(2, 4); - fg2.push_factor(3, 5); - fg2.push_factor(5, 6); + auto fg1 = testGraph1(), fg2 = testGraph2(); SymbolicFactorGraph fgCombined; fgCombined.push_back(fg1); fgCombined.push_back(fg2); @@ -118,15 +117,7 @@ TEST(VariableIndex, remove) { /* ************************************************************************* */ TEST(VariableIndex, deep_copy) { - SymbolicFactorGraph fg1, fg2; - fg1.push_factor(0, 1); - fg1.push_factor(0, 2); - fg1.push_factor(5, 9); - fg1.push_factor(2, 3); - fg2.push_factor(1, 3); - fg2.push_factor(2, 4); - fg2.push_factor(3, 5); - fg2.push_factor(5, 6); + auto fg1 = testGraph1(), fg2 = testGraph2(); // Create original graph and VariableIndex SymbolicFactorGraph fgOriginal; fgOriginal.push_back(fg1); fgOriginal.push_back(fg2); diff --git a/gtsam_extra.cmake.in b/gtsam_extra.cmake.in index a1a8e3a90..b7990b3cc 100644 --- a/gtsam_extra.cmake.in +++ b/gtsam_extra.cmake.in @@ -9,18 +9,6 @@ set (GTSAM_VERSION_STRING "@GTSAM_VERSION_STRING@") set (GTSAM_USE_TBB @GTSAM_USE_TBB@) set (GTSAM_DEFAULT_ALLOCATOR @GTSAM_DEFAULT_ALLOCATOR@) -if("@GTSAM_USE_TBB@") - list(APPEND GTSAM_INCLUDE_DIR "@TBB_INCLUDE_DIRS@") -endif() - -# Append Eigen include path, set in top-level CMakeLists.txt to either -# system-eigen, or GTSAM eigen path -list(APPEND GTSAM_INCLUDE_DIR "@GTSAM_EIGEN_INCLUDE_PREFIX@") - -if("@GTSAM_USE_EIGEN_MKL@") - list(APPEND GTSAM_INCLUDE_DIR "@MKL_INCLUDE_DIR@") -endif() - if("@GTSAM_INSTALL_CYTHON_TOOLBOX@") list(APPEND GTSAM_EIGENCY_INSTALL_PATH "@GTSAM_EIGENCY_INSTALL_PATH@") endif() diff --git a/gtsam_unstable/discrete/tests/testLoopyBelief.cpp b/gtsam_unstable/discrete/tests/testLoopyBelief.cpp index ab21a1924..9929938d5 100644 --- a/gtsam_unstable/discrete/tests/testLoopyBelief.cpp +++ b/gtsam_unstable/discrete/tests/testLoopyBelief.cpp @@ -175,19 +175,19 @@ private: // collect all factors involving this key in the original graph DiscreteFactorGraph::shared_ptr star(new DiscreteFactorGraph()); - for(size_t factorIdx: varIndex[key]) { - star->push_back(graph.at(factorIdx)); + for(size_t factorIndex: varIndex[key]) { + star->push_back(graph.at(factorIndex)); // accumulate unary factors - if (graph.at(factorIdx)->size() == 1) { + if (graph.at(factorIndex)->size() == 1) { if (!prodOfUnaries) prodOfUnaries = boost::dynamic_pointer_cast( - graph.at(factorIdx)); + graph.at(factorIndex)); else prodOfUnaries = boost::make_shared( *prodOfUnaries * (*boost::dynamic_pointer_cast( - graph.at(factorIdx)))); + graph.at(factorIndex)))); } } diff --git a/gtsam_unstable/examples/FixedLagSmootherExample.cpp b/gtsam_unstable/examples/FixedLagSmootherExample.cpp index 4d9304f1a..71153ee31 100644 --- a/gtsam_unstable/examples/FixedLagSmootherExample.cpp +++ b/gtsam_unstable/examples/FixedLagSmootherExample.cpp @@ -111,23 +111,27 @@ int main(int argc, char** argv) { noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector3(0.05, 0.05, 0.05)); newFactors.push_back(BetweenFactor(previousKey, currentKey, odometryMeasurement2, odometryNoise2)); - // Update the smoothers with the new factors - smootherBatch.update(newFactors, newValues, newTimestamps); - smootherISAM2.update(newFactors, newValues, newTimestamps); - for(size_t i = 1; i < 2; ++i) { // Optionally perform multiple iSAM2 iterations - smootherISAM2.update(); + // Update the smoothers with the new factors. In this example, batch smoother needs one iteration + // to accurately converge. The ISAM smoother doesn't, but we only start getting estiates when + // both are ready for simplicity. + if (time >= 0.50) { + smootherBatch.update(newFactors, newValues, newTimestamps); + smootherISAM2.update(newFactors, newValues, newTimestamps); + for(size_t i = 1; i < 2; ++i) { // Optionally perform multiple iSAM2 iterations + smootherISAM2.update(); + } + + // Print the optimized current pose + cout << setprecision(5) << "Timestamp = " << time << endl; + smootherBatch.calculateEstimate(currentKey).print("Batch Estimate:"); + smootherISAM2.calculateEstimate(currentKey).print("iSAM2 Estimate:"); + cout << endl; + + // Clear contains for the next iteration + newTimestamps.clear(); + newValues.clear(); + newFactors.resize(0); } - - // Print the optimized current pose - cout << setprecision(5) << "Timestamp = " << time << endl; - smootherBatch.calculateEstimate(currentKey).print("Batch Estimate:"); - smootherISAM2.calculateEstimate(currentKey).print("iSAM2 Estimate:"); - cout << endl; - - // Clear contains for the next iteration - newTimestamps.clear(); - newValues.clear(); - newFactors.resize(0); } // And to demonstrate the fixed-lag aspect, print the keys contained in each smoother after 3.0 seconds diff --git a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp index 94a70470a..e290cef7e 100644 --- a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp @@ -43,8 +43,6 @@ using namespace std; using namespace gtsam; int main(int argc, char** argv){ - - typedef PinholePose Camera; typedef SmartProjectionPoseFactor SmartFactor; Values initial_estimate; diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index f6fc99512..d77895d86 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -505,132 +505,130 @@ virtual class DiscreteEulerPoincareHelicopter : gtsam::NoiseModelFactor { //************************************************************************* // nonlinear //************************************************************************* -// #include -// gtsam::GaussianFactorGraph* summarizeGraphSequential( -// const gtsam::GaussianFactorGraph& full_graph, const gtsam::KeyVector& indices); -// gtsam::GaussianFactorGraph* summarizeGraphSequential( -// const gtsam::GaussianFactorGraph& full_graph, const gtsam::Ordering& ordering, const gtsam::KeySet& saved_keys); +#include +class FixedLagSmootherKeyTimestampMapValue { + FixedLagSmootherKeyTimestampMapValue(size_t key, double timestamp); + FixedLagSmootherKeyTimestampMapValue(const gtsam::FixedLagSmootherKeyTimestampMapValue& other); +}; -// #include -// class FixedLagSmootherKeyTimestampMapValue { -// FixedLagSmootherKeyTimestampMapValue(const gtsam::Key& key, double timestamp); -// FixedLagSmootherKeyTimestampMapValue(const gtsam::FixedLagSmootherKeyTimestampMapValue& other); -// }; -// -// class FixedLagSmootherKeyTimestampMap { -// FixedLagSmootherKeyTimestampMap(); -// FixedLagSmootherKeyTimestampMap(const gtsam::FixedLagSmootherKeyTimestampMap& other); -// -// // Note: no print function -// -// // common STL methods -// size_t size() const; -// bool empty() const; -// void clear(); -// -// double at(const gtsam::Key& key) const; -// void insert(const gtsam::FixedLagSmootherKeyTimestampMapValue& value); -// }; -// -// class FixedLagSmootherResult { -// size_t getIterations() const; -// size_t getNonlinearVariables() const; -// size_t getLinearVariables() const; -// double getError() const; -// }; -// -// #include -// virtual class FixedLagSmoother { -// void print(string s) const; -// bool equals(const gtsam::FixedLagSmoother& rhs, double tol) const; -// -// gtsam::FixedLagSmootherKeyTimestampMap timestamps() const; -// double smootherLag() const; -// -// gtsam::FixedLagSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FixedLagSmootherKeyTimestampMap& timestamps); -// gtsam::Values calculateEstimate() const; -// }; -// -// #include -// virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother { -// BatchFixedLagSmoother(); -// BatchFixedLagSmoother(double smootherLag); -// BatchFixedLagSmoother(double smootherLag, const gtsam::LevenbergMarquardtParams& params); -// -// gtsam::LevenbergMarquardtParams params() const; -// }; -// -// #include -// virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother { -// IncrementalFixedLagSmoother(); -// IncrementalFixedLagSmoother(double smootherLag); -// IncrementalFixedLagSmoother(double smootherLag, const gtsam::ISAM2Params& params); -// -// gtsam::ISAM2Params params() const; -// }; -// -// #include -// virtual class ConcurrentFilter { -// void print(string s) const; -// bool equals(const gtsam::ConcurrentFilter& rhs, double tol) const; -// }; -// -// virtual class ConcurrentSmoother { -// void print(string s) const; -// bool equals(const gtsam::ConcurrentSmoother& rhs, double tol) const; -// }; -// -// // Synchronize function -// void synchronize(gtsam::ConcurrentFilter& filter, gtsam::ConcurrentSmoother& smoother); -// -// #include -// class ConcurrentBatchFilterResult { -// size_t getIterations() const; -// size_t getLambdas() const; -// size_t getNonlinearVariables() const; -// size_t getLinearVariables() const; -// double getError() const; -// }; -// -// virtual class ConcurrentBatchFilter : gtsam::ConcurrentFilter { -// ConcurrentBatchFilter(); -// ConcurrentBatchFilter(const gtsam::LevenbergMarquardtParams& parameters); -// -// gtsam::NonlinearFactorGraph getFactors() const; -// gtsam::Values getLinearizationPoint() const; -// gtsam::Ordering getOrdering() const; -// gtsam::VectorValues getDelta() const; -// -// gtsam::ConcurrentBatchFilterResult update(); -// gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors); -// gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); -// gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyList& keysToMove); -// gtsam::Values calculateEstimate() const; -// }; -// -// #include -// class ConcurrentBatchSmootherResult { -// size_t getIterations() const; -// size_t getLambdas() const; -// size_t getNonlinearVariables() const; -// size_t getLinearVariables() const; -// double getError() const; -// }; -// -// virtual class ConcurrentBatchSmoother : gtsam::ConcurrentSmoother { -// ConcurrentBatchSmoother(); -// ConcurrentBatchSmoother(const gtsam::LevenbergMarquardtParams& parameters); -// -// gtsam::NonlinearFactorGraph getFactors() const; -// gtsam::Values getLinearizationPoint() const; -// gtsam::Ordering getOrdering() const; -// gtsam::VectorValues getDelta() const; -// -// gtsam::ConcurrentBatchSmootherResult update(); -// gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors); -// gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); -// gtsam::Values calculateEstimate() const; -// }; +class FixedLagSmootherKeyTimestampMap { + FixedLagSmootherKeyTimestampMap(); + FixedLagSmootherKeyTimestampMap(const gtsam::FixedLagSmootherKeyTimestampMap& other); + + // Note: no print function + + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + double at(const size_t key) const; + void insert(const gtsam::FixedLagSmootherKeyTimestampMapValue& value); +}; + +class FixedLagSmootherResult { + size_t getIterations() const; + size_t getNonlinearVariables() const; + size_t getLinearVariables() const; + double getError() const; +}; + +#include +virtual class FixedLagSmoother { + void print(string s) const; + bool equals(const gtsam::FixedLagSmoother& rhs, double tol) const; + + gtsam::FixedLagSmootherKeyTimestampMap timestamps() const; + double smootherLag() const; + + gtsam::FixedLagSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FixedLagSmootherKeyTimestampMap& timestamps); + gtsam::Values calculateEstimate() const; +}; + +#include +virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother { + BatchFixedLagSmoother(); + BatchFixedLagSmoother(double smootherLag); + BatchFixedLagSmoother(double smootherLag, const gtsam::LevenbergMarquardtParams& params); + + gtsam::LevenbergMarquardtParams params() const; + template + VALUE calculateEstimate(size_t key) const; +}; + +#include +virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother { + IncrementalFixedLagSmoother(); + IncrementalFixedLagSmoother(double smootherLag); + IncrementalFixedLagSmoother(double smootherLag, const gtsam::ISAM2Params& params); + + gtsam::ISAM2Params params() const; +}; + +#include +virtual class ConcurrentFilter { + void print(string s) const; + bool equals(const gtsam::ConcurrentFilter& rhs, double tol) const; +}; + +virtual class ConcurrentSmoother { + void print(string s) const; + bool equals(const gtsam::ConcurrentSmoother& rhs, double tol) const; +}; + +// Synchronize function +void synchronize(gtsam::ConcurrentFilter& filter, gtsam::ConcurrentSmoother& smoother); + +#include +class ConcurrentBatchFilterResult { + size_t getIterations() const; + size_t getLambdas() const; + size_t getNonlinearVariables() const; + size_t getLinearVariables() const; + double getError() const; +}; + +virtual class ConcurrentBatchFilter : gtsam::ConcurrentFilter { + ConcurrentBatchFilter(); + ConcurrentBatchFilter(const gtsam::LevenbergMarquardtParams& parameters); + + gtsam::NonlinearFactorGraph getFactors() const; + gtsam::Values getLinearizationPoint() const; + gtsam::Ordering getOrdering() const; + gtsam::VectorValues getDelta() const; + + gtsam::ConcurrentBatchFilterResult update(); + gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors); + gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); + gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyList& keysToMove); + gtsam::Values calculateEstimate() const; +}; + +#include +class ConcurrentBatchSmootherResult { + size_t getIterations() const; + size_t getLambdas() const; + size_t getNonlinearVariables() const; + size_t getLinearVariables() const; + double getError() const; +}; + +virtual class ConcurrentBatchSmoother : gtsam::ConcurrentSmoother { + ConcurrentBatchSmoother(); + ConcurrentBatchSmoother(const gtsam::LevenbergMarquardtParams& parameters); + + gtsam::NonlinearFactorGraph getFactors() const; + gtsam::Values getLinearizationPoint() const; + gtsam::Ordering getOrdering() const; + gtsam::VectorValues getDelta() const; + + gtsam::ConcurrentBatchSmootherResult update(); + gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors); + gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); + gtsam::Values calculateEstimate() const; +}; //************************************************************************* // slam diff --git a/gtsam_unstable/linear/QPSParser.cpp b/gtsam_unstable/linear/QPSParser.cpp index a6fc072c7..3039185f2 100644 --- a/gtsam_unstable/linear/QPSParser.cpp +++ b/gtsam_unstable/linear/QPSParser.cpp @@ -17,56 +17,440 @@ #define BOOST_SPIRIT_USE_PHOENIX_V3 1 +#include +#include +#include +#include #include #include -#include -#include +#include +#include #include #include #include +#include + +#include +#include +#include +#include +#include +#include + +using boost::fusion::at_c; +using namespace std; namespace bf = boost::fusion; namespace qi = boost::spirit::qi; +using Chars = std::vector; + +// Get a string from a fusion vector of Chars +template +static string fromChars(const FusionVector &vars) { + const Chars &chars = at_c(vars); + return string(chars.begin(), chars.end()); +} + namespace gtsam { + +/** + * As the parser reads a file, it call functions in this visitor. This visitor + * in turn stores what the parser has read in a way that can be later used to + * build the full QP problem in the file. + */ +class QPSVisitor { + private: + typedef std::unordered_map coefficient_v; + typedef std::unordered_map constraint_v; + + std::unordered_map + row_to_constraint_v; // Maps QPS ROWS to Variable-Matrix pairs + constraint_v E; // Equalities + constraint_v IG; // Inequalities >= + constraint_v IL; // Inequalities <= + unsigned int numVariables; + std::unordered_map + b; // maps from constraint name to b value for Ax = b equality + // constraints + std::unordered_map + ranges; // Inequalities can be specified as ranges on a variable + std::unordered_map g; // linear term of quadratic cost + std::unordered_map + varname_to_key; // Variable QPS string name to key + std::unordered_map> + H; // H from hessian + double f; // Constant term of quadratic cost + std::string obj_name; // the objective function has a name in the QPS + std::string name_; // the quadratic program has a name in the QPS + std::unordered_map + up; // Upper Bound constraints on variable where X < MAX + std::unordered_map + lo; // Lower Bound constraints on variable where MIN < X + std::unordered_map + fx; // Equalities specified as FX in BOUNDS part of QPS + KeyVector free; // Variables can be specified as free (to which no + // constraints apply) + const bool debug = false; + + public: + QPSVisitor() : numVariables(1) {} + + void setName(boost::fusion::vector const &name) { + name_ = fromChars<1>(name); + if (debug) { + cout << "Parsing file: " << name_ << endl; + } + } + + void addColumn(boost::fusion::vector const &vars) { + string var_ = fromChars<1>(vars); + string row_ = fromChars<3>(vars); + Matrix11 coefficient = at_c<5>(vars) * I_1x1; + if (debug) { + cout << "Added Column for Var: " << var_ << " Row: " << row_ + << " Coefficient: " << coefficient << endl; + } + if (!varname_to_key.count(var_)) + varname_to_key[var_] = Symbol('X', numVariables++); + if (row_ == obj_name) { + g[varname_to_key[var_]] = coefficient; + return; + } + (*row_to_constraint_v[row_])[row_][varname_to_key[var_]] = coefficient; + } + + void addColumnDouble( + boost::fusion::vector const &vars) { + string var_ = fromChars<0>(vars); + string row1_ = fromChars<2>(vars); + string row2_ = fromChars<6>(vars); + Matrix11 coefficient1 = at_c<4>(vars) * I_1x1; + Matrix11 coefficient2 = at_c<8>(vars) * I_1x1; + if (!varname_to_key.count(var_)) + varname_to_key.insert({var_, Symbol('X', numVariables++)}); + if (row1_ == obj_name) + g[varname_to_key[var_]] = coefficient1; + else + (*row_to_constraint_v[row1_])[row1_][varname_to_key[var_]] = coefficient1; + if (row2_ == obj_name) + g[varname_to_key[var_]] = coefficient2; + else + (*row_to_constraint_v[row2_])[row2_][varname_to_key[var_]] = coefficient2; + } + + void addRangeSingle(boost::fusion::vector const &vars) { + string var_ = fromChars<1>(vars); + string row_ = fromChars<3>(vars); + double range = at_c<5>(vars); + ranges[row_] = range; + if (debug) { + cout << "SINGLE RANGE ADDED" << endl; + cout << "VAR:" << var_ << " ROW: " << row_ << " RANGE: " << range << endl; + } + } + void addRangeDouble( + boost::fusion::vector const &vars) { + string var_ = fromChars<1>(vars); + string row1_ = fromChars<3>(vars); + string row2_ = fromChars<7>(vars); + double range1 = at_c<5>(vars); + double range2 = at_c<9>(vars); + ranges[row1_] = range1; + ranges[row2_] = range2; + if (debug) { + cout << "DOUBLE RANGE ADDED" << endl; + cout << "VAR: " << var_ << " ROW1: " << row1_ << " RANGE1: " << range1 + << " ROW2: " << row2_ << " RANGE2: " << range2 << endl; + } + } + + void addRHS(boost::fusion::vector const &vars) { + string var_ = fromChars<1>(vars); + string row_ = fromChars<3>(vars); + double coefficient = at_c<5>(vars); + if (row_ == obj_name) + f = -coefficient; + else + b[row_] = coefficient; + + if (debug) { + cout << "Added RHS for Var: " << var_ << " Row: " << row_ + << " Coefficient: " << coefficient << endl; + } + } + + void addRHSDouble( + boost::fusion::vector const &vars) { + string var_ = fromChars<1>(vars); + string row1_ = fromChars<3>(vars); + string row2_ = fromChars<7>(vars); + double coefficient1 = at_c<5>(vars); + double coefficient2 = at_c<9>(vars); + if (row1_ == obj_name) + f = -coefficient1; + else + b[row1_] = coefficient1; + + if (row2_ == obj_name) + f = -coefficient2; + else + b[row2_] = coefficient2; + + if (debug) { + cout << "Added RHS for Var: " << var_ << " Row: " << row1_ + << " Coefficient: " << coefficient1 << endl; + cout << " " + << "Row: " << row2_ << " Coefficient: " << coefficient2 << endl; + } + } + + void addRow( + boost::fusion::vector const &vars) { + string name_ = fromChars<3>(vars); + char type = at_c<1>(vars); + switch (type) { + case 'N': + obj_name = name_; + break; + case 'L': + row_to_constraint_v[name_] = &IL; + break; + case 'G': + row_to_constraint_v[name_] = &IG; + break; + case 'E': + row_to_constraint_v[name_] = &E; + break; + default: + cout << "invalid type: " << type << endl; + break; + } + if (debug) { + cout << "Added Row Type: " << type << " Name: " << name_ << endl; + } + } + + void addBound(boost::fusion::vector const &vars) { + string type_ = fromChars<1>(vars); + string var_ = fromChars<5>(vars); + double number = at_c<7>(vars); + if (type_.compare(string("UP")) == 0) + up[varname_to_key[var_]] = number; + else if (type_.compare(string("LO")) == 0) + lo[varname_to_key[var_]] = number; + else if (type_.compare(string("FX")) == 0) + fx[varname_to_key[var_]] = number; + else + cout << "Invalid Bound Type: " << type_ << endl; + + if (debug) { + cout << "Added Bound Type: " << type_ << " Var: " << var_ + << " Amount: " << number << endl; + } + } + + void addFreeBound(boost::fusion::vector const &vars) { + string type_ = fromChars<1>(vars); + string var_ = fromChars<5>(vars); + free.push_back(varname_to_key[var_]); + if (debug) { + cout << "Added Free Bound Type: " << type_ << " Var: " << var_ + << " Amount: " << endl; + } + } + + void addQuadTerm(boost::fusion::vector const &vars) { + string var1_ = fromChars<1>(vars); + string var2_ = fromChars<3>(vars); + Matrix11 coefficient = at_c<5>(vars) * I_1x1; + + H[varname_to_key[var1_]][varname_to_key[var2_]] = coefficient; + H[varname_to_key[var2_]][varname_to_key[var1_]] = coefficient; + if (debug) { + cout << "Added QuadTerm for Var: " << var1_ << " Row: " << var2_ + << " Coefficient: " << coefficient << endl; + } + } + + QP makeQP() { + // Create the keys from the variable names + KeyVector keys; + for (auto kv : varname_to_key) { + keys.push_back(kv.second); + } + + // Fill the G matrices and g vectors from + vector Gs; + vector gs; + sort(keys.begin(), keys.end()); + for (size_t i = 0; i < keys.size(); ++i) { + for (size_t j = i; j < keys.size(); ++j) { + if (H.count(keys[i]) > 0 && H[keys[i]].count(keys[j]) > 0) { + Gs.emplace_back(H[keys[i]][keys[j]]); + } else { + Gs.emplace_back(Z_1x1); + } + } + } + for (Key key1 : keys) { + if (g.count(key1) > 0) { + gs.emplace_back(-g[key1]); + } else { + gs.emplace_back(Z_1x1); + } + } + + // Construct the quadratic program + QP madeQP; + auto obj = HessianFactor(keys, Gs, gs, 2 * f); + madeQP.cost.push_back(obj); + + // Add equality and inequality constraints into the QP + size_t dual_key_num = keys.size() + 1; + for (auto kv : E) { + map keyMatrixMapPos; + map keyMatrixMapNeg; + if (ranges.count(kv.first) == 1) { + for (auto km : kv.second) { + keyMatrixMapPos.insert(km); + km.second = -km.second; + keyMatrixMapNeg.insert(km); + } + if (ranges[kv.first] > 0) { + madeQP.inequalities.push_back( + LinearInequality(keyMatrixMapNeg, -b[kv.first], dual_key_num++)); + madeQP.inequalities.push_back(LinearInequality( + keyMatrixMapPos, b[kv.first] + ranges[kv.first], dual_key_num++)); + } else if (ranges[kv.first] < 0) { + madeQP.inequalities.push_back( + LinearInequality(keyMatrixMapPos, b[kv.first], dual_key_num++)); + madeQP.inequalities.push_back(LinearInequality( + keyMatrixMapNeg, ranges[kv.first] - b[kv.first], dual_key_num++)); + } else { + cerr << "ERROR: CANNOT ADD A RANGE OF ZERO" << endl; + throw; + } + continue; + } + map keyMatrixMap; + for (auto km : kv.second) { + keyMatrixMap.insert(km); + } + madeQP.equalities.push_back( + LinearEquality(keyMatrixMap, b[kv.first] * I_1x1, dual_key_num++)); + } + + for (auto kv : IG) { + map keyMatrixMapNeg; + map keyMatrixMapPos; + for (auto km : kv.second) { + keyMatrixMapPos.insert(km); + km.second = -km.second; + keyMatrixMapNeg.insert(km); + } + madeQP.inequalities.push_back( + LinearInequality(keyMatrixMapNeg, -b[kv.first], dual_key_num++)); + if (ranges.count(kv.first) == 1) { + madeQP.inequalities.push_back(LinearInequality( + keyMatrixMapPos, b[kv.first] + ranges[kv.first], dual_key_num++)); + } + } + + for (auto kv : IL) { + map keyMatrixMapPos; + map keyMatrixMapNeg; + for (auto km : kv.second) { + keyMatrixMapPos.insert(km); + km.second = -km.second; + keyMatrixMapNeg.insert(km); + } + madeQP.inequalities.push_back( + LinearInequality(keyMatrixMapPos, b[kv.first], dual_key_num++)); + if (ranges.count(kv.first) == 1) { + madeQP.inequalities.push_back(LinearInequality( + keyMatrixMapNeg, ranges[kv.first] - b[kv.first], dual_key_num++)); + } + } + + for (Key k : keys) { + if (find(free.begin(), free.end(), k) != free.end()) continue; + if (fx.count(k) == 1) + madeQP.equalities.push_back( + LinearEquality(k, I_1x1, fx[k] * I_1x1, dual_key_num++)); + if (up.count(k) == 1) + madeQP.inequalities.push_back( + LinearInequality(k, I_1x1, up[k], dual_key_num++)); + if (lo.count(k) == 1) + madeQP.inequalities.push_back( + LinearInequality(k, -I_1x1, -lo[k], dual_key_num++)); + else + madeQP.inequalities.push_back( + LinearInequality(k, -I_1x1, 0, dual_key_num++)); + } + return madeQP; + } +}; + typedef qi::grammar> base_grammar; -struct QPSParser::MPSGrammar: base_grammar { +struct QPSParser::MPSGrammar : base_grammar { typedef std::vector Chars; - RawQP * rqp_; - boost::function const&)> setName; - boost::function const &)> addRow; - boost::function< - void(bf::vector const &)> rhsSingle; - boost::function< - void( - bf::vector)> rhsDouble; - boost::function< - void(bf::vector)> colSingle; - boost::function< - void( - bf::vector const &)> colDouble; - boost::function< - void(bf::vector const &)> addQuadTerm; - boost::function< - void( - bf::vector const &)> addBound; - boost::function< - void(bf::vector const &)> addBoundFr; - MPSGrammar(RawQP * rqp) : - base_grammar(start), rqp_(rqp), setName( - boost::bind(&RawQP::setName, rqp, ::_1)), addRow( - boost::bind(&RawQP::addRow, rqp, ::_1)), rhsSingle( - boost::bind(&RawQP::addRHS, rqp, ::_1)), rhsDouble( - boost::bind(&RawQP::addRHSDouble, rqp, ::_1)), colSingle( - boost::bind(&RawQP::addColumn, rqp, ::_1)), colDouble( - boost::bind(&RawQP::addColumnDouble, rqp, ::_1)), addQuadTerm( - boost::bind(&RawQP::addQuadTerm, rqp, ::_1)), addBound( - boost::bind(&RawQP::addBound, rqp, ::_1)), addBoundFr( - boost::bind(&RawQP::addBoundFr, rqp, ::_1)) { + QPSVisitor *rqp_; + boost::function const &)> setName; + boost::function const &)> + addRow; + boost::function const &)> + rhsSingle; + boost::function)> + rhsDouble; + boost::function const &)> + rangeSingle; + boost::function)> + rangeDouble; + boost::function)> + colSingle; + boost::function const &)> + colDouble; + boost::function const &)> + addQuadTerm; + boost::function const &)> + addBound; + boost::function const &)> + addFreeBound; + MPSGrammar(QPSVisitor *rqp) + : base_grammar(start), + rqp_(rqp), + setName(boost::bind(&QPSVisitor::setName, rqp, ::_1)), + addRow(boost::bind(&QPSVisitor::addRow, rqp, ::_1)), + rhsSingle(boost::bind(&QPSVisitor::addRHS, rqp, ::_1)), + rhsDouble(boost::bind(&QPSVisitor::addRHSDouble, rqp, ::_1)), + rangeSingle(boost::bind(&QPSVisitor::addRangeSingle, rqp, ::_1)), + rangeDouble(boost::bind(&QPSVisitor::addRangeDouble, rqp, ::_1)), + colSingle(boost::bind(&QPSVisitor::addColumn, rqp, ::_1)), + colDouble(boost::bind(&QPSVisitor::addColumnDouble, rqp, ::_1)), + addQuadTerm(boost::bind(&QPSVisitor::addQuadTerm, rqp, ::_1)), + addBound(boost::bind(&QPSVisitor::addBound, rqp, ::_1)), + addFreeBound(boost::bind(&QPSVisitor::addFreeBound, rqp, ::_1)) { using namespace boost::spirit; using namespace boost::spirit::qi; character = lexeme[alnum | '_' | '-' | '.']; @@ -74,43 +458,54 @@ struct QPSParser::MPSGrammar: base_grammar { word = lexeme[+character]; name = lexeme[lit("NAME") >> *blank >> title >> +space][setName]; row = lexeme[*blank >> character >> +blank >> word >> *blank][addRow]; - rhs_single = lexeme[*blank >> word >> +blank >> word >> +blank >> double_ - >> *blank][rhsSingle]; - rhs_double = lexeme[(*blank >> word >> +blank >> word >> +blank >> double_ - >> +blank >> word >> +blank >> double_)[rhsDouble] >> *blank]; - col_single = lexeme[*blank >> word >> +blank >> word >> +blank >> double_ - >> *blank][colSingle]; - col_double = lexeme[*blank - >> (word >> +blank >> word >> +blank >> double_ >> +blank >> word - >> +blank >> double_)[colDouble] >> *blank]; - quad_l = lexeme[*blank >> word >> +blank >> word >> +blank >> double_ - >> *blank][addQuadTerm]; - bound = lexeme[(*blank >> word >> +blank >> word >> +blank >> word >> +blank - >> double_)[addBound] >> *blank]; - bound_fr = lexeme[*blank >> word >> +blank >> word >> +blank >> word - >> *blank][addBoundFr]; + rhs_single = lexeme[*blank >> word >> +blank >> word >> +blank >> double_ >> + *blank][rhsSingle]; + rhs_double = + lexeme[(*blank >> word >> +blank >> word >> +blank >> double_ >> + +blank >> word >> +blank >> double_)[rhsDouble] >> + *blank]; + range_single = lexeme[*blank >> word >> +blank >> word >> +blank >> + double_ >> *blank][rangeSingle]; + range_double = + lexeme[(*blank >> word >> +blank >> word >> +blank >> double_ >> + +blank >> word >> +blank >> double_)[rangeDouble] >> + *blank]; + col_single = lexeme[*blank >> word >> +blank >> word >> +blank >> double_ >> + *blank][colSingle]; + col_double = + lexeme[*blank >> (word >> +blank >> word >> +blank >> double_ >> + +blank >> word >> +blank >> double_)[colDouble] >> + *blank]; + quad_l = lexeme[*blank >> word >> +blank >> word >> +blank >> double_ >> + *blank][addQuadTerm]; + bound = lexeme[(*blank >> word >> +blank >> word >> +blank >> word >> + +blank >> double_)[addBound] >> + *blank]; + bound_fr = lexeme[*blank >> word >> +blank >> word >> +blank >> word >> + *blank][addFreeBound]; rows = lexeme[lit("ROWS") >> *blank >> eol >> +(row >> eol)]; - rhs = lexeme[lit("RHS") >> *blank >> eol - >> +((rhs_double | rhs_single) >> eol)]; - cols = lexeme[lit("COLUMNS") >> *blank >> eol - >> +((col_double | col_single) >> eol)]; + rhs = lexeme[lit("RHS") >> *blank >> eol >> + +((rhs_double | rhs_single) >> eol)]; + cols = lexeme[lit("COLUMNS") >> *blank >> eol >> + +((col_double | col_single) >> eol)]; quad = lexeme[lit("QUADOBJ") >> *blank >> eol >> +(quad_l >> eol)]; - bounds = lexeme[lit("BOUNDS") >> +space >> +((bound | bound_fr) >> eol)]; - ranges = lexeme[lit("RANGES") >> +space]; + bounds = lexeme[lit("BOUNDS") >> +space >> *((bound | bound_fr) >> eol)]; + ranges = lexeme[lit("RANGES") >> +space >> + *((range_double | range_single) >> eol)]; end = lexeme[lit("ENDATA") >> *space]; - start = lexeme[name >> rows >> cols >> rhs >> -ranges >> bounds >> quad - >> end]; + start = + lexeme[name >> rows >> cols >> rhs >> -ranges >> bounds >> quad >> end]; } qi::rule, char()> character; qi::rule, Chars()> word, title; - qi::rule > row, end, col_single, - col_double, rhs_single, rhs_double, ranges, bound, bound_fr, bounds, quad, - quad_l, rows, cols, rhs, name, start; + qi::rule> row, end, col_single, + col_double, rhs_single, rhs_double, range_single, range_double, ranges, + bound, bound_fr, bounds, quad, quad_l, rows, cols, rhs, name, start; }; QP QPSParser::Parse() { - RawQP rawData; + QPSVisitor rawData; std::fstream stream(fileName_.c_str()); stream.unsetf(std::ios::skipws); boost::spirit::basic_istream_iterator begin(stream); @@ -123,4 +518,4 @@ QP QPSParser::Parse() { return rawData.makeQP(); } -} +} // namespace gtsam diff --git a/gtsam_unstable/linear/QPSolver.h b/gtsam_unstable/linear/QPSolver.h index 9efc23a67..3854d2a15 100644 --- a/gtsam_unstable/linear/QPSolver.h +++ b/gtsam_unstable/linear/QPSolver.h @@ -32,9 +32,14 @@ struct QPPolicy { static constexpr double maxAlpha = 1.0; /// Simply the cost of the QP problem - static const GaussianFactorGraph& buildCostFunction( - const QP& qp, const VectorValues& xk = VectorValues()) { - return qp.cost; + static const GaussianFactorGraph buildCostFunction(const QP& qp, + const VectorValues& xk = VectorValues()) { + GaussianFactorGraph no_constant_factor; + for (auto factor : qp.cost) { + HessianFactor hf = static_cast(*factor); + no_constant_factor.push_back(hf); + } + return no_constant_factor; } }; diff --git a/gtsam_unstable/linear/RawQP.cpp b/gtsam_unstable/linear/RawQP.cpp deleted file mode 100644 index 0d6982bfd..000000000 --- a/gtsam_unstable/linear/RawQP.cpp +++ /dev/null @@ -1,271 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 RawQP.cpp - * @brief - * @author Ivan Dario Jimenez - * @date 3/5/16 - */ - -#include -#include - -using boost::fusion::at_c; - -namespace gtsam { - -void RawQP::setName( - boost::fusion::vector, std::vector, - std::vector> const &name) { - name_ = std::string(at_c < 1 > (name).begin(), at_c < 1 > (name).end()); - if (debug) { - std::cout << "Parsing file: " << name_ << std::endl; - } -} - -void RawQP::addColumn( - boost::fusion::vector, std::vector, - std::vector, std::vector, std::vector, double, - std::vector> const &vars) { - - std::string var_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end()); - std::string row_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end()); - Matrix11 coefficient = at_c < 5 > (vars) * I_1x1; - - if (!varname_to_key.count(var_)) - varname_to_key[var_] = Symbol('X', varNumber++); - if (row_ == obj_name) { - g[varname_to_key[var_]] = coefficient; - return; - } - (*row_to_constraint_v[row_])[row_][varname_to_key[var_]] = coefficient; - if (debug) { - std::cout << "Added Column for Var: " << var_ << " Row: " << row_ - << " Coefficient: " << coefficient << std::endl; - } - -} - -void RawQP::addColumnDouble( - boost::fusion::vector, std::vector, - std::vector, std::vector, double, std::vector, - std::vector, std::vector, double> const &vars) { - - std::string var_(at_c < 0 > (vars).begin(), at_c < 0 > (vars).end()); - std::string row1_(at_c < 2 > (vars).begin(), at_c < 2 > (vars).end()); - std::string row2_(at_c < 6 > (vars).begin(), at_c < 6 > (vars).end()); - Matrix11 coefficient1 = at_c < 4 > (vars) * I_1x1; - Matrix11 coefficient2 = at_c < 8 > (vars) * I_1x1; - if (!varname_to_key.count(var_)) - varname_to_key.insert( { var_, Symbol('X', varNumber++) }); - if (row1_ == obj_name) - g[varname_to_key[var_]] = coefficient1; - else - (*row_to_constraint_v[row1_])[row1_][varname_to_key[var_]] = coefficient1; - if (row2_ == obj_name) - g[varname_to_key[var_]] = coefficient2; - else - (*row_to_constraint_v[row2_])[row2_][varname_to_key[var_]] = coefficient2; -} - -void RawQP::addRHS( - boost::fusion::vector, std::vector, - std::vector, std::vector, std::vector, double, - std::vector> const &vars) { - - std::string var_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end()); - std::string row_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end()); - double coefficient = at_c < 5 > (vars); - if (row_ == obj_name) - f = -coefficient; - else - b[row_] = coefficient; - - if (debug) { - std::cout << "Added RHS for Var: " << var_ << " Row: " << row_ - << " Coefficient: " << coefficient << std::endl; - } -} - -void RawQP::addRHSDouble( - boost::fusion::vector, std::vector, - std::vector, std::vector, std::vector, double, - std::vector, std::vector, std::vector, double> const &vars) { - - std::string var_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end()); - std::string row1_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end()); - std::string row2_(at_c < 7 > (vars).begin(), at_c < 7 > (vars).end()); - double coefficient1 = at_c < 5 > (vars); - double coefficient2 = at_c < 9 > (vars); - if (row1_ == obj_name) - f = -coefficient1; - else - b[row1_] = coefficient1; - - if (row2_ == obj_name) - f = -coefficient2; - else - b[row2_] = coefficient2; - - if (debug) { - std::cout << "Added RHS for Var: " << var_ << " Row: " << row1_ - << " Coefficient: " << coefficient1 << std::endl; - std::cout << " " << "Row: " << row2_ - << " Coefficient: " << coefficient2 << std::endl; - } -} - -void RawQP::addRow( - boost::fusion::vector, char, std::vector, - std::vector, std::vector> const &vars) { - - std::string name_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end()); - char type = at_c < 1 > (vars); - switch (type) { - case 'N': - obj_name = name_; - break; - case 'L': - row_to_constraint_v[name_] = &IL; - break; - case 'G': - row_to_constraint_v[name_] = &IG; - break; - case 'E': - row_to_constraint_v[name_] = &E; - break; - default: - std::cout << "invalid type: " << type << std::endl; - break; - } - if (debug) { - std::cout << "Added Row Type: " << type << " Name: " << name_ << std::endl; - } -} - -void RawQP::addBound( - boost::fusion::vector, std::vector, - std::vector, std::vector, std::vector, - std::vector, std::vector, double> const &vars) { - - std::string type_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end()); - std::string var_(at_c < 5 > (vars).begin(), at_c < 5 > (vars).end()); - double number = at_c < 7 > (vars); - if (type_.compare(std::string("UP")) == 0) - up[varname_to_key[var_]] = number; - else if (type_.compare(std::string("LO")) == 0) - lo[varname_to_key[var_]] = number; - else - std::cout << "Invalid Bound Type: " << type_ << std::endl; - - if (debug) { - std::cout << "Added Bound Type: " << type_ << " Var: " << var_ - << " Amount: " << number << std::endl; - } -} - -void RawQP::addBoundFr( - boost::fusion::vector, std::vector, - std::vector, std::vector, std::vector, - std::vector, std::vector> const &vars) { - std::string type_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end()); - std::string var_(at_c < 5 > (vars).begin(), at_c < 5 > (vars).end()); - Free.push_back(varname_to_key[var_]); - if (debug) { - std::cout << "Added Free Bound Type: " << type_ << " Var: " << var_ - << " Amount: " << std::endl; - } -} - -void RawQP::addQuadTerm( - boost::fusion::vector, std::vector, - std::vector, std::vector, std::vector, double, - std::vector> const &vars) { - std::string var1_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end()); - std::string var2_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end()); - Matrix11 coefficient = at_c < 5 > (vars) * I_1x1; - - H[varname_to_key[var1_]][varname_to_key[var2_]] = coefficient; - H[varname_to_key[var2_]][varname_to_key[var1_]] = coefficient; - if (debug) { - std::cout << "Added QuadTerm for Var: " << var1_ << " Row: " << var2_ - << " Coefficient: " << coefficient << std::endl; - } -} - -QP RawQP::makeQP() { - KeyVector keys; - std::vector Gs; - std::vector gs; - for (auto kv : varname_to_key) { - keys.push_back(kv.second); - } - std::sort(keys.begin(), keys.end()); - for (unsigned int i = 0; i < keys.size(); ++i) { - for (unsigned int j = i; j < keys.size(); ++j) { - Gs.push_back(H[keys[i]][keys[j]]); - } - } - for (Key key1 : keys) { - gs.push_back(-g[key1]); - } - int dual_key_num = keys.size() + 1; - QP madeQP; - auto obj = HessianFactor(keys, Gs, gs, f); - - madeQP.cost.push_back(obj); - - for (auto kv : E) { - std::map keyMatrixMap; - for (auto km : kv.second) { - keyMatrixMap.insert(km); - } - madeQP.equalities.push_back( - LinearEquality(keyMatrixMap, b[kv.first] * I_1x1, dual_key_num++)); - } - - for (auto kv : IG) { - std::map keyMatrixMap; - for (auto km : kv.second) { - km.second = -km.second; - keyMatrixMap.insert(km); - } - madeQP.inequalities.push_back( - LinearInequality(keyMatrixMap, -b[kv.first], dual_key_num++)); - } - - for (auto kv : IL) { - std::map keyMatrixMap; - for (auto km : kv.second) { - keyMatrixMap.insert(km); - } - madeQP.inequalities.push_back( - LinearInequality(keyMatrixMap, b[kv.first], dual_key_num++)); - } - - for (Key k : keys) { - if (std::find(Free.begin(), Free.end(), k) != Free.end()) - continue; - if (up.count(k) == 1) - madeQP.inequalities.push_back( - LinearInequality(k, I_1x1, up[k], dual_key_num++)); - if (lo.count(k) == 1) - madeQP.inequalities.push_back( - LinearInequality(k, -I_1x1, lo[k], dual_key_num++)); - else - madeQP.inequalities.push_back( - LinearInequality(k, -I_1x1, 0, dual_key_num++)); - } - return madeQP; -} -} - diff --git a/gtsam_unstable/linear/RawQP.h b/gtsam_unstable/linear/RawQP.h deleted file mode 100644 index 6d36f57ad..000000000 --- a/gtsam_unstable/linear/RawQP.h +++ /dev/null @@ -1,106 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 RawQP.h - * @brief - * @author Ivan Dario Jimenez - * @date 3/5/16 - */ - -#pragma once - -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -namespace gtsam { -class RawQP { -private: - typedef std::unordered_map coefficient_v; - typedef std::unordered_map constraint_v; - - std::unordered_map row_to_constraint_v; - constraint_v E; - constraint_v IG; - constraint_v IL; - unsigned int varNumber; - std::unordered_map b; - std::unordered_map g; - std::unordered_map varname_to_key; - std::unordered_map > H; - double f; - std::string obj_name; - std::string name_; - std::unordered_map up; - std::unordered_map lo; - KeyVector Free; - const bool debug = false; - -public: - RawQP() : - row_to_constraint_v(), E(), IG(), IL(), varNumber(1), b(), g(), varname_to_key(), H(), f(), obj_name(), name_(), up(), lo(), Free() { - } - - void setName( - boost::fusion::vector, std::vector, - std::vector> const & name); - - void addColumn( - boost::fusion::vector, std::vector, - std::vector, std::vector, std::vector, double, - std::vector> const & vars); - - void addColumnDouble( - boost::fusion::vector, std::vector, - std::vector, std::vector, double, std::vector, - std::vector, std::vector, double> const & vars); - - void addRHS( - boost::fusion::vector, std::vector, - std::vector, std::vector, std::vector, double, - std::vector> const & vars); - - void addRHSDouble( - boost::fusion::vector, std::vector, - std::vector, std::vector, std::vector, double, - std::vector, std::vector, std::vector, double> const & vars); - - void addRow( - boost::fusion::vector, char, std::vector, - std::vector, std::vector> const & vars); - - void addBound( - boost::fusion::vector, std::vector, - std::vector, std::vector, std::vector, - std::vector, std::vector, double> const & vars); - - void addBoundFr( - boost::fusion::vector, std::vector, - std::vector, std::vector, std::vector, - std::vector, std::vector> const & vars); - - void addQuadTerm( - boost::fusion::vector, std::vector, - std::vector, std::vector, std::vector, double, - std::vector> const & vars); - - QP makeQP(); -} -; -} diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp index bc9dd1f98..2292c63d7 100644 --- a/gtsam_unstable/linear/tests/testQPSolver.cpp +++ b/gtsam_unstable/linear/tests/testQPSolver.cpp @@ -14,6 +14,7 @@ * @brief Test simple QP solver for a linear inequality constraint * @date Apr 10, 2014 * @author Duy-Nguyen Ta + * @author Ivan Dario Jimenez */ #include @@ -218,7 +219,7 @@ pair testParser(QPSParser parser) { // min f(x,y) = 4 + 1.5x -y + 0.58x^2 + 2xy + 2yx + 10y^2 expectedqp.cost.push_back( HessianFactor(X1, X2, 8.0 * I_1x1, 2.0 * I_1x1, -1.5 * kOne, 10.0 * I_1x1, - 2.0 * kOne, 4.0)); + 2.0 * kOne, 8.0)); // 2x + y >= 2 // -x + 2y <= 6 expectedqp.inequalities.push_back( @@ -269,6 +270,66 @@ TEST(QPSolver, QPExampleTest){ CHECK(assert_equal(error_expected, error_actual)) } +TEST(QPSolver, HS21) { + QP problem = QPSParser("HS21.QPS").Parse(); + VectorValues actualSolution; + VectorValues expectedSolution; + expectedSolution.insert(Symbol('X',1), 2.0*I_1x1); + expectedSolution.insert(Symbol('X',2), 0.0*I_1x1); + boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize(); + double error_actual = problem.cost.error(actualSolution); + CHECK(assert_equal(-99.9599999, error_actual, 1e-7)) + CHECK(assert_equal(expectedSolution, actualSolution)) +} + +TEST(QPSolver, HS35) { + QP problem = QPSParser("HS35.QPS").Parse(); + VectorValues actualSolution; + boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize(); + double error_actual = problem.cost.error(actualSolution); + CHECK(assert_equal(1.11111111e-01,error_actual, 1e-7)) +} + +TEST(QPSolver, HS35MOD) { + QP problem = QPSParser("HS35MOD.QPS").Parse(); + VectorValues actualSolution; + boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize(); + double error_actual = problem.cost.error(actualSolution); + CHECK(assert_equal(2.50000001e-01,error_actual, 1e-7)) +} + +TEST(QPSolver, HS51) { + QP problem = QPSParser("HS51.QPS").Parse(); + VectorValues actualSolution; + boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize(); + double error_actual = problem.cost.error(actualSolution); + CHECK(assert_equal(8.88178420e-16,error_actual, 1e-7)) +} + +TEST(QPSolver, HS52) { + QP problem = QPSParser("HS52.QPS").Parse(); + VectorValues actualSolution; + boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize(); + double error_actual = problem.cost.error(actualSolution); + CHECK(assert_equal(5.32664756,error_actual, 1e-7)) +} + +TEST(QPSolver, HS268) { // This test needs an extra order of magnitude of tolerance than the rest + QP problem = QPSParser("HS268.QPS").Parse(); + VectorValues actualSolution; + boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize(); + double error_actual = problem.cost.error(actualSolution); + CHECK(assert_equal(5.73107049e-07,error_actual, 1e-6)) +} + +TEST(QPSolver, QPTEST) { // REQUIRES Jacobian Fix + QP problem = QPSParser("QPTEST.QPS").Parse(); + VectorValues actualSolution; + boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize(); + double error_actual = problem.cost.error(actualSolution); + CHECK(assert_equal(0.437187500e01,error_actual, 1e-7)) +} + /* ************************************************************************* */ // Create Matlab's test graph as in http://www.mathworks.com/help/optim/ug/quadprog.html QP createTestMatlabQPEx() { diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp index a0ede5f74..56de7feab 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp @@ -309,7 +309,7 @@ void BatchFixedLagSmoother::marginalize(const KeyVector& marginalizeKeys) { set removedFactorSlots; const VariableIndex variableIndex(factors_); for(Key key: marginalizeKeys) { - const FastVector& slots = variableIndex[key]; + const auto& slots = variableIndex[key]; removedFactorSlots.insert(slots.begin(), slots.end()); } diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp index 215e63f0e..97df375d5 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp @@ -536,7 +536,7 @@ void ConcurrentBatchFilter::moveSeparator(const FastList& keysToMove) { std::vector removedFactorSlots; VariableIndex variableIndex(factors_); for(Key key: keysToMove) { - const FastVector& slots = variableIndex[key]; + const auto& slots = variableIndex[key]; removedFactorSlots.insert(removedFactorSlots.end(), slots.begin(), slots.end()); } diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp index 78bd977f5..b70b9efc2 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp @@ -291,7 +291,7 @@ FactorIndices ConcurrentIncrementalFilter::FindAdjacentFactors(const ISAM2& isam FactorIndices removedFactorSlots; const VariableIndex& variableIndex = isam2.getVariableIndex(); for(Key key: keys) { - const FastVector& slots = variableIndex[key]; + const auto& slots = variableIndex[key]; removedFactorSlots.insert(removedFactorSlots.end(), slots.begin(), slots.end()); } diff --git a/gtsam_unstable/partition/GenericGraph.cpp b/gtsam_unstable/partition/GenericGraph.cpp index f941407e9..92f0266d0 100644 --- a/gtsam_unstable/partition/GenericGraph.cpp +++ b/gtsam_unstable/partition/GenericGraph.cpp @@ -353,8 +353,6 @@ namespace gtsam { namespace partition { void reduceGenericGraph(const GenericGraph3D& graph, const std::vector& cameraKeys, const std::vector& landmarkKeys, const std::vector& dictionary, GenericGraph3D& reducedGraph) { - typedef size_t CameraKey; - typedef pair CameraPair; typedef size_t LandmarkKey; // get a mapping from each landmark to its connected cameras vector > cameraToLandmarks(dictionary.size()); diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 2ceba2a97..db80956fa 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -206,12 +206,9 @@ public: std::vector Gs(numKeys * (numKeys + 1) / 2); std::vector gs(numKeys); - if (this->measured_.size() != cameras.size()) { - std::cout - << "SmartStereoProjectionHessianFactor: this->measured_.size() inconsistent with input" - << std::endl; - exit(1); - } + if (this->measured_.size() != cameras.size()) + throw std::runtime_error("SmartStereoProjectionHessianFactor: this->" + "measured_.size() inconsistent with input"); triangulateSafe(cameras); diff --git a/gtsam_unstable/timing/timeDSFvariants.cpp b/gtsam_unstable/timing/timeDSFvariants.cpp index d05869a9c..26ed76d02 100644 --- a/gtsam_unstable/timing/timeDSFvariants.cpp +++ b/gtsam_unstable/timing/timeDSFvariants.cpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include diff --git a/matlab/README-gtsam-toolbox.md b/matlab/README-gtsam-toolbox.md new file mode 100644 index 000000000..66a02e969 --- /dev/null +++ b/matlab/README-gtsam-toolbox.md @@ -0,0 +1,91 @@ +# GTSAM - Georgia Tech Smoothing and Mapping Library + +## MATLAB wrapper + +http://borg.cc.gatech.edu/projects/gtsam + +This is the GTSAM MATLAB toolbox, a MATLAB wrapper around the GTSAM C++ library. To build it, enable `GTSAM_INSTALL_MATLAB_TOOLBOX=ON` in CMake. + +The interface to the toolbox is generated automatically by the wrap +tool which directly parses C++ header files. The tool generates matlab proxy objects together with all the native functions for wrapping and unwrapping scalar and non scalar types and objects. The interface generated by the wrap tool also redirects the standard output stream (cout) to matlab's console. + +## Ubuntu + +If you have a newer Ubuntu system (later than 10.04), you must make a small modification to your MATLAB installation, due to MATLAB being distributed with an old version of the C++ standard library. Delete or rename all files starting with `libstdc++` in your MATLAB installation directory, in paths: + + /usr/local/MATLAB/[version]/sys/os/[system]/ + /usr/local/MATLAB/[version]/bin/[system]/ + + +## Adding the toolbox to your MATLAB path + +To get started, first add the `toolbox` (or `gtsam_toolbox`) folder to your MATLAB path - in the MATLAB file browser, right-click on the folder and click 'Add to path -> This folder' (**do not add the subfolders to your path**). + +## Final setup on Linux + +MATLAB needs to know where the GTSAM shared object file (`libgtsam.so.4`) is so that it can link to it correctly. + +### System-wide + +If you installed GTSAM system-wide (e.g. with `sudo make install`), then simply run `sudo ldconfig`. + +### Custom Install + +If you have a custom install location, denoted by ``, you need to update your `LD_LIBRARY_PATH` environment variable. + +```sh +export LD_LIBRARY_PATH=/gtsam:$LD_LIBRARY_PATH +``` + +### Linker issues + +If you compile the MATLAB toolbox and everything compiles smoothly, but when you run any MATLAB script, you get following error messages in MATLAB +``` +Invalid MEX-file '/usr/local/gtsam_toolbox/gtsam_wrapper.mexa64': +Missing symbol 'mexAtExit' required by '/usr/local/gtsam_toolbox/gtsam_wrapper.mexa64' +Missing symbol 'mexCallMATLABWithObject' required by '/usr/local/gtsam_toolbox/gtsam_wrapper.mexa64' +... +``` +run following shell line +```sh +export LD_PRELOAD=/usr/lib/x86_64-linux-gnu/libstdc++.so.6 +``` +before you run MATLAB from the same shell. + +This mainly happens if you have GCC >= 5 and newer version MATLAB like R2017a. + + +## Trying out the examples + +The examples are located in the 'gtsam_examples' subfolder. You may either run them individually at the MATLAB command line, or open the GTSAM example GUI by running 'gtsamExamples'. Example: + +```matlab +>> cd /Users/yourname/toolbox % Change to wherever you installed the toolbox +>> cd gtsam_examples % Change to the examples directory +>> gtsamExamples % Run the GTSAM examples GUI +``` + +## Running the unit tests + +The GTSAM MATLAB toolbox also has a small set of unit tests located in the gtsam_tests directory. Example: + +```matlab +>> cd /Users/yourname/toolbox % Change to wherever you installed the toolbox +>> cd gtsam_tests % Change to the examples directory +>> test_gtsam % Run the unit tests +Starting: testJacobianFactor +Starting: testKalmanFilter +Starting: testLocalizationExample +Starting: testOdometryExample +Starting: testPlanarSLAMExample +Starting: testPose2SLAMExample +Starting: testPose3SLAMExample +Starting: testSFMExample +Starting: testStereoVOExample +Starting: testVisualISAMExample +Tests complete! +``` + +## Writing your own code + +Coding for the GTSAM MATLAB toolbox is straightforward and very fast once you understand a few basic concepts! Please see the manual to get started. diff --git a/matlab/README-gtsam-toolbox.txt b/matlab/README-gtsam-toolbox.txt deleted file mode 100644 index a1691be32..000000000 --- a/matlab/README-gtsam-toolbox.txt +++ /dev/null @@ -1,82 +0,0 @@ -================================================================================ -GTSAM - Georgia Tech Smoothing and Mapping Library - -MATLAB wrapper - -http://borg.cc.gatech.edu/projects/gtsam -================================================================================ - -This is the GTSAM MATLAB toolbox, a MATLAB wrapper around the GTSAM C++ -library. To build it, enable GTSAM_INSTALL_MATLAB_TOOLBOX in CMake. - -The interface to the toolbox is generated automatically by the wrap -tool which directly parses C++ header files. The tool generates matlab -proxy objects together with all the native functions for wrapping and -unwrapping scalar and non scalar types and objects. The interface -generated by the wrap tool also redirects the standard output stream -(cout) to matlab's console. - ----------------------------------------- -Note about newer Ubuntu versions unsupported by MATLAB (later than 10.04) ----------------------------------------- - -If you have a newer Ubuntu system, you must make a small modification to your -MATLAB installation, due to MATLAB being distributed with an old version of -the C++ standard library. Delete or rename all files starting with -'libstdc++' in your MATLAB installation directory, in paths: - - /usr/local/MATLAB/[version]/sys/os/[system]/ - /usr/local/MATLAB/[version]/bin/[system]/ - - ----------------------------------------- -Adding the toolbox to your MATLAB path ----------------------------------------- - -To get started, first add the 'toolbox' folder to your MATLAB path - in the -MATLAB file browser, right-click on the folder and click 'Add to path -> This -folder' (do not add the subfolders to your path). - - ----------------------------------------- -Trying out the examples ----------------------------------------- - -The examples are located in the 'gtsam_examples' subfolder. You may either -run them individually at the MATLAB command line, or open the GTSAM example -GUI by running 'gtsamExamples'. Example: - ->> cd /Users/yourname/toolbox % Change to wherever you installed the toolbox ->> cd gtsam_examples % Change to the examples directory ->> gtsamExamples % Run the GTSAM examples GUI - - ----------------------------------------- -Running the unit tests ----------------------------------------- - -The GTSAM MATLAB toolbox also has a small set of unit tests located in the -gtsam_tests directory. Example: - ->> cd /Users/yourname/toolbox % Change to wherever you installed the toolbox ->> cd gtsam_tests % Change to the examples directory ->> test_gtsam % Run the unit tests -Starting: testJacobianFactor -Starting: testKalmanFilter -Starting: testLocalizationExample -Starting: testOdometryExample -Starting: testPlanarSLAMExample -Starting: testPose2SLAMExample -Starting: testPose3SLAMExample -Starting: testSFMExample -Starting: testStereoVOExample -Starting: testVisualISAMExample -Tests complete! - - ----------------------------------------- -Writing your own code ----------------------------------------- - -Coding for the GTSAM MATLAB toolbox is straightforward and very fast once you -understand a few basic concepts! Please see the manual to get started. diff --git a/python/README.md b/python/README.md index f1c218cfb..396bcea89 100644 --- a/python/README.md +++ b/python/README.md @@ -1,6 +1,14 @@ Python Wrapper and Packaging ============================ +# Deprecation Notice + +We are in the process of deprecating the old Python bindings and Cython bindings in favor of the new Pybind11 binding system. + +As such, the examples in this directory are no longer guaranteed to work. Please refer to the [cython examples](https://bitbucket.org/gtborg/gtsam/src/develop/cython/gtsam/examples). + +--- + This directory contains the basic setup script and directory structure for the gtsam python module. During the build of gtsam, when GTSAM_BUILD_PYTHON is enabled, the following instructions will run. diff --git a/tests/smallExample.h b/tests/smallExample.h index 20ebcd1ba..146289dac 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -26,7 +26,6 @@ #include #include #include -#include #include namespace gtsam { @@ -131,7 +130,7 @@ namespace example { * -x11-x21-x31 * with x11 clamped at (1,1), and others related by 2D odometry. */ -// inline boost::tuple planarGraph(size_t N); +// inline std::pair planarGraph(size_t N); /* * Create canonical ordering for planar graph that also works for tree @@ -399,7 +398,7 @@ inline std::pair createNonlinearSmoother(int T) { inline GaussianFactorGraph createSmoother(int T) { NonlinearFactorGraph nlfg; Values poses; - boost::tie(nlfg, poses) = createNonlinearSmoother(T); + std::tie(nlfg, poses) = createNonlinearSmoother(T); return *nlfg.linearize(poses); } @@ -563,7 +562,7 @@ inline Symbol key(size_t x, size_t y) { } // \namespace impl /* ************************************************************************* */ -inline boost::tuple planarGraph(size_t N) { +inline std::pair planarGraph(size_t N) { using namespace impl; // create empty graph @@ -601,7 +600,7 @@ inline boost::tuple planarGraph(size_t N) { // linearize around zero boost::shared_ptr gfg = nlfg.linearize(zeros); - return boost::make_tuple(*gfg, xtrue); + return std::make_pair(*gfg, xtrue); } /* ************************************************************************* */ @@ -614,26 +613,26 @@ inline Ordering planarOrdering(size_t N) { } /* ************************************************************************* */ -inline std::pair splitOffPlanarTree(size_t N, +inline std::pair splitOffPlanarTree(size_t N, const GaussianFactorGraph& original) { - GaussianFactorGraph T, C; + auto T = boost::make_shared(), C= boost::make_shared(); // Add the x11 constraint to the tree - T.push_back(original[0]); + T->push_back(original[0]); // Add all horizontal constraints to the tree size_t i = 1; for (size_t x = 1; x < N; x++) for (size_t y = 1; y <= N; y++, i++) - T.push_back(original[i]); + T->push_back(original[i]); // Add first vertical column of constraints to T, others to C for (size_t x = 1; x <= N; x++) for (size_t y = 1; y < N; y++, i++) if (x == 1) - T.push_back(original[i]); + T->push_back(original[i]); else - C.push_back(original[i]); + C->push_back(original[i]); return std::make_pair(T, C); } diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index 769b458e4..161e6976b 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -243,7 +243,6 @@ TEST(ExpressionFactor, Shallow) { // traceExecution of shallow tree typedef internal::UnaryExpression Unary; - typedef internal::BinaryExpression Binary; size_t size = expression.traceSize(); internal::ExecutionTraceStorage traceStorage[size]; internal::ExecutionTrace trace; diff --git a/tests/testSubgraphPreconditioner.cpp b/tests/testSubgraphPreconditioner.cpp index ed16c7c07..fb9f7a5a2 100644 --- a/tests/testSubgraphPreconditioner.cpp +++ b/tests/testSubgraphPreconditioner.cpp @@ -15,210 +15,302 @@ * @author Frank Dellaert **/ -#include - -#if 0 - #include + +#include +#include #include -#include +#include #include #include -#include -#include +#include +#include +#include -#include +#include + +#include #include +#include +#include +#include using namespace boost::assign; +#include + using namespace std; using namespace gtsam; using namespace example; // define keys // Create key for simulated planar graph -Symbol key(int x, int y) { - return symbol_shorthand::X(1000*x+y); -} +Symbol key(int x, int y) { return symbol_shorthand::X(1000 * x + y); } /* ************************************************************************* */ -TEST( SubgraphPreconditioner, planarOrdering ) { +TEST(SubgraphPreconditioner, planarOrdering) { // Check canonical ordering Ordering expected, ordering = planarOrdering(3); expected += key(3, 3), key(2, 3), key(1, 3), key(3, 2), key(2, 2), key(1, 2), key(3, 1), key(2, 1), key(1, 1); - CHECK(assert_equal(expected,ordering)); + EXPECT(assert_equal(expected, ordering)); } /* ************************************************************************* */ /** unnormalized error */ static double error(const GaussianFactorGraph& fg, const VectorValues& x) { double total_error = 0.; - for(const GaussianFactor::shared_ptr& factor: fg) + for (const GaussianFactor::shared_ptr& factor : fg) total_error += factor->error(x); return total_error; } /* ************************************************************************* */ -TEST( SubgraphPreconditioner, planarGraph ) - { +TEST(SubgraphPreconditioner, planarGraph) { // Check planar graph construction GaussianFactorGraph A; VectorValues xtrue; boost::tie(A, xtrue) = planarGraph(3); - LONGS_EQUAL(13,A.size()); - LONGS_EQUAL(9,xtrue.size()); - DOUBLES_EQUAL(0,error(A,xtrue),1e-9); // check zero error for xtrue + LONGS_EQUAL(13, A.size()); + LONGS_EQUAL(9, xtrue.size()); + DOUBLES_EQUAL(0, error(A, xtrue), 1e-9); // check zero error for xtrue // Check that xtrue is optimal - GaussianBayesNet::shared_ptr R1 = GaussianSequentialSolver(A).eliminate(); - VectorValues actual = optimize(*R1); - CHECK(assert_equal(xtrue,actual)); + GaussianBayesNet::shared_ptr R1 = A.eliminateSequential(); + VectorValues actual = R1->optimize(); + EXPECT(assert_equal(xtrue, actual)); } /* ************************************************************************* */ -TEST( SubgraphPreconditioner, splitOffPlanarTree ) -{ +TEST(SubgraphPreconditioner, splitOffPlanarTree) { // Build a planar graph GaussianFactorGraph A; VectorValues xtrue; boost::tie(A, xtrue) = planarGraph(3); // Get the spanning tree and constraints, and check their sizes - GaussianFactorGraph T, C; + GaussianFactorGraph::shared_ptr T, C; boost::tie(T, C) = splitOffPlanarTree(3, A); - LONGS_EQUAL(9,T.size()); - LONGS_EQUAL(4,C.size()); + LONGS_EQUAL(9, T->size()); + LONGS_EQUAL(4, C->size()); // Check that the tree can be solved to give the ground xtrue - GaussianBayesNet::shared_ptr R1 = GaussianSequentialSolver(T).eliminate(); - VectorValues xbar = optimize(*R1); - CHECK(assert_equal(xtrue,xbar)); + GaussianBayesNet::shared_ptr R1 = T->eliminateSequential(); + VectorValues xbar = R1->optimize(); + EXPECT(assert_equal(xtrue, xbar)); } /* ************************************************************************* */ - -TEST( SubgraphPreconditioner, system ) -{ +TEST(SubgraphPreconditioner, system) { // Build a planar graph GaussianFactorGraph Ab; VectorValues xtrue; size_t N = 3; - boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b + boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b - // Get the spanning tree and corresponding ordering - GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 - boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); - SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_)); - SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_)); + // Get the spanning tree and remaining graph + GaussianFactorGraph::shared_ptr Ab1, Ab2; // A1*x-b1 and A2*x-b2 + boost::tie(Ab1, Ab2) = splitOffPlanarTree(N, Ab); // Eliminate the spanning tree to build a prior - SubgraphPreconditioner::sharedBayesNet Rc1 = GaussianSequentialSolver(Ab1_).eliminate(); // R1*x-c1 - VectorValues xbar = optimize(*Rc1); // xbar = inv(R1)*c1 + const Ordering ord = planarOrdering(N); + auto Rc1 = Ab1->eliminateSequential(ord); // R1*x-c1 + VectorValues xbar = Rc1->optimize(); // xbar = inv(R1)*c1 // Create Subgraph-preconditioned system - VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible - SubgraphPreconditioner system(Ab2, Rc1, xbarShared); + VectorValues::shared_ptr xbarShared( + new VectorValues(xbar)); // TODO: horrible + const SubgraphPreconditioner system(Ab2, Rc1, xbarShared); - // Create zero config - VectorValues zeros = VectorValues::Zero(xbar); + // Get corresponding matrices for tests. Add dummy factors to Ab2 to make + // sure it works with the ordering. + Ordering ordering = Rc1->ordering(); // not ord in general! + Ab2->add(key(1, 1), Z_2x2, Z_2x1); + Ab2->add(key(1, 2), Z_2x2, Z_2x1); + Ab2->add(key(1, 3), Z_2x2, Z_2x1); + Matrix A, A1, A2; + Vector b, b1, b2; + std::tie(A, b) = Ab.jacobian(ordering); + std::tie(A1, b1) = Ab1->jacobian(ordering); + std::tie(A2, b2) = Ab2->jacobian(ordering); + Matrix R1 = Rc1->matrix(ordering).first; + Matrix Abar(13 * 2, 9 * 2); + Abar.topRows(9 * 2) = Matrix::Identity(9 * 2, 9 * 2); + Abar.bottomRows(8) = A2.topRows(8) * R1.inverse(); + + // Helper function to vectorize in correct order, which is the order in which + // we eliminated the spanning tree. + auto vec = [ordering](const VectorValues& x) { return x.vector(ordering); }; // Set up y0 as all zeros - VectorValues y0 = zeros; + const VectorValues y0 = system.zero(); // y1 = perturbed y0 - VectorValues y1 = zeros; - y1[1] = Vector2(1.0, -1.0); + VectorValues y1 = system.zero(); + y1[key(3, 3)] = Vector2(1.0, -1.0); - // Check corresponding x values - VectorValues expected_x1 = xtrue, x1 = system.x(y1); - expected_x1[1] = Vector2(2.01, 2.99); - expected_x1[0] = Vector2(3.01, 2.99); - CHECK(assert_equal(xtrue, system.x(y0))); - CHECK(assert_equal(expected_x1,system.x(y1))); + // Check backSubstituteTranspose works with R1 + VectorValues actual = Rc1->backSubstituteTranspose(y1); + Vector expected = R1.transpose().inverse() * vec(y1); + EXPECT(assert_equal(expected, vec(actual))); + + // Check corresponding x values + // for y = 0, we get xbar: + EXPECT(assert_equal(xbar, system.x(y0))); + // for non-zero y, answer is x = xbar + inv(R1)*y + const Vector expected_x1 = vec(xbar) + R1.inverse() * vec(y1); + const VectorValues x1 = system.x(y1); + EXPECT(assert_equal(expected_x1, vec(x1))); // Check errors - DOUBLES_EQUAL(0,error(Ab,xtrue),1e-9); - DOUBLES_EQUAL(3,error(Ab,x1),1e-9); - DOUBLES_EQUAL(0,error(system,y0),1e-9); - DOUBLES_EQUAL(3,error(system,y1),1e-9); + DOUBLES_EQUAL(0, error(Ab, xbar), 1e-9); + DOUBLES_EQUAL(0, system.error(y0), 1e-9); + DOUBLES_EQUAL(2, error(Ab, x1), 1e-9); + DOUBLES_EQUAL(2, system.error(y1), 1e-9); - // Test gradient in x - VectorValues expected_gx0 = zeros; - VectorValues expected_gx1 = zeros; - CHECK(assert_equal(expected_gx0,gradient(Ab,xtrue))); - expected_gx1[2] = Vector2(-100., 100.); - expected_gx1[4] = Vector2(-100., 100.); - expected_gx1[1] = Vector2(200., -200.); - expected_gx1[3] = Vector2(-100., 100.); - expected_gx1[0] = Vector2(100., -100.); - CHECK(assert_equal(expected_gx1,gradient(Ab,x1))); + // Check that transposeMultiplyAdd <=> y += alpha * Abar' * e + // We check for e1 =[1;0] and e2=[0;1] corresponding to T and C + const double alpha = 0.5; + Errors e1, e2; + for (size_t i = 0; i < 13; i++) { + e1 += i < 9 ? Vector2(1, 1) : Vector2(0, 0); + e2 += i >= 9 ? Vector2(1, 1) : Vector2(0, 0); + } + Vector ee1(13 * 2), ee2(13 * 2); + ee1 << Vector::Ones(9 * 2), Vector::Zero(4 * 2); + ee2 << Vector::Zero(9 * 2), Vector::Ones(4 * 2); + + // Check transposeMultiplyAdd for e1 + VectorValues y = system.zero(); + system.transposeMultiplyAdd(alpha, e1, y); + Vector expected_y = alpha * Abar.transpose() * ee1; + EXPECT(assert_equal(expected_y, vec(y))); + + // Check transposeMultiplyAdd for e2 + y = system.zero(); + system.transposeMultiplyAdd(alpha, e2, y); + expected_y = alpha * Abar.transpose() * ee2; + EXPECT(assert_equal(expected_y, vec(y))); // Test gradient in y - VectorValues expected_gy0 = zeros; - VectorValues expected_gy1 = zeros; - expected_gy1[2] = Vector2(2., -2.); - expected_gy1[4] = Vector2(-2., 2.); - expected_gy1[1] = Vector2(3., -3.); - expected_gy1[3] = Vector2(-1., 1.); - expected_gy1[0] = Vector2(1., -1.); - CHECK(assert_equal(expected_gy0,gradient(system,y0))); - CHECK(assert_equal(expected_gy1,gradient(system,y1))); - - // Check it numerically for good measure - // TODO use boost::bind(&SubgraphPreconditioner::error,&system,_1) - // Vector numerical_g1 = numericalGradient (error, y1, 0.001); - // Vector expected_g1 = (Vector(18) << 0., 0., 0., 0., 2., -2., 0., 0., -2., 2., - // 3., -3., 0., 0., -1., 1., 1., -1.); - // CHECK(assert_equal(expected_g1,numerical_g1)); + auto g = system.gradient(y0); + Vector expected_g = Vector::Zero(18); + EXPECT(assert_equal(expected_g, vec(g))); } /* ************************************************************************* */ -TEST( SubgraphPreconditioner, conjugateGradients ) -{ +BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "JacobianFactor"); + +// Read from XML file +static GaussianFactorGraph read(const string& name) { + auto inputFile = findExampleDataFile(name); + ifstream is(inputFile); + if (!is.is_open()) throw runtime_error("Cannot find file " + inputFile); + boost::archive::xml_iarchive in_archive(is); + GaussianFactorGraph Ab; + in_archive >> boost::serialization::make_nvp("graph", Ab); + return Ab; +} + +TEST(SubgraphSolver, Solves) { + // Create preconditioner + SubgraphPreconditioner system; + + // We test on three different graphs + const auto Ab1 = planarGraph(3).first; + const auto Ab2 = read("toy3D"); + const auto Ab3 = read("randomGrid3D"); + + // For all graphs, test solve and solveTranspose + for (const auto& Ab : {Ab1, Ab2, Ab3}) { + // Call build, a non-const method needed to make solve work :-( + KeyInfo keyInfo(Ab); + std::map lambda; + system.build(Ab, keyInfo, lambda); + + // Create a perturbed (non-zero) RHS + const auto xbar = system.Rc1()->optimize(); // merely for use in zero below + auto values_y = VectorValues::Zero(xbar); + auto it = values_y.begin(); + it->second.setConstant(100); + ++it; + it->second.setConstant(-100); + + // Solve the VectorValues way + auto values_x = system.Rc1()->backSubstitute(values_y); + + // Solve the matrix way, this really just checks BN::backSubstitute + // This only works with Rc1 ordering, not with keyInfo ! + // TODO(frank): why does this not work with an arbitrary ordering? + const auto ord = system.Rc1()->ordering(); + const Matrix R1 = system.Rc1()->matrix(ord).first; + auto ord_y = values_y.vector(ord); + auto vector_x = R1.inverse() * ord_y; + EXPECT(assert_equal(vector_x, values_x.vector(ord))); + + // Test that 'solve' does implement x = R^{-1} y + // We do this by asserting it gives same answer as backSubstitute + // Only works with keyInfo ordering: + const auto ordering = keyInfo.ordering(); + auto vector_y = values_y.vector(ordering); + const size_t N = R1.cols(); + Vector solve_x = Vector::Zero(N); + system.solve(vector_y, solve_x); + EXPECT(assert_equal(values_x.vector(ordering), solve_x)); + + // Test that transposeSolve does implement x = R^{-T} y + // We do this by asserting it gives same answer as backSubstituteTranspose + auto values_x2 = system.Rc1()->backSubstituteTranspose(values_y); + Vector solveT_x = Vector::Zero(N); + system.transposeSolve(vector_y, solveT_x); + EXPECT(assert_equal(values_x2.vector(ordering), solveT_x)); + } +} + +/* ************************************************************************* */ +TEST(SubgraphPreconditioner, conjugateGradients) { // Build a planar graph GaussianFactorGraph Ab; VectorValues xtrue; size_t N = 3; - boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b + boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b - // Get the spanning tree and corresponding ordering - GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 - boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); - SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_)); - SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_)); + // Get the spanning tree + GaussianFactorGraph::shared_ptr Ab1, Ab2; // A1*x-b1 and A2*x-b2 + boost::tie(Ab1, Ab2) = splitOffPlanarTree(N, Ab); // Eliminate the spanning tree to build a prior - Ordering ordering = planarOrdering(N); - SubgraphPreconditioner::sharedBayesNet Rc1 = GaussianSequentialSolver(Ab1_).eliminate(); // R1*x-c1 - VectorValues xbar = optimize(*Rc1); // xbar = inv(R1)*c1 + SubgraphPreconditioner::sharedBayesNet Rc1 = + Ab1->eliminateSequential(); // R1*x-c1 + VectorValues xbar = Rc1->optimize(); // xbar = inv(R1)*c1 // Create Subgraph-preconditioned system - VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible + VectorValues::shared_ptr xbarShared( + new VectorValues(xbar)); // TODO: horrible SubgraphPreconditioner system(Ab2, Rc1, xbarShared); // Create zero config y0 and perturbed config y1 VectorValues y0 = VectorValues::Zero(xbar); VectorValues y1 = y0; - y1[1] = Vector2(1.0, -1.0); + y1[key(2, 2)] = Vector2(1.0, -1.0); VectorValues x1 = system.x(y1); // Solve for the remaining constraints using PCG ConjugateGradientParameters parameters; VectorValues actual = conjugateGradients(system, y1, parameters); - CHECK(assert_equal(y0,actual)); + EXPECT(assert_equal(y0,actual)); // Compare with non preconditioned version: VectorValues actual2 = conjugateGradientDescent(Ab, x1, parameters); - CHECK(assert_equal(xtrue,actual2,1e-4)); + EXPECT(assert_equal(xtrue, actual2, 1e-4)); } -#endif - /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/tests/testSubgraphSolver.cpp b/tests/testSubgraphSolver.cpp index aeeed1b9f..cca13c822 100644 --- a/tests/testSubgraphSolver.cpp +++ b/tests/testSubgraphSolver.cpp @@ -15,26 +15,28 @@ * @author Yong-Dian Jian **/ -#include - -#if 0 +#include #include -#include #include #include #include -#include +#include +#include #include #include -#include +#include + #include using namespace boost::assign; using namespace std; using namespace gtsam; -using namespace example; + +static size_t N = 3; +static SubgraphSolverParameters kParameters; +static auto kOrdering = example::planarOrdering(N); /* ************************************************************************* */ /** unnormalized error */ @@ -45,6 +47,32 @@ static double error(const GaussianFactorGraph& fg, const VectorValues& x) { return total_error; } +/* ************************************************************************* */ +TEST( SubgraphSolver, Parameters ) +{ + LONGS_EQUAL(SubgraphSolverParameters::SILENT, kParameters.verbosity()); + LONGS_EQUAL(500, kParameters.maxIterations()); +} + +/* ************************************************************************* */ +TEST( SubgraphSolver, splitFactorGraph ) +{ + // Build a planar graph + GaussianFactorGraph Ab; + VectorValues xtrue; + std::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b + + SubgraphBuilderParameters params; + params.augmentationFactor = 0.0; + SubgraphBuilder builder(params); + auto subgraph = builder(Ab); + EXPECT_LONGS_EQUAL(9, subgraph.size()); + + GaussianFactorGraph::shared_ptr Ab1, Ab2; + std::tie(Ab1, Ab2) = splitFactorGraph(Ab, subgraph); + EXPECT_LONGS_EQUAL(9, Ab1->size()); + EXPECT_LONGS_EQUAL(13, Ab2->size()); +} /* ************************************************************************* */ TEST( SubgraphSolver, constructor1 ) @@ -52,13 +80,11 @@ TEST( SubgraphSolver, constructor1 ) // Build a planar graph GaussianFactorGraph Ab; VectorValues xtrue; - size_t N = 3; - boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b + std::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b - // The first constructor just takes a factor graph (and parameters) + // The first constructor just takes a factor graph (and kParameters) // and it will split the graph into A1 and A2, where A1 is a spanning tree - SubgraphSolverParameters parameters; - SubgraphSolver solver(Ab, parameters); + SubgraphSolver solver(Ab, kParameters, kOrdering); VectorValues optimized = solver.optimize(); // does PCG optimization DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5); } @@ -70,16 +96,15 @@ TEST( SubgraphSolver, constructor2 ) GaussianFactorGraph Ab; VectorValues xtrue; size_t N = 3; - boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b + std::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b - // Get the spanning tree and corresponding ordering - GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 - boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); + // Get the spanning tree + GaussianFactorGraph::shared_ptr Ab1, Ab2; // A1*x-b1 and A2*x-b2 + std::tie(Ab1, Ab2) = example::splitOffPlanarTree(N, Ab); - // The second constructor takes two factor graphs, - // so the caller can specify the preconditioner (Ab1) and the constraints that are left out (Ab2) - SubgraphSolverParameters parameters; - SubgraphSolver solver(Ab1_, Ab2_, parameters); + // The second constructor takes two factor graphs, so the caller can specify + // the preconditioner (Ab1) and the constraints that are left out (Ab2) + SubgraphSolver solver(*Ab1, Ab2, kParameters, kOrdering); VectorValues optimized = solver.optimize(); DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5); } @@ -91,26 +116,22 @@ TEST( SubgraphSolver, constructor3 ) GaussianFactorGraph Ab; VectorValues xtrue; size_t N = 3; - boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b + std::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b - // Get the spanning tree and corresponding ordering - GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2 - boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab); + // Get the spanning tree and corresponding kOrdering + GaussianFactorGraph::shared_ptr Ab1, Ab2; // A1*x-b1 and A2*x-b2 + std::tie(Ab1, Ab2) = example::splitOffPlanarTree(N, Ab); - // The caller solves |A1*x-b1|^2 == |R1*x-c1|^2 via QR factorization, where R1 is square UT - GaussianBayesNet::shared_ptr Rc1 = // - EliminationTree::Create(Ab1_)->eliminate(&EliminateQR); + // The caller solves |A1*x-b1|^2 == |R1*x-c1|^2, where R1 is square UT + auto Rc1 = Ab1->eliminateSequential(); // The third constructor allows the caller to pass an already solved preconditioner Rc1_ // as a Bayes net, in addition to the "loop closing constraints" Ab2, as before - SubgraphSolverParameters parameters; - SubgraphSolver solver(Rc1, Ab2_, parameters); + SubgraphSolver solver(Rc1, Ab2, kParameters); VectorValues optimized = solver.optimize(); DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5); } -#endif - /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/wrap/CMakeLists.txt b/wrap/CMakeLists.txt index 04d471b39..3027db19c 100644 --- a/wrap/CMakeLists.txt +++ b/wrap/CMakeLists.txt @@ -1,22 +1,37 @@ # Build/install Wrap -set(WRAP_BOOST_LIBRARIES ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY} ${Boost_THREAD_LIBRARY}) +set(WRAP_BOOST_LIBRARIES + optimized + ${Boost_FILESYSTEM_LIBRARY_RELEASE} + ${Boost_SYSTEM_LIBRARY_RELEASE} + ${Boost_THREAD_LIBRARY_RELEASE} + optimized + ${Boost_FILESYSTEM_LIBRARY_DEBUG} + ${Boost_SYSTEM_LIBRARY_DEBUG} + ${Boost_THREAD_LIBRARY_DEBUG} +) # Allow for disabling serialization to handle errors related to Clang's linker option(GTSAM_WRAP_SERIALIZATION "If enabled, allows for wrapped objects to be saved via boost.serialization" ON) -if (NOT GTSAM_WRAP_SERIALIZATION) - add_definitions(-DWRAP_DISABLE_SERIALIZE) -endif() # Build the executable itself file(GLOB wrap_srcs "*.cpp") file(GLOB wrap_headers "*.h") list(REMOVE_ITEM wrap_srcs ${CMAKE_CURRENT_SOURCE_DIR}/wrap.cpp) add_library(wrap_lib STATIC ${wrap_srcs} ${wrap_headers}) +target_include_directories(wrap_lib PUBLIC + $ +) +if (NOT GTSAM_WRAP_SERIALIZATION) + target_compile_definitions(wrap_lib PUBLIC -DWRAP_DISABLE_SERIALIZE) +endif() + target_link_libraries(wrap_lib ${WRAP_BOOST_LIBRARIES}) +target_include_directories(wrap_lib PUBLIC ${Boost_INCLUDE_DIR}) + gtsam_assign_source_folders(${wrap_srcs} ${wrap_headers}) add_executable(wrap wrap.cpp) -target_link_libraries(wrap wrap_lib ${WRAP_BOOST_LIBRARIES}) +target_link_libraries(wrap PRIVATE wrap_lib) # Set folder in Visual Studio file(RELATIVE_PATH relative_path "${PROJECT_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}") @@ -32,4 +47,3 @@ install(FILES matlab.h DESTINATION include/wrap) # Build tests add_subdirectory(tests) - diff --git a/wrap/FullyOverloadedFunction.h b/wrap/FullyOverloadedFunction.h index d6214ef2e..6b40f6a70 100644 --- a/wrap/FullyOverloadedFunction.h +++ b/wrap/FullyOverloadedFunction.h @@ -79,8 +79,8 @@ public: if (argLCount != nrOverloads() - 1) proxyFile.oss << ", "; else - proxyFile.oss << " : returns " << returnValue(0).return_type(false) - << std::endl; + proxyFile.oss << " : returns " << returnValue(0).returnType() + << std::endl; argLCount++; } } @@ -91,8 +91,8 @@ public: for(ArgumentList argList: argLists_) { proxyFile.oss << "%"; argList.emit_prototype(proxyFile, name); - proxyFile.oss << " : returns " << returnVals_[i++].return_type(false) - << std::endl; + proxyFile.oss << " : returns " << returnVals_[i++].returnType() + << std::endl; } } diff --git a/wrap/GlobalFunction.cpp b/wrap/GlobalFunction.cpp index 6da729607..13616d64a 100644 --- a/wrap/GlobalFunction.cpp +++ b/wrap/GlobalFunction.cpp @@ -94,8 +94,6 @@ void GlobalFunction::generateSingleFunction(const string& toolboxPath, // start file.oss << "{\n"; - returnVal.wrapTypeUnwrap(file); - // check arguments // NOTE: for static functions, there is no object passed file.oss << " checkArguments(\"" << matlabUniqueName diff --git a/wrap/Method.cpp b/wrap/Method.cpp index 8f0ef17b9..f7247341c 100644 --- a/wrap/Method.cpp +++ b/wrap/Method.cpp @@ -64,8 +64,8 @@ string Method::wrapper_call(FileWriter& wrapperFile, Str cppClassName, << "\",nargout,nargin-1," << args.size() << ");\n"; // get class pointer - // example: shared_ptr = unwrap_shared_ptr< Test >(in[0], "Test"); - wrapperFile.oss << " Shared obj = unwrap_shared_ptr<" << cppClassName + // example: auto obj = unwrap_shared_ptr< Test >(in[0], "Test"); + wrapperFile.oss << " auto obj = unwrap_shared_ptr<" << cppClassName << ">(in[0], \"ptr_" << matlabUniqueName << "\");" << endl; // unwrap arguments, see Argument.cpp, we start at 1 as first is obj diff --git a/wrap/MethodBase.cpp b/wrap/MethodBase.cpp index ef169d989..a2ed68780 100644 --- a/wrap/MethodBase.cpp +++ b/wrap/MethodBase.cpp @@ -108,11 +108,6 @@ string MethodBase::wrapper_fragment( // start wrapperFile.oss << "{\n"; - returnVal.wrapTypeUnwrap(wrapperFile); - - wrapperFile.oss << " typedef boost::shared_ptr<" << cppClassName - << "> Shared;" << endl; - // get call // for static methods: cppClassName::staticMethod // for instance methods: obj->instanceMethod diff --git a/wrap/Module.cpp b/wrap/Module.cpp index a3b8df630..a7db9e1f6 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -394,14 +394,18 @@ void Module::emit_cython_pxd(FileWriter& pxdFile) const { /* ************************************************************************* */ void Module::emit_cython_pyx(FileWriter& pyxFile) const { + // directives... + // allow str to automatically coerce to std::string and back (for python3) + pyxFile.oss << "# cython: c_string_type=str, c_string_encoding=ascii\n\n"; + // headers... string pxdHeader = name; pyxFile.oss << "cimport numpy as np\n" "import numpy as npp\n" "cimport " << pxdHeader << "\n" - "from "<< pxdHeader << " cimport shared_ptr\n" - "from "<< pxdHeader << " cimport dynamic_pointer_cast\n" - "from "<< pxdHeader << " cimport make_shared\n"; + "from ."<< pxdHeader << " cimport shared_ptr\n" + "from ."<< pxdHeader << " cimport dynamic_pointer_cast\n" + "from ."<< pxdHeader << " cimport make_shared\n"; pyxFile.oss << "# C helper function that copies all arguments into a positional list.\n" "cdef list process_args(list keywords, tuple args, dict kwargs):\n" diff --git a/wrap/ReturnType.cpp b/wrap/ReturnType.cpp index c8f8c1cca..fdf86d975 100644 --- a/wrap/ReturnType.cpp +++ b/wrap/ReturnType.cpp @@ -12,11 +12,6 @@ using namespace std; using namespace wrap; -/* ************************************************************************* */ -string ReturnType::str(bool add_ptr) const { - return maybe_shared_ptr(add_ptr && isPtr, qualifiedName("::"), name()); -} - /* ************************************************************************* */ void ReturnType::wrap_result(const string& out, const string& result, FileWriter& wrapperFile, @@ -35,9 +30,10 @@ void ReturnType::wrap_result(const string& out, const string& result, // A virtual class needs to be cloned, so the whole hierarchy is // returned objCopy = result + ".clone()"; - else + else { // ...but a non-virtual class can just be copied - objCopy = "Shared" + name() + "(new " + cppType + "(" + result + "))"; + objCopy = "boost::make_shared<" + cppType + ">(" + result + ")"; + } } // e.g. out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Point3", false); wrapperFile.oss << out << " = wrap_shared_ptr(" << objCopy << ",\"" @@ -46,25 +42,18 @@ void ReturnType::wrap_result(const string& out, const string& result, } else if (isPtr) { // Handle shared pointer case for BASIS/EIGEN/VOID - wrapperFile.oss << " {\n Shared" << name() << "* ret = new Shared" - << name() << "(" << result << ");" << endl; - wrapperFile.oss << out << " = wrap_shared_ptr(ret,\"" << matlabType + // This case does not actually occur in GTSAM wrappers, so untested! + wrapperFile.oss << " {\n boost::shared_ptr<" << qualifiedName("::") + << "> shared(" << result << ");" << endl; + wrapperFile.oss << out << " = wrap_shared_ptr(shared,\"" << matlabType << "\");\n }\n"; } else if (matlabType != "void") - // Handle normal case case for BASIS/EIGEN - wrapperFile.oss << out << " = wrap< " << str(false) << " >(" << result + wrapperFile.oss << out << " = wrap< " << qualifiedName("::") << " >(" << result << ");\n"; } -/* ************************************************************************* */ -void ReturnType::wrapTypeUnwrap(FileWriter& wrapperFile) const { - if (category == CLASS) - wrapperFile.oss << " typedef boost::shared_ptr<" << qualifiedName("::") - << "> Shared" << name() << ";" << endl; -} - /* ************************************************************************* */ void ReturnType::emit_cython_pxd( FileWriter& file, const std::string& className, diff --git a/wrap/ReturnType.h b/wrap/ReturnType.h index de1835f28..ba5086507 100644 --- a/wrap/ReturnType.h +++ b/wrap/ReturnType.h @@ -54,15 +54,10 @@ struct ReturnType : public Qualified { private: friend struct ReturnValue; - std::string str(bool add_ptr) const; - /// Example: out[1] = wrap_shared_ptr(pairResult.second,"Test", false); void wrap_result(const std::string& out, const std::string& result, FileWriter& wrapperFile, const TypeAttributesTable& typeAttributes) const; - - /// Creates typedef - void wrapTypeUnwrap(FileWriter& wrapperFile) const; }; //****************************************************************************** diff --git a/wrap/ReturnValue.cpp b/wrap/ReturnValue.cpp index 3f318eddc..e58e85602 100644 --- a/wrap/ReturnValue.cpp +++ b/wrap/ReturnValue.cpp @@ -22,11 +22,12 @@ ReturnValue ReturnValue::expandTemplate(const TemplateSubstitution& ts) const { } /* ************************************************************************* */ -string ReturnValue::return_type(bool add_ptr) const { +string ReturnValue::returnType() const { if (isPair) - return "pair< " + type1.str(add_ptr) + ", " + type2.str(add_ptr) + " >"; + return "pair< " + type1.qualifiedName("::") + ", " + + type2.qualifiedName("::") + " >"; else - return type1.str(add_ptr); + return type1.qualifiedName("::"); } /* ************************************************************************* */ @@ -40,7 +41,7 @@ void ReturnValue::wrap_result(const string& result, FileWriter& wrapperFile, if (isPair) { // For a pair, store the returned pair so we do not evaluate the function // twice - wrapperFile.oss << " " << return_type(true) << " pairResult = " << result + wrapperFile.oss << " auto pairResult = " << result << ";\n"; type1.wrap_result(" out[0]", "pairResult.first", wrapperFile, typeAttributes); @@ -51,12 +52,6 @@ void ReturnValue::wrap_result(const string& result, FileWriter& wrapperFile, } } -/* ************************************************************************* */ -void ReturnValue::wrapTypeUnwrap(FileWriter& wrapperFile) const { - type1.wrapTypeUnwrap(wrapperFile); - if (isPair) type2.wrapTypeUnwrap(wrapperFile); -} - /* ************************************************************************* */ void ReturnValue::emit_matlab(FileWriter& proxyFile) const { string output; diff --git a/wrap/ReturnValue.h b/wrap/ReturnValue.h index 589db5bd6..721132797 100644 --- a/wrap/ReturnValue.h +++ b/wrap/ReturnValue.h @@ -63,15 +63,13 @@ struct ReturnValue { /// Substitute template argument ReturnValue expandTemplate(const TemplateSubstitution& ts) const; - std::string return_type(bool add_ptr) const; + std::string returnType() const; std::string matlab_returnType() const; void wrap_result(const std::string& result, FileWriter& wrapperFile, const TypeAttributesTable& typeAttributes) const; - void wrapTypeUnwrap(FileWriter& wrapperFile) const; - void emit_matlab(FileWriter& proxyFile) const; /// @param className the actual class name to use when "This" is specified @@ -84,7 +82,7 @@ struct ReturnValue { if (!r.isPair && r.type1.category == ReturnType::VOID) os << "void"; else - os << r.return_type(true); + os << r.returnType(); return os; } diff --git a/wrap/tests/expected-cython/geometry.pyx b/wrap/tests/expected-cython/geometry.pyx index 4bd14b130..cae19d600 100644 --- a/wrap/tests/expected-cython/geometry.pyx +++ b/wrap/tests/expected-cython/geometry.pyx @@ -1,9 +1,11 @@ +# cython: c_string_type=str, c_string_encoding=ascii + cimport numpy as np import numpy as npp cimport geometry -from geometry cimport shared_ptr -from geometry cimport dynamic_pointer_cast -from geometry cimport make_shared +from .geometry cimport shared_ptr +from .geometry cimport dynamic_pointer_cast +from .geometry cimport make_shared # C helper function that copies all arguments into a positional list. cdef list process_args(list keywords, tuple args, dict kwargs): cdef str keyword diff --git a/wrap/tests/expected/geometry_wrapper.cpp b/wrap/tests/expected/geometry_wrapper.cpp index dec78b80c..6946a969b 100644 --- a/wrap/tests/expected/geometry_wrapper.cpp +++ b/wrap/tests/expected/geometry_wrapper.cpp @@ -183,35 +183,31 @@ void gtsamPoint2_deconstructor_3(int nargout, mxArray *out[], int nargin, const void gtsamPoint2_argChar_4(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("argChar",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); + auto obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); char a = unwrap< char >(in[1]); obj->argChar(a); } void gtsamPoint2_argUChar_5(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("argUChar",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); + auto obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); unsigned char a = unwrap< unsigned char >(in[1]); obj->argUChar(a); } void gtsamPoint2_dim_6(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("dim",nargout,nargin-1,0); - Shared obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); + auto obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); out[0] = wrap< int >(obj->dim()); } void gtsamPoint2_eigenArguments_7(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("eigenArguments",nargout,nargin-1,2); - Shared obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); + auto obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); Vector v = unwrap< Vector >(in[1]); Matrix m = unwrap< Matrix >(in[2]); obj->eigenArguments(v,m); @@ -219,34 +215,29 @@ void gtsamPoint2_eigenArguments_7(int nargout, mxArray *out[], int nargin, const void gtsamPoint2_returnChar_8(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("returnChar",nargout,nargin-1,0); - Shared obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); + auto obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); out[0] = wrap< char >(obj->returnChar()); } void gtsamPoint2_vectorConfusion_9(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedVectorNotEigen; - typedef boost::shared_ptr Shared; checkArguments("vectorConfusion",nargout,nargin-1,0); - Shared obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); - out[0] = wrap_shared_ptr(SharedVectorNotEigen(new VectorNotEigen(obj->vectorConfusion())),"VectorNotEigen", false); + auto obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); + out[0] = wrap_shared_ptr(boost::make_shared(obj->vectorConfusion()),"VectorNotEigen", false); } void gtsamPoint2_x_10(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("x",nargout,nargin-1,0); - Shared obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); + auto obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); out[0] = wrap< double >(obj->x()); } void gtsamPoint2_y_11(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("y",nargout,nargin-1,0); - Shared obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); + auto obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint2"); out[0] = wrap< double >(obj->y()); } @@ -288,9 +279,8 @@ void gtsamPoint3_deconstructor_14(int nargout, mxArray *out[], int nargin, const void gtsamPoint3_norm_15(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("norm",nargout,nargin-1,0); - Shared obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint3"); + auto obj = unwrap_shared_ptr(in[0], "ptr_gtsamPoint3"); out[0] = wrap< double >(obj->norm()); } @@ -306,16 +296,13 @@ void gtsamPoint3_string_serialize_16(int nargout, mxArray *out[], int nargin, co } void gtsamPoint3_StaticFunctionRet_17(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedPoint3; - typedef boost::shared_ptr Shared; checkArguments("gtsamPoint3.StaticFunctionRet",nargout,nargin,1); double z = unwrap< double >(in[0]); - out[0] = wrap_shared_ptr(SharedPoint3(new gtsam::Point3(gtsam::Point3::StaticFunctionRet(z))),"gtsam.Point3", false); + out[0] = wrap_shared_ptr(boost::make_shared(gtsam::Point3::StaticFunctionRet(z)),"gtsam.Point3", false); } void gtsamPoint3_staticFunction_18(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("gtsamPoint3.staticFunction",nargout,nargin,0); out[0] = wrap< double >(gtsam::Point3::staticFunction()); } @@ -379,187 +366,159 @@ void Test_deconstructor_23(int nargout, mxArray *out[], int nargin, const mxArra void Test_arg_EigenConstRef_24(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("arg_EigenConstRef",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); Matrix value = unwrap< Matrix >(in[1]); obj->arg_EigenConstRef(value); } void Test_create_MixedPtrs_25(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedTest; - typedef boost::shared_ptr SharedTest; - typedef boost::shared_ptr Shared; checkArguments("create_MixedPtrs",nargout,nargin-1,0); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); - pair< Test, SharedTest > pairResult = obj->create_MixedPtrs(); - out[0] = wrap_shared_ptr(SharedTest(new Test(pairResult.first)),"Test", false); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto pairResult = obj->create_MixedPtrs(); + out[0] = wrap_shared_ptr(boost::make_shared(pairResult.first),"Test", false); out[1] = wrap_shared_ptr(pairResult.second,"Test", false); } void Test_create_ptrs_26(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedTest; - typedef boost::shared_ptr SharedTest; - typedef boost::shared_ptr Shared; checkArguments("create_ptrs",nargout,nargin-1,0); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); - pair< SharedTest, SharedTest > pairResult = obj->create_ptrs(); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto pairResult = obj->create_ptrs(); out[0] = wrap_shared_ptr(pairResult.first,"Test", false); out[1] = wrap_shared_ptr(pairResult.second,"Test", false); } void Test_print_27(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("print",nargout,nargin-1,0); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); obj->print(); } void Test_return_Point2Ptr_28(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedPoint2; - typedef boost::shared_ptr Shared; checkArguments("return_Point2Ptr",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); bool value = unwrap< bool >(in[1]); out[0] = wrap_shared_ptr(obj->return_Point2Ptr(value),"gtsam.Point2", false); } void Test_return_Test_29(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedTest; - typedef boost::shared_ptr Shared; checkArguments("return_Test",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); boost::shared_ptr value = unwrap_shared_ptr< Test >(in[1], "ptr_Test"); - out[0] = wrap_shared_ptr(SharedTest(new Test(obj->return_Test(value))),"Test", false); + out[0] = wrap_shared_ptr(boost::make_shared(obj->return_Test(value)),"Test", false); } void Test_return_TestPtr_30(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedTest; - typedef boost::shared_ptr Shared; checkArguments("return_TestPtr",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); boost::shared_ptr value = unwrap_shared_ptr< Test >(in[1], "ptr_Test"); out[0] = wrap_shared_ptr(obj->return_TestPtr(value),"Test", false); } void Test_return_bool_31(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("return_bool",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); bool value = unwrap< bool >(in[1]); out[0] = wrap< bool >(obj->return_bool(value)); } void Test_return_double_32(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("return_double",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); double value = unwrap< double >(in[1]); out[0] = wrap< double >(obj->return_double(value)); } void Test_return_field_33(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("return_field",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); Test& t = *unwrap_shared_ptr< Test >(in[1], "ptr_Test"); out[0] = wrap< bool >(obj->return_field(t)); } void Test_return_int_34(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("return_int",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); int value = unwrap< int >(in[1]); out[0] = wrap< int >(obj->return_int(value)); } void Test_return_matrix1_35(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("return_matrix1",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); Matrix value = unwrap< Matrix >(in[1]); out[0] = wrap< Matrix >(obj->return_matrix1(value)); } void Test_return_matrix2_36(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("return_matrix2",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); Matrix value = unwrap< Matrix >(in[1]); out[0] = wrap< Matrix >(obj->return_matrix2(value)); } void Test_return_pair_37(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("return_pair",nargout,nargin-1,2); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); Vector v = unwrap< Vector >(in[1]); Matrix A = unwrap< Matrix >(in[2]); - pair< Vector, Matrix > pairResult = obj->return_pair(v,A); + auto pairResult = obj->return_pair(v,A); out[0] = wrap< Vector >(pairResult.first); out[1] = wrap< Matrix >(pairResult.second); } void Test_return_ptrs_38(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedTest; - typedef boost::shared_ptr SharedTest; - typedef boost::shared_ptr Shared; checkArguments("return_ptrs",nargout,nargin-1,2); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); boost::shared_ptr p1 = unwrap_shared_ptr< Test >(in[1], "ptr_Test"); boost::shared_ptr p2 = unwrap_shared_ptr< Test >(in[2], "ptr_Test"); - pair< SharedTest, SharedTest > pairResult = obj->return_ptrs(p1,p2); + auto pairResult = obj->return_ptrs(p1,p2); out[0] = wrap_shared_ptr(pairResult.first,"Test", false); out[1] = wrap_shared_ptr(pairResult.second,"Test", false); } void Test_return_size_t_39(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("return_size_t",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); size_t value = unwrap< size_t >(in[1]); out[0] = wrap< size_t >(obj->return_size_t(value)); } void Test_return_string_40(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("return_string",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); string value = unwrap< string >(in[1]); out[0] = wrap< string >(obj->return_string(value)); } void Test_return_vector1_41(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("return_vector1",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); Vector value = unwrap< Vector >(in[1]); out[0] = wrap< Vector >(obj->return_vector1(value)); } void Test_return_vector2_42(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("return_vector2",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); + auto obj = unwrap_shared_ptr(in[0], "ptr_Test"); Vector value = unwrap< Vector >(in[1]); out[0] = wrap< Vector >(obj->return_vector2(value)); } @@ -647,114 +606,93 @@ void MyTemplatePoint2_deconstructor_49(int nargout, mxArray *out[], int nargin, void MyTemplatePoint2_accept_T_50(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("accept_T",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); gtsam::Point2& value = *unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2"); obj->accept_T(value); } void MyTemplatePoint2_accept_Tptr_51(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("accept_Tptr",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); boost::shared_ptr value = unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2"); obj->accept_Tptr(value); } void MyTemplatePoint2_create_MixedPtrs_52(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedPoint2; - typedef boost::shared_ptr SharedPoint2; - typedef boost::shared_ptr Shared; checkArguments("create_MixedPtrs",nargout,nargin-1,0); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); - pair< gtsam::Point2, SharedPoint2 > pairResult = obj->create_MixedPtrs(); - out[0] = wrap_shared_ptr(SharedPoint2(new gtsam::Point2(pairResult.first)),"gtsam.Point2", false); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + auto pairResult = obj->create_MixedPtrs(); + out[0] = wrap_shared_ptr(boost::make_shared(pairResult.first),"gtsam.Point2", false); out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Point2", false); } void MyTemplatePoint2_create_ptrs_53(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedPoint2; - typedef boost::shared_ptr SharedPoint2; - typedef boost::shared_ptr Shared; checkArguments("create_ptrs",nargout,nargin-1,0); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); - pair< SharedPoint2, SharedPoint2 > pairResult = obj->create_ptrs(); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + auto pairResult = obj->create_ptrs(); out[0] = wrap_shared_ptr(pairResult.first,"gtsam.Point2", false); out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Point2", false); } void MyTemplatePoint2_return_T_54(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedPoint2; - typedef boost::shared_ptr Shared; checkArguments("return_T",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); boost::shared_ptr value = unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2"); - out[0] = wrap_shared_ptr(SharedPoint2(new gtsam::Point2(obj->return_T(value))),"gtsam.Point2", false); + out[0] = wrap_shared_ptr(boost::make_shared(obj->return_T(value)),"gtsam.Point2", false); } void MyTemplatePoint2_return_Tptr_55(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedPoint2; - typedef boost::shared_ptr Shared; checkArguments("return_Tptr",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); boost::shared_ptr value = unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2"); out[0] = wrap_shared_ptr(obj->return_Tptr(value),"gtsam.Point2", false); } void MyTemplatePoint2_return_ptrs_56(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedPoint2; - typedef boost::shared_ptr SharedPoint2; - typedef boost::shared_ptr Shared; checkArguments("return_ptrs",nargout,nargin-1,2); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); boost::shared_ptr p1 = unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2"); boost::shared_ptr p2 = unwrap_shared_ptr< gtsam::Point2 >(in[2], "ptr_gtsamPoint2"); - pair< SharedPoint2, SharedPoint2 > pairResult = obj->return_ptrs(p1,p2); + auto pairResult = obj->return_ptrs(p1,p2); out[0] = wrap_shared_ptr(pairResult.first,"gtsam.Point2", false); out[1] = wrap_shared_ptr(pairResult.second,"gtsam.Point2", false); } void MyTemplatePoint2_templatedMethod_57(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("templatedMethodMatrix",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); Matrix t = unwrap< Matrix >(in[1]); out[0] = wrap< Matrix >(obj->templatedMethod(t)); } void MyTemplatePoint2_templatedMethod_58(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedPoint2; - typedef boost::shared_ptr Shared; checkArguments("templatedMethodPoint2",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); gtsam::Point2& t = *unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2"); - out[0] = wrap_shared_ptr(SharedPoint2(new gtsam::Point2(obj->templatedMethod(t))),"gtsam.Point2", false); + out[0] = wrap_shared_ptr(boost::make_shared(obj->templatedMethod(t)),"gtsam.Point2", false); } void MyTemplatePoint2_templatedMethod_59(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedPoint3; - typedef boost::shared_ptr Shared; checkArguments("templatedMethodPoint3",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); gtsam::Point3& t = *unwrap_shared_ptr< gtsam::Point3 >(in[1], "ptr_gtsamPoint3"); - out[0] = wrap_shared_ptr(SharedPoint3(new gtsam::Point3(obj->templatedMethod(t))),"gtsam.Point3", false); + out[0] = wrap_shared_ptr(boost::make_shared(obj->templatedMethod(t)),"gtsam.Point3", false); } void MyTemplatePoint2_templatedMethod_60(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("templatedMethodVector",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); Vector t = unwrap< Vector >(in[1]); out[0] = wrap< Vector >(obj->templatedMethod(t)); } @@ -811,124 +749,111 @@ void MyTemplateMatrix_deconstructor_64(int nargout, mxArray *out[], int nargin, void MyTemplateMatrix_accept_T_65(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("accept_T",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); Matrix value = unwrap< Matrix >(in[1]); obj->accept_T(value); } void MyTemplateMatrix_accept_Tptr_66(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("accept_Tptr",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); Matrix value = unwrap< Matrix >(in[1]); obj->accept_Tptr(value); } void MyTemplateMatrix_create_MixedPtrs_67(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("create_MixedPtrs",nargout,nargin-1,0); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); - pair< Matrix, SharedMatrix > pairResult = obj->create_MixedPtrs(); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); + auto pairResult = obj->create_MixedPtrs(); out[0] = wrap< Matrix >(pairResult.first); { - SharedMatrix* ret = new SharedMatrix(pairResult.second); - out[1] = wrap_shared_ptr(ret,"Matrix"); + boost::shared_ptr shared(pairResult.second); + out[1] = wrap_shared_ptr(shared,"Matrix"); } } void MyTemplateMatrix_create_ptrs_68(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("create_ptrs",nargout,nargin-1,0); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); - pair< SharedMatrix, SharedMatrix > pairResult = obj->create_ptrs(); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); + auto pairResult = obj->create_ptrs(); { - SharedMatrix* ret = new SharedMatrix(pairResult.first); - out[0] = wrap_shared_ptr(ret,"Matrix"); + boost::shared_ptr shared(pairResult.first); + out[0] = wrap_shared_ptr(shared,"Matrix"); } { - SharedMatrix* ret = new SharedMatrix(pairResult.second); - out[1] = wrap_shared_ptr(ret,"Matrix"); + boost::shared_ptr shared(pairResult.second); + out[1] = wrap_shared_ptr(shared,"Matrix"); } } void MyTemplateMatrix_return_T_69(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("return_T",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); Matrix value = unwrap< Matrix >(in[1]); out[0] = wrap< Matrix >(obj->return_T(value)); } void MyTemplateMatrix_return_Tptr_70(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("return_Tptr",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); Matrix value = unwrap< Matrix >(in[1]); { - SharedMatrix* ret = new SharedMatrix(obj->return_Tptr(value)); - out[0] = wrap_shared_ptr(ret,"Matrix"); + boost::shared_ptr shared(obj->return_Tptr(value)); + out[0] = wrap_shared_ptr(shared,"Matrix"); } } void MyTemplateMatrix_return_ptrs_71(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("return_ptrs",nargout,nargin-1,2); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); Matrix p1 = unwrap< Matrix >(in[1]); Matrix p2 = unwrap< Matrix >(in[2]); - pair< SharedMatrix, SharedMatrix > pairResult = obj->return_ptrs(p1,p2); + auto pairResult = obj->return_ptrs(p1,p2); { - SharedMatrix* ret = new SharedMatrix(pairResult.first); - out[0] = wrap_shared_ptr(ret,"Matrix"); + boost::shared_ptr shared(pairResult.first); + out[0] = wrap_shared_ptr(shared,"Matrix"); } { - SharedMatrix* ret = new SharedMatrix(pairResult.second); - out[1] = wrap_shared_ptr(ret,"Matrix"); + boost::shared_ptr shared(pairResult.second); + out[1] = wrap_shared_ptr(shared,"Matrix"); } } void MyTemplateMatrix_templatedMethod_72(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("templatedMethodMatrix",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); Matrix t = unwrap< Matrix >(in[1]); out[0] = wrap< Matrix >(obj->templatedMethod(t)); } void MyTemplateMatrix_templatedMethod_73(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedPoint2; - typedef boost::shared_ptr Shared; checkArguments("templatedMethodPoint2",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); gtsam::Point2& t = *unwrap_shared_ptr< gtsam::Point2 >(in[1], "ptr_gtsamPoint2"); - out[0] = wrap_shared_ptr(SharedPoint2(new gtsam::Point2(obj->templatedMethod(t))),"gtsam.Point2", false); + out[0] = wrap_shared_ptr(boost::make_shared(obj->templatedMethod(t)),"gtsam.Point2", false); } void MyTemplateMatrix_templatedMethod_74(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedPoint3; - typedef boost::shared_ptr Shared; checkArguments("templatedMethodPoint3",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); gtsam::Point3& t = *unwrap_shared_ptr< gtsam::Point3 >(in[1], "ptr_gtsamPoint3"); - out[0] = wrap_shared_ptr(SharedPoint3(new gtsam::Point3(obj->templatedMethod(t))),"gtsam.Point3", false); + out[0] = wrap_shared_ptr(boost::make_shared(obj->templatedMethod(t)),"gtsam.Point3", false); } void MyTemplateMatrix_templatedMethod_75(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("templatedMethodVector",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); + auto obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); Vector t = unwrap< Vector >(in[1]); out[0] = wrap< Vector >(obj->templatedMethod(t)); } diff --git a/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp b/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp index 0f5c70348..6e0c5670d 100644 --- a/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp +++ b/wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp @@ -199,34 +199,29 @@ void ns2ClassA_deconstructor_8(int nargout, mxArray *out[], int nargin, const mx void ns2ClassA_memberFunction_9(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("memberFunction",nargout,nargin-1,0); - Shared obj = unwrap_shared_ptr(in[0], "ptr_ns2ClassA"); + auto obj = unwrap_shared_ptr(in[0], "ptr_ns2ClassA"); out[0] = wrap< double >(obj->memberFunction()); } void ns2ClassA_nsArg_10(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("nsArg",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_ns2ClassA"); + auto obj = unwrap_shared_ptr(in[0], "ptr_ns2ClassA"); ns1::ClassB& arg = *unwrap_shared_ptr< ns1::ClassB >(in[1], "ptr_ns1ClassB"); out[0] = wrap< int >(obj->nsArg(arg)); } void ns2ClassA_nsReturn_11(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedClassB; - typedef boost::shared_ptr Shared; checkArguments("nsReturn",nargout,nargin-1,1); - Shared obj = unwrap_shared_ptr(in[0], "ptr_ns2ClassA"); + auto obj = unwrap_shared_ptr(in[0], "ptr_ns2ClassA"); double q = unwrap< double >(in[1]); - out[0] = wrap_shared_ptr(SharedClassB(new ns2::ns3::ClassB(obj->nsReturn(q))),"ns2.ns3.ClassB", false); + out[0] = wrap_shared_ptr(boost::make_shared(obj->nsReturn(q)),"ns2.ns3.ClassB", false); } void ns2ClassA_afunction_12(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr Shared; checkArguments("ns2ClassA.afunction",nargout,nargin,0); out[0] = wrap< double >(ns2::ClassA::afunction()); } @@ -343,18 +338,16 @@ void ns2aGlobalFunction_23(int nargout, mxArray *out[], int nargin, const mxArra } void ns2overloadedGlobalFunction_24(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedClassA; checkArguments("ns2overloadedGlobalFunction",nargout,nargin,1); ns1::ClassA& a = *unwrap_shared_ptr< ns1::ClassA >(in[0], "ptr_ns1ClassA"); - out[0] = wrap_shared_ptr(SharedClassA(new ns1::ClassA(ns2::overloadedGlobalFunction(a))),"ns1.ClassA", false); + out[0] = wrap_shared_ptr(boost::make_shared(ns2::overloadedGlobalFunction(a)),"ns1.ClassA", false); } void ns2overloadedGlobalFunction_25(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { - typedef boost::shared_ptr SharedClassA; checkArguments("ns2overloadedGlobalFunction",nargout,nargin,2); ns1::ClassA& a = *unwrap_shared_ptr< ns1::ClassA >(in[0], "ptr_ns1ClassA"); double b = unwrap< double >(in[1]); - out[0] = wrap_shared_ptr(SharedClassA(new ns1::ClassA(ns2::overloadedGlobalFunction(a,b))),"ns1.ClassA", false); + out[0] = wrap_shared_ptr(boost::make_shared(ns2::overloadedGlobalFunction(a,b)),"ns1.ClassA", false); } void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])