12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'origin/develop' into feature/LPSolver 

# Conflicts:
#	gtsam_unstable/linear/QPSVisitor.h
#	gtsam_unstable/linear/RawQP.cpp
release/4.3a0
dellaert 2019-04-24 16:08:50 -04:00
parent 2d9d3af8e8 94225862f8
commit a450ba1e55
543 changed files with 13660 additions and 6175 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
.gitignore vendored
View File

@ -1,4 +1,5 @@
/build*
/debug*
.idea
*.pyc
*.DS_Store
@ -17,3 +18,7 @@ cython/gtsam.so
cython/gtsam_wrapper.pxd
.vscode
.env
/.vs/
/CMakeSettings.json
# for QtCreator:
CMakeLists.txt.user*

195
CMakeLists.txt
View File

@ -1,6 +1,6 @@
project(GTSAM CXX C)
cmake_minimum_required(VERSION 2.6)
cmake_minimum_required(VERSION 3.0)
# new feature to Cmake Version > 2.8.12
# Mac ONLY. Define Relative Path on Mac OS
@ -55,26 +55,30 @@ endif()
if(GTSAM_UNSTABLE_AVAILABLE)
option(GTSAM_BUILD_UNSTABLE "Enable/Disable libgtsam_unstable" ON)
endif()
option(GTSAM_BUILD_STATIC_LIBRARY "Build a static gtsam library, instead of shared" OFF)
option(BUILD_SHARED_LIBS "Build shared gtsam library, instead of static" ON)
option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF)
option(GTSAM_POSE3_EXPMAP "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." OFF)
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)
option(GTSAM_TYPEDEF_POINTS_TO_VECTORS "Typdef Point2 and Point3 to Eigen::Vector equivalents" OFF)
option(GTSAM_SUPPORT_NESTED_DISSECTION "Support Metis-based nested dissection" ON)
option(GTSAM_TANGENT_PREINTEGRATION "Use new ImuFactor with integration on tangent space" ON)
if(NOT MSVC AND NOT XCODE_VERSION)
option(GTSAM_BUILD_WITH_CCACHE "Use ccache compiler cache" ON)
endif()
# Options relating to MATLAB wrapper
# TODO: Check for matlab mex binary before handling building of binaries
option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" OFF)
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)
@ -84,8 +88,8 @@ if(GTSAM_INSTALL_WRAP AND NOT GTSAM_BUILD_WRAP)
message(FATAL_ERROR "GTSAM_INSTALL_WRAP is enabled, please also enable GTSAM_BUILD_WRAP")
endif()
if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_BUILD_STATIC_LIBRARY)
message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and GTSAM_BUILD_STATIC_LIBRARY are both enabled. The MATLAB wrapper cannot be compiled with a static GTSAM library because mex modules are themselves shared libraries. If you want a self-contained mex module, enable GTSAM_MEX_BUILD_STATIC_MODULE instead of GTSAM_BUILD_STATIC_LIBRARY.")
if(GTSAM_INSTALL_MATLAB_TOOLBOX AND NOT BUILD_SHARED_LIBS)
message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and BUILD_SHARED_LIBS=OFF. The MATLAB wrapper cannot be compiled with a static GTSAM library because mex modules are themselves shared libraries. If you want a self-contained mex module, enable GTSAM_MEX_BUILD_STATIC_MODULE instead of BUILD_SHARED_LIBS=OFF.")
endif()
if(GTSAM_BUILD_PYTHON AND GTSAM_ALLOW_DEPRECATED_SINCE_V4)
@ -111,17 +115,34 @@ set(CPACK_GENERATOR "TGZ" CACHE STRING "CPack Default Binary Generator")
# BOOST_ROOT: path to install prefix for boost
# Boost_NO_SYSTEM_PATHS: set to true to keep the find script from ignoring BOOST_ROOT
# If using Boost shared libs, disable auto linking
if(MSVC)
# Some libraries, at least Boost Program Options, rely on this to export DLL symbols
# Disable autolinking
# By default, boost only builds static libraries on windows
set(Boost_USE_STATIC_LIBS ON) # only find static libs
# If we ever reset above on windows and, ...
# If we use Boost shared libs, disable auto linking.
# Some libraries, at least Boost Program Options, rely on this to export DLL symbols.
if(NOT Boost_USE_STATIC_LIBS)
add_definitions(-DBOOST_ALL_NO_LIB)
add_definitions(-DBOOST_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_PRIVATE -Zm295)
endif()
endif()
find_package(Boost 1.43 COMPONENTS serialization system filesystem thread program_options date_time timer chrono)
# 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
@ -129,17 +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_LIBRARY} ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY}
${Boost_THREAD_LIBRARY} ${Boost_DATE_TIME_LIBRARY})
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")
add_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_LIBRARY} ${Boost_CHRONO_LIBRARY})
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.")
@ -149,29 +196,19 @@ endif()
if(NOT (${Boost_VERSION} LESS 105600))
message("Ignoring Boost restriction on optional lvalue assignment from rvalues")
add_definitions(-DBOOST_OPTIONAL_ALLOW_BINDING_TO_RVALUES -DBOOST_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()
###############################################################################
# Find TBB
find_package(TBB)
find_package(TBB COMPONENTS tbb tbbmalloc)
# Set up variables if we're using TBB
if(TBB_FOUND AND GTSAM_WITH_TBB)
set(GTSAM_USE_TBB 1) # This will go into config.h
include_directories(BEFORE ${TBB_INCLUDE_DIRS})
set(GTSAM_TBB_LIBRARIES "")
if(TBB_DEBUG_LIBRARIES)
foreach(lib ${TBB_LIBRARIES})
list(APPEND GTSAM_TBB_LIBRARIES optimized "${lib}")
endforeach()
foreach(lib ${TBB_DEBUG_LIBRARIES})
list(APPEND GTSAM_TBB_LIBRARIES debug "${lib}")
endforeach()
else()
set(GTSAM_TBB_LIBRARIES ${TBB_LIBRARIES})
endif()
list(APPEND GTSAM_ADDITIONAL_LIBRARIES ${GTSAM_TBB_LIBRARIES})
# all definitions and link requisites will go via imported targets:
# tbb & tbbmalloc
list(APPEND GTSAM_ADDITIONAL_LIBRARIES tbb tbbmalloc)
else()
set(GTSAM_USE_TBB 0) # This will go into config.h
endif()
@ -187,6 +224,20 @@ endif()
# Find Google perftools
find_package(GooglePerfTools)
###############################################################################
# Support ccache, if installed
if(NOT MSVC AND NOT XCODE_VERSION)
find_program(CCACHE_FOUND ccache)
if(CCACHE_FOUND)
if(GTSAM_BUILD_WITH_CCACHE)
set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE ccache)
set_property(GLOBAL PROPERTY RULE_LAUNCH_LINK ccache)
else()
set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE "")
set_property(GLOBAL PROPERTY RULE_LAUNCH_LINK "")
endif()
endif(CCACHE_FOUND)
endif()
###############################################################################
# Find MKL
@ -195,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
@ -230,30 +280,45 @@ 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. <Eigen/Core>
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
if(EIGEN_USE_MKL_ALL AND (EIGEN3_VERSION VERSION_LESS 3.2.5))
message(FATAL_ERROR "MKL requires at least Eigen 3.2.5, and your system appears to have an older version. Disable GTSAM_USE_SYSTEM_EIGEN to use GTSAM's copy of Eigen, or disable GTSAM_WITH_EIGEN_MKL")
endif()
# Check for Eigen version which doesn't work with MKL
# See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1527 for details.
if(EIGEN_USE_MKL_ALL AND (EIGEN3_VERSION VERSION_EQUAL 3.3.4))
message(FATAL_ERROR "MKL does not work with Eigen 3.3.4 because of a bug in Eigen. See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1527. Disable GTSAM_USE_SYSTEM_EIGEN to use GTSAM's copy of Eigen, disable GTSAM_WITH_EIGEN_MKL, or upgrade/patch your installation of Eigen.")
endif()
# The actual include directory (for BUILD cmake target interface):
set(GTSAM_EIGEN_INCLUDE_FOR_BUILD "${EIGEN3_INCLUDE_DIR}")
else()
# Use bundled Eigen include path.
# Clear any variables set by FindEigen3
if(EIGEN3_INCLUDE_DIR)
set(EIGEN3_INCLUDE_DIR NOTFOUND CACHE STRING "" FORCE)
endif()
# Add the bundled version of eigen to the include path so that it can still be included
# with #include <Eigen/Core>
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_PUBLIC DEIGEN_NO_STATIC_ASSERT)
endif()
list(APPEND GTSAM_COMPILE_OPTIONS_PRIVATE "/wd4244") # Disable loss of precision which is thrown all over our Eigen
endif()
###############################################################################
@ -294,51 +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)
add_definitions(-D_CRT_SECURE_NO_WARNINGS -D_SCL_SECURE_NO_WARNINGS)
add_definitions(/wd4251 /wd4275 /wd4251 /wd4661 /wd4344 /wd4503) # Disable non-DLL-exported base class and other warnings
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)
add_definitions(-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)
add_definitions(-Wno-unused-local-typedefs)
list(APPEND GTSAM_COMPILE_OPTIONS_PRIVATE -Wno-unused-local-typedefs)
endif()
endif()
if(GTSAM_ENABLE_CONSISTENCY_CHECKS)
add_definitions(-DGTSAM_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()
###############################################################################
@ -457,7 +500,7 @@ print_config_flag(${GTSAM_BUILD_TIMING_ALWAYS} "Build timing scripts wit
if (DOXYGEN_FOUND)
print_config_flag(${GTSAM_BUILD_DOCS} "Build Docs ")
endif()
print_config_flag(${GTSAM_BUILD_STATIC_LIBRARY} "Build static GTSAM library instead of shared")
print_config_flag(${BUILD_SHARED_LIBS} "Build shared GTSAM libraries ")
print_config_flag(${GTSAM_BUILD_TYPE_POSTFIXES} "Put build type in library name ")
if(GTSAM_UNSTABLE_AVAILABLE)
print_config_flag(${GTSAM_BUILD_UNSTABLE} "Build libgtsam_unstable ")
@ -500,6 +543,15 @@ else()
endif()
message(STATUS " Default allocator : ${GTSAM_DEFAULT_ALLOCATOR}")
if(NOT MSVC AND NOT XCODE_VERSION)
if(CCACHE_FOUND AND GTSAM_BUILD_WITH_CCACHE)
message(STATUS " Build with ccache : Yes")
elseif(CCACHE_FOUND)
message(STATUS " Build with ccache : ccache found but GTSAM_BUILD_WITH_CCACHE is disabled")
else()
message(STATUS " Build with ccache : No")
endif()
endif()
message(STATUS "Packaging flags ")
message(STATUS " CPack Source Generator : ${CPACK_SOURCE_GENERATOR}")
@ -532,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 "===============================================================")
@ -540,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}")

1
CppUnitLite/CMakeLists.txt
View File

@ -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

14
GTSAM-Concepts.md
View File

@ -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<Class,N>::inverse` so you get an inverse with a derivative.
Finally, to create the traits automatically you can use `internal::LieGroupTraits<Class>`
Vector Space
------------

146
INSTALL
View File

@ -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.

168
INSTALL.md Normal file
View File

@ -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.

27
LICENSE
View File

@ -1,18 +1,25 @@
GTSAM is released under the simplified BSD license, reproduced in the file
LICENSE.BSD in this directory.
GTSAM contains two third party libraries, with documentation of licensing and
modifications as follows:
GTSAM contains several third party libraries, with documentation of licensing
and modifications as follows:
- CCOLAMD 2.9.3: Tim Davis' constrained column approximate minimum degree
- CCOLAMD 2.9.6: Tim Davis' constrained column approximate minimum degree
ordering library
- Included unmodified in gtsam/3rdparty/CCOLAMD and gtsam/3rdparty/UFconfig
- Included unmodified in gtsam/3rdparty/CCOLAMD and
gtsam/3rdparty/SuiteSparse_config
- http://faculty.cse.tamu.edu/davis/suitesparse.html
- Licenced under BSD-3, provided in gtsam/3rdparty/CCOLAMD/Doc/License.txt
- Eigen 3.2: General C++ matrix and linear algebra library
- Modified with 3 patches that have been contributed back to the Eigen team:
- http://eigen.tuxfamily.org/bz/show_bug.cgi?id=704 (Householder QR MKL selection)
- http://eigen.tuxfamily.org/bz/show_bug.cgi?id=705 (Fix MKL LLT return code)
- http://eigen.tuxfamily.org/bz/show_bug.cgi?id=716 (Improved comma initialization)
- ceres: Google's nonlinear least-squares optimization library
- Includes only auto-diff/jet code, with minor modifications to includes
- http://ceres-solver.org/license.html
- Eigen 3.3.7: General C++ matrix and linear algebra library
- Licenced under MPL2, provided in gtsam/3rdparty/Eigen/COPYING.README
- Some code that is 3rd-party to Eigen is BSD and LGPL
- Some code that is 3rd-party to Eigen is BSD and LGPL
- GeographicLib 1.35: Charles Karney's geographic conversion utility library
- Included unmodified in gtsam/3rdparty/GeographicLib
- Licenced under MIT, provided in gtsam/3rdparty/GeographicLib/LICENSE.txt
- METIS 5.1.0: Graph partitioning and fill-reducing matrix ordering library
- Included unmodified in gtsam/3rdparty/metis
- Licenced under Apache License v 2.0, provided in
gtsam/3rdparty/metis/LICENSE.txt

12
README.md
View File

@ -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).
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).

2
THANKS
View File

@ -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

8
cmake/Config.cmake.in
View File

@ -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)

11
cmake/FindCython.cmake
View File

@ -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

9
cmake/FindMKL.cmake
View File

@ -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)

8
cmake/FindNumPy.cmake
View File

@ -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)

532
cmake/FindTBB.cmake
View File

@ -1,13 +1,6 @@
# Locate Intel Threading Building Blocks include paths and libraries
# FindTBB.cmake can be found at https://code.google.com/p/findtbb/
# Written by Hannes Hofmann <hannes.hofmann _at_ informatik.uni-erlangen.de>
# Improvements by Gino van den Bergen <gino _at_ dtecta.com>,
# Florian Uhlig <F.Uhlig _at_ gsi.de>,
# Jiri Marsik <jiri.marsik89 _at_ gmail.com>
# The MIT License
# The MIT License (MIT)
#
# Copyright (c) 2011 Hannes Hofmann
# Copyright (c) 2015 Justus Calvin
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
@ -16,295 +9,306 @@
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in
# all copies or substantial portions of the Software.
# The above copyright notice and this permission notice shall be included in all
# copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
# THE SOFTWARE.
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.
# GvdB: This module uses the environment variable TBB_ARCH_PLATFORM which defines architecture and compiler.
# e.g. "ia32/vc8" or "em64t/cc4.1.0_libc2.4_kernel2.6.16.21"
# TBB_ARCH_PLATFORM is set by the build script tbbvars[.bat|.sh|.csh], which can be found
# in the TBB installation directory (TBB_INSTALL_DIR).
#
# GvdB: Mac OS X distribution places libraries directly in lib directory.
# FindTBB
# -------
#
# For backwards compatibility, you may explicitely set the CMake variables TBB_ARCHITECTURE and TBB_COMPILER.
# TBB_ARCHITECTURE [ ia32 | em64t | itanium ]
# which architecture to use
# TBB_COMPILER e.g. vc9 or cc3.2.3_libc2.3.2_kernel2.4.21 or cc4.0.1_os10.4.9
# which compiler to use (detected automatically on Windows)
# Find TBB include directories and libraries.
#
# Usage:
#
# find_package(TBB [major[.minor]] [EXACT]
# [QUIET] [REQUIRED]
# [[COMPONENTS] [components...]]
# [OPTIONAL_COMPONENTS components...])
#
# where the allowed components are tbbmalloc and tbb_preview. Users may modify
# the behavior of this module with the following variables:
#
# * TBB_ROOT_DIR - The base directory the of TBB installation.
# * TBB_INCLUDE_DIR - The directory that contains the TBB headers files.
# * TBB_LIBRARY - The directory that contains the TBB library files.
# * TBB_<library>_LIBRARY - The path of the TBB the corresponding TBB library.
# These libraries, if specified, override the
# corresponding library search results, where <library>
# may be tbb, tbb_debug, tbbmalloc, tbbmalloc_debug,
# tbb_preview, or tbb_preview_debug.
# * TBB_USE_DEBUG_BUILD - The debug version of tbb libraries, if present, will
# be used instead of the release version.
#
# Users may modify the behavior of this module with the following environment
# variables:
#
# * TBB_INSTALL_DIR
# * TBBROOT
# * LIBRARY_PATH
#
# This module will set the following variables:
#
# * TBB_FOUND - Set to false, or undefined, if we haven’t found, or
# don’t want to use TBB.
# * TBB_<component>_FOUND - If False, optional <component> part of TBB sytem is
# not available.
# * TBB_VERSION - The full version string
# * TBB_VERSION_MAJOR - The major version
# * TBB_VERSION_MINOR - The minor version
# * TBB_INTERFACE_VERSION - The interface version number defined in
# tbb/tbb_stddef.h.
# * TBB_<library>_LIBRARY_RELEASE - The path of the TBB release version of
# <library>, where <library> may be tbb, tbb_debug,
# tbbmalloc, tbbmalloc_debug, tbb_preview, or
# tbb_preview_debug.
# * TBB_<library>_LIBRARY_DEGUG - The path of the TBB release version of
# <library>, where <library> may be tbb, tbb_debug,
# tbbmalloc, tbbmalloc_debug, tbb_preview, or
# tbb_preview_debug.
#
# The following varibles should be used to build and link with TBB:
#
# * TBB_INCLUDE_DIRS - The include directory for TBB.
# * TBB_LIBRARIES - The libraries to link against to use TBB.
# * TBB_LIBRARIES_RELEASE - The release libraries to link against to use TBB.
# * TBB_LIBRARIES_DEBUG - The debug libraries to link against to use TBB.
# * TBB_DEFINITIONS - Definitions to use when compiling code that uses
# TBB.
# * TBB_DEFINITIONS_RELEASE - Definitions to use when compiling release code that
# uses TBB.
# * TBB_DEFINITIONS_DEBUG - Definitions to use when compiling debug code that
# uses TBB.
#
# This module will also create the "tbb" target that may be used when building
# executables and libraries.
# This module respects
# TBB_INSTALL_DIR or $ENV{TBB21_INSTALL_DIR} or $ENV{TBB_INSTALL_DIR}
include(FindPackageHandleStandardArgs)
# This module defines
# TBB_INCLUDE_DIRS, where to find task_scheduler_init.h, etc.
# TBB_LIBRARY_DIRS, where to find libtbb, libtbbmalloc
# TBB_DEBUG_LIBRARY_DIRS, where to find libtbb_debug, libtbbmalloc_debug
# TBB_INSTALL_DIR, the base TBB install directory
# TBB_LIBRARIES, the libraries to link against to use TBB.
# TBB_DEBUG_LIBRARIES, the libraries to link against to use TBB with debug symbols.
# TBB_FOUND, If false, don't try to use TBB.
# TBB_INTERFACE_VERSION, as defined in tbb/tbb_stddef.h
if(NOT TBB_FOUND)
##################################
# Check the build type
##################################
if (WIN32)
# has em64t/vc8 em64t/vc9
# has ia32/vc7.1 ia32/vc8 ia32/vc9
set(_TBB_DEFAULT_INSTALL_DIR "C:/Program Files/Intel/TBB")
set(_TBB_LIB_NAME "tbb")
set(_TBB_LIB_MALLOC_NAME "${_TBB_LIB_NAME}malloc")
set(_TBB_LIB_DEBUG_NAME "${_TBB_LIB_NAME}_debug")
set(_TBB_LIB_MALLOC_DEBUG_NAME "${_TBB_LIB_MALLOC_NAME}_debug")
if (MSVC71)
set (_TBB_COMPILER "vc7.1")
set (TBB_COMPILER "vc7.1")
endif(MSVC71)
if (MSVC80)
set(_TBB_COMPILER "vc8")
set(TBB_COMPILER "vc8")
endif(MSVC80)
if (MSVC90)
set(_TBB_COMPILER "vc9")
set(TBB_COMPILER "vc9")
endif(MSVC90)
if(MSVC10)
set(_TBB_COMPILER "vc10")
set(TBB_COMPILER "vc10")
endif(MSVC10)
if(MSVC11)
set(_TBB_COMPILER "vc11")
set(TBB_COMPILER "vc11")
endif(MSVC11)
if(MSVC14)
set(_TBB_COMPILER "vc14")
set(TBB_COMPILER "vc14")
endif(MSVC14)
# Todo: add other Windows compilers such as ICL.
if(TBB_ARCHITECTURE)
set(_TBB_ARCHITECTURE ${TBB_ARCHITECTURE})
elseif("$ENV{TBB_ARCH_PLATFORM}" STREQUAL "")
# Try to guess the architecture
if(CMAKE_CL_64)
set(_TBB_ARCHITECTURE intel64)
set(TBB_ARCHITECTURE intel64)
else()
set(_TBB_ARCHITECTURE ia32)
set(TBB_ARCHITECTURE ia32)
endif()
endif()
endif (WIN32)
if(NOT DEFINED TBB_USE_DEBUG_BUILD)
if(CMAKE_BUILD_TYPE MATCHES "(Debug|DEBUG|debug|RelWithDebInfo|RELWITHDEBINFO|relwithdebinfo)")
set(TBB_BUILD_TYPE DEBUG)
else()
set(TBB_BUILD_TYPE RELEASE)
endif()
elseif(TBB_USE_DEBUG_BUILD)
set(TBB_BUILD_TYPE DEBUG)
else()
set(TBB_BUILD_TYPE RELEASE)
endif()
if (UNIX)
if (APPLE)
# MAC
set(_TBB_DEFAULT_INSTALL_DIR "/Library/Frameworks/Intel_TBB.framework/Versions")
# libs: libtbb.dylib, libtbbmalloc.dylib, *_debug
set(_TBB_LIB_NAME "tbb")
set(_TBB_LIB_MALLOC_NAME "${_TBB_LIB_NAME}malloc")
set(_TBB_LIB_DEBUG_NAME "${_TBB_LIB_NAME}_debug")
set(_TBB_LIB_MALLOC_DEBUG_NAME "${_TBB_LIB_MALLOC_NAME}_debug")
# default flavor on apple: ia32/cc4.0.1_os10.4.9
# Jiri: There is no reason to presume there is only one flavor and
# that user's setting of variables should be ignored.
if(NOT TBB_COMPILER)
set(_TBB_COMPILER "cc4.0.1_os10.4.9")
elseif (NOT TBB_COMPILER)
set(_TBB_COMPILER ${TBB_COMPILER})
endif(NOT TBB_COMPILER)
if(NOT TBB_ARCHITECTURE)
set(_TBB_ARCHITECTURE "ia32")
elseif(NOT TBB_ARCHITECTURE)
set(_TBB_ARCHITECTURE ${TBB_ARCHITECTURE})
endif(NOT TBB_ARCHITECTURE)
else (APPLE)
# LINUX
set(_TBB_DEFAULT_INSTALL_DIR "/opt/intel/tbb" "/usr/local/include" "/usr/include")
set(_TBB_LIB_NAME "tbb")
set(_TBB_LIB_MALLOC_NAME "${_TBB_LIB_NAME}malloc")
set(_TBB_LIB_DEBUG_NAME "${_TBB_LIB_NAME}_debug")
set(_TBB_LIB_MALLOC_DEBUG_NAME "${_TBB_LIB_MALLOC_NAME}_debug")
# has em64t/cc3.2.3_libc2.3.2_kernel2.4.21 em64t/cc3.3.3_libc2.3.3_kernel2.6.5 em64t/cc3.4.3_libc2.3.4_kernel2.6.9 em64t/cc4.1.0_libc2.4_kernel2.6.16.21
# has ia32/*
# has itanium/*
set(_TBB_COMPILER ${TBB_COMPILER})
set(_TBB_ARCHITECTURE ${TBB_ARCHITECTURE})
endif (APPLE)
endif (UNIX)
##################################
# Set the TBB search directories
##################################
if (CMAKE_SYSTEM MATCHES "SunOS.*")
# SUN
# not yet supported
# has em64t/cc3.4.3_kernel5.10
# has ia32/*
endif (CMAKE_SYSTEM MATCHES "SunOS.*")
# Define search paths based on user input and environment variables
set(TBB_SEARCH_DIR ${TBB_ROOT_DIR} $ENV{TBB_INSTALL_DIR} $ENV{TBBROOT})
# Define the search directories based on the current platform
if(CMAKE_SYSTEM_NAME STREQUAL "Windows")
set(TBB_DEFAULT_SEARCH_DIR "C:/Program Files/Intel/TBB"
"C:/Program Files (x86)/Intel/TBB")
#-- Clear the public variables
set (TBB_FOUND "NO")
# Set the target architecture
if(CMAKE_SIZEOF_VOID_P EQUAL 8)
set(TBB_ARCHITECTURE "intel64")
else()
set(TBB_ARCHITECTURE "ia32")
endif()
# Set the TBB search library path search suffix based on the version of VC
if(WINDOWS_STORE)
set(TBB_LIB_PATH_SUFFIX "lib/${TBB_ARCHITECTURE}/vc11_ui")
elseif(MSVC14)
set(TBB_LIB_PATH_SUFFIX "lib/${TBB_ARCHITECTURE}/vc14")
elseif(MSVC12)
set(TBB_LIB_PATH_SUFFIX "lib/${TBB_ARCHITECTURE}/vc12")
elseif(MSVC11)
set(TBB_LIB_PATH_SUFFIX "lib/${TBB_ARCHITECTURE}/vc11")
elseif(MSVC10)
set(TBB_LIB_PATH_SUFFIX "lib/${TBB_ARCHITECTURE}/vc10")
endif()
#-- Find TBB install dir and set ${_TBB_INSTALL_DIR} and cached ${TBB_INSTALL_DIR}
# first: use CMake variable TBB_INSTALL_DIR
if (TBB_INSTALL_DIR)
set (_TBB_INSTALL_DIR ${TBB_INSTALL_DIR})
endif (TBB_INSTALL_DIR)
# second: use environment variable
if (NOT _TBB_INSTALL_DIR)
if (NOT "$ENV{TBB_INSTALL_DIR}" STREQUAL "")
set (_TBB_INSTALL_DIR $ENV{TBB_INSTALL_DIR})
endif (NOT "$ENV{TBB_INSTALL_DIR}" STREQUAL "")
# Intel recommends setting TBB21_INSTALL_DIR
if (NOT "$ENV{TBB21_INSTALL_DIR}" STREQUAL "")
set (_TBB_INSTALL_DIR $ENV{TBB21_INSTALL_DIR})
endif (NOT "$ENV{TBB21_INSTALL_DIR}" STREQUAL "")
if (NOT "$ENV{TBB22_INSTALL_DIR}" STREQUAL "")
set (_TBB_INSTALL_DIR $ENV{TBB22_INSTALL_DIR})
endif (NOT "$ENV{TBB22_INSTALL_DIR}" STREQUAL "")
if (NOT "$ENV{TBB30_INSTALL_DIR}" STREQUAL "")
set (_TBB_INSTALL_DIR $ENV{TBB30_INSTALL_DIR})
endif (NOT "$ENV{TBB30_INSTALL_DIR}" STREQUAL "")
endif (NOT _TBB_INSTALL_DIR)
# third: try to find path automatically
if (NOT _TBB_INSTALL_DIR)
if (_TBB_DEFAULT_INSTALL_DIR)
set (_TBB_INSTALL_DIR ${_TBB_DEFAULT_INSTALL_DIR})
endif (_TBB_DEFAULT_INSTALL_DIR)
endif (NOT _TBB_INSTALL_DIR)
# sanity check
if (NOT _TBB_INSTALL_DIR)
message (STATUS "TBB: Unable to find Intel TBB install directory. ${_TBB_INSTALL_DIR}")
else (NOT _TBB_INSTALL_DIR)
# finally: set the cached CMake variable TBB_INSTALL_DIR
if (NOT TBB_INSTALL_DIR)
set (TBB_INSTALL_DIR ${_TBB_INSTALL_DIR} CACHE PATH "Intel TBB install directory")
mark_as_advanced(TBB_INSTALL_DIR)
endif (NOT TBB_INSTALL_DIR)
# Add the library path search suffix for the VC independent version of TBB
list(APPEND TBB_LIB_PATH_SUFFIX "lib/${TBB_ARCHITECTURE}/vc_mt")
elseif(CMAKE_SYSTEM_NAME STREQUAL "Darwin")
# OS X
set(TBB_DEFAULT_SEARCH_DIR "/opt/intel/tbb")
#-- A macro to rewrite the paths of the library. This is necessary, because
# find_library() always found the em64t/vc9 version of the TBB libs
macro(TBB_CORRECT_LIB_DIR var_name)
# if (NOT "${_TBB_ARCHITECTURE}" STREQUAL "em64t")
string(REPLACE em64t "${_TBB_ARCHITECTURE}" ${var_name} ${${var_name}})
# endif (NOT "${_TBB_ARCHITECTURE}" STREQUAL "em64t")
string(REPLACE ia32 "${_TBB_ARCHITECTURE}" ${var_name} ${${var_name}})
string(REPLACE vc7.1 "${_TBB_COMPILER}" ${var_name} ${${var_name}})
string(REPLACE vc8 "${_TBB_COMPILER}" ${var_name} ${${var_name}})
string(REPLACE vc9 "${_TBB_COMPILER}" ${var_name} ${${var_name}})
string(REPLACE vc10 "${_TBB_COMPILER}" ${var_name} ${${var_name}})
string(REPLACE vc11 "${_TBB_COMPILER}" ${var_name} ${${var_name}})
endmacro(TBB_CORRECT_LIB_DIR var_content)
# TODO: Check to see which C++ library is being used by the compiler.
if(NOT ${CMAKE_SYSTEM_VERSION} VERSION_LESS 13.0)
# The default C++ library on OS X 10.9 and later is libc++
set(TBB_LIB_PATH_SUFFIX "lib/libc++" "lib")
else()
set(TBB_LIB_PATH_SUFFIX "lib")
endif()
elseif(CMAKE_SYSTEM_NAME STREQUAL "Linux")
# Linux
set(TBB_DEFAULT_SEARCH_DIR "/opt/intel/tbb")
# TODO: Check compiler version to see the suffix should be <arch>/gcc4.1 or
# <arch>/gcc4.1. For now, assume that the compiler is more recent than
# gcc 4.4.x or later.
if(CMAKE_SYSTEM_PROCESSOR STREQUAL "x86_64")
set(TBB_LIB_PATH_SUFFIX "lib/intel64/gcc4.4")
elseif(CMAKE_SYSTEM_PROCESSOR MATCHES "^i.86$")
set(TBB_LIB_PATH_SUFFIX "lib/ia32/gcc4.4")
endif()
endif()
#-- Look for include directory and set ${TBB_INCLUDE_DIR}
set (TBB_INC_SEARCH_DIR ${_TBB_INSTALL_DIR}/include)
# Jiri: tbbvars now sets the CPATH environment variable to the directory
# containing the headers.
find_path(TBB_INCLUDE_DIR
tbb/task_scheduler_init.h
PATHS ${TBB_INC_SEARCH_DIR} ENV CPATH
)
mark_as_advanced(TBB_INCLUDE_DIR)
##################################
# Find the TBB include dir
##################################
find_path(TBB_INCLUDE_DIRS tbb/tbb.h
HINTS ${TBB_INCLUDE_DIR} ${TBB_SEARCH_DIR}
PATHS ${TBB_DEFAULT_SEARCH_DIR}
PATH_SUFFIXES include)
#-- Look for libraries
# GvdB: $ENV{TBB_ARCH_PLATFORM} is set by the build script tbbvars[.bat|.sh|.csh]
if (NOT $ENV{TBB_ARCH_PLATFORM} STREQUAL "")
set (_TBB_LIBRARY_DIR
${_TBB_INSTALL_DIR}/lib/$ENV{TBB_ARCH_PLATFORM}
${_TBB_INSTALL_DIR}/$ENV{TBB_ARCH_PLATFORM}/lib
)
endif (NOT $ENV{TBB_ARCH_PLATFORM} STREQUAL "")
# Jiri: This block isn't mutually exclusive with the previous one
# (hence no else), instead I test if the user really specified
# the variables in question.
if ((NOT ${TBB_ARCHITECTURE} STREQUAL "") AND (NOT ${TBB_COMPILER} STREQUAL ""))
# HH: deprecated
message(STATUS "[Warning] FindTBB.cmake: The use of TBB_ARCHITECTURE and TBB_COMPILER is deprecated and may not be supported in future versions. Please set \$ENV{TBB_ARCH_PLATFORM} (using tbbvars.[bat|csh|sh]).")
# Jiri: It doesn't hurt to look in more places, so I store the hints from
# ENV{TBB_ARCH_PLATFORM} and the TBB_ARCHITECTURE and TBB_COMPILER
# variables and search them both.
set (
_TBB_LIBRARY_DIR "${_TBB_INSTALL_DIR}/${_TBB_ARCHITECTURE}/${_TBB_COMPILER}/lib" ${_TBB_LIBRARY_DIR}
_TBB_LIBRARY_DIR "${_TBB_INSTALL_DIR}/lib/${_TBB_ARCHITECTURE}/${_TBB_COMPILER}" ${_TBB_LIBRARY_DIR}
)
endif ((NOT ${TBB_ARCHITECTURE} STREQUAL "") AND (NOT ${TBB_COMPILER} STREQUAL ""))
##################################
# Set version strings
##################################
# GvdB: Mac OS X distribution places libraries directly in lib directory.
list(APPEND _TBB_LIBRARY_DIR ${_TBB_INSTALL_DIR}/lib)
if(TBB_INCLUDE_DIRS)
file(READ "${TBB_INCLUDE_DIRS}/tbb/tbb_stddef.h" _tbb_version_file)
string(REGEX REPLACE ".*#define TBB_VERSION_MAJOR ([0-9]+).*" "\\1"
TBB_VERSION_MAJOR "${_tbb_version_file}")
string(REGEX REPLACE ".*#define TBB_VERSION_MINOR ([0-9]+).*" "\\1"
TBB_VERSION_MINOR "${_tbb_version_file}")
string(REGEX REPLACE ".*#define TBB_INTERFACE_VERSION ([0-9]+).*" "\\1"
TBB_INTERFACE_VERSION "${_tbb_version_file}")
set(TBB_VERSION "${TBB_VERSION_MAJOR}.${TBB_VERSION_MINOR}")
endif()
# Jiri: No reason not to check the default paths. From recent versions,
# tbbvars has started exporting the LIBRARY_PATH and LD_LIBRARY_PATH
# variables, which now point to the directories of the lib files.
# It all makes more sense to use the ${_TBB_LIBRARY_DIR} as a HINTS
# argument instead of the implicit PATHS as it isn't hard-coded
# but computed by system introspection. Searching the LIBRARY_PATH
# and LD_LIBRARY_PATH environment variables is now even more important
# that tbbvars doesn't export TBB_ARCH_PLATFORM and it facilitates
# the use of TBB built from sources.
find_library(TBB_LIBRARY ${_TBB_LIB_NAME} HINTS ${_TBB_LIBRARY_DIR}
PATHS ENV LIBRARY_PATH ENV LD_LIBRARY_PATH)
find_library(TBB_MALLOC_LIBRARY ${_TBB_LIB_MALLOC_NAME} HINTS ${_TBB_LIBRARY_DIR}
PATHS ENV LIBRARY_PATH ENV LD_LIBRARY_PATH)
##################################
# Find TBB components
##################################
#Extract path from TBB_LIBRARY name
get_filename_component(TBB_LIBRARY_DIR ${TBB_LIBRARY} PATH)
if(TBB_VERSION VERSION_LESS 4.3)
set(TBB_SEARCH_COMPOMPONENTS tbb_preview tbbmalloc tbb)
else()
set(TBB_SEARCH_COMPOMPONENTS tbb_preview tbbmalloc_proxy tbbmalloc tbb)
endif()
#TBB_CORRECT_LIB_DIR(TBB_LIBRARY)
#TBB_CORRECT_LIB_DIR(TBB_MALLOC_LIBRARY)
mark_as_advanced(TBB_LIBRARY TBB_MALLOC_LIBRARY)
# Find each component
foreach(_comp ${TBB_SEARCH_COMPOMPONENTS})
if(";${TBB_FIND_COMPONENTS};tbb;" MATCHES ";${_comp};")
#-- Look for debug libraries
# Jiri: Changed the same way as for the release libraries.
find_library(TBB_LIBRARY_DEBUG ${_TBB_LIB_DEBUG_NAME} HINTS ${_TBB_LIBRARY_DIR}
PATHS ENV LIBRARY_PATH ENV LD_LIBRARY_PATH)
find_library(TBB_MALLOC_LIBRARY_DEBUG ${_TBB_LIB_MALLOC_DEBUG_NAME} HINTS ${_TBB_LIBRARY_DIR}
PATHS ENV LIBRARY_PATH ENV LD_LIBRARY_PATH)
# Search for the libraries
find_library(TBB_${_comp}_LIBRARY_RELEASE ${_comp}
HINTS ${TBB_LIBRARY} ${TBB_SEARCH_DIR}
PATHS ${TBB_DEFAULT_SEARCH_DIR} ENV LIBRARY_PATH
PATH_SUFFIXES ${TBB_LIB_PATH_SUFFIX})
# Jiri: Self-built TBB stores the debug libraries in a separate directory.
# Extract path from TBB_LIBRARY_DEBUG name
get_filename_component(TBB_LIBRARY_DEBUG_DIR ${TBB_LIBRARY_DEBUG} PATH)
find_library(TBB_${_comp}_LIBRARY_DEBUG ${_comp}_debug
HINTS ${TBB_LIBRARY} ${TBB_SEARCH_DIR}
PATHS ${TBB_DEFAULT_SEARCH_DIR} ENV LIBRARY_PATH
PATH_SUFFIXES ${TBB_LIB_PATH_SUFFIX})
#TBB_CORRECT_LIB_DIR(TBB_LIBRARY_DEBUG)
#TBB_CORRECT_LIB_DIR(TBB_MALLOC_LIBRARY_DEBUG)
mark_as_advanced(TBB_LIBRARY_DEBUG TBB_MALLOC_LIBRARY_DEBUG)
if(TBB_${_comp}_LIBRARY_DEBUG)
list(APPEND TBB_LIBRARIES_DEBUG "${TBB_${_comp}_LIBRARY_DEBUG}")
endif()
if(TBB_${_comp}_LIBRARY_RELEASE)
list(APPEND TBB_LIBRARIES_RELEASE "${TBB_${_comp}_LIBRARY_RELEASE}")
endif()
if(TBB_${_comp}_LIBRARY_${TBB_BUILD_TYPE} AND NOT TBB_${_comp}_LIBRARY)
set(TBB_${_comp}_LIBRARY "${TBB_${_comp}_LIBRARY_${TBB_BUILD_TYPE}}")
endif()
if(TBB_${_comp}_LIBRARY AND EXISTS "${TBB_${_comp}_LIBRARY}")
set(TBB_${_comp}_FOUND TRUE)
else()
set(TBB_${_comp}_FOUND FALSE)
endif()
if (TBB_INCLUDE_DIR)
if (TBB_LIBRARY)
set (TBB_FOUND "YES")
set (TBB_LIBRARIES ${TBB_LIBRARY} ${TBB_MALLOC_LIBRARY} ${TBB_LIBRARIES})
set (TBB_DEBUG_LIBRARIES ${TBB_LIBRARY_DEBUG} ${TBB_MALLOC_LIBRARY_DEBUG} ${TBB_DEBUG_LIBRARIES})
set (TBB_INCLUDE_DIRS ${TBB_INCLUDE_DIR} CACHE PATH "TBB include directory" FORCE)
set (TBB_LIBRARY_DIRS ${TBB_LIBRARY_DIR} CACHE PATH "TBB library directory" FORCE)
# Jiri: Self-built TBB stores the debug libraries in a separate directory.
set (TBB_DEBUG_LIBRARY_DIRS ${TBB_LIBRARY_DEBUG_DIR} CACHE PATH "TBB debug library directory" FORCE)
mark_as_advanced(TBB_INCLUDE_DIRS TBB_LIBRARY_DIRS TBB_DEBUG_LIBRARY_DIRS TBB_LIBRARIES TBB_DEBUG_LIBRARIES)
message(STATUS "Found Intel TBB")
endif (TBB_LIBRARY)
endif (TBB_INCLUDE_DIR)
# Mark internal variables as advanced
mark_as_advanced(TBB_${_comp}_LIBRARY_RELEASE)
mark_as_advanced(TBB_${_comp}_LIBRARY_DEBUG)
mark_as_advanced(TBB_${_comp}_LIBRARY)
if (NOT TBB_FOUND)
message(STATUS "TBB: Intel TBB NOT found!")
message(STATUS "TBB: Looked for Threading Building Blocks in ${_TBB_INSTALL_DIR}")
# do only throw fatal, if this pkg is REQUIRED
if (TBB_FIND_REQUIRED)
message(FATAL_ERROR "Could NOT find TBB library.")
endif (TBB_FIND_REQUIRED)
endif (NOT TBB_FOUND)
endif()
endforeach()
endif (NOT _TBB_INSTALL_DIR)
##################################
# Set compile flags and libraries
##################################
if (TBB_FOUND)
set(TBB_INTERFACE_VERSION 0)
FILE(READ "${TBB_INCLUDE_DIRS}/tbb/tbb_stddef.h" _TBB_VERSION_CONTENTS)
STRING(REGEX REPLACE ".*#define TBB_INTERFACE_VERSION ([0-9]+).*" "\\1" TBB_INTERFACE_VERSION "${_TBB_VERSION_CONTENTS}")
set(TBB_INTERFACE_VERSION "${TBB_INTERFACE_VERSION}")
endif (TBB_FOUND)
set(TBB_DEFINITIONS_RELEASE "")
set(TBB_DEFINITIONS_DEBUG "-DTBB_USE_DEBUG=1")
if(TBB_LIBRARIES_${TBB_BUILD_TYPE})
set(TBB_DEFINITIONS "${TBB_DEFINITIONS_${TBB_BUILD_TYPE}}")
set(TBB_LIBRARIES "${TBB_LIBRARIES_${TBB_BUILD_TYPE}}")
elseif(TBB_LIBRARIES_RELEASE)
set(TBB_DEFINITIONS "${TBB_DEFINITIONS_RELEASE}")
set(TBB_LIBRARIES "${TBB_LIBRARIES_RELEASE}")
elseif(TBB_LIBRARIES_DEBUG)
set(TBB_DEFINITIONS "${TBB_DEFINITIONS_DEBUG}")
set(TBB_LIBRARIES "${TBB_LIBRARIES_DEBUG}")
endif()
find_package_handle_standard_args(TBB
REQUIRED_VARS TBB_INCLUDE_DIRS TBB_LIBRARIES
HANDLE_COMPONENTS
VERSION_VAR TBB_VERSION)
##################################
# Create targets
##################################
if(NOT CMAKE_VERSION VERSION_LESS 3.0 AND TBB_FOUND)
# Start fix to support different targets for tbb, tbbmalloc, etc.
# (Jose Luis Blanco, Jan 2019)
# Iterate over tbb, tbbmalloc, etc.
foreach(libname ${TBB_SEARCH_COMPOMPONENTS})
if ((NOT TBB_${libname}_LIBRARY_RELEASE) AND (NOT TBB_${libname}_LIBRARY_DEBUG))
continue()
endif()
add_library(${libname} SHARED IMPORTED)
set_target_properties(${libname} PROPERTIES
INTERFACE_INCLUDE_DIRECTORIES ${TBB_INCLUDE_DIRS}
IMPORTED_LOCATION ${TBB_${libname}_LIBRARY_RELEASE})
if(TBB_${libname}_LIBRARY_RELEASE AND TBB_${libname}_LIBRARY_DEBUG)
set_target_properties(${libname} PROPERTIES
INTERFACE_COMPILE_DEFINITIONS "$<$<OR:$<CONFIG:Debug>,$<CONFIG:RelWithDebInfo>>:TBB_USE_DEBUG=1>"
IMPORTED_LOCATION_DEBUG ${TBB_${libname}_LIBRARY_DEBUG}
IMPORTED_LOCATION_RELWITHDEBINFO ${TBB_${libname}_LIBRARY_DEBUG}
IMPORTED_LOCATION_RELEASE ${TBB_${libname}_LIBRARY_RELEASE}
IMPORTED_LOCATION_MINSIZEREL ${TBB_${libname}_LIBRARY_RELEASE}
)
elseif(TBB_${libname}_LIBRARY_RELEASE)
set_target_properties(${libname} PROPERTIES IMPORTED_LOCATION ${TBB_${libname}_LIBRARY_RELEASE})
else()
set_target_properties(${libname} PROPERTIES
INTERFACE_COMPILE_DEFINITIONS "${TBB_DEFINITIONS_DEBUG}"
IMPORTED_LOCATION ${TBB_${libname}_LIBRARY_DEBUG}
)
endif()
endforeach()
# End of fix to support different targets
endif()
mark_as_advanced(TBB_INCLUDE_DIRS TBB_LIBRARIES)
unset(TBB_ARCHITECTURE)
unset(TBB_BUILD_TYPE)
unset(TBB_LIB_PATH_SUFFIX)
unset(TBB_DEFAULT_SEARCH_DIR)
endif()

27
cmake/GtsamAddPch.cmake Normal file
View File

@ -0,0 +1,27 @@
###############################################################################
# Macro:
#
# gtsamAddPch(precompiledHeader precompiledSource sources)
#
# Adds a precompiled header to compile all sources with. Currently only on MSVC.
# Inspired by https://stackoverflow.com/questions/148570/
#
# Arguments:
# precompiledHeader: the header file that includes headers to be precompiled.
# precompiledSource: the source file that simply includes that header above.
# sources: the list of source files to apply this to.
#
macro(gtsamAddPch precompiledHeader precompiledSource sources)
get_filename_component(pchBasename ${precompiledHeader} NAME_WE)
SET(precompiledBinary "${CMAKE_CURRENT_BINARY_DIR}/${pchBasename}.pch")
IF(MSVC)
message(STATUS "Adding precompiled header for MSVC")
set_source_files_properties(${precompiledSource}
PROPERTIES COMPILE_FLAGS "/Yc\"${precompiledHeader}\" /Fp\"${precompiledBinary}\""
OBJECT_OUTPUTS "${precompiledBinary}")
set_source_files_properties(${sources}
PROPERTIES COMPILE_FLAGS "/Yu\"${precompiledHeader}\" /FI\"${precompiledHeader}\" /Fp\"${precompiledBinary}\""
OBJECT_DEPENDS "${precompiledBinary}")
ENDIF(MSVC)
endmacro(gtsamAddPch)

19
cmake/GtsamCythonWrap.cmake
View File

@ -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()

29
cmake/dllexport.h.in
View File

@ -26,21 +26,30 @@
// class __declspec(dllexport) MyClass { ... };
// When included while compiling other code against GTSAM:
// class __declspec(dllimport) MyClass { ... };
#pragma once
// Whether GTSAM is compiled as static or DLL in windows.
// This will be used to decide whether include __declspec(dllimport) or not in headers
#cmakedefine BUILD_SHARED_LIBS
#ifdef _WIN32
# ifdef @library_name@_EXPORTS
# define @library_name@_EXPORT __declspec(dllexport)
# define @library_name@_EXTERN_EXPORT __declspec(dllexport) extern
# ifndef BUILD_SHARED_LIBS
# define @library_name@_EXPORT
# define @library_name@_EXTERN_EXPORT extern
# else
# ifndef @library_name@_IMPORT_STATIC
# ifdef @library_name@_EXPORTS
# define @library_name@_EXPORT __declspec(dllexport)
# define @library_name@_EXTERN_EXPORT __declspec(dllexport) extern
# else
# define @library_name@_EXPORT __declspec(dllimport)
# define @library_name@_EXTERN_EXPORT __declspec(dllimport)
# else /* @library_name@_IMPORT_STATIC */
# define @library_name@_EXPORT
# define @library_name@_EXTERN_EXPORT extern
# endif /* @library_name@_IMPORT_STATIC */
# endif /* @library_name@_EXPORTS */
#else /* _WIN32 */
# endif
# endif
#else
# define @library_name@_EXPORT
# define @library_name@_EXTERN_EXPORT extern
#endif
#undef BUILD_SHARED_LIBS

7
cmake/example_project/CMakeLists.txt
View File

@ -7,7 +7,7 @@
###################################################################################
# To create your own project, replace "example" with the actual name of your project
cmake_minimum_required(VERSION 2.6)
cmake_minimum_required(VERSION 3.0)
project(example CXX C)
# Include GTSAM CMake tools
@ -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

15
cython/CMakeLists.txt
View File

@ -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 ()

17
cython/README.md
View File

@ -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 <gtsam_folder>/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: <your CMAKE_INSTALL_PREFIX>/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: `<your CMAKE_INSTALL_PREFIX>/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:<GTSAM_CYTHON_INSTALL_PATH>
```
- 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
==========

26
cython/gtsam/__init__.py Normal file
View File

@ -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

2
cython/gtsam/__init__.py.in
View File

@ -1,2 +0,0 @@
from gtsam import *
${GTSAM_UNSTABLE_IMPORT}

17
cython/gtsam/examples/OdometryExample.py
View File

@ -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()

334
cython/gtsam/examples/PlanarManipulatorExample.py Normal file
View File

@ -0,0 +1,334 @@
"""
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
Kinematics of three-link manipulator with GTSAM poses and product of exponential maps.
Author: Frank Dellaert
"""
# pylint: disable=invalid-name, E1101
from __future__ import print_function
import math
import unittest
from functools import reduce
import matplotlib.pyplot as plt
import numpy as np
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):
"""Create 3D double numpy array."""
return np.array([x, y, z], dtype=np.float)
def compose(*poses):
"""Compose all Pose2 transforms given as arguments from left to right."""
return reduce((lambda x, y: x.compose(y)), poses)
def vee(M):
"""Pose2 vee operator."""
return vector3(M[0, 2], M[1, 2], M[1, 0])
def delta(g0, g1):
"""Difference between x,y,,theta components of SE(2) poses."""
return vector3(g1.x() - g0.x(), g1.y() - g0.y(), g1.theta() - g0.theta())
def trajectory(g0, g1, N=20):
""" Create an interpolated trajectory in SE(2), treating x,y, and theta separately.
g0 and g1 are the initial and final pose, respectively.
N is the number of *intervals*
Returns N+1 poses
"""
e = delta(g0, g1)
return [Pose2(g0.x()+e[0]*t, g0.y()+e[1]*t, g0.theta()+e[2]*t) for t in np.linspace(0, 1, N)]
class ThreeLinkArm(object):
"""Three-link arm class."""
def __init__(self):
self.L1 = 3.5
self.L2 = 3.5
self.L3 = 2.5
self.xi1 = vector3(0, 0, 1)
self.xi2 = vector3(self.L1, 0, 1)
self.xi3 = vector3(self.L1+self.L2, 0, 1)
self.sXt0 = Pose2(0, self.L1+self.L2 + self.L3, math.radians(90))
def fk(self, q):
""" Forward kinematics.
Takes numpy array of joint angles, in radians.
"""
sXl1 = Pose2(0, 0, math.radians(90))
l1Zl1 = Pose2(0, 0, q[0])
l1Xl2 = Pose2(self.L1, 0, 0)
l2Zl2 = Pose2(0, 0, q[1])
l2Xl3 = Pose2(self.L2, 0, 0)
l3Zl3 = Pose2(0, 0, q[2])
l3Xt = Pose2(self.L3, 0, 0)
return compose(sXl1, l1Zl1, l1Xl2, l2Zl2, l2Xl3, l3Zl3, l3Xt)
def jacobian(self, q):
""" Calculate manipulator Jacobian.
Takes numpy array of joint angles, in radians.
"""
a = q[0]+q[1]
b = a+q[2]
return np.array([[-self.L1*math.cos(q[0]) - self.L2*math.cos(a)-self.L3*math.cos(b),
-self.L1*math.cos(a)-self.L3*math.cos(b),
- self.L3*math.cos(b)],
[-self.L1*math.sin(q[0]) - self.L2*math.sin(a)-self.L3*math.sin(b),
-self.L1*math.sin(a)-self.L3*math.sin(b),
- self.L3*math.sin(b)],
[1, 1, 1]], np.float)
def poe(self, q):
""" Forward kinematics.
Takes numpy array of joint angles, in radians.
"""
l1Zl1 = Pose2.Expmap(self.xi1 * q[0])
l2Zl2 = Pose2.Expmap(self.xi2 * q[1])
l3Zl3 = Pose2.Expmap(self.xi3 * q[2])
return compose(l1Zl1, l2Zl2, l3Zl3, self.sXt0)
def con(self, q):
""" Forward kinematics, conjugation form.
Takes numpy array of joint angles, in radians.
"""
def expmap(x, y, theta):
"""Implement exponential map via conjugation with axis (x,y)."""
return compose(Pose2(x, y, 0), Pose2(0, 0, theta), Pose2(-x, -y, 0))
l1Zl1 = expmap(0.0, 0.0, q[0])
l2Zl2 = expmap(0.0, self.L1, q[1])
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):
""" Inverse kinematics.
Takes desired Pose2 of tool T with respect to base S.
Optional: mu, gradient descent rate; e: error norm threshold
"""
q = np.radians(vector3(30, -30, 45)) # well within workspace
error = vector3(100, 100, 100)
while np.linalg.norm(error) > e:
error = delta(sTt_desired, self.fk(q))
J = self.jacobian(q)
q -= np.dot(np.linalg.pinv(J), error)
# return result in interval [-pi,pi)
return np.remainder(q+math.pi, 2*math.pi)-math.pi
def manipulator_jacobian(self, q):
""" Calculate manipulator Jacobian.
Takes numpy array of joint angles, in radians.
Returns the manipulator Jacobian of differential twists. When multiplied with
a vector of joint velocities, will yield a single differential twist which is
the spatial velocity d(sTt)/dt * inv(sTt) of the end-effector pose.
Just like always, differential twists can be hatted and multiplied with spatial
coordinates of a point to give the spatial velocity of the point.
"""
l1Zl1 = Pose2.Expmap(self.xi1 * q[0])
l2Zl2 = Pose2.Expmap(self.xi2 * q[1])
# l3Zl3 = Pose2.Expmap(self.xi3 * q[2])
p1 = self.xi1
# p1 = Pose2().Adjoint(self.xi1)
sTl1 = l1Zl1
p2 = sTl1.Adjoint(self.xi2)
sTl2 = compose(l1Zl1, l2Zl2)
p3 = sTl2.Adjoint(self.xi3)
differential_twists = [p1, p2, p3]
return np.stack(differential_twists, axis=1)
def plot(self, fignum, q):
""" Plot arm.
Takes figure number, and numpy array of joint angles, in radians.
"""
fig = plt.figure(fignum)
axes = fig.gca()
sXl1 = Pose2(0, 0, math.radians(90))
t = sXl1.translation()
p1 = np.array([t.x(), t.y()])
gtsam_plot.plot_pose2_on_axes(axes, sXl1)
def plot_line(p, g, color):
t = g.translation()
q = np.array([t.x(), t.y()])
line = np.append(p[np.newaxis], q[np.newaxis], axis=0)
axes.plot(line[:, 0], line[:, 1], color)
return q
l1Zl1 = Pose2(0, 0, q[0])
l1Xl2 = Pose2(self.L1, 0, 0)
sTl2 = compose(sXl1, l1Zl1, l1Xl2)
p2 = plot_line(p1, sTl2, 'r-')
gtsam_plot.plot_pose2_on_axes(axes, sTl2)
l2Zl2 = Pose2(0, 0, q[1])
l2Xl3 = Pose2(self.L2, 0, 0)
sTl3 = compose(sTl2, l2Zl2, l2Xl3)
p3 = plot_line(p2, sTl3, 'g-')
gtsam_plot.plot_pose2_on_axes(axes, sTl3)
l3Zl3 = Pose2(0, 0, q[2])
l3Xt = Pose2(self.L3, 0, 0)
sTt = compose(sTl3, l3Zl3, l3Xt)
plot_line(p3, sTt, 'b-')
gtsam_plot.plot_pose2_on_axes(axes, sTt)
# Create common example configurations.
Q0 = vector3(0, 0, 0)
Q1 = np.radians(vector3(-30, -45, -90))
Q2 = np.radians(vector3(-90, 90, 0))
class TestPose2SLAMExample(GtsamTestCase):
"""Unit tests for functions used below."""
def setUp(self):
self.arm = ThreeLinkArm()
def assertPose2Equals(self, actual, expected, tol=1e-2):
"""Helper function that prints out actual and expected if not equal."""
equal = actual.equals(expected, tol)
if not equal:
raise self.failureException(
"Poses are not equal:\n{}!={}".format(actual, expected))
def test_fk_arm(self):
"""Make sure forward kinematics is correct for some known test configurations."""
# at rest
expected = Pose2(0, 2*3.5 + 2.5, math.radians(90))
sTt = self.arm.fk(Q0)
self.assertIsInstance(sTt, Pose2)
self.assertPose2Equals(sTt, expected)
# -30, -45, -90
expected = Pose2(5.78, 1.52, math.radians(-75))
sTt = self.arm.fk(Q1)
self.assertPose2Equals(sTt, expected)
def test_jacobian(self):
"""Test Jacobian calculation."""
# at rest
expected = np.array([[-9.5, -6, -2.5], [0, 0, 0], [1, 1, 1]], np.float)
J = self.arm.jacobian(Q0)
np.testing.assert_array_almost_equal(J, expected)
# at -90, 90, 0
expected = np.array([[-6, -6, -2.5], [3.5, 0, 0], [1, 1, 1]], np.float)
J = self.arm.jacobian(Q2)
np.testing.assert_array_almost_equal(J, expected)
def test_con_arm(self):
"""Make sure POE is correct for some known test configurations."""
# at rest
expected = Pose2(0, 2*3.5 + 2.5, math.radians(90))
sTt = self.arm.con(Q0)
self.assertIsInstance(sTt, Pose2)
self.assertPose2Equals(sTt, expected)
# -30, -45, -90
expected = Pose2(5.78, 1.52, math.radians(-75))
sTt = self.arm.con(Q1)
self.assertPose2Equals(sTt, expected)
def test_poe_arm(self):
"""Make sure POE is correct for some known test configurations."""
# at rest
expected = Pose2(0, 2*3.5 + 2.5, math.radians(90))
sTt = self.arm.poe(Q0)
self.assertIsInstance(sTt, Pose2)
self.assertPose2Equals(sTt, expected)
# -30, -45, -90
expected = Pose2(5.78, 1.52, math.radians(-75))
sTt = self.arm.poe(Q1)
self.assertPose2Equals(sTt, expected)
def test_ik(self):
"""Check iterative inverse kinematics function."""
# at rest
actual = self.arm.ik(Pose2(0, 2*3.5 + 2.5, math.radians(90)))
np.testing.assert_array_almost_equal(actual, Q0, decimal=2)
# -30, -45, -90
sTt_desired = Pose2(5.78, 1.52, math.radians(-75))
actual = self.arm.ik(sTt_desired)
self.assertPose2Equals(self.arm.fk(actual), sTt_desired)
np.testing.assert_array_almost_equal(actual, Q1, decimal=2)
def test_manipulator_jacobian(self):
"""Test Jacobian calculation."""
# at rest
expected = np.array([[0, 3.5, 7], [0, 0, 0], [1, 1, 1]], np.float)
J = self.arm.manipulator_jacobian(Q0)
np.testing.assert_array_almost_equal(J, expected)
# at -90, 90, 0
expected = np.array(
[[0, 0, 3.5], [0, -3.5, -3.5], [1, 1, 1]], np.float)
J = self.arm.manipulator_jacobian(Q2)
np.testing.assert_array_almost_equal(J, expected)
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()
axes.set_xlim(-5, 5)
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)
J = arm.jacobian(q)
q += np.dot(np.linalg.inv(J), error)
arm.plot(fignum, q)
plt.pause(0.01)
plt.pause(10)
if __name__ == "__main__":
run_example()
unittest.main()

10
cython/gtsam/examples/Pose2SLAMExample.py
View File

@ -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()

89
cython/gtsam/examples/Pose2SLAMExample_g2o.py Normal file
View File

@ -0,0 +1,89 @@
"""
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
A 2D Pose SLAM example that reads input from g2o, converts it to a factor graph
and does the optimization. Output is written on a file, in g2o format
"""
# pylint: disable=invalid-name, E1101
from __future__ import print_function
import argparse
import math
import numpy as np
import matplotlib.pyplot as plt
import gtsam
from gtsam.utils import plot
def vector3(x, y, z):
"""Create 3d double numpy array."""
return np.array([x, y, z], dtype=np.float)
parser = argparse.ArgumentParser(
description="A 2D Pose SLAM example that reads input from g2o, "
"converts it to a factor graph and does the optimization. "
"Output is written on a file, in g2o format")
parser.add_argument('-i', '--input', help='input file g2o format')
parser.add_argument('-o', '--output',
help="the path to the output file with optimized graph")
parser.add_argument('-m', '--maxiter', type=int,
help="maximum number of iterations for optimizer")
parser.add_argument('-k', '--kernel', choices=['none', 'huber', 'tukey'],
default="none", help="Type of kernel used")
parser.add_argument("-p", "--plot", action="store_true",
help="Flag to plot results")
args = parser.parse_args()
g2oFile = gtsam.findExampleDataFile("noisyToyGraph.txt") if args.input is None\
else args.input
maxIterations = 100 if args.maxiter is None else args.maxiter
is3D = False
graph, initial = gtsam.readG2o(g2oFile, is3D)
assert args.kernel == "none", "Supplied kernel type is not yet implemented"
# Add prior on the pose having index (key) = 0
graphWithPrior = graph
priorModel = gtsam.noiseModel_Diagonal.Variances(vector3(1e-6, 1e-6, 1e-8))
graphWithPrior.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(), priorModel))
params = gtsam.GaussNewtonParams()
params.setVerbosity("Termination")
params.setMaxIterations(maxIterations)
# parameters.setRelativeErrorTol(1e-5)
# Create the optimizer ...
optimizer = gtsam.GaussNewtonOptimizer(graphWithPrior, initial, params)
# ... and optimize
result = optimizer.optimize()
print("Optimization complete")
print("initial error = ", graph.error(initial))
print("final error = ", graph.error(result))
if args.output is None:
print("\nFactor Graph:\n{}".format(graph))
print("\nInitial Estimate:\n{}".format(initial))
print("Final Result:\n{}".format(result))
else:
outputFile = args.output
print("Writing results to file: ", outputFile)
graphNoKernel, _ = gtsam.readG2o(g2oFile, is3D)
gtsam.writeG2o(graphNoKernel, result, outputFile)
print ("Done!")
if args.plot:
resultPoses = gtsam.extractPose2(result)
for i in range(resultPoses.shape[0]):
plot.plot_pose2(1, gtsam.Pose2(resultPoses[i, :]))
plt.show()

72
cython/gtsam/examples/Pose3SLAMExample_g2o.py Normal file
View File

@ -0,0 +1,72 @@
"""
* @file Pose3SLAMExample_initializePose3.cpp
* @brief A 3D Pose SLAM example that reads input from g2o, and initializes the
* Pose3 using InitializePose3
* @date Jan 17, 2019
* @author Vikrant Shah based on CPP example by Luca Carlone
"""
# pylint: disable=invalid-name, E1101
from __future__ import print_function
import argparse
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import gtsam
from gtsam.utils import plot
def vector6(x, y, z, a, b, c):
"""Create 6d double numpy array."""
return np.array([x, y, z, a, b, c], dtype=np.float)
parser = argparse.ArgumentParser(
description="A 3D Pose SLAM example that reads input from g2o, and "
"initializes Pose3")
parser.add_argument('-i', '--input', help='input file g2o format')
parser.add_argument('-o', '--output',
help="the path to the output file with optimized graph")
parser.add_argument("-p", "--plot", action="store_true",
help="Flag to plot results")
args = parser.parse_args()
g2oFile = gtsam.findExampleDataFile("pose3example.txt") if args.input is None \
else args.input
is3D = True
graph, initial = gtsam.readG2o(g2oFile, is3D)
# Add Prior on the first key
priorModel = gtsam.noiseModel_Diagonal.Variances(vector6(1e-6, 1e-6, 1e-6,
1e-4, 1e-4, 1e-4))
print("Adding prior to g2o file ")
graphWithPrior = graph
firstKey = initial.keys().at(0)
graphWithPrior.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel))
params = gtsam.GaussNewtonParams()
params.setVerbosity("Termination") # this will show info about stopping conds
optimizer = gtsam.GaussNewtonOptimizer(graphWithPrior, initial, params)
result = optimizer.optimize()
print("Optimization complete")
print("initial error = ", graphWithPrior.error(initial))
print("final error = ", graphWithPrior.error(result))
if args.output is None:
print("Final Result:\n{}".format(result))
else:
outputFile = args.output
print("Writing results to file: ", outputFile)
graphNoKernel, _ = gtsam.readG2o(g2oFile, is3D)
gtsam.writeG2o(graphNoKernel, result, outputFile)
print ("Done!")
if args.plot:
resultPoses = gtsam.allPose3s(result)
for i in range(resultPoses.size()):
plot.plot_pose3(1, resultPoses.atPose3(i))
plt.show()

35
cython/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py Normal file
View File

@ -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)

57
cython/gtsam/examples/README.md
View File

@ -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

118
cython/gtsam/examples/SFMExample.py Normal file
View File

@ -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()

84
cython/gtsam/examples/SimpleRotation.py Normal file
View File

@ -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()

106
cython/gtsam/examples/VisualISAMExample.py Normal file
View File

@ -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()

112
cython/gtsam/tests/experiments.py
View File

@ -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)

772
cython/gtsam/tests/gtsam_test.h
View File

@ -1,772 +0,0 @@
namespace gtsam {
#include <gtsam/inference/Key.h>
typedef size_t Key;
#include <gtsam/base/FastVector.h>
template<T> 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<gtsam::Key> KeyVector;
//*************************************************************************
// geometry
//*************************************************************************
#include <gtsam/geometry/Point2.h>
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 <gtsam/geometry/Point3.h>
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 <gtsam/geometry/Rot2.h>
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 <gtsam/geometry/Rot3.h>
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 <gtsam/geometry/Pose2.h>
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 <gtsam/geometry/Pose3.h>
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 <gtsam/linear/NoiseModel.h>
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 <gtsam/linear/Sampler.h>
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 <gtsam/linear/VectorValues.h>
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 <gtsam/linear/GaussianFactor.h>
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<Matrix, Vector> jacobian() const;
size_t size() const;
bool empty() const;
};
#include <gtsam/linear/JacobianFactor.h>
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<Matrix, Vector> jacobianUnweighted() const;
Matrix augmentedJacobianUnweighted() const;
void transposeMultiplyAdd(double alpha, const Vector& e, gtsam::VectorValues& x) const;
gtsam::JacobianFactor whiten() const;
//pair<gtsam::GaussianConditional*, gtsam::JacobianFactor*> 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 <gtsam/linear/HessianFactor.h>
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 <gtsam/nonlinear/Values.h>
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 <T = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
// gtsam::Rot3, gtsam::Pose3}>
// void insert(size_t j, const T& t);
// template <T = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
// gtsam::Rot3, gtsam::Pose3}>
// 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 = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
gtsam::Rot3, gtsam::Pose3, Vector, Matrix}>
T at(size_t j);
/// version for double
void insertDouble(size_t j, double c);
double atDouble(size_t j) const;
};
#include <gtsam/nonlinear/NonlinearFactor.h>
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 <gtsam/nonlinear/NonlinearFactorGraph.h>
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<gtsam::Key,int>& constraints) const;
//gtsam::GaussianFactorGraph* linearize(const gtsam::Values& values) const;
gtsam::NonlinearFactorGraph clone() const;
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/nonlinear/NonlinearFactor.h>
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 <gtsam/slam/PriorFactor.h>
template<T = {Vector, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3}>
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 <gtsam/slam/BetweenFactor.h>
template<T = {Vector, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3}>
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 <gtsam/inference/Symbol.h>
size_t symbol(char chr, size_t index);
char symbolChr(size_t key);
size_t symbolIndex(size_t key);
#include <gtsam/inference/Key.h>
// Default keyformatter
void PrintKeyVector(const gtsam::KeyVector& keys);
void PrintKeyVector(const gtsam::KeyVector& keys, string s);
#include <gtsam/nonlinear/NonlinearOptimizer.h>
bool checkConvergence(double relativeErrorTreshold,
double absoluteErrorTreshold, double errorThreshold,
double currentError, double newError);
#include <gtsam/slam/dataset.h>
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise, bool smart);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
gtsam::noiseModel::Diagonal* model, int maxID);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
gtsam::noiseModel::Diagonal* model);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> 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<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename);
void writeG2o(const gtsam::NonlinearFactorGraph& graph,
const gtsam::Values& estimate, string filename);
//*************************************************************************
// Utilities
//*************************************************************************
namespace utilities {
#include <gtsam/nonlinear/utilities.h>
// 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 <gtsam/nonlinear/utilities.h>
class RedirectCout {
RedirectCout();
string str();
};
} //\namespace gtsam

26
cython/gtsam/tests/test_Cal3Unified.py
View File

@ -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()

22
cython/gtsam/tests/test_JacobianFactor.py
View File

@ -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()

18
cython/gtsam/tests/test_KalmanFilter.py
View File

@ -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)

20
cython/gtsam/tests/test_LocalizationExample.py
View File

@ -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__":

72
cython/gtsam/tests/test_NonlinearOptimizer.py Normal file
View File

@ -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()

20
cython/gtsam/tests/test_OdometryExample.py
View File

@ -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()

22
cython/gtsam/tests/test_PlanarSLAMExample.py
View File

@ -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)

32
cython/gtsam/tests/test_Pose2.py Normal file
View File

@ -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()

20
cython/gtsam/tests/test_Pose2SLAMExample.py
View File

@ -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()

53
cython/gtsam/tests/test_Pose3.py
View File

@ -1,25 +1,43 @@
"""
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
from gtsam import Point3, Rot3, Pose3
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):
T2 = Pose3(Rot3.Rodrigues(0.3,0.2,0.1),Point3(3.5,-8.2,4.2))
def test_between(self):
"""Test between method."""
T2 = Pose3(Rot3.Rodrigues(0.3, 0.2, 0.1), Point3(3.5, -8.2, 4.2))
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):
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))
"""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.gtsamAssertEquals(actual, expected, 1e-6)
def test_range(self):
"""Test range method."""
l1 = Point3(1, 0, 0)
l2 = Point3(1, 1, 0)
x1 = Pose3()
@ -28,16 +46,23 @@ class TestPose3(unittest.TestCase):
xl2 = Pose3(Rot3.Ypr(0.0, 1.0, 0.0), Point3(1, 1, 0))
# establish range is indeed zero
self.assertEqual(1,x1.range(point=l1))
self.assertEqual(1, x1.range(point=l1))
# establish range is indeed sqrt2
self.assertEqual(math.sqrt(2.0),x1.range(point=l2))
self.assertEqual(math.sqrt(2.0), x1.range(point=l2))
# establish range is indeed zero
self.assertEqual(1,x1.range(pose=xl1))
self.assertEqual(1, x1.range(pose=xl1))
# establish range is indeed sqrt2
self.assertEqual(math.sqrt(2.0),x1.range(pose=xl2))
self.assertEqual(math.sqrt(2.0), x1.range(pose=xl2))
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)
np.testing.assert_array_equal(actual, expected)
if __name__ == "__main__":

28
cython/gtsam/tests/test_Pose3SLAMExample.py
View File

@ -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()

18
cython/gtsam/tests/test_PriorFactor.py
View File

@ -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()

23
cython/gtsam/tests/test_SFMExample.py
View File

@ -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()

21
cython/gtsam/tests/test_Scenario.py
View File

@ -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__':

25
cython/gtsam/tests/test_SimpleCamera.py
View File

@ -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__":

23
cython/gtsam/tests/test_StereoVOExample.py
View File

@ -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()

61
cython/gtsam/tests/test_Values.py
View File

@ -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()

23
cython/gtsam/tests/test_VisualISAMExample.py
View File

@ -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()

45
cython/gtsam/tests/test_dataset.py Normal file
View File

@ -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()

38
cython/gtsam/tests/test_dsf_map.py Normal file
View File

@ -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()

89
cython/gtsam/tests/test_initialize_pose3.py Normal file
View File

@ -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()

20
cython/gtsam/utils/plot.py
View File

@ -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):

27
cython/gtsam/utils/test_case.py Normal file
View File

@ -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))

11
cython/gtsam_eigency/CMakeLists.txt
View File

@ -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
$<BUILD_INTERFACE:${GTSAM_EIGEN_INCLUDE_FOR_BUILD}>
$<INSTALL_INTERFACE:${GTSAM_EIGEN_INCLUDE_FOR_INSTALL}>
)
target_include_directories(cythonize_eigency_core PUBLIC
$<BUILD_INTERFACE:${GTSAM_EIGEN_INCLUDE_FOR_BUILD}>
$<INSTALL_INTERFACE:${GTSAM_EIGEN_INCLUDE_FOR_INSTALL}>
)
add_dependencies(cythonize_eigency_core cythonize_eigency_conversions)
add_custom_target(cythonize_eigency)
add_dependencies(cythonize_eigency cythonize_eigency_conversions cythonize_eigency_core)

2
cython/gtsam_eigency/__init__.py.in
View File

@ -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__)

1
cython/gtsam_unstable/__init__.py Normal file
View File

@ -0,0 +1 @@
from .gtsam_unstable import *

102
cython/gtsam_unstable/examples/FixedLagSmootherExample.py Normal file
View File

@ -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")

0
cython/gtsam_unstable/examples/__init__.py Normal file
View File

0
cython/gtsam_unstable/tests/__init__.py Normal file
View File

135
cython/gtsam_unstable/tests/test_FixedLagSmootherExample.py Normal file
View File

@ -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()

49
cython/setup.py.in Normal file
View File

@ -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('#')]
)

812
examples/Data/ISAM2_SmartFactorStereo_IMU.txt Normal file
View File

@ -0,0 +1,812 @@
i 1 0.485693 0.334961 10.0536 0.00585324 0.00159634 0.00532113
i 1 0.449805 0.346924 10.0536 0.00744958 0.00159634 0.00478901
i 1 0.430664 0.351709 10.0249 0.00904591 0.00159634 0.0042569
i 1 0.442627 0.346924 9.99858 0.0106423 0.00159634 0.00478901
i 1 0.449805 0.344531 9.99619 0.0117065 0.00159634 0.00478901
i 1 0.44502 0.330176 10.0034 0.0127707 0.00159634 0.00478901
i 1 0.411523 0.325391 9.99141 0.0138349 0.00159634 0.0042569
i 1 0.411523 0.322998 9.98184 0.014367 0.00106423 0.00372479
i 1 0.413916 0.325391 9.97944 0.0148992 0.000532113 0.00266056
i 1 0.413916 0.332568 9.9938 0.0159634 0 0.00212845
i 1 0.385205 0.327783 9.97944 0.0175597 0.000532113 0.00159634
i 1 0.358887 0.332568 9.93398 0.0186239 0.00106423 0.00106423
i 1 0.368457 0.327783 9.8981 0.0196882 0.00212845 0.00159634
i 1 0.37085 0.342139 9.88613 0.0212845 0.00266056 0.00212845
i 1 0.361279 0.346924 9.88374 0.0223487 0.00319268 0.00266056
i 1 0.337354 0.356494 9.86221 0.023413 0.00266056 0.00266056
i 1 0.330176 0.37085 9.82632 0.0244772 0.00212845 0.00266056
i 1 0.349316 0.38042 9.80957 0.0250093 0.00159634 0.00266056
i 1 0.361279 0.392383 9.81675 0.0255414 0.00159634 0.00266056
i 1 0.344531 0.399561 9.80239 0.0266056 0.00159634 0.00266056
i 1 0.339746 0.394775 9.78086 0.0276699 0.00212845 0.00212845
i 1 0.330176 0.387598 9.74736 0.028202 0.00266056 0.00212845
i 1 0.332568 0.382812 9.74736 0.028202 0.00319268 0.00212845
i 1 0.313428 0.373242 9.75933 0.028202 0.00319268 0.00212845
i 1 0.282324 0.361279 9.74497 0.0276699 0.00319268 0.00266056
i 1 0.284717 0.363672 9.74497 0.028202 0.00319268 0.00319268
i 1 0.303857 0.358887 9.74497 0.0287341 0.00266056 0.00372479
i 1 0.327783 0.363672 9.77129 0.0303304 0.00319268 0.00478901
i 2 0.313428 0.368457 9.77129 0.0313946 0.00372479 0.00532113
i 2 0.313428 0.37085 9.74019 0.0324589 0.00478901 0.00585324
i 2 0.322998 0.368457 9.71387 0.0335231 0.00532113 0.00691746
i 2 0.344531 0.382812 9.70669 0.032991 0.00585324 0.00744958
i 2 0.342139 0.38999 9.72344 0.032991 0.00585324 0.0085138
i 2 0.322998 0.399561 9.71147 0.0324589 0.00532113 0.0085138
i 2 0.322998 0.409131 9.69233 0.0324589 0.00478901 0.0085138
i 2 0.349316 0.409131 9.69233 0.0324589 0.0042569 0.0085138
i 2 0.358887 0.416309 9.71865 0.0324589 0.00372479 0.0085138
i 2 0.361279 0.418701 9.73779 0.032991 0.00372479 0.0085138
i 2 0.361279 0.430664 9.73779 0.0335231 0.0042569 0.00904591
i 2 0.382812 0.437842 9.72822 0.0340552 0.00478901 0.00904591
i 2 0.411523 0.452197 9.73301 0.0340552 0.00532113 0.00957803
i 2 0.416309 0.449805 9.74258 0.0340552 0.00585324 0.00957803
i 2 0.399561 0.45459 9.72583 0.0340552 0.00638535 0.00957803
i 2 0.406738 0.452197 9.7043 0.0335231 0.00691746 0.00904591
i 2 0.433057 0.45459 9.69233 0.0335231 0.00744958 0.0085138
i 2 0.456982 0.452197 9.7019 0.032991 0.00744958 0.0085138
i 2 0.461768 0.449805 9.69473 0.032991 0.00798169 0.00798169
i 2 0.452197 0.437842 9.65884 0.032991 0.00798169 0.00744958
i 2 0.466553 0.430664 9.62534 0.0335231 0.0085138 0.00691746
i 2 0.483301 0.428271 9.62534 0.032991 0.0085138 0.00585324
i 2 0.480908 0.433057 9.64448 0.032991 0.0085138 0.00532113
i 2 0.46416 0.437842 9.64688 0.032991 0.0085138 0.00478901
i 2 0.45459 0.449805 9.6397 0.032991 0.0085138 0.00478901
i 2 0.478516 0.461768 9.6397 0.032991 0.0085138 0.00478901
i 2 0.483301 0.471338 9.65405 0.032991 0.00904591 0.0042569
s 2 1 885 772.028 190.341
s 2 5 467 386.865 243.455
s 2 8 814 710.317 241.333
s 2 9 820 715.802 246.233
s 2 10 815 711.209 245.21
s 2 11 883 770.583 198.045
s 2 12 877 764.741 195.175
s 2 14 877 764.741 195.175
s 2 17 941 820.999 242.057
s 2 19 948 827.372 235.193
s 2 20 948 827.445 241.199
s 2 23 557 473.955 298.317
s 2 28 760 663.371 287.116
s 2 33 813 709.517 280.212
s 2 34 818 713.39 277.34
s 2 36 815 711.652 286.066
s 2 37 893 779.04 309.272
s 2 39 899 784.538 308.196
s 2 40 908 793.502 319.35
s 2 41 902 787.564 319.381
s 2 42 938 819.201 257.197
s 2 43 989 863.847 282.826
s 2 44 972 848.495 314.307
s 2 46 975 852.26 260.734
s 2 51 232 144.289 321.996
s 2 53 598 513.866 338.268
s 2 54 693 603.198 367.966
s 2 58 815 710.993 377.494
s 2 59 799 697.398 356.37
s 2 63 893 779.706 351.987
s 2 67 900 786.329 357.122
s 2 68 898 784.262 353.125
s 2 70 916 800.114 348.207
s 2 71 986 861.517 326.212
s 2 72 986 861.517 326.212
s 2 73 976 854.046 367.861
s 2 79 565 493.761 419.421
s 2 80 560 488.614 422.453
s 2 81 575 501.453 445.367
s 2 83 602 525.726 413.384
s 2 86 701 611.86 434.26
s 2 87 701 611.86 434.26
s 2 88 653 570.815 405.361
s 2 91 747 652.271 432.316
s 2 92 754 658.581 441.421
s 2 93 708 618.271 430.162
s 2 94 744 649.626 434.257
s 2 95 823 718.362 385.033
s 2 96 797 696.56 426.269
s 2 97 801 700.128 396.737
s 2 98 773 675.088 390.936
s 2 101 885 773.221 414.264
s 2 102 880 768.822 387.314
s 2 103 888 776.039 410.211
s 2 105 950 831.12 397.844
s 2 106 907 792.351 408.117
s 2 116 405 331.508 456.012
s 2 118 402 328.617 500.451
s 2 119 400 326.365 502.275
s 2 121 476 406.683 478.349
s 2 127 585 511.319 485.324
s 2 128 617 538.861 453.39
s 2 130 582 508.155 452.366
s 2 131 643 561.073 503.265
s 2 132 703 613.588 462.253
s 2 134 766 669.159 502.063
s 2 136 772 674.493 495.248
s 2 137 781 682.123 471.232
s 2 138 784 685.118 468.173
s 2 139 781 682.191 474.374
s 2 141 885 773.615 494.269
s 2 145 888 776.431 492.348
s 2 147 904 790.471 502.254
s 2 148 902 788.507 504.272
s 2 149 938 820.465 502.391
s 2 152 963 842.721 503.376
s 2 156 242 148.008 545.764
s 2 158 248 154.208 545.888
s 2 162 314 224.505 546.204
s 2 163 315 225.905 530.765
s 2 164 315 225.261 552.166
s 2 166 368 291.55 564.057
s 2 171 571 498.553 561.333
s 2 172 572 499.545 563.335
s 2 173 638 556.502 563.852
s 2 175 636 554.844 552.416
s 2 176 641 559.993 556.383
s 2 178 692 604.702 533.137
s 2 180 722 630.968 528.786
s 2 181 716 624.803 518.298
s 2 182 764 666.755 536.361
s 2 185 769 671.642 537.317
s 2 187 810 708.021 538.302
s 2 189 841 735.447 558.082
s 2 190 881 770.988 555.105
s 2 192 860 754.238 555.186
s 2 193 938 820.657 574.383
s 2 194 948 829.586 514.034
s 2 195 915 800.701 574.39
s 2 196 904 790.859 538.891
s 2 197 976 854.831 525.328
s 2 204 321 230.814 584.287
s 2 205 322 231.43 590.824
s 2 210 507 413.694 594.058
s 2 216 586 512.591 601.786
s 2 217 585 511.221 598.227
s 2 218 657 574.472 587.268
s 2 219 675 589.664 581.33
s 2 220 656 573.502 582.378
s 2 221 651 568.578 581.394
s 2 223 715 624.955 635.236
s 2 224 828 724.063 600.264
s 2 225 813 710.869 596.413
s 2 227 882 771.755 624.292
s 2 231 977 855.606 577.181
s 2 233 252 132.188 646.925
s 2 234 253 133.288 649.949
s 2 236 264 144.633 647.915
s 2 237 335 226.123 646.194
s 2 240 448 356.089 665.294
s 2 241 465 362.168 676.569
s 2 242 481 383.624 673.992
s 2 244 478 380.89 673.024
s 2 245 562 458.491 663.422
s 2 246 565 461.494 669.205
s 2 247 548 446.265 666.974
s 2 249 607 530.299 640.373
s 2 250 613 512.347 667.362
s 2 252 687 591.589 664.392
s 2 253 698 601.698 663.19
s 2 254 700 603.327 667.232
s 2 255 656 563.305 657.902
s 2 256 694 598.036 667.268
s 2 257 714 618.145 662.355
s 2 258 708 618.512 645.366
s 2 259 726 634.945 643.204
s 2 260 713 616.887 667.415
s 2 261 730 638.995 646.386
s 2 264 776 678.805 643.356
s 2 265 769 672.092 642.262
s 2 267 895 781.553 683.16
s 2 268 895 781.553 683.16
s 2 269 891 777.506 679.182
s 2 271 888 774.288 683.233
s 2 272 898 784.251 690.38
s 2 273 956 836.692 655.37
s 2 274 947 828.873 673.098
s 2 275 359 196.329 730.773
s 2 276 359 196.329 730.773
s 2 277 444 291.606 761.777
s 2 278 463 357.487 706.022
s 2 282 617 476.167 725.153
s 2 285 664 515.317 729.1
s 2 286 661 513.039 724.11
s 2 288 659 511.731 743.188
s 2 289 744 604.533 735.338
s 2 290 740 601.769 711.221
s 2 291 742 601.937 737.397
s 2 292 740 600.916 732.141
s 2 295 775 641.709 727.255
s 2 301 313 146.314 779.858
s 2 302 291 111.606 782.816
s 2 304 376 193.757 790.556
s 2 305 373 191.601 783.684
s 2 306 385 200.302 789.599
s 2 307 406 218.92 809.926
s 2 310 455 295.709 773.864
s 2 311 472 310.994 772.922
s 2 312 544 356.24 799.68
s 2 316 758 564.263 809.259
s 2 321 801 633.92 774.402
s 2 323 446 219.679 857.011
s 2 328 668 430.1 872.24
s 2 329 750 498.621 887.234
s 2 330 765 511.029 890.334
s 2 331 745 494.602 886.002
s 2 333 862 641.477 846.36
s 2 335 872 644.41 859.881
s 2 339 404 147.16 898.548
s 2 345 589 323.535 907.722
i 3 0.471338 0.478516 9.65645 0.032991 0.0101101 0.0042569
i 3 0.471338 0.490479 9.62773 0.0324589 0.0111744 0.00319268
i 3 0.480908 0.492871 9.58706 0.0319268 0.0117065 0.00266056
i 3 0.502441 0.504834 9.56553 0.0313946 0.0117065 0.00212845
i 3 0.502441 0.502441 9.55596 0.0313946 0.0122386 0.00266056
i 3 0.46416 0.507227 9.53203 0.0313946 0.0122386 0.00319268
i 3 0.456982 0.509619 9.50332 0.0319268 0.0122386 0.00372479
i 3 0.45459 0.516797 9.49136 0.0313946 0.0127707 0.00372479
i 3 0.466553 0.526367 9.5105 0.0308625 0.0133028 0.00372479
i 3 0.452197 0.52876 9.52485 0.0303304 0.014367 0.00319268
i 3 0.425879 0.535937 9.50571 0.0292662 0.0154313 0.00266056
i 3 0.428271 0.533545 9.50571 0.0292662 0.0164955 0.00212845
i 3 0.425879 0.545508 9.51528 0.0287341 0.0170276 0.00159634
i 3 0.411523 0.5479 9.54399 0.028202 0.0170276 0.00159634
i 3 0.382812 0.559863 9.56074 0.0276699 0.0170276 0.00106423
i 3 0.375635 0.571826 9.54878 0.0276699 0.0159634 0.000532113
i 3 0.387598 0.581396 9.54878 0.0266056 0.0148992 0.000532113
i 3 0.401953 0.586182 9.55835 0.0250093 0.014367 0.000532113
i 3 0.382812 0.590967 9.56074 0.0239451 0.0138349 0.000532113
i 3 0.363672 0.593359 9.52725 0.0228808 0.0138349 0.000532113
i 3 0.366064 0.598145 9.50571 0.0223487 0.0138349 0.000532113
i 3 0.37085 0.617285 9.5105 0.0223487 0.0138349 0
i 3 0.356494 0.624463 9.53921 0.0228808 0.014367 0
i 3 0.327783 0.634033 9.55596 0.0223487 0.0148992 0
i 3 0.308643 0.629248 9.56074 0.0218166 0.0159634 0
i 3 0.320605 0.638818 9.57031 0.0212845 0.0164955 0
i 3 0.322998 0.641211 9.59185 0.0202203 0.0170276 0
s 3 1 885 771.886 190.394
s 3 8 814 710.222 241.36
s 3 10 815 711.117 244.276
s 3 11 883 770.656 198.02
s 3 13 874 761.801 193.21
s 3 15 882 769.349 194.094
s 3 16 952 831.218 238.215
s 3 17 940 820.031 242.02
s 3 19 948 827.354 235.194
s 3 20 948 827.391 241.213
s 3 28 760 663.312 287.142
s 3 33 813 709.44 279.254
s 3 34 819 714.246 276.35
s 3 36 815 711.636 286.046
s 3 37 893 779.028 309.293
s 3 39 898 783.513 307.182
s 3 40 908 793.523 318.373
s 3 42 938 819.064 256.113
s 3 43 989 863.966 281.804
s 3 45 967 843.818 310.173
s 3 46 975 852.424 259.949
s 3 51 231 143.198 321.116
s 3 52 229 140.636 323.223
s 3 53 598 513.854 337.163
s 3 58 815 711.008 377.495
s 3 59 798 696.404 357.311
s 3 61 798 696.404 357.311
s 3 63 893 779.621 352
s 3 67 900 786.305 357.088
s 3 68 898 784.227 353.135
s 3 70 917 801.17 347.212
s 3 71 986 861.382 325.185
s 3 72 985 860.395 327.196
s 3 79 564 492.659 419.338
s 3 80 560 488.554 421.427
s 3 81 575 501.51 445.356
s 3 83 603 526.725 413.46
s 3 84 586 511.864 447.453
s 3 85 650 567.813 402.398
s 3 86 700 611.102 433.159
s 3 87 703 613.898 434.235
s 3 88 652 569.851 405.336
s 3 90 706 616.823 433.228
s 3 91 747 652.473 432.3
s 3 92 754 658.648 441.454
s 3 93 708 618.268 429.213
s 3 94 743 648.861 434.243
s 3 95 823 718.34 385.05
s 3 96 796 695.581 425.304
s 3 97 800 699.235 396.68
s 3 98 773 675.006 390.852
s 3 101 884 772.387 414.266
s 3 102 879 768.099 387.4
s 3 103 888 775.703 409.171
s 3 106 907 793.163 407.121
s 3 109 224 130.501 479.026
s 3 110 232 140.654 474.298
s 3 112 353 276.953 460.195
s 3 113 326 247.913 479.275
s 3 114 323 244.999 481.419
s 3 116 405 331.557 456.078
s 3 118 402 328.474 500.448
s 3 121 476 406.808 478.331
s 3 124 560 488.874 453.428
s 3 125 559 487.866 451.429
s 3 127 585 511.21 485.366
s 3 130 582 508.117 452.34
s 3 131 642 560.191 503.303
s 3 132 703 613.512 462.274
s 3 134 766 669.178 501.123
s 3 137 780 681.115 471.213
s 3 138 785 686.111 468.15
s 3 140 766 669.178 501.123
s 3 147 904 790.478 501.294
s 3 149 938 820.46 502.412
s 3 152 963 842.7 502.39
s 3 156 242 148.04 545.744
s 3 158 248 154.13 544.882
s 3 162 314 224.573 545.257
s 3 163 315 225.942 530.779
s 3 164 315 225.25 552.16
s 3 165 320 230.24 575.172
s 3 166 368 292.361 563.739
s 3 167 411 337.518 559.362
s 3 169 456 385.345 575.417
s 3 170 576 503.558 560.339
s 3 171 572 499.542 562.338
s 3 172 572 499.542 562.338
s 3 173 638 556.345 563.543
s 3 175 636 554.93 552.498
s 3 178 692 604.626 533.07
s 3 179 763 665.69 529.344
s 3 181 716 624.895 518.341
s 3 182 764 666.738 535.365
s 3 185 769 671.706 536.364
s 3 186 769 671.706 536.364
s 3 189 841 735.519 558.219
s 3 190 881 771.578 554.18
s 3 191 839 733.85 555.19
s 3 193 938 820.706 573.366
s 3 194 948 829.543 513.025
s 3 196 904 790.951 538.836
s 3 197 976 854.393 525.427
s 3 198 236 140.322 603.995
s 3 199 235 139.053 600.975
s 3 203 290 208.701 584.181
s 3 204 321 230.851 584.311
s 3 205 322 231.422 590.784
s 3 207 425 352.676 598.285
s 3 210 507 413.915 594.08
s 3 216 586 512.672 602.104
s 3 217 585 511.513 598.4
s 3 218 657 574.43 587.301
s 3 219 675 589.734 581.331
s 3 220 656 573.443 582.387
s 3 221 650 567.724 580.446
s 3 223 715 624.944 635.149
s 3 224 828 723.983 600.259
s 3 225 813 710.87 596.451
s 3 231 977 855.78 576.339
s 3 233 252 132.151 646.953
s 3 235 270 151.285 641.899
s 3 236 263 143.885 646.853
s 3 237 335 226.072 646.176
s 3 240 448 356.109 665.276
s 3 241 465 362.114 676.566
s 3 242 481 383.735 674.003
s 3 243 472 384.478 660.065
s 3 244 478 381.18 672.03
s 3 246 564 460.41 668.205
s 3 247 548 446.541 666.983
s 3 249 607 530.254 640.36
s 3 251 615 515.283 663.243
s 3 252 687 591.632 664.369
s 3 253 698 601.651 663.156
s 3 254 700 603.327 667.192
s 3 256 693 597.206 667.286
s 3 257 714 618.171 662.386
s 3 258 708 618.448 645.308
s 3 259 726 634.919 642.193
s 3 260 713 616.921 667.437
s 3 261 730 638.986 646.352
s 3 264 776 678.909 643.341
s 3 268 893 779.545 683.224
s 3 271 888 774.345 683.25
s 3 272 898 784.392 690.3
s 3 273 956 836.616 654.36
s 3 274 947 828.918 673.067
s 3 275 359 196.307 730.769
s 3 276 356 193.389 729.874
s 3 277 444 291.736 761.796
s 3 278 463 357.497 706.028
s 3 279 610 473.389 731.03
s 3 281 602 465.988 723.373
s 3 285 665 516.633 728.092
s 3 286 661 513.323 723.09
s 3 288 659 511.572 743.182
s 3 290 738 599.252 709.299
s 3 292 740 600.731 731.179
s 3 301 313 146.474 779.852
s 3 304 375 193.685 790.593
s 3 305 373 191.613 783.699
s 3 306 385 200.249 789.607
s 3 307 406 218.946 809.916
s 3 310 455 295.762 773.851
s 3 311 472 311.008 772.914
s 3 312 544 356.234 798.671
s 3 316 757 563.508 808.199
s 3 323 445 218.732 856.915
s 3 328 668 427.886 871.306
s 3 329 750 498.489 887.267
s 3 330 765 510.975 890.36
s 3 331 744 494.093 885.015
s 3 333 863 642.396 846.332
s 3 334 857 636.646 843.358
s 3 343 514 229.103 934.981
i 4 0.294287 0.660352 9.6062 0.0191561 0.0175597 -0.000532113
i 4 0.265576 0.674707 9.57988 0.0180918 0.0175597 0
i 4 0.256006 0.698633 9.56074 0.0170276 0.0170276 0
i 4 0.265576 0.715381 9.56553 0.0164955 0.0164955 0.000532113
i 4 0.265576 0.727344 9.58467 0.0164955 0.0154313 0.000532113
i 4 0.239258 0.732129 9.59424 0.0164955 0.0148992 0.00106423
i 4 0.22251 0.732129 9.57749 0.0164955 0.0138349 0.00159634
i 4 0.23208 0.729736 9.57988 0.0164955 0.0133028 0.00159634
i 4 0.227295 0.727344 9.58945 0.0159634 0.0133028 0.00212845
i 4 0.203369 0.736914 9.60142 0.0148992 0.0127707 0.00159634
i 4 0.179443 0.734522 9.59902 0.0138349 0.0122386 0.00159634
i 4 0.172266 0.741699 9.59424 0.0133028 0.0117065 0.00159634
i 4 0.193799 0.741699 9.63013 0.0127707 0.0111744 0.00212845
i 4 0.186621 0.741699 9.66841 0.0122386 0.0106423 0.00266056
i 4 0.172266 0.741699 9.68994 0.0122386 0.0101101 0.00319268
i 4 0.160303 0.748877 9.67798 0.0122386 0.0101101 0.00372479
i 4 0.181836 0.756055 9.67798 0.0117065 0.00957803 0.0042569
i 4 0.200977 0.775195 9.69951 0.0117065 0.00904591 0.00478901
i 4 0.186621 0.77998 9.7043 0.0111744 0.00957803 0.00478901
i 4 0.162695 0.777588 9.68755 0.0106423 0.0101101 0.00478901
i 4 0.155518 0.768018 9.66123 0.0106423 0.0111744 0.00532113
i 4 0.16748 0.748877 9.67319 0.0111744 0.0122386 0.00638535
i 4 0.162695 0.739307 9.7043 0.0111744 0.0127707 0.00798169
i 4 0.153125 0.732129 9.70908 0.0117065 0.0122386 0.00904591
i 4 0.150732 0.727344 9.69233 0.0122386 0.0117065 0.00957803
i 4 0.179443 0.739307 9.66841 0.0133028 0.0111744 0.0101101
s 4 1 885 771.994 190.366
s 4 8 814 710.223 241.363
s 4 10 815 711.068 244.306
s 4 11 882 769.488 198.098
s 4 12 876 763.708 195.193
s 4 13 874 761.731 192.256
s 4 15 882 769.341 194.117
s 4 16 952 831.207 237.222
s 4 17 940 820.094 241.983
s 4 18 945 824.618 239.15
s 4 19 948 827.346 234.198
s 4 22 432 359.221 264.591
s 4 28 760 663.293 287.118
s 4 33 813 709.512 279.215
s 4 34 818 713.314 276.343
s 4 36 815 711.529 285.057
s 4 37 893 778.938 308.242
s 4 39 898 783.55 307.164
s 4 40 907 792.695 318.382
s 4 41 902 787.654 318.396
s 4 42 938 819.106 256.202
s 4 43 989 863.908 281.834
s 4 44 971 847.662 314.346
s 4 45 968 844.579 310.273
s 4 46 975 852.254 260.043
s 4 52 229 140.68 323.217
s 4 53 598 514.121 337.175
s 4 58 815 710.937 376.471
s 4 59 798 696.344 357.345
s 4 61 796 694.481 358.445
s 4 63 893 779.58 351.02
s 4 67 899 785.244 357.125
s 4 68 898 784.179 353.158
s 4 70 916 800.164 347.205
s 4 71 987 862.374 323.193
s 4 72 984 859.517 326.195
s 4 78 349 274.104 444.811
s 4 79 564 492.65 419.414
s 4 81 575 501.478 444.409
s 4 83 601 524.76 412.446
s 4 84 586 511.86 446.439
s 4 85 650 567.837 402.416
s 4 86 700 611.156 433.123
s 4 87 702 612.913 434.209
s 4 90 706 616.807 433.199
s 4 91 746 651.408 432.284
s 4 93 707 617.559 430.146
s 4 94 743 648.795 434.231
s 4 95 823 718.332 385.052
s 4 96 796 695.579 425.228
s 4 97 800 699.055 396.852
s 4 98 773 674.916 390.759
s 4 102 879 768.454 386.971
s 4 103 888 776.252 409.162
s 4 106 907 793.277 407.058
s 4 109 224 130.613 478.998
s 4 110 232 140.621 473.277
s 4 112 353 277.014 460.171
s 4 114 323 244.961 480.36
s 4 115 349 272.766 463.341
s 4 116 405 331.398 455.088
s 4 118 401 327.557 499.323
s 4 119 401 327.557 499.323
s 4 120 395 321.112 493.435
s 4 121 476 406.743 478.377
s 4 127 586 512.557 483.379
s 4 130 582 508.157 452.348
s 4 131 642 560.134 502.312
s 4 132 703 613.486 461.235
s 4 134 764 667.34 501.126
s 4 136 773 675.456 494.29
s 4 137 780 681.045 471.208
s 4 138 784 685.045 467.118
s 4 139 781 682.087 473.259
s 4 140 766 669.129 501.112
s 4 141 884 772.588 493.247
s 4 143 860 752.712 506.715
s 4 149 938 820.417 502.393
s 4 152 963 842.708 502.408
s 4 156 242 148.092 545.745
s 4 158 248 154.161 544.898
s 4 162 314 224.577 545.216
s 4 163 315 225.946 530.744
s 4 164 315 225.235 552.134
s 4 165 320 230.327 575.153
s 4 167 411 337.774 559.343
s 4 169 456 385.542 575.339
s 4 170 575 502.563 557.259
s 4 171 572 499.587 562.29
s 4 172 572 499.587 562.29
s 4 173 638 556.429 563.579
s 4 175 636 554.961 551.494
s 4 177 641 559.492 561.262
s 4 178 692 604.626 533.116
s 4 179 763 665.738 529.302
s 4 181 716 624.827 517.256
s 4 182 764 666.839 535.368
s 4 185 769 671.733 536.347
s 4 186 769 671.733 536.347
s 4 187 810 708.024 538.294
s 4 189 841 735.558 558.172
s 4 190 881 772.053 554.336
s 4 191 840 734.563 554.241
s 4 192 860 753.225 554.14
s 4 193 938 820.664 573.398
s 4 194 948 829.589 513.021
s 4 196 904 790.866 537.836
s 4 197 976 854.583 524.319
s 4 198 236 140.38 603.999
s 4 201 288 206.836 586.271
s 4 204 321 230.91 583.331
s 4 207 425 352.585 598.27
s 4 210 507 413.476 594.048
s 4 216 586 512.578 601.8
s 4 217 585 511.154 598.205
s 4 218 657 574.497 587.3
s 4 220 656 573.483 582.382
s 4 221 651 568.555 580.399
s 4 223 715 624.942 635.151
s 4 224 827 722.978 600.292
s 4 231 977 855.254 576.338
s 4 234 252 132.144 646.968
s 4 240 448 356.099 665.285
s 4 242 481 383.757 674.017
s 4 244 478 381.174 672.047
s 4 246 564 460.379 668.196
s 4 247 548 446.621 666.998
s 4 249 607 530.219 640.38
s 4 250 613 512.601 667.348
s 4 251 616 516.659 662.172
s 4 252 687 591.584 663.361
s 4 253 698 601.767 663.178
s 4 254 700 603.389 667.207
s 4 256 693 597.195 667.276
s 4 258 708 618.612 644.382
s 4 259 726 634.907 642.21
s 4 260 713 616.921 667.416
s 4 261 730 638.979 646.36
s 4 267 895 781.565 682.132
s 4 268 893 779.549 683.206
s 4 272 898 784.406 690.256
s 4 273 956 836.687 654.365
s 4 274 947 828.881 671.937
s 4 275 359 196.281 730.773
s 4 276 356 193.356 729.874
s 4 277 444 291.69 761.788
s 4 278 463 357.45 704.999
s 4 281 602 466.206 723.378
s 4 283 621 486.721 706.163
s 4 286 661 513.021 723.096
s 4 288 657 510.171 742.231
s 4 290 738 599.306 709.267
s 4 292 740 600.83 731.158
s 4 301 313 146.359 779.839
s 4 303 308 140.181 777.88
s 4 310 454 294.447 773.873
i 5 0.200977 0.746484 9.66602 0.0133028 0.0106423 0.00957803
i 5 0.196191 0.756055 9.66602 0.0133028 0.0101101 0.00957803
i 5 0.189014 0.765625 9.65166 0.0122386 0.00957803 0.00957803
i 5 0.193799 0.772803 9.64209 0.0106423 0.00957803 0.00957803
i 5 0.217725 0.777588 9.66123 0.00904591 0.00904591 0.00957803
i 5 0.24165 0.777588 9.69233 0.00744958 0.00904591 0.0106423
i 5 0.234473 0.77041 9.7019 0.00691746 0.00957803 0.0111744
i 5 0.248828 0.765625 9.68276 0.00638535 0.00957803 0.0122386
i 5 0.275146 0.75127 9.67319 0.00691746 0.00957803 0.0122386
i 5 0.291895 0.748877 9.67319 0.00744958 0.00904591 0.0117065
i 5 0.270361 0.744092 9.68276 0.00798169 0.0085138 0.0106423
i 5 0.246436 0.746484 9.66602 0.00904591 0.00798169 0.0101101
i 5 0.234473 0.748877 9.64448 0.00957803 0.00798169 0.00904591
i 5 0.248828 0.758447 9.64209 0.0101101 0.00798169 0.00904591
i 5 0.253613 0.765625 9.6397 0.0106423 0.00798169 0.00904591
i 5 0.246436 0.753662 9.61816 0.0106423 0.00904591 0.00904591
i 5 0.248828 0.744092 9.58706 0.0106423 0.00904591 0.00957803
i 5 0.282324 0.720166 9.57271 0.0106423 0.0085138 0.0101101
i 5 0.30625 0.710596 9.58945 0.0101101 0.00744958 0.0106423
i 5 0.30625 0.708203 9.60142 0.00904591 0.00638535 0.0111744
i 5 0.294287 0.710596 9.59663 0.00798169 0.00585324 0.0117065
i 5 0.287109 0.720166 9.57749 0.00744958 0.00585324 0.0111744
i 5 0.30625 0.724951 9.59185 0.00691746 0.00585324 0.0111744
i 5 0.299072 0.722559 9.61577 0.00691746 0.00585324 0.0106423
i 5 0.267969 0.712988 9.61338 0.00638535 0.00532113 0.0101101
i 5 0.260791 0.703418 9.57988 0.00585324 0.00478901 0.00957803
i 5 0.272754 0.693848 9.55356 0.00585324 0.0042569 0.00957803
s 5 1 885 771.967 190.374
s 5 8 814 710.258 241.348
s 5 11 882 769.5 198.11
s 5 12 876 763.725 195.196
s 5 13 874 761.761 192.265
s 5 14 876 763.725 195.196
s 5 15 882 769.333 194.13
s 5 16 952 831.211 238.222
s 5 17 941 820.874 241.088
s 5 19 948 827.331 234.199
s 5 28 760 663.304 287.152
s 5 33 813 709.417 279.246
s 5 34 818 713.31 276.339
s 5 36 815 711.553 285.069
s 5 37 893 778.921 308.279
s 5 39 898 783.503 307.195
s 5 40 908 793.456 318.342
s 5 41 902 787.598 318.335
s 5 42 938 819.203 256.15
s 5 43 989 863.912 281.803
s 5 44 972 848.245 313.371
s 5 45 968 844.392 308.24
s 5 46 975 852.111 259.768
s 5 52 229 140.643 323.25
s 5 58 815 711.009 376.479
s 5 59 797 695.427 357.337
s 5 61 797 695.427 357.337
s 5 63 893 779.641 350.98
s 5 67 899 785.25 357.122
s 5 70 916 800.144 347.212
s 5 71 986 861.363 325.258
s 5 72 986 861.363 325.258
s 5 78 349 272.718 444.977
s 5 79 564 492.645 419.352
s 5 81 575 501.478 444.458
s 5 83 601 524.669 412.369
s 5 84 586 511.845 446.414
s 5 85 650 567.806 402.399
s 5 86 701 611.648 433.295
s 5 87 701 611.648 433.295
s 5 90 706 616.746 433.269
s 5 91 747 652.381 432.302
s 5 93 708 618.322 429.229
s 5 94 744 649.616 434.254
s 5 95 823 718.328 385.098
s 5 96 796 695.506 425.257
s 5 97 800 699.015 396.835
s 5 98 773 674.898 390.815
s 5 103 888 776.053 409.196
s 5 106 907 793.174 407.16
s 5 109 224 130.598 479.009
s 5 112 353 277.059 460.178
s 5 113 326 247.919 479.3
s 5 114 323 245.04 481.416
s 5 118 402 328.417 500.439
s 5 119 400 326.248 502.231
s 5 121 476 406.735 478.368
s 5 127 585 511.229 485.269
s 5 130 582 508.086 452.315
s 5 131 643 561.152 502.328
s 5 132 703 613.454 461.25
s 5 134 766 669.174 501.069
s 5 136 773 675.449 494.288
s 5 137 781 682.128 471.206
s 5 138 784 685.119 467.122
s 5 139 781 682.162 473.269
s 5 140 766 669.174 501.069
s 5 145 888 776.3 491.33
s 5 149 938 820.54 502.447
s 5 152 963 842.697 502.391
s 5 156 242 148.049 545.748
s 5 158 248 154.189 544.861
s 5 162 314 224.58 545.228
s 5 163 315 225.943 530.706
s 5 164 315 225.254 552.161
s 5 165 320 230.248 573.933
s 5 167 411 337.603 559.374
s 5 169 456 385.477 575.333
s 5 170 575 502.564 557.297
s 5 171 572 499.585 562.324
s 5 172 572 499.585 562.324
s 5 173 638 556.357 563.566
s 5 177 641 559.473 561.27
s 5 178 693 605.572 532.124
s 5 179 763 665.768 529.292
s 5 181 716 624.878 517.287
s 5 182 764 666.826 535.34
s 5 185 769 671.754 536.33
s 5 186 769 671.754 536.33
s 5 187 810 707.981 538.318
s 5 189 841 735.367 557.132
s 5 190 881 771.284 554.167
s 5 191 840 734.433 554.139
s 5 192 860 752.832 554.234
s 5 193 938 820.661 573.411
s 5 194 948 829.548 513.027
s 5 196 904 790.811 537.922
s 5 197 976 854.459 524.408
s 5 198 236 140.438 604.965
s 5 199 235 139.069 600.972
s 5 201 288 206.856 586.255
s 5 203 290 208.734 584.196
s 5 204 321 230.86 584.302
s 5 205 322 231.412 590.747
s 5 207 425 352.658 598.284
s 5 208 430 357.846 600.368
s 5 209 430 357.846 600.368
s 5 210 507 413.862 594.141
s 5 216 586 512.524 601.846
s 5 217 585 511.371 598.24
s 5 220 656 573.388 582.4
s 5 221 650 567.67 580.444
s 5 223 715 625.005 635.16
s 5 224 828 724.003 600.283
s 5 231 977 856.036 576.324
s 5 233 252 132.27 646.898
s 5 235 270 151.147 641.895
s 5 237 335 226.3 646.265
s 5 240 448 356.101 665.311
s 5 241 465 362.419 675.526
s 5 242 482 384.778 674.024
s 5 244 478 381.143 672.035
s 5 246 564 460.402 668.186
s 5 247 548 446.693 666.997
s 5 250 613 512.473 667.358
s 5 251 615 515.389 663.256
s 5 252 687 591.542 663.355
s 5 253 698 601.734 663.164
s 5 254 700 603.352 667.193
s 5 257 714 618.128 662.357
s 5 258 708 618.561 644.403
s 5 259 726 634.882 642.196
s 5 260 713 616.859 667.397
s 5 261 730 638.896 646.366
s 5 264 776 678.964 643.331
s 5 268 893 779.594 683.216
s 5 271 888 774.41 682.262
s 5 273 956 836.775 654.374
s 5 274 947 828.877 671.995
s 5 275 359 196.336 730.802
s 5 276 356 193.409 729.896
s 5 277 444 291.551 761.797
s 5 278 464 358.548 705.089
s 5 279 610 473.571 731.066
s 5 281 602 466.135 723.367
s 5 282 617 475.86 725.172
s 5 285 665 516.486 728.097
s 5 286 661 513.146 723.099
s 5 289 743 603.491 734.339
s 5 290 738 599.323 709.259
s 5 292 740 600.943 731.142
s 5 301 313 146.284 779.851
s 5 304 375 193.624 790.587
s 5 306 385 200.303 789.594
s 5 307 406 218.914 809.947
s 5 310 455 295.69 773.843
s 5 311 472 310.886 772.897
s 5 312 544 356.329 798.683
s 5 329 750 498.867 886.161
s 5 330 765 511.01 889.347
s 5 331 745 495.029 884.943
s 5 333 862 641.525 845.353
s 5 334 858 637.569 843.345
s 5 345 589 322.892 907.696
s 5 349 765 431.497 988.976

3414
examples/Data/randomGrid3D.xml Normal file
View File

File diff suppressed because it is too large Load Diff

169
examples/Data/toy3D.xml Normal file
View File

@ -0,0 +1,169 @@
<?xml version="1.0" encoding="UTF-8" standalone="yes" ?>
<!DOCTYPE boost_serialization>
<boost_serialization signature="serialization::archive" version="17">
<graph class_id="0" tracking_level="0" version="0">
<Base class_id="1" tracking_level="0" version="0">
<factors_ class_id="2" tracking_level="0" version="0">
<count>2</count>
<item_version>1</item_version>
<item class_id="3" tracking_level="0" version="1">
<px class_id="4" class_name="JacobianFactor" tracking_level="1" version="0" object_id="_0">
<Base class_id="5" tracking_level="0" version="0">
<Base class_id="6" tracking_level="0" version="0">
<keys_>
<count>2</count>
<item_version>0</item_version>
<item>0</item>
<item>1</item>
</keys_>
</Base>
</Base>
<Ab_ class_id="8" tracking_level="0" version="0">
<matrix_ class_id="9" tracking_level="0" version="0">
<rows>9</rows>
<cols>7</cols>
<data>
<item>1.22427709071730959e+01</item>
<item>1.51514613104920048e+01</item>
<item>3.60934366857813060e+00</item>
<item>-1.18407259026383116e+01</item>
<item>7.84826117220921216e+00</item>
<item>1.23509165494819051e+01</item>
<item>-6.09875015991639735e+00</item>
<item>6.16547190708139126e-01</item>
<item>3.94972084922329048e+00</item>
<item>-4.89208482920378174e+00</item>
<item>3.02091647632478866e+00</item>
<item>-8.95328692238917512e+00</item>
<item>7.89831607220345955e+00</item>
<item>-2.36793602009719084e+00</item>
<item>1.48517612051941725e+01</item>
<item>-3.97284286249233731e-01</item>
<item>-1.95744531643153863e+01</item>
<item>-3.85954855417462017e+00</item>
<item>4.79268277145419042e+00</item>
<item>-9.01707953629520453e+00</item>
<item>1.37848069005841385e+01</item>
<item>1.04829326688375950e+01</item>
<item>-5.00630568442241675e+00</item>
<item>4.70463561852773182e+00</item>
<item>-1.59179134598689274e+01</item>
<item>-2.04767784956723942e+00</item>
<item>9.54135497908261954e+00</item>
<item>0.00000000000000000e+00</item>
<item>0.00000000000000000e+00</item>
<item>-0.00000000000000000e+00</item>
<item>-1.76201757312015372e+00</item>
<item>1.98634190821282672e+01</item>
<item>1.52966546661624236e+00</item>
<item>1.94817649567575373e+01</item>
<item>1.39684693294110307e+00</item>
<item>4.30228460420588288e+00</item>
<item>1.76201757312015372e+00</item>
<item>-1.98634190821282672e+01</item>
<item>-1.52966546661624236e+00</item>
<item>-0.00000000000000000e+00</item>
<item>0.00000000000000000e+00</item>
<item>0.00000000000000000e+00</item>
<item>4.16606867942304948e+00</item>
<item>1.86906420801308037e+00</item>
<item>-1.94717865319198360e+01</item>
<item>-1.94817649567575373e+01</item>
<item>-1.39684693294110307e+00</item>
<item>-4.30228460420588288e+00</item>
<item>-4.16606867942304948e+00</item>
<item>-1.86906420801308037e+00</item>
<item>1.94717865319198360e+01</item>
<item>0.00000000000000000e+00</item>
<item>0.00000000000000000e+00</item>
<item>0.00000000000000000e+00</item>
<item>1.00891462904330531e+01</item>
<item>-1.08132497987816869e+01</item>
<item>8.66487736568128497e+00</item>
<item>2.88370015604634311e+01</item>
<item>1.89391698948574643e+01</item>
<item>2.12398960190661290e+00</item>
<item>1.22150946112124039e+01</item>
<item>-2.33658532501596561e+01</item>
<item>1.51576204760307363e+01</item>
</data>
</matrix_>
<variableColOffsets_>
<count>4</count>
<item_version>0</item_version>
<item>0</item>
<item>3</item>
<item>6</item>
<item>7</item>
</variableColOffsets_>
<rowStart_>0</rowStart_>
<rowEnd_>9</rowEnd_>
<blockStart_>0</blockStart_>
</Ab_>
<model_ class_id="11" tracking_level="0" version="1">
<px class_id="-1"></px>
</model_>
</px>
</item>
<item>
<px class_id_reference="4" object_id="_1">
<Base>
<Base>
<keys_>
<count>2</count>
<item_version>0</item_version>
<item>0</item>
<item>1</item>
</keys_>
</Base>
</Base>
<Ab_>
<matrix_>
<rows>3</rows>
<cols>7</cols>
<data>
<item>1.00000000000000000e+00</item>
<item>0.00000000000000000e+00</item>
<item>0.00000000000000000e+00</item>
<item>0.00000000000000000e+00</item>
<item>1.00000000000000000e+00</item>
<item>0.00000000000000000e+00</item>
<item>0.00000000000000000e+00</item>
<item>0.00000000000000000e+00</item>
<item>1.00000000000000000e+00</item>
<item>1.00000000000000000e+00</item>
<item>0.00000000000000000e+00</item>
<item>0.00000000000000000e+00</item>
<item>0.00000000000000000e+00</item>
<item>1.00000000000000000e+00</item>
<item>0.00000000000000000e+00</item>
<item>0.00000000000000000e+00</item>
<item>0.00000000000000000e+00</item>
<item>1.00000000000000000e+00</item>
<item>0.00000000000000000e+00</item>
<item>0.00000000000000000e+00</item>
<item>0.00000000000000000e+00</item>
</data>
</matrix_>
<variableColOffsets_>
<count>4</count>
<item_version>0</item_version>
<item>0</item>
<item>3</item>
<item>6</item>
<item>7</item>
</variableColOffsets_>
<rowStart_>0</rowStart_>
<rowEnd_>3</rowEnd_>
<blockStart_>0</blockStart_>
</Ab_>
<model_>
<px class_id="-1"></px>
</model_>
</px>
</item>
</factors_>
</Base>
</graph>
</boost_serialization>

235
examples/ISAM2_SmartFactorStereo_IMU.cpp Normal file
View File

@ -0,0 +1,235 @@
/**
* @file ISAM2_SmartFactorStereo_IMU.cpp
* @brief test of iSAM2 with smart stereo factors and IMU preintegration,
* originally used to debug valgrind invalid reads with Eigen
* @author Nghia Ho
*
* Setup is a stationary stereo camera with an IMU attached.
* The data file is at examples/Data/ISAM2_SmartFactorStereo_IMU.txt
* It contains 5 frames of stereo matches and IMU data.
*/
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h>
#include <fstream>
#include <iostream>
#include <sstream>
#include <string>
#include <vector>
using namespace std;
using namespace gtsam;
using symbol_shorthand::X;
using symbol_shorthand::V;
using symbol_shorthand::B;
struct IMUHelper {
IMUHelper() {
{
auto gaussian = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(5.0e-2), Vector3::Constant(5.0e-3))
.finished());
auto huber = noiseModel::Robust::Create(
noiseModel::mEstimator::Huber::Create(1.345), gaussian);
biasNoiseModel = huber;
}
{
auto gaussian = noiseModel::Isotropic::Sigma(3, 0.01);
auto huber = noiseModel::Robust::Create(
noiseModel::mEstimator::Huber::Create(1.345), gaussian);
velocityNoiseModel = huber;
}
// expect IMU to be rotated in image space co-ords
auto p = boost::make_shared<PreintegratedCombinedMeasurements::Params>(
Vector3(0.0, 9.8, 0.0));
p->accelerometerCovariance =
I_3x3 * pow(0.0565, 2.0); // acc white noise in continuous
p->integrationCovariance =
I_3x3 * 1e-9; // integration uncertainty continuous
p->gyroscopeCovariance =
I_3x3 * pow(4.0e-5, 2.0); // gyro white noise in continuous
p->biasAccCovariance = I_3x3 * pow(0.00002, 2.0); // acc bias in continuous
p->biasOmegaCovariance =
I_3x3 * pow(0.001, 2.0); // gyro bias in continuous
p->biasAccOmegaInt = Matrix::Identity(6, 6) * 1e-5;
// body to IMU rotation
Rot3 iRb(0.036129, -0.998727, 0.035207,
0.045417, -0.033553, -0.998404,
0.998315, 0.037670, 0.044147);
// body to IMU translation (meters)
Point3 iTb(0.03, -0.025, -0.06);
// body in this example is the left camera
p->body_P_sensor = Pose3(iRb, iTb);
Rot3 prior_rotation = Rot3(I_3x3);
Pose3 prior_pose(prior_rotation, Point3(0, 0, 0));
Vector3 acc_bias(0.0, -0.0942015, 0.0); // in camera frame
Vector3 gyro_bias(-0.00527483, -0.00757152, -0.00469968);
priorImuBias = imuBias::ConstantBias(acc_bias, gyro_bias);
prevState = NavState(prior_pose, Vector3(0, 0, 0));
propState = prevState;
prevBias = priorImuBias;
preintegrated = new PreintegratedCombinedMeasurements(p, priorImuBias);
}
imuBias::ConstantBias priorImuBias; // assume zero initial bias
noiseModel::Robust::shared_ptr velocityNoiseModel;
noiseModel::Robust::shared_ptr biasNoiseModel;
NavState prevState;
NavState propState;
imuBias::ConstantBias prevBias;
PreintegratedCombinedMeasurements* preintegrated;
};
int main(int argc, char* argv[]) {
if (argc != 2) {
cout << "./ISAM2_SmartFactorStereo_IMU [data.txt]\n";
return 0;
}
ifstream in(argv[1]);
if (!in) {
cerr << "error opening: " << argv[1] << "\n";
return 1;
}
// Camera parameters
double fx = 822.37;
double fy = 822.37;
double cx = 538.73;
double cy = 579.10;
double baseline = 0.372; // meters
Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx, fy, 0.0, cx, cy, baseline));
ISAM2Params parameters;
parameters.relinearizeThreshold = 0.1;
ISAM2 isam(parameters);
// Create a factor graph
std::map<size_t, SmartStereoProjectionPoseFactor::shared_ptr> smartFactors;
NonlinearFactorGraph graph;
Values initialEstimate;
IMUHelper imu;
// Pose prior - at identity
auto priorPoseNoise = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.1)).finished());
graph.emplace_shared<PriorFactor<Pose3>>(X(1), Pose3::identity(),
priorPoseNoise);
initialEstimate.insert(X(0), Pose3::identity());
// Bias prior
graph.add(PriorFactor<imuBias::ConstantBias>(B(1), imu.priorImuBias,
imu.biasNoiseModel));
initialEstimate.insert(B(0), imu.priorImuBias);
// Velocity prior - assume stationary
graph.add(
PriorFactor<Vector3>(V(1), Vector3(0, 0, 0), imu.velocityNoiseModel));
initialEstimate.insert(V(0), Vector3(0, 0, 0));
int lastFrame = 1;
int frame;
while (true) {
char line[1024];
in.getline(line, sizeof(line));
stringstream ss(line);
char type;
ss >> type;
ss >> frame;
if (frame != lastFrame || in.eof()) {
cout << "Running iSAM for frame: " << lastFrame << "\n";
initialEstimate.insert(X(lastFrame), Pose3::identity());
initialEstimate.insert(V(lastFrame), Vector3(0, 0, 0));
initialEstimate.insert(B(lastFrame), imu.prevBias);
CombinedImuFactor imuFactor(X(lastFrame - 1), V(lastFrame - 1),
X(lastFrame), V(lastFrame), B(lastFrame - 1),
B(lastFrame), *imu.preintegrated);
graph.add(imuFactor);
isam.update(graph, initialEstimate);
Values currentEstimate = isam.calculateEstimate();
imu.propState = imu.preintegrated->predict(imu.prevState, imu.prevBias);
imu.prevState = NavState(currentEstimate.at<Pose3>(X(lastFrame)),
currentEstimate.at<Vector3>(V(lastFrame)));
imu.prevBias = currentEstimate.at<imuBias::ConstantBias>(B(lastFrame));
imu.preintegrated->resetIntegrationAndSetBias(imu.prevBias);
graph.resize(0);
initialEstimate.clear();
if (in.eof()) {
break;
}
}
if (type == 'i') { // Process IMU measurement
double ax, ay, az;
double gx, gy, gz;
double dt = 1 / 800.0; // IMU at ~800Hz
ss >> ax;
ss >> ay;
ss >> az;
ss >> gx;
ss >> gy;
ss >> gz;
Vector3 acc(ax, ay, az);
Vector3 gyr(gx, gy, gz);
imu.preintegrated->integrateMeasurement(acc, gyr, dt);
} else if (type == 's') { // Process stereo measurement
int landmark;
double xl, xr, y;
ss >> landmark;
ss >> xl;
ss >> xr;
ss >> y;
if (smartFactors.count(landmark) == 0) {
auto gaussian = noiseModel::Isotropic::Sigma(3, 1.0);
SmartProjectionParams params(HESSIAN, ZERO_ON_DEGENERACY);
smartFactors[landmark] = SmartStereoProjectionPoseFactor::shared_ptr(
new SmartStereoProjectionPoseFactor(gaussian, params));
graph.push_back(smartFactors[landmark]);
}
smartFactors[landmark]->add(StereoPoint2(xl, xr, y), X(frame), K);
} else {
throw runtime_error("unexpected data type: " + string(1, type));
}
lastFrame = frame;
}
return 0;
}

6
examples/ImuFactorExample2.cpp
View File

@ -108,9 +108,9 @@ int main(int argc, char* argv[]) {
initialEstimate.insert(biasKey, imuBias::ConstantBias());
}
// Predict acceleration and gyro measurements in (actual) body frame
auto measuredAcc = scenario.acceleration_b(t) -
scenario.rotation(t).transpose() * params->n_gravity;
auto measuredOmega = scenario.omega_b(t);
Vector3 measuredAcc = scenario.acceleration_b(t) -
scenario.rotation(t).transpose() * params->n_gravity;
Vector3 measuredOmega = scenario.omega_b(t);
accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t);
// Add Imu Factor

92
examples/InverseKinematicsExampleExpressions.cpp Normal file
View File

@ -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 <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/ExpressionFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/expressions.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/expressions.h>
#include <cmath>
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;
}

84
examples/Pose2SLAMStressTest.cpp Normal file
View File

@ -0,0 +1,84 @@
/**
* @file Pose2SLAMStressTest.cpp
* @brief Test GTSAM on large open-loop chains
* @date May 23, 2018
* @author Wenqiang Zhou
*/
// Create N 3D poses, add relative motion between each consecutive poses. (The
// relative motion is simply a unit translation(1, 0, 0), no rotation). For each
// each pose, add some random noise to the x value of the translation part.
// Use gtsam to create a prior factor for the first pose and N-1 between factors
// and run optimization.
#include <gtsam/geometry/Cal3_S2Stereo.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/StereoFactor.h>
#include <random>
using namespace std;
using namespace gtsam;
void testGtsam(int numberNodes) {
std::random_device rd;
std::mt19937 e2(rd());
std::uniform_real_distribution<> dist(0, 1);
vector<Pose3> poses;
for (int i = 0; i < numberNodes; ++i) {
Matrix4 M;
double r = dist(e2);
r = (r - 0.5) / 10 + i;
M << 1, 0, 0, r, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1;
poses.push_back(Pose3(M));
}
// prior factor for the first pose
auto priorModel = noiseModel::Isotropic::Variance(6, 1e-4);
Matrix4 first_M;
first_M << 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1;
Pose3 first = Pose3(first_M);
NonlinearFactorGraph graph;
graph.add(PriorFactor<Pose3>(0, first, priorModel));
// vo noise model
auto VOCovarianceModel = noiseModel::Isotropic::Variance(6, 1e-3);
// relative VO motion
Matrix4 vo_M;
vo_M << 1, 0, 0, 1, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1;
Pose3 relativeMotion(vo_M);
for (int i = 0; i < numberNodes - 1; ++i) {
graph.add(
BetweenFactor<Pose3>(i, i + 1, relativeMotion, VOCovarianceModel));
}
// inital values
Values initial;
for (int i = 0; i < numberNodes; ++i) {
initial.insert(i, poses[i]);
}
LevenbergMarquardtParams params;
params.setVerbosity("ERROR");
params.setOrderingType("METIS");
params.setLinearSolverType("MULTIFRONTAL_CHOLESKY");
LevenbergMarquardtOptimizer optimizer(graph, initial, params);
auto result = optimizer.optimize();
}
int main(int args, char* argv[]) {
int numberNodes = stoi(argv[1]);
cout << "number of_nodes: " << numberNodes << endl;
testGtsam(numberNodes);
return 0;
}

102
examples/Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp Normal file
View File

@ -0,0 +1,102 @@
/**
* @file Pose3SLAMExampleExpressions_BearingRangeWithTransform.cpp
* @brief A simultaneous optimization of trajectory, landmarks and sensor-pose with respect to body-pose using bearing-range measurements done with Expressions
* @author Thomas Horstink
* @date January 4th, 2019
*/
#include <gtsam/inference/Symbol.h>
#include <gtsam/geometry/BearingRange.h>
#include <gtsam/slam/expressions.h>
#include <gtsam/nonlinear/ExpressionFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Values.h>
#include <examples/SFMdata.h>
using namespace gtsam;
typedef BearingRange<Pose3, Point3> BearingRange3D;
/* ************************************************************************* */
int main(int argc, char* argv[]) {
// Move around so the whole state (including the sensor tf) is observable
Pose3 init_pose = Pose3();
Pose3 delta_pose1 = Pose3(Rot3().Yaw(2*M_PI/8).Pitch(M_PI/8), Point3(1, 0, 0));
Pose3 delta_pose2 = Pose3(Rot3().Pitch(-M_PI/8), Point3(1, 0, 0));
Pose3 delta_pose3 = Pose3(Rot3().Yaw(-2*M_PI/8), Point3(1, 0, 0));
int steps = 4;
auto poses = createPoses(init_pose, delta_pose1, steps);
auto poses2 = createPoses(init_pose, delta_pose2, steps);
auto poses3 = createPoses(init_pose, delta_pose3, steps);
// Concatenate poses to create trajectory
poses.insert( poses.end(), poses2.begin(), poses2.end() );
poses.insert( poses.end(), poses3.begin(), poses3.end() ); // std::vector of Pose3
auto points = createPoints(); // std::vector of Point3
// (ground-truth) sensor pose in body frame, further an unknown variable
Pose3 body_T_sensor_gt(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
// The graph
ExpressionFactorGraph graph;
// Specify uncertainty on first pose prior and also for between factor (simplicity reasons)
auto poseNoise = noiseModel::Diagonal::Sigmas((Vector(6)<<0.3,0.3,0.3,0.1,0.1,0.1).finished());
// Uncertainty bearing range measurement;
auto bearingRangeNoise = noiseModel::Diagonal::Sigmas((Vector(3)<<0.01,0.03,0.05).finished());
// Expressions for body-frame at key 0 and sensor-tf
Pose3_ x_('x', 0);
Pose3_ body_T_sensor_('T', 0);
// Add a prior on the body-pose
graph.addExpressionFactor(x_, poses[0], poseNoise);
// Simulated measurements from pose
for (size_t i = 0; i < poses.size(); ++i) {
auto world_T_sensor = poses[i].compose(body_T_sensor_gt);
for (size_t j = 0; j < points.size(); ++j) {
// This expression is the key feature of this example: it creates a differentiable expression of the measurement after being displaced by sensor transform.
auto prediction_ = Expression<BearingRange3D>( BearingRange3D::Measure, Pose3_('x',i)*body_T_sensor_, Point3_('l',j));
// Create a *perfect* measurement
auto measurement = BearingRange3D(world_T_sensor.bearing(points[j]), world_T_sensor.range(points[j]));
// Add factor
graph.addExpressionFactor(prediction_, measurement, bearingRangeNoise);
}
// and add a between factor to the graph
if (i > 0)
{
// And also we have a *perfect* measurement for the between factor.
graph.addExpressionFactor(between(Pose3_('x', i-1),Pose3_('x', i)), poses[i-1].between(poses[i]), poseNoise);
}
}
// Create perturbed initial
Values initial;
Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
for (size_t i = 0; i < poses.size(); ++i)
initial.insert(Symbol('x', i), poses[i].compose(delta));
for (size_t j = 0; j < points.size(); ++j)
initial.insert<Point3>(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15));
// Initialize body_T_sensor wrongly (because we do not know!)
initial.insert<Pose3>(Symbol('T',0), Pose3());
std::cout << "initial error: " << graph.error(initial) << std::endl;
Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();
std::cout << "final error: " << graph.error(result) << std::endl;
initial.at<Pose3>(Symbol('T',0)).print("\nInitial estimate body_T_sensor\n"); /* initial sensor_P_body estimate */
result.at<Pose3>(Symbol('T',0)).print("\nFinal estimate body_T_sensor\n"); /* optimized sensor_P_body estimate */
body_T_sensor_gt.print("\nGround truth body_T_sensor\n"); /* sensor_P_body ground truth */
return 0;
}
/* ************************************************************************* */

28
examples/SFMdata.h
View File

@ -16,9 +16,10 @@
*/
/**
* A structure-from-motion example with landmarks
* A structure-from-motion example with landmarks, default function arguments give
* - The landmarks form a 10 meter cube
* - The robot rotates around the landmarks, always facing towards the cube
* Passing function argument allows to specificy an initial position, a pose increment and step count.
*/
// As this is a full 3D problem, we will use Pose3 variables to represent the camera
@ -49,20 +50,19 @@ std::vector<gtsam::Point3> createPoints() {
}
/* ************************************************************************* */
std::vector<gtsam::Pose3> createPoses() {
std::vector<gtsam::Pose3> createPoses(
const gtsam::Pose3& init = gtsam::Pose3(gtsam::Rot3::Ypr(M_PI/2,0,-M_PI/2), gtsam::Point3(30, 0, 0)),
const gtsam::Pose3& delta = gtsam::Pose3(gtsam::Rot3::Ypr(0,-M_PI/4,0), gtsam::Point3(sin(M_PI/4)*30, 0, 30*(1-sin(M_PI/4)))),
int steps = 8) {
// Create the set of ground-truth poses
// Default values give a circular trajectory, radius 30 at pi/4 intervals, always facing the circle center
std::vector<gtsam::Pose3> poses;
double radius = 30.0;
int i = 0;
double theta = 0.0;
gtsam::Point3 up(0,0,1);
gtsam::Point3 target(0,0,0);
for(; i < 8; ++i, theta += 2*M_PI/8) {
gtsam::Point3 position = gtsam::Point3(radius*cos(theta), radius*sin(theta), 0.0);
gtsam::SimpleCamera camera = gtsam::SimpleCamera::Lookat(position, target, up);
poses.push_back(camera.pose());
int i = 1;
poses.push_back(init);
for(; i < steps; ++i) {
poses.push_back(poses[i-1].compose(delta));
}
return poses;
}
/* ************************************************************************* */
}

48
examples/SolverComparer.cpp
View File

@ -251,10 +251,10 @@ void runIncremental()
Key firstPose;
while(nextMeasurement < datasetMeasurements.size())
{
if(BetweenFactor<Pose>::shared_ptr measurement =
if(BetweenFactor<Pose>::shared_ptr factor =
boost::dynamic_pointer_cast<BetweenFactor<Pose> >(datasetMeasurements[nextMeasurement]))
{
Key key1 = measurement->key1(), key2 = measurement->key2();
Key key1 = factor->key1(), key2 = factor->key2();
if(((int)key1 >= firstStep && key1 < key2) || ((int)key2 >= firstStep && key2 < key1)) {
// We found an odometry starting at firstStep
firstPose = std::min(key1, key2);
@ -302,52 +302,53 @@ void runIncremental()
NonlinearFactor::shared_ptr measurementf = datasetMeasurements[nextMeasurement];
if(BetweenFactor<Pose>::shared_ptr measurement =
if(BetweenFactor<Pose>::shared_ptr factor =
boost::dynamic_pointer_cast<BetweenFactor<Pose> >(measurementf))
{
// Stop collecting measurements that are for future steps
if(measurement->key1() > step || measurement->key2() > step)
if(factor->key1() > step || factor->key2() > step)
break;
// Require that one of the nodes is the current one
if(measurement->key1() != step && measurement->key2() != step)
if(factor->key1() != step && factor->key2() != step)
throw runtime_error("Problem in data file, out-of-sequence measurements");
// Add a new factor
newFactors.push_back(measurement);
newFactors.push_back(factor);
const auto& measured = factor->measured();
// Initialize the new variable
if(measurement->key1() > measurement->key2()) {
if(!newVariables.exists(measurement->key1())) { // Only need to check newVariables since loop closures come after odometry
if(factor->key1() > factor->key2()) {
if(!newVariables.exists(factor->key1())) { // Only need to check newVariables since loop closures come after odometry
if(step == 1)
newVariables.insert(measurement->key1(), measurement->measured().inverse());
newVariables.insert(factor->key1(), measured.inverse());
else {
Pose prevPose = isam2.calculateEstimate<Pose>(measurement->key2());
newVariables.insert(measurement->key1(), prevPose * measurement->measured().inverse());
Pose prevPose = isam2.calculateEstimate<Pose>(factor->key2());
newVariables.insert(factor->key1(), prevPose * measured.inverse());
}
}
} else {
if(!newVariables.exists(measurement->key2())) { // Only need to check newVariables since loop closures come after odometry
if(!newVariables.exists(factor->key2())) { // Only need to check newVariables since loop closures come after odometry
if(step == 1)
newVariables.insert(measurement->key2(), measurement->measured());
newVariables.insert(factor->key2(), measured);
else {
Pose prevPose = isam2.calculateEstimate<Pose>(measurement->key1());
newVariables.insert(measurement->key2(), prevPose * measurement->measured());
Pose prevPose = isam2.calculateEstimate<Pose>(factor->key1());
newVariables.insert(factor->key2(), prevPose * measured);
}
}
}
}
else if(BearingRangeFactor<Pose, Point2>::shared_ptr measurement =
else if(BearingRangeFactor<Pose, Point2>::shared_ptr factor =
boost::dynamic_pointer_cast<BearingRangeFactor<Pose, Point2> >(measurementf))
{
Key poseKey = measurement->keys()[0], lmKey = measurement->keys()[1];
Key poseKey = factor->keys()[0], lmKey = factor->keys()[1];
// Stop collecting measurements that are for future steps
if(poseKey > step)
throw runtime_error("Problem in data file, out-of-sequence measurements");
// Add new factor
newFactors.push_back(measurement);
newFactors.push_back(factor);
// Initialize new landmark
if(!isam2.getLinearizationPoint().exists(lmKey))
@ -357,8 +358,9 @@ void runIncremental()
pose = isam2.calculateEstimate<Pose>(poseKey);
else
pose = newVariables.at<Pose>(poseKey);
Rot2 measuredBearing = measurement->measured().first;
double measuredRange = measurement->measured().second;
const auto& measured = factor->measured();
Rot2 measuredBearing = measured.bearing();
double measuredRange = measured.range();
newVariables.insert(lmKey,
pose.transform_from(measuredBearing.rotate(Point2(measuredRange, 0.0))));
}
@ -427,7 +429,7 @@ void runIncremental()
// for (Key key12: boost::adaptors::reverse(values.keys())) {
// if(i != j) {
// gttic_(jointMarginalInformation);
// std::vector<Key> keys(2);
// KeyVector keys(2);
// keys[0] = key1;
// keys[1] = key2;
// JointMarginal info = marginals.jointMarginalInformation(keys);
@ -522,7 +524,7 @@ void runCompare()
// Check solution for equality
cout << "Comparing solutions..." << endl;
vector<Key> missingKeys;
KeyVector missingKeys;
br::set_symmetric_difference(soln1.keys(), soln2.keys(), std::back_inserter(missingKeys));
if(!missingKeys.empty()) {
cout << " Keys unique to one solution file: ";
@ -533,7 +535,7 @@ void runCompare()
}
cout << endl;
}
vector<Key> commonKeys;
KeyVector commonKeys;
br::set_intersection(soln1.keys(), soln2.keys(), std::back_inserter(commonKeys));
double maxDiff = 0.0;
for(Key j: commonKeys)

196
gtsam.h
View File

@ -209,11 +209,60 @@ class KeyGroupMap {
bool insert2(size_t key, int val);
};
// Actually a FastSet<FactorIndex>
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<FactorIndex>
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 <gtsam/base/DSFMap.h>
class IndexPair {
IndexPair();
IndexPair(size_t i, size_t j);
size_t i() const;
size_t j() const;
};
template<KEY = {gtsam::IndexPair}>
class DSFMap {
DSFMap();
KEY find(const KEY& key) const;
void merge(const KEY& x, const KEY& y);
};
#include <gtsam/base/Matrix.h>
bool linear_independent(Matrix A, Matrix B, double tol);
@ -228,6 +277,12 @@ virtual class Value {
size_t dim() const;
};
#include <gtsam/base/GenericValue.h>
template<T = {Vector, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::StereoPoint2, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
virtual class GenericValue : gtsam::Value {
void serializable() const;
};
#include <gtsam/base/deprecated/LieScalar.h>
class LieScalar {
// Standard constructors
@ -567,8 +622,13 @@ class Pose2 {
// Lie Group
static gtsam::Pose2 Expmap(Vector v);
static Vector Logmap(const gtsam::Pose2& p);
static Matrix ExpmapDerivative(Vector v);
static Matrix LogmapDerivative(const gtsam::Pose2& v);
Matrix AdjointMap() const;
Vector Adjoint(Vector xi) const;
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
@ -617,6 +677,11 @@ 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 Vector adjointTranspose(Vector xi, Vector y);
static Matrix ExpmapDerivative(Vector xi);
static Matrix LogmapDerivative(const gtsam::Pose3& xi);
static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz);
// Group Action on Point3
@ -1202,14 +1267,30 @@ class VariableIndex {
namespace noiseModel {
#include <gtsam/linear/NoiseModel.h>
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;
@ -1220,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;
@ -1247,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;
@ -1255,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;
@ -1263,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
@ -1276,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
@ -1285,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
@ -1294,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
@ -1306,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;
@ -1697,7 +1779,7 @@ virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters {
virtual class SubgraphSolver {
SubgraphSolver(const gtsam::GaussianFactorGraph &A, const gtsam::SubgraphSolverParameters &parameters, const gtsam::Ordering& ordering);
SubgraphSolver(const gtsam::GaussianFactorGraph &Ab1, const gtsam::GaussianFactorGraph &Ab2, const gtsam::SubgraphSolverParameters &parameters, const gtsam::Ordering& ordering);
SubgraphSolver(const gtsam::GaussianFactorGraph &Ab1, const gtsam::GaussianFactorGraph* Ab2, const gtsam::SubgraphSolverParameters &parameters, const gtsam::Ordering& ordering);
gtsam::VectorValues optimize() const;
};
@ -1986,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;
@ -2010,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 <gtsam/nonlinear/DoglegOptimizer.h>
@ -2157,8 +2258,6 @@ class ISAM2Result {
size_t getCliques() const;
};
class FactorIndices {};
class ISAM2 {
ISAM2();
ISAM2(const gtsam::ISAM2Params& params);
@ -2259,10 +2358,13 @@ virtual class NonlinearEquality : gtsam::NoiseModelFactor {
template<POSE, POINT>
virtual class RangeFactor : gtsam::NoiseModelFactor {
RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel);
// enabling serialization functionality
void serialize() const;
};
typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Point2> RangeFactorPosePoint2;
typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Point3> RangeFactorPosePoint3;
typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Point2> RangeFactor2D;
typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Point3> RangeFactor3D;
typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Pose2> RangeFactorPose2;
typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Pose3> RangeFactorPose3;
typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::Point3> RangeFactorCalibratedCameraPoint;
@ -2275,10 +2377,13 @@ typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::SimpleCamera> RangeFactor
template<POSE, POINT>
virtual class RangeFactorWithTransform : gtsam::NoiseModelFactor {
RangeFactorWithTransform(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel, const POSE& body_T_sensor);
// enabling serialization functionality
void serialize() const;
};
typedef gtsam::RangeFactorWithTransform<gtsam::Pose2, gtsam::Point2> RangeFactorWithTransformPosePoint2;
typedef gtsam::RangeFactorWithTransform<gtsam::Pose3, gtsam::Point3> RangeFactorWithTransformPosePoint3;
typedef gtsam::RangeFactorWithTransform<gtsam::Pose2, gtsam::Point2> RangeFactorWithTransform2D;
typedef gtsam::RangeFactorWithTransform<gtsam::Pose3, gtsam::Point3> RangeFactorWithTransform3D;
typedef gtsam::RangeFactorWithTransform<gtsam::Pose2, gtsam::Pose2> RangeFactorWithTransformPose2;
typedef gtsam::RangeFactorWithTransform<gtsam::Pose3, gtsam::Pose3> RangeFactorWithTransformPose3;
@ -2292,6 +2397,22 @@ virtual class BearingFactor : gtsam::NoiseModelFactor {
};
typedef gtsam::BearingFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2> BearingFactor2D;
typedef gtsam::BearingFactor<gtsam::Pose2, gtsam::Pose2, gtsam::Rot2> BearingFactorPose2;
#include <gtsam/geometry/BearingRange.h>
template <POSE, POINT, BEARING, RANGE>
class BearingRange {
BearingRange(const BEARING& b, const RANGE& r);
BEARING bearing() const;
RANGE range() const;
// TODO(frank): can't class instance itself?
// static gtsam::BearingRange Measure(const POSE& pose, const POINT& point);
static BEARING MeasureBearing(const POSE& pose, const POINT& point);
static RANGE MeasureRange(const POSE& pose, const POINT& point);
void print(string s) const;
};
typedef gtsam::BearingRange<gtsam::Pose2, gtsam::Point2, gtsam::Rot2, double> BearingRange2D;
#include <gtsam/sam/BearingRangeFactor.h>
template<POSE, POINT, BEARING, RANGE>
@ -2305,6 +2426,7 @@ virtual class BearingRangeFactor : gtsam::NoiseModelFactor {
};
typedef gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2, double> BearingRangeFactor2D;
typedef gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Pose2, gtsam::Rot2, double> BearingRangeFactorPose2;
#include <gtsam/slam/ProjectionFactor.h>
@ -2425,6 +2547,7 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor {
};
#include <gtsam/slam/dataset.h>
string findExampleDataFile(string name);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise, bool smart);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
@ -2440,7 +2563,38 @@ void save2D(const gtsam::NonlinearFactorGraph& graph,
const gtsam::Values& config, gtsam::noiseModel::Diagonal* model,
string filename);
// std::vector<gtsam::BetweenFactor<Pose3>::shared_ptr>
class BetweenFactorPose3s
{
size_t size() const;
gtsam::BetweenFactorPose3* at(size_t i) const;
};
#include <gtsam/slam/InitializePose3.h>
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<gtsam::NonlinearFactorGraph*, gtsam::Values*> load3D(string filename);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename, bool is3D);
void writeG2o(const gtsam::NonlinearFactorGraph& graph,
const gtsam::Values& estimate, string filename);

70
gtsam/3rdparty/Eigen/CMakeLists.txt vendored
View File

@ -41,10 +41,13 @@ string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen_minor_
set(EIGEN_MINOR_VERSION "${CMAKE_MATCH_1}")
set(EIGEN_VERSION_NUMBER ${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION})
# if the mercurial program is absent, this will leave the EIGEN_HG_CHANGESET string empty,
# but won't stop CMake.
execute_process(COMMAND hg tip -R ${CMAKE_SOURCE_DIR} OUTPUT_VARIABLE EIGEN_HGTIP_OUTPUT)
execute_process(COMMAND hg branch -R ${CMAKE_SOURCE_DIR} OUTPUT_VARIABLE EIGEN_BRANCH_OUTPUT)
# if we are not in a mercurial clone
if(IS_DIRECTORY ${CMAKE_SOURCE_DIR}/.hg)
# if the mercurial program is absent or this will leave the EIGEN_HG_CHANGESET string empty,
# but won't stop CMake.
execute_process(COMMAND hg tip -R ${CMAKE_SOURCE_DIR} OUTPUT_VARIABLE EIGEN_HGTIP_OUTPUT)
execute_process(COMMAND hg branch -R ${CMAKE_SOURCE_DIR} OUTPUT_VARIABLE EIGEN_BRANCH_OUTPUT)
endif()
# if this is the default (aka development) branch, extract the mercurial changeset number from the hg tip output...
if(EIGEN_BRANCH_OUTPUT MATCHES "default")
@ -64,6 +67,33 @@ include(GNUInstallDirs)
set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
option(EIGEN_TEST_CXX11 "Enable testing with C++11 and C++11 features (e.g. Tensor module)." OFF)
macro(ei_add_cxx_compiler_flag FLAG)
string(REGEX REPLACE "-" "" SFLAG1 ${FLAG})
string(REGEX REPLACE "\\+" "p" SFLAG ${SFLAG1})
check_cxx_compiler_flag(${FLAG} COMPILER_SUPPORT_${SFLAG})
if(COMPILER_SUPPORT_${SFLAG})
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${FLAG}")
endif()
endmacro(ei_add_cxx_compiler_flag)
check_cxx_compiler_flag("-std=c++11" EIGEN_COMPILER_SUPPORT_CPP11)
if(EIGEN_TEST_CXX11)
set(CMAKE_CXX_STANDARD 11)
set(CMAKE_CXX_EXTENSIONS OFF)
if(EIGEN_COMPILER_SUPPORT_CPP11)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
endif()
else()
#set(CMAKE_CXX_STANDARD 03)
#set(CMAKE_CXX_EXTENSIONS OFF)
ei_add_cxx_compiler_flag("-std=c++03")
endif()
#############################################################################
# find how to link to the standard libraries #
#############################################################################
@ -115,15 +145,6 @@ endif()
set(EIGEN_TEST_MAX_SIZE "320" CACHE STRING "Maximal matrix/vector size, default is 320")
macro(ei_add_cxx_compiler_flag FLAG)
string(REGEX REPLACE "-" "" SFLAG1 ${FLAG})
string(REGEX REPLACE "\\+" "p" SFLAG ${SFLAG1})
check_cxx_compiler_flag(${FLAG} COMPILER_SUPPORT_${SFLAG})
if(COMPILER_SUPPORT_${SFLAG})
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${FLAG}")
endif()
endmacro(ei_add_cxx_compiler_flag)
if(NOT MSVC)
# We assume that other compilers are partly compatible with GNUCC
@ -359,8 +380,6 @@ if(EIGEN_TEST_NO_EXCEPTIONS)
message(STATUS "Disabling exceptions in tests/examples")
endif()
option(EIGEN_TEST_CXX11 "Enable testing with C++11 and C++11 features (e.g. Tensor module)." OFF)
set(EIGEN_CUDA_COMPUTE_ARCH 30 CACHE STRING "The CUDA compute architecture level to target when compiling CUDA code")
include_directories(${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_BINARY_DIR})
@ -416,16 +435,15 @@ add_subdirectory(Eigen)
add_subdirectory(doc EXCLUDE_FROM_ALL)
include(EigenConfigureTesting)
option(BUILD_TESTING "Enable creation of Eigen tests." ON)
if(BUILD_TESTING)
include(EigenConfigureTesting)
# fixme, not sure this line is still needed:
enable_testing() # must be called from the root CMakeLists, see man page
if(EIGEN_LEAVE_TEST_IN_ALL_TARGET)
add_subdirectory(test) # can't do EXCLUDE_FROM_ALL here, breaks CTest
else()
add_subdirectory(test EXCLUDE_FROM_ALL)
if(EIGEN_LEAVE_TEST_IN_ALL_TARGET)
add_subdirectory(test) # can't do EXCLUDE_FROM_ALL here, breaks CTest
else()
add_subdirectory(test EXCLUDE_FROM_ALL)
endif()
endif()
if(EIGEN_LEAVE_TEST_IN_ALL_TARGET)
@ -461,7 +479,9 @@ endif(NOT WIN32)
configure_file(scripts/cdashtesting.cmake.in cdashtesting.cmake @ONLY)
ei_testing_print_summary()
if(BUILD_TESTING)
ei_testing_print_summary()
endif()
message(STATUS "")
message(STATUS "Configured Eigen ${EIGEN_VERSION_NUMBER}")

4
gtsam/3rdparty/Eigen/CTestConfig.cmake vendored
View File

@ -4,10 +4,10 @@
## # The following are required to uses Dart and the Cdash dashboard
## ENABLE_TESTING()
## INCLUDE(CTest)
set(CTEST_PROJECT_NAME "Eigen3.3")
set(CTEST_PROJECT_NAME "Eigen 3.3")
set(CTEST_NIGHTLY_START_TIME "00:00:00 UTC")
set(CTEST_DROP_METHOD "http")
set(CTEST_DROP_SITE "manao.inria.fr")
set(CTEST_DROP_LOCATION "/CDash/submit.php?project=Eigen3.3")
set(CTEST_DROP_LOCATION "/CDash/submit.php?project=Eigen+3.3")
set(CTEST_DROP_SITE_CDASH TRUE)

1
gtsam/3rdparty/Eigen/CTestCustom.cmake.in vendored
View File

@ -1,3 +1,4 @@
set(CTEST_CUSTOM_MAXIMUM_NUMBER_OF_WARNINGS "2000")
set(CTEST_CUSTOM_MAXIMUM_NUMBER_OF_ERRORS "2000")
list(APPEND CTEST_CUSTOM_ERROR_EXCEPTION @EIGEN_CTEST_ERROR_EXCEPTION@)

5
gtsam/3rdparty/Eigen/Eigen/Cholesky vendored
View File

@ -9,6 +9,7 @@
#define EIGEN_CHOLESKY_MODULE_H
#include "Core"
#include "Jacobi"
#include "src/Core/util/DisableStupidWarnings.h"
@ -31,7 +32,11 @@
#include "src/Cholesky/LLT.h"
#include "src/Cholesky/LDLT.h"
#ifdef EIGEN_USE_LAPACKE
#ifdef EIGEN_USE_MKL
#include "mkl_lapacke.h"
#else
#include "src/misc/lapacke.h"
#endif
#include "src/Cholesky/LLT_LAPACKE.h"
#endif

27
gtsam/3rdparty/Eigen/Eigen/Core vendored
View File

@ -14,6 +14,22 @@
// first thing Eigen does: stop the compiler from committing suicide
#include "src/Core/util/DisableStupidWarnings.h"
#if defined(__CUDACC__) && !defined(EIGEN_NO_CUDA)
#define EIGEN_CUDACC __CUDACC__
#endif
#if defined(__CUDA_ARCH__) && !defined(EIGEN_NO_CUDA)
#define EIGEN_CUDA_ARCH __CUDA_ARCH__
#endif
#if defined(__CUDACC_VER_MAJOR__) && (__CUDACC_VER_MAJOR__ >= 9)
#define EIGEN_CUDACC_VER ((__CUDACC_VER_MAJOR__ * 10000) + (__CUDACC_VER_MINOR__ * 100))
#elif defined(__CUDACC_VER__)
#define EIGEN_CUDACC_VER __CUDACC_VER__
#else
#define EIGEN_CUDACC_VER 0
#endif
// Handle NVCC/CUDA/SYCL
#if defined(__CUDACC__) || defined(__SYCL_DEVICE_ONLY__)
// Do not try asserts on CUDA and SYCL!
@ -37,9 +53,9 @@
#endif
#define EIGEN_DEVICE_FUNC __host__ __device__
// We need math_functions.hpp to ensure that that EIGEN_USING_STD_MATH macro
// We need cuda_runtime.h to ensure that that EIGEN_USING_STD_MATH macro
// works properly on the device side
#include <math_functions.hpp>
#include <cuda_runtime.h>
#else
#define EIGEN_DEVICE_FUNC
#endif
@ -155,6 +171,9 @@
#ifdef __AVX512DQ__
#define EIGEN_VECTORIZE_AVX512DQ
#endif
#ifdef __AVX512ER__
#define EIGEN_VECTORIZE_AVX512ER
#endif
#endif
// include files
@ -229,7 +248,7 @@
#if defined __CUDACC__
#define EIGEN_VECTORIZE_CUDA
#include <vector_types.h>
#if defined __CUDACC_VER__ && __CUDACC_VER__ >= 70500
#if EIGEN_CUDACC_VER >= 70500
#define EIGEN_HAS_CUDA_FP16
#endif
#endif
@ -352,6 +371,7 @@ using std::ptrdiff_t;
#include "src/Core/MathFunctions.h"
#include "src/Core/GenericPacketMath.h"
#include "src/Core/MathFunctionsImpl.h"
#include "src/Core/arch/Default/ConjHelper.h"
#if defined EIGEN_VECTORIZE_AVX512
#include "src/Core/arch/SSE/PacketMath.h"
@ -367,6 +387,7 @@ using std::ptrdiff_t;
#include "src/Core/arch/AVX/MathFunctions.h"
#include "src/Core/arch/AVX/Complex.h"
#include "src/Core/arch/AVX/TypeCasting.h"
#include "src/Core/arch/SSE/TypeCasting.h"
#elif defined EIGEN_VECTORIZE_SSE
#include "src/Core/arch/SSE/PacketMath.h"
#include "src/Core/arch/SSE/MathFunctions.h"

4
gtsam/3rdparty/Eigen/Eigen/Eigenvalues vendored
View File

@ -45,7 +45,11 @@
#include "src/Eigenvalues/GeneralizedEigenSolver.h"
#include "src/Eigenvalues/MatrixBaseEigenvalues.h"
#ifdef EIGEN_USE_LAPACKE
#ifdef EIGEN_USE_MKL
#include "mkl_lapacke.h"
#else
#include "src/misc/lapacke.h"
#endif
#include "src/Eigenvalues/RealSchur_LAPACKE.h"
#include "src/Eigenvalues/ComplexSchur_LAPACKE.h"
#include "src/Eigenvalues/SelfAdjointEigenSolver_LAPACKE.h"

4
gtsam/3rdparty/Eigen/Eigen/LU vendored
View File

@ -28,7 +28,11 @@
#include "src/LU/FullPivLU.h"
#include "src/LU/PartialPivLU.h"
#ifdef EIGEN_USE_LAPACKE
#ifdef EIGEN_USE_MKL
#include "mkl_lapacke.h"
#else
#include "src/misc/lapacke.h"
#endif
#include "src/LU/PartialPivLU_LAPACKE.h"
#endif
#include "src/LU/Determinant.h"

4
gtsam/3rdparty/Eigen/Eigen/QR vendored
View File

@ -36,7 +36,11 @@
#include "src/QR/ColPivHouseholderQR.h"
#include "src/QR/CompleteOrthogonalDecomposition.h"
#ifdef EIGEN_USE_LAPACKE
#ifdef EIGEN_USE_MKL
#include "mkl_lapacke.h"
#else
#include "src/misc/lapacke.h"
#endif
#include "src/QR/HouseholderQR_LAPACKE.h"
#include "src/QR/ColPivHouseholderQR_LAPACKE.h"
#endif

2
gtsam/3rdparty/Eigen/Eigen/QtAlignedMalloc vendored
View File

@ -27,7 +27,7 @@ void qFree(void *ptr)
void *qRealloc(void *ptr, std::size_t size)
{
void* newPtr = Eigen::internal::aligned_malloc(size);
memcpy(newPtr, ptr, size);
std::memcpy(newPtr, ptr, size);
Eigen::internal::aligned_free(ptr);
return newPtr;
}

4
gtsam/3rdparty/Eigen/Eigen/SVD vendored
View File

@ -37,7 +37,11 @@
#include "src/SVD/JacobiSVD.h"
#include "src/SVD/BDCSVD.h"
#if defined(EIGEN_USE_LAPACKE) && !defined(EIGEN_USE_LAPACKE_STRICT)
#ifdef EIGEN_USE_MKL
#include "mkl_lapacke.h"
#else
#include "src/misc/lapacke.h"
#endif
#include "src/SVD/JacobiSVD_LAPACKE.h"
#endif

14
gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h vendored
View File

@ -248,7 +248,7 @@ template<typename _MatrixType, int _UpLo> class LDLT
/** \brief Reports whether previous computation was successful.
*
* \returns \c Success if computation was succesful,
* \c NumericalIssue if the matrix.appears to be negative.
* \c NumericalIssue if the factorization failed because of a zero pivot.
*/
ComputationInfo info() const
{
@ -305,7 +305,8 @@ template<> struct ldlt_inplace<Lower>
if (size <= 1)
{
transpositions.setIdentity();
if (numext::real(mat.coeff(0,0)) > static_cast<RealScalar>(0) ) sign = PositiveSemiDef;
if(size==0) sign = ZeroSign;
else if (numext::real(mat.coeff(0,0)) > static_cast<RealScalar>(0) ) sign = PositiveSemiDef;
else if (numext::real(mat.coeff(0,0)) < static_cast<RealScalar>(0)) sign = NegativeSemiDef;
else sign = ZeroSign;
return true;
@ -376,6 +377,8 @@ template<> struct ldlt_inplace<Lower>
if((rs>0) && pivot_is_valid)
A21 /= realAkk;
else if(rs>0)
ret = ret && (A21.array()==Scalar(0)).all();
if(found_zero_pivot && pivot_is_valid) ret = false; // factorization failed
else if(!pivot_is_valid) found_zero_pivot = true;
@ -568,13 +571,14 @@ void LDLT<_MatrixType,_UpLo>::_solve_impl(const RhsType &rhs, DstType &dst) cons
// more precisely, use pseudo-inverse of D (see bug 241)
using std::abs;
const typename Diagonal<const MatrixType>::RealReturnType vecD(vectorD());
// In some previous versions, tolerance was set to the max of 1/highest and the maximal diagonal entry * epsilon
// as motivated by LAPACK's xGELSS:
// In some previous versions, tolerance was set to the max of 1/highest (or rather numeric_limits::min())
// and the maximal diagonal entry * epsilon as motivated by LAPACK's xGELSS:
// RealScalar tolerance = numext::maxi(vecD.array().abs().maxCoeff() * NumTraits<RealScalar>::epsilon(),RealScalar(1) / NumTraits<RealScalar>::highest());
// However, LDLT is not rank revealing, and so adjusting the tolerance wrt to the highest
// diagonal element is not well justified and leads to numerical issues in some cases.
// Moreover, Lapack's xSYTRS routines use 0 for the tolerance.
RealScalar tolerance = RealScalar(1) / NumTraits<RealScalar>::highest();
// Using numeric_limits::min() gives us more robustness to denormals.
RealScalar tolerance = (std::numeric_limits<RealScalar>::min)();
for (Index i = 0; i < vecD.size(); ++i)
{

26
gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LLT.h vendored
View File

@ -24,7 +24,7 @@ template<typename MatrixType, int UpLo> struct LLT_Traits;
*
* \tparam _MatrixType the type of the matrix of which we are computing the LL^T Cholesky decomposition
* \tparam _UpLo the triangular part that will be used for the decompositon: Lower (default) or Upper.
* The other triangular part won't be read.
* The other triangular part won't be read.
*
* This class performs a LL^T Cholesky decomposition of a symmetric, positive definite
* matrix A such that A = LL^* = U^*U, where L is lower triangular.
@ -41,14 +41,18 @@ template<typename MatrixType, int UpLo> struct LLT_Traits;
* Example: \include LLT_example.cpp
* Output: \verbinclude LLT_example.out
*
* \b Performance: for best performance, it is recommended to use a column-major storage format
* with the Lower triangular part (the default), or, equivalently, a row-major storage format
* with the Upper triangular part. Otherwise, you might get a 20% slowdown for the full factorization
* step, and rank-updates can be up to 3 times slower.
*
* This class supports the \link InplaceDecomposition inplace decomposition \endlink mechanism.
*
* Note that during the decomposition, only the lower (or upper, as defined by _UpLo) triangular part of A is considered.
* Therefore, the strict lower part does not have to store correct values.
*
* \sa MatrixBase::llt(), SelfAdjointView::llt(), class LDLT
*/
/* HEY THIS DOX IS DISABLED BECAUSE THERE's A BUG EITHER HERE OR IN LDLT ABOUT THAT (OR BOTH)
* Note that during the decomposition, only the upper triangular part of A is considered. Therefore,
* the strict lower part does not have to store correct values.
*/
template<typename _MatrixType, int _UpLo> class LLT
{
public:
@ -146,7 +150,7 @@ template<typename _MatrixType, int _UpLo> class LLT
}
template<typename Derived>
void solveInPlace(MatrixBase<Derived> &bAndX) const;
void solveInPlace(const MatrixBase<Derived> &bAndX) const;
template<typename InputType>
LLT& compute(const EigenBase<InputType>& matrix);
@ -177,7 +181,7 @@ template<typename _MatrixType, int _UpLo> class LLT
/** \brief Reports whether previous computation was successful.
*
* \returns \c Success if computation was succesful,
* \c NumericalIssue if the matrix.appears to be negative.
* \c NumericalIssue if the matrix.appears not to be positive definite.
*/
ComputationInfo info() const
{
@ -425,7 +429,8 @@ LLT<MatrixType,_UpLo>& LLT<MatrixType,_UpLo>::compute(const EigenBase<InputType>
eigen_assert(a.rows()==a.cols());
const Index size = a.rows();
m_matrix.resize(size, size);
m_matrix = a.derived();
if (!internal::is_same_dense(m_matrix, a.derived()))
m_matrix = a.derived();
// Compute matrix L1 norm = max abs column sum.
m_l1_norm = RealScalar(0);
@ -485,11 +490,14 @@ void LLT<_MatrixType,_UpLo>::_solve_impl(const RhsType &rhs, DstType &dst) const
*
* This version avoids a copy when the right hand side matrix b is not needed anymore.
*
* \warning The parameter is only marked 'const' to make the C++ compiler accept a temporary expression here.
* This function will const_cast it, so constness isn't honored here.
*
* \sa LLT::solve(), MatrixBase::llt()
*/
template<typename MatrixType, int _UpLo>
template<typename Derived>
void LLT<MatrixType,_UpLo>::solveInPlace(MatrixBase<Derived> &bAndX) const
void LLT<MatrixType,_UpLo>::solveInPlace(const MatrixBase<Derived> &bAndX) const
{
eigen_assert(m_isInitialized && "LLT is not initialized.");
eigen_assert(m_matrix.rows()==bAndX.rows());

2
gtsam/3rdparty/Eigen/Eigen/src/Core/Array.h vendored
View File

@ -153,8 +153,6 @@ class Array
: Base(std::move(other))
{
Base::_check_template_params();
if (RowsAtCompileTime!=Dynamic && ColsAtCompileTime!=Dynamic)
Base::_set_noalias(other);
}
EIGEN_DEVICE_FUNC
Array& operator=(Array&& other) EIGEN_NOEXCEPT_IF(std::is_nothrow_move_assignable<Scalar>::value)

6
gtsam/3rdparty/Eigen/Eigen/src/Core/Assign_MKL.h vendored
View File

@ -84,7 +84,8 @@ class vml_assign_traits
struct Assignment<DstXprType, CwiseUnaryOp<scalar_##EIGENOP##_op<EIGENTYPE>, SrcXprNested>, assign_op<EIGENTYPE,EIGENTYPE>, \
Dense2Dense, typename enable_if<vml_assign_traits<DstXprType,SrcXprNested>::EnableVml>::type> { \
typedef CwiseUnaryOp<scalar_##EIGENOP##_op<EIGENTYPE>, SrcXprNested> SrcXprType; \
static void run(DstXprType &dst, const SrcXprType &src, const assign_op<EIGENTYPE,EIGENTYPE> &/*func*/) { \
static void run(DstXprType &dst, const SrcXprType &src, const assign_op<EIGENTYPE,EIGENTYPE> &func) { \
resize_if_allowed(dst, src, func); \
eigen_assert(dst.rows() == src.rows() && dst.cols() == src.cols()); \
if(vml_assign_traits<DstXprType,SrcXprNested>::Traversal==LinearTraversal) { \
VMLOP(dst.size(), (const VMLTYPE*)src.nestedExpression().data(), \
@ -144,7 +145,8 @@ EIGEN_MKL_VML_DECLARE_UNARY_CALLS_REAL(ceil, Ceil, _)
Dense2Dense, typename enable_if<vml_assign_traits<DstXprType,SrcXprNested>::EnableVml>::type> { \
typedef CwiseBinaryOp<scalar_##EIGENOP##_op<EIGENTYPE,EIGENTYPE>, SrcXprNested, \
const CwiseNullaryOp<internal::scalar_constant_op<EIGENTYPE>,Plain> > SrcXprType; \
static void run(DstXprType &dst, const SrcXprType &src, const assign_op<EIGENTYPE,EIGENTYPE> &/*func*/) { \
static void run(DstXprType &dst, const SrcXprType &src, const assign_op<EIGENTYPE,EIGENTYPE> &func) { \
resize_if_allowed(dst, src, func); \
eigen_assert(dst.rows() == src.rows() && dst.cols() == src.cols()); \
VMLTYPE exponent = reinterpret_cast<const VMLTYPE&>(src.rhs().functor().m_other); \
if(vml_assign_traits<DstXprType,SrcXprNested>::Traversal==LinearTraversal) \

2
gtsam/3rdparty/Eigen/Eigen/src/Core/ConditionEstimator.h vendored
View File

@ -160,7 +160,7 @@ rcond_estimate_helper(typename Decomposition::RealScalar matrix_norm, const Deco
{
typedef typename Decomposition::RealScalar RealScalar;
eigen_assert(dec.rows() == dec.cols());
if (dec.rows() == 0) return RealScalar(1);
if (dec.rows() == 0) return NumTraits<RealScalar>::infinity();
if (matrix_norm == RealScalar(0)) return RealScalar(0);
if (dec.rows() == 1) return RealScalar(1);
const RealScalar inverse_matrix_norm = rcond_invmatrix_L1_norm_estimate(dec);

39
gtsam/3rdparty/Eigen/Eigen/src/Core/CoreEvaluators.h vendored
View File

@ -977,7 +977,7 @@ struct evaluator<Block<ArgType, BlockRows, BlockCols, InnerPanel> >
OuterStrideAtCompileTime = HasSameStorageOrderAsArgType
? int(outer_stride_at_compile_time<ArgType>::ret)
: int(inner_stride_at_compile_time<ArgType>::ret),
MaskPacketAccessBit = (InnerStrideAtCompileTime == 1) ? PacketAccessBit : 0,
MaskPacketAccessBit = (InnerStrideAtCompileTime == 1 || HasSameStorageOrderAsArgType) ? PacketAccessBit : 0,
FlagsLinearAccessBit = (RowsAtCompileTime == 1 || ColsAtCompileTime == 1 || (InnerPanel && (evaluator<ArgType>::Flags&LinearAccessBit))) ? LinearAccessBit : 0,
FlagsRowMajorBit = XprType::Flags&RowMajorBit,
@ -987,7 +987,9 @@ struct evaluator<Block<ArgType, BlockRows, BlockCols, InnerPanel> >
Flags = Flags0 | FlagsLinearAccessBit | FlagsRowMajorBit,
PacketAlignment = unpacket_traits<PacketScalar>::alignment,
Alignment0 = (InnerPanel && (OuterStrideAtCompileTime!=Dynamic) && (((OuterStrideAtCompileTime * int(sizeof(Scalar))) % int(PacketAlignment)) == 0)) ? int(PacketAlignment) : 0,
Alignment0 = (InnerPanel && (OuterStrideAtCompileTime!=Dynamic)
&& (OuterStrideAtCompileTime!=0)
&& (((OuterStrideAtCompileTime * int(sizeof(Scalar))) % int(PacketAlignment)) == 0)) ? int(PacketAlignment) : 0,
Alignment = EIGEN_PLAIN_ENUM_MIN(evaluator<ArgType>::Alignment, Alignment0)
};
typedef block_evaluator<ArgType, BlockRows, BlockCols, InnerPanel> block_evaluator_type;
@ -1018,14 +1020,16 @@ struct unary_evaluator<Block<ArgType, BlockRows, BlockCols, InnerPanel>, IndexBa
EIGEN_DEVICE_FUNC explicit unary_evaluator(const XprType& block)
: m_argImpl(block.nestedExpression()),
m_startRow(block.startRow()),
m_startCol(block.startCol())
m_startCol(block.startCol()),
m_linear_offset(InnerPanel?(XprType::IsRowMajor ? block.startRow()*block.cols() : block.startCol()*block.rows()):0)
{ }
typedef typename XprType::Scalar Scalar;
typedef typename XprType::CoeffReturnType CoeffReturnType;
enum {
RowsAtCompileTime = XprType::RowsAtCompileTime
RowsAtCompileTime = XprType::RowsAtCompileTime,
ForwardLinearAccess = InnerPanel && bool(evaluator<ArgType>::Flags&LinearAccessBit)
};
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
@ -1037,7 +1041,10 @@ struct unary_evaluator<Block<ArgType, BlockRows, BlockCols, InnerPanel>, IndexBa
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
CoeffReturnType coeff(Index index) const
{
return coeff(RowsAtCompileTime == 1 ? 0 : index, RowsAtCompileTime == 1 ? index : 0);
if (ForwardLinearAccess)
return m_argImpl.coeff(m_linear_offset.value() + index);
else
return coeff(RowsAtCompileTime == 1 ? 0 : index, RowsAtCompileTime == 1 ? index : 0);
}
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
@ -1049,7 +1056,10 @@ struct unary_evaluator<Block<ArgType, BlockRows, BlockCols, InnerPanel>, IndexBa
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Scalar& coeffRef(Index index)
{
return coeffRef(RowsAtCompileTime == 1 ? 0 : index, RowsAtCompileTime == 1 ? index : 0);
if (ForwardLinearAccess)
return m_argImpl.coeffRef(m_linear_offset.value() + index);
else
return coeffRef(RowsAtCompileTime == 1 ? 0 : index, RowsAtCompileTime == 1 ? index : 0);
}
template<int LoadMode, typename PacketType>
@ -1063,8 +1073,11 @@ struct unary_evaluator<Block<ArgType, BlockRows, BlockCols, InnerPanel>, IndexBa
EIGEN_STRONG_INLINE
PacketType packet(Index index) const
{
return packet<LoadMode,PacketType>(RowsAtCompileTime == 1 ? 0 : index,
RowsAtCompileTime == 1 ? index : 0);
if (ForwardLinearAccess)
return m_argImpl.template packet<LoadMode,PacketType>(m_linear_offset.value() + index);
else
return packet<LoadMode,PacketType>(RowsAtCompileTime == 1 ? 0 : index,
RowsAtCompileTime == 1 ? index : 0);
}
template<int StoreMode, typename PacketType>
@ -1078,15 +1091,19 @@ struct unary_evaluator<Block<ArgType, BlockRows, BlockCols, InnerPanel>, IndexBa
EIGEN_STRONG_INLINE
void writePacket(Index index, const PacketType& x)
{
return writePacket<StoreMode,PacketType>(RowsAtCompileTime == 1 ? 0 : index,
RowsAtCompileTime == 1 ? index : 0,
x);
if (ForwardLinearAccess)
return m_argImpl.template writePacket<StoreMode,PacketType>(m_linear_offset.value() + index, x);
else
return writePacket<StoreMode,PacketType>(RowsAtCompileTime == 1 ? 0 : index,
RowsAtCompileTime == 1 ? index : 0,
x);
}
protected:
evaluator<ArgType> m_argImpl;
const variable_if_dynamic<Index, (ArgType::RowsAtCompileTime == 1 && BlockRows==1) ? 0 : Dynamic> m_startRow;
const variable_if_dynamic<Index, (ArgType::ColsAtCompileTime == 1 && BlockCols==1) ? 0 : Dynamic> m_startCol;
const variable_if_dynamic<Index, InnerPanel ? Dynamic : 0> m_linear_offset;
};
// TODO: This evaluator does not actually use the child evaluator;

5
gtsam/3rdparty/Eigen/Eigen/src/Core/Diagonal.h vendored
View File

@ -70,7 +70,10 @@ template<typename MatrixType, int _DiagIndex> class Diagonal
EIGEN_DENSE_PUBLIC_INTERFACE(Diagonal)
EIGEN_DEVICE_FUNC
explicit inline Diagonal(MatrixType& matrix, Index a_index = DiagIndex) : m_matrix(matrix), m_index(a_index) {}
explicit inline Diagonal(MatrixType& matrix, Index a_index = DiagIndex) : m_matrix(matrix), m_index(a_index)
{
eigen_assert( a_index <= m_matrix.cols() && -a_index <= m_matrix.rows() );
}
EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Diagonal)

17
gtsam/3rdparty/Eigen/Eigen/src/Core/Dot.h vendored
View File

@ -31,7 +31,8 @@ struct dot_nocheck
typedef scalar_conj_product_op<typename traits<T>::Scalar,typename traits<U>::Scalar> conj_prod;
typedef typename conj_prod::result_type ResScalar;
EIGEN_DEVICE_FUNC
static inline ResScalar run(const MatrixBase<T>& a, const MatrixBase<U>& b)
EIGEN_STRONG_INLINE
static ResScalar run(const MatrixBase<T>& a, const MatrixBase<U>& b)
{
return a.template binaryExpr<conj_prod>(b).sum();
}
@ -43,7 +44,8 @@ struct dot_nocheck<T, U, true>
typedef scalar_conj_product_op<typename traits<T>::Scalar,typename traits<U>::Scalar> conj_prod;
typedef typename conj_prod::result_type ResScalar;
EIGEN_DEVICE_FUNC
static inline ResScalar run(const MatrixBase<T>& a, const MatrixBase<U>& b)
EIGEN_STRONG_INLINE
static ResScalar run(const MatrixBase<T>& a, const MatrixBase<U>& b)
{
return a.transpose().template binaryExpr<conj_prod>(b).sum();
}
@ -65,6 +67,7 @@ struct dot_nocheck<T, U, true>
template<typename Derived>
template<typename OtherDerived>
EIGEN_DEVICE_FUNC
EIGEN_STRONG_INLINE
typename ScalarBinaryOpTraits<typename internal::traits<Derived>::Scalar,typename internal::traits<OtherDerived>::Scalar>::ReturnType
MatrixBase<Derived>::dot(const MatrixBase<OtherDerived>& other) const
{
@ -102,7 +105,7 @@ EIGEN_STRONG_INLINE typename NumTraits<typename internal::traits<Derived>::Scala
* \sa lpNorm(), dot(), squaredNorm()
*/
template<typename Derived>
inline typename NumTraits<typename internal::traits<Derived>::Scalar>::Real MatrixBase<Derived>::norm() const
EIGEN_STRONG_INLINE typename NumTraits<typename internal::traits<Derived>::Scalar>::Real MatrixBase<Derived>::norm() const
{
return numext::sqrt(squaredNorm());
}
@ -117,7 +120,7 @@ inline typename NumTraits<typename internal::traits<Derived>::Scalar>::Real Matr
* \sa norm(), normalize()
*/
template<typename Derived>
inline const typename MatrixBase<Derived>::PlainObject
EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::PlainObject
MatrixBase<Derived>::normalized() const
{
typedef typename internal::nested_eval<Derived,2>::type _Nested;
@ -139,7 +142,7 @@ MatrixBase<Derived>::normalized() const
* \sa norm(), normalized()
*/
template<typename Derived>
inline void MatrixBase<Derived>::normalize()
EIGEN_STRONG_INLINE void MatrixBase<Derived>::normalize()
{
RealScalar z = squaredNorm();
// NOTE: after extensive benchmarking, this conditional does not impact performance, at least on recent x86 CPU
@ -160,7 +163,7 @@ inline void MatrixBase<Derived>::normalize()
* \sa stableNorm(), stableNormalize(), normalized()
*/
template<typename Derived>
inline const typename MatrixBase<Derived>::PlainObject
EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::PlainObject
MatrixBase<Derived>::stableNormalized() const
{
typedef typename internal::nested_eval<Derived,3>::type _Nested;
@ -185,7 +188,7 @@ MatrixBase<Derived>::stableNormalized() const
* \sa stableNorm(), stableNormalized(), normalize()
*/
template<typename Derived>
inline void MatrixBase<Derived>::stableNormalize()
EIGEN_STRONG_INLINE void MatrixBase<Derived>::stableNormalize()
{
RealScalar w = cwiseAbs().maxCoeff();
RealScalar z = (derived()/w).squaredNorm();

21
gtsam/3rdparty/Eigen/Eigen/src/Core/GeneralProduct.h vendored
View File

@ -24,12 +24,17 @@ template<int Rows, int Cols, int Depth> struct product_type_selector;
template<int Size, int MaxSize> struct product_size_category
{
enum { is_large = MaxSize == Dynamic ||
Size >= EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD ||
(Size==Dynamic && MaxSize>=EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD),
value = is_large ? Large
: Size == 1 ? 1
: Small
enum {
#ifndef EIGEN_CUDA_ARCH
is_large = MaxSize == Dynamic ||
Size >= EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD ||
(Size==Dynamic && MaxSize>=EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD),
#else
is_large = 0,
#endif
value = is_large ? Large
: Size == 1 ? 1
: Small
};
};
@ -379,8 +384,6 @@ template<> struct gemv_dense_selector<OnTheRight,RowMajor,false>
*
* \sa lazyProduct(), operator*=(const MatrixBase&), Cwise::operator*()
*/
#ifndef __CUDACC__
template<typename Derived>
template<typename OtherDerived>
inline const Product<Derived, OtherDerived>
@ -412,8 +415,6 @@ MatrixBase<Derived>::operator*(const MatrixBase<OtherDerived> &other) const
return Product<Derived, OtherDerived>(derived(), other.derived());
}
#endif // __CUDACC__
/** \returns an expression of the matrix product of \c *this and \a other without implicit evaluation.
*
* The returned product will behave like any other expressions: the coefficients of the product will be

17
gtsam/3rdparty/Eigen/Eigen/src/Core/Map.h vendored
View File

@ -20,11 +20,17 @@ struct traits<Map<PlainObjectType, MapOptions, StrideType> >
{
typedef traits<PlainObjectType> TraitsBase;
enum {
PlainObjectTypeInnerSize = ((traits<PlainObjectType>::Flags&RowMajorBit)==RowMajorBit)
? PlainObjectType::ColsAtCompileTime
: PlainObjectType::RowsAtCompileTime,
InnerStrideAtCompileTime = StrideType::InnerStrideAtCompileTime == 0
? int(PlainObjectType::InnerStrideAtCompileTime)
: int(StrideType::InnerStrideAtCompileTime),
OuterStrideAtCompileTime = StrideType::OuterStrideAtCompileTime == 0
? int(PlainObjectType::OuterStrideAtCompileTime)
? (InnerStrideAtCompileTime==Dynamic || PlainObjectTypeInnerSize==Dynamic
? Dynamic
: int(InnerStrideAtCompileTime) * int(PlainObjectTypeInnerSize))
: int(StrideType::OuterStrideAtCompileTime),
Alignment = int(MapOptions)&int(AlignedMask),
Flags0 = TraitsBase::Flags & (~NestByRefBit),
@ -107,10 +113,11 @@ template<typename PlainObjectType, int MapOptions, typename StrideType> class Ma
EIGEN_DEVICE_FUNC
inline Index outerStride() const
{
return StrideType::OuterStrideAtCompileTime != 0 ? m_stride.outer()
: IsVectorAtCompileTime ? this->size()
: int(Flags)&RowMajorBit ? this->cols()
: this->rows();
return int(StrideType::OuterStrideAtCompileTime) != 0 ? m_stride.outer()
: int(internal::traits<Map>::OuterStrideAtCompileTime) != Dynamic ? Index(internal::traits<Map>::OuterStrideAtCompileTime)
: IsVectorAtCompileTime ? (this->size() * innerStride())
: (int(Flags)&RowMajorBit) ? (this->cols() * innerStride())
: (this->rows() * innerStride());
}
/** Constructor in the fixed-size case.

6
gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h vendored
View File

@ -43,6 +43,7 @@ template<typename Derived> class MapBase<Derived, ReadOnlyAccessors>
enum {
RowsAtCompileTime = internal::traits<Derived>::RowsAtCompileTime,
ColsAtCompileTime = internal::traits<Derived>::ColsAtCompileTime,
InnerStrideAtCompileTime = internal::traits<Derived>::InnerStrideAtCompileTime,
SizeAtCompileTime = Base::SizeAtCompileTime
};
@ -187,8 +188,11 @@ template<typename Derived> class MapBase<Derived, ReadOnlyAccessors>
void checkSanity(typename internal::enable_if<(internal::traits<T>::Alignment>0),void*>::type = 0) const
{
#if EIGEN_MAX_ALIGN_BYTES>0
// innerStride() is not set yet when this function is called, so we optimistically assume the lowest plausible value:
const Index minInnerStride = InnerStrideAtCompileTime == Dynamic ? 1 : Index(InnerStrideAtCompileTime);
EIGEN_ONLY_USED_FOR_DEBUG(minInnerStride);
eigen_assert(( ((internal::UIntPtr(m_data) % internal::traits<Derived>::Alignment) == 0)
|| (cols() * rows() * innerStride() * sizeof(Scalar)) < internal::traits<Derived>::Alignment ) && "data is not aligned");
|| (cols() * rows() * minInnerStride * sizeof(Scalar)) < internal::traits<Derived>::Alignment ) && "data is not aligned");
#endif
}

64
gtsam/3rdparty/Eigen/Eigen/src/Core/MathFunctions.h vendored
View File

@ -348,31 +348,7 @@ struct norm1_retval
* Implementation of hypot *
****************************************************************************/
template<typename Scalar>
struct hypot_impl
{
typedef typename NumTraits<Scalar>::Real RealScalar;
static inline RealScalar run(const Scalar& x, const Scalar& y)
{
EIGEN_USING_STD_MATH(abs);
EIGEN_USING_STD_MATH(sqrt);
RealScalar _x = abs(x);
RealScalar _y = abs(y);
Scalar p, qp;
if(_x>_y)
{
p = _x;
qp = _y / p;
}
else
{
p = _y;
qp = _x / p;
}
if(p==RealScalar(0)) return RealScalar(0);
return p * sqrt(RealScalar(1) + qp*qp);
}
};
template<typename Scalar> struct hypot_impl;
template<typename Scalar>
struct hypot_retval
@ -495,7 +471,7 @@ namespace std_fallback {
typedef typename NumTraits<Scalar>::Real RealScalar;
EIGEN_USING_STD_MATH(log);
Scalar x1p = RealScalar(1) + x;
return ( x1p == Scalar(1) ) ? x : x * ( log(x1p) / (x1p - RealScalar(1)) );
return numext::equal_strict(x1p, Scalar(1)) ? x : x * ( log(x1p) / (x1p - RealScalar(1)) );
}
}
@ -640,21 +616,28 @@ template<typename Scalar>
struct random_default_impl<Scalar, false, true>
{
static inline Scalar run(const Scalar& x, const Scalar& y)
{
typedef typename conditional<NumTraits<Scalar>::IsSigned,std::ptrdiff_t,std::size_t>::type ScalarX;
if(y<x)
{
if (y <= x)
return x;
// the following difference might overflow on a 32 bits system,
// but since y>=x the result converted to an unsigned long is still correct.
std::size_t range = ScalarX(y)-ScalarX(x);
std::size_t offset = 0;
// rejection sampling
std::size_t divisor = 1;
std::size_t multiplier = 1;
if(range<RAND_MAX) divisor = (std::size_t(RAND_MAX)+1)/(range+1);
else multiplier = 1 + range/(std::size_t(RAND_MAX)+1);
// ScalarU is the unsigned counterpart of Scalar, possibly Scalar itself.
typedef typename make_unsigned<Scalar>::type ScalarU;
// ScalarX is the widest of ScalarU and unsigned int.
// We'll deal only with ScalarX and unsigned int below thus avoiding signed
// types and arithmetic and signed overflows (which are undefined behavior).
typedef typename conditional<(ScalarU(-1) > unsigned(-1)), ScalarU, unsigned>::type ScalarX;
// The following difference doesn't overflow, provided our integer types are two's
// complement and have the same number of padding bits in signed and unsigned variants.
// This is the case in most modern implementations of C++.
ScalarX range = ScalarX(y) - ScalarX(x);
ScalarX offset = 0;
ScalarX divisor = 1;
ScalarX multiplier = 1;
const unsigned rand_max = RAND_MAX;
if (range <= rand_max) divisor = (rand_max + 1) / (range + 1);
else multiplier = 1 + range / (rand_max + 1);
// Rejection sampling.
do {
offset = (std::size_t(std::rand()) * multiplier) / divisor;
offset = (unsigned(std::rand()) * multiplier) / divisor;
} while (offset > range);
return Scalar(ScalarX(x) + offset);
}
@ -1030,7 +1013,8 @@ inline int log2(int x)
/** \returns the square root of \a x.
*
* It is essentially equivalent to \code using std::sqrt; return sqrt(x); \endcode,
* It is essentially equivalent to
* \code using std::sqrt; return sqrt(x); \endcode
* but slightly faster for float/double and some compilers (e.g., gcc), thanks to
* specializations when SSE is enabled.
*

23
gtsam/3rdparty/Eigen/Eigen/src/Core/MathFunctionsImpl.h vendored
View File

@ -71,6 +71,29 @@ T generic_fast_tanh_float(const T& a_x)
return pdiv(p, q);
}
template<typename RealScalar>
EIGEN_STRONG_INLINE
RealScalar positive_real_hypot(const RealScalar& x, const RealScalar& y)
{
EIGEN_USING_STD_MATH(sqrt);
RealScalar p, qp;
p = numext::maxi(x,y);
if(p==RealScalar(0)) return RealScalar(0);
qp = numext::mini(y,x) / p;
return p * sqrt(RealScalar(1) + qp*qp);
}
template<typename Scalar>
struct hypot_impl
{
typedef typename NumTraits<Scalar>::Real RealScalar;
static inline RealScalar run(const Scalar& x, const Scalar& y)
{
EIGEN_USING_STD_MATH(abs);
return positive_real_hypot<RealScalar>(abs(x), abs(y));
}
};
} // end namespace internal
} // end namespace Eigen

2
gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h vendored
View File

@ -274,8 +274,6 @@ class Matrix
: Base(std::move(other))
{
Base::_check_template_params();
if (RowsAtCompileTime!=Dynamic && ColsAtCompileTime!=Dynamic)
Base::_set_noalias(other);
}
EIGEN_DEVICE_FUNC
Matrix& operator=(Matrix&& other) EIGEN_NOEXCEPT_IF(std::is_nothrow_move_assignable<Scalar>::value)

Some files were not shown because too many files have changed in this diff Show More