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

Merge remote-tracking branch 'origin/develop' into feature/Feature/FixedValues 

Conflicts:
	gtsam.h
	python/handwritten/nonlinear/Values.cpp
release/4.3a0
dellaert 2016-04-10 17:40:26 -07:00
parent 6ccc2a84b3 5d32a321cc
commit 1233a9c9b7
295 changed files with 4942 additions and 2778 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
CMakeLists.txt
View File

@ -64,8 +64,10 @@ option(GTSAM_WITH_TBB "Use Intel Threaded Building Blocks (TB
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_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" 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_USE_VECTOR3_POINTS "Simply typdef Point3 to eigen::Vector3d" OFF)
option(GTSAM_SUPPORT_NESTED_DISSECTION "Support Metis-based nested dissection" ON)
# Options relating to MATLAB wrapper
# TODO: Check for matlab mex binary before handling building of binaries
@ -85,7 +87,11 @@ if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_BUILD_STATIC_LIBRARY)
endif()
if(GTSAM_BUILD_PYTHON AND GTSAM_ALLOW_DEPRECATED_SINCE_V4)
message(FATAL_ERROR "GTSAM_BUILD_PYTHON and GTSAM_ALLOW_DEPRECATED_SINCE_V4 are both enabled. The python module cannot be compiled with deprecated functions turned on. Turn one of the two options off.")
message(FATAL_ERROR "GTSAM_BUILD_PYTHON and GTSAM_ALLOW_DEPRECATED_SINCE_V4 are both enabled. The python module cannot be compiled with deprecated functions turned on. Turn one of the two options off.")
endif()
if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_USE_VECTOR3_POINTS)
message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and GTSAM_USE_VECTOR3_POINTS are both enabled. For now, the MATLAB toolbox cannot deal with this yet. Please turn one of the two options off.")
endif()
# Flags for choosing default packaging tools
@ -185,10 +191,10 @@ if(MKL_FOUND AND GTSAM_WITH_EIGEN_MKL)
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
if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,--no-as-needed")
set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,--no-as-needed")
endif()
else()
set(GTSAM_USE_EIGEN_MKL 0)
@ -219,16 +225,17 @@ option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', us
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}")
# check if MKL is also enabled - can have one or the other, but not both!
if(EIGEN_USE_MKL_ALL)
message(FATAL_ERROR "MKL cannot be used together with system-installed Eigen, as MKL support relies on patches which are not yet in the system-installed Eigen. Disable either GTSAM_USE_SYSTEM_EIGEN or GTSAM_WITH_EIGEN_MKL")
# 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()
else()
# Use bundled Eigen include path.
# Use bundled Eigen include path.
# Clear any variables set by FindEigen3
if(EIGEN3_INCLUDE_DIR)
set(EIGEN3_INCLUDE_DIR NOTFOUND CACHE STRING "" FORCE)
@ -236,11 +243,11 @@ else()
# 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/")
endif()
###############################################################################
@ -283,18 +290,24 @@ endif()
# Include boost - use 'BEFORE' so that a specific boost specified to CMake
# takes precedence over a system-installed one.
# Use 'SYSTEM' to ignore compiler warnings in Boost headers
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/UFconfig
gtsam/3rdparty/CCOLAMD/Include
gtsam/3rdparty/metis/include
gtsam/3rdparty/metis/libmetis
gtsam/3rdparty/metis/GKlib
${METIS_INCLUDE_DIRECTORIES}
${PROJECT_SOURCE_DIR}
${PROJECT_BINARY_DIR} # So we can include generated config header files
CppUnitLite)
@ -322,10 +335,6 @@ if(GTSAM_ENABLE_CONSISTENCY_CHECKS)
add_definitions(-DGTSAM_EXTRA_CONSISTENCY_CHECKS)
endif()
if(GTSAM_ALLOW_DEPRECATED_SINCE_V4)
add_definitions(-DGTSAM_ALLOW_DEPRECATED_SINCE_V4)
endif()
###############################################################################
# Add components
@ -360,9 +369,9 @@ if (GTSAM_BUILD_PYTHON)
# NOTE: The automatic generation of python wrapper from the gtsampy.h interface is
# not working yet, so we're using a handwritten wrapper files on python/handwritten.
# Once the python wrapping from the interface file is working, you can _swap_ the
# Once the python wrapping from the interface file is working, you can _swap_ the
# comments on the next lines
# wrap_and_install_python(gtsampy.h "${GTSAM_ADDITIONAL_LIBRARIES}" "")
add_subdirectory(python)
@ -481,6 +490,8 @@ print_config_flag(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency chec
print_config_flag(${GTSAM_ROT3_EXPMAP} "Rot3 retract is full ExpMap ")
print_config_flag(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ")
print_config_flag(${GTSAM_ALLOW_DEPRECATED_SINCE_V4} "Deprecated in GTSAM 4 allowed ")
print_config_flag(${GTSAM_USE_VECTOR3_POINTS} "Point3 is typedef to Vector3 ")
print_config_flag(${GTSAM_SUPPORT_NESTED_DISSECTION} "Metis-based Nested Dissection ")
message(STATUS "MATLAB toolbox flags ")
print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install matlab toolbox ")

12
GTSAM-Concepts.md
View File

@ -18,14 +18,14 @@ To optimize over continuous types, we assume they are manifolds. This is central
[Manifolds](http://en.wikipedia.org/wiki/Manifold#Charts.2C_atlases.2C_and_transition_maps) and [charts](http://en.wikipedia.org/wiki/Manifold#Charts.2C_atlases.2C_and_transition_maps) are intimately linked concepts. We are only interested here in [differentiable manifolds](http://en.wikipedia.org/wiki/Differentiable_manifold#Definition), continuous spaces that can be locally approximated *at any point* using a local vector space, called the [tangent space](http://en.wikipedia.org/wiki/Tangent_space). A *chart* is an invertible map from the manifold to that tangent space.
In GTSAM, all properties and operations needed to use a type must be defined through template specialization of the struct `gtsam::traits`. Concept checks are used to check that all required functions are implemented.
In detail, we ask the following are defined in the traits object:
In GTSAM, all properties and operations needed to use a type must be defined through template specialization of the struct `gtsam::traits`. Concept checks are used to check that all required functions are implemented.
In detail, we ask that the following items are defined in the traits object (although, not all are needed for optimization):
* values:
* `enum { dimension = D};`, an enum that indicates the dimensionality *n* of the manifold. In Eigen-fashion, we also support manifolds whose dimenionality is only defined at runtime, by specifying the value -1.
* types:
* `enum { dimension = D};`, an enum that indicates the dimensionality *n* of the manifold. In Eigen-fashion, we also support manifolds whose dimensionality is only defined at runtime, by specifying the value -1.
* types:
* `TangentVector`, type that lives in tangent space. This will almost always be an `Eigen::Matrix<double,n,1>`.
* `ChartJacobian`, a typedef for `OptionalJacobian<dimension, dimension>`.
* `ChartJacobian`, a typedef for `OptionalJacobian<dimension, dimension>`.
* `ManifoldType`, a pointer back to the type.
* `structure_category`, a tag type that defines what requirements the type fulfills, and therefore what requirements this traits class must fulfill. It should be defined to be one of the following:
* `gtsam::traits::manifold_tag` -- Everything in this list is expected
@ -33,7 +33,7 @@ In detail, we ask the following are defined in the traits object:
* `gtsam::traits::lie_group_tag` -- Everything in this list is expected, plus the functions defined under **Groups**, and **Lie Groups** below.
* `gtsam::traits::vector_space_tag` -- Everything in this list is expected, plus the functions defined under **Groups**, and **Lie Groups** below.
* valid expressions:
* `size_t dim = traits<T>::getDimension(p);` static function should be defined. This is mostly useful if the size is not known at compile time.
* `size_t dim = traits<T>::GetDimension(p);` static function should be defined. This is mostly useful if the size is not known at compile time.
* `v = traits<T>::Local(p,q)`, the chart, from manifold to tangent space, think of it as *q (-) p*, where *p* and *q* are elements of the manifold and the result, *v* is an element of the vector space.
* `p = traits<T>::Retract(p,v)`, the inverse chart, from tangent space to manifold, think of it as *p (+) v*, where *p* is an element of the manifold and the result, *v* is an element of the vector space.
* invariants

7
README.md
View File

@ -38,6 +38,13 @@ Optional prerequisites - used automatically if findable by CMake:
- [Intel Threaded Building Blocks (TBB)](http://www.threadingbuildingblocks.org/) (Ubuntu: `sudo apt-get install libtbb-dev`)
- [Intel Math Kernel Library (MKL)](http://software.intel.com/en-us/intel-mkl)
GTSAM 4 Compatibility
---------------------
GTSAM 4 will introduce several new features, most notably Expressions and a python toolbox. We will also deprecate some legacy functionality and wrongly named methods, but by default the flag GTSAM_ALLOW_DEPRECATED_SINCE_V4 is enabled, allowing anyone to just pull V4 and compile. To build the python toolbox, however, you will have to explicitly disable that flag.
Also, GTSAM 4 introduces traits, a C++ technique that allows optimizing with non-GTSAM types. That opens the door to retiring geometric types such as Point2 and Point3 to pure Eigen types, which we will also do. A significant change which will not trigger a compile error is that zero-initializing of Point2 and Point3 will be deprecated, so please be aware that this might render functions using their default constructor incorrect.
Additional Information
----------------------

143
cmake/GtsamBuildTypes.cmake
View File

@ -3,56 +3,84 @@
list(APPEND CMAKE_PREFIX_PATH "${CMAKE_INSTALL_PREFIX}")
# Default to Release mode
if(NOT FIRST_PASS_DONE AND NOT CMAKE_BUILD_TYPE AND NOT MSVC AND NOT XCODE_VERSION)
set(CMAKE_BUILD_TYPE "Release" CACHE STRING
"Choose the type of build, options are: None Debug Release Timing Profiling RelWithDebInfo."
FORCE)
if(NOT CMAKE_BUILD_TYPE AND NOT MSVC AND NOT XCODE_VERSION)
set(GTSAM_CMAKE_BUILD_TYPE "Release" CACHE STRING
"Choose the type of build, options are: None Debug Release Timing Profiling RelWithDebInfo.")
set(CMAKE_BUILD_TYPE ${GTSAM_CMAKE_BUILD_TYPE})
endif()
# Add option for using build type postfixes to allow installing multiple build modes
option(GTSAM_BUILD_TYPE_POSTFIXES "Enable/Disable appending the build type to the name of compiled libraries" ON)
# Add debugging flags but only on the first pass
if(NOT FIRST_PASS_DONE)
if(MSVC)
set(CMAKE_C_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN /DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE)
set(CMAKE_CXX_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN /DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE)
set(CMAKE_C_FLAGS_RELWITHDEBINFO "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /Zi /d2Zi+ /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE)
set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /Zi /d2Zi+ /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE)
set(CMAKE_C_FLAGS_RELEASE "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during release builds." FORCE)
set(CMAKE_CXX_FLAGS_RELEASE "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during release builds." FORCE)
set(CMAKE_C_FLAGS_TIMING "${CMAKE_C_FLAGS_RELEASE} /DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds." FORCE)
set(CMAKE_CXX_FLAGS_TIMING "${CMAKE_CXX_FLAGS_RELEASE} /DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds." FORCE)
set(CMAKE_EXE_LINKER_FLAGS_TIMING "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds." FORCE)
set(CMAKE_SHARED_LINKER_FLAGS_TIMING "${CMAKE_SHARED_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds." FORCE)
set(CMAKE_MODULE_LINKER_FLAGS_TIMING "${CMAKE_MODULE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds." FORCE)
mark_as_advanced(CMAKE_C_FLAGS_TIMING CMAKE_CXX_FLAGS_TIMING CMAKE_EXE_LINKER_FLAGS_TIMING CMAKE_SHARED_LINKER_FLAGS_TIMING CMAKE_MODULE_LINKER_FLAGS_TIMING)
set(CMAKE_C_FLAGS_PROFILING "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /Zi /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during profiling builds." FORCE)
set(CMAKE_CXX_FLAGS_PROFILING "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /Zi /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during profiling builds." FORCE)
set(CMAKE_EXE_LINKER_FLAGS_PROFILING "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds." FORCE)
set(CMAKE_SHARED_LINKER_FLAGS_PROFILING "${CMAKE_SHARED_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds." FORCE)
set(CMAKE_MODULE_LINKER_FLAGS_PROFILING "${CMAKE_MODULE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds." FORCE)
mark_as_advanced(CMAKE_C_FLAGS_PROFILING CMAKE_CXX_FLAGS_PROFILING CMAKE_EXE_LINKER_FLAGS_PROFILING CMAKE_SHARED_LINKER_FLAGS_PROFILING CMAKE_MODULE_LINKER_FLAGS_PROFILING)
else()
set(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS_DEBUG} -std=c11 -fno-inline -Wall -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE)
set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -std=c++11 -fno-inline -Wall -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE)
set(CMAKE_C_FLAGS_RELWITHDEBINFO "-std=c11 -g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE)
set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "-std=c++11 -g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE)
set(CMAKE_C_FLAGS_RELEASE "-std=c11 -O3 -Wall -DNDEBUG -Wall" CACHE STRING "Flags used by the compiler during release builds." FORCE)
set(CMAKE_CXX_FLAGS_RELEASE "-std=c++11 -O3 -Wall -DNDEBUG -Wall" CACHE STRING "Flags used by the compiler during release builds." FORCE)
set(CMAKE_C_FLAGS_TIMING "${CMAKE_C_FLAGS_RELEASE} -DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds." FORCE)
set(CMAKE_CXX_FLAGS_TIMING "${CMAKE_CXX_FLAGS_RELEASE} -DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds." FORCE)
set(CMAKE_EXE_LINKER_FLAGS_TIMING "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds." FORCE)
set(CMAKE_SHARED_LINKER_FLAGS_TIMING "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds." FORCE)
mark_as_advanced(CMAKE_C_FLAGS_TIMING CMAKE_CXX_FLAGS_TIMING CMAKE_EXE_LINKER_FLAGS_TIMING CMAKE_SHARED_LINKER_FLAGS_TIMING)
set(CMAKE_C_FLAGS_PROFILING "-std=c11 -g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during profiling builds." FORCE)
set(CMAKE_CXX_FLAGS_PROFILING "-std=c++11 -g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during profiling builds." FORCE)
set(CMAKE_EXE_LINKER_FLAGS_PROFILING "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds." FORCE)
set(CMAKE_SHARED_LINKER_FLAGS_PROFILING "${CMAKE__LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds." FORCE)
mark_as_advanced(CMAKE_C_FLAGS_PROFILING CMAKE_CXX_FLAGS_PROFILING CMAKE_EXE_LINKER_FLAGS_PROFILING CMAKE_SHARED_LINKER_FLAGS_PROFILING)
endif()
# Set custom compilation flags.
# NOTE: We set all the CACHE variables with a GTSAM prefix, and then set a normal local variable below
# so that we don't "pollute" the global variable namespace in the cmake cache.
# Set all CMAKE_BUILD_TYPE flags:
# (see https://cmake.org/Wiki/CMake_Useful_Variables#Compilers_and_Tools)
if(MSVC)
set(GTSAM_CMAKE_C_FLAGS "/W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler for all builds.")
set(GTSAM_CMAKE_CXX_FLAGS "/W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler for all builds.")
set(GTSAM_CMAKE_C_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Extra flags used by the compiler during debug builds.")
set(GTSAM_CMAKE_CXX_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Extra flags used by the compiler during debug builds.")
set(GTSAM_CMAKE_C_FLAGS_RELWITHDEBINFO "/MD /O2 /DNDEBUG /Zi /d2Zi+" CACHE STRING "Extra flags used by the compiler during relwithdebinfo builds.")
set(GTSAM_CMAKE_CXX_FLAGS_RELWITHDEBINFO "/MD /O2 /DNDEBUG /Zi /d2Zi+" CACHE STRING "Extra flags used by the compiler during relwithdebinfo builds.")
set(GTSAM_CMAKE_C_FLAGS_RELEASE "/MD /O2 /DNDEBUG" CACHE STRING "Extra flags used by the compiler during release builds.")
set(GTSAM_CMAKE_CXX_FLAGS_RELEASE "/MD /O2 /DNDEBUG" CACHE STRING "Extra flags used by the compiler during release builds.")
set(GTSAM_CMAKE_C_FLAGS_PROFILING "${GTSAM_CMAKE_C_FLAGS_RELEASE} /Zi" CACHE STRING "Extra flags used by the compiler during profiling builds.")
set(GTSAM_CMAKE_CXX_FLAGS_PROFILING "${GTSAM_CMAKE_CXX_FLAGS_RELEASE} /Zi" CACHE STRING "Extra flags used by the compiler during profiling builds.")
set(GTSAM_CMAKE_C_FLAGS_TIMING "${GTSAM_CMAKE_C_FLAGS_RELEASE} /DENABLE_TIMING" CACHE STRING "Extra flags used by the compiler during timing builds.")
set(GTSAM_CMAKE_CXX_FLAGS_TIMING "${GTSAM_CMAKE_CXX_FLAGS_RELEASE} /DENABLE_TIMING" CACHE STRING "Extra flags used by the compiler during timing builds.")
else()
set(GTSAM_CMAKE_C_FLAGS "-std=c11 -Wall" CACHE STRING "Flags used by the compiler for all builds.")
set(GTSAM_CMAKE_CXX_FLAGS "-std=c++11 -Wall" CACHE STRING "Flags used by the compiler for all builds.")
set(GTSAM_CMAKE_C_FLAGS_DEBUG "-g -fno-inline -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Extra flags used by the compiler during debug builds.")
set(GTSAM_CMAKE_CXX_FLAGS_DEBUG "-g -fno-inline -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Extra flags used by the compiler during debug builds.")
set(GTSAM_CMAKE_C_FLAGS_RELWITHDEBINFO "-g -O3 -DNDEBUG" CACHE STRING "Extra flags used by the compiler during relwithdebinfo builds.")
set(GTSAM_CMAKE_CXX_FLAGS_RELWITHDEBINFO "-g -O3 -DNDEBUG" CACHE STRING "Extra flags used by the compiler during relwithdebinfo builds.")
set(GTSAM_CMAKE_C_FLAGS_RELEASE " -O3 -DNDEBUG" CACHE STRING "Extra flags used by the compiler during release builds.")
set(GTSAM_CMAKE_CXX_FLAGS_RELEASE " -O3 -DNDEBUG" CACHE STRING "Extra flags used by the compiler during release builds.")
set(GTSAM_CMAKE_C_FLAGS_PROFILING "${GTSAM_CMAKE_C_FLAGS_RELEASE}" CACHE STRING "Extra flags used by the compiler during profiling builds.")
set(GTSAM_CMAKE_CXX_FLAGS_PROFILING "${GTSAM_CMAKE_CXX_FLAGS_RELEASE}" CACHE STRING "Extra flags used by the compiler during profiling builds.")
set(GTSAM_CMAKE_C_FLAGS_TIMING "${GTSAM_CMAKE_C_FLAGS_RELEASE} -DENABLE_TIMING" CACHE STRING "Extra flags used by the compiler during timing builds.")
set(GTSAM_CMAKE_CXX_FLAGS_TIMING "${GTSAM_CMAKE_CXX_FLAGS_RELEASE} -DENABLE_TIMING" CACHE STRING "Extra flags used by the compiler during timing builds.")
endif()
set(GTSAM_CMAKE_SHARED_LINKER_FLAGS_TIMING "${CMAKE_SHARED_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds.")
set(GTSAM_CMAKE_MODULE_LINKER_FLAGS_TIMING "${CMAKE_MODULE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds.")
set(GTSAM_CMAKE_EXE_LINKER_FLAGS_TIMING "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds.")
set(GTSAM_CMAKE_SHARED_LINKER_FLAGS_PROFILING "${CMAKE_SHARED_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds.")
set(GTSAM_CMAKE_MODULE_LINKER_FLAGS_PROFILING "${CMAKE_MODULE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds.")
set(GTSAM_CMAKE_EXE_LINKER_FLAGS_PROFILING "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds.")
mark_as_advanced(GTSAM_CMAKE_C_FLAGS_TIMING GTSAM_CMAKE_CXX_FLAGS_TIMING GTSAM_CMAKE_EXE_LINKER_FLAGS_TIMING
GTSAM_CMAKE_SHARED_LINKER_FLAGS_TIMING GTSAM_CMAKE_MODULE_LINKER_FLAGS_TIMING
GTSAM_CMAKE_C_FLAGS_PROFILING GTSAM_CMAKE_CXX_FLAGS_PROFILING GTSAM_CMAKE_EXE_LINKER_FLAGS_PROFILING
GTSAM_CMAKE_SHARED_LINKER_FLAGS_PROFILING GTSAM_CMAKE_MODULE_LINKER_FLAGS_PROFILING)
# Apply the gtsam specific build flags as normal variables. This makes it so that they only
# apply to the gtsam part of the build if gtsam is built as a subproject
set(CMAKE_C_FLAGS ${GTSAM_CMAKE_C_FLAGS})
set(CMAKE_CXX_FLAGS ${GTSAM_CMAKE_CXX_FLAGS})
set(CMAKE_C_FLAGS_DEBUG ${GTSAM_CMAKE_C_FLAGS_DEBUG})
set(CMAKE_CXX_FLAGS_DEBUG ${GTSAM_CMAKE_CXX_FLAGS_DEBUG})
set(CMAKE_C_FLAGS_RELWITHDEBINFO ${GTSAM_CMAKE_C_FLAGS_RELWITHDEBINFO})
set(CMAKE_CXX_FLAGS_RELWITHDEBINFO ${GTSAM_CMAKE_CXX_FLAGS_RELWITHDEBINFO})
set(CMAKE_C_FLAGS_RELEASE ${GTSAM_CMAKE_C_FLAGS_RELEASE})
set(CMAKE_CXX_FLAGS_RELEASE ${GTSAM_CMAKE_CXX_FLAGS_RELEASE})
set(CMAKE_C_FLAGS_PROFILING ${GTSAM_CMAKE_C_FLAGS_PROFILING})
set(CMAKE_CXX_FLAGS_PROFILING ${GTSAM_CMAKE_CXX_FLAGS_PROFILING})
set(CMAKE_C_FLAGS_TIMING ${GTSAM_CMAKE_C_FLAGS_TIMING})
set(CMAKE_CXX_FLAGS_TIMING ${GTSAM_CMAKE_CXX_FLAGS_TIMING})
set(CMAKE_SHARED_LINKER_FLAGS_TIMING ${GTSAM_CMAKE_SHARED_LINKER_FLAGS_TIMING})
set(CMAKE_MODULE_LINKER_FLAGS_TIMING ${GTSAM_CMAKE_MODULE_LINKER_FLAGS_TIMING})
set(CMAKE_EXE_LINKER_FLAGS_TIMING ${GTSAM_CMAKE_EXE_LINKER_FLAGS_TIMING})
set(CMAKE_SHARED_LINKER_FLAGS_PROFILING ${GTSAM_CMAKE_SHARED_LINKER_FLAGS_PROFILING})
set(CMAKE_MODULE_LINKER_FLAGS_PROFILING ${GTSAM_CMAKE_MODULE_LINKER_FLAGS_PROFILING})
set(CMAKE_EXE_LINKER_FLAGS_PROFILING ${GTSAM_CMAKE_EXE_LINKER_FLAGS_PROFILING})
# Clang uses a template depth that is less than standard and is too small
if(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang")
# Apple Clang before 5.0 does not support -ftemplate-depth.
@ -63,10 +91,10 @@ endif()
# Set up build type library postfixes
if(GTSAM_BUILD_TYPE_POSTFIXES)
foreach(build_type Debug Timing Profiling RelWithDebInfo MinSizeRel)
string(TOUPPER "${build_type}" build_type_toupper)
set(CMAKE_${build_type_toupper}_POSTFIX ${build_type})
endforeach()
foreach(build_type Debug Timing Profiling RelWithDebInfo MinSizeRel)
string(TOUPPER "${build_type}" build_type_toupper)
set(CMAKE_${build_type_toupper}_POSTFIX ${build_type})
endforeach()
endif()
# Make common binary output directory when on Windows
@ -78,17 +106,16 @@ endif()
# Set up build type list for cmake-gui
if(NOT "${CMAKE_BUILD_TYPE}" STREQUAL "")
if(${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} VERSION_GREATER 2.8 OR ${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} VERSION_EQUAL 2.8)
set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS None Debug Release Timing Profiling RelWithDebInfo MinSizeRel)
endif()
if(${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} VERSION_GREATER 2.8 OR ${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} VERSION_EQUAL 2.8)
set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS None Debug Release Timing Profiling RelWithDebInfo MinSizeRel)
endif()
endif()
# Set up build types for MSVC and XCode
if(NOT FIRST_PASS_DONE)
set(CMAKE_CONFIGURATION_TYPES Debug Release Timing Profiling RelWithDebInfo MinSizeRel
CACHE STRING "Build types available to MSVC and XCode" FORCE)
mark_as_advanced(FORCE CMAKE_CONFIGURATION_TYPES)
endif()
set(GTSAM_CMAKE_CONFIGURATION_TYPES Debug Release Timing Profiling RelWithDebInfo MinSizeRel
CACHE STRING "Build types available to MSVC and XCode")
mark_as_advanced(FORCE GTSAM_CMAKE_CONFIGURATION_TYPES)
set(CMAKE_CONFIGURATION_TYPES ${GTSAM_CMAKE_CONFIGURATION_TYPES})
# Check build types
string(TOLOWER "${CMAKE_BUILD_TYPE}" cmake_build_type_tolower)
@ -100,13 +127,9 @@ if( NOT cmake_build_type_tolower STREQUAL ""
AND NOT cmake_build_type_tolower STREQUAL "profiling"
AND NOT cmake_build_type_tolower STREQUAL "relwithdebinfo"
AND NOT cmake_build_type_tolower STREQUAL "minsizerel")
message(FATAL_ERROR "Unknown build type \"${CMAKE_BUILD_TYPE}\". Allowed values are None, Debug, Release, Timing, Profiling, RelWithDebInfo (case-insensitive).")
message(FATAL_ERROR "Unknown build type \"${CMAKE_BUILD_TYPE}\". Allowed values are None, Debug, Release, Timing, Profiling, RelWithDebInfo, MinSizeRel (case-insensitive).")
endif()
# Mark that first pass is done
set(FIRST_PASS_DONE TRUE CACHE INTERNAL "Internally used to mark whether cmake has been run multiple times")
mark_as_advanced(FIRST_PASS_DONE)
# Enable Visual Studio solution folders
set_property(GLOBAL PROPERTY USE_FOLDERS On)

6
cmake/GtsamTesting.cmake
View File

@ -164,9 +164,9 @@ macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries)
add_test(NAME ${script_name} COMMAND ${script_name})
add_dependencies(check.${groupName} ${script_name})
add_dependencies(check ${script_name})
add_dependencies(all.tests ${script_name})
add_dependencies(all.tests ${script_name})
if(NOT MSVC AND NOT XCODE_VERSION)
add_custom_target(${script_name}.run ${EXECUTABLE_OUTPUT_PATH}${script_name})
add_custom_target(${script_name}.run ${EXECUTABLE_OUTPUT_PATH}${script_name} DEPENDS ${script_name})
endif()
# Add TOPSRCDIR
@ -254,7 +254,7 @@ macro(gtsamAddExesGlob_impl globPatterns excludedFiles linkLibraries groupName b
# Add target dependencies
add_dependencies(${groupName} ${script_name})
if(NOT MSVC AND NOT XCODE_VERSION)
add_custom_target(${script_name}.run ${EXECUTABLE_OUTPUT_PATH}${script_name})
add_custom_target(${script_name}.run ${EXECUTABLE_OUTPUT_PATH}${script_name} DEPENDS ${script_name})
endif()
# Add TOPSRCDIR

7
doc/CMakeLists.txt
View File

@ -23,12 +23,17 @@ if (GTSAM_BUILD_DOCS)
# GTSAM core subfolders
set(gtsam_doc_subdirs
gtsam/base
gtsam/discrete
gtsam/geometry
gtsam/inference
gtsam/discrete
gtsam/linear
gtsam/navigation
gtsam/nonlinear
gtsam/sam
gtsam/sfm
gtsam/slam
gtsam/smart
gtsam/symbolic
gtsam
)

2
examples/RangeISAMExample_plaza2.cpp
View File

@ -83,7 +83,7 @@ vector<RangeTriple> readTriples() {
while (is) {
double t, sender, range;
size_t receiver;
size_t receiver;
is >> t >> sender >> receiver >> range;
triples.push_back(RangeTriple(t, receiver, range));
}

2
examples/SFMExample.cpp
View File

@ -99,7 +99,7 @@ int main(int argc, char* argv[]) {
for (size_t i = 0; i < poses.size(); ++i)
initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
for (size_t j = 0; j < points.size(); ++j)
initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15)));
initialEstimate.insert<Point3>(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15));
initialEstimate.print("Initial Estimates:\n");
/* Optimize the graph and print results */

2
examples/SFMExampleExpressions.cpp
View File

@ -88,7 +88,7 @@ int main(int argc, char* argv[]) {
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(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15)));
initial.insert<Point3>(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15));
cout << "initial error = " << graph.error(initial) << endl;
/* Optimize the graph and print results */

2
examples/SelfCalibrationExample.cpp
View File

@ -84,7 +84,7 @@ int main(int argc, char* argv[]) {
for (size_t i = 0; i < poses.size(); ++i)
initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
for (size_t j = 0; j < points.size(); ++j)
initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15)));
initialEstimate.insert<Point3>(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15));
/* Optimize the graph and print results */
Values result = DoglegOptimizer(graph, initialEstimate).optimize();

2
examples/VisualISAM2Example.cpp
View File

@ -113,7 +113,7 @@ int main(int argc, char* argv[]) {
// Add initial guesses to all observed landmarks
// Intentionally initialize the variables off from the ground truth
for (size_t j = 0; j < points.size(); ++j)
initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15)));
initialEstimate.insert<Point3>(Symbol('l', j), points[j] +Point3(-0.25, 0.20, 0.15));
} else {
// Update iSAM with the new factors

2
examples/VisualISAMExample.cpp
View File

@ -120,7 +120,7 @@ int main(int argc, char* argv[]) {
Point3 noise(-0.25, 0.20, 0.15);
for (size_t j = 0; j < points.size(); ++j) {
// Intentionally initialize the variables off from the ground truth
Point3 initial_lj = points[j].compose(noise);
Point3 initial_lj = points[j] + noise;
initialEstimate.insert(Symbol('l', j), initial_lj);
}

70
gtsam.h
View File

@ -127,13 +127,13 @@ namespace std {
void pop_back();*/
};
//typedef std::vector
#include<list>
template<T>
class list
{
};
}
@ -360,17 +360,6 @@ class Point3 {
// Group
static gtsam::Point3 identity();
gtsam::Point3 inverse() const;
gtsam::Point3 compose(const gtsam::Point3& p2) const;
gtsam::Point3 between(const gtsam::Point3& p2) const;
// Manifold
gtsam::Point3 retract(Vector v) const;
Vector localCoordinates(const gtsam::Point3& p) const;
// Lie Group
static gtsam::Point3 Expmap(Vector v);
static Vector Logmap(const gtsam::Point3& p);
// Standard Interface
Vector vector() const;
@ -1069,7 +1058,7 @@ class SymbolicBayesTree {
void clear();
void deleteCachedShortcuts();
size_t numCachedSeparatorMarginals() const;
gtsam::SymbolicConditional* marginalFactor(size_t key) const;
gtsam::SymbolicFactorGraph* joint(size_t key1, size_t key2) const;
gtsam::SymbolicBayesNet* jointBayesNet(size_t key1, size_t key2) const;
@ -1079,19 +1068,19 @@ class SymbolicBayesTree {
// BayesTreeClique();
// BayesTreeClique(CONDITIONAL* conditional);
// // BayesTreeClique(const std::pair<typename ConditionalType::shared_ptr, typename ConditionalType::FactorType::shared_ptr>& result) : Base(result) {}
//
//
// bool equals(const This& other, double tol) const;
// void print(string s) const;
// void printTree() const; // Default indent of ""
// void printTree(string indent) const;
// size_t numCachedSeparatorMarginals() const;
//
//
// CONDITIONAL* conditional() const;
// bool isRoot() const;
// size_t treeSize() const;
// // const std::list<derived_ptr>& children() const { return children_; }
// // derived_ptr parent() const { return parent_.lock(); }
//
//
// // FIXME: need wrapped versions graphs, BayesNet
// // BayesNet<ConditionalType> shortcut(derived_ptr root, Eliminate function) const;
// // FactorGraph<FactorType> marginal(derived_ptr root, Eliminate function) const;
@ -1345,7 +1334,7 @@ virtual class JacobianFactor : gtsam::GaussianFactor {
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);
@ -1422,7 +1411,7 @@ class GaussianFactorGraph {
gtsam::GaussianFactorGraph clone() const;
gtsam::GaussianFactorGraph negate() const;
// Optimizing and linear algebra
// Optimizing and linear algebra
gtsam::VectorValues optimize() const;
gtsam::VectorValues optimize(const gtsam::Ordering& ordering) const;
gtsam::VectorValues optimizeGradientSearch() const;
@ -1511,7 +1500,7 @@ virtual class GaussianBayesNet {
//Constructors
GaussianBayesNet();
GaussianBayesNet(const gtsam::GaussianConditional* conditional);
// Testable
void print(string s) const;
bool equals(const gtsam::GaussianBayesNet& other, double tol) const;
@ -1527,7 +1516,7 @@ virtual class GaussianBayesNet {
gtsam::GaussianConditional* back() const;
void push_back(gtsam::GaussianConditional* conditional);
void push_back(const gtsam::GaussianBayesNet& bayesNet);
gtsam::VectorValues optimize() const;
gtsam::VectorValues optimize(gtsam::VectorValues& solutionForMissing) const;
gtsam::VectorValues optimizeGradientSearch() const;
@ -1551,7 +1540,7 @@ virtual class GaussianBayesTree {
bool empty() const;
size_t numCachedSeparatorMarginals() const;
void saveGraph(string s) const;
gtsam::VectorValues optimize() const;
gtsam::VectorValues optimizeGradientSearch() const;
gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const;
@ -1753,7 +1742,8 @@ virtual class NonlinearFactor {
virtual class NoiseModelFactor: gtsam::NonlinearFactor {
void equals(const gtsam::NoiseModelFactor& other, double tol) const;
gtsam::noiseModel::Base* get_noiseModel() 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;
};
@ -1995,7 +1985,7 @@ 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);
@ -2162,6 +2152,8 @@ class ISAM2Result {
size_t getCliques() const;
};
class FactorIndices {};
class ISAM2 {
ISAM2();
ISAM2(const gtsam::ISAM2Params& params);
@ -2174,8 +2166,8 @@ class ISAM2 {
gtsam::ISAM2Result update();
gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta);
gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices);
gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, const gtsam::KeyGroupMap& constrainedKeys);
gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices);
gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, const gtsam::KeyGroupMap& constrainedKeys);
// TODO: wrap the full version of update
//void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, FastMap<Key,int>& constrainedKeys);
//void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, FastMap<Key,int>& constrainedKeys, bool force_relinearize);
@ -2488,10 +2480,30 @@ class NavState {
gtsam::Pose3 pose() const;
};
#include <gtsam/navigation/PreintegratedRotation.h>
virtual class PreintegratedRotationParams {
PreintegratedRotationParams();
void setGyroscopeCovariance(Matrix cov);
void setOmegaCoriolis(Vector omega);
void setBodyPSensor(const gtsam::Pose3& pose);
Matrix getGyroscopeCovariance() const;
// TODO(frank): allow optional
// boost::optional<Vector3> getOmegaCoriolis() const;
// boost::optional<Pose3> getBodyPSensor() const;
};
#include <gtsam/navigation/PreintegrationParams.h>
class PreintegrationParams {
virtual class PreintegrationParams : gtsam::PreintegratedRotationParams {
PreintegrationParams(Vector n_gravity);
// TODO(frank): add setters/getters or make this MATLAB wrapper feature
void setAccelerometerCovariance(Matrix cov);
void setIntegrationCovariance(Matrix cov);
void setUse2ndOrderCoriolis(bool flag);
Matrix getAccelerometerCovariance() const;
Matrix getIntegrationCovariance() const;
bool getUse2ndOrderCoriolis() const;
};
#include <gtsam/navigation/PreintegrationBase.h>

4
gtsam/3rdparty/CMakeLists.txt vendored
View File

@ -43,7 +43,9 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN)
endif()
option(GTSAM_BUILD_METIS_EXECUTABLES "Build metis library executables" OFF)
add_subdirectory(metis)
if(GTSAM_SUPPORT_NESTED_DISSECTION)
add_subdirectory(metis)
endif()
add_subdirectory(ceres)

4
gtsam/3rdparty/Eigen/.hg_archival.txt vendored
View File

@ -1,4 +1,4 @@
repo: 8a21fd850624c931e448cbcfb38168cb2717c790
node: b30b87236a1b1552af32ac34075ee5696a9b5a33
node: 07105f7124f9aef00a68c85e0fc606e65d3d6c15
branch: 3.2
tag: 3.2.7
tag: 3.2.8

1
gtsam/3rdparty/Eigen/.hgtags vendored
View File

@ -30,3 +30,4 @@ ffa86ffb557094721ca71dcea6aed2651b9fd610 3.2.0
10219c95fe653d4962aa9db4946f6fbea96dd740 3.2.4
bdd17ee3b1b3a166cd5ec36dcad4fc1f3faf774a 3.2.5
c58038c56923e0fd86de3ded18e03df442e66dfb 3.2.6
b30b87236a1b1552af32ac34075ee5696a9b5a33 3.2.7

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

@ -1,6 +1,5 @@
project(Eigen)
cmake_minimum_required(VERSION 2.8.2)
cmake_minimum_required(VERSION 2.8.5)
# guard against in-source builds
@ -55,6 +54,7 @@ endif(EIGEN_HG_CHANGESET)
include(CheckCXXCompilerFlag)
include(GNUInstallDirs)
set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
@ -288,25 +288,26 @@ option(EIGEN_TEST_C++0x "Enables all C++0x features." OFF)
include_directories(${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_BINARY_DIR})
# the user modifiable install path for header files
set(EIGEN_INCLUDE_INSTALL_DIR ${EIGEN_INCLUDE_INSTALL_DIR} CACHE PATH "The directory where we install the header files (optional)")
# set the internal install path for header files which depends on wether the user modifiable
# EIGEN_INCLUDE_INSTALL_DIR has been set by the user or not.
if(EIGEN_INCLUDE_INSTALL_DIR)
set(INCLUDE_INSTALL_DIR
${EIGEN_INCLUDE_INSTALL_DIR}
CACHE INTERNAL
"The directory where we install the header files (internal)"
)
# Backward compatibility support for EIGEN_INCLUDE_INSTALL_DIR
if(EIGEN_INCLUDE_INSTALL_DIR AND NOT INCLUDE_INSTALL_DIR)
set(INCLUDE_INSTALL_DIR ${EIGEN_INCLUDE_INSTALL_DIR}
CACHE PATH "The directory relative to CMAKE_PREFIX_PATH where Eigen header files are installed")
else()
set(INCLUDE_INSTALL_DIR
"include/eigen3"
CACHE INTERNAL
"The directory where we install the header files (internal)"
)
"${CMAKE_INSTALL_INCLUDEDIR}/eigen3"
CACHE PATH "The directory relative to CMAKE_PREFIX_PATH where Eigen header files are installed"
)
endif()
set(CMAKEPACKAGE_INSTALL_DIR
"${CMAKE_INSTALL_LIBDIR}/cmake/eigen3"
CACHE PATH "The directory relative to CMAKE_PREFIX_PATH where Eigen3Config.cmake is installed"
)
set(PKGCONFIG_INSTALL_DIR
"${CMAKE_INSTALL_DATADIR}/pkgconfig"
CACHE PATH "The directory relative to CMAKE_PREFIX_PATH where eigen3.pc is installed"
)
# similar to set_target_properties but append the property instead of overwriting it
macro(ei_add_target_property target prop value)
@ -324,21 +325,9 @@ install(FILES
)
if(EIGEN_BUILD_PKGCONFIG)
SET(path_separator ":")
STRING(REPLACE ${path_separator} ";" pkg_config_libdir_search "$ENV{PKG_CONFIG_LIBDIR}")
message(STATUS "searching for 'pkgconfig' directory in PKG_CONFIG_LIBDIR ( $ENV{PKG_CONFIG_LIBDIR} ), ${CMAKE_INSTALL_PREFIX}/share, and ${CMAKE_INSTALL_PREFIX}/lib")
FIND_PATH(pkg_config_libdir pkgconfig ${pkg_config_libdir_search} ${CMAKE_INSTALL_PREFIX}/share ${CMAKE_INSTALL_PREFIX}/lib ${pkg_config_libdir_search})
if(pkg_config_libdir)
SET(pkg_config_install_dir ${pkg_config_libdir})
message(STATUS "found ${pkg_config_libdir}/pkgconfig" )
else(pkg_config_libdir)
SET(pkg_config_install_dir ${CMAKE_INSTALL_PREFIX}/share)
message(STATUS "pkgconfig not found; installing in ${pkg_config_install_dir}" )
endif(pkg_config_libdir)
configure_file(eigen3.pc.in eigen3.pc)
configure_file(eigen3.pc.in eigen3.pc @ONLY)
install(FILES ${CMAKE_CURRENT_BINARY_DIR}/eigen3.pc
DESTINATION ${pkg_config_install_dir}/pkgconfig
DESTINATION ${PKGCONFIG_INSTALL_DIR}
)
endif(EIGEN_BUILD_PKGCONFIG)
@ -401,12 +390,15 @@ if(cmake_generator_tolower MATCHES "makefile")
message(STATUS "--------------+--------------------------------------------------------------")
message(STATUS "Command | Description")
message(STATUS "--------------+--------------------------------------------------------------")
message(STATUS "make install | Install to ${CMAKE_INSTALL_PREFIX}. To change that:")
message(STATUS " | cmake . -DCMAKE_INSTALL_PREFIX=yourpath")
message(STATUS " | Eigen headers will then be installed to:")
message(STATUS " | ${CMAKE_INSTALL_PREFIX}/${INCLUDE_INSTALL_DIR}")
message(STATUS " | To install Eigen headers to a separate location, do:")
message(STATUS " | cmake . -DEIGEN_INCLUDE_INSTALL_DIR=yourpath")
message(STATUS "make install | Install Eigen. Headers will be installed to:")
message(STATUS " | <CMAKE_INSTALL_PREFIX>/<INCLUDE_INSTALL_DIR>")
message(STATUS " | Using the following values:")
message(STATUS " | CMAKE_INSTALL_PREFIX: ${CMAKE_INSTALL_PREFIX}")
message(STATUS " | INCLUDE_INSTALL_DIR: ${INCLUDE_INSTALL_DIR}")
message(STATUS " | Change the install location of Eigen headers using:")
message(STATUS " | cmake . -DCMAKE_INSTALL_PREFIX=yourprefix")
message(STATUS " | Or:")
message(STATUS " | cmake . -DINCLUDE_INSTALL_DIR=yourdir")
message(STATUS "make doc | Generate the API documentation, requires Doxygen & LaTeX")
message(STATUS "make check | Build and run the unit-tests. Read this page:")
message(STATUS " | http://eigen.tuxfamily.org/index.php?title=Tests")

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

@ -12,7 +12,7 @@ extern "C" {
/** \ingroup Support_modules
* \defgroup CholmodSupport_Module CholmodSupport module
*
* This module provides an interface to the Cholmod library which is part of the <a href="http://www.cise.ufl.edu/research/sparse/SuiteSparse/">suitesparse</a> package.
* This module provides an interface to the Cholmod library which is part of the <a href="http://www.suitesparse.com">suitesparse</a> package.
* It provides the two following main factorization classes:
* - class CholmodSupernodalLLT: a supernodal LLT Cholesky factorization.
* - class CholmodDecomposiiton: a general L(D)LT Cholesky factorization with automatic or explicit runtime selection of the underlying factorization method (supernodal or simplicial).

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

@ -10,7 +10,7 @@
/** \ingroup Support_modules
* \defgroup SPQRSupport_Module SuiteSparseQR module
*
* This module provides an interface to the SPQR library, which is part of the <a href="http://www.cise.ufl.edu/research/sparse/SuiteSparse/">suitesparse</a> package.
* This module provides an interface to the SPQR library, which is part of the <a href="http://www.suitesparse.com">suitesparse</a> package.
*
* \code
* #include <Eigen/SPQRSupport>

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

@ -12,7 +12,7 @@ extern "C" {
/** \ingroup Support_modules
* \defgroup UmfPackSupport_Module UmfPackSupport module
*
* This module provides an interface to the UmfPack library which is part of the <a href="http://www.cise.ufl.edu/research/sparse/SuiteSparse/">suitesparse</a> package.
* This module provides an interface to the UmfPack library which is part of the <a href="http://www.suitesparse.com">suitesparse</a> package.
* It provides the following factorization class:
* - class UmfPackLU: a multifrontal sequential LU factorization.
*

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

@ -38,7 +38,7 @@ struct traits<CwiseUnaryView<ViewOp, MatrixType> >
typedef typename remove_all<MatrixTypeNested>::type _MatrixTypeNested;
enum {
Flags = (traits<_MatrixTypeNested>::Flags & (HereditaryBits | LvalueBit | LinearAccessBit | DirectAccessBit)),
CoeffReadCost = traits<_MatrixTypeNested>::CoeffReadCost + functor_traits<ViewOp>::Cost,
CoeffReadCost = EIGEN_ADD_COST(traits<_MatrixTypeNested>::CoeffReadCost, functor_traits<ViewOp>::Cost),
MatrixTypeInnerStride = inner_stride_at_compile_time<MatrixType>::ret,
// need to cast the sizeof's from size_t to int explicitly, otherwise:
// "error: no integral type can represent all of the enumerator values

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

@ -40,7 +40,7 @@ static inline void check_DenseIndex_is_signed() {
*/
template<typename Derived> class DenseBase
#ifndef EIGEN_PARSED_BY_DOXYGEN
: public internal::special_scalar_op_base<Derived,typename internal::traits<Derived>::Scalar,
: public internal::special_scalar_op_base<Derived, typename internal::traits<Derived>::Scalar,
typename NumTraits<typename internal::traits<Derived>::Scalar>::Real,
DenseCoeffsBase<Derived> >
#else

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

@ -425,15 +425,18 @@ template<> struct gemv_selector<OnTheRight,ColMajor,true>
ResScalar actualAlpha = alpha * LhsBlasTraits::extractScalarFactor(prod.lhs())
* RhsBlasTraits::extractScalarFactor(prod.rhs());
// make sure Dest is a compile-time vector type (bug 1166)
typedef typename conditional<Dest::IsVectorAtCompileTime, Dest, typename Dest::ColXpr>::type ActualDest;
enum {
// FIXME find a way to allow an inner stride on the result if packet_traits<Scalar>::size==1
// on, the other hand it is good for the cache to pack the vector anyways...
EvalToDestAtCompileTime = Dest::InnerStrideAtCompileTime==1,
EvalToDestAtCompileTime = (ActualDest::InnerStrideAtCompileTime==1),
ComplexByReal = (NumTraits<LhsScalar>::IsComplex) && (!NumTraits<RhsScalar>::IsComplex),
MightCannotUseDest = (Dest::InnerStrideAtCompileTime!=1) || ComplexByReal
MightCannotUseDest = (ActualDest::InnerStrideAtCompileTime!=1) || ComplexByReal
};
gemv_static_vector_if<ResScalar,Dest::SizeAtCompileTime,Dest::MaxSizeAtCompileTime,MightCannotUseDest> static_dest;
gemv_static_vector_if<ResScalar,ActualDest::SizeAtCompileTime,ActualDest::MaxSizeAtCompileTime,MightCannotUseDest> static_dest;
bool alphaIsCompatible = (!ComplexByReal) || (numext::imag(actualAlpha)==RealScalar(0));
bool evalToDest = EvalToDestAtCompileTime && alphaIsCompatible;
@ -522,7 +525,7 @@ template<> struct gemv_selector<OnTheRight,RowMajor,true>
actualLhs.rows(), actualLhs.cols(),
actualLhs.data(), actualLhs.outerStride(),
actualRhsPtr, 1,
dest.data(), dest.innerStride(),
dest.data(), dest.col(0).innerStride(), //NOTE if dest is not a vector at compile-time, then dest.innerStride() might be wrong. (bug 1166)
actualAlpha);
}
};

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

@ -149,6 +149,10 @@ template<typename Derived> class MapBase<Derived, ReadOnlyAccessors>
checkSanity();
}
#ifdef EIGEN_MAPBASE_PLUGIN
#include EIGEN_MAPBASE_PLUGIN
#endif
protected:
void checkSanity() const

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

@ -707,21 +707,21 @@ struct scalar_fuzzy_impl : scalar_fuzzy_default_impl<Scalar, NumTraits<Scalar>::
template<typename Scalar, typename OtherScalar>
inline bool isMuchSmallerThan(const Scalar& x, const OtherScalar& y,
typename NumTraits<Scalar>::Real precision = NumTraits<Scalar>::dummy_precision())
const typename NumTraits<Scalar>::Real &precision = NumTraits<Scalar>::dummy_precision())
{
return scalar_fuzzy_impl<Scalar>::template isMuchSmallerThan<OtherScalar>(x, y, precision);
}
template<typename Scalar>
inline bool isApprox(const Scalar& x, const Scalar& y,
typename NumTraits<Scalar>::Real precision = NumTraits<Scalar>::dummy_precision())
const typename NumTraits<Scalar>::Real &precision = NumTraits<Scalar>::dummy_precision())
{
return scalar_fuzzy_impl<Scalar>::isApprox(x, y, precision);
}
template<typename Scalar>
inline bool isApproxOrLessThan(const Scalar& x, const Scalar& y,
typename NumTraits<Scalar>::Real precision = NumTraits<Scalar>::dummy_precision())
const typename NumTraits<Scalar>::Real &precision = NumTraits<Scalar>::dummy_precision())
{
return scalar_fuzzy_impl<Scalar>::isApproxOrLessThan(x, y, precision);
}

8
gtsam/3rdparty/Eigen/Eigen/src/Core/SolveTriangular.h vendored
View File

@ -116,17 +116,17 @@ template<typename Lhs, typename Rhs, int Mode, int Index, int Size>
struct triangular_solver_unroller<Lhs,Rhs,Mode,Index,Size,false> {
enum {
IsLower = ((Mode&Lower)==Lower),
I = IsLower ? Index : Size - Index - 1,
S = IsLower ? 0 : I+1
RowIndex = IsLower ? Index : Size - Index - 1,
S = IsLower ? 0 : RowIndex+1
};
static void run(const Lhs& lhs, Rhs& rhs)
{
if (Index>0)
rhs.coeffRef(I) -= lhs.row(I).template segment<Index>(S).transpose()
rhs.coeffRef(RowIndex) -= lhs.row(RowIndex).template segment<Index>(S).transpose()
.cwiseProduct(rhs.template segment<Index>(S)).sum();
if(!(Mode & UnitDiag))
rhs.coeffRef(I) /= lhs.coeff(I,I);
rhs.coeffRef(RowIndex) /= lhs.coeff(RowIndex,RowIndex);
triangular_solver_unroller<Lhs,Rhs,Mode,Index+1,Size>::run(lhs,rhs);
}

7
gtsam/3rdparty/Eigen/Eigen/src/Core/Visitor.h vendored
View File

@ -76,14 +76,17 @@ template<typename Derived>
template<typename Visitor>
void DenseBase<Derived>::visit(Visitor& visitor) const
{
typedef typename internal::remove_all<typename Derived::Nested>::type ThisNested;
typename Derived::Nested thisNested(derived());
enum { unroll = SizeAtCompileTime != Dynamic
&& CoeffReadCost != Dynamic
&& (SizeAtCompileTime == 1 || internal::functor_traits<Visitor>::Cost != Dynamic)
&& SizeAtCompileTime * CoeffReadCost + (SizeAtCompileTime-1) * internal::functor_traits<Visitor>::Cost
<= EIGEN_UNROLLING_LIMIT };
return internal::visitor_impl<Visitor, Derived,
return internal::visitor_impl<Visitor, ThisNested,
unroll ? int(SizeAtCompileTime) : Dynamic
>::run(derived(), visitor);
>::run(thisNested, visitor);
}
namespace internal {

44
gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/PacketMath.h vendored
View File

@ -235,63 +235,27 @@ template<> EIGEN_STRONG_INLINE Packet4i pload<Packet4i>(const int* from) { E
return _mm_loadu_ps(from);
#endif
}
template<> EIGEN_STRONG_INLINE Packet2d ploadu<Packet2d>(const double* from) { EIGEN_DEBUG_UNALIGNED_LOAD return _mm_loadu_pd(from); }
template<> EIGEN_STRONG_INLINE Packet4i ploadu<Packet4i>(const int* from) { EIGEN_DEBUG_UNALIGNED_LOAD return _mm_loadu_si128(reinterpret_cast<const Packet4i*>(from)); }
#else
// Fast unaligned loads. Note that here we cannot directly use intrinsics: this would
// require pointer casting to incompatible pointer types and leads to invalid code
// because of the strict aliasing rule. The "dummy" stuff are required to enforce
// a correct instruction dependency.
// TODO: do the same for MSVC (ICC is compatible)
// NOTE: with the code below, MSVC's compiler crashes!
#if defined(__GNUC__) && defined(__i386__)
// bug 195: gcc/i386 emits weird x87 fldl/fstpl instructions for _mm_load_sd
#define EIGEN_AVOID_CUSTOM_UNALIGNED_LOADS 1
#elif defined(__clang__)
// bug 201: Segfaults in __mm_loadh_pd with clang 2.8
#define EIGEN_AVOID_CUSTOM_UNALIGNED_LOADS 1
#else
#define EIGEN_AVOID_CUSTOM_UNALIGNED_LOADS 0
#endif
template<> EIGEN_STRONG_INLINE Packet4f ploadu<Packet4f>(const float* from)
{
EIGEN_DEBUG_UNALIGNED_LOAD
#if EIGEN_AVOID_CUSTOM_UNALIGNED_LOADS
return _mm_loadu_ps(from);
#else
__m128d res;
res = _mm_load_sd((const double*)(from)) ;
res = _mm_loadh_pd(res, (const double*)(from+2)) ;
return _mm_castpd_ps(res);
#endif
}
#endif
template<> EIGEN_STRONG_INLINE Packet2d ploadu<Packet2d>(const double* from)
{
EIGEN_DEBUG_UNALIGNED_LOAD
#if EIGEN_AVOID_CUSTOM_UNALIGNED_LOADS
return _mm_loadu_pd(from);
#else
__m128d res;
res = _mm_load_sd(from) ;
res = _mm_loadh_pd(res,from+1);
return res;
#endif
}
template<> EIGEN_STRONG_INLINE Packet4i ploadu<Packet4i>(const int* from)
{
EIGEN_DEBUG_UNALIGNED_LOAD
#if EIGEN_AVOID_CUSTOM_UNALIGNED_LOADS
return _mm_loadu_si128(reinterpret_cast<const Packet4i*>(from));
#else
__m128d res;
res = _mm_load_sd((const double*)(from)) ;
res = _mm_loadh_pd(res, (const double*)(from+2)) ;
return _mm_castpd_si128(res);
#endif
return _mm_loadu_si128(reinterpret_cast<const __m128i*>(from));
}
#endif
template<> EIGEN_STRONG_INLINE Packet4f ploaddup<Packet4f>(const float* from)
{

8
gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrix.h vendored
View File

@ -140,8 +140,10 @@ static void run(Index rows, Index cols, Index depth,
// Release all the sub blocks B'_j of B' for the current thread,
// i.e., we simply decrement the number of users by 1
for(Index j=0; j<threads; ++j)
{
#pragma omp atomic
--(info[j].users);
info[j].users -= 1;
}
}
}
else
@ -390,13 +392,17 @@ class GeneralProduct<Lhs, Rhs, GemmProduct>
GeneralProduct(const Lhs& lhs, const Rhs& rhs) : Base(lhs,rhs)
{
#if !(defined(EIGEN_NO_STATIC_ASSERT) && defined(EIGEN_NO_DEBUG))
typedef internal::scalar_product_op<LhsScalar,RhsScalar> BinOp;
EIGEN_CHECK_BINARY_COMPATIBILIY(BinOp,LhsScalar,RhsScalar);
#endif
}
template<typename Dest> void scaleAndAddTo(Dest& dst, const Scalar& alpha) const
{
eigen_assert(dst.rows()==m_lhs.rows() && dst.cols()==m_rhs.cols());
if(m_lhs.cols()==0 || m_lhs.rows()==0 || m_rhs.cols()==0)
return;
typename internal::add_const_on_value_type<ActualLhsType>::type lhs = LhsBlasTraits::extract(m_lhs);
typename internal::add_const_on_value_type<ActualRhsType>::type rhs = RhsBlasTraits::extract(m_rhs);

4
gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularSolverMatrix.h vendored
View File

@ -115,8 +115,9 @@ EIGEN_DONT_INLINE void triangular_solve_matrix<Scalar,Index,OnTheLeft,Mode,Conju
{
// TODO write a small kernel handling this (can be shared with trsv)
Index i = IsLower ? k2+k1+k : k2-k1-k-1;
Index s = IsLower ? k2+k1 : i+1;
Index rs = actualPanelWidth - k - 1; // remaining size
Index s = TriStorageOrder==RowMajor ? (IsLower ? k2+k1 : i+1)
: IsLower ? i+1 : i-rs;
Scalar a = (Mode & UnitDiag) ? Scalar(1) : Scalar(1)/conj(tri(i,i));
for (Index j=j2; j<j2+actual_cols; ++j)
@ -133,7 +134,6 @@ EIGEN_DONT_INLINE void triangular_solve_matrix<Scalar,Index,OnTheLeft,Mode,Conju
}
else
{
Index s = IsLower ? i+1 : i-rs;
Scalar b = (other(i,j) *= a);
Scalar* r = &other(s,j);
const Scalar* l = &tri(s,i);

287
gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h vendored
View File

@ -13,23 +13,292 @@
#define EIGEN_WORLD_VERSION 3
#define EIGEN_MAJOR_VERSION 2
#define EIGEN_MINOR_VERSION 7
#define EIGEN_MINOR_VERSION 8
#define EIGEN_VERSION_AT_LEAST(x,y,z) (EIGEN_WORLD_VERSION>x || (EIGEN_WORLD_VERSION>=x && \
(EIGEN_MAJOR_VERSION>y || (EIGEN_MAJOR_VERSION>=y && \
EIGEN_MINOR_VERSION>=z))))
// Compiler identification, EIGEN_COMP_*
/// \internal EIGEN_COMP_GNUC set to 1 for all compilers compatible with GCC
#ifdef __GNUC__
#define EIGEN_COMP_GNUC 1
#else
#define EIGEN_COMP_GNUC 0
#endif
/// \internal EIGEN_COMP_CLANG set to 1 if the compiler is clang (alias for __clang__)
#if defined(__clang__)
#define EIGEN_COMP_CLANG 1
#else
#define EIGEN_COMP_CLANG 0
#endif
/// \internal EIGEN_COMP_LLVM set to 1 if the compiler backend is llvm
#if defined(__llvm__)
#define EIGEN_COMP_LLVM 1
#else
#define EIGEN_COMP_LLVM 0
#endif
/// \internal EIGEN_COMP_ICC set to __INTEL_COMPILER if the compiler is Intel compiler, 0 otherwise
#if defined(__INTEL_COMPILER)
#define EIGEN_COMP_ICC __INTEL_COMPILER
#else
#define EIGEN_COMP_ICC 0
#endif
/// \internal EIGEN_COMP_MINGW set to 1 if the compiler is mingw
#if defined(__MINGW32__)
#define EIGEN_COMP_MINGW 1
#else
#define EIGEN_COMP_MINGW 0
#endif
/// \internal EIGEN_COMP_SUNCC set to 1 if the compiler is Solaris Studio
#if defined(__SUNPRO_CC)
#define EIGEN_COMP_SUNCC 1
#else
#define EIGEN_COMP_SUNCC 0
#endif
/// \internal EIGEN_COMP_MSVC set to _MSC_VER if the compiler is Microsoft Visual C++, 0 otherwise.
#if defined(_MSC_VER)
#define EIGEN_COMP_MSVC _MSC_VER
#else
#define EIGEN_COMP_MSVC 0
#endif
/// \internal EIGEN_COMP_MSVC_STRICT set to 1 if the compiler is really Microsoft Visual C++ and not ,e.g., ICC
#if EIGEN_COMP_MSVC && !(EIGEN_COMP_ICC)
#define EIGEN_COMP_MSVC_STRICT _MSC_VER
#else
#define EIGEN_COMP_MSVC_STRICT 0
#endif
/// \internal EIGEN_COMP_IBM set to 1 if the compiler is IBM XL C++
#if defined(__IBMCPP__) || defined(__xlc__)
#define EIGEN_COMP_IBM 1
#else
#define EIGEN_COMP_IBM 0
#endif
/// \internal EIGEN_COMP_PGI set to 1 if the compiler is Portland Group Compiler
#if defined(__PGI)
#define EIGEN_COMP_PGI 1
#else
#define EIGEN_COMP_PGI 0
#endif
/// \internal EIGEN_COMP_ARM set to 1 if the compiler is ARM Compiler
#if defined(__CC_ARM) || defined(__ARMCC_VERSION)
#define EIGEN_COMP_ARM 1
#else
#define EIGEN_COMP_ARM 0
#endif
/// \internal EIGEN_GNUC_STRICT set to 1 if the compiler is really GCC and not a compatible compiler (e.g., ICC, clang, mingw, etc.)
#if EIGEN_COMP_GNUC && !(EIGEN_COMP_CLANG || EIGEN_COMP_ICC || EIGEN_COMP_MINGW || EIGEN_COMP_PGI || EIGEN_COMP_IBM || EIGEN_COMP_ARM )
#define EIGEN_COMP_GNUC_STRICT 1
#else
#define EIGEN_COMP_GNUC_STRICT 0
#endif
#if EIGEN_COMP_GNUC
#define EIGEN_GNUC_AT_LEAST(x,y) ((__GNUC__==x && __GNUC_MINOR__>=y) || __GNUC__>x)
#define EIGEN_GNUC_AT_MOST(x,y) ((__GNUC__==x && __GNUC_MINOR__<=y) || __GNUC__<x)
#define EIGEN_GNUC_AT(x,y) ( __GNUC__==x && __GNUC_MINOR__==y )
#else
#define EIGEN_GNUC_AT_LEAST(x,y) 0
#define EIGEN_GNUC_AT_MOST(x,y) 0
#define EIGEN_GNUC_AT(x,y) 0
#endif
#ifdef __GNUC__
#define EIGEN_GNUC_AT_MOST(x,y) ((__GNUC__==x && __GNUC_MINOR__<=y) || __GNUC__<x)
// FIXME: could probably be removed as we do not support gcc 3.x anymore
#if EIGEN_COMP_GNUC && (__GNUC__ <= 3)
#define EIGEN_GCC3_OR_OLDER 1
#else
#define EIGEN_GNUC_AT_MOST(x,y) 0
#define EIGEN_GCC3_OR_OLDER 0
#endif
// Architecture identification, EIGEN_ARCH_*
#if defined(__x86_64__) || defined(_M_X64) || defined(__amd64)
#define EIGEN_ARCH_x86_64 1
#else
#define EIGEN_ARCH_x86_64 0
#endif
#if defined(__i386__) || defined(_M_IX86) || defined(_X86_) || defined(__i386)
#define EIGEN_ARCH_i386 1
#else
#define EIGEN_ARCH_i386 0
#endif
#if EIGEN_ARCH_x86_64 || EIGEN_ARCH_i386
#define EIGEN_ARCH_i386_OR_x86_64 1
#else
#define EIGEN_ARCH_i386_OR_x86_64 0
#endif
/// \internal EIGEN_ARCH_ARM set to 1 if the architecture is ARM
#if defined(__arm__)
#define EIGEN_ARCH_ARM 1
#else
#define EIGEN_ARCH_ARM 0
#endif
/// \internal EIGEN_ARCH_ARM64 set to 1 if the architecture is ARM64
#if defined(__aarch64__)
#define EIGEN_ARCH_ARM64 1
#else
#define EIGEN_ARCH_ARM64 0
#endif
#if EIGEN_ARCH_ARM || EIGEN_ARCH_ARM64
#define EIGEN_ARCH_ARM_OR_ARM64 1
#else
#define EIGEN_ARCH_ARM_OR_ARM64 0
#endif
/// \internal EIGEN_ARCH_MIPS set to 1 if the architecture is MIPS
#if defined(__mips__) || defined(__mips)
#define EIGEN_ARCH_MIPS 1
#else
#define EIGEN_ARCH_MIPS 0
#endif
/// \internal EIGEN_ARCH_SPARC set to 1 if the architecture is SPARC
#if defined(__sparc__) || defined(__sparc)
#define EIGEN_ARCH_SPARC 1
#else
#define EIGEN_ARCH_SPARC 0
#endif
/// \internal EIGEN_ARCH_IA64 set to 1 if the architecture is Intel Itanium
#if defined(__ia64__)
#define EIGEN_ARCH_IA64 1
#else
#define EIGEN_ARCH_IA64 0
#endif
/// \internal EIGEN_ARCH_PPC set to 1 if the architecture is PowerPC
#if defined(__powerpc__) || defined(__ppc__) || defined(_M_PPC)
#define EIGEN_ARCH_PPC 1
#else
#define EIGEN_ARCH_PPC 0
#endif
// Operating system identification, EIGEN_OS_*
/// \internal EIGEN_OS_UNIX set to 1 if the OS is a unix variant
#if defined(__unix__) || defined(__unix)
#define EIGEN_OS_UNIX 1
#else
#define EIGEN_OS_UNIX 0
#endif
/// \internal EIGEN_OS_LINUX set to 1 if the OS is based on Linux kernel
#if defined(__linux__)
#define EIGEN_OS_LINUX 1
#else
#define EIGEN_OS_LINUX 0
#endif
/// \internal EIGEN_OS_ANDROID set to 1 if the OS is Android
// note: ANDROID is defined when using ndk_build, __ANDROID__ is defined when using a standalone toolchain.
#if defined(__ANDROID__) || defined(ANDROID)
#define EIGEN_OS_ANDROID 1
#else
#define EIGEN_OS_ANDROID 0
#endif
/// \internal EIGEN_OS_GNULINUX set to 1 if the OS is GNU Linux and not Linux-based OS (e.g., not android)
#if defined(__gnu_linux__) && !(EIGEN_OS_ANDROID)
#define EIGEN_OS_GNULINUX 1
#else
#define EIGEN_OS_GNULINUX 0
#endif
/// \internal EIGEN_OS_BSD set to 1 if the OS is a BSD variant
#if defined(__FreeBSD__) || defined(__NetBSD__) || defined(__OpenBSD__) || defined(__bsdi__) || defined(__DragonFly__)
#define EIGEN_OS_BSD 1
#else
#define EIGEN_OS_BSD 0
#endif
/// \internal EIGEN_OS_MAC set to 1 if the OS is MacOS
#if defined(__APPLE__)
#define EIGEN_OS_MAC 1
#else
#define EIGEN_OS_MAC 0
#endif
/// \internal EIGEN_OS_QNX set to 1 if the OS is QNX
#if defined(__QNX__)
#define EIGEN_OS_QNX 1
#else
#define EIGEN_OS_QNX 0
#endif
/// \internal EIGEN_OS_WIN set to 1 if the OS is Windows based
#if defined(_WIN32)
#define EIGEN_OS_WIN 1
#else
#define EIGEN_OS_WIN 0
#endif
/// \internal EIGEN_OS_WIN64 set to 1 if the OS is Windows 64bits
#if defined(_WIN64)
#define EIGEN_OS_WIN64 1
#else
#define EIGEN_OS_WIN64 0
#endif
/// \internal EIGEN_OS_WINCE set to 1 if the OS is Windows CE
#if defined(_WIN32_WCE)
#define EIGEN_OS_WINCE 1
#else
#define EIGEN_OS_WINCE 0
#endif
/// \internal EIGEN_OS_CYGWIN set to 1 if the OS is Windows/Cygwin
#if defined(__CYGWIN__)
#define EIGEN_OS_CYGWIN 1
#else
#define EIGEN_OS_CYGWIN 0
#endif
/// \internal EIGEN_OS_WIN_STRICT set to 1 if the OS is really Windows and not some variants
#if EIGEN_OS_WIN && !( EIGEN_OS_WINCE || EIGEN_OS_CYGWIN )
#define EIGEN_OS_WIN_STRICT 1
#else
#define EIGEN_OS_WIN_STRICT 0
#endif
/// \internal EIGEN_OS_SUN set to 1 if the OS is SUN
#if (defined(sun) || defined(__sun)) && !(defined(__SVR4) || defined(__svr4__))
#define EIGEN_OS_SUN 1
#else
#define EIGEN_OS_SUN 0
#endif
/// \internal EIGEN_OS_SOLARIS set to 1 if the OS is Solaris
#if (defined(sun) || defined(__sun)) && (defined(__SVR4) || defined(__svr4__))
#define EIGEN_OS_SOLARIS 1
#else
#define EIGEN_OS_SOLARIS 0
#endif
#if EIGEN_GNUC_AT_MOST(4,3) && !defined(__clang__)
// see bug 89
#define EIGEN_SAFE_TO_USE_STANDARD_ASSERT_MACRO 0
@ -37,12 +306,6 @@
#define EIGEN_SAFE_TO_USE_STANDARD_ASSERT_MACRO 1
#endif
#if defined(__GNUC__) && (__GNUC__ <= 3)
#define EIGEN_GCC3_OR_OLDER 1
#else
#define EIGEN_GCC3_OR_OLDER 0
#endif
// 16 byte alignment is only useful for vectorization. Since it affects the ABI, we need to enable
// 16 byte alignment on all platforms where vectorization might be enabled. In theory we could always
// enable alignment, but it can be a cause of problems on some platforms, so we just disable it in
@ -104,7 +367,7 @@
// Do we support r-value references?
#if (__has_feature(cxx_rvalue_references) || \
defined(__GXX_EXPERIMENTAL_CXX0X__) || \
(defined(__cplusplus) && __cplusplus >= 201103L) || \
(defined(_MSC_VER) && _MSC_VER >= 1600))
#define EIGEN_HAVE_RVALUE_REFERENCES
#endif

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

@ -26,7 +26,7 @@
#ifndef EIGEN_NO_STATIC_ASSERT
#if defined(__GXX_EXPERIMENTAL_CXX0X__) || (defined(_MSC_VER) && (_MSC_VER >= 1600))
#if __has_feature(cxx_static_assert) || (defined(__cplusplus) && __cplusplus >= 201103L) || (EIGEN_COMP_MSVC >= 1600)
// if native static_assert is enabled, let's use it
#define EIGEN_STATIC_ASSERT(X,MSG) static_assert(X,#MSG);

1
gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/ComplexSchur_MKL.h vendored
View File

@ -45,7 +45,6 @@ ComplexSchur<Matrix<EIGTYPE, Dynamic, Dynamic, EIGCOLROW> >& \
ComplexSchur<Matrix<EIGTYPE, Dynamic, Dynamic, EIGCOLROW> >::compute(const Matrix<EIGTYPE, Dynamic, Dynamic, EIGCOLROW>& matrix, bool computeU) \
{ \
typedef Matrix<EIGTYPE, Dynamic, Dynamic, EIGCOLROW> MatrixType; \
typedef MatrixType::Scalar Scalar; \
typedef MatrixType::RealScalar RealScalar; \
typedef std::complex<RealScalar> ComplexScalar; \
\

4
gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealSchur_MKL.h vendored
View File

@ -44,10 +44,6 @@ template<> inline \
RealSchur<Matrix<EIGTYPE, Dynamic, Dynamic, EIGCOLROW> >& \
RealSchur<Matrix<EIGTYPE, Dynamic, Dynamic, EIGCOLROW> >::compute(const Matrix<EIGTYPE, Dynamic, Dynamic, EIGCOLROW>& matrix, bool computeU) \
{ \
typedef Matrix<EIGTYPE, Dynamic, Dynamic, EIGCOLROW> MatrixType; \
typedef MatrixType::Scalar Scalar; \
typedef MatrixType::RealScalar RealScalar; \
\
eigen_assert(matrix.cols() == matrix.rows()); \
\
lapack_int n = matrix.cols(), sdim, info; \

7
gtsam/3rdparty/Eigen/Eigen/src/Geometry/AngleAxis.h vendored
View File

@ -83,10 +83,17 @@ public:
template<typename Derived>
inline explicit AngleAxis(const MatrixBase<Derived>& m) { *this = m; }
/** \returns the value of the rotation angle in radian */
Scalar angle() const { return m_angle; }
/** \returns a read-write reference to the stored angle in radian */
Scalar& angle() { return m_angle; }
/** \returns the rotation axis */
const Vector3& axis() const { return m_axis; }
/** \returns a read-write reference to the stored rotation axis.
*
* \warning The rotation axis must remain a \b unit vector.
*/
Vector3& axis() { return m_axis; }
/** Concatenates two rotations */

2
gtsam/3rdparty/Eigen/Eigen/src/Geometry/ParametrizedLine.h vendored
View File

@ -129,7 +129,7 @@ public:
* determined by \a prec.
*
* \sa MatrixBase::isApprox() */
bool isApprox(const ParametrizedLine& other, typename NumTraits<Scalar>::Real prec = NumTraits<Scalar>::dummy_precision()) const
bool isApprox(const ParametrizedLine& other, const typename NumTraits<Scalar>::Real& prec = NumTraits<Scalar>::dummy_precision()) const
{ return m_origin.isApprox(other.m_origin, prec) && m_direction.isApprox(other.m_direction, prec); }
protected:

49
gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h vendored
View File

@ -102,15 +102,15 @@ template<int Mode> struct transform_make_affine;
*
* However, unlike a plain matrix, the Transform class provides many features
* simplifying both its assembly and usage. In particular, it can be composed
* with any other transformations (Transform,Translation,RotationBase,Matrix)
* with any other transformations (Transform,Translation,RotationBase,DiagonalMatrix)
* and can be directly used to transform implicit homogeneous vectors. All these
* operations are handled via the operator*. For the composition of transformations,
* its principle consists to first convert the right/left hand sides of the product
* to a compatible (Dim+1)^2 matrix and then perform a pure matrix product.
* Of course, internally, operator* tries to perform the minimal number of operations
* according to the nature of each terms. Likewise, when applying the transform
* to non homogeneous vectors, the latters are automatically promoted to homogeneous
* one before doing the matrix product. The convertions to homogeneous representations
* to points, the latters are automatically promoted to homogeneous vectors
* before doing the matrix product. The conventions to homogeneous representations
* are performed as follow:
*
* \b Translation t (Dim)x(1):
@ -124,7 +124,7 @@ template<int Mode> struct transform_make_affine;
* R & 0\\
* 0\,...\,0 & 1
* \end{array} \right) \f$
*
*<!--
* \b Linear \b Matrix L (Dim)x(Dim):
* \f$ \left( \begin{array}{cc}
* L & 0\\
@ -136,14 +136,20 @@ template<int Mode> struct transform_make_affine;
* A\\
* 0\,...\,0\,1
* \end{array} \right) \f$
*-->
* \b Scaling \b DiagonalMatrix S (Dim)x(Dim):
* \f$ \left( \begin{array}{cc}
* S & 0\\
* 0\,...\,0 & 1
* \end{array} \right) \f$
*
* \b Column \b vector v (Dim)x(1):
* \b Column \b point v (Dim)x(1):
* \f$ \left( \begin{array}{c}
* v\\
* 1
* \end{array} \right) \f$
*
* \b Set \b of \b column \b vectors V1...Vn (Dim)x(n):
* \b Set \b of \b column \b points V1...Vn (Dim)x(n):
* \f$ \left( \begin{array}{ccc}
* v_1 & ... & v_n\\
* 1 & ... & 1
@ -384,26 +390,39 @@ public:
/** \returns a writable expression of the translation vector of the transformation */
inline TranslationPart translation() { return TranslationPart(m_matrix,0,Dim); }
/** \returns an expression of the product between the transform \c *this and a matrix expression \a other
/** \returns an expression of the product between the transform \c *this and a matrix expression \a other.
*
* The right hand side \a other might be either:
* \li a vector of size Dim,
* The right-hand-side \a other can be either:
* \li an homogeneous vector of size Dim+1,
* \li a set of vectors of size Dim x Dynamic,
* \li a set of homogeneous vectors of size Dim+1 x Dynamic,
* \li a linear transformation matrix of size Dim x Dim,
* \li an affine transformation matrix of size Dim x Dim+1,
* \li a set of homogeneous vectors of size Dim+1 x N,
* \li a transformation matrix of size Dim+1 x Dim+1.
*
* Moreover, if \c *this represents an affine transformation (i.e., Mode!=Projective), then \a other can also be:
* \li a point of size Dim (computes: \code this->linear() * other + this->translation()\endcode),
* \li a set of N points as a Dim x N matrix (computes: \code (this->linear() * other).colwise() + this->translation()\endcode),
*
* In all cases, the return type is a matrix or vector of same sizes as the right-hand-side \a other.
*
* If you want to interpret \a other as a linear or affine transformation, then first convert it to a Transform<> type,
* or do your own cooking.
*
* Finally, if you want to apply Affine transformations to vectors, then explicitly apply the linear part only:
* \code
* Affine3f A;
* Vector3f v1, v2;
* v2 = A.linear() * v1;
* \endcode
*
*/
// note: this function is defined here because some compilers cannot find the respective declaration
template<typename OtherDerived>
EIGEN_STRONG_INLINE const typename internal::transform_right_product_impl<Transform, OtherDerived>::ResultType
EIGEN_STRONG_INLINE const typename OtherDerived::PlainObject
operator * (const EigenBase<OtherDerived> &other) const
{ return internal::transform_right_product_impl<Transform, OtherDerived>::run(*this,other.derived()); }
/** \returns the product expression of a transformation matrix \a a times a transform \a b
*
* The left hand side \a other might be either:
* The left hand side \a other can be either:
* \li a linear transformation matrix of size Dim x Dim,
* \li an affine transformation matrix of size Dim x Dim+1,
* \li a general transformation matrix of size Dim+1 x Dim+1.

2
gtsam/3rdparty/Eigen/Eigen/src/Geometry/Translation.h vendored
View File

@ -162,7 +162,7 @@ public:
* determined by \a prec.
*
* \sa MatrixBase::isApprox() */
bool isApprox(const Translation& other, typename NumTraits<Scalar>::Real prec = NumTraits<Scalar>::dummy_precision()) const
bool isApprox(const Translation& other, const typename NumTraits<Scalar>::Real& prec = NumTraits<Scalar>::dummy_precision()) const
{ return m_coeffs.isApprox(other.m_coeffs, prec); }
};

2
gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/ConjugateGradient.h vendored
View File

@ -139,6 +139,8 @@ struct traits<ConjugateGradient<_MatrixType,_UpLo,_Preconditioner> >
* By default the iterations start with x=0 as an initial guess of the solution.
* One can control the start using the solveWithGuess() method.
*
* ConjugateGradient can also be used in a matrix-free context, see the following \link MatrixfreeSolverExample example \endlink.
*
* \sa class SimplicialCholesky, DiagonalPreconditioner, IdentityPreconditioner
*/
template< typename _MatrixType, int _UpLo, typename _Preconditioner>

2
gtsam/3rdparty/Eigen/Eigen/src/LU/FullPivLU.h vendored
View File

@ -688,7 +688,7 @@ struct solve_retval<FullPivLU<_MatrixType>, Rhs>
*/
const Index rows = dec().rows(), cols = dec().cols(),
nonzero_pivots = dec().nonzeroPivots();
nonzero_pivots = dec().rank();
eigen_assert(rhs().rows() == rows);
const Index smalldim = (std::min)(rows, cols);

2
gtsam/3rdparty/Eigen/Eigen/src/OrderingMethods/Amd.h vendored
View File

@ -8,7 +8,7 @@
NOTE: this routine has been adapted from the CSparse library:
Copyright (c) 2006, Timothy A. Davis.
http://www.cise.ufl.edu/research/sparse/CSparse
http://www.suitesparse.com
CSparse is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public

31
gtsam/3rdparty/Eigen/Eigen/src/OrderingMethods/Eigen_Colamd.h vendored
View File

@ -41,12 +41,8 @@
//
// The colamd/symamd library is available at
//
// http://www.cise.ufl.edu/research/sparse/colamd/
// http://www.suitesparse.com
// This is the http://www.cise.ufl.edu/research/sparse/colamd/colamd.h
// file. It is required by the colamd.c, colamdmex.c, and symamdmex.c
// files, and by any C code that calls the routines whose prototypes are
// listed below, or that uses the colamd/symamd definitions listed below.
#ifndef EIGEN_COLAMD_H
#define EIGEN_COLAMD_H
@ -102,9 +98,6 @@ namespace internal {
/* === Definitions ========================================================== */
/* ========================================================================== */
#define COLAMD_MAX(a,b) (((a) > (b)) ? (a) : (b))
#define COLAMD_MIN(a,b) (((a) < (b)) ? (a) : (b))
#define ONES_COMPLEMENT(r) (-(r)-1)
/* -------------------------------------------------------------------------- */
@ -516,7 +509,7 @@ static Index init_rows_cols /* returns true if OK, or false otherwise */
Col [col].start = p [col] ;
Col [col].length = p [col+1] - p [col] ;
if (Col [col].length < 0)
if ((Col [col].length) < 0) // extra parentheses to work-around gcc bug 10200
{
/* column pointers must be non-decreasing */
stats [COLAMD_STATUS] = COLAMD_ERROR_col_length_negative ;
@ -739,8 +732,8 @@ static void init_scoring
/* === Extract knobs ==================================================== */
dense_row_count = COLAMD_MAX (0, COLAMD_MIN (knobs [COLAMD_DENSE_ROW] * n_col, n_col)) ;
dense_col_count = COLAMD_MAX (0, COLAMD_MIN (knobs [COLAMD_DENSE_COL] * n_row, n_row)) ;
dense_row_count = std::max<Index>(0, (std::min)(Index(knobs [COLAMD_DENSE_ROW] * n_col), n_col)) ;
dense_col_count = std::max<Index>(0, (std::min)(Index(knobs [COLAMD_DENSE_COL] * n_row), n_row)) ;
COLAMD_DEBUG1 (("colamd: densecount: %d %d\n", dense_row_count, dense_col_count)) ;
max_deg = 0 ;
n_col2 = n_col ;
@ -804,7 +797,7 @@ static void init_scoring
else
{
/* keep track of max degree of remaining rows */
max_deg = COLAMD_MAX (max_deg, deg) ;
max_deg = (std::max)(max_deg, deg) ;
}
}
COLAMD_DEBUG1 (("colamd: Dense and null rows killed: %d\n", n_row - n_row2)) ;
@ -842,7 +835,7 @@ static void init_scoring
/* add row's external degree */
score += Row [row].shared1.degree - 1 ;
/* guard against integer overflow */
score = COLAMD_MIN (score, n_col) ;
score = (std::min)(score, n_col) ;
}
/* determine pruned column length */
col_length = (Index) (new_cp - &A [Col [c].start]) ;
@ -914,7 +907,7 @@ static void init_scoring
head [score] = c ;
/* see if this score is less than current min */
min_score = COLAMD_MIN (min_score, score) ;
min_score = (std::min)(min_score, score) ;
}
@ -1040,7 +1033,7 @@ static Index find_ordering /* return the number of garbage collections */
/* === Garbage_collection, if necessary ============================= */
needed_memory = COLAMD_MIN (pivot_col_score, n_col - k) ;
needed_memory = (std::min)(pivot_col_score, n_col - k) ;
if (pfree + needed_memory >= Alen)
{
pfree = Eigen::internal::garbage_collection (n_row, n_col, Row, Col, A, &A [pfree]) ;
@ -1099,7 +1092,7 @@ static Index find_ordering /* return the number of garbage collections */
/* clear tag on pivot column */
Col [pivot_col].shared1.thickness = pivot_col_thickness ;
max_deg = COLAMD_MAX (max_deg, pivot_row_degree) ;
max_deg = (std::max)(max_deg, pivot_row_degree) ;
/* === Kill all rows used to construct pivot row ==================== */
@ -1273,7 +1266,7 @@ static Index find_ordering /* return the number of garbage collections */
/* add set difference */
cur_score += row_mark - tag_mark ;
/* integer overflow... */
cur_score = COLAMD_MIN (cur_score, n_col) ;
cur_score = (std::min)(cur_score, n_col) ;
}
/* recompute the column's length */
@ -1386,7 +1379,7 @@ static Index find_ordering /* return the number of garbage collections */
cur_score -= Col [col].shared1.thickness ;
/* make sure score is less or equal than the max score */
cur_score = COLAMD_MIN (cur_score, max_score) ;
cur_score = (std::min)(cur_score, max_score) ;
COLAMD_ASSERT (cur_score >= 0) ;
/* store updated score */
@ -1409,7 +1402,7 @@ static Index find_ordering /* return the number of garbage collections */
head [cur_score] = col ;
/* see if this score is less than current min */
min_score = COLAMD_MIN (min_score, cur_score) ;
min_score = (std::min)(min_score, cur_score) ;
}

1
gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR_MKL.h vendored
View File

@ -49,7 +49,6 @@ ColPivHouseholderQR<Matrix<EIGTYPE, Dynamic, Dynamic, EIGCOLROW, Dynamic, Dynami
{ \
using std::abs; \
typedef Matrix<EIGTYPE, Dynamic, Dynamic, EIGCOLROW, Dynamic, Dynamic> MatrixType; \
typedef MatrixType::Scalar Scalar; \
typedef MatrixType::RealScalar RealScalar; \
Index rows = matrix.rows();\
Index cols = matrix.cols();\

2
gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h vendored
View File

@ -816,7 +816,7 @@ void JacobiSVD<MatrixType, QRPreconditioner>::allocate(Index rows, Index cols, u
if(m_cols>m_rows) m_qr_precond_morecols.allocate(*this);
if(m_rows>m_cols) m_qr_precond_morerows.allocate(*this);
if(m_cols!=m_cols) m_scaledMatrix.resize(rows,cols);
if(m_rows!=m_cols) m_scaledMatrix.resize(rows,cols);
}
template<typename MatrixType, int QRPreconditioner>

4
gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD_MKL.h vendored
View File

@ -45,8 +45,8 @@ JacobiSVD<Matrix<EIGTYPE, Dynamic, Dynamic, EIGCOLROW, Dynamic, Dynamic>, ColPiv
JacobiSVD<Matrix<EIGTYPE, Dynamic, Dynamic, EIGCOLROW, Dynamic, Dynamic>, ColPivHouseholderQRPreconditioner>::compute(const Matrix<EIGTYPE, Dynamic, Dynamic, EIGCOLROW, Dynamic, Dynamic>& matrix, unsigned int computationOptions) \
{ \
typedef Matrix<EIGTYPE, Dynamic, Dynamic, EIGCOLROW, Dynamic, Dynamic> MatrixType; \
typedef MatrixType::Scalar Scalar; \
typedef MatrixType::RealScalar RealScalar; \
/*typedef MatrixType::Scalar Scalar;*/ \
/*typedef MatrixType::RealScalar RealScalar;*/ \
allocate(matrix.rows(), matrix.cols(), computationOptions); \
\
/*const RealScalar precision = RealScalar(2) * NumTraits<Scalar>::epsilon();*/ \

6
gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseBlock.h vendored
View File

@ -364,10 +364,11 @@ public:
protected:
EIGEN_INHERIT_ASSIGNMENT_OPERATORS(BlockImpl)
typename SparseMatrixType::Nested m_matrix;
Index m_outerStart;
const internal::variable_if_dynamic<Index, OuterSize> m_outerSize;
};
//----------
@ -528,7 +529,8 @@ public:
const internal::variable_if_dynamic<Index, XprType::ColsAtCompileTime == 1 ? 0 : Dynamic> m_startCol;
const internal::variable_if_dynamic<Index, RowsAtCompileTime> m_blockRows;
const internal::variable_if_dynamic<Index, ColsAtCompileTime> m_blockCols;
private:
Index nonZeros() const;
};
} // end namespace Eigen

5
gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseCwiseBinaryOp.h vendored
View File

@ -55,10 +55,9 @@ class CwiseBinaryOpImpl<BinaryOp, Lhs, Rhs, Sparse>
EIGEN_SPARSE_PUBLIC_INTERFACE(Derived)
CwiseBinaryOpImpl()
{
typedef typename internal::traits<Lhs>::StorageKind LhsStorageKind;
typedef typename internal::traits<Rhs>::StorageKind RhsStorageKind;
EIGEN_STATIC_ASSERT((
(!internal::is_same<LhsStorageKind,RhsStorageKind>::value)
(!internal::is_same<typename internal::traits<Lhs>::StorageKind,
typename internal::traits<Rhs>::StorageKind>::value)
|| ((Lhs::Flags&RowMajorBit) == (Rhs::Flags&RowMajorBit))),
THE_STORAGE_ORDER_OF_BOTH_SIDES_MUST_MATCH);
}

6
gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseView.h vendored
View File

@ -35,9 +35,9 @@ class SparseView : public SparseMatrixBase<SparseView<MatrixType> >
public:
EIGEN_SPARSE_PUBLIC_INTERFACE(SparseView)
SparseView(const MatrixType& mat, const Scalar& m_reference = Scalar(0),
typename NumTraits<Scalar>::Real m_epsilon = NumTraits<Scalar>::dummy_precision()) :
m_matrix(mat), m_reference(m_reference), m_epsilon(m_epsilon) {}
explicit SparseView(const MatrixType& mat, const Scalar& reference = Scalar(0),
const RealScalar &epsilon = NumTraits<Scalar>::dummy_precision())
: m_matrix(mat), m_reference(reference), m_epsilon(epsilon) {}
class InnerIterator;

18
gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdDeque.h vendored
View File

@ -13,32 +13,24 @@
#include "details.h"
// Define the explicit instantiation (e.g. necessary for the Intel compiler)
#if defined(__INTEL_COMPILER) || defined(__GNUC__)
#define EIGEN_EXPLICIT_STL_DEQUE_INSTANTIATION(...) template class std::deque<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__> >;
#else
#define EIGEN_EXPLICIT_STL_DEQUE_INSTANTIATION(...)
#endif
/**
* This section contains a convenience MACRO which allows an easy specialization of
* std::deque such that for data types with alignment issues the correct allocator
* is used automatically.
*/
#define EIGEN_DEFINE_STL_DEQUE_SPECIALIZATION(...) \
EIGEN_EXPLICIT_STL_DEQUE_INSTANTIATION(__VA_ARGS__) \
namespace std \
{ \
template<typename _Ay> \
class deque<__VA_ARGS__, _Ay> \
template<> \
class deque<__VA_ARGS__, std::allocator<__VA_ARGS__> > \
: public deque<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__> > \
{ \
typedef deque<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__> > deque_base; \
public: \
typedef __VA_ARGS__ value_type; \
typedef typename deque_base::allocator_type allocator_type; \
typedef typename deque_base::size_type size_type; \
typedef typename deque_base::iterator iterator; \
typedef deque_base::allocator_type allocator_type; \
typedef deque_base::size_type size_type; \
typedef deque_base::iterator iterator; \
explicit deque(const allocator_type& a = allocator_type()) : deque_base(a) {} \
template<typename InputIterator> \
deque(InputIterator first, InputIterator last, const allocator_type& a = allocator_type()) : deque_base(first, last, a) {} \

18
gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdList.h vendored
View File

@ -12,32 +12,24 @@
#include "details.h"
// Define the explicit instantiation (e.g. necessary for the Intel compiler)
#if defined(__INTEL_COMPILER) || defined(__GNUC__)
#define EIGEN_EXPLICIT_STL_LIST_INSTANTIATION(...) template class std::list<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__> >;
#else
#define EIGEN_EXPLICIT_STL_LIST_INSTANTIATION(...)
#endif
/**
* This section contains a convenience MACRO which allows an easy specialization of
* std::list such that for data types with alignment issues the correct allocator
* is used automatically.
*/
#define EIGEN_DEFINE_STL_LIST_SPECIALIZATION(...) \
EIGEN_EXPLICIT_STL_LIST_INSTANTIATION(__VA_ARGS__) \
namespace std \
{ \
template<typename _Ay> \
class list<__VA_ARGS__, _Ay> \
template<> \
class list<__VA_ARGS__, std::allocator<__VA_ARGS__> > \
: public list<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__> > \
{ \
typedef list<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__> > list_base; \
public: \
typedef __VA_ARGS__ value_type; \
typedef typename list_base::allocator_type allocator_type; \
typedef typename list_base::size_type size_type; \
typedef typename list_base::iterator iterator; \
typedef list_base::allocator_type allocator_type; \
typedef list_base::size_type size_type; \
typedef list_base::iterator iterator; \
explicit list(const allocator_type& a = allocator_type()) : list_base(a) {} \
template<typename InputIterator> \
list(InputIterator first, InputIterator last, const allocator_type& a = allocator_type()) : list_base(first, last, a) {} \

1
gtsam/3rdparty/Eigen/doc/CMakeLists.txt vendored
View File

@ -89,6 +89,7 @@ add_dependencies(doc-unsupported-prerequisites unsupported_snippets unsupported_
add_custom_target(doc ALL
COMMAND doxygen
COMMAND doxygen Doxyfile-unsupported
COMMAND ${CMAKE_COMMAND} -E copy ${Eigen_BINARY_DIR}/doc/html/group__TopicUnalignedArrayAssert.html ${Eigen_BINARY_DIR}/doc/html/TopicUnalignedArrayAssert.html
COMMAND ${CMAKE_COMMAND} -E rename html eigen-doc
COMMAND ${CMAKE_COMMAND} -E remove eigen-doc/eigen-doc.tgz
COMMAND ${CMAKE_COMMAND} -E tar cfz eigen-doc.tgz eigen-doc

2
gtsam/3rdparty/Eigen/doc/Manual.dox vendored
View File

@ -121,6 +121,8 @@ namespace Eigen {
\ingroup Sparse_chapter */
/** \addtogroup TopicSparseSystems
\ingroup Sparse_chapter */
/** \addtogroup MatrixfreeSolverExample
\ingroup Sparse_chapter */
/** \addtogroup Sparse_Reference
\ingroup Sparse_chapter */

1
gtsam/3rdparty/Eigen/doc/PreprocessorDirectives.dox vendored
View File

@ -91,6 +91,7 @@ following macros are supported; none of them are defined by default.
- \b EIGEN_MATRIX_PLUGIN - filename of plugin for extending the Matrix class.
- \b EIGEN_MATRIXBASE_PLUGIN - filename of plugin for extending the MatrixBase class.
- \b EIGEN_PLAINOBJECTBASE_PLUGIN - filename of plugin for extending the PlainObjectBase class.
- \b EIGEN_MAPBASE_PLUGIN - filename of plugin for extending the MapBase class.
- \b EIGEN_QUATERNIONBASE_PLUGIN - filename of plugin for extending the QuaternionBase class.
- \b EIGEN_SPARSEMATRIX_PLUGIN - filename of plugin for extending the SparseMatrix class.
- \b EIGEN_SPARSEMATRIXBASE_PLUGIN - filename of plugin for extending the SparseMatrixBase class.

6
gtsam/3rdparty/Eigen/doc/SparseLinearSystems.dox vendored
View File

@ -35,17 +35,17 @@ They are summarized in the following table:
<td>Requires the <a href="http://pastix.gforge.inria.fr">PaStiX</a> package, \b CeCILL-C </td>
<td>optimized for tough problems and symmetric patterns</td></tr>
<tr><td>CholmodSupernodalLLT</td><td>\link CholmodSupport_Module CholmodSupport \endlink</td><td>Direct LLt factorization</td><td>SPD</td><td>Fill-in reducing, Leverage fast dense algebra</td>
<td>Requires the <a href="http://www.cise.ufl.edu/research/sparse/SuiteSparse/">SuiteSparse</a> package, \b GPL </td>
<td>Requires the <a href="http://www.suitesparse.com">SuiteSparse</a> package, \b GPL </td>
<td></td></tr>
<tr><td>UmfPackLU</td><td>\link UmfPackSupport_Module UmfPackSupport \endlink</td><td>Direct LU factorization</td><td>Square</td><td>Fill-in reducing, Leverage fast dense algebra</td>
<td>Requires the <a href="http://www.cise.ufl.edu/research/sparse/SuiteSparse/">SuiteSparse</a> package, \b GPL </td>
<td>Requires the <a href="http://www.suitesparse.com">SuiteSparse</a> package, \b GPL </td>
<td></td></tr>
<tr><td>SuperLU</td><td>\link SuperLUSupport_Module SuperLUSupport \endlink</td><td>Direct LU factorization</td><td>Square</td><td>Fill-in reducing, Leverage fast dense algebra</td>
<td>Requires the <a href="http://crd-legacy.lbl.gov/~xiaoye/SuperLU/">SuperLU</a> library, (BSD-like)</td>
<td></td></tr>
<tr><td>SPQR</td><td>\link SPQRSupport_Module SPQRSupport \endlink </td> <td> QR factorization </td>
<td> Any, rectangular</td><td>fill-in reducing, multithreaded, fast dense algebra</td>
<td> requires the <a href="http://www.cise.ufl.edu/research/sparse/SuiteSparse/">SuiteSparse</a> package, \b GPL </td><td>recommended for linear least-squares problems, has a rank-revealing feature</tr>
<td> requires the <a href="http://www.suitesparse.com">SuiteSparse</a> package, \b GPL </td><td>recommended for linear least-squares problems, has a rank-revealing feature</tr>
</table>
Here \c SPD means symmetric positive definite.

62
gtsam/3rdparty/Eigen/doc/SparseQuickReference.dox vendored
View File

@ -21,7 +21,7 @@ i.e either row major or column major. The default is column major. Most arithmet
<td> Resize/Reserve</td>
<td>
\code
sm1.resize(m,n); //Change sm1 to a m x n matrix.
sm1.resize(m,n); // Change sm1 to a m x n matrix.
sm1.reserve(nnz); // Allocate room for nnz nonzeros elements.
\endcode
</td>
@ -151,10 +151,10 @@ It is easy to perform arithmetic operations on sparse matrices provided that the
<td> Permutation </td>
<td>
\code
perm.indices(); // Reference to the vector of indices
perm.indices(); // Reference to the vector of indices
sm1.twistedBy(perm); // Permute rows and columns
sm2 = sm1 * perm; //Permute the columns
sm2 = perm * sm1; // Permute the columns
sm2 = sm1 * perm; // Permute the columns
sm2 = perm * sm1; // Permute the columns
\endcode
</td>
<td>
@ -181,9 +181,9 @@ sm2 = perm * sm1; // Permute the columns
\section sparseotherops Other supported operations
<table class="manual">
<tr><th>Operations</th> <th> Code </th> <th> Notes</th> </tr>
<tr><th style="min-width:initial"> Code </th> <th> Notes</th> </tr>
<tr><td colspan="2">Sub-matrices</td></tr>
<tr>
<td>Sub-matrices</td>
<td>
\code
sm1.block(startRow, startCol, rows, cols);
@ -193,25 +193,31 @@ sm2 = perm * sm1; // Permute the columns
sm1.bottomLeftCorner( rows, cols);
sm1.bottomRightCorner( rows, cols);
\endcode
</td> <td> </td>
</td><td>
Contrary to dense matrices, here <strong>all these methods are read-only</strong>.\n
See \ref TutorialSparse_SubMatrices and below for read-write sub-matrices.
</td>
</tr>
<tr>
<td> Range </td>
<tr class="alt"><td colspan="2"> Range </td></tr>
<tr class="alt">
<td>
\code
sm1.innerVector(outer);
sm1.innerVectors(start, size);
sm1.leftCols(size);
sm2.rightCols(size);
sm1.middleRows(start, numRows);
sm1.middleCols(start, numCols);
sm1.col(j);
sm1.innerVector(outer); // RW
sm1.innerVectors(start, size); // RW
sm1.leftCols(size); // RW
sm2.rightCols(size); // RO because sm2 is row-major
sm1.middleRows(start, numRows); // RO becasue sm1 is column-major
sm1.middleCols(start, numCols); // RW
sm1.col(j); // RW
\endcode
</td>
<td>A inner vector is either a row (for row-major) or a column (for column-major). As stated earlier, the evaluation can be done in a matrix with different storage order </td>
<td>
A inner vector is either a row (for row-major) or a column (for column-major).\n
As stated earlier, for a read-write sub-matrix (RW), the evaluation can be done in a matrix with different storage order.
</td>
</tr>
<tr><td colspan="2"> Triangular and selfadjoint views</td></tr>
<tr>
<td> Triangular and selfadjoint views</td>
<td>
\code
sm2 = sm1.triangularview<Lower>();
@ -222,26 +228,30 @@ sm2 = perm * sm1; // Permute the columns
\code
\endcode </td>
</tr>
<tr>
<td>Triangular solve </td>
<tr class="alt"><td colspan="2">Triangular solve </td></tr>
<tr class="alt">
<td>
\code
dv2 = sm1.triangularView<Upper>().solve(dv1);
dv2 = sm1.topLeftCorner(size, size).triangularView<Lower>().solve(dv1);
dv2 = sm1.topLeftCorner(size, size)
.triangularView<Lower>().solve(dv1);
\endcode
</td>
<td> For general sparse solve, Use any suitable module described at \ref TopicSparseSystems </td>
</tr>
<tr><td colspan="2"> Low-level API</td></tr>
<tr>
<td> Low-level API</td>
<td>
\code
sm1.valuePtr(); // Pointer to the values
sm1.innerIndextr(); // Pointer to the indices.
sm1.outerIndexPtr(); //Pointer to the beginning of each inner vector
sm1.valuePtr(); // Pointer to the values
sm1.innerIndextr(); // Pointer to the indices.
sm1.outerIndexPtr(); // Pointer to the beginning of each inner vector
\endcode
</td>
<td> If the matrix is not in compressed form, makeCompressed() should be called before. Note that these functions are mostly provided for interoperability purposes with external libraries. A better access to the values of the matrix is done by using the InnerIterator class as described in \link TutorialSparse the Tutorial Sparse \endlink section</td>
<td>
If the matrix is not in compressed form, makeCompressed() should be called before.\n
Note that these functions are mostly provided for interoperability purposes with external libraries.\n
A better access to the values of the matrix is done by using the InnerIterator class as described in \link TutorialSparse the Tutorial Sparse \endlink section</td>
</tr>
</table>
*/

24
gtsam/3rdparty/Eigen/doc/TutorialSparse.dox vendored
View File

@ -241,11 +241,11 @@ In the following \em sm denotes a sparse matrix, \em sv a sparse vector, \em dm
sm1.real() sm1.imag() -sm1 0.5*sm1
sm1+sm2 sm1-sm2 sm1.cwiseProduct(sm2)
\endcode
However, a strong restriction is that the storage orders must match. For instance, in the following example:
However, <strong>a strong restriction is that the storage orders must match</strong>. For instance, in the following example:
\code
sm4 = sm1 + sm2 + sm3;
\endcode
sm1, sm2, and sm3 must all be row-major or all column major.
sm1, sm2, and sm3 must all be row-major or all column-major.
On the other hand, there is no restriction on the target matrix sm4.
For instance, this means that for computing \f$ A^T + A \f$, the matrix \f$ A^T \f$ must be evaluated into a temporary matrix of compatible storage order:
\code
@ -307,6 +307,26 @@ sm2 = sm1.transpose() * P;
\endcode
\subsection TutorialSparse_SubMatrices Block operations
Regarding read-access, sparse matrices expose the same API than for dense matrices to access to sub-matrices such as blocks, columns, and rows. See \ref TutorialBlockOperations for a detailed introduction.
However, for performance reasons, writing to a sub-sparse-matrix is much more limited, and currently only contiguous sets of columns (resp. rows) of a column-major (resp. row-major) SparseMatrix are writable. Moreover, this information has to be known at compile-time, leaving out methods such as <tt>block(...)</tt> and <tt>corner*(...)</tt>. The available API for write-access to a SparseMatrix are summarized below:
\code
SparseMatrix<double,ColMajor> sm1;
sm1.col(j) = ...;
sm1.leftCols(ncols) = ...;
sm1.middleCols(j,ncols) = ...;
sm1.rightCols(ncols) = ...;
SparseMatrix<double,RowMajor> sm2;
sm2.row(i) = ...;
sm2.topRows(nrows) = ...;
sm2.middleRows(i,nrows) = ...;
sm2.bottomRows(nrows) = ...;
\endcode
In addition, sparse matrices expose the SparseMatrixBase::innerVector() and SparseMatrixBase::innerVectors() methods, which are aliases to the col/middleCols methods for a column-major storage, and to the row/middleRows methods for a row-major storage.
\subsection TutorialSparse_TriangularSelfadjoint Triangular and selfadjoint views
Just as with dense matrices, the triangularView() function can be used to address a triangular part of the matrix, and perform triangular solves with a dense right hand side:

15
gtsam/3rdparty/Eigen/doc/UnalignedArrayAssert.dox vendored
View File

@ -7,8 +7,8 @@ Hello! You are seeing this webpage because your program terminated on an asserti
my_program: path/to/eigen/Eigen/src/Core/DenseStorage.h:44:
Eigen::internal::matrix_array<T, Size, MatrixOptions, Align>::internal::matrix_array()
[with T = double, int Size = 2, int MatrixOptions = 2, bool Align = true]:
Assertion `(reinterpret_cast<size_t>(array) & 0xf) == 0 && "this assertion
is explained here: http://eigen.tuxfamily.org/dox/UnalignedArrayAssert.html
Assertion `(reinterpret_cast<size_t>(array) & (sizemask)) == 0 && "this assertion
is explained here: http://eigen.tuxfamily.org/dox/group__TopicUnalignedArrayAssert.html
**** READ THIS WEB PAGE !!! ****"' failed.
</pre>
@ -46,9 +46,9 @@ then you need to read this separate page: \ref TopicStructHavingEigenMembers "St
Note that here, Eigen::Vector2d is only used as an example, more generally the issue arises for all \ref TopicFixedSizeVectorizable "fixed-size vectorizable Eigen types".
\section c2 Cause 2: STL Containers
\section c2 Cause 2: STL Containers or manual memory allocation
If you use STL Containers such as std::vector, std::map, ..., with Eigen objects, or with classes containing Eigen objects, like this,
If you use STL Containers such as std::vector, std::map, ..., with %Eigen objects, or with classes containing %Eigen objects, like this,
\code
std::vector<Eigen::Matrix2f> my_vector;
@ -60,6 +60,8 @@ then you need to read this separate page: \ref TopicStlContainers "Using STL Con
Note that here, Eigen::Matrix2f is only used as an example, more generally the issue arises for all \ref TopicFixedSizeVectorizable "fixed-size vectorizable Eigen types" and \ref TopicStructHavingEigenMembers "structures having such Eigen objects as member".
The same issue will be exhibited by any classes/functions by-passing operator new to allocate memory, that is, by performing custom memory allocation followed by calls to the placement new operator. This is for instance typically the case of \c std::make_shared or \c std::allocate_shared for which is the solution is to use an \ref aligned_allocator "aligned allocator" as detailed in the \ref TopicStlContainers "solution for STL containers".
\section c3 Cause 3: Passing Eigen objects by value
If some function in your code is getting an Eigen object passed by value, like this,
@ -107,7 +109,10 @@ Two possibilities:
128-bit alignment code and thus preserves ABI compatibility, but completely disables vectorization.</li>
</ul>
For more information, see <a href="http://eigen.tuxfamily.org/index.php?title=FAQ#I_disabled_vectorization.2C_but_I.27m_still_getting_annoyed_about_alignment_issues.21">this FAQ</a>.
If you want to know why defining EIGEN_DONT_VECTORIZE does not by itself disable 128-bit alignment and the assertion, here's the explanation:
It doesn't disable the assertion, because otherwise code that runs fine without vectorization would suddenly crash when enabling vectorization.
It doesn't disable 128bit alignment, because that would mean that vectorized and non-vectorized code are not mutually ABI-compatible. This ABI compatibility is very important, even for people who develop only an in-house application, as for instance one may want to have in the same application a vectorized path and a non-vectorized path.
*/

7
gtsam/3rdparty/Eigen/eigen3.pc.in vendored
View File

@ -1,6 +1,9 @@
prefix=@CMAKE_INSTALL_PREFIX@
exec_prefix=${prefix}
Name: Eigen3
Description: A C++ template library for linear algebra: vectors, matrices, and related algorithms
Requires:
Version: ${EIGEN_VERSION_NUMBER}
Version: @EIGEN_VERSION_NUMBER@
Libs:
Cflags: -I${INCLUDE_INSTALL_DIR}
Cflags: -I${prefix}/@INCLUDE_INSTALL_DIR@

2
gtsam/3rdparty/Eigen/test/CMakeLists.txt vendored
View File

@ -202,7 +202,9 @@ ei_add_test(geo_alignedbox)
ei_add_test(stdvector)
ei_add_test(stdvector_overload)
ei_add_test(stdlist)
ei_add_test(stdlist_overload)
ei_add_test(stddeque)
ei_add_test(stddeque_overload)
ei_add_test(resize)
ei_add_test(sparse_vector)
ei_add_test(sparse_basic)

39
gtsam/3rdparty/Eigen/test/dynalloc.cpp vendored
View File

@ -87,6 +87,32 @@ template<typename T> void check_dynaligned()
delete obj;
}
template<typename T> void check_custom_new_delete()
{
{
T* t = new T;
delete t;
}
{
std::size_t N = internal::random<std::size_t>(1,10);
T* t = new T[N];
delete[] t;
}
#ifdef EIGEN_ALIGN
{
T* t = static_cast<T *>((T::operator new)(sizeof(T)));
(T::operator delete)(t, sizeof(T));
}
{
T* t = static_cast<T *>((T::operator new)(sizeof(T)));
(T::operator delete)(t);
}
#endif
}
void test_dynalloc()
{
// low level dynamic memory allocation
@ -94,7 +120,9 @@ void test_dynalloc()
CALL_SUBTEST(check_aligned_malloc());
CALL_SUBTEST(check_aligned_new());
CALL_SUBTEST(check_aligned_stack_alloc());
// check static allocation, who knows ?
#if EIGEN_ALIGN_STATICALLY
for (int i=0; i<g_repeat*100; ++i)
{
CALL_SUBTEST(check_dynaligned<Vector4f>() );
@ -102,10 +130,13 @@ void test_dynalloc()
CALL_SUBTEST(check_dynaligned<Matrix4f>() );
CALL_SUBTEST(check_dynaligned<Vector4d>() );
CALL_SUBTEST(check_dynaligned<Vector4i>() );
CALL_SUBTEST( check_custom_new_delete<Vector4f>() );
CALL_SUBTEST( check_custom_new_delete<Vector2f>() );
CALL_SUBTEST( check_custom_new_delete<Matrix4f>() );
CALL_SUBTEST( check_custom_new_delete<MatrixXi>() );
}
// check static allocation, who knows ?
#if EIGEN_ALIGN_STATICALLY
{
MyStruct foo0; VERIFY(size_t(foo0.avec.data())%ALIGNMENT==0);
MyClassA fooA; VERIFY(size_t(fooA.avec.data())%ALIGNMENT==0);

37
gtsam/3rdparty/Eigen/test/product.h vendored
View File

@ -136,9 +136,27 @@ template<typename MatrixType> void product(const MatrixType& m)
VERIFY_IS_APPROX(res.col(r).noalias() = square.adjoint() * square.col(r), (square.adjoint() * square.col(r)).eval());
VERIFY_IS_APPROX(res.col(r).noalias() = square * square.col(r), (square * square.col(r)).eval());
// vector at runtime (see bug 1166)
{
RowSquareMatrixType ref(square);
ColSquareMatrixType ref2(square2);
ref = res = square;
VERIFY_IS_APPROX(res.block(0,0,1,rows).noalias() = m1.col(0).transpose() * square.transpose(), (ref.row(0) = m1.col(0).transpose() * square.transpose()));
VERIFY_IS_APPROX(res.block(0,0,1,rows).noalias() = m1.block(0,0,rows,1).transpose() * square.transpose(), (ref.row(0) = m1.col(0).transpose() * square.transpose()));
VERIFY_IS_APPROX(res.block(0,0,1,rows).noalias() = m1.col(0).transpose() * square, (ref.row(0) = m1.col(0).transpose() * square));
VERIFY_IS_APPROX(res.block(0,0,1,rows).noalias() = m1.block(0,0,rows,1).transpose() * square, (ref.row(0) = m1.col(0).transpose() * square));
ref2 = res2 = square2;
VERIFY_IS_APPROX(res2.block(0,0,1,cols).noalias() = m1.row(0) * square2.transpose(), (ref2.row(0) = m1.row(0) * square2.transpose()));
VERIFY_IS_APPROX(res2.block(0,0,1,cols).noalias() = m1.block(0,0,1,cols) * square2.transpose(), (ref2.row(0) = m1.row(0) * square2.transpose()));
VERIFY_IS_APPROX(res2.block(0,0,1,cols).noalias() = m1.row(0) * square2, (ref2.row(0) = m1.row(0) * square2));
VERIFY_IS_APPROX(res2.block(0,0,1,cols).noalias() = m1.block(0,0,1,cols) * square2, (ref2.row(0) = m1.row(0) * square2));
}
// inner product
Scalar x = square2.row(c) * square2.col(c2);
VERIFY_IS_APPROX(x, square2.row(c).transpose().cwiseProduct(square2.col(c2)).sum());
{
Scalar x = square2.row(c) * square2.col(c2);
VERIFY_IS_APPROX(x, square2.row(c).transpose().cwiseProduct(square2.col(c2)).sum());
}
// outer product
VERIFY_IS_APPROX(m1.col(c) * m1.row(r), m1.block(0,c,rows,1) * m1.block(r,0,1,cols));
@ -146,5 +164,18 @@ template<typename MatrixType> void product(const MatrixType& m)
VERIFY_IS_APPROX(m1.block(0,c,rows,1) * m1.row(r), m1.block(0,c,rows,1) * m1.block(r,0,1,cols));
VERIFY_IS_APPROX(m1.col(c) * m1.block(r,0,1,cols), m1.block(0,c,rows,1) * m1.block(r,0,1,cols));
VERIFY_IS_APPROX(m1.leftCols(1) * m1.row(r), m1.block(0,0,rows,1) * m1.block(r,0,1,cols));
VERIFY_IS_APPROX(m1.col(c) * m1.topRows(1), m1.block(0,c,rows,1) * m1.block(0,0,1,cols));
VERIFY_IS_APPROX(m1.col(c) * m1.topRows(1), m1.block(0,c,rows,1) * m1.block(0,0,1,cols));
// Aliasing
{
ColVectorType x(cols); x.setRandom();
ColVectorType z(x);
ColVectorType y(cols); y.setZero();
ColSquareMatrixType A(cols,cols); A.setRandom();
// CwiseBinaryOp
VERIFY_IS_APPROX(x = y + A*x, A*z);
x = z;
// CwiseUnaryOp
VERIFY_IS_APPROX(x = Scalar(1.)*(A*x), A*z);
}
}

23
gtsam/3rdparty/Eigen/test/product_large.cpp vendored
View File

@ -9,6 +9,27 @@
#include "product.h"
template<typename T>
void test_aliasing()
{
int rows = internal::random<int>(1,12);
int cols = internal::random<int>(1,12);
typedef Matrix<T,Dynamic,Dynamic> MatrixType;
typedef Matrix<T,Dynamic,1> VectorType;
VectorType x(cols); x.setRandom();
VectorType z(x);
VectorType y(rows); y.setZero();
MatrixType A(rows,cols); A.setRandom();
// CwiseBinaryOp
VERIFY_IS_APPROX(x = y + A*x, A*z);
x = z;
// CwiseUnaryOp
VERIFY_IS_APPROX(x = T(1.)*(A*x), A*z);
x = z;
VERIFY_IS_APPROX(x = y+(-(A*x)), -A*z);
x = z;
}
void test_product_large()
{
for(int i = 0; i < g_repeat; i++) {
@ -17,6 +38,8 @@ void test_product_large()
CALL_SUBTEST_3( product(MatrixXi(internal::random<int>(1,EIGEN_TEST_MAX_SIZE), internal::random<int>(1,EIGEN_TEST_MAX_SIZE))) );
CALL_SUBTEST_4( product(MatrixXcf(internal::random<int>(1,EIGEN_TEST_MAX_SIZE/2), internal::random<int>(1,EIGEN_TEST_MAX_SIZE/2))) );
CALL_SUBTEST_5( product(Matrix<float,Dynamic,Dynamic,RowMajor>(internal::random<int>(1,EIGEN_TEST_MAX_SIZE), internal::random<int>(1,EIGEN_TEST_MAX_SIZE))) );
CALL_SUBTEST_1( test_aliasing<float>() );
}
#if defined EIGEN_TEST_PART_6

9
gtsam/3rdparty/Eigen/test/product_notemporary.cpp vendored
View File

@ -58,10 +58,19 @@ template<typename MatrixType> void product_notemporary(const MatrixType& m)
r1 = internal::random<Index>(8,rows-r0);
VERIFY_EVALUATION_COUNT( m3 = (m1 * m2.adjoint()), 1);
VERIFY_EVALUATION_COUNT( m3 = (m1 * m2.adjoint()).transpose(), 1);
VERIFY_EVALUATION_COUNT( m3.noalias() = m1 * m2.adjoint(), 0);
VERIFY_EVALUATION_COUNT( m3 = s1 * (m1 * m2.transpose()), 1);
VERIFY_EVALUATION_COUNT( m3 = m3 + s1 * (m1 * m2.transpose()), 1);
VERIFY_EVALUATION_COUNT( m3.noalias() = s1 * (m1 * m2.transpose()), 0);
VERIFY_EVALUATION_COUNT( m3 = m3 + (m1 * m2.adjoint()), 1);
VERIFY_EVALUATION_COUNT( m3 = m3 + (m1 * m2.adjoint()).transpose(), 1);
VERIFY_EVALUATION_COUNT( m3.noalias() = m3 + m1 * m2.transpose(), 1); // 0 in 3.3
VERIFY_EVALUATION_COUNT( m3.noalias() += m3 + m1 * m2.transpose(), 1); // 0 in 3.3
VERIFY_EVALUATION_COUNT( m3.noalias() -= m3 + m1 * m2.transpose(), 1); // 0 in 3.3
VERIFY_EVALUATION_COUNT( m3.noalias() = s1 * m1 * s2 * m2.adjoint(), 0);
VERIFY_EVALUATION_COUNT( m3.noalias() = s1 * m1 * s2 * (m1*s3+m2*s2).adjoint(), 1);
VERIFY_EVALUATION_COUNT( m3.noalias() = (s1 * m1).adjoint() * s2 * m2, 0);

13
gtsam/3rdparty/Eigen/test/ref.cpp vendored
View File

@ -34,6 +34,18 @@ inline void on_temporary_creation(int) {
// test Ref.h
// Deal with i387 extended precision
#if EIGEN_ARCH_i386 && !(EIGEN_ARCH_x86_64)
#if EIGEN_COMP_GNUC_STRICT && EIGEN_GNUC_AT_LEAST(4,4)
#pragma GCC optimize ("-ffloat-store")
#else
#undef VERIFY_IS_EQUAL
#define VERIFY_IS_EQUAL(X,Y) VERIFY_IS_APPROX(X,Y)
#endif
#endif
template<typename MatrixType> void ref_matrix(const MatrixType& m)
{
typedef typename MatrixType::Index Index;
@ -71,7 +83,6 @@ template<typename MatrixType> void ref_matrix(const MatrixType& m)
rm2 = m2.block(i,j,brows,bcols);
VERIFY_IS_EQUAL(m1, m2);
ConstRefDynMat rm3 = m1.block(i,j,brows,bcols);
m1.block(i,j,brows,bcols) *= 2;
m2.block(i,j,brows,bcols) *= 2;

5
gtsam/3rdparty/Eigen/test/visitor.cpp vendored
View File

@ -55,6 +55,11 @@ template<typename MatrixType> void matrixVisitor(const MatrixType& p)
VERIFY_IS_APPROX(maxc, eigen_maxc);
VERIFY_IS_APPROX(minc, m.minCoeff());
VERIFY_IS_APPROX(maxc, m.maxCoeff());
eigen_maxc = (m.adjoint()*m).maxCoeff(&eigen_maxrow,&eigen_maxcol);
eigen_maxc = (m.adjoint()*m).eval().maxCoeff(&maxrow,&maxcol);
VERIFY(maxrow == eigen_maxrow);
VERIFY(maxcol == eigen_maxcol);
}
template<typename VectorType> void vectorVisitor(const VectorType& w)

2
gtsam/3rdparty/Eigen/unsupported/Eigen/AlignedVector3 vendored
View File

@ -177,7 +177,7 @@ template<typename _Scalar> class AlignedVector3
}
template<typename Derived>
inline bool isApprox(const MatrixBase<Derived>& other, RealScalar eps=NumTraits<Scalar>::dummy_precision()) const
inline bool isApprox(const MatrixBase<Derived>& other, const RealScalar& eps=NumTraits<Scalar>::dummy_precision()) const
{
return m_coeffs.template head<3>().isApprox(other,eps);
}

2
gtsam/3rdparty/ceres/fixed_array.h vendored
View File

@ -33,7 +33,7 @@
#define CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_
#include <cstddef>
#include <Eigen/Core>
#include <gtsam/3rdparty/ceres/eigen.h>
#include <gtsam/3rdparty/ceres/macros.h>
#include <gtsam/3rdparty/ceres/manual_constructor.h>

2
gtsam/3rdparty/ceres/jet.h vendored
View File

@ -162,7 +162,7 @@
#include <limits>
#include <string>
#include <Eigen/Core>
#include <gtsam/3rdparty/ceres/eigen.h>
#include <gtsam/3rdparty/ceres/fpclassify.h>
namespace ceres {

6
gtsam/3rdparty/metis/CMakeLists.txt vendored
View File

@ -20,7 +20,7 @@ if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
endif()
set(GKLIB_PATH ${PROJECT_SOURCE_DIR}/GKlib CACHE PATH "path to GKlib")
set(SHARED FALSE CACHE BOOL "build a shared library")
set(METIS_SHARED TRUE CACHE BOOL "build a shared library")
if(MSVC)
set(METIS_INSTALL FALSE)
@ -29,11 +29,11 @@ else()
endif()
# Configure libmetis library.
if(SHARED)
if(METIS_SHARED)
set(METIS_LIBRARY_TYPE SHARED)
else()
set(METIS_LIBRARY_TYPE STATIC)
endif(SHARED)
endif(METIS_SHARED)
include(${GKLIB_PATH}/GKlibSystem.cmake)
# Add include directories.

4
gtsam/CMakeLists.txt
View File

@ -88,7 +88,9 @@ configure_file("${PROJECT_SOURCE_DIR}/cmake/dllexport.h.in" "dllexport.h")
list(APPEND gtsam_srcs "${PROJECT_BINARY_DIR}/gtsam/config.h" "${PROJECT_BINARY_DIR}/gtsam/dllexport.h")
install(FILES "${PROJECT_BINARY_DIR}/gtsam/config.h" "${PROJECT_BINARY_DIR}/gtsam/dllexport.h" DESTINATION include/gtsam)
list(APPEND GTSAM_ADDITIONAL_LIBRARIES metis)
if(GTSAM_SUPPORT_NESTED_DISSECTION)
list(APPEND GTSAM_ADDITIONAL_LIBRARIES metis)
endif()
# Versions
set(gtsam_version ${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH})

18
gtsam/base/Lie.h
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* 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)
@ -295,19 +295,18 @@ private:
};
/**
* Three term approximation of the Baker<EFBFBD>Campbell<EFBFBD>Hausdorff formula
* Three term approximation of the Baker-Campbell-Hausdorff formula
* In non-commutative Lie groups, when composing exp(Z) = exp(X)exp(Y)
* it is not true that Z = X+Y. Instead, Z can be calculated using the BCH
* formula: Z = X + Y + [X,Y]/2 + [X-Y,[X,Y]]/12 - [Y,[X,[X,Y]]]/24
* http://en.wikipedia.org/wiki/Baker<EFBFBD>Campbell<EFBFBD>Hausdorff_formula
* http://en.wikipedia.org/wiki/Baker-Campbell-Hausdorff_formula
*/
/// AGC: bracket() only appears in Rot3 tests, should this be used elsewhere?
template<class T>
T BCH(const T& X, const T& Y) {
static const double _2 = 1. / 2., _12 = 1. / 12., _24 = 1. / 24.;
T X_Y = bracket(X, Y);
return X + Y + _2 * X_Y + _12 * bracket(X - Y, X_Y) - _24 * bracket(Y,
bracket(X, X_Y));
return T(X + Y + _2 * X_Y + _12 * bracket(X - Y, X_Y) - _24 * bracket(Y, bracket(X, X_Y)));
}
/**
@ -328,6 +327,15 @@ T expm(const Vector& x, int K=7) {
return T(expm(xhat,K));
}
/**
* Linear interpolation between X and Y by coefficient t in [0, 1].
*/
template <typename T>
T interpolate(const T& X, const T& Y, double t) {
assert(t >= 0 && t <= 1);
return traits<T>::Compose(X, traits<T>::Expmap(t * traits<T>::Logmap(traits<T>::Between(X, Y))));
}
} // namespace gtsam
/**

8
gtsam/base/Matrix.cpp
View File

@ -189,7 +189,7 @@ void print(const Matrix& A, const string &s, ostream& stream) {
0, // flags
" ", // coeffSeparator
";\n", // rowSeparator
" \t", // rowPrefix
" \t", // rowPrefix
"", // rowSuffix
"[\n", // matPrefix
"\n ]" // matSuffix
@ -339,7 +339,7 @@ weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas) {
list<boost::tuple<Vector, double, double> > results;
Vector pseudo(m); // allocate storage for pseudo-inverse
Vector weights = reciprocal(emul(sigmas,sigmas)); // calculate weights once
Vector weights = sigmas.array().square().inverse(); // calculate weights once
// We loop over all columns, because the columns that can be eliminated
// are not necessarily contiguous. For each one, estimate the corresponding
@ -704,11 +704,9 @@ void inplace_QR(Matrix& A){
HCoeffsType hCoeffs(size);
RowVectorType temp(cols);
#ifdef GTSAM_USE_SYSTEM_EIGEN
// System-Eigen is used, and MKL is off
#if !EIGEN_VERSION_AT_LEAST(3,2,5)
Eigen::internal::householder_qr_inplace_blocked<Matrix, HCoeffsType>(A, hCoeffs, 48, temp.data());
#else
// Patched Eigen is used, and MKL is either on or off
Eigen::internal::householder_qr_inplace_blocked<Matrix, HCoeffsType>::run(A, hCoeffs, 48, temp.data());
#endif

52
gtsam/base/Matrix.h
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* 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)
@ -46,29 +46,29 @@ typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> M
// Create handy typedefs and constants for square-size matrices
// MatrixMN, MatrixN = MatrixNN, I_NxN, and Z_NxN, for M,N=1..9
#define GTSAM_MAKE_TYPEDEFS(SIZE, SUFFIX) \
typedef Eigen::Matrix<double, SIZE, SIZE> Matrix##SUFFIX; \
typedef Eigen::Matrix<double, 1, SIZE> Matrix1##SUFFIX; \
typedef Eigen::Matrix<double, 2, SIZE> Matrix2##SUFFIX; \
typedef Eigen::Matrix<double, 3, SIZE> Matrix3##SUFFIX; \
typedef Eigen::Matrix<double, 4, SIZE> Matrix4##SUFFIX; \
typedef Eigen::Matrix<double, 5, SIZE> Matrix5##SUFFIX; \
typedef Eigen::Matrix<double, 6, SIZE> Matrix6##SUFFIX; \
typedef Eigen::Matrix<double, 7, SIZE> Matrix7##SUFFIX; \
typedef Eigen::Matrix<double, 8, SIZE> Matrix8##SUFFIX; \
typedef Eigen::Matrix<double, 9, SIZE> Matrix9##SUFFIX; \
static const Eigen::MatrixBase<Matrix##SUFFIX>::IdentityReturnType I_##SUFFIX##x##SUFFIX = Matrix##SUFFIX::Identity(); \
static const Eigen::MatrixBase<Matrix##SUFFIX>::ConstantReturnType Z_##SUFFIX##x##SUFFIX = Matrix##SUFFIX::Zero();
#define GTSAM_MAKE_MATRIX_DEFS(N) \
typedef Eigen::Matrix<double, N, N> Matrix##N; \
typedef Eigen::Matrix<double, 1, N> Matrix1##N; \
typedef Eigen::Matrix<double, 2, N> Matrix2##N; \
typedef Eigen::Matrix<double, 3, N> Matrix3##N; \
typedef Eigen::Matrix<double, 4, N> Matrix4##N; \
typedef Eigen::Matrix<double, 5, N> Matrix5##N; \
typedef Eigen::Matrix<double, 6, N> Matrix6##N; \
typedef Eigen::Matrix<double, 7, N> Matrix7##N; \
typedef Eigen::Matrix<double, 8, N> Matrix8##N; \
typedef Eigen::Matrix<double, 9, N> Matrix9##N; \
static const Eigen::MatrixBase<Matrix##N>::IdentityReturnType I_##N##x##N = Matrix##N::Identity(); \
static const Eigen::MatrixBase<Matrix##N>::ConstantReturnType Z_##N##x##N = Matrix##N::Zero();
GTSAM_MAKE_TYPEDEFS(1,1);
GTSAM_MAKE_TYPEDEFS(2,2);
GTSAM_MAKE_TYPEDEFS(3,3);
GTSAM_MAKE_TYPEDEFS(4,4);
GTSAM_MAKE_TYPEDEFS(5,5);
GTSAM_MAKE_TYPEDEFS(6,6);
GTSAM_MAKE_TYPEDEFS(7,7);
GTSAM_MAKE_TYPEDEFS(8,8);
GTSAM_MAKE_TYPEDEFS(9,9);
GTSAM_MAKE_MATRIX_DEFS(1);
GTSAM_MAKE_MATRIX_DEFS(2);
GTSAM_MAKE_MATRIX_DEFS(3);
GTSAM_MAKE_MATRIX_DEFS(4);
GTSAM_MAKE_MATRIX_DEFS(5);
GTSAM_MAKE_MATRIX_DEFS(6);
GTSAM_MAKE_MATRIX_DEFS(7);
GTSAM_MAKE_MATRIX_DEFS(8);
GTSAM_MAKE_MATRIX_DEFS(9);
// Matrix expressions for accessing parts of matrices
typedef Eigen::Block<Matrix> SubMatrix;
@ -135,7 +135,7 @@ inline bool operator==(const Matrix& A, const Matrix& B) {
}
/**
* inequality
* inequality
*/
inline bool operator!=(const Matrix& A, const Matrix& B) {
return !(A==B);
@ -371,7 +371,7 @@ GTSAM_EXPORT Matrix inverse(const Matrix& A);
* m*n matrix -> m*m Q, m*n R
* @param A a matrix
* @return <Q,R> rotation matrix Q, upper triangular R
*/
*/
GTSAM_EXPORT std::pair<Matrix,Matrix> qr(const Matrix& A);
/**
@ -434,7 +434,7 @@ GTSAM_EXPORT Vector backSubstituteUpper(const Vector& b, const Matrix& U, bool u
* @param b an RHS vector
* @param unit, set true if unit triangular
* @return the solution x of L*x=b
*/
*/
GTSAM_EXPORT Vector backSubstituteLower(const Matrix& L, const Vector& b, bool unit=false);
/**

12
gtsam/base/Testable.h
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* 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)
@ -13,20 +13,20 @@
* @file Testable.h
* @brief Concept check for values that can be used in unit tests
* @author Frank Dellaert
*
*
* The necessary functions to implement for Testable are defined
* below with additional details as to the interface.
* The concept checking function will check whether or not
* the function exists in derived class and throw compile-time errors.
*
*
* print with optional string naming the object
* void print(const std::string& name) const = 0;
*
*
* equality up to tolerance
* tricky to implement, see NoiseModelFactor1 for an example
* equals is not supposed to print out *anything*, just return true|false
* bool equals(const Derived& expected, double tol) const = 0;
*
*
*/
// \callgraph
@ -123,7 +123,7 @@ namespace gtsam {
double tol_;
equals_star(double tol = 1e-9) : tol_(tol) {}
bool operator()(const boost::shared_ptr<V>& expected, const boost::shared_ptr<V>& actual) {
if (!actual || !expected) return true;
if (!actual && !expected) return true;
return actual && expected && traits<V>::Equals(*actual,*expected, tol_);
}
};

61
gtsam/base/Vector.cpp
View File

@ -42,11 +42,6 @@ bool zero(const Vector& v) {
return result;
}
/* ************************************************************************* */
Vector repeat(size_t n, double value) {
return Vector::Constant(n, value);
}
/* ************************************************************************* */
Vector delta(size_t n, size_t i, double value) {
return Vector::Unit(n, i) * value;
@ -176,28 +171,6 @@ bool linear_dependent(const Vector& vec1, const Vector& vec2, double tol) {
return flag;
}
/* ************************************************************************* */
ConstSubVector sub(const Vector &v, size_t i1, size_t i2) {
return v.segment(i1,i2-i1);
}
/* ************************************************************************* */
void subInsert(Vector& fullVector, const Vector& subVector, size_t i) {
fullVector.segment(i, subVector.size()) = subVector;
}
/* ************************************************************************* */
Vector emul(const Vector &a, const Vector &b) {
assert (b.size()==a.size());
return a.cwiseProduct(b);
}
/* ************************************************************************* */
Vector ediv(const Vector &a, const Vector &b) {
assert (b.size()==a.size());
return a.cwiseQuotient(b);
}
/* ************************************************************************* */
Vector ediv_(const Vector &a, const Vector &b) {
size_t n = a.size();
@ -210,36 +183,6 @@ Vector ediv_(const Vector &a, const Vector &b) {
return c;
}
/* ************************************************************************* */
double sum(const Vector &a) {
return a.sum();
}
/* ************************************************************************* */
double norm_2(const Vector& v) {
return v.norm();
}
/* ************************************************************************* */
Vector reciprocal(const Vector &a) {
return a.array().inverse();
}
/* ************************************************************************* */
Vector esqrt(const Vector& v) {
return v.cwiseSqrt();
}
/* ************************************************************************* */
Vector abs(const Vector& v) {
return v.cwiseAbs();
}
/* ************************************************************************* */
double max(const Vector &a) {
return a.maxCoeff();
}
/* ************************************************************************* */
// imperative version, pass in x
double houseInPlace(Vector &v) {
@ -292,7 +235,7 @@ double weightedPseudoinverse(const Vector& a, const Vector& weights,
// Basically, instead of doing a normal QR step with the weighted
// pseudoinverse, we enforce the constraint by turning
// ax + AS = b into x + (A/a)S = b/a, for the first row where a!=0
pseudo = delta(m, i, 1 / a[i]);
pseudo = delta(m, i, 1.0 / a[i]);
return inf;
}
}
@ -363,6 +306,4 @@ Vector concatVectors(size_t nrVectors, ...)
return concatVectors(vs);
}
/* ************************************************************************* */
} // namespace gtsam

129
gtsam/base/Vector.h
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* 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)
@ -20,7 +20,6 @@
#pragma once
#ifndef MKL_BLAS
#define MKL_BLAS MKL_DOMAIN_BLAS
#endif
@ -41,24 +40,29 @@ typedef Eigen::VectorXd Vector;
typedef Eigen::Matrix<double, 1, 1> Vector1;
typedef Eigen::Vector2d Vector2;
typedef Eigen::Vector3d Vector3;
typedef Eigen::Matrix<double, 4, 1> Vector4;
typedef Eigen::Matrix<double, 5, 1> Vector5;
typedef Eigen::Matrix<double, 6, 1> Vector6;
typedef Eigen::Matrix<double, 7, 1> Vector7;
typedef Eigen::Matrix<double, 8, 1> Vector8;
typedef Eigen::Matrix<double, 9, 1> Vector9;
typedef Eigen::Matrix<double, 10, 1> Vector10;
static const Eigen::MatrixBase<Vector2>::ConstantReturnType Z_2x1 = Vector2::Zero();
static const Eigen::MatrixBase<Vector3>::ConstantReturnType Z_3x1 = Vector3::Zero();
// Create handy typedefs and constants for vectors with N>3
// VectorN and Z_Nx1, for N=1..9
#define GTSAM_MAKE_VECTOR_DEFS(N) \
typedef Eigen::Matrix<double, N, 1> Vector##N; \
static const Eigen::MatrixBase<Vector##N>::ConstantReturnType Z_##N##x1 = Vector##N::Zero();
GTSAM_MAKE_VECTOR_DEFS(4);
GTSAM_MAKE_VECTOR_DEFS(5);
GTSAM_MAKE_VECTOR_DEFS(6);
GTSAM_MAKE_VECTOR_DEFS(7);
GTSAM_MAKE_VECTOR_DEFS(8);
GTSAM_MAKE_VECTOR_DEFS(9);
GTSAM_MAKE_VECTOR_DEFS(10);
GTSAM_MAKE_VECTOR_DEFS(11);
GTSAM_MAKE_VECTOR_DEFS(12);
typedef Eigen::VectorBlock<Vector> SubVector;
typedef Eigen::VectorBlock<const Vector> ConstSubVector;
/**
* Create vector initialized to a constant value
* @param n is the size of the vector
* @param value is a constant value to insert into the vector
*/
GTSAM_EXPORT Vector repeat(size_t n, double value);
/**
* Create basis vector of dimension n,
* with a constant in spot i
@ -89,7 +93,7 @@ inline Vector zero(size_t n) { return Vector::Zero(n);}
* @param n size
*/
inline Vector ones(size_t n) { return Vector::Ones(n); }
/**
* check if all zero
*/
@ -184,39 +188,6 @@ GTSAM_EXPORT bool assert_equal(const ConstSubVector& vec1, const ConstSubVector&
*/
GTSAM_EXPORT bool linear_dependent(const Vector& vec1, const Vector& vec2, double tol=1e-9);
/**
* extract subvector, slice semantics, i.e. range = [i1,i2[ excluding i2
* @param v Vector
* @param i1 first row index
* @param i2 last row index + 1
* @return subvector v(i1:i2)
*/
GTSAM_EXPORT ConstSubVector sub(const Vector &v, size_t i1, size_t i2);
/**
* Inserts a subvector into a vector IN PLACE
* @param fullVector is the vector to be changed
* @param subVector is the vector to insert
* @param i is the index where the subvector should be inserted
*/
GTSAM_EXPORT void subInsert(Vector& fullVector, const Vector& subVector, size_t i);
/**
* elementwise multiplication
* @param a first vector
* @param b second vector
* @return vector [a(i)*b(i)]
*/
GTSAM_EXPORT Vector emul(const Vector &a, const Vector &b);
/**
* elementwise division
* @param a first vector
* @param b second vector
* @return vector [a(i)/b(i)]
*/
GTSAM_EXPORT Vector ediv(const Vector &a, const Vector &b);
/**
* elementwise division, but 0/0 = 0, not inf
* @param a first vector
@ -225,49 +196,6 @@ GTSAM_EXPORT Vector ediv(const Vector &a, const Vector &b);
*/
GTSAM_EXPORT Vector ediv_(const Vector &a, const Vector &b);
/**
* sum vector elements
* @param a vector
* @return sum_i a(i)
*/
GTSAM_EXPORT double sum(const Vector &a);
/**
* Calculates L2 norm for a vector
* modeled after boost.ublas for compatibility
* @param v vector
* @return the L2 norm
*/
GTSAM_EXPORT double norm_2(const Vector& v);
/**
* Elementwise reciprocal of vector elements
* @param a vector
* @return [1/a(i)]
*/
GTSAM_EXPORT Vector reciprocal(const Vector &a);
/**
* Elementwise sqrt of vector elements
* @param v is a vector
* @return [sqrt(a(i))]
*/
GTSAM_EXPORT Vector esqrt(const Vector& v);
/**
* Absolute values of vector elements
* @param v is a vector
* @return [abs(a(i))]
*/
GTSAM_EXPORT Vector abs(const Vector& v);
/**
* Return the max element of a vector
* @param a is a vector
* @return max(a)
*/
GTSAM_EXPORT double max(const Vector &a);
/**
* Dot product
*/
@ -344,6 +272,21 @@ GTSAM_EXPORT Vector concatVectors(const std::list<Vector>& vs);
*/
GTSAM_EXPORT Vector concatVectors(size_t nrVectors, ...);
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
GTSAM_EXPORT inline Vector abs(const Vector& v){return v.cwiseAbs();}
GTSAM_EXPORT inline Vector ediv(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseQuotient(b);}
GTSAM_EXPORT inline Vector esqrt(const Vector& v) { return v.cwiseSqrt();}
GTSAM_EXPORT inline Vector emul(const Vector &a, const Vector &b) {assert (b.size()==a.size()); return a.cwiseProduct(b);}
GTSAM_EXPORT inline double max(const Vector &a){return a.maxCoeff();}
GTSAM_EXPORT inline double norm_2(const Vector& v) {return v.norm();}
GTSAM_EXPORT inline Vector reciprocal(const Vector &a) {return a.array().inverse();}
GTSAM_EXPORT inline Vector repeat(size_t n, double value) {return Vector::Constant(n, value);}
GTSAM_EXPORT inline const Vector sub(const Vector &v, size_t i1, size_t i2) {return v.segment(i1,i2-i1);}
GTSAM_EXPORT inline void subInsert(Vector& fullVector, const Vector& subVector, size_t i) {fullVector.segment(i, subVector.size()) = subVector;}
GTSAM_EXPORT inline double sum(const Vector &a){return a.sum();}
#endif
} // namespace gtsam
#include <boost/serialization/nvp.hpp>

37
gtsam/base/VectorSpace.h
View File

@ -43,7 +43,7 @@ struct VectorSpaceImpl {
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
if (H1) *H1 = Jacobian::Identity();
if (H2) *H2 = Jacobian::Identity();
return origin + (Class)v;
return origin + v;
}
/// @}
@ -117,7 +117,7 @@ struct VectorSpaceImpl<Class,Eigen::Dynamic> {
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
if (H1) *H1 = Eye(origin);
if (H2) *H2 = Eye(origin);
return origin + Class(v);
return origin + v;
}
/// @}
@ -159,12 +159,34 @@ struct VectorSpaceImpl<Class,Eigen::Dynamic> {
/// @}
};
/// A helper that implements the traits interface for GTSAM vector space types.
/// To use this for your gtsam type, define:
/// template<> struct traits<Type> : public VectorSpaceTraits<Type> { };
/// Requirements on type to pass it to Manifold template below
template<class Class>
struct HasVectorSpacePrereqs {
enum { dim = Class::dimension };
Class p, q;
Vector v;
BOOST_CONCEPT_USAGE(HasVectorSpacePrereqs) {
p = Class::identity(); // identity
q = p + p; // addition
q = p - p; // subtraction
v = p.vector(); // conversion to vector
q = p + v; // addition of a vector on the right
}
};
/// A helper that implements the traits interface for *classes* that define vector spaces
/// To use this for your class, define:
/// template<> struct traits<Class> : public VectorSpaceTraits<Class> {};
/// The class needs to support the requirements defined by HasVectorSpacePrereqs above
template<class Class>
struct VectorSpaceTraits: VectorSpaceImpl<Class, Class::dimension> {
// Check that Class has the necessary machinery
BOOST_CONCEPT_ASSERT((HasVectorSpacePrereqs<Class>));
typedef vector_space_tag structure_category;
/// @name Group
@ -180,12 +202,11 @@ struct VectorSpaceTraits: VectorSpaceImpl<Class, Class::dimension> {
/// @}
};
/// Both VectorSpaceTRaits and Testable
/// VectorSpace provides both Testable and VectorSpaceTraits
template<class Class>
struct VectorSpace: Testable<Class>, VectorSpaceTraits<Class> {};
/// A helper that implements the traits interface for GTSAM lie groups.
/// To use this for your gtsam type, define:
/// A helper that implements the traits interface for scalar vector spaces. Usage:
/// template<> struct traits<Type> : public ScalarTraits<Type> { };
template<typename Scalar>
struct ScalarTraits : VectorSpaceImpl<Scalar, 1> {

18
gtsam/base/VerticalBlockMatrix.h
View File

@ -165,24 +165,24 @@ namespace gtsam {
return variableColOffsets_[actualBlock];
}
/** Get the apparent first row of the underlying matrix for all operations */
const DenseIndex& rowStart() const { return rowStart_; }
/** Get or set the apparent first row of the underlying matrix for all operations */
DenseIndex& rowStart() { return rowStart_; }
/** Get the apparent last row (exclusive, i.e. rows() == rowEnd() - rowStart()) of the underlying matrix for all operations */
const DenseIndex& rowEnd() const { return rowEnd_; }
/** Get or set the apparent last row (exclusive, i.e. rows() == rowEnd() - rowStart()) of the underlying matrix for all operations */
DenseIndex& rowEnd() { return rowEnd_; }
/** Get the apparent first block for all operations */
const DenseIndex& firstBlock() const { return blockStart_; }
/** Get or set the apparent first block for all operations */
DenseIndex& firstBlock() { return blockStart_; }
/** Get the apparent first row of the underlying matrix for all operations */
DenseIndex rowStart() const { return rowStart_; }
/** Get the apparent last row (exclusive, i.e. rows() == rowEnd() - rowStart()) of the underlying matrix for all operations */
DenseIndex rowEnd() const { return rowEnd_; }
/** Get the apparent first block for all operations */
DenseIndex firstBlock() const { return blockStart_; }
/** Access to full matrix (*including* any portions excluded by rowStart(), rowEnd(), and firstBlock()) */
const Matrix& matrix() const { return matrix_; }

14
gtsam/base/numericalDerivative.h
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* 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)
@ -131,11 +131,11 @@ typename internal::FixedSizeMatrix<Y,X>::type numericalDerivative11(boost::funct
typedef typename TraitsX::TangentVector TangentX;
// get value at x, and corresponding chart
Y hx = h(x);
const Y hx = h(x);
// Bit of a hack for now to find number of rows
TangentY zeroY = TraitsY::Local(hx, hx);
size_t m = zeroY.size();
const TangentY zeroY = TraitsY::Local(hx, hx);
const size_t m = zeroY.size();
// Prepare a tangent vector to perturb x with, only works for fixed size
TangentX dx;
@ -143,12 +143,12 @@ typename internal::FixedSizeMatrix<Y,X>::type numericalDerivative11(boost::funct
// Fill in Jacobian H
Matrix H = zeros(m, N);
double factor = 1.0 / (2.0 * delta);
const double factor = 1.0 / (2.0 * delta);
for (int j = 0; j < N; j++) {
dx(j) = delta;
TangentY dy1 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx)));
const TangentY dy1 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx)));
dx(j) = -delta;
TangentY dy2 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx)));
const TangentY dy2 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx)));
dx(j) = 0;
H.col(j) << (dy1 - dy2) * factor;
}

27
gtsam/base/serializationTestHelpers.h
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* 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)
@ -11,7 +11,7 @@
/**
* @file serializationTestHelpers.h
* @brief
* @brief
* @author Alex Cunningham
* @author Richard Roberts
* @date Feb 7, 2012
@ -30,6 +30,11 @@ const bool verbose = false;
namespace gtsam {
namespace serializationTestHelpers {
// templated default object creation so we only need to declare one friend (if applicable)
template<class T>
T create() {
return T();
}
// Templated round-trip serialization
template<class T>
@ -44,7 +49,7 @@ void roundtrip(const T& input, T& output) {
// This version requires equality operator
template<class T>
bool equality(const T& input = T()) {
T output;
T output = create<T>();
roundtrip<T>(input,output);
return input==output;
}
@ -52,7 +57,7 @@ bool equality(const T& input = T()) {
// This version requires Testable
template<class T>
bool equalsObj(const T& input = T()) {
T output;
T output = create<T>();
roundtrip<T>(input,output);
return assert_equal(input, output);
}
@ -60,7 +65,7 @@ bool equalsObj(const T& input = T()) {
// De-referenced version for pointers, requires equals method
template<class T>
bool equalsDereferenced(const T& input) {
T output;
T output = create<T>();
roundtrip<T>(input,output);
return input->equals(*output);
}
@ -79,7 +84,7 @@ void roundtripXML(const T& input, T& output) {
// This version requires equality operator
template<class T>
bool equalityXML(const T& input = T()) {
T output;
T output = create<T>();
roundtripXML<T>(input,output);
return input==output;
}
@ -87,7 +92,7 @@ bool equalityXML(const T& input = T()) {
// This version requires Testable
template<class T>
bool equalsXML(const T& input = T()) {
T output;
T output = create<T>();
roundtripXML<T>(input,output);
return assert_equal(input, output);
}
@ -95,7 +100,7 @@ bool equalsXML(const T& input = T()) {
// This version is for pointers, requires equals method
template<class T>
bool equalsDereferencedXML(const T& input = T()) {
T output;
T output = create<T>();
roundtripXML<T>(input,output);
return input->equals(*output);
}
@ -114,7 +119,7 @@ void roundtripBinary(const T& input, T& output) {
// This version requires equality operator
template<class T>
bool equalityBinary(const T& input = T()) {
T output;
T output = create<T>();
roundtripBinary<T>(input,output);
return input==output;
}
@ -122,7 +127,7 @@ bool equalityBinary(const T& input = T()) {
// This version requires Testable
template<class T>
bool equalsBinary(const T& input = T()) {
T output;
T output = create<T>();
roundtripBinary<T>(input,output);
return assert_equal(input, output);
}
@ -130,7 +135,7 @@ bool equalsBinary(const T& input = T()) {
// This version is for pointers, requires equals method
template<class T>
bool equalsDereferencedBinary(const T& input = T()) {
T output;
T output = create<T>();
roundtripBinary<T>(input,output);
return input->equals(*output);
}

6
gtsam/base/tests/testFastContainers.cpp
View File

@ -22,11 +22,11 @@ using namespace gtsam;
/* ************************************************************************* */
TEST( testFastContainers, KeySet ) {
FastVector<Key> init_vector;
KeyVector init_vector;
init_vector += 2, 3, 4, 5;
FastSet<size_t> actSet(init_vector);
FastSet<size_t> expSet; expSet += 2, 3, 4, 5;
KeySet actSet(init_vector);
KeySet expSet; expSet += 2, 3, 4, 5;
EXPECT(actSet == expSet);
}

55
gtsam/base/tests/testVector.cpp
View File

@ -119,36 +119,6 @@ TEST(Vector, negate )
EXPECT(assert_equal(b, -a));
}
/* ************************************************************************* */
TEST(Vector, sub )
{
Vector a(6);
a(0) = 10; a(1) = 20; a(2) = 3;
a(3) = 34; a(4) = 11; a(5) = 2;
Vector result(sub(a,2,5));
Vector b(3);
b(0) = 3; b(1) = 34; b(2) =11;
EXPECT(b==result);
EXPECT(assert_equal(b, result));
}
/* ************************************************************************* */
TEST(Vector, subInsert )
{
Vector big = zero(6),
small = ones(3);
size_t i = 2;
subInsert(big, small, i);
Vector expected = (Vector(6) << 0.0, 0.0, 1.0, 1.0, 1.0, 0.0).finished();
EXPECT(assert_equal(expected, big));
}
/* ************************************************************************* */
TEST(Vector, householder )
{
@ -198,7 +168,7 @@ TEST(Vector, weightedPseudoinverse )
// create sigmas
Vector sigmas(2);
sigmas(0) = 0.1; sigmas(1) = 0.2;
Vector weights = reciprocal(emul(sigmas,sigmas));
Vector weights = sigmas.array().square().inverse();
// perform solve
Vector actual; double precision;
@ -224,8 +194,7 @@ TEST(Vector, weightedPseudoinverse_constraint )
// create sigmas
Vector sigmas(2);
sigmas(0) = 0.0; sigmas(1) = 0.2;
Vector weights = reciprocal(emul(sigmas,sigmas));
Vector weights = sigmas.array().square().inverse();
// perform solve
Vector actual; double precision;
boost::tie(actual, precision) = weightedPseudoinverse(x, weights);
@ -244,7 +213,7 @@ TEST(Vector, weightedPseudoinverse_nan )
{
Vector a = (Vector(4) << 1., 0., 0., 0.).finished();
Vector sigmas = (Vector(4) << 0.1, 0.1, 0., 0.).finished();
Vector weights = reciprocal(emul(sigmas,sigmas));
Vector weights = sigmas.array().square().inverse();
Vector pseudo; double precision;
boost::tie(pseudo, precision) = weightedPseudoinverse(a, weights);
@ -253,17 +222,6 @@ TEST(Vector, weightedPseudoinverse_nan )
DOUBLES_EQUAL(100, precision, 1e-5);
}
/* ************************************************************************* */
TEST(Vector, ediv )
{
Vector a = Vector3(10., 20., 30.);
Vector b = Vector3(2.0, 5.0, 6.0);
Vector actual(ediv(a,b));
Vector c = Vector3(5.0, 4.0, 5.0);
EXPECT(assert_equal(c,actual));
}
/* ************************************************************************* */
TEST(Vector, dot )
{
@ -303,13 +261,6 @@ TEST(Vector, greater_than )
EXPECT(greaterThanOrEqual(v1, v2)); // test equals
}
/* ************************************************************************* */
TEST(Vector, reciprocal )
{
Vector v = Vector3(1.0, 2.0, 4.0);
EXPECT(assert_equal(Vector3(1.0, 0.5, 0.25),reciprocal(v)));
}
/* ************************************************************************* */
TEST(Vector, linear_dependent )
{

5
gtsam/base/types.h
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* 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)
@ -25,6 +25,7 @@
#include <gtsam/config.h> // for GTSAM_USE_TBB
#include <cstddef>
#include <cstdint>
#ifdef GTSAM_USE_TBB
#include <tbb/task_scheduler_init.h>
@ -53,7 +54,7 @@
namespace gtsam {
/// Integer nonlinear key type
typedef size_t Key;
typedef std::uint64_t Key;
/// The index type for Eigen objects
typedef ptrdiff_t DenseIndex;

13
gtsam/config.h.in
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* 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)
@ -32,11 +32,11 @@
#cmakedefine GTSAM_USE_QUATERNIONS
// Whether GTSAM is compiled to use Pose3::EXPMAP as the default coordinates mode for Pose3's retract and localCoordinates (otherwise, Pose3::FIRST_ORDER will be used)
#cmakedefine GTSAM_POSE3_EXPMAP
#cmakedefine GTSAM_POSE3_EXPMAP
// Whether GTSAM is compiled to use Rot3::EXPMAP as the default coordinates mode for Rot3's retract and localCoordinates (otherwise, Pose3::CAYLEY will be used)
#ifndef GTSAM_USE_QUATERNIONS
#cmakedefine GTSAM_ROT3_EXPMAP
#cmakedefine GTSAM_ROT3_EXPMAP
#endif
// Whether we are using TBB (if TBB was found and GTSAM_WITH_TBB is enabled in CMake)
@ -60,4 +60,11 @@
// Option for not throwing the CheiralityException for points that are behind a camera
#cmakedefine GTSAM_THROW_CHEIRALITY_EXCEPTION
// Make sure dependent projects that want it can see deprecated functions
#cmakedefine GTSAM_ALLOW_DEPRECATED_SINCE_V4
// Publish flag about Eigen typedef
#cmakedefine GTSAM_USE_VECTOR3_POINTS
// Support Metis-based nested dissection
#cmakedefine GTSAM_SUPPORT_NESTED_DISSECTION

10
gtsam/discrete/tests/testAlgebraicDecisionTree.cpp
View File

@ -421,16 +421,16 @@ TEST(ADT, conversion)
ADT fDiscreteKey(X & Y, "0.2 0.5 0.3 0.6");
dot(fDiscreteKey, "conversion-f1");
std::map<size_t, size_t> ordering;
ordering[0] = 5;
ordering[1] = 2;
std::map<Key, Key> keyMap;
keyMap[0] = 5;
keyMap[1] = 2;
AlgebraicDecisionTree<size_t> fIndexKey(fDiscreteKey, ordering);
AlgebraicDecisionTree<Key> fIndexKey(fDiscreteKey, keyMap);
// f1.print("f1");
// f2.print("f2");
dot(fIndexKey, "conversion-f2");
Assignment<size_t> x00, x01, x02, x10, x11, x12;
Assignment<Key> x00, x01, x02, x10, x11, x12;
x00[5] = 0, x00[2] = 0;
x01[5] = 0, x01[2] = 1;
x10[5] = 1, x10[2] = 0;

5
gtsam/geometry/Cal3_S2.h
View File

@ -95,6 +95,11 @@ public:
return fy_;
}
/// aspect ratio
inline double aspectRatio() const {
return fx_/fy_;
}
/// skew
inline double skew() const {
return s_;

6
gtsam/geometry/CameraSet.h
View File

@ -220,17 +220,15 @@ public:
static SymmetricBlockMatrix SchurComplement(const FBlocks& Fblocks,
const Matrix& E, const Vector& b, const double lambda = 0.0,
bool diagonalDamping = false) {
SymmetricBlockMatrix augmentedHessian;
if (E.cols() == 2) {
Matrix2 P;
ComputePointCovariance(P, E, lambda, diagonalDamping);
augmentedHessian = SchurComplement(Fblocks, E, P, b);
return SchurComplement(Fblocks, E, P, b);
} else {
Matrix3 P;
ComputePointCovariance(P, E, lambda, diagonalDamping);
augmentedHessian = SchurComplement(Fblocks, E, P, b);
return SchurComplement(Fblocks, E, P, b);
}
return augmentedHessian;
}
/**

13
gtsam/geometry/EssentialMatrix.cpp
View File

@ -12,6 +12,17 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
EssentialMatrix EssentialMatrix::FromRotationAndDirection(const Rot3& aRb, const Unit3& aTb,
OptionalJacobian<5, 3> H1,
OptionalJacobian<5, 2> H2) {
if (H1)
*H1 << I_3x3, Matrix23::Zero();
if (H2)
*H2 << Matrix32::Zero(), I_2x2;
return EssentialMatrix(aRb, aTb);
}
/* ************************************************************************* */
EssentialMatrix EssentialMatrix::FromPose3(const Pose3& aPb,
OptionalJacobian<5, 6> H) {
@ -107,7 +118,7 @@ ostream& operator <<(ostream& os, const EssentialMatrix& E) {
Rot3 R = E.rotation();
Unit3 d = E.direction();
os.precision(10);
os << R.xyz().transpose() << " " << d.point3().vector().transpose() << " ";
os << R.xyz().transpose() << " " << d.point3().transpose() << " ";
return os;
}

5
gtsam/geometry/EssentialMatrix.h
View File

@ -52,6 +52,11 @@ public:
Base(aRb, aTb), E_(direction().skew() * rotation().matrix()) {
}
/// Named constructor with derivatives
static EssentialMatrix FromRotationAndDirection(const Rot3& aRb, const Unit3& aTb,
OptionalJacobian<5, 3> H1 = boost::none,
OptionalJacobian<5, 2> H2 = boost::none);
/// Named constructor converting a Pose3 with scale to EssentialMatrix (no scale)
static EssentialMatrix FromPose3(const Pose3& _1P2_,
OptionalJacobian<5, 6> H = boost::none);

24
gtsam/geometry/OrientedPlane3.cpp
View File

@ -39,7 +39,7 @@ OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3>
Unit3 n_rotated = xr.rotation().unrotate(n_, D_rotated_plane, D_rotated_pose);
Vector3 unit_vec = n_rotated.unitVector();
double pred_d = n_.unitVector().dot(xr.translation().vector()) + d_;
double pred_d = n_.unitVector().dot(xr.translation()) + d_;
if (Hr) {
*Hr = zeros(3, 6);
@ -47,7 +47,7 @@ OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3>
Hr->block<1, 3>(2, 3) = unit_vec;
}
if (Hp) {
Vector2 hpp = n_.basis().transpose() * xr.translation().vector();
Vector2 hpp = n_.basis().transpose() * xr.translation();
*Hp = Z_3x3;
Hp->block<2, 2>(0, 0) = D_rotated_pose;
Hp->block<1, 2>(2, 0) = hpp;
@ -57,13 +57,31 @@ OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3>
return OrientedPlane3(unit_vec(0), unit_vec(1), unit_vec(2), pred_d);
}
/* ************************************************************************* */
Vector3 OrientedPlane3::error(const OrientedPlane3& plane) const {
Vector2 n_error = -n_.localCoordinates(plane.n_);
return Vector3(n_error(0), n_error(1), d_ - plane.d_);
}
/* ************************************************************************* */
Vector3 OrientedPlane3::errorVector(const OrientedPlane3& other, OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const {
Matrix22 H_n_error_this, H_n_error_other;
Vector2 n_error = n_.errorVector(other.normal(), H1 ? &H_n_error_this : 0,
H2 ? &H_n_error_other : 0);
double d_error = d_ - other.d_;
if (H1) {
*H1 << H_n_error_this, Vector2::Zero(), 0, 0, 1;
}
if (H2) {
*H2 << H_n_error_other, Vector2::Zero(), 0, 0, -1;
}
return Vector3(n_error(0), n_error(1), d_error);
}
/* ************************************************************************* */
OrientedPlane3 OrientedPlane3::retract(const Vector3& v) const {
return OrientedPlane3(n_.retract(Vector2(v(0), v(1))), d_ + v(2));

9
gtsam/geometry/OrientedPlane3.h
View File

@ -114,6 +114,15 @@ public:
*/
Vector3 error(const OrientedPlane3& plane) const;
/** Computes the error between the two planes, with derivatives.
* This uses Unit3::errorVector, as opposed to the other .error() in this class, which uses
* Unit3::localCoordinates. This one has correct derivatives.
* NOTE(hayk): The derivatives are zero when normals are exactly orthogonal.
* @param the other plane
*/
Vector3 errorVector(const OrientedPlane3& other, OptionalJacobian<3, 3> H1 = boost::none, //
OptionalJacobian<3, 3> H2 = boost::none) const;
/// Dimensionality of tangent space = 3 DOF
inline static size_t Dim() {
return 3;

5
gtsam/geometry/PinholePose.h
View File

@ -328,6 +328,11 @@ public:
virtual ~PinholePose() {
}
/// return shared pointer to calibration
const boost::shared_ptr<CALIBRATION>& sharedCalibration() const {
return K_;
}
/// return calibration
virtual const CALIBRATION& calibration() const {
return *K_;

6
gtsam/geometry/Point2.cpp
View File

@ -140,4 +140,10 @@ ostream &operator<<(ostream &os, const Point2& p) {
return os;
}
/* ************************************************************************* */
ostream &operator<<(ostream &os, const gtsam::Point2Pair &p) {
os << p.first << " <-> " << p.second;
return os;
}
} // namespace gtsam

11
gtsam/geometry/Point2.h
View File

@ -50,7 +50,7 @@ public:
/// @{
/// construct from 2D vector
Point2(const Vector2& v) {
explicit Point2(const Vector2& v) {
x_ = v(0);
y_ = v(1);
}
@ -112,6 +112,11 @@ public:
/// inverse
inline Point2 operator- () const {return Point2(-x_,-y_);}
/// add vector on right
inline Point2 operator +(const Vector2& v) const {
return Point2(x_ + v[0], y_ + v[1]);
}
/// add
inline Point2 operator + (const Point2& q) const {return Point2(x_+q.x_,y_+q.y_);}
@ -194,6 +199,10 @@ private:
/// @}
};
// Convenience typedef
typedef std::pair<Point2, Point2> Point2Pair;
std::ostream &operator<<(std::ostream &os, const gtsam::Point2Pair &p);
// For MATLAB wrapper
typedef std::vector<Point2> Point2Vector;

148
gtsam/geometry/Point3.cpp
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* 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)
@ -15,66 +15,54 @@
*/
#include <gtsam/geometry/Point3.h>
#include <cmath>
using namespace std;
namespace gtsam {
/* ************************************************************************* */
bool Point3::equals(const Point3 & q, double tol) const {
return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol
&& fabs(z_ - q.z()) < tol);
#ifndef GTSAM_USE_VECTOR3_POINTS
bool Point3::equals(const Point3 &q, double tol) const {
return (fabs(x() - q.x()) < tol && fabs(y() - q.y()) < tol &&
fabs(z() - q.z()) < tol);
}
/* ************************************************************************* */
void Point3::print(const string& s) const {
cout << s << *this << endl;
}
/* ************************************************************************* */
bool Point3::operator==(const Point3& q) const {
return x_ == q.x_ && y_ == q.y_ && z_ == q.z_;
}
/* ************************************************************************* */
Point3 Point3::operator+(const Point3& q) const {
return Point3(x_ + q.x_, y_ + q.y_, z_ + q.z_);
}
/* ************************************************************************* */
Point3 Point3::operator-(const Point3& q) const {
return Point3(x_ - q.x_, y_ - q.y_, z_ - q.z_);
}
/* ************************************************************************* */
Point3 Point3::operator*(double s) const {
return Point3(x_ * s, y_ * s, z_ * s);
}
/* ************************************************************************* */
Point3 Point3::operator/(double s) const {
return Point3(x_ / s, y_ / s, z_ / s);
}
/* ************************************************************************* */
double Point3::distance(const Point3 &p2, OptionalJacobian<1, 3> H1,
double Point3::distance(const Point3 &q, OptionalJacobian<1, 3> H1,
OptionalJacobian<1, 3> H2) const {
double d = (p2 - *this).norm();
if (H1) {
*H1 << x_ - p2.x(), y_ - p2.y(), z_ - p2.z();
*H1 = *H1 *(1. / d);
}
return gtsam::distance(*this,q,H1,H2);
}
if (H2) {
*H2 << -x_ + p2.x(), -y_ + p2.y(), -z_ + p2.z();
*H2 = *H2 *(1. / d);
}
return d;
double Point3::norm(OptionalJacobian<1,3> H) const {
return gtsam::norm(*this, H);
}
Point3 Point3::normalized(OptionalJacobian<3,3> H) const {
return gtsam::normalize(*this, H);
}
Point3 Point3::cross(const Point3 &q, OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const {
return gtsam::cross(*this, q, H1, H2);
}
double Point3::dot(const Point3 &q, OptionalJacobian<1, 3> H1,
OptionalJacobian<1, 3> H2) const {
return gtsam::dot(*this, q, H1, H2);
}
/* ************************************************************************* */
ostream &operator<<(ostream &os, const Point3& p) {
os << '[' << p.x() << ", " << p.y() << ", " << p.z() << "]\';";
return os;
}
/* ************************************************************************* */
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1,
OptionalJacobian<3,3> H2) const {
if (H1) *H1 = I_3x3;
@ -82,68 +70,66 @@ Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1,
return *this + q;
}
/* ************************************************************************* */
Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1,
OptionalJacobian<3,3> H2) const {
if (H1) *H1 = I_3x3;
if (H2) *H2 = I_3x3;
if (H2) *H2 = -I_3x3;
return *this - q;
}
#endif
#endif
/* ************************************************************************* */
Point3 Point3::cross(const Point3 &q, OptionalJacobian<3, 3> H_p, OptionalJacobian<3, 3> H_q) const {
if (H_p) {
*H_p << skewSymmetric(-q.vector());
double distance(const Point3 &p1, const Point3 &q, OptionalJacobian<1, 3> H1,
OptionalJacobian<1, 3> H2) {
double d = (q - p1).norm();
if (H1) {
*H1 << p1.x() - q.x(), p1.y() - q.y(), p1.z() - q.z();
*H1 = *H1 *(1. / d);
}
if (H_q) {
*H_q << skewSymmetric(vector());
if (H2) {
*H2 << -p1.x() + q.x(), -p1.y() + q.y(), -p1.z() + q.z();
*H2 = *H2 *(1. / d);
}
return Point3(y_ * q.z_ - z_ * q.y_, z_ * q.x_ - x_ * q.z_,
x_ * q.y_ - y_ * q.x_);
return d;
}
/* ************************************************************************* */
double Point3::dot(const Point3 &q, OptionalJacobian<1, 3> H_p, OptionalJacobian<1, 3> H_q) const {
if (H_p) {
*H_p << q.vector().transpose();
}
if (H_q) {
*H_q << vector().transpose();
}
return (x_ * q.x_ + y_ * q.y_ + z_ * q.z_);
}
/* ************************************************************************* */
double Point3::norm(OptionalJacobian<1,3> H) const {
double r = sqrt(x_ * x_ + y_ * y_ + z_ * z_);
double norm(const Point3 &p, OptionalJacobian<1, 3> H) {
double r = sqrt(p.x() * p.x() + p.y() * p.y() + p.z() * p.z());
if (H) {
if (fabs(r) > 1e-10)
*H << x_ / r, y_ / r, z_ / r;
*H << p.x() / r, p.y() / r, p.z() / r;
else
*H << 1, 1, 1; // really infinity, why 1 ?
*H << 1, 1, 1; // really infinity, why 1 ?
}
return r;
}
/* ************************************************************************* */
Point3 Point3::normalize(OptionalJacobian<3,3> H) const {
Point3 normalized = *this / norm();
Point3 normalize(const Point3 &p, OptionalJacobian<3, 3> H) {
Point3 normalized = p / norm(p);
if (H) {
// 3*3 Derivative
double x2 = x_ * x_, y2 = y_ * y_, z2 = z_ * z_;
double xy = x_ * y_, xz = x_ * z_, yz = y_ * z_;
*H << y2 + z2, -xy, -xz, /**/-xy, x2 + z2, -yz, /**/-xz, -yz, x2 + y2;
double x2 = p.x() * p.x(), y2 = p.y() * p.y(), z2 = p.z() * p.z();
double xy = p.x() * p.y(), xz = p.x() * p.z(), yz = p.y() * p.z();
*H << y2 + z2, -xy, -xz, /**/ -xy, x2 + z2, -yz, /**/ -xz, -yz, x2 + y2;
*H /= pow(x2 + y2 + z2, 1.5);
}
return normalized;
}
/* ************************************************************************* */
ostream &operator<<(ostream &os, const Point3& p) {
os << '[' << p.x() << ", " << p.y() << ", " << p.z() << "]\';";
return os;
Point3 cross(const Point3 &p, const Point3 &q, OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) {
if (H1) *H1 << skewSymmetric(-q.x(), -q.y(), -q.z());
if (H2) *H2 << skewSymmetric(p.x(), p.y(), p.z());
return Point3(p.y() * q.z() - p.z() * q.y(), p.z() * q.x() - p.x() * q.z(),
p.x() * q.y() - p.y() * q.x());
}
double dot(const Point3 &p, const Point3 &q, OptionalJacobian<1, 3> H1,
OptionalJacobian<1, 3> H2) {
if (H1) *H1 << q.x(), q.y(), q.z();
if (H2) *H2 << p.x(), p.y(), p.z();
return p.x() * q.x() + p.y() * q.y() + p.z() * q.z();
}
/* ************************************************************************* */

156
gtsam/geometry/Point3.h
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* 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)
@ -21,46 +21,42 @@
#pragma once
#include <gtsam/config.h>
#include <gtsam/base/VectorSpace.h>
#include <gtsam/base/Vector.h>
#include <gtsam/dllexport.h>
#include <boost/serialization/nvp.hpp>
#include <cmath>
namespace gtsam {
/**
* A 3D point
#ifdef GTSAM_USE_VECTOR3_POINTS
/// As of GTSAM 4, in order to make GTSAM more lean,
/// it is now possible to just typedef Point3 to Vector3
typedef Vector3 Point3;
#else
/**
* A 3D point is just a Vector3 with some additional methods
* @addtogroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT Point3 {
private:
double x_, y_, z_;
class GTSAM_EXPORT Point3 : public Vector3 {
public:
enum { dimension = 3 };
/// @name Standard Constructors
/// @{
/// Default constructor creates a zero-Point3
Point3(): x_(0), y_(0), z_(0) {}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
// Deprecated default constructor initializes to zero, in contrast to new behavior below
Point3() { setZero(); }
#endif
/// Construct from x, y, and z coordinates
Point3(double x, double y, double z): x_(x), y_(y), z_(z) {}
/// @}
/// @name Advanced Constructors
/// @{
/// Construct from 3-element vector
explicit Point3(const Vector3& v) {
x_ = v(0);
y_ = v(1);
z_ = v(2);
}
using Vector3::Vector3;
/// @}
/// @name Testable
@ -77,46 +73,21 @@ namespace gtsam {
/// @{
/// identity for group operation
inline static Point3 identity() { return Point3();}
/// inverse
Point3 operator - () const { return Point3(-x_,-y_,-z_);}
/// add vector on right
inline Point3 operator +(const Vector3& v) const {
return Point3(x_ + v[0], y_ + v[1], z_ + v[2]);
}
/// add
Point3 operator + (const Point3& q) const;
/// subtract
Point3 operator - (const Point3& q) const;
inline static Point3 identity() { return Point3(0.0, 0.0, 0.0); }
/// @}
/// @name Vector Space
/// @{
///multiply with a scalar
Point3 operator * (double s) const;
///divide by a scalar
Point3 operator / (double s) const;
/** distance between two points */
double distance(const Point3& p2, OptionalJacobian<1, 3> H1 = boost::none,
OptionalJacobian<1, 3> H2 = boost::none) const;
/** @deprecated The following function has been deprecated, use distance above */
inline double dist(const Point3& p2) const {
return (p2 - *this).norm();
}
/** Distance of the point from the origin, with Jacobian */
double norm(OptionalJacobian<1,3> H = boost::none) const;
/** normalize, with optional Jacobian */
Point3 normalize(OptionalJacobian<3, 3> H = boost::none) const;
Point3 normalized(OptionalJacobian<3, 3> H = boost::none) const;
/** cross product @return this x q */
Point3 cross(const Point3 &q, OptionalJacobian<3, 3> H_p = boost::none, //
@ -130,34 +101,24 @@ namespace gtsam {
/// @name Standard Interface
/// @{
/// equality
bool operator ==(const Point3& q) const;
/** return vectorized form (column-wise)*/
Vector3 vector() const { return Vector3(x_,y_,z_); }
/// return as Vector3
const Vector3& vector() const { return *this; }
/// get x
inline double x() const {return x_;}
inline double x() const {return (*this)[0];}
/// get y
inline double y() const {return y_;}
inline double y() const {return (*this)[1];}
/// get z
inline double z() const {return z_;}
/** add two points, add(this,q) is same as this + q */
Point3 add (const Point3 &q,
OptionalJacobian<3, 3> H1=boost::none, OptionalJacobian<3, 3> H2=boost::none) const;
/** subtract two points, sub(this,q) is same as this - q */
Point3 sub (const Point3 &q,
OptionalJacobian<3,3> H1=boost::none, OptionalJacobian<3,3> H2=boost::none) const;
inline double z() const {return (*this)[2];}
/// @}
/// Output stream operator
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Point3& p);
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @name Deprecated
/// @{
Point3 inverse() const { return -(*this);}
@ -167,39 +128,62 @@ namespace gtsam {
Point3 retract(const Vector3& v) const { return compose(Point3(v));}
static Vector3 Logmap(const Point3& p) { return p.vector();}
static Point3 Expmap(const Vector3& v) { return Point3(v);}
/// @}
inline double dist(const Point3& q) const { return (q - *this).norm(); }
Point3 normalize(OptionalJacobian<3, 3> H = boost::none) const { return normalized(H);}
Point3 add(const Point3& q, OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const;
Point3 sub(const Point3& q, OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const;
/// @}
#endif
private:
/// @name Advanced Interface
/// @{
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/)
{
ar & BOOST_SERIALIZATION_NVP(x_);
ar & BOOST_SERIALIZATION_NVP(y_);
ar & BOOST_SERIALIZATION_NVP(z_);
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Vector3);
}
/// @}
};
// Convenience typedef
typedef std::pair<Point3, Point3> Point3Pair;
std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p);
/// Syntactic sugar for multiplying coordinates by a scalar s*p
inline Point3 operator*(double s, const Point3& p) { return p*s;}
template<>
struct traits<Point3> : public internal::VectorSpace<Point3> {};
template<>
struct traits<const Point3> : public internal::VectorSpace<Point3> {};
#endif
// Convenience typedef
typedef std::pair<Point3, Point3> Point3Pair;
std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p);
/// distance between two points
double distance(const Point3& p1, const Point3& q,
OptionalJacobian<1, 3> H1 = boost::none,
OptionalJacobian<1, 3> H2 = boost::none);
/// Distance of the point from the origin, with Jacobian
double norm(const Point3& p, OptionalJacobian<1, 3> H = boost::none);
/// normalize, with optional Jacobian
Point3 normalize(const Point3& p, OptionalJacobian<3, 3> H = boost::none);
/// cross product @return this x q
Point3 cross(const Point3& p, const Point3& q,
OptionalJacobian<3, 3> H_p = boost::none,
OptionalJacobian<3, 3> H_q = boost::none);
/// dot product
double dot(const Point3& p, const Point3& q,
OptionalJacobian<1, 3> H_p = boost::none,
OptionalJacobian<1, 3> H_q = boost::none);
// Convenience typedef
typedef std::pair<Point3, Point3> Point3Pair;
std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p);
template <typename A1, typename A2>
struct Range;
@ -209,7 +193,7 @@ struct Range<Point3, Point3> {
double operator()(const Point3& p, const Point3& q,
OptionalJacobian<1, 3> H1 = boost::none,
OptionalJacobian<1, 3> H2 = boost::none) {
return p.distance(q, H1, H2);
return distance(p, q, H1, H2);
}
};

4
gtsam/geometry/Pose2.cpp
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* 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)
@ -216,7 +216,7 @@ Point2 Pose2::transform_from(const Point2& point,
OptionalJacobian<2, 2> Htranslation = Hpose.cols<2>(0);
OptionalJacobian<2, 1> Hrotation = Hpose.cols<1>(2);
const Point2 q = r_.rotate(point, Hrotation, Hpoint);
if (Htranslation) *Htranslation = Hpoint ? *Hpoint : r_.matrix();
if (Htranslation) *Htranslation = (Hpoint ? *Hpoint : r_.matrix());
return q + t_;
}

132
gtsam/geometry/Pose3.cpp
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* 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)
@ -36,6 +36,14 @@ Pose3::Pose3(const Pose2& pose2) :
Point3(pose2.x(), pose2.y(), 0)) {
}
/* ************************************************************************* */
Pose3 Pose3::Create(const Rot3& R, const Point3& t, OptionalJacobian<6, 3> H1,
OptionalJacobian<6, 3> H2) {
if (H1) *H1 << I_3x3, Z_3x3;
if (H2) *H2 << Z_3x3, R.transpose();
return Pose3(R, t);
}
/* ************************************************************************* */
Pose3 Pose3::inverse() const {
Rot3 Rt = R_.inverse();
@ -48,8 +56,7 @@ Pose3 Pose3::inverse() const {
// Experimental - unit tests of derivatives based on it do not check out yet
Matrix6 Pose3::AdjointMap() const {
const Matrix3 R = R_.matrix();
const Vector3 t = t_.vector();
Matrix3 A = skewSymmetric(t) * R;
Matrix3 A = skewSymmetric(t_.x(), t_.y(), t_.z()) * R;
Matrix6 adj;
adj << R, Z_3x3, A, R;
return adj;
@ -101,12 +108,12 @@ Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y,
void Pose3::print(const string& s) const {
cout << s;
R_.print("R:\n");
t_.print("t: ");
cout << '[' << t_.x() << ", " << t_.y() << ", " << t_.z() << "]\';";
}
/* ************************************************************************* */
bool Pose3::equals(const Pose3& pose, double tol) const {
return R_.equals(pose.R_, tol) && t_.equals(pose.t_, tol);
return R_.equals(pose.R_, tol) && traits<Point3>::Equals(t_, pose.t_, tol);
}
/* ************************************************************************* */
@ -115,14 +122,14 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> H) {
if (H) *H = ExpmapDerivative(xi);
// get angular velocity omega and translational velocity v from twist xi
Point3 omega(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5));
Vector3 omega(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5));
Rot3 R = Rot3::Expmap(omega.vector());
Rot3 R = Rot3::Expmap(omega);
double theta2 = omega.dot(omega);
if (theta2 > std::numeric_limits<double>::epsilon()) {
Point3 t_parallel = omega * omega.dot(v); // translation parallel to axis
Point3 omega_cross_v = omega.cross(v); // points towards axis
Point3 t = (omega_cross_v - R * omega_cross_v + t_parallel) / theta2;
Vector3 t_parallel = omega * omega.dot(v); // translation parallel to axis
Vector3 omega_cross_v = omega.cross(v); // points towards axis
Vector3 t = (omega_cross_v - R * omega_cross_v + t_parallel) / theta2;
return Pose3(R, t);
} else {
return Pose3(R, v);
@ -132,19 +139,20 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> H) {
/* ************************************************************************* */
Vector6 Pose3::Logmap(const Pose3& p, OptionalJacobian<6, 6> H) {
if (H) *H = LogmapDerivative(p);
Vector3 w = Rot3::Logmap(p.rotation()), T = p.translation().vector();
double t = w.norm();
const Vector3 w = Rot3::Logmap(p.rotation());
const Vector3 T = p.translation();
const double t = w.norm();
if (t < 1e-10) {
Vector6 log;
log << w, T;
return log;
} else {
Matrix3 W = skewSymmetric(w / t);
const Matrix3 W = skewSymmetric(w / t);
// Formula from Agrawal06iros, equation (14)
// simplified with Mathematica, and multiplying in T to avoid matrix math
double Tan = tan(0.5 * t);
Vector3 WT = W * T;
Vector3 u = T - (0.5 * t) * WT + (1 - t / (2. * Tan)) * (W * WT);
const double Tan = tan(0.5 * t);
const Vector3 WT = W * T;
const Vector3 u = T - (0.5 * t) * WT + (1 - t / (2. * Tan)) * (W * WT);
Vector6 log;
log << w, u;
return log;
@ -178,7 +186,7 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& T, ChartJacobian H) {
H->topLeftCorner<3,3>() = DR;
}
Vector6 xi;
xi << omega, T.translation().vector();
xi << omega, T.translation();
return xi;
#endif
}
@ -193,8 +201,8 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& T, ChartJacobian H) {
* The closed-form formula is similar to formula 102 in Barfoot14tro)
*/
static Matrix3 computeQforExpmapDerivative(const Vector6& xi) {
const Vector3 w = xi.head<3>();
const Vector3 v = xi.tail<3>();
const auto w = xi.head<3>();
const auto v = xi.tail<3>();
const Matrix3 V = skewSymmetric(v);
const Matrix3 W = skewSymmetric(w);
@ -260,11 +268,20 @@ const Point3& Pose3::translation(OptionalJacobian<3, 6> H) const {
return t_;
}
/* ************************************************************************* */
const Rot3& Pose3::rotation(OptionalJacobian<3, 6> H) const {
if (H) {
*H << I_3x3, Z_3x3;
}
return R_;
}
/* ************************************************************************* */
Matrix4 Pose3::matrix() const {
static const Matrix14 A14 = (Matrix14() << 0.0, 0.0, 0.0, 1.0).finished();
static const auto A14 = Eigen::RowVector4d(0,0,0,1);
Matrix4 mat;
mat << R_.matrix(), t_.vector(), A14;
mat << R_.matrix(), t_, A14;
return mat;
}
@ -275,6 +292,14 @@ Pose3 Pose3::transform_to(const Pose3& pose) const {
return Pose3(cRv, t);
}
/* ************************************************************************* */
Pose3 Pose3::transform_pose_to(const Pose3& pose, OptionalJacobian<6, 6> H1,
OptionalJacobian<6, 6> H2) const {
if (H1) *H1 = -pose.inverse().AdjointMap() * AdjointMap();
if (H2) *H2 = I_6x6;
return inverse() * pose;
}
/* ************************************************************************* */
Point3 Pose3::transform_from(const Point3& p, OptionalJacobian<3,6> Dpose,
OptionalJacobian<3,3> Dpoint) const {
@ -288,7 +313,7 @@ Point3 Pose3::transform_from(const Point3& p, OptionalJacobian<3,6> Dpose,
if (Dpoint) {
*Dpoint = R;
}
return Point3(R * p.vector()) + t_;
return R_ * p + t_;
}
/* ************************************************************************* */
@ -297,7 +322,7 @@ Point3 Pose3::transform_to(const Point3& p, OptionalJacobian<3,6> Dpose,
// Only get transpose once, to avoid multiple allocations,
// as well as multiple conversions in the Quaternion case
const Matrix3 Rt = R_.transpose();
const Point3 q(Rt*(p - t_).vector());
const Point3 q(Rt*(p - t_));
if (Dpose) {
const double wx = q.x(), wy = q.y(), wz = q.z();
(*Dpose) <<
@ -321,7 +346,7 @@ double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1,
return local.norm();
} else {
Matrix13 D_r_local;
const double r = local.norm(D_r_local);
const double r = norm(local, D_r_local);
if (H1) *H1 = D_r_local * D_local_pose;
if (H2) *H2 = D_r_local * D_local_point;
return r;
@ -355,46 +380,61 @@ Unit3 Pose3::bearing(const Point3& point, OptionalJacobian<2, 6> H1,
}
/* ************************************************************************* */
boost::optional<Pose3> align(const vector<Point3Pair>& pairs) {
const size_t n = pairs.size();
boost::optional<Pose3> Pose3::Align(const std::vector<Point3Pair>& abPointPairs) {
const size_t n = abPointPairs.size();
if (n < 3)
return boost::none; // we need at least three pairs
return boost::none; // we need at least three pairs
// calculate centroids
Vector3 cp = Vector3::Zero(), cq = Vector3::Zero();
BOOST_FOREACH(const Point3Pair& pair, pairs) {
cp += pair.first.vector();
cq += pair.second.vector();
Point3 aCentroid(0,0,0), bCentroid(0,0,0);
for(const Point3Pair& abPair: abPointPairs) {
aCentroid += abPair.first;
bCentroid += abPair.second;
}
double f = 1.0 / n;
cp *= f;
cq *= f;
aCentroid *= f;
bCentroid *= f;
// Add to form H matrix
Matrix3 H = Z_3x3;
BOOST_FOREACH(const Point3Pair& pair, pairs) {
Vector3 dp = pair.first.vector() - cp;
Vector3 dq = pair.second.vector() - cq;
H += dp * dq.transpose();
}
for(const Point3Pair& abPair: abPointPairs) {
Point3 da = abPair.first - aCentroid;
Point3 db = abPair.second - bCentroid;
H += db * da.transpose();
}
// Compute SVD
Matrix U, V;
Vector S;
svd(H, U, S, V);
// Compute SVD
Eigen::JacobiSVD<Matrix> svd(H, Eigen::ComputeThinU | Eigen::ComputeThinV);
Matrix U = svd.matrixU();
Vector S = svd.singularValues();
Matrix V = svd.matrixV();
// Check rank
if (S[1] < 1e-10)
return boost::none;
// Recover transform with correction from Eggert97machinevisionandapplications
Matrix3 UVtranspose = U * V.transpose();
Matrix3 detWeighting = I_3x3;
detWeighting(2, 2) = UVtranspose.determinant();
Rot3 R(Matrix3(V * detWeighting * U.transpose()));
Point3 t = Point3(cq) - R * Point3(cp);
return Pose3(R, t);
Rot3 aRb(Matrix(V * detWeighting * U.transpose()));
Point3 aTb = Point3(aCentroid) - aRb * Point3(bCentroid);
return Pose3(aRb, aTb);
}
boost::optional<Pose3> align(const vector<Point3Pair>& baPointPairs) {
vector<Point3Pair> abPointPairs;
BOOST_FOREACH (const Point3Pair& baPair, baPointPairs) {
abPointPairs.push_back(make_pair(baPair.second, baPair.first));
}
return Pose3::Align(abPointPairs);
}
/* ************************************************************************* */
std::ostream &operator<<(std::ostream &os, const Pose3& pose) {
os << pose.rotation() << "\n" << pose.translation() << endl;
os << pose.rotation() << "\n";
const Point3& t = pose.translation();
os << '[' << t.x() << ", " << t.y() << ", " << t.z() << "]\';\n";
return os;
}

45
gtsam/geometry/Pose3.h
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* 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)
@ -52,8 +52,7 @@ public:
/// @{
/** Default constructor is origin */
Pose3() {
}
Pose3() : R_(traits<Rot3>::Identity()), t_(traits<Point3>::Identity()) {}
/** Copy constructor */
Pose3(const Pose3& pose) :
@ -65,11 +64,6 @@ public:
R_(R), t_(t) {
}
/** Construct from R,t, where t \in vector3 */
Pose3(const Rot3& R, const Vector3& t) :
R_(R), t_(t) {
}
/** Construct from Pose2 */
explicit Pose3(const Pose2& pose2);
@ -79,6 +73,19 @@ public:
T(2, 2)), t_(T(0, 3), T(1, 3), T(2, 3)) {
}
/// Named constructor with derivatives
static Pose3 Create(const Rot3& R, const Point3& t,
OptionalJacobian<6, 3> H1 = boost::none,
OptionalJacobian<6, 3> H2 = boost::none);
/**
* Create Pose3 by aligning two point pairs
* A pose aTb is estimated between pairs (a_point, b_point) such that a_point = aTb * b_point
* Meant to replace the deprecated function 'align', which orders the pairs the opposite way.
* Note this allows for noise on the points but in that case the mapping will not be exact.
*/
static boost::optional<Pose3> Align(const std::vector<Point3Pair>& abPointPairs);
/// @}
/// @name Testable
/// @{
@ -219,9 +226,7 @@ public:
/// @{
/// get rotation
const Rot3& rotation() const {
return R_;
}
const Rot3& rotation(OptionalJacobian<3, 6> H = boost::none) const;
/// get translation
const Point3& translation(OptionalJacobian<3, 6> H = boost::none) const;
@ -244,9 +249,15 @@ public:
/** convert to 4*4 matrix */
Matrix4 matrix() const;
/** receives a pose in world coordinates and transforms it to local coordinates */
/** receives a pose in local coordinates and transforms it to world coordinates
* @deprecated: This is actually equivalent to transform_from, so it is WRONG! Use
* transform_pose_to instead. */
Pose3 transform_to(const Pose3& pose) const;
/** receives a pose in world coordinates and transforms it to local coordinates */
Pose3 transform_pose_to(const Pose3& pose, OptionalJacobian<6, 6> H1 = boost::none,
OptionalJacobian<6, 6> H2 = boost::none) const;
/**
* Calculate range to a landmark
* @param point 3D location of landmark
@ -297,7 +308,7 @@ public:
GTSAM_EXPORT
friend std::ostream &operator<<(std::ostream &os, const Pose3& p);
private:
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
@ -323,11 +334,11 @@ inline Matrix wedge<Pose3>(const Vector& xi) {
}
/**
* Calculate pose between a vector of 3D point correspondences (p,q)
* where q = Pose3::transform_from(p) = t + R*p
* Calculate pose between a vector of 3D point correspondences (b_point, a_point)
* where a_point = Pose3::transform_from(b_point) = t + R*b_point
* @deprecated: use Pose3::Align with point pairs ordered the opposite way
*/
typedef std::pair<Point3, Point3> Point3Pair;
GTSAM_EXPORT boost::optional<Pose3> align(const std::vector<Point3Pair>& pairs);
GTSAM_EXPORT boost::optional<Pose3> align(const std::vector<Point3Pair>& baPointPairs);
// For MATLAB wrapper
typedef std::vector<Pose3> Pose3Vector;

61
gtsam/geometry/Rot3.cpp
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* 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)
@ -42,6 +42,58 @@ Rot3 Rot3::Random(boost::mt19937& rng) {
return AxisAngle(axis, angle);
}
/* ************************************************************************* */
Rot3 Rot3::AlignPair(const Unit3& axis, const Unit3& a_p, const Unit3& b_p) {
// if a_p is already aligned with b_p, return the identity rotation
if (std::abs(a_p.dot(b_p)) > 0.999999999) {
return Rot3();
}
// Check axis was not degenerate cross product
const Vector3 z = axis.unitVector();
if (z.hasNaN())
throw std::runtime_error("AlignSinglePair: axis has Nans");
// Now, calculate rotation that takes b_p to a_p
const Matrix3 P = I_3x3 - z * z.transpose(); // orthogonal projector
const Vector3 a_po = P * a_p.unitVector(); // point in a orthogonal to axis
const Vector3 b_po = P * b_p.unitVector(); // point in b orthogonal to axis
const Vector3 x = a_po.normalized(); // x-axis in axis-orthogonal plane, along a_p vector
const Vector3 y = z.cross(x); // y-axis in axis-orthogonal plane
const double u = x.dot(b_po); // x-coordinate for b_po
const double v = y.dot(b_po); // y-coordinate for b_po
double angle = std::atan2(v, u);
return Rot3::AxisAngle(z, -angle);
}
/* ************************************************************************* */
Rot3 Rot3::AlignTwoPairs(const Unit3& a_p, const Unit3& b_p, //
const Unit3& a_q, const Unit3& b_q) {
// there are three frames in play:
// a: the first frame in which p and q are measured
// b: the second frame in which p and q are measured
// i: intermediate, after aligning first pair
// First, find rotation around that aligns a_p and b_p
Rot3 i_R_b = AlignPair(a_p.cross(b_p), a_p, b_p);
// Rotate points in frame b to the intermediate frame,
// in which we expect the point p to be aligned now
Unit3 i_q = i_R_b * b_q;
assert(assert_equal(a_p, i_R_b * b_p, 1e-6));
// Now align second pair: we need to align i_q to a_q
Rot3 a_R_i = AlignPair(a_p, a_q, i_q);
assert(assert_equal(a_p, a_R_i * a_p, 1e-6));
assert(assert_equal(a_q, a_R_i * i_q, 1e-6));
// The desired rotation is the product of both
Rot3 a_R_b = a_R_i * i_R_b;
return a_R_b;
}
/* ************************************************************************* */
bool Rot3::equals(const Rot3 & R, double tol) const {
return equal_with_abs_tol(matrix(), R.matrix(), tol);
@ -82,7 +134,7 @@ Unit3 Rot3::operator*(const Unit3& p) const {
Point3 Rot3::unrotate(const Point3& p, OptionalJacobian<3,3> H1,
OptionalJacobian<3,3> H2) const {
const Matrix3& Rt = transpose();
Point3 q(Rt * p.vector()); // q = Rt*p
Point3 q(Rt * p); // q = Rt*p
const double wx = q.x(), wy = q.y(), wz = q.z();
if (H1)
*H1 << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;
@ -172,10 +224,7 @@ ostream &operator<<(ostream &os, const Rot3& R) {
/* ************************************************************************* */
Rot3 Rot3::slerp(double t, const Rot3& other) const {
// amazingly simple in GTSAM :-)
assert(t>=0 && t<=1);
Vector3 omega = Logmap(between(other));
return compose(Expmap(t * omega));
return interpolate(*this, other, t);
}
/* ************************************************************************* */

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