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

Merge branch 'release/3.0.0' 

Conflicts:
	gtsam/slam/tests/testBetweenFactor.cpp
release/4.3a0
cbeall3 2015-03-07 00:05:58 -05:00
parent 69f2dcce60 a69dc05a7c
commit ff95ea3f6a
812 changed files with 46736 additions and 35374 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3289
.cproject
View File

File diff suppressed because it is too large Load Diff

6
.gitignore vendored
View File

@ -1,2 +1,4 @@
/build/
/*.DS_Store
/build*
/doc*
*.pyc
*.DS_Store

263
CMakeLists.txt
View File

@ -3,10 +3,11 @@ project(GTSAM CXX C)
cmake_minimum_required(VERSION 2.6)
# Set the version number for the library
set (GTSAM_VERSION_MAJOR 2)
set (GTSAM_VERSION_MINOR 4)
set (GTSAM_VERSION_MAJOR 3)
set (GTSAM_VERSION_MINOR 0)
set (GTSAM_VERSION_PATCH 0)
math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}")
set (GTSAM_VERSION_STRING "${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}")
###############################################################################
# Gather information, perform checks, set defaults
@ -45,30 +46,26 @@ endif()
# Set up options
# Configurable Options
option(GTSAM_BUILD_TESTS "Enable/Disable building of tests" ON)
option(GTSAM_BUILD_TIMING "Enable/Disable building of timing scripts" OFF) # These do not currently work
option(GTSAM_BUILD_EXAMPLES "Enable/Disable building of examples" ON)
if(GTSAM_UNSTABLE_AVAILABLE)
option(GTSAM_BUILD_UNSTABLE "Enable/Disable libgtsam_unstable" ON)
endif()
option(GTSAM_BUILD_SHARED_LIBRARY "Enable/Disable building of a shared version of gtsam" ON)
option(GTSAM_BUILD_STATIC_LIBRARY "Enable/Disable building of a static version of gtsam" OFF)
option(GTSAM_BUILD_STATIC_LIBRARY "Build a static gtsam library, instead of shared" OFF)
option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF)
if(NOT MSVC)
option(GTSAM_BUILD_CONVENIENCE_LIBRARIES "Enable/Disable use of convenience libraries for faster development rebuilds, but slower install" OFF)
endif()
option(GTSAM_POSE3_EXPMAP "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." OFF)
option(GTSAM_ROT3_EXPMAP "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." OFF)
option(GTSAM_ENABLE_CONSISTENCY_CHECKS "Enable/Disable expensive consistency checks" OFF)
option(GTSAM_WITH_TBB "Use Intel Threaded Building Blocks (TBB) if available" ON)
option(GTSAM_WITH_EIGEN_MKL "Eigen will use Intel MKL if available" ON)
option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" ON)
option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON)
# Options relating to MATLAB wrapper
# TODO: Check for matlab mex binary before handling building of binaries
option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" OFF)
option(GTSAM_BUILD_WRAP "Enable/Disable building of matlab wrap utility (necessary for matlab interface)" ON)
option(GTSAM_INSTALL_WRAP "Enable/Disable installation of wrap utility for wrapping other libraries" ON)
# Check / set dependent variables for MATLAB wrapper
set(GTSAM_WRAP_HEADER_PATH "${PROJECT_SOURCE_DIR}/wrap")
if(GTSAM_INSTALL_MATLAB_TOOLBOX AND NOT GTSAM_BUILD_WRAP)
message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX is enabled, please also enable GTSAM_BUILD_WRAP")
endif()
@ -76,33 +73,29 @@ if(GTSAM_INSTALL_WRAP AND NOT GTSAM_BUILD_WRAP)
message(FATAL_ERROR "GTSAM_INSTALL_WRAP is enabled, please also enable GTSAM_BUILD_WRAP")
endif()
if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_BUILD_STATIC_LIBRARY)
message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and GTSAM_BUILD_STATIC_LIBRARY are both enabled. The MATLAB wrapper cannot be compiled with a static GTSAM library because mex modules are themselves shared libraries. If you want a self-contained mex module, enable GTSAM_MEX_BUILD_STATIC_MODULE instead of GTSAM_BUILD_STATIC_LIBRARY.")
endif()
# Flags for choosing default packaging tools
set(CPACK_SOURCE_GENERATOR "TGZ" CACHE STRING "CPack Default Source Generator")
set(CPACK_GENERATOR "TGZ" CACHE STRING "CPack Default Binary Generator")
# Sanity check building of libraries
if (NOT GTSAM_BUILD_SHARED_LIBRARY AND NOT GTSAM_BUILD_STATIC_LIBRARY)
message(FATAL_ERROR "Both shared and static version of GTSAM library disabled - need to choose at least one!")
endif()
# Flags to determine whether tests and examples are build during 'make install'
# Note that these remove the targets from the 'all'
option(GTSAM_DISABLE_TESTS_ON_INSTALL "Disables building tests during install" ON)
option(GTSAM_DISABLE_EXAMPLES_ON_INSTALL "Disables building examples during install" OFF)
# Pull in infrastructure
if (GTSAM_BUILD_TESTS)
enable_testing()
include(Dart)
include(CTest)
endif()
###############################################################################
# Find boost
# If using Boost shared libs, set up auto linking for shared libs
if(MSVC AND NOT Boost_USE_STATIC_LIBS)
# To change the path for boost, you will need to set:
# BOOST_ROOT: path to install prefix for boost
# Boost_NO_SYSTEM_PATHS: set to true to keep the find script from ignoring BOOST_ROOT
# If using Boost shared libs, disable auto linking
if(MSVC)
# Some libraries, at least Boost Program Options, rely on this to export DLL symbols
add_definitions(-DBOOST_ALL_DYN_LINK)
# Disable autolinking
if(NOT Boost_USE_STATIC_LIBS)
add_definitions(-DBOOST_ALL_NO_LIB)
endif()
endif()
find_package(Boost 1.43 COMPONENTS serialization system filesystem thread program_options date_time regex timer chrono)
@ -115,7 +108,9 @@ endif()
option(GTSAM_DISABLE_NEW_TIMERS "Disables using Boost.chrono for timing" OFF)
# Allow for not using the timer libraries on boost < 1.48 (GTSAM timing code falls back to old timer library)
set(GTSAM_BOOST_LIBRARIES ${Boost_SERIALIZATION_LIBRARY} ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY} ${Boost_THREAD_LIBRARY} ${Boost_DATE_TIME_LIBRARY} ${Boost_REGEX_LIBRARY})
set(GTSAM_BOOST_LIBRARIES
${Boost_SERIALIZATION_LIBRARY} ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY}
${Boost_THREAD_LIBRARY} ${Boost_DATE_TIME_LIBRARY} ${Boost_REGEX_LIBRARY})
if (GTSAM_DISABLE_NEW_TIMERS)
message("WARNING: GTSAM timing instrumentation manually disabled")
add_definitions(-DGTSAM_DISABLE_NEW_TIMERS)
@ -123,7 +118,64 @@ else()
if(Boost_TIMER_LIBRARY)
list(APPEND GTSAM_BOOST_LIBRARIES ${Boost_TIMER_LIBRARY} ${Boost_CHRONO_LIBRARY})
else()
message("WARNING: Boost older than 1.48 was found, GTSAM timing instrumentation will use the older, less accurate, Boost timer library.")
list(APPEND GTSAM_BOOST_LIBRARIES rt) # When using the header-only boost timer library, need -lrt
message("WARNING: GTSAM timing instrumentation will use the older, less accurate, Boost timer library because boost older than 1.48 was found.")
endif()
endif()
###############################################################################
# Find TBB
find_package(TBB)
# Set up variables if we're using TBB
if(TBB_FOUND AND GTSAM_WITH_TBB)
set(GTSAM_USE_TBB 1) # This will go into config.h
include_directories(BEFORE ${TBB_INCLUDE_DIRS})
set(GTSAM_TBB_LIBRARIES "")
if(TBB_DEBUG_LIBRARIES)
foreach(lib ${TBB_LIBRARIES})
list(APPEND GTSAM_TBB_LIBRARIES optimized "${lib}")
endforeach()
foreach(lib ${TBB_DEBUG_LIBRARIES})
list(APPEND GTSAM_TBB_LIBRARIES debug "${lib}")
endforeach()
else()
set(GTSAM_TBB_LIBRARIES ${TBB_LIBRARIES})
endif()
list(APPEND GTSAM_ADDITIONAL_LIBRARIES ${GTSAM_TBB_LIBRARIES})
else()
set(GTSAM_USE_TBB 0) # This will go into config.h
endif()
###############################################################################
# Find Google perftools
find_package(GooglePerfTools)
###############################################################################
# Find MKL
if(GTSAM_USE_EIGEN_MKL)
find_package(MKL)
if(MKL_FOUND AND GTSAM_WITH_EIGEN_MKL)
set(GTSAM_USE_EIGEN_MKL 1) # This will go into config.h
set(EIGEN_USE_MKL_ALL 1) # This will go into config.h - it makes Eigen use MKL
include_directories(${MKL_INCLUDE_DIR})
list(APPEND GTSAM_ADDITIONAL_LIBRARIES ${MKL_LIBRARIES})
endif()
endif()
###############################################################################
# Find OpenMP (if we're also using MKL)
if(GTSAM_USE_EIGEN_MKL AND GTSAM_USE_EIGEN_MKL_OPENMP AND GTSAM_USE_EIGEN_MKL)
find_package(OpenMP)
if(OPENMP_FOUND AND GTSAM_USE_EIGEN_MKL AND GTSAM_WITH_EIGEN_MKL_OPENMP)
set(GTSAM_USE_EIGEN_MKL_OPENMP 1) # This will go into config.h
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
endif()
endif()
@ -164,6 +216,41 @@ install(FILES ${PROJECT_BINARY_DIR}/gtsam/3rdparty/gtsam_eigen_includes.h DESTIN
###############################################################################
# Global compile options
# Build list of possible allocators
set(possible_allocators "")
if(GTSAM_USE_TBB)
list(APPEND possible_allocators TBB)
set(preferred_allocator TBB)
else()
list(APPEND possible_allocators BoostPool STL)
set(preferred_allocator STL)
endif()
if(GOOGLE_PERFTOOLS_FOUND)
list(APPEND possible_allocators tcmalloc)
endif()
# Check if current allocator choice is valid and set cache option
list(FIND possible_allocators "${GTSAM_DEFAULT_ALLOCATOR}" allocator_valid)
if(allocator_valid EQUAL -1)
set(GTSAM_DEFAULT_ALLOCATOR ${preferred_allocator} CACHE STRING "Default allocator" FORCE)
else()
set(GTSAM_DEFAULT_ALLOCATOR ${preferred_allocator} CACHE STRING "Default allocator")
endif()
set_property(CACHE GTSAM_DEFAULT_ALLOCATOR PROPERTY STRINGS ${possible_allocators})
mark_as_advanced(GTSAM_DEFAULT_ALLOCATOR)
# Define compile flags depending on allocator
if("${GTSAM_DEFAULT_ALLOCATOR}" STREQUAL "BoostPool")
set(GTSAM_ALLOCATOR_BOOSTPOOL 1)
elseif("${GTSAM_DEFAULT_ALLOCATOR}" STREQUAL "STL")
set(GTSAM_ALLOCATOR_STL 1)
elseif("${GTSAM_DEFAULT_ALLOCATOR}" STREQUAL "TBB")
set(GTSAM_ALLOCATOR_TBB 1)
elseif("${GTSAM_DEFAULT_ALLOCATOR}" STREQUAL "tcmalloc")
set(GTSAM_ALLOCATOR_STL 1) # tcmalloc replaces malloc, so to use it we use the STL allocator
list(APPEND GTSAM_ADDITIONAL_LIBRARIES "tcmalloc")
endif()
# Include boost - use 'BEFORE' so that a specific boost specified to CMake
# takes precedence over a system-installed one.
include_directories(BEFORE ${Boost_INCLUDE_DIR})
@ -174,35 +261,25 @@ include_directories(BEFORE ${Boost_INCLUDE_DIR})
include_directories(BEFORE
gtsam/3rdparty/UFconfig
gtsam/3rdparty/CCOLAMD/Include
gtsam/3rdparty/metis-5.1.0/include
gtsam/3rdparty/metis-5.1.0/libmetis
gtsam/3rdparty/metis-5.1.0/GKlib
${PROJECT_SOURCE_DIR}
${PROJECT_BINARY_DIR} # So we can include generated config header files
CppUnitLite)
if(MSVC)
add_definitions(-D_CRT_SECURE_NO_WARNINGS -D_SCL_SECURE_NO_WARNINGS)
add_definitions(/wd4251 /wd4275 /wd4251) # Disable non-DLL-exported base class warnings
add_definitions(/wd4251 /wd4275 /wd4251 /wd4661 /wd4344) # Disable non-DLL-exported base class and other warnings
endif()
if(GTSAM_ENABLE_CONSISTENCY_CHECKS)
add_definitions(-DGTSAM_EXTRA_CONSISTENCY_CHECKS)
endif()
endif()
###############################################################################
# Add components
# Set default library - static or shared, before adding subdirectories
if(GTSAM_BUILD_SHARED_LIBRARY)
set(gtsam-default gtsam-shared)
if(GTSAM_BUILD_UNSTABLE)
set(gtsam_unstable-default gtsam_unstable-shared)
endif()
else()
set(gtsam-default gtsam-static)
if(GTSAM_BUILD_UNSTABLE)
set(gtsam_unstable-default gtsam_unstable-static)
endif()
endif()
# Build CppUnitLite
add_subdirectory(CppUnitLite)
@ -218,9 +295,7 @@ add_subdirectory(gtsam)
add_subdirectory(tests)
# Build examples
if (GTSAM_BUILD_EXAMPLES)
add_subdirectory(examples)
endif(GTSAM_BUILD_EXAMPLES)
add_subdirectory(examples)
# Matlab toolbox
if (GTSAM_INSTALL_MATLAB_TOOLBOX)
@ -233,7 +308,7 @@ if (GTSAM_BUILD_UNSTABLE)
endif(GTSAM_BUILD_UNSTABLE)
# Install config and export files
GtsamMakeConfigFile(GTSAM)
GtsamMakeConfigFile(GTSAM "${CMAKE_CURRENT_SOURCE_DIR}/gtsam_extra.cmake.in")
export(TARGETS ${GTSAM_EXPORTED_TARGETS} FILE GTSAM-exports.cmake)
# Check for doxygen availability - optional dependency
@ -244,13 +319,16 @@ if (DOXYGEN_FOUND)
add_subdirectory(doc)
endif()
# CMake Tools
add_subdirectory(cmake)
###############################################################################
# Set up CPack
set(CPACK_PACKAGE_DESCRIPTION_SUMMARY "GTSAM")
set(CPACK_PACKAGE_VENDOR "Frank Dellaert, Georgia Institute of Technology")
set(CPACK_PACKAGE_CONTACT "Frank Dellaert, dellaert@cc.gatech.edu")
set(CPACK_PACKAGE_DESCRIPTION_FILE "${CMAKE_CURRENT_SOURCE_DIR}/README")
set(CPACK_PACKAGE_DESCRIPTION_FILE "${CMAKE_CURRENT_SOURCE_DIR}/README.md")
set(CPACK_RESOURCE_FILE_LICENSE "${CMAKE_CURRENT_SOURCE_DIR}/LICENSE")
set(CPACK_PACKAGE_VERSION_MAJOR ${GTSAM_VERSION_MAJOR})
set(CPACK_PACKAGE_VERSION_MINOR ${GTSAM_VERSION_MINOR})
@ -274,42 +352,75 @@ set(CPACK_DEBIAN_PACKAGE_DEPENDS "libboost-dev (>= 1.43)") #Example: "libc6 (>=
message(STATUS "===============================================================")
message(STATUS "================ Configuration Options ======================")
message(STATUS "Build flags ")
#print_config_flag(${GTSAM_BUILD_TIMING} "Build Timing scripts ")
print_config_flag(${GTSAM_BUILD_EXAMPLES} "Build Examples ")
print_config_flag(${GTSAM_BUILD_TESTS} "Build Tests ")
print_config_flag(${GTSAM_BUILD_EXAMPLES_ALWAYS} "Build examples with 'make all' ")
print_config_flag(${GTSAM_BUILD_TIMING} "Build Timing scripts ")
if (DOXYGEN_FOUND)
print_config_flag(${GTSAM_BUILD_DOCS} "Build Docs ")
print_config_flag(${GTSAM_BUILD_DOCS} "Build Docs ")
endif()
print_config_flag(${GTSAM_BUILD_SHARED_LIBRARY} "Build shared GTSAM Library ")
print_config_flag(${GTSAM_BUILD_STATIC_LIBRARY} "Build static GTSAM Library ")
if(NOT MSVC)
print_config_flag(${GTSAM_BUILD_CONVENIENCE_LIBRARIES} "Build Convenience Libraries ")
endif()
print_config_flag(${GTSAM_BUILD_TYPE_POSTFIXES} "Put build-type in library name ")
print_config_flag(${GTSAM_BUILD_STATIC_LIBRARY} "Build static GTSAM library instead of shared")
print_config_flag(${GTSAM_BUILD_TYPE_POSTFIXES} "Put build type in library name ")
if(GTSAM_UNSTABLE_AVAILABLE)
print_config_flag(${GTSAM_BUILD_UNSTABLE} "Build libgtsam_unstable ")
print_config_flag(${GTSAM_BUILD_UNSTABLE} "Build libgtsam_unstable ")
endif()
print_config_flag(${GTSAM_DISABLE_TESTS_ON_INSTALL} "Tests excluded from all/install target ")
print_config_flag(${GTSAM_DISABLE_EXAMPLES_ON_INSTALL} "Examples excluded from all/install target ")
string(TOUPPER "${CMAKE_BUILD_TYPE}" cmake_build_type_toupper)
message(STATUS " Build type : ${CMAKE_BUILD_TYPE}")
message(STATUS " C compilation flags : ${CMAKE_C_FLAGS} ${CMAKE_C_FLAGS_${cmake_build_type_toupper}}")
message(STATUS " C++ compilation flags : ${CMAKE_CXX_FLAGS} ${CMAKE_CXX_FLAGS_${cmake_build_type_toupper}}")
if(NOT MSVC AND NOT XCODE_VERSION)
message(STATUS " Build type : ${CMAKE_BUILD_TYPE}")
message(STATUS " C compilation flags : ${CMAKE_C_FLAGS} ${CMAKE_C_FLAGS_${cmake_build_type_toupper}}")
message(STATUS " C++ compilation flags : ${CMAKE_CXX_FLAGS} ${CMAKE_CXX_FLAGS_${cmake_build_type_toupper}}")
endif()
if(GTSAM_USE_TBB)
message(STATUS " Use Intel TBB : Yes")
elseif(TBB_FOUND)
message(STATUS " Use Intel TBB : TBB found but GTSAM_WITH_TBB is disabled")
else()
message(STATUS " Use Intel TBB : TBB not found")
endif()
if(GTSAM_USE_EIGEN_MKL)
message(STATUS " Eigen will use MKL : Yes")
elseif(MKL_FOUND)
message(STATUS " Eigen will use MKL : MKL found but GTSAM_WITH_EIGEN_MKL is disabled")
else()
message(STATUS " Eigen will use MKL : MKL not found")
endif()
if(GTSAM_USE_EIGEN_MKL_OPENMP)
message(STATUS " Eigen will use MKL and OpenMP : Yes")
elseif(OPENMP_FOUND AND NOT GTSAM_WITH_EIGEN_MKL)
message(STATUS " Eigen will use MKL and OpenMP : OpenMP found but GTSAM_WITH_EIGEN_MKL is disabled")
elseif(OPENMP_FOUND AND NOT MKL_FOUND)
message(STATUS " Eigen will use MKL and OpenMP : OpenMP found but MKL not found")
elseif(OPENMP_FOUND)
message(STATUS " Eigen will use MKL and OpenMP : OpenMP found but GTSAM_WITH_EIGEN_MKL_OPENMP is disabled")
else()
message(STATUS " Eigen will use MKL and OpenMP : OpenMP not found")
endif()
message(STATUS " Default allocator : ${GTSAM_DEFAULT_ALLOCATOR}")
message(STATUS "Packaging flags ")
message(STATUS " CPack Source Generator : ${CPACK_SOURCE_GENERATOR}")
message(STATUS " CPack Generator : ${CPACK_GENERATOR}")
message(STATUS " CPack Source Generator : ${CPACK_SOURCE_GENERATOR}")
message(STATUS " CPack Generator : ${CPACK_GENERATOR}")
message(STATUS "GTSAM flags ")
print_config_flag(${GTSAM_USE_QUATERNIONS} "Quaternions as default Rot3 ")
print_config_flag(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency checking ")
print_config_flag(${GTSAM_POSE3_EXPMAP} "Using full expmap as defaul retract for Pose3 ")
print_config_flag(${GTSAM_USE_QUATERNIONS} "Quaternions as default Rot3 ")
print_config_flag(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency checking ")
print_config_flag(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ")
message(STATUS "MATLAB toolbox flags ")
print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install matlab toolbox ")
print_config_flag(${GTSAM_BUILD_WRAP} "Build Wrap ")
print_config_flag(${GTSAM_INSTALL_WRAP} "Install wrap utility ")
print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install matlab toolbox ")
print_config_flag(${GTSAM_BUILD_WRAP} "Build Wrap ")
message(STATUS "===============================================================")
# Print warnings at the end
if(GTSAM_WITH_TBB AND NOT TBB_FOUND)
message(WARNING "TBB was not found - this is ok, but note that GTSAM parallelization will be disabled. Set GTSAM_WITH_TBB to 'Off' to avoid this warning.")
endif()
if(GTSAM_WITH_EIGEN_MKL AND NOT MKL_FOUND)
message(WARNING "MKL was not found - this is ok, but note that MKL yields better performance. Set GTSAM_WITH_EIGEN_MKL to 'Off' to disable this warning.")
endif()
if(GTSAM_WITH_EIGEN_MKL_OPENMP AND NOT OPENMP_FOUND AND MKL_FOUND)
message(WARNING "Your compiler does not support OpenMP - this is ok, but performance may be improved with OpenMP. Set GTSAM_WITH_EIGEN_MKL_OPENMP to 'Off' to avoid this warning.")
endif()
# Include CPack *after* all flags
include(CPack)

19
DEVELOP Normal file
View File

@ -0,0 +1,19 @@
Information for developers
Coding Conventions:
* Classes are Uppercase, methods and functions lowerMixedCase
* We use a modified K&R Style, with 2-space tabs, inserting spaces for tabs
* Use meaningful variable names, e.g., measurement not msm
Windows:
On Windows it is necessary to explicitly export all functions from the library
which should be externally accessible. To do this, include the macro
GTSAM_EXPORT in your class or function definition.
For example:
class GTSAM_EXPORT MyClass { ... };
GTSAM_EXPORT myFunction();

71
README → INSTALL
View File

@ -1,6 +1,3 @@
README - Georgia Tech Smoothing and Mapping library
---------------------------------------------------
Quickstart
In the root library folder execute:
@ -11,42 +8,6 @@ $] cmake ..
$] make check (optional, runs unit tests)
$] make install
---------------------------------------------------
What is GTSAM?
GTSAM is a library of C++ classes that implement smoothing and
mapping (SAM) in robotics and vision, using factor graphs and Bayes
networks as the underlying computing paradigm rather than sparse
matrices.
GTSAM is not (yet) open source: See COPYING & LICENSE
Please see USAGE for an example on how to use GTSAM.
The library is organized according to the following directory structure:
3rdparty local copies of third party libraries - Eigen3 and CCOLAMD
base provides some base Math and data structures, as well as test-related utilities
geometry points, poses, tensors, etc
inference core graphical model inference such as factor graphs, junction trees, Bayes nets, Bayes trees
linear inference specialized to Gaussian linear case, GaussianFactorGraph etc...
nonlinear non-linear factor graphs and non-linear optimization
slam SLAM and visual SLAM application code
This library contains unchanged copies of two third party libraries, with documentation
of licensing as follows:
- CCOLAMD 2.73: Tim Davis' constrained column approximate minimum degree ordering library
- http://www.cise.ufl.edu/research/sparse
- Licenced under LGPL v2.1, provided in gtsam/3rdparty/CCOLAMD/Doc/lesser.txt
- Eigen 3.2: General C++ matrix and linear algebra library
- Licenced under MPL2, provided in gtsam/3rdparty/Eigen/COPYING.README (some code that is 3rd-party to
Eigen is BSD and LGPL)
There are two supporting libraries:
CppUnitLite unit test library customized for use with gtsam
wrap code generation utility for the Matlab interface to gtsam
Important Installation Notes
----------------------------
@ -56,16 +17,30 @@ GTSAM requires the following libraries to be installed on your system:
- Cmake version 2.6 or higher
- Support for XCode 4.3 command line tools on Mac requires CMake 2.8.8 or higher
Tested compilers
- GCC 4.2-4.7
- Clang 2.9-3.2
- OSX GCC 4.2
- MSVC 2010, 2012
Optional dependent libraries:
- If TBB is installed and detectable by CMake GTSAM will use it automatically.
Ensure that CMake prints "Use Intel TBB : Yes". To disable the use of TBB,
disable the CMake flag GTSAM_WITH_TBB (enabled by default). On Ubuntu, TBB
may be installed from the Ubuntu repositories, and for other platforms it
may be downloaded from https://www.threadingbuildingblocks.org/
Tested compilers:
- GCC 4.2-4.7
- OSX Clang 2.9-5.0
- OSX GCC 4.2
- MSVC 2010, 2012
Tested systems:
- Ubuntu 11.04, 11.10, 12.04, 12.10, 13.04
- MacOS 10.6, 10.7
- Windows 7
- Ubuntu 11.04 - 13.10
- MacOS 10.6 - 10.9
- Windows 7, 8, 8.1
Known issues:
- MSVC 2013 is not yet supported because it cannot build the serialization module
of Boost 1.55 (or earlier).
2)
GTSAM makes extensive use of debug assertions, and we highly recommend you work

43
LICENSE
View File

@ -1,29 +1,18 @@
Copyright (c) 2010, Georgia Tech Research Corporation
Atlanta, Georgia 30332-0415
All Rights Reserved
GTSAM is released under the simplified BSD license, reproduced in the file
LICENSE.BSD in this directory.
See also README for licensing of 3rd-party code included in GTSAM.
GTSAM contains two third party libraries, with documentation of licensing and
modifications as follows:
Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice,
this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
* Neither the name of the copyright holders nor the names of its contributors
may be used to endorse or promote products derived from this software without
specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY
WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- CCOLAMD 2.73: Tim Davis' constrained column approximate minimum degree
ordering library
- Included unmodified in gtsam/3rdparty/CCOLAMD and gtsam/3rdparty/UFconfig
- http://www.cise.ufl.edu/research/sparse
- Licenced under LGPL v2.1, provided in gtsam/3rdparty/CCOLAMD/Doc/lesser.txt
- Eigen 3.2: General C++ matrix and linear algebra library
- Modified with 3 patches that have been contributed back to the Eigen team:
- http://eigen.tuxfamily.org/bz/show_bug.cgi?id=704 (Householder QR MKL selection)
- http://eigen.tuxfamily.org/bz/show_bug.cgi?id=705 (Fix MKL LLT return code)
- http://eigen.tuxfamily.org/bz/show_bug.cgi?id=716 (Improved comma initialization)
- Licenced under MPL2, provided in gtsam/3rdparty/Eigen/COPYING.README
- Some code that is 3rd-party to Eigen is BSD and LGPL

13
LICENSE.BSD Normal file
View File

@ -0,0 +1,13 @@
Copyright (c) 2010, Georgia Tech Research Corporation
Atlanta, Georgia 30332-0415
All Rights Reserved
Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

47
README.md Normal file
View File

@ -0,0 +1,47 @@
README - Georgia Tech Smoothing and Mapping library
===================================================
What is GTSAM?
--------------
GTSAM is a library of C++ classes that implement smoothing and
mapping (SAM) in robotics and vision, using factor graphs and Bayes
networks as the underlying computing paradigm rather than sparse
matrices.
On top of the C++ library, GTSAM includes a MATLAB interface (enable
GTSAM_INSTALL_MATLAB_TOOLBOX in CMake to build it). A Python interface
is under development.
Quickstart
----------
In the root library folder execute:
```
#!bash
$ mkdir build
$ cd build
$ cmake ..
$ make check (optional, runs unit tests)
$ make install
```
Prerequisites:
- [Boost](http://www.boost.org/users/download/) >= 1.43 (Ubuntu: `sudo apt-get install libboost-all-dev`)
- [CMake](http://www.cmake.org/cmake/resources/software.html) >= 2.6 (Ubuntu: `sudo apt-get install cmake`)
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)
Additional Information
----------------------
See the [`INSTALL`](https://bitbucket.org/gtborg/gtsam/src/develop/INSTALL) file for more detailed installation instructions.
GTSAM is open source under the BSD license, see the [`LICENSE`](https://bitbucket.org/gtborg/gtsam/src/develop/LICENSE) and [`LICENSE.BSD`](https://bitbucket.org/gtborg/gtsam/src/develop/LICENSE.BSD) files.
Please see the [`examples/`](https://bitbucket.org/gtborg/gtsam/src/develop/examples) directory and the [`USAGE`](https://bitbucket.org/gtborg/gtsam/src/develop/USAGE) file for examples on how to use GTSAM.

11
USAGE
View File

@ -47,6 +47,17 @@ Factors:
SLAM example, is a measurement such as a visual reading on a landmark or
odometry.
The library is organized according to the following directory structure:
3rdparty local copies of third party libraries - Eigen3 and CCOLAMD
base provides some base Math and data structures, as well as test-related utilities
geometry points, poses, tensors, etc
inference core graphical model inference such as factor graphs, junction trees, Bayes nets, Bayes trees
linear inference specialized to Gaussian linear case, GaussianFactorGraph etc...
nonlinear non-linear factor graphs and non-linear optimization
slam SLAM and visual SLAM application code
VSLAM Example
---------------------------------------------------

26
cmake/CMakeLists.txt Normal file
View File

@ -0,0 +1,26 @@
# This file installs the scripts from this directory that may be used in other
# projects. See README.txt in this directory for documentation.
# Set the install directory depending on the platform so it will be found by
# find_package(GTSAMCMakeTools)
if(WIN32 AND NOT CYGWIN)
set(SCRIPT_INSTALL_DIR "${CMAKE_INSTALL_PREFIX}/CMake")
else()
set(SCRIPT_INSTALL_DIR "${CMAKE_INSTALL_PREFIX}/lib/cmake")
endif()
# Install scripts
install(FILES
GTSAMCMakeToolsConfig.cmake
Config.cmake.in
dllexport.h.in
GtsamBuildTypes.cmake
GtsamMakeConfigFile.cmake
GtsamMatlabWrap.cmake
GtsamPythonWrap.cmake
GtsamTesting.cmake
GtsamTestingObsolete.cmake
README.html
DESTINATION "${SCRIPT_INSTALL_DIR}/GTSAMCMakeTools")

3
cmake/GTSAMCMakeToolsConfig.cmake Normal file
View File

@ -0,0 +1,3 @@
# This config file modifies CMAKE_MODULE_PATH so that the GTSAM-CMakeTools files may be included
list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}")

10
cmake/GtsamBuildTypes.cmake
View File

@ -10,11 +10,7 @@ if(NOT FIRST_PASS_DONE AND NOT CMAKE_BUILD_TYPE AND NOT MSVC AND NOT XCODE_VERSI
endif()
# Add option for using build type postfixes to allow installing multiple build modes
if(MSVC OR XCODE_VERSION)
option(GTSAM_BUILD_TYPE_POSTFIXES "Enable/Disable appending the build type to the name of compiled libraries" ON)
else()
option(GTSAM_BUILD_TYPE_POSTFIXES "Enable/Disable appending the build type to the name of compiled libraries" OFF)
endif()
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)
@ -66,7 +62,7 @@ endif()
# Set up build type library postfixes
if(GTSAM_BUILD_TYPE_POSTFIXES)
foreach(build_type Debug Timing RelWithDebInfo MinSizeRel)
foreach(build_type Debug Timing Profiling RelWithDebInfo MinSizeRel)
string(TOUPPER "${build_type}" build_type_toupper)
set(CMAKE_${build_type_toupper}_POSTFIX ${build_type})
endforeach()
@ -106,7 +102,7 @@ if( NOT cmake_build_type_tolower STREQUAL ""
endif()
# Mark that first pass is done
set(FIRST_PASS_DONE TRUE CACHE BOOL "Internally used to mark whether cmake has been run multiple times" FORCE)
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

4
cmake/GtsamMakeConfigFile.cmake
View File

@ -1,5 +1,7 @@
# Writes a config file
set(GTSAM_CONFIG_TEMPLATE_PATH ${CMAKE_CURRENT_LIST_DIR})
function(GtsamMakeConfigFile PACKAGE_NAME)
if(WIN32 AND NOT CYGWIN)
@ -20,7 +22,7 @@ function(GtsamMakeConfigFile PACKAGE_NAME)
file(RELATIVE_PATH CONF_REL_INCLUDE_DIR "${CMAKE_INSTALL_PREFIX}/${DEF_INSTALL_CMAKE_DIR}" "${CMAKE_INSTALL_PREFIX}/include")
file(RELATIVE_PATH CONF_REL_LIB_DIR "${CMAKE_INSTALL_PREFIX}/${DEF_INSTALL_CMAKE_DIR}" "${CMAKE_INSTALL_PREFIX}/lib")
configure_file(${PROJECT_SOURCE_DIR}/cmake/Config.cmake.in "${PROJECT_BINARY_DIR}/${PACKAGE_NAME}Config.cmake" @ONLY)
configure_file(${GTSAM_CONFIG_TEMPLATE_PATH}/Config.cmake.in "${PROJECT_BINARY_DIR}/${PACKAGE_NAME}Config.cmake" @ONLY)
message(STATUS "Wrote ${PROJECT_BINARY_DIR}/${PACKAGE_NAME}Config.cmake")
# Install config and exports files (for find scripts)

157
cmake/GtsamMatlabWrap.cmake
View File

@ -40,19 +40,28 @@ set(MATLAB_ROOT "${MATLAB_ROOT}" CACHE PATH "Path to MATLAB installation root (e
# User-friendly wrapping function. Builds a mex module from the provided
# interfaceHeader. For example, for the interface header /path/to/gtsam.h,
# interfaceHeader. For example, for the interface header gtsam.h,
# this will build the wrap module 'gtsam'.
# Params:
# interfaceHeader : Absolute or relative path to the interface definition file
# linkLibraries : All dependent CMake target names, library names, or full library paths
# extraIncludeDirs : Extra include directories, in addition to those already passed to include_directories(...)
# extraMexFlags : Any additional compiler flags
#
# Arguments:
#
# interfaceHeader: The relative path to the wrapper interface definition file.
# linkLibraries: Any *additional* libraries to link. Your project library
# (e.g. `lba`), libraries it depends on, and any necessary
# MATLAB libraries will be linked automatically. So normally,
# leave this empty.
# extraIncludeDirs: Any *additional* include paths required by dependent
# libraries that have not already been added by
# include_directories. Again, normally, leave this empty.
# extraMexFlags: Any *additional* flags to pass to the compiler when building
# the wrap code. Normally, leave this empty.
function(wrap_and_install_library interfaceHeader linkLibraries extraIncludeDirs extraMexFlags)
wrap_library_internal("${interfaceHeader}" "${otherLibraries}" "${extraIncludeDirs}" "${mexFlags}")
wrap_library_internal("${interfaceHeader}" "${linkLibraries}" "${extraIncludeDirs}" "${mexFlags}")
install_wrapped_library_internal("${interfaceHeader}")
endfunction()
# Internal function that wraps a library and compiles the wrapper
function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs extraMexFlags)
if(UNIX AND NOT APPLE)
if(CMAKE_SIZEOF_VOID_P EQUAL 8)
@ -103,7 +112,8 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex
# Add -shared or -static suffix to targets
set(correctedOtherLibraries "")
set(otherLibraryTargets "")
foreach(lib ${moduleName} ${otherLibraries})
set(otherLibraryNontargets "")
foreach(lib ${moduleName} ${linkLibraries})
if(TARGET ${lib})
list(APPEND correctedOtherLibraries ${lib})
list(APPEND otherLibraryTargets ${lib})
@ -115,8 +125,20 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex
list(APPEND otherLibraryTargets ${lib}-static)
else()
list(APPEND correctedOtherLibraries ${lib})
list(APPEND otherLibraryNontargets ${lib})
endif()
endforeach()
# Check libraries for conflicting versions built-in to MATLAB
set(dependentLibraries "")
if(NOT "${otherLibraryTargets}" STREQUAL "")
foreach(target ${otherLibraryTargets})
get_target_property(dependentLibrariesOne ${target} INTERFACE_LINK_LIBRARIES)
list(APPEND dependentLibraries ${dependentLibrariesOne})
endforeach()
endif()
list(APPEND dependentLibraries ${otherLibraryNontargets})
check_conflicting_libraries_internal("${dependentLibraries}")
# Set up generation of module source file
file(MAKE_DIRECTORY "${generated_files_path}")
@ -174,6 +196,7 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex
COMMAND cmake -E remove_directory ${compiled_mex_modules_root})
endfunction()
# Internal function that installs a wrap toolbox
function(install_wrapped_library_internal interfaceHeader)
get_filename_component(moduleName "${interfaceHeader}" NAME_WE)
set(generated_files_path "${PROJECT_BINARY_DIR}/wrap/${moduleName}")
@ -205,53 +228,77 @@ function(install_wrapped_library_internal interfaceHeader)
endif()
endfunction()
# Function to setup codegen and building of the wrap toolbox
#
# params:
# moduleName : the name of the module, interface file must be called moduleName.h
# mexFlags : Compilation flags to be passed to the mex compiler
# modulePath : relative path to module markup header file (called moduleName.h)
# otherLibraries : list of library targets this should depend on
# toolboxPath : the directory in which to generate/build wrappers
# wrap_header_path : path to the installed wrap header
function(wrap_library_generic moduleName mexFlags modulePath otherLibraries toolbox_path wrap_header_path)
if(NOT "${CMAKE_PROJECT_NAME}" STREQUAL "GTSAM")
message("Your project uses wrap_library or wrap_library_generic - this is deprecated, please use the more user-friendly function wrap_and_install_library")
endif()
# Append module name to link libraries to keep original behavior
list(APPEND otherLibraries ${moduleName})
# Set up arguments
set(interfaceHeader ${modulePath}/${moduleName}.h)
# Call internal function
wrap_library_internal("${interfaceHeader}" "${otherLibraries}" "" "${mexFlags}")
endfunction(wrap_library_generic)
# Function to setup codegen, building and installation of the wrap toolbox
# This wrap setup function assumes that the toolbox will be installed directly,
# with predictable matlab.h sourcing. Use this version when the toolbox will be used
# from the installed version, rather than in place.
# Assumes variable GTSAM_WRAP_HEADER_PATH has been set
# params:
# moduleName : the name of the module, interface file must be called moduleName.h
# mexFlags : Compilation flags to be passed to the mex compiler
# modulePath : relative path to module markup header file (called moduleName.h)
# otherLibraries : list of library targets this should depend on
function(wrap_library moduleName mexFlags modulePath otherLibraries)
# Toolbox generation path goes in build folder
set(toolbox_base_path ${PROJECT_BINARY_DIR}/wrap)
set(toolbox_path ${toolbox_base_path}/${moduleName})
# Call generic version of function
wrap_library_generic("${moduleName}" "${mexFlags}" "${modulePath}" "${otherLibraries}" "${toolbox_path}" "${GTSAM_WRAP_HEADER_PATH}")
install_wrapped_library_internal("${modulePath}/${moduleName}.h")
endfunction(wrap_library)
# Internal function to check for libraries installed with MATLAB that may conflict
# and prints a warning to move them if problems occur.
function(check_conflicting_libraries_internal libraries)
if(UNIX)
# Set path for matlab's built-in libraries
if(APPLE)
set(mxLibPath "${MATLAB_ROOT}/bin/maci64")
else()
if(CMAKE_CL_64)
set(mxLibPath "${MATLAB_ROOT}/bin/glnxa64")
else()
set(mxLibPath "${MATLAB_ROOT}/bin/glnx86")
endif()
endif()
# List matlab's built-in libraries
file(GLOB matlabLibs RELATIVE "${mxLibPath}" "${mxLibPath}/lib*")
# Convert to base names
set(matlabLibNames "")
foreach(lib ${matlabLibs})
get_filename_component(libName "${lib}" NAME_WE)
list(APPEND matlabLibNames "${libName}")
endforeach()
# Get names of link libraries
set(linkLibNames "")
foreach(lib ${libraries})
string(FIND "${lib}" "/" slashPos)
if(NOT slashPos EQUAL -1)
# If the name is a path, just get the library name
get_filename_component(libName "${lib}" NAME_WE)
list(APPEND linkLibNames "${libName}")
else()
# It's not a path, so see if it looks like a filename
get_filename_component(ext "${lib}" EXT)
if(NOT "${ext}" STREQUAL "")
# It's a filename, so get the base name
get_filename_component(libName "${lib}" NAME_WE)
list(APPEND linkLibNames "${libName}")
else()
# It's not a filename so it must be a short name, add the "lib" prefix
list(APPEND linkLibNames "lib${lib}")
endif()
endif()
endforeach()
# Remove duplicates
list(REMOVE_DUPLICATES linkLibNames)
set(conflictingLibs "")
foreach(lib ${linkLibNames})
list(FIND matlabLibNames "${lib}" libPos)
if(NOT libPos EQUAL -1)
if(NOT conflictingLibs STREQUAL "")
set(conflictingLibs "${conflictingLibs}, ")
endif()
set(conflictingLibs "${conflictingLibs}${lib}")
endif()
endforeach()
if(NOT "${conflictingLibs}" STREQUAL "")
message(WARNING "GTSAM links to the libraries [ ${conflictingLibs} ] on your system, but "
"MATLAB is distributed with its own versions of these libraries which may conflict. "
"If you get strange errors or crashes with the GTSAM MATLAB wrapper, move these "
"libraries out of MATLAB's built-in library directory, which is ${mxLibPath} on "
"your system. MATLAB will usually still work with these libraries moved away, but "
"if not, you'll have to compile the static GTSAM MATLAB wrapper module.")
endif()
endif()
endfunction()
# Helper function to install MATLAB scripts and handle multiple build types where the scripts
# should be installed to all build type toolboxes

375
cmake/GtsamTesting.cmake
View File

@ -1,111 +1,204 @@
# Build macros for using tests
# This file defines the two macros below for easily adding groups of unit tests and scripts,
# as well as sets up unit testing and defines several cache options used to control how
# tests and scripts are built and run.
###############################################################################
# Macro:
#
# gtsamAddTestsGlob(groupName globPatterns excludedFiles linkLibraries)
#
# Add a group of unit tests. A list of unit test .cpp files or glob patterns specifies the
# tests to create. Tests are assigned into a group name so they can easily by run
# independently with a make target. Running 'make check' builds and runs all tests.
#
# Usage example:
# gtsamAddTestsGlob(basic "test*.cpp" "testBroken.cpp" "gtsam;GeographicLib")
#
# Arguments:
# groupName: A name that will allow this group of tests to be run independently, e.g.
# 'basic' causes a 'check.basic' target to be created to run this test
# group.
# globPatterns: The list of files or glob patterns from which to create unit tests, with
# one test created for each cpp file. e.g. "test*.cpp", or
# "testA*.cpp;testB*.cpp;testOneThing.cpp".
# excludedFiles: A list of files or globs to exclude, e.g. "testC*.cpp;testBroken.cpp".
# Pass an empty string "" if nothing needs to be excluded.
# linkLibraries: The list of libraries to link to in addition to CppUnitLite.
macro(gtsamAddTestsGlob groupName globPatterns excludedFiles linkLibraries)
gtsamAddTestsGlob_impl("${groupName}" "${globPatterns}" "${excludedFiles}" "${linkLibraries}")
endmacro()
###############################################################################
# Macro:
#
# gtsamAddExamplesGlob(globPatterns excludedFiles linkLibraries)
#
# Add scripts that will serve as examples of how to use the library. A list of files or
# glob patterns is specified, and one executable will be created for each matching .cpp
# file. These executables will not be installed. They are build with 'make all' if
# GTSAM_BUILD_EXAMPLES_ALWAYS is enabled. They may also be built with 'make examples'.
#
# Usage example:
# gtsamAddExamplesGlob("*.cpp" "BrokenExample.cpp" "gtsam;GeographicLib")
#
# Arguments:
# globPatterns: The list of files or glob patterns from which to create unit tests, with
# one test created for each cpp file. e.g. "*.cpp", or
# "A*.cpp;B*.cpp;MyExample.cpp".
# excludedFiles: A list of files or globs to exclude, e.g. "C*.cpp;BrokenExample.cpp". Pass
# an empty string "" if nothing needs to be excluded.
# linkLibraries: The list of libraries to link to.
macro(gtsamAddExamplesGlob globPatterns excludedFiles linkLibraries)
gtsamAddExamplesGlob_impl("${globPatterns}" "${excludedFiles}" "${linkLibraries}")
endmacro()
# Implementation follows:
# Build macros for using tests
enable_testing()
# Enable make check (http://www.cmake.org/Wiki/CMakeEmulateMakeCheck)
add_custom_target(check COMMAND ${CMAKE_CTEST_COMMAND} -C $<CONFIGURATION> --output-on-failure)
add_custom_target(timing)
option(GTSAM_BUILD_TESTS "Enable/Disable building of tests" ON)
option(GTSAM_BUILD_EXAMPLES_ALWAYS "Build examples with 'make all' (build with 'make examples' if not)" ON)
# Add option for combining unit tests
if(MSVC)
if(MSVC OR XCODE_VERSION)
option(GTSAM_SINGLE_TEST_EXE "Combine unit tests into single executable (faster compile)" ON)
else()
option(GTSAM_SINGLE_TEST_EXE "Combine unit tests into single executable (faster compile)" OFF)
endif()
mark_as_advanced(GTSAM_SINGLE_TEST_EXE)
# Macro for adding categorized tests in a "tests" folder, with
# optional exclusion of tests and convenience library linking options
#
# By default, all tests are linked with CppUnitLite and boost
# Arguments:
# - subdir The name of the category for this test
# - local_libs A list of convenience libraries to use (if GTSAM_BUILD_CONVENIENCE_LIBRARIES is true)
# - full_libs The main library to link against if not using convenience libraries
# - excluded_tests A list of test files that should not be compiled - use for debugging
function(gtsam_add_subdir_tests subdir local_libs full_libs excluded_tests)
# Subdirectory target for tests
add_custom_target(check.${subdir} COMMAND ${CMAKE_CTEST_COMMAND} -C $<CONFIGURATION> --output-on-failure)
set(is_test TRUE)
# Enable make check (http://www.cmake.org/Wiki/CMakeEmulateMakeCheck)
if(GTSAM_BUILD_TESTS)
add_custom_target(check COMMAND ${CMAKE_CTEST_COMMAND} -C $<CONFIGURATION> --output-on-failure)
endif()
# Put check target in Visual Studio solution folder
file(RELATIVE_PATH relative_path "${PROJECT_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}")
set_property(TARGET check.${subdir} PROPERTY FOLDER "${relative_path}")
# Link with CppUnitLite - pulled from gtsam installation
list(APPEND local_libs CppUnitLite)
list(APPEND full_libs CppUnitLite)
# Add examples target
add_custom_target(examples)
# Build grouped tests
gtsam_add_grouped_scripts("${subdir}" # Use subdirectory as group label
"tests/test*.cpp" check "Test" # Standard for all tests
"${local_libs}"
"${full_libs}" "${excluded_tests}" # Pass in linking and exclusion lists
${is_test}) # Set all as tests
endfunction()
# Include obsolete macros - will be removed in the near future
include(GtsamTestingObsolete)
# Macro for adding categorized timing scripts in a "tests" folder, with
# optional exclusion of tests and convenience library linking options
#
# By default, all tests are linked with boost
# Arguments:
# - subdir The name of the category for this timing script
# - local_libs A list of convenience libraries to use (if GTSAM_BUILD_CONVENIENCE_LIBRARIES is true)
# - full_libs The main library to link against if not using convenience libraries
# - excluded_srcs A list of timing files that should not be compiled - use for debugging
macro(gtsam_add_subdir_timing subdir local_libs full_libs excluded_srcs)
# Subdirectory target for timing - does not actually execute the scripts
add_custom_target(timing.${subdir})
set(is_test FALSE)
# Build grouped benchmarks
gtsam_add_grouped_scripts("${subdir}" # Use subdirectory as group label
"tests/time*.cpp" timing "Timing Benchmark" # Standard for all timing scripts
"${local_libs}" "${full_libs}" "${excluded_srcs}" # Pass in linking and exclusion lists
${is_test}) # Treat as not a test
# Implementations of this file's macros:
macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries)
if(GTSAM_BUILD_TESTS)
# Add group target if it doesn't already exist
if(NOT TARGET check.${groupName})
add_custom_target(check.${groupName} COMMAND ${CMAKE_CTEST_COMMAND} -C $<CONFIGURATION> --output-on-failure)
endif()
# Get all script files
file(GLOB script_files ${globPatterns})
# Remove excluded scripts from the list
if(NOT "${excludedFiles}" STREQUAL "")
file(GLOB excludedFilePaths ${excludedFiles})
if("${excludedFilePaths}" STREQUAL "")
message(WARNING "The pattern '${excludedFiles}' for excluding tests from group ${groupName} did not match any files")
else()
list(REMOVE_ITEM script_files ${excludedFilePaths})
endif()
endif()
# Separate into source files and headers (allows for adding headers to show up in
# MSVC and Xcode projects).
set(script_srcs "")
set(script_headers "")
foreach(script_file IN ITEMS ${script_files})
get_filename_component(script_ext ${script_file} EXT)
if(script_ext MATCHES "(h|H)")
list(APPEND script_headers ${script_file})
else()
list(APPEND script_srcs ${script_file})
endif()
endforeach()
# Don't put test files in folders in MSVC and Xcode because they're already grouped
source_group("" FILES ${script_srcs} ${script_headers})
if(NOT GTSAM_SINGLE_TEST_EXE)
# Default for Makefiles - each test in its own executable
foreach(script_src IN ITEMS ${script_srcs})
# Get test base name
get_filename_component(script_name ${script_src} NAME_WE)
# Add executable
add_executable(${script_name} ${script_src} ${script_headers})
target_link_libraries(${script_name} CppUnitLite ${linkLibraries})
# Add target dependencies
add_test(NAME ${script_name} COMMAND ${script_name})
add_dependencies(check.${groupName} ${script_name})
add_dependencies(check ${script_name})
if(NOT MSVC AND NOT XCODE_VERSION)
add_custom_target(${script_name}.run ${EXECUTABLE_OUTPUT_PATH}${script_name})
endif()
# Add TOPSRCDIR
set_property(SOURCE ${script_src} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"")
# Exclude from 'make all' and 'make install'
set_target_properties(${script_name} PROPERTIES EXCLUDE_FROM_ALL ON)
# Configure target folder (for MSVC and Xcode)
set_property(TARGET ${script_name} PROPERTY FOLDER "Unit tests/${groupName}")
endforeach()
else()
# Default on MSVC and XCode - combine test group into a single exectuable
set(target_name check_${groupName}_program)
# Add executable
add_executable(${target_name} ${script_srcs} ${script_headers})
target_link_libraries(${target_name} CppUnitLite ${linkLibraries})
# Only have a main function in one script - use preprocessor
set(rest_script_srcs ${script_srcs})
list(REMOVE_AT rest_script_srcs 0)
set_property(SOURCE ${rest_script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "main=inline no_main")
# Add target dependencies
add_test(NAME ${target_name} COMMAND ${target_name})
add_dependencies(check.${groupName} ${target_name})
add_dependencies(check ${target_name})
# Add TOPSRCDIR
set_property(SOURCE ${script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"")
# Exclude from 'make all' and 'make install'
set_target_properties(${target_name} PROPERTIES EXCLUDE_FROM_ALL ON)
# Configure target folder (for MSVC and Xcode)
set_property(TARGET ${script_name} PROPERTY FOLDER "Unit tests")
endif()
endif()
endmacro()
# Macro for adding executables matching a pattern - builds one executable for
# each file matching the pattern. These exectuables are automatically linked
# with boost.
# Arguments:
# - pattern The glob pattern to match source files
# - local_libs A list of convenience libraries to use (if GTSAM_BUILD_CONVENIENCE_LIBRARIES is true)
# - full_libs The main library to link against if not using convenience libraries
# - excluded_srcs A list of timing files that should not be compiled - use for debugging
function(gtsam_add_executables pattern local_libs full_libs excluded_srcs)
set(is_test FALSE)
if(NOT excluded_srcs)
set(excluded_srcs "")
endif()
# Build executables
gtsam_add_grouped_scripts("" "${pattern}" "" "Executable" "${local_libs}" "${full_libs}" "${excluded_srcs}" ${is_test})
endfunction()
# General-purpose script for adding tests with categories and linking options
macro(gtsam_add_grouped_scripts group pattern target_prefix pretty_prefix_name local_libs full_libs excluded_srcs is_test)
macro(gtsamAddExamplesGlob_impl globPatterns excludedFiles linkLibraries)
# Get all script files
set(script_files "")
foreach(one_pattern ${pattern})
file(GLOB one_script_files "${one_pattern}")
list(APPEND script_files "${one_script_files}")
endforeach()
file(GLOB script_files ${globPatterns})
# Remove excluded scripts from the list
set(exclusions "") # Need to copy out exclusion list for logic to work
foreach(one_exclusion ${excluded_srcs})
file(GLOB one_exclusion_srcs "${one_exclusion}")
list(APPEND exclusions "${one_exclusion_srcs}")
endforeach()
if(exclusions)
list(REMOVE_ITEM script_files ${exclusions})
endif(exclusions)
if(NOT "${excludedFiles}" STREQUAL "")
file(GLOB excludedFilePaths ${excludedFiles})
if("${excludedFilePaths}" STREQUAL "")
message(WARNING "The script exclusion pattern '${excludedFiles}' did not match any files")
else()
list(REMOVE_ITEM script_files ${excludedFilePaths})
endif()
endif()
# Separate into source files and headers
# Separate into source files and headers (allows for adding headers to show up in
# MSVC and Xcode projects).
set(script_srcs "")
set(script_headers "")
foreach(script_file ${script_files})
foreach(script_file IN ITEMS ${script_files})
get_filename_component(script_ext ${script_file} EXT)
if(script_ext MATCHES "(h|H)")
list(APPEND script_headers ${script_file})
@ -113,94 +206,34 @@ macro(gtsam_add_grouped_scripts group pattern target_prefix pretty_prefix_name l
list(APPEND script_srcs ${script_file})
endif()
endforeach()
# Add targets and dependencies for each script
if(NOT "${group}" STREQUAL "")
message(STATUS "Adding ${pretty_prefix_name}s in ${group}")
endif()
# Create exe's for each script, unless we're in SINGLE_TEST_EXE mode
if(NOT is_test OR NOT GTSAM_SINGLE_TEST_EXE)
foreach(script_src ${script_srcs})
get_filename_component(script_base ${script_src} NAME_WE)
if (script_base) # Check for null filenames and headers
set( script_bin ${script_base} )
message(STATUS "Adding ${pretty_prefix_name} ${script_bin}")
add_executable(${script_bin} ${script_src} ${script_headers})
if(NOT "${target_prefix}" STREQUAL "")
if(NOT "${group}" STREQUAL "")
add_dependencies(${target_prefix}.${group} ${script_bin})
endif()
add_dependencies(${target_prefix} ${script_bin})
endif()
# Add TOPSRCDIR
set_property(SOURCE ${script_src} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"")
# Disable building during make all/install
if (GTSAM_DISABLE_TESTS_ON_INSTALL)
set_target_properties(${script_bin} PROPERTIES EXCLUDE_FROM_ALL ON)
endif()
if (is_test)
add_test(NAME ${script_base} COMMAND ${script_bin})
endif()
# Linking and dependendencies
if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
target_link_libraries(${script_bin} ${local_libs} ${GTSAM_BOOST_LIBRARIES})
else()
target_link_libraries(${script_bin} ${full_libs} ${GTSAM_BOOST_LIBRARIES})
endif()
# Add .run target
if(NOT MSVC)
add_custom_target(${script_bin}.run ${EXECUTABLE_OUTPUT_PATH}${script_bin} ${ARGN})
endif()
# Set up Visual Studio folders
file(RELATIVE_PATH relative_path "${PROJECT_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}")
set_property(TARGET ${script_bin} PROPERTY FOLDER "${relative_path}")
endif()
endforeach(script_src)
if(MSVC)
source_group("" FILES ${script_srcs} ${script_headers})
# Don't put test files in folders in MSVC and Xcode because they're already grouped
source_group("" FILES ${script_srcs} ${script_headers})
# Create executables
foreach(script_src IN ITEMS ${script_srcs})
# Get script base name
get_filename_component(script_name ${script_src} NAME_WE)
# Add executable
add_executable(${script_name} ${script_src} ${script_headers})
target_link_libraries(${script_name} ${linkLibraries})
# Add target dependencies
add_dependencies(examples ${script_name})
if(NOT MSVC AND NOT XCODE_VERSION)
add_custom_target(${script_name}.run ${EXECUTABLE_OUTPUT_PATH}${script_name})
endif()
else()
# Create single unit test exe from all test scripts
set(script_bin ${target_prefix}_${group}_prog)
add_executable(${script_bin} ${script_srcs} ${script_headers})
if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
target_link_libraries(${script_bin} ${local_libs} ${Boost_LIBRARIES})
else()
target_link_libraries(${script_bin} ${Boost_LIBRARIES} ${full_libs})
endif()
# Only have a main function in one script
set(rest_script_srcs ${script_srcs})
list(REMOVE_AT rest_script_srcs 0)
set_property(SOURCE ${rest_script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "main=static no_main")
# Add TOPSRCDIR
set_property(SOURCE ${script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"")
# Add test
add_dependencies(${target_prefix}.${group} ${script_bin})
add_dependencies(${target_prefix} ${script_bin})
add_test(NAME ${target_prefix}.${group} COMMAND ${script_bin})
# Disable building during make all/install
if (GTSAM_DISABLE_TESTS_ON_INSTALL)
set_target_properties(${script_bin} PROPERTIES EXCLUDE_FROM_ALL ON)
set_property(SOURCE ${script_src} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"")
if(NOT GTSAM_BUILD_EXAMPLES_ALWAYS)
# Exclude from 'make all' and 'make install'
set_target_properties(${target_name} PROPERTIES EXCLUDE_FROM_ALL ON)
endif()
# Set up Visual Studio folders
if(MSVC)
file(RELATIVE_PATH relative_path "${PROJECT_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}")
set_property(TARGET ${script_bin} PROPERTY FOLDER "${relative_path}")
source_group("" FILES ${script_srcs} ${script_headers})
endif()
endif()
# Configure target folder (for MSVC and Xcode)
set_property(TARGET ${script_name} PROPERTY FOLDER "Examples")
endforeach()
endmacro()

195
cmake/GtsamTestingObsolete.cmake Normal file
View File

@ -0,0 +1,195 @@
# Macro for adding categorized tests in a "tests" folder, with
# optional exclusion of tests and convenience library linking options
#
# By default, all tests are linked with CppUnitLite and boost
# Arguments:
# - subdir The name of the category for this test
# - local_libs A list of convenience libraries to use (if GTSAM_BUILD_CONVENIENCE_LIBRARIES is true)
# - full_libs The main library to link against if not using convenience libraries
# - excluded_tests A list of test files that should not be compiled - use for debugging
function(gtsam_add_subdir_tests subdir local_libs full_libs excluded_tests)
# Subdirectory target for tests
add_custom_target(check.${subdir} COMMAND ${CMAKE_CTEST_COMMAND} -C $<CONFIGURATION> --output-on-failure)
set(is_test TRUE)
# Put check target in Visual Studio solution folder
file(RELATIVE_PATH relative_path "${PROJECT_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}")
set_property(TARGET check.${subdir} PROPERTY FOLDER "${relative_path}")
# Link with CppUnitLite - pulled from gtsam installation
list(APPEND local_libs CppUnitLite)
list(APPEND full_libs CppUnitLite)
# Build grouped tests
gtsam_add_grouped_scripts("${subdir}" # Use subdirectory as group label
"tests/test*.cpp" check "Test" # Standard for all tests
"${local_libs}"
"${full_libs}" "${excluded_tests}" # Pass in linking and exclusion lists
${is_test}) # Set all as tests
endfunction()
# Macro for adding categorized timing scripts in a "tests" folder, with
# optional exclusion of tests and convenience library linking options
#
# By default, all tests are linked with boost
# Arguments:
# - subdir The name of the category for this timing script
# - local_libs A list of convenience libraries to use (if GTSAM_BUILD_CONVENIENCE_LIBRARIES is true)
# - full_libs The main library to link against if not using convenience libraries
# - excluded_srcs A list of timing files that should not be compiled - use for debugging
macro(gtsam_add_subdir_timing subdir local_libs full_libs excluded_srcs)
# Subdirectory target for timing - does not actually execute the scripts
add_custom_target(timing.${subdir})
set(is_test FALSE)
# Build grouped benchmarks
gtsam_add_grouped_scripts("${subdir}" # Use subdirectory as group label
"tests/time*.cpp" timing "Timing Benchmark" # Standard for all timing scripts
"${local_libs}" "${full_libs}" "${excluded_srcs}" # Pass in linking and exclusion lists
${is_test}) # Treat as not a test
endmacro()
# Macro for adding executables matching a pattern - builds one executable for
# each file matching the pattern. These exectuables are automatically linked
# with boost.
# Arguments:
# - pattern The glob pattern to match source files
# - local_libs A list of convenience libraries to use (if GTSAM_BUILD_CONVENIENCE_LIBRARIES is true)
# - full_libs The main library to link against if not using convenience libraries
# - excluded_srcs A list of timing files that should not be compiled - use for debugging
function(gtsam_add_executables pattern local_libs full_libs excluded_srcs)
set(is_test FALSE)
if(NOT excluded_srcs)
set(excluded_srcs "")
endif()
# Build executables
gtsam_add_grouped_scripts("" "${pattern}" "" "Executable" "${local_libs}" "${full_libs}" "${excluded_srcs}" ${is_test})
endfunction()
# General-purpose script for adding tests with categories and linking options
macro(gtsam_add_grouped_scripts group pattern target_prefix pretty_prefix_name local_libs full_libs excluded_srcs is_test)
# Print warning about using this obsolete function
message(AUTHOR_WARNING "Warning: Please see GtsamTesting.cmake - obsolete cmake cmake macro for creating unit tests, examples, and scripts was called. This will be removed in the future. The new macros are much easier anyway!!")
# Get all script files
set(script_files "")
foreach(one_pattern ${pattern})
file(GLOB one_script_files "${one_pattern}")
list(APPEND script_files "${one_script_files}")
endforeach()
# Remove excluded scripts from the list
set(exclusions "") # Need to copy out exclusion list for logic to work
foreach(one_exclusion ${excluded_srcs})
file(GLOB one_exclusion_srcs "${one_exclusion}")
list(APPEND exclusions "${one_exclusion_srcs}")
endforeach()
if(exclusions)
list(REMOVE_ITEM script_files ${exclusions})
endif(exclusions)
# Separate into source files and headers
set(script_srcs "")
set(script_headers "")
foreach(script_file ${script_files})
get_filename_component(script_ext ${script_file} EXT)
if(script_ext MATCHES "(h|H)")
list(APPEND script_headers ${script_file})
else()
list(APPEND script_srcs ${script_file})
endif()
endforeach()
# Add targets and dependencies for each script
if(NOT "${group}" STREQUAL "")
message(STATUS "Adding ${pretty_prefix_name}s in ${group}")
endif()
# Create exe's for each script, unless we're in SINGLE_TEST_EXE mode
if(NOT is_test OR NOT GTSAM_SINGLE_TEST_EXE)
foreach(script_src ${script_srcs})
get_filename_component(script_base ${script_src} NAME_WE)
if (script_base) # Check for null filenames and headers
set( script_bin ${script_base} )
message(STATUS "Adding ${pretty_prefix_name} ${script_bin}")
add_executable(${script_bin} ${script_src} ${script_headers})
if(NOT "${target_prefix}" STREQUAL "")
if(NOT "${group}" STREQUAL "")
add_dependencies(${target_prefix}.${group} ${script_bin})
endif()
add_dependencies(${target_prefix} ${script_bin})
endif()
# Add TOPSRCDIR
set_property(SOURCE ${script_src} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"")
# Disable building during make all/install
if (GTSAM_DISABLE_TESTS_ON_INSTALL)
set_target_properties(${script_bin} PROPERTIES EXCLUDE_FROM_ALL ON)
endif()
if (is_test)
add_test(NAME ${script_base} COMMAND ${script_bin})
endif()
# Linking and dependendencies
if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
target_link_libraries(${script_bin} ${local_libs} ${GTSAM_BOOST_LIBRARIES})
else()
target_link_libraries(${script_bin} ${full_libs} ${GTSAM_BOOST_LIBRARIES})
endif()
# Add .run target
if(NOT MSVC AND NOT XCODE_VERSION)
add_custom_target(${script_bin}.run ${EXECUTABLE_OUTPUT_PATH}${script_bin} ${ARGN})
endif()
# Set up Visual Studio folders
file(RELATIVE_PATH relative_path "${PROJECT_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}")
set_property(TARGET ${script_bin} PROPERTY FOLDER "${relative_path}")
endif()
endforeach(script_src)
if(MSVC)
source_group("" FILES ${script_srcs} ${script_headers})
endif()
else()
# Create single unit test exe from all test scripts
set(script_bin ${target_prefix}_${group}_prog)
add_executable(${script_bin} ${script_srcs} ${script_headers})
if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
target_link_libraries(${script_bin} ${local_libs} ${Boost_LIBRARIES})
else()
target_link_libraries(${script_bin} ${Boost_LIBRARIES} ${full_libs})
endif()
# Only have a main function in one script
set(rest_script_srcs ${script_srcs})
list(REMOVE_AT rest_script_srcs 0)
set_property(SOURCE ${rest_script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "main=static no_main")
# Add TOPSRCDIR
set_property(SOURCE ${script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"")
# Add test
add_dependencies(${target_prefix}.${group} ${script_bin})
add_dependencies(${target_prefix} ${script_bin})
add_test(NAME ${target_prefix}.${group} COMMAND ${script_bin})
# Disable building during make all/install
if (GTSAM_DISABLE_TESTS_ON_INSTALL)
set_target_properties(${script_bin} PROPERTIES EXCLUDE_FROM_ALL ON)
endif()
# Set up Visual Studio folders
if(MSVC)
file(RELATIVE_PATH relative_path "${PROJECT_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}")
set_property(TARGET ${script_bin} PROPERTY FOLDER "${relative_path}")
source_group("" FILES ${script_srcs} ${script_headers})
endif()
endif()
endmacro()

92
cmake/README.html Normal file
View File

@ -0,0 +1,92 @@
<h1>GTSAMCMakeTools</h1>
<p>This is the collection of GTSAM CMake tools that may be useful in external projects. The way to use this collection is by first making a find_package call:</p>
<pre><code>find_package(GTSAMCMakeTools)
</code></pre>
<p>which will add a directory containing the GTSAM CMake tools to the CMAKE_MODULE_PATH variable. After that, you may include the files you would like to use. These files and the functions they define are explained below.</p>
<h2>GtsamBuildTypes</h2>
<pre><code>include(GtsamBuildTypes)
</code></pre>
<p>Including this file immediately sets up the following build types and a drop-down list in cmake-gui:</p>
<ul>
<li><code>Debug</code></li>
<li><code>Release</code></li>
<li><code>RelWithDebInfo</code></li>
<li><code>Profiling</code>: All optimizations enabled and minimal debug symbols</li>
<li><code>Timing</code>: Defines the symbol GTSAM_ENABLE_TIMING for using GTSAM timing instrumentation</li>
</ul>
<p>It also configures several minor details, as follows:</p>
<ul>
<li>The compile flag <code>-ftemplate-depth=1024</code> is set for newer versions of Clang to handle complex templates.</li>
<li>On Windows, executable and dll output paths are set to <code>${CMAKE_BINARY_DIR}/bin</code> and import library output to <code>${CMAKE_BINARY_DIR}/bin</code>.</li>
</ul>
<p>It defines the following functions:</p>
<ul>
<li><code>gtsam_assign_source_folders( [files] )</code> Organizes files in the IDE into folders to reflect the actual directory structure of those files. Folders will be determined relative to the current source folder when this function is called.</li>
<li><code>gtsam_assign_all_source_folders()</code> Calls <code>gtsam_assign_source_folders</code> on all cpp, c, and h files recursively in the current source folder.</li>
</ul>
<h2>GtsamTesting</h2>
<pre><code>include(GtsamTesting)
</code></pre>
<p>Defines two useful functions for creating CTest unit tests. Also immediately creates a <code>check</code> target that builds and runs all unit tests.</p>
<ul>
<li>
<p><code>gtsamAddTestsGlob(groupName globPatterns excludedFiles linkLibraries)</code> Add a group of unit tests. A list of unit test .cpp files or glob patterns specifies the tests to create. Tests are assigned into a group name so they can easily by run independently with a make target. Running 'make check' builds and runs all tests.</p>
<p>Usage example:</p>
<pre><code>gtsamAddTestsGlob(basic "test*.cpp" "testBroken.cpp" "gtsam;GeographicLib")
</code></pre>
<p>Arguments:</p>
<pre><code>groupName: A name that will allow this group of tests to be run independently, e.g.
'basic' causes a 'check.basic' target to be created to run this test
group.
globPatterns: The list of files or glob patterns from which to create unit tests, with
one test created for each cpp file. e.g. "test*.cpp", or
"testA*.cpp;testB*.cpp;testOneThing.cpp".
excludedFiles: A list of files or globs to exclude, e.g. "testC*.cpp;testBroken.cpp".
Pass an empty string "" if nothing needs to be excluded.
linkLibraries: The list of libraries to link to in addition to CppUnitLite.
</code></pre>
</li>
<li>
<p><code>gtsamAddExamplesGlob(globPatterns excludedFiles linkLibraries)</code> Add scripts that will serve as examples of how to use the library. A list of files or glob patterns is specified, and one executable will be created for each matching .cpp file. These executables will not be installed. They are build with 'make all' if GTSAM_BUILD_EXAMPLES_ALWAYS is enabled. They may also be built with 'make examples'.</p>
<p>Usage example:</p>
<pre><code>gtsamAddExamplesGlob("*.cpp" "BrokenExample.cpp" "gtsam;GeographicLib")
</code></pre>
<p>Arguments:</p>
<pre><code>globPatterns: The list of files or glob patterns from which to create unit tests, with
one test created for each cpp file. e.g. "*.cpp", or
"A*.cpp;B*.cpp;MyExample.cpp".
excludedFiles: A list of files or globs to exclude, e.g. "C*.cpp;BrokenExample.cpp". Pass
an empty string "" if nothing needs to be excluded.
linkLibraries: The list of libraries to link to.
</code></pre>
</li>
</ul>
<h2>GtsamMatlabWrap</h2>
<pre><code>include(GtsamMatlabWrap)
</code></pre>
<p>Defines functions for generating MATLAB wrappers. Also immediately creates several CMake options for configuring the wrapper.</p>
<ul>
<li>
<p><code>wrap_and_install_library(interfaceHeader linkLibraries extraIncludeDirs extraMexFlags)</code> Generates wrap code and compiles the wrapper.</p>
<p>Usage example:</p>
<pre><code>`wrap_and_install_library("lba.h" "" "" "")`
</code></pre>
<p>Arguments:</p>
<pre><code>interfaceHeader: The relative or absolute path to the wrapper interface
definition file.
linkLibraries: Any *additional* libraries to link. Your project library
(e.g. `lba`), libraries it depends on, and any necessary
MATLAB libraries will be linked automatically. So normally,
leave this empty.
extraIncludeDirs: Any *additional* include paths required by dependent
libraries that have not already been added by
include_directories. Again, normally, leave this empty.
extraMexFlags: Any *additional* flags to pass to the compiler when building
the wrap code. Normally, leave this empty.
</code></pre>
</li>
</ul>
<h2>GtsamMakeConfigFile</h2>
<pre><code>include(GtsamMakeConfigFile)
</code></pre>
<p>Defines a function for generating a config file so your project may be found with the CMake <code>find_package</code> function. TODO: Write documentation.</p>

105
cmake/README.md Normal file
View File

@ -0,0 +1,105 @@
GTSAMCMakeTools
===============
This is the collection of GTSAM CMake tools that may be useful in external projects. The way to use this collection is by first making a find_package call:
find_package(GTSAMCMakeTools)
which will add a directory containing the GTSAM CMake tools to the CMAKE_MODULE_PATH variable. After that, you may include the files you would like to use. These files and the functions they define are explained below.
GtsamBuildTypes
---------------
include(GtsamBuildTypes)
Including this file immediately sets up the following build types and a drop-down list in cmake-gui:
* `Debug`
* `Release`
* `RelWithDebInfo`
* `Profiling`: All optimizations enabled and minimal debug symbols
* `Timing`: Defines the symbol GTSAM_ENABLE_TIMING for using GTSAM timing instrumentation
It also configures several minor details, as follows:
* The compile flag `-ftemplate-depth=1024` is set for newer versions of Clang to handle complex templates.
* On Windows, executable and dll output paths are set to `${CMAKE_BINARY_DIR}/bin` and import library output to `${CMAKE_BINARY_DIR}/lib`.
It defines the following functions:
* `gtsam_assign_source_folders( [files] )` Organizes files in the IDE into folders to reflect the actual directory structure of those files. Folders will be determined relative to the current source folder when this function is called.
* `gtsam_assign_all_source_folders()` Calls `gtsam_assign_source_folders` on all cpp, c, and h files recursively in the current source folder.
GtsamTesting
------------
include(GtsamTesting)
Defines two useful functions for creating CTest unit tests. Also immediately creates a `check` target that builds and runs all unit tests.
* `gtsamAddTestsGlob(groupName globPatterns excludedFiles linkLibraries)` Add a group of unit tests. A list of unit test .cpp files or glob patterns specifies the tests to create. Tests are assigned into a group name so they can easily by run independently with a make target. Running 'make check' builds and runs all tests.
Usage example:
gtsamAddTestsGlob(basic "test*.cpp" "testBroken.cpp" "gtsam;GeographicLib")
Arguments:
groupName: A name that will allow this group of tests to be run independently, e.g.
'basic' causes a 'check.basic' target to be created to run this test
group.
globPatterns: The list of files or glob patterns from which to create unit tests, with
one test created for each cpp file. e.g. "test*.cpp", or
"testA*.cpp;testB*.cpp;testOneThing.cpp".
excludedFiles: A list of files or globs to exclude, e.g. "testC*.cpp;testBroken.cpp".
Pass an empty string "" if nothing needs to be excluded.
linkLibraries: The list of libraries to link to in addition to CppUnitLite.
* `gtsamAddExamplesGlob(globPatterns excludedFiles linkLibraries)` Add scripts that will serve as examples of how to use the library. A list of files or glob patterns is specified, and one executable will be created for each matching .cpp file. These executables will not be installed. They are build with 'make all' if GTSAM_BUILD_EXAMPLES_ALWAYS is enabled. They may also be built with 'make examples'.
Usage example:
gtsamAddExamplesGlob("*.cpp" "BrokenExample.cpp" "gtsam;GeographicLib")
Arguments:
globPatterns: The list of files or glob patterns from which to create unit tests, with
one test created for each cpp file. e.g. "*.cpp", or
"A*.cpp;B*.cpp;MyExample.cpp".
excludedFiles: A list of files or globs to exclude, e.g. "C*.cpp;BrokenExample.cpp". Pass
an empty string "" if nothing needs to be excluded.
linkLibraries: The list of libraries to link to.
GtsamMatlabWrap
---------------
include(GtsamMatlabWrap)
Defines functions for generating MATLAB wrappers. Also immediately creates several CMake options for configuring the wrapper.
* `wrap_and_install_library(interfaceHeader linkLibraries extraIncludeDirs extraMexFlags)` Generates wrap code and compiles the wrapper.
Usage example:
`wrap_and_install_library("lba.h" "" "" "")`
Arguments:
interfaceHeader: The relative or absolute path to the wrapper interface
definition file.
linkLibraries: Any *additional* libraries to link. Your project library
(e.g. `lba`), libraries it depends on, and any necessary
MATLAB libraries will be linked automatically. So normally,
leave this empty.
extraIncludeDirs: Any *additional* include paths required by dependent
libraries that have not already been added by
include_directories. Again, normally, leave this empty.
extraMexFlags: Any *additional* flags to pass to the compiler when building
the wrap code. Normally, leave this empty.
GtsamMakeConfigFile
-------------------
include(GtsamMakeConfigFile)
Defines a function for generating a config file so your project may be found with the CMake `find_package` function. TODO: Write documentation.

170
cmake/example_project/CMakeLists.txt
View File

@ -1,173 +1,43 @@
# This file should be used as a template for creating new projects using the CMake tools
# This project has the following features
# - GTSAM linking
# - Boost linking
# - Unit tests via CppUnitLite
# - Automatic detection of sources and headers in subfolders
# - Installation of library and headers
# - Matlab wrap interface with within-project building
# - Use of GTSAM cmake macros
# - Scripts
# - Automatic MATLAB wrapper generation
###################################################################################
# To create your own project, replace "myproject" with the actual name of your project
# To create your own project, replace "example" with the actual name of your project
cmake_minimum_required(VERSION 2.6)
enable_testing()
project(myproject CXX C)
project(example CXX C)
# Add the cmake subfolder to the cmake module path - necessary to use macros
set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH}" "${PROJECT_SOURCE_DIR}/cmake")
# Include GTSAM CMake tools
find_package(GTSAMCMakeTools)
include(GtsamBuildTypes) # Load build type flags and default to Debug mode
include(GtsamTesting) # Easy functions for creating unit tests and scripts
include(GtsamMatlabWrap) # Automatic MATLAB wrapper generation
# Ensure that local folder is searched before library folders
include_directories(BEFORE "${PROJECT_SOURCE_DIR}")
# Load build type flags and default to Debug mode
include(GtsamBuildTypes)
###################################################################################
# Create a list of library dependencies
# These will be linked with executables
set(library_deps "")
set(linking_mode "static")
# Find GTSAM components
find_package(GTSAM REQUIRED) # Uses installed package
list(APPEND library_deps gtsam-${linking_mode} gtsam_unstable-${linking_mode})
# Include ransac
find_package(ransac REQUIRED) # Uses installed package
list(APPEND library_deps ransac-${linking_mode})
# Boost - same requirement as gtsam
find_package(Boost 1.43 COMPONENTS
serialization
system
filesystem
thread
date_time
REQUIRED)
list(APPEND library_deps
${Boost_SERIALIZATION_LIBRARY}
${Boost_SYSTEM_LIBRARY}
${Boost_FILESYSTEM_LIBRARY}
${Boost_THREAD_LIBRARY}
${Boost_DATE_TIME_LIBRARY})
include_directories(${Boost_INCLUDE_DIR} ${GTSAM_INCLUDE_DIR} ${ransac_INCLUDE_DIR})
include_directories(${GTSAM_INCLUDE_DIR})
###################################################################################
# List subdirs to process tests/sources
# Each of these will be scanned for new files
set (myproject_subdirs
"." # ensure root folder gets included
stuff
things
)
# loop through subdirs to install and build up source lists
set(myproject_lib_source "")
set(myproject_tests_source "")
set(myproject_scripts_source "")
foreach(subdir ${myproject_subdirs})
# Installing headers
message(STATUS "Installing ${subdir}")
file(GLOB sub_myproject_headers "myproject/${subdir}/*.h")
install(FILES ${sub_myproject_headers} DESTINATION include/myproject/${subdir})
# add sources to main sources list
file(GLOB subdir_srcs "myproject/${subdir}/*.cpp")
list(APPEND myproject_lib_source ${subdir_srcs})
# add tests to main tests list
file(GLOB subdir_test_srcs "myproject/${subdir}/tests/*.cpp")
list(APPEND myproject_tests_source ${subdir_test_srcs})
# add scripts to main tests list
file(GLOB subdir_scripts_srcs "myproject/${subdir}/scripts/*.cpp")
list(APPEND myproject_scripts_source ${subdir_scripts_srcs})
endforeach(subdir)
set(myproject_version ${myproject_VERSION_MAJOR}.${myproject_VERSION_MINOR}.${myproject_VERSION_PATCH})
set(myproject_soversion ${myproject_VERSION_MAJOR})
message(STATUS "GTSAM Version: ${gtsam_version}")
message(STATUS "Install prefix: ${CMAKE_INSTALL_PREFIX}")
# Build library (static and shared versions)
# Include installed versions
SET(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
add_library(${PROJECT_NAME}-shared SHARED ${myproject_lib_source})
set_target_properties(${PROJECT_NAME}-shared PROPERTIES
OUTPUT_NAME ${PROJECT_NAME}
CLEAN_DIRECT_OUTPUT 1)
install(TARGETS myproject-shared EXPORT myproject-exports LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin)
list(APPEND myproject_EXPORTED_TARGETS myproject-shared)
add_library(${PROJECT_NAME}-static STATIC ${myproject_lib_source})
set_target_properties(${PROJECT_NAME}-static PROPERTIES
OUTPUT_NAME ${PROJECT_NAME}
CLEAN_DIRECT_OUTPUT 1)
install(TARGETS myproject-static EXPORT myproject-exports ARCHIVE DESTINATION lib)
list(APPEND myproject_EXPORTED_TARGETS myproject-static)
install(TARGETS ${PROJECT_NAME}-shared LIBRARY DESTINATION lib )
install(TARGETS ${PROJECT_NAME}-static ARCHIVE DESTINATION lib )
# Disabled tests - subtract these from the test files
# Note the need for a full path
set(disabled_tests
"dummy"
#"${PROJECT_SOURCE_DIR}/myproject/geometry/tests/testCovarianceEllipse.cpp"
)
list(REMOVE_ITEM myproject_tests_source ${disabled_tests})
# Build static library from common sources
set(CONVENIENCE_LIB_NAME ${PROJECT_NAME})
add_library(${CONVENIENCE_LIB_NAME} STATIC example/PrintExamples.h example/PrintExamples.cpp)
target_link_libraries(${CONVENIENCE_LIB_NAME} gtsam)
###################################################################################
# Build tests
add_custom_target(check COMMAND ${CMAKE_CTEST_COMMAND})
foreach(test_src_file ${myproject_tests_source})
get_filename_component(test_base ${test_src_file} NAME_WE)
message(STATUS "Adding test ${test_src_file} with base name ${test_base}" )
add_executable(${test_base} ${test_src_file})
target_link_libraries(${test_base} ${PROJECT_NAME}-${linking_mode} ${library_deps} CppUnitLite)
add_test(${test_base} ${EXECUTABLE_OUTPUT_PATH}/${test_base})
add_custom_target(${test_base}.run ${test_base} ${ARGN})
add_dependencies(check ${test_base})
endforeach(test_src_file)
# Build scripts
foreach(script_src_file ${myproject_scripts_source})
get_filename_component(script_base ${script_src_file} NAME_WE)
message(STATUS "Adding script ${script_src_file} with base name ${script_base}" )
add_executable(${script_base} ${script_src_file})
target_link_libraries(${script_base} ${PROJECT_NAME}-${linking_mode} ${library_deps} CppUnitLite)
add_custom_target(${script_base}.run ${script_base} ${ARGN})
endforeach(script_src_file)
# Build tests (CMake tracks the dependecy to link with GTSAM through our project's static library)
gtsamAddTestsGlob("example" "tests/test*.cpp" "" "${CONVENIENCE_LIB_NAME}")
###################################################################################
# Matlab wrapping
include(GtsamMatlabWrap)
set(MEX_COMMAND "mex" CACHE STRING "Command to use for executing mex (if on path, 'mex' will work)")
set(GTSAM_BUILD_MEX_BINARY_FLAGS "" CACHE STRING "Extra flags for running Matlab MEX compilation")
set(MYPROJECT_TOOLBOX_DIR "../matlab/myproject" CACHE PATH "Install folder for matlab toolbox - defaults to inside project")
set(WRAP_HEADER_PATH "${GTSAM_DIR}/../../../include")
set(MYPROJECT_TOOLBOX_FLAGS
${GTSAM_BUILD_MEX_BINARY_FLAGS} -I${PROJECT_SOURCE_DIR} -I${PROJECT_SOURCE_DIR}/myproject -I${Boost_INCLUDE_DIR} -I${MEX_INCLUDE_ROOT} -I${GTSAM_INCLUDE_DIR} -I${WRAP_HEADER_PATH} -Wl,-rpath,${CMAKE_BINARY_DIR}:${CMAKE_INSTALL_PREFIX}/lib)
set(MYPROJECT_LIBRARY_DEPS gtsam gtsam_unstable ransac myproject)
set(GTSAM_BUILD_MEX_BIN ON)
# Function to setup codegen, building and installation of the wrap toolbox
# This wrap setup function assumes that the toolbox will be installed directly,
# with predictable matlab.h sourcing
# params:
# moduleName : the name of the module, interface file must be called moduleName.h
# mexFlags : Compilation flags to be passed to the mex compiler
# modulePath : relative path to module markup header file (called moduleName.h)
# otherLibraries : list of library targets this should depend on
# toolboxPath : the directory in which to generate/build wrappers
# wrap_header_path : path to the installed wrap header
wrap_library_generic(myproject "${MYPROJECT_TOOLBOX_FLAGS}" "" "${MYPROJECT_LIBRARY_DEPS}" "${MYPROJECT_TOOLBOX_DIR}" "${WRAP_HEADER_PATH}")
# Build scripts (CMake tracks the dependecy to link with GTSAM through our project's static library)
gtsamAddExamplesGlob("*.cpp" "" "${CONVENIENCE_LIB_NAME}")
###################################################################################
# Create Install config and export files
# This config file takes the place of FindXXX.cmake scripts
include(GtsamMakeConfigFile)
GtsamMakeConfigFile(myproject)
export(TARGETS ${myproject_EXPORTED_TARGETS} FILE myproject-exports.cmake)
# Build MATLAB wrapper (CMake tracks the dependecy to link with GTSAM through our project's static library)
wrap_and_install_library("example.h" "${CONVENIENCE_LIB_NAME}" "" "")

16
gtsam/inference/SymbolicMultifrontalSolver.cpp → cmake/example_project/SayGoodbye.cpp
View File

@ -10,16 +10,14 @@
* -------------------------------------------------------------------------- */
/**
* @file SymbolicMultifrontalSolver.cpp
* @author Richard Roberts
* @date Oct 22, 2010
* @file SayGoodbye.cpp
* @brief Example script for example project
* @author Richard Roberts
*/
#include <gtsam/inference/SymbolicMultifrontalSolver.h>
#include <gtsam/inference/JunctionTree.h>
namespace gtsam {
//template class GenericMultifrontalSolver<IndexFactor, JunctionTree<FactorGraph<IndexFactor> > >;
#include <example/PrintExamples.h>
int main(int argc, char *argv[]) {
example::PrintExamples().sayGoodbye();
return 0;
}

23
cmake/example_project/SayHello.cpp Normal file
View File

@ -0,0 +1,23 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file SayHello.cpp
* @brief Example script for example project
* @author Richard Roberts
*/
#include <example/PrintExamples.h>
int main(int argc, char *argv[]) {
example::PrintExamples().sayHello();
return 0;
}

18
gtsam/inference/SymbolicSequentialSolver.cpp → cmake/example_project/example.h
View File

@ -10,17 +10,19 @@
* -------------------------------------------------------------------------- */
/**
* @file SymbolicSequentialSolver.cpp
* @author Richard Roberts
* @date Oct 21, 2010
* @file example.h
* @brief Example wrapper interface file
* @author Richard Roberts
*/
#include <gtsam/inference/SymbolicSequentialSolver.h>
#include <gtsam/inference/GenericSequentialSolver-inl.h>
// This is an interface file for automatic MATLAB wrapper generation. See
// gtsam.h for full documentation and more examples.
namespace gtsam {
namespace example {
// An explicit instantiation to be compiled into the library
//template class GenericSequentialSolver<IndexFactor>;
class PrintExamples {
void sayHello() const;
void sayGoodbye() const;
};
}

44
cmake/example_project/example/PrintExamples.cpp Normal file
View File

@ -0,0 +1,44 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file print_examples.cpp
* @brief Example library file
* @author Richard Roberts
*/
#include <iostream>
#include <example/PrintExamples.h>
namespace example {
void PrintExamples::sayHello() const {
std::cout << internal::getHelloString() << std::endl;
}
void PrintExamples::sayGoodbye() const {
std::cout << internal::getGoodbyeString() << std::endl;
}
namespace internal {
std::string getHelloString() {
return "Hello!";
}
std::string getGoodbyeString() {
return "See you soon!";
}
} // namespace internal
} // namespace example

41
cmake/example_project/example/PrintExamples.h Normal file
View File

@ -0,0 +1,41 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file print_examples.h
* @brief Example library file
* @author Richard Roberts
*/
#pragma once
#include <string>
namespace example {
class PrintExamples {
public:
/// Print a greeting
void sayHello() const;
/// Print a farewell
void sayGoodbye() const;
};
namespace internal {
std::string getHelloString();
std::string getGoodbyeString();
} // namespace internal
} // namespace example

43
cmake/example_project/tests/testExample.cpp Normal file
View File

@ -0,0 +1,43 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testExample.cpp
* @brief Unit tests for example
* @author Richard Roberts
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h>
#include <example/PrintExamples.h>
using namespace gtsam;
TEST(Example, HelloString) {
const std::string expectedString = "Hello!";
EXPECT(assert_equal(expectedString, example::internal::getHelloString()));
}
TEST(Example, GoodbyeString) {
const std::string expectedString = "See you soon!";
EXPECT(assert_equal(expectedString, example::internal::getGoodbyeString()));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

38
cmake/example_project_simple/CMakeLists.txt
View File

@ -1,38 +0,0 @@
# This file should be used as a template for creating new projects using the CMake tools
# This project has the following features
# - GTSAM linking
# - Unit tests via CppUnitLite
# - Scripts
###################################################################################
# To create your own project, replace "myproject" with the actual name of your project
cmake_minimum_required(VERSION 2.6)
enable_testing()
project(myproject CXX C)
# Add the cmake subfolder to the cmake module path - necessary to use macros
list(APPEND CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake")
# Ensure that local folder is searched before library folders
include_directories(BEFORE "${PROJECT_SOURCE_DIR}")
# Load build type flags and default to Debug mode
include(GtsamBuildTypes)
###################################################################################
# Find GTSAM components
find_package(GTSAM REQUIRED) # Uses installed package
include_directories(${GTSAM_INCLUDE_DIR})
###################################################################################
# Build static library from common sources
add_library(${PROJECT_NAME} STATIC ${PROJECT_NAME}/MySourceFiles.cpp)
target_link_libraries(${PROJECT_NAME} gtsam-shared)
###################################################################################
# Build tests (CMake tracks the dependecy to link with GTSAM through our project's static library)
gtsam_add_subdir_tests(${PROJECT_NAME} "${PROJECT_NAME}" "${PROJECT_NAME}" "")
###################################################################################
# Build scripts (CMake tracks the dependecy to link with GTSAM through our project's static library)
gtsam_add_executables("${PROJECT_NAME}/myScripts.cpp" "${PROJECT_NAME}" "${PROJECT_NAME}" "")

1
doc/.gitignore vendored
View File

@ -1 +0,0 @@
/*.DS_Store

BIN
doc/CodingGuidelines.docx Normal file
View File

Binary file not shown.

481
doc/CodingGuidelines.lyx Normal file
View File

@ -0,0 +1,481 @@
#LyX 2.0 created this file. For more info see http://www.lyx.org/
\lyxformat 413
\begin_document
\begin_header
\textclass article
\begin_preamble
\usepackage{color}
\definecolor{mygreen}{rgb}{0,0.6,0}
\definecolor{mygray}{rgb}{0.5,0.5,0.5}
\definecolor{mymauve}{rgb}{0.58,0,0.82}
\lstset{ %
backgroundcolor=\color{white}, % choose the background color; you must add \usepackage{color} or \usepackage{xcolor}
basicstyle=\footnotesize, % the size of the fonts that are used for the code
breakatwhitespace=false, % sets if automatic breaks should only happen at whitespace
breaklines=true, % sets automatic line breaking
captionpos=b, % sets the caption-position to bottom
commentstyle=\color{mygreen}, % comment style
% deletekeywords={...}, % if you want to delete keywords from the given language
escapeinside={\%*}{*)}, % if you want to add LaTeX within your code
extendedchars=true, % lets you use non-ASCII characters; for 8-bits encodings only, does not work with UTF-8
frame=single, % adds a frame around the code
keepspaces=true, % keeps spaces in text, useful for keeping indentation of code (possibly needs columns=flexible)
keywordstyle=\color{blue}, % keyword style
language=C++, % the language of the code
morekeywords={*,...}, % if you want to add more keywords to the set
numbers=left, % where to put the line-numbers; possible values are (none, left, right)
numbersep=5pt, % how far the line-numbers are from the code
numberstyle=\tiny\color{mygray}, % the style that is used for the line-numbers
rulecolor=\color{black}, % if not set, the frame-color may be changed on line-breaks within not-black text (e.g. comments (green here))
showspaces=false, % show spaces everywhere adding particular underscores; it overrides 'showstringspaces'
showstringspaces=false, % underline spaces within strings only
showtabs=false, % show tabs within strings adding particular underscores
stepnumber=2, % the step between two line-numbers. If it's 1, each line will be numbered
stringstyle=\color{mymauve}, % string literal style
tabsize=2, % sets default tabsize to 2 spaces
title=\lstname % show the filename of files included with \lstinputlisting; also try caption instead of title
}
\end_preamble
\use_default_options true
\maintain_unincluded_children false
\language english
\language_package default
\inputencoding auto
\fontencoding global
\font_roman lmodern
\font_sans lmss
\font_typewriter lmtt
\font_default_family default
\use_non_tex_fonts false
\font_sc false
\font_osf false
\font_sf_scale 100
\font_tt_scale 100
\graphics default
\default_output_format default
\output_sync 0
\bibtex_command default
\index_command default
\paperfontsize default
\spacing single
\use_hyperref false
\papersize default
\use_geometry false
\use_amsmath 1
\use_esint 1
\use_mhchem 1
\use_mathdots 1
\cite_engine basic
\use_bibtopic false
\use_indices false
\paperorientation portrait
\suppress_date false
\use_refstyle 1
\index Index
\shortcut idx
\color #008000
\end_index
\secnumdepth 3
\tocdepth 3
\paragraph_separation indent
\paragraph_indentation default
\quotes_language english
\papercolumns 1
\papersides 1
\paperpagestyle default
\tracking_changes false
\output_changes false
\html_math_output 0
\html_css_as_file 0
\html_be_strict false
\end_header
\begin_body
\begin_layout Section
Template Classes
\end_layout
\begin_layout Standard
Templated classes are great for writing generic code for multiple types
(e.g.
the same elimination algorithm code for symbolic, discrete, and Gaussian
elimination) without the drawbacks of virtual inheritance (which include
rigid class interfaces, downcasting from returned base class pointers,
and additional runtime overhead).
Depending on how they're used, though, templates can result in very slow
compile times, large binary files, and hard-to-use code.
This section describes the
\begin_inset Quotes eld
\end_inset
best practices
\begin_inset Quotes erd
\end_inset
we have developed for gaining the benefits of templates without the drawbacks.
\end_layout
\begin_layout Standard
If you need to write generic code or classes, here are several programming
patterns we have found to work very well:
\end_layout
\begin_layout Subsection
The
\begin_inset Quotes eld
\end_inset
Templated Base, Specialized Derived
\begin_inset Quotes erd
\end_inset
Pattern
\end_layout
\begin_layout Standard
This pattern is for when you have a generic class containing algorithm or
data structure code that will be specialized to several types.
The templated base class should never be used directly, instead only the
specializations should be used.
Some specialized types can be pre-compiled into the library, but the option
remains to specialize new types in external libraries or projects.
\end_layout
\begin_layout Subsubsection
Basic Class Structure
\end_layout
\begin_layout Standard
We'll use
\family typewriter
FactorGraph
\family default
as an example.
It is templated on the factor type stored in it and has several specializations.
The templated base class
\family typewriter
FactorGraph<class FACTOR>
\family default
is divided into a header file (
\family typewriter
.h
\family default
) and an
\begin_inset Quotes eld
\end_inset
instantiation
\begin_inset Quotes erd
\end_inset
file (
\family typewriter
-inst.h
\family default
).
The basic class structure is as follows.
\begin_inset listings
lstparams "basicstyle={\scriptsize\ttfamily},language={C++}"
inline false
status open
\begin_layout Plain Layout
// File FactorGraph.h
\end_layout
\begin_layout Plain Layout
\end_layout
\begin_layout Plain Layout
%*
\backslash
bfseries{
\backslash
emph{
\backslash
color{red}{// Include a minimal set of headers.
Do not include any '-inst.h' files (this is the key to fast compiles).}}}*)
\end_layout
\begin_layout Plain Layout
#include <boost/serialization/nvp.hpp>
\end_layout
\begin_layout Plain Layout
...
\end_layout
\begin_layout Plain Layout
\end_layout
\begin_layout Plain Layout
namespace gtsam {
\end_layout
\begin_layout Plain Layout
/** Class description */
\end_layout
\begin_layout Plain Layout
template<class FACTOR>
\end_layout
\begin_layout Plain Layout
class FactorGraph
\end_layout
\begin_layout Plain Layout
{
\end_layout
\begin_layout Plain Layout
%*
\backslash
bfseries{
\backslash
emph{
\backslash
color{red}{// Make 'private' any typedefs that must be redefined in derived
classes.
E.g.
'This' in the context of the derived class should refer to the derived
class.
These typedefs will be used only by the generic code in this base class.}}}*)
\end_layout
\begin_layout Plain Layout
private:
\end_layout
\begin_layout Plain Layout
typedef FactorGraph<FACTOR> This; ///< Typedef for this class
\end_layout
\begin_layout Plain Layout
typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer to
this
\end_layout
\begin_layout Plain Layout
\end_layout
\begin_layout Plain Layout
%*
\backslash
bfseries{
\backslash
emph{
\backslash
color{red}{// Make 'public' the typedefs that will be valid in the derived
class.}}}*)
\end_layout
\begin_layout Plain Layout
public:
\end_layout
\begin_layout Plain Layout
typedef FACTOR FactorType; ///< Factor type stored in this graph
\end_layout
\begin_layout Plain Layout
typedef boost::shared_ptr<FACTOR> sharedFactor; ///< Shared pointer
to a factor
\end_layout
\begin_layout Plain Layout
...
\end_layout
\begin_layout Plain Layout
\end_layout
\begin_layout Plain Layout
%*
\backslash
bfseries{
\backslash
emph{
\backslash
color{red}{// Normally, data is 'protected' so the derived class can access
it.}}}*)
\end_layout
\begin_layout Plain Layout
protected:
\end_layout
\begin_layout Plain Layout
/** Collection of factors */
\end_layout
\begin_layout Plain Layout
std::vector<sharedFactor> factors_;
\end_layout
\begin_layout Plain Layout
\end_layout
\begin_layout Plain Layout
%*
\backslash
bfseries{
\backslash
emph{
\backslash
color{red}{// Make 'protected' all constructors, named constructors, or
methods returning the base class type.
These are not public - the derived class will call them and properly convert
returned base classes to the derived class.}}}*)
\end_layout
\begin_layout Plain Layout
/// @name Standard Constructors
\end_layout
\begin_layout Plain Layout
/// @{
\end_layout
\begin_layout Plain Layout
\end_layout
\begin_layout Plain Layout
/** Default constructor */
\end_layout
\begin_layout Plain Layout
FactorGraphUnordered() {}
\end_layout
\begin_layout Plain Layout
\end_layout
\begin_layout Plain Layout
/** Named constructor from iterator over factors */
\end_layout
\begin_layout Plain Layout
template<typename ITERATOR>
\end_layout
\begin_layout Plain Layout
static This FromIterator(ITERATOR firstFactor, ITERATOR lastFactor);
\end_layout
\begin_layout Plain Layout
/// @}
\end_layout
\begin_layout Plain Layout
\end_layout
\begin_layout Plain Layout
%*
\backslash
bfseries{
\backslash
emph{
\backslash
color{red}{// Make 'public' standard methods that will be available in the
derived class's API.}}}*)
\end_layout
\begin_layout Plain Layout
public:
\end_layout
\begin_layout Plain Layout
/// @name Adding Factors
\end_layout
\begin_layout Plain Layout
/// @{
\end_layout
\begin_layout Plain Layout
/** ...
*/
\end_layout
\begin_layout Plain Layout
void reserve(size_t size);
\end_layout
\begin_layout Plain Layout
...
\end_layout
\begin_layout Plain Layout
/// @}
\end_layout
\begin_layout Plain Layout
};
\end_layout
\begin_layout Plain Layout
}
\end_layout
\end_inset
\end_layout
\end_body
\end_document

2
doc/Doxyfile.in
View File

@ -1554,7 +1554,7 @@ TAGFILES =
# When a file name is specified after GENERATE_TAGFILE, doxygen will create
# a tag file that is based on the input files it reads.
GENERATE_TAGFILE =
GENERATE_TAGFILE = html/gtsam.tag
# If the ALLEXTERNALS tag is set to YES all external classes will be listed
# in the class index. If set to NO only the inherited external classes

4
doc/LieGroups.lyx
View File

@ -3037,10 +3037,10 @@ key "Murray94book"
\color none
\begin_inset Formula
\[
\exp\left(\left[\begin{array}{c}
\exp\left(\widehat{\left[\begin{array}{c}
\omega\\
v
\end{array}\right]t\right)=\left[\begin{array}{cc}
\end{array}\right]}t\right)=\left[\begin{array}{cc}
e^{\Skew{\omega}t} & (I-e^{\Skew{\omega}t})\left(\omega\times v\right)+\omega\omega^{T}vt\\
0 & 1
\end{array}\right]

2527
doc/gtsam-coordinate-frames.lyx Normal file
View File

File diff suppressed because it is too large Load Diff

BIN
doc/gtsam-coordinate-frames.pdf Normal file
View File

Binary file not shown.

1
doc/images/.gitignore vendored
View File

@ -1 +0,0 @@
/*.DS_Store

929
doc/math.lyx
View File

@ -1594,7 +1594,7 @@ First, the derivative
\begin_inset Formula $D_{2}f$
\end_inset
with respect to in
with respect to
\begin_inset Formula $p$
\end_inset
@ -1614,11 +1614,11 @@ For the derivative
\begin_inset Formula $T$
\end_inset
, we want
\end_layout
\begin_layout Proof
, we want to find the linear map
\begin_inset Formula $D_{1}f$
\end_inset
such that
\family roman
\series medium
\shape up
@ -1630,9 +1630,10 @@ For the derivative
\uwave off
\noun off
\color none
\begin_inset Formula
\[
f(Te^{\hat{\xi}},p)=Te^{\hat{\xi}}p\approx Tp+D_{1}f(\xi)
Tp+D_{1}f(\xi)\approx f(Te^{\hat{\xi}},p)=Te^{\hat{\xi}}p
\]
\end_inset
@ -1661,7 +1662,12 @@ Te^{\hat{\xi}}p\approx T(I+\hat{\xi})p=Tp+T\hat{\xi}p
\end_inset
and
\begin_inset Formula $D_{1}f(\xi)=T\hat{\xi}p$
\end_inset
.
\begin_inset Note Note
status collapsed
@ -1679,7 +1685,7 @@ T\hat{\xi}p=\left(T\hat{\xi}T^{-1}\right)Tp=\left(\Ad T\xihat\right)\left(Tp\rig
\end_inset
Hence, we need to show that
Hence, to complete the proof, we need to show that
\begin_inset Formula
\begin{equation}
\xihat p=H(p)\xi\label{eq:Hp}
@ -4173,9 +4179,9 @@ so
.
Hence, the final derivative of an action in its first argument is
\begin_inset Formula
\[
\deriv{\left(Rp\right)}{\omega}=RH(p)=-R\Skew p
\]
\begin{equation}
\deriv{\left(Rp\right)}{\omega}=RH(p)=-R\Skew p\label{eq:Rot3action}
\end{equation}
\end_inset
@ -5027,6 +5033,909 @@ Re^{\Skew{\omega}} & t+R\left[v+\left(\omega\times v\right)/2\right]\\
\end_inset
\end_layout
\begin_layout Section
The Sphere
\begin_inset Formula $S^{2}$
\end_inset
\end_layout
\begin_layout Subsection
Definitions
\end_layout
\begin_layout Standard
The sphere
\begin_inset Formula $S^{2}$
\end_inset
is the set of all unit vectors in
\begin_inset Formula $\Rthree$
\end_inset
, i.e., all directions in three-space:
\begin_inset Formula
\[
S^{2}=\{p\in\Rthree|\left\Vert p\right\Vert =1\}
\]
\end_inset
The tangent space
\begin_inset Formula $T_{p}S^{2}$
\end_inset
at a point
\begin_inset Formula $p$
\end_inset
consists of three-vectors
\begin_inset Formula $\xihat$
\end_inset
such that
\begin_inset Formula $\xihat$
\end_inset
is tangent to
\begin_inset Formula $S^{2}$
\end_inset
at
\begin_inset Formula $p$
\end_inset
, i.e.,
\begin_inset Formula
\[
T_{p}S^{2}\define\left\{ \xihat\in\Rthree|p^{T}\xihat=0\right\}
\]
\end_inset
While not a Lie group, we can define an exponential map, which is given
in Ma et.
al
\begin_inset CommandInset citation
LatexCommand cite
key "Ma01ijcv"
\end_inset
, as well as in this CVPR tutorial by Anuj Srivastava:
\begin_inset CommandInset href
LatexCommand href
name "http://stat.fsu.edu/~anuj/CVPR_Tutorial/Part2.pdf"
\end_inset
.
\begin_inset Formula
\[
\exp_{p}\xihat=\cos\left(\left\Vert \xihat\right\Vert \right)p+\sin\left(\left\Vert \xihat\right\Vert \right)\frac{\xihat}{\left\Vert \xihat\right\Vert }
\]
\end_inset
The latter also gives the inverse, i.e., get the tangent vector
\begin_inset Formula $z$
\end_inset
to go from
\begin_inset Formula $p$
\end_inset
to
\begin_inset Formula $q$
\end_inset
:
\begin_inset Formula
\[
z=\log_{p}q=\frac{\theta}{\sin\theta}\left(q-p\cos\theta\right)p
\]
\end_inset
with
\begin_inset Formula $\theta=\cos^{-1}\left(p^{T}q\right)$
\end_inset
.
\end_layout
\begin_layout Subsection
Local Coordinates
\end_layout
\begin_layout Standard
We can find a basis
\begin_inset Formula $B_{p}$
\end_inset
for the tangent space
\begin_inset Formula $T_{p}S^{2}$
\end_inset
, with
\begin_inset Formula $B_{p}=\left[b_{1}|b_{2}\right]$
\end_inset
a
\begin_inset Formula $3\times2$
\end_inset
matrix, by either
\end_layout
\begin_layout Enumerate
Decompose
\begin_inset Formula $p=QR$
\end_inset
, with
\begin_inset Formula $Q$
\end_inset
orthonormal and
\begin_inset Formula $R$
\end_inset
of the form
\begin_inset Formula $[1\,0\,0]^{T}$
\end_inset
, and hence
\begin_inset Formula $p=Q_{1}$
\end_inset
.
The basis
\begin_inset Formula $B_{p}=\left[Q_{2}|Q_{3}\right]$
\end_inset
, i.e., the last two columns of
\begin_inset Formula $Q$
\end_inset
.
\end_layout
\begin_layout Enumerate
Form
\begin_inset Formula $b_{1}=p\times a$
\end_inset
, with
\begin_inset Formula $a$
\end_inset
(consistently) chosen to be non-parallel to
\begin_inset Formula $p$
\end_inset
, and
\begin_inset Formula $b_{2}=p\times b_{1}$
\end_inset
.
\begin_inset Note Note
status collapsed
\begin_layout Plain Layout
To choose
\begin_inset Formula $a$
\end_inset
, one way is to divide the sphere into regions, e.g., pick the axis
\begin_inset Formula $e_{i}$
\end_inset
such that
\begin_inset Formula $e_{i}^{T}p$
\end_inset
is smallest.
However, that leads to discontinuous boundaries.
Since
\begin_inset Formula $0\leq\left|e_{i}^{T}p\right|\leq1$
\end_inset
for all
\begin_inset Formula $p\in S^{2}$
\end_inset
, a better idea might be to use a mixture, e.g.,
\begin_inset Formula
\[
a=\frac{1}{2(x^{2}+y^{2}+z^{2})}\left[\begin{array}{c}
y^{2}+z^{2}\\
x^{2}+z^{2}\\
x^{2}+y^{2}
\end{array}\right]
\]
\end_inset
\end_layout
\end_inset
\end_layout
\begin_layout Standard
Now we can write
\begin_inset Formula $\xihat=B_{p}\xi$
\end_inset
with
\begin_inset Formula $\xi\in R^{2}$
\end_inset
the 2D coordinate in the tangent plane basis
\begin_inset Formula $B_{p}$
\end_inset
.
\end_layout
\begin_layout Subsection
Retraction
\end_layout
\begin_layout Standard
The exponential map uses
\begin_inset Formula $\cos$
\end_inset
and
\begin_inset Formula $\sin$
\end_inset
, and is more than we need for optimization.
Suppose we have a point
\begin_inset Formula $p\in S^{2}$
\end_inset
and a 3-vector
\begin_inset Formula $\xihat\in T_{p}S^{2}$
\end_inset
, Absil
\begin_inset CommandInset citation
LatexCommand cite
key "Absil07book"
\end_inset
tells us we can simply add
\begin_inset Formula $\xihat$
\end_inset
to
\begin_inset Formula $p$
\end_inset
and renormalize to get a new point
\begin_inset Formula $q$
\end_inset
on the sphere.
This is what he calls a
\series bold
retraction
\family roman
\series medium
\shape up
\size normal
\emph off
\bar no
\strikeout off
\uuline off
\uwave off
\noun off
\color none
\begin_inset Formula $\retract_{p}(\xihat)$
\end_inset
,
\family default
\series default
\shape default
\size default
\emph default
\bar default
\strikeout default
\uuline default
\uwave default
\noun default
\color inherit
\begin_inset Formula
\[
q=\retract_{p}(\xihat)=\frac{p+\xihat}{\left\Vert p+z\right\Vert }=\frac{p+\xihat}{\alpha}
\]
\end_inset
with
\begin_inset Formula $\alpha$
\end_inset
the norm of
\begin_inset Formula $p+\xihat$
\end_inset
.
\end_layout
\begin_layout Standard
We can also define a retraction from local coordinates
\begin_inset Formula $\xi\in\Rtwo$
\end_inset
:
\begin_inset Formula
\[
q=\retract_{p}(\xi)=\frac{p+B_{p}\xi}{\left\Vert p+B_{p}\xi\right\Vert }
\]
\end_inset
\end_layout
\begin_layout Subsubsection*
Inverse Retraction
\end_layout
\begin_layout Standard
If
\begin_inset Formula $\xihat=B_{p}\xi$
\end_inset
with
\begin_inset Formula $\xi\in R^{2}$
\end_inset
the 2D coordinate in the tangent plane basis
\begin_inset Formula $B_{p}$
\end_inset
, we have
\begin_inset Formula
\[
\xi=\frac{B_{p}^{T}q}{p^{T}q}
\]
\end_inset
\end_layout
\begin_layout Proof
We seek
\begin_inset Formula
\[
\alpha q=p+B_{p}\xi
\]
\end_inset
If we multiply both sides with
\begin_inset Formula $B_{p}^{T}$
\end_inset
(project on the basis
\begin_inset Formula $B_{p}$
\end_inset
) we obtain
\begin_inset Formula
\[
\alpha B_{p}^{T}q=B_{p}^{T}p+B_{p}^{T}B_{p}\xi
\]
\end_inset
and because
\begin_inset Formula $B_{p}^{T}p=0$
\end_inset
and
\begin_inset Formula $B_{p}^{T}B_{p}=I$
\end_inset
we trivially obtain
\begin_inset Formula $\xi$
\end_inset
as the scaled projection
\begin_inset Formula $B_{p}^{T}q$
\end_inset
:
\begin_inset Formula
\[
\xi=\alpha B_{p}^{T}q
\]
\end_inset
To recover the scale factor
\begin_inset Formula $\alpha$
\end_inset
we multiply with
\begin_inset Formula $p^{T}$
\end_inset
on both sides, and we get
\begin_inset Formula
\[
\alpha p^{T}q=p^{T}p+p^{T}B_{p}\xi
\]
\end_inset
Since
\begin_inset Formula $p^{T}p=1$
\end_inset
and
\begin_inset Formula $p^{T}B_{p}\xi=0$
\end_inset
, we then obtain
\begin_inset Formula $\alpha=1/(p^{T}q)$
\end_inset
, which completes the proof.
\end_layout
\begin_layout Subsection
Rotation acting on a 3D Direction
\end_layout
\begin_layout Standard
Rotating a point
\begin_inset Formula $p\in S^{2}$
\end_inset
on the sphere obviously yields another point
\begin_inset Formula $q=Rp\in S^{2}$
\end_inset
, as rotation preserves the norm.
The derivative of
\begin_inset Formula $f(R,p)$
\end_inset
with respect to
\begin_inset Formula $R$
\end_inset
can be found by equating
\begin_inset Formula
\[
Rp+B_{Rp}\xi=R(I+\Skew{\omega})p=Rp+R\Skew{\omega}p
\]
\end_inset
\begin_inset Formula
\[
B_{Rp}\xi=-R\Skew p\omega
\]
\end_inset
\begin_inset Formula
\[
\xi=-B_{Rp}^{T}R\Skew p\omega
\]
\end_inset
whereas with respect to
\begin_inset Formula $p$
\end_inset
we have
\begin_inset Formula
\[
Rp+B_{Rp}\xi_{q}=R(p+B_{p}\xi_{p})
\]
\end_inset
\begin_inset Formula
\[
\xi_{q}=B_{Rp}^{T}RB_{p}\xi_{p}
\]
\end_inset
\end_layout
\begin_layout Standard
In other words, the Jacobian matrix is given by
\family roman
\series medium
\shape up
\size normal
\emph off
\bar no
\strikeout off
\uuline off
\uwave off
\noun off
\color none
\begin_inset Formula
\[
f'(R,p)=\left[\begin{array}{cc}
-B_{Rp}^{T}R\Skew p & B_{Rp}^{T}RB_{p}\end{array}\right]
\]
\end_inset
\end_layout
\begin_layout Subsection
Error between 3D Directions
\end_layout
\begin_layout Standard
We would like to define a distance metric
\begin_inset Formula $e(p,q)$
\end_inset
between two directions
\begin_inset Formula $p,q\in S^{2}$
\end_inset
.
An obvious choice is
\begin_inset Formula
\[
\theta=\cos^{-1}\left(p^{T}q\right)
\]
\end_inset
which is exactly the distance along the shortest path (geodesic) on the
sphere, i.e., this is the distance metric associated with the exponential.
The advantage is that it is defined everywhere, but it involves
\begin_inset Formula $\cos^{-1}$
\end_inset
.
The derivative with respect to a change in
\begin_inset Formula $q$
\end_inset
, via
\begin_inset Formula $\xi$
\end_inset
, is then
\begin_inset Formula
\[
\frac{\partial\theta(p,q)}{\partial\xi}=\frac{\partial\cos^{-1}\left(p^{T}q\right)}{\partial\xi}=\frac{p^{T}B_{q}}{\sqrt{1-\left(p^{T}q\right)^{2}}}
\]
\end_inset
which is also undefined for
\begin_inset Formula $p=q$
\end_inset
.
\end_layout
\begin_layout Standard
A simpler metric is derived from the retraction but only holds when
\begin_inset Formula $q\approx p$
\end_inset
.
It simply projects
\begin_inset Formula $q$
\end_inset
onto the local coordinate basis
\begin_inset Formula $B_{p}$
\end_inset
defined by
\begin_inset Formula $p$
\end_inset
, and takes the norm:
\begin_inset Formula
\[
\theta(p,q)=\left\Vert B_{p}^{T}q\right\Vert
\]
\end_inset
The derivative with respect to a change in
\begin_inset Formula $q$
\end_inset
, via
\begin_inset Formula $\xi$
\end_inset
, is then
\begin_inset Formula
\[
\frac{\partial\theta(p,q)}{\partial\xi_{q}}=\frac{\partial}{\partial\xi_{q}}\sqrt{\left(B_{p}^{T}q\right)^{2}}=\frac{1}{\sqrt{\left(B_{p}^{T}q\right)^{2}}}\left(B_{p}^{T}q\right)B_{p}^{T}B_{q}=\frac{B_{p}^{T}q}{\theta(q;p)}B_{p}^{T}B_{q}
\]
\end_inset
Note that this again is undefined for
\begin_inset Formula $\theta=0$
\end_inset
.
\end_layout
\begin_layout Standard
For use in a probabilistic factor, a signed, vector-valued error will not
have the discontinuity:
\begin_inset Formula
\[
\theta(p,q)=B_{p}^{T}q
\]
\end_inset
Note this is the inverse retraction up to a scale.
The derivative with respect to a change in
\begin_inset Formula $q$
\end_inset
, via
\begin_inset Formula $\xi$
\end_inset
, is found by
\begin_inset Formula
\[
\frac{\partial\theta(p,q)}{\partial\xi_{q}}=B_{p}^{T}\frac{\partial q}{\partial\xi_{q}}=B_{p}^{T}B_{q}
\]
\end_inset
\end_layout
\begin_layout Subsubsection*
Application
\end_layout
\begin_layout Standard
We can use the above to find the unknown rotation between a camera and an
IMU.
If we measure the rotation between two frames as
\begin_inset Formula $c_{1}Zc_{2}$
\end_inset
, and the predicted rotation from the IMU is
\begin_inset Formula $i_{1}Ri_{2}$
\end_inset
, then we can predict
\begin_inset Formula
\[
c_{1}Zc_{2}=iRc^{T}\cdot i_{1}Ri_{2}\cdot iRc
\]
\end_inset
and the axis of the incremental rotations will relate as
\begin_inset Formula
\[
p=iRc\cdot z
\]
\end_inset
with
\begin_inset Formula $p$
\end_inset
the angular velocity axis in the IMU frame, and
\begin_inset Formula $z$
\end_inset
the measured axis of rotation between the two cameras.
Note this only makes sense if the rotation is non-zero.
So, given an initial estimate
\begin_inset Formula $R$
\end_inset
for the unknown rotation
\begin_inset Formula $iRc$
\end_inset
between IMU and camera, the derivative of the error is (using
\begin_inset CommandInset ref
LatexCommand ref
reference "eq:Rot3action"
\end_inset
)
\family roman
\series medium
\shape up
\size normal
\emph off
\bar no
\strikeout off
\uuline off
\uwave off
\noun off
\color none
\begin_inset Formula
\[
\frac{\partial\theta(Rz;p)}{\partial\omega}=B_{p}^{T}\left(Rz\right)B_{p}^{T}B_{Rz}\frac{\partial\left(Rz\right)}{\partial\omega}=B_{p}^{T}\left(Rz\right)B_{p}^{T}R\Skew z
\]
\end_inset
Here the
\begin_inset Formula $2\times3$
\end_inset
matrix
\begin_inset Formula $B_{Rz}^{T}\Skew z$
\end_inset
translates changes in
\begin_inset Formula $R$
\end_inset
to changes in
\begin_inset Formula $Rz$
\end_inset
, and the
\begin_inset Formula $1\times2$
\end_inset
matrix
\begin_inset Formula $B_{p}^{T}\left(Rz\right)$
\end_inset
describes the downstream effect on the error metric.
\end_layout
\begin_layout Section
The Essential Matrix Manifold
\end_layout
\begin_layout Standard
We parameterize essential matrices as a pair
\begin_inset Formula $(R,t)$
\end_inset
, where
\begin_inset Formula $R\in\SOthree$
\end_inset
and
\begin_inset Formula $t\in S^{2}$
\end_inset
, the unit sphere.
The epipolar matrix is then given by
\begin_inset Formula
\[
E=\Skew tR
\]
\end_inset
and the epipolar error given two corresponding points
\begin_inset Formula $a$
\end_inset
and
\begin_inset Formula $b$
\end_inset
is
\begin_inset Formula
\[
e(R,t;a,b)=a^{T}Eb
\]
\end_inset
We are of course interested in the derivative with respect to orientation
(using
\begin_inset CommandInset ref
LatexCommand ref
reference "eq:Rot3action"
\end_inset
)
\begin_inset Formula
\[
\frac{\partial(a^{T}[t]_{\times}Rb)}{\partial\omega}=a^{T}[t]_{\times}\frac{\partial(Rb)}{\partial\omega}=-a^{T}[t]_{\times}R\Skew b=-a^{T}E[b]_{\times}
\]
\end_inset
and with respect to change in the direction
\begin_inset Formula $t$
\end_inset
\begin_inset Formula
\[
\frac{\partial e(a^{T}[t]_{\times}Rb)}{\partial\xi}=a^{T}\frac{\partial(B\xi\times Rb)}{\partial v}=-a^{T}[Rb]_{\times}B
\]
\end_inset
where we made use of the fact that the retraction can be written as
\begin_inset Formula $t+B\xi$
\end_inset
, with
\begin_inset Formula $B$
\end_inset
a local basis, and we made use of
\begin_inset CommandInset ref
LatexCommand eqref
reference "eq:Dcross1"
\end_inset
:
\family roman
\series medium
\shape up
\size normal
\emph off
\bar no
\strikeout off
\uuline off
\uwave off
\noun off
\color none
\begin_inset Formula
\[
\frac{\partial(a\times b)}{\partial a}=\Skew{-b}
\]
\end_inset
\end_layout
\begin_layout Section

BIN
doc/math.pdf
View File

Binary file not shown.

12
doc/stats.html
View File

File diff suppressed because one or more lines are too long

1
examples/.gitignore vendored
View File

@ -1 +0,0 @@
/*.DS_Store

46
examples/CMakeLists.txt
View File

@ -1,42 +1,8 @@
if(NOT MSVC)
add_custom_target(examples)
endif()
# Build example executables
FILE(GLOB example_srcs "*.cpp")
set (excluded_examples #"")
"${CMAKE_CURRENT_SOURCE_DIR}/DiscreteBayesNet_FG.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/UGM_chain.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/UGM_small.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/elaboratePoint2KalmanFilter.cpp"
set (excluded_examples
DiscreteBayesNet_FG.cpp
UGM_chain.cpp
UGM_small.cpp
elaboratePoint2KalmanFilter.cpp
)
list(REMOVE_ITEM example_srcs ${excluded_examples})
foreach(example_src ${example_srcs} )
get_filename_component(example_base ${example_src} NAME_WE)
set( example_bin ${example_base} )
message(STATUS "Adding Example ${example_bin}")
if(NOT MSVC)
add_dependencies(examples ${example_bin})
endif()
add_executable(${example_bin} ${example_src})
# Disable building during make all/install
if (GTSAM_DISABLE_EXAMPLES_ON_INSTALL)
set_target_properties(${example_bin} PROPERTIES EXCLUDE_FROM_ALL ON)
endif()
target_link_libraries(${example_bin} ${gtsam-default} ${Boost_PROGRAM_OPTIONS_LIBRARY})
if(NOT MSVC)
add_custom_target(${example_bin}.run ${EXECUTABLE_OUTPUT_PATH}${example_bin} ${ARGN})
endif()
# Set up Visual Studio folder
if(MSVC)
set_property(TARGET ${example_bin} PROPERTY FOLDER "Examples")
endif()
endforeach(example_src)
gtsamAddExamplesGlob("*.cpp" "${excluded_examples}" "gtsam;${Boost_PROGRAM_OPTIONS_LIBRARY}")

2
examples/CameraResectioning.cpp
View File

@ -16,7 +16,7 @@
* @date Aug 23, 2011
*/
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <boost/make_shared.hpp>

1
examples/Data/.gitignore vendored
View File

@ -1 +0,0 @@
/*.DS_Store

17
examples/Data/dubrovnik-1-1-pre.txt Normal file
View File

@ -0,0 +1,17 @@
1 1 1
0 0 -3.859900e+02 3.871200e+02
-1.6943983532198115e-02
1.1171804676513932e-02
2.4643508831711991e-03
7.3030995682610689e-01
-2.6490818471043420e-01
-1.7127892627337182e+00
1.4300319432711681e+03
-7.5572758535864072e-08
3.2377569465570913e-14
-1.2055995050700867e+01
1.2838775976205760e+01
-4.1099369264082803e+01

79
examples/Data/dubrovnik-3-7-18-pre.txt Normal file
View File

@ -0,0 +1,79 @@
3 7 18
0 0 -3.859900e+02 3.871200e+02
1 0 -3.844000e+01 4.921200e+02
2 0 -6.679200e+02 1.231100e+02
0 1 3.838800e+02 -1.529999e+01
1 1 5.597500e+02 -1.061500e+02
0 2 5.915500e+02 1.364400e+02
1 2 8.638600e+02 -2.346997e+01
2 2 4.947200e+02 1.125200e+02
0 3 5.925000e+02 1.257500e+02
1 3 8.610800e+02 -3.521997e+01
2 3 4.985400e+02 1.015600e+02
0 4 3.487200e+02 5.583800e+02
1 4 7.760300e+02 4.835300e+02
2 4 7.780029e+00 3.263500e+02
0 5 1.401001e+01 9.642001e+01
1 5 2.071300e+02 1.183600e+02
1 6 5.431801e+02 2.948100e+02
2 6 -5.841998e+01 1.108300e+02
-1.6943983532198115e-02
1.1171804676513932e-02
2.4643508831711991e-03
7.3030995682610689e-01
-2.6490818471043420e-01
-1.7127892627337182e+00
1.4300319432711681e+03
-7.5572758535864072e-08
3.2377569465570913e-14
1.5049725341485708e-02
-1.8504564785154357e-01
-2.9278402790141456e-01
-1.0590476152349551e+00
-3.6017862414345798e-02
-1.5720340175803784e+00
1.4321374541298685e+03
-7.3171919892612292e-08
3.1759419019880947e-14
-3.0793597986873011e-01
3.2077907982952031e-01
2.2253985096991455e-01
8.5034483295909009e+00
6.7499603629668741e+00
-3.6383814384447088e+00
1.5720470590375264e+03
-1.5962623661947355e-08
-1.6507904848058800e-14
-1.2055995050700867e+01
1.2838775976205760e+01
-4.1099369264082803e+01
6.4168905904672933e+00
3.8897031177598462e-01
-2.3586282709150449e+01
1.3051100355717297e+01
3.8387587111611952e+00
-2.9777932175344951e+01
1.3060946673472820e+01
3.5910521225905803e+00
-2.9759080795372942e+01
1.4265764475421857e+01
2.4096216156436530e+01
-5.4823971067225500e+01
-2.5292283211391348e-01
2.2166082122808284e+00
-2.1712127480255084e+01
7.6465738085189585e+00
1.4185331909846619e+01
-5.2070299568846060e+01

12
examples/LocalizationExample.cpp
View File

@ -27,7 +27,7 @@
#include <gtsam/geometry/Pose2.h>
// We will use simple integer Keys to refer to the robot poses.
#include <gtsam/nonlinear/Key.h>
#include <gtsam/inference/Key.h>
// As in OdometryExample.cpp, we use a BetweenFactor to model odometry measurements.
#include <gtsam/slam/BetweenFactor.h>
@ -120,15 +120,15 @@ int main(int argc, char** argv) {
// For simplicity, we will use the same noise model for each odometry factor
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1));
// Create odometry (Between) factors between consecutive poses
graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise));
graph.add(BetweenFactor<Pose2>(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise));
graph.push_back(BetweenFactor<Pose2>(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise));
graph.push_back(BetweenFactor<Pose2>(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise));
// 2b. Add "GPS-like" measurements
// We will use our custom UnaryFactor for this.
noiseModel::Diagonal::shared_ptr unaryNoise = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1)); // 10cm std on x,y
graph.add(boost::make_shared<UnaryFactor>(1, 0.0, 0.0, unaryNoise));
graph.add(boost::make_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise));
graph.add(boost::make_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise));
graph.push_back(boost::make_shared<UnaryFactor>(1, 0.0, 0.0, unaryNoise));
graph.push_back(boost::make_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise));
graph.push_back(boost::make_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise));
graph.print("\nFactor Graph:\n"); // print
// 3. Create the data structure to hold the initialEstimate estimate to the solution

6
examples/OdometryExample.cpp
View File

@ -65,15 +65,15 @@ int main(int argc, char** argv) {
// A prior factor consists of a mean and a noise model (covariance matrix)
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1));
graph.add(PriorFactor<Pose2>(1, priorMean, priorNoise));
graph.push_back(PriorFactor<Pose2>(1, priorMean, priorNoise));
// Add odometry factors
Pose2 odometry(2.0, 0.0, 0.0);
// For simplicity, we will use the same noise model for each odometry factor
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1));
// Create odometry (Between) factors between consecutive poses
graph.add(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));
graph.add(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));
graph.push_back(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));
graph.push_back(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));
graph.print("\nFactor Graph:\n"); // print
// Create the data structure to hold the initialEstimate estimate to the solution

14
examples/PlanarSLAMExample.cpp
View File

@ -33,7 +33,7 @@
// Each variable in the system (poses and landmarks) must be identified with a unique key.
// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
// Here we will use Symbols
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/inference/Symbol.h>
// In GTSAM, measurement functions are represented as 'factors'. Several common factors
// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
@ -81,13 +81,13 @@ int main(int argc, char** argv) {
// Add a prior on pose x1 at the origin. A prior factor consists of a mean and a noise model (covariance matrix)
Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
graph.add(PriorFactor<Pose2>(x1, prior, priorNoise)); // add directly to graph
graph.push_back(PriorFactor<Pose2>(x1, prior, priorNoise)); // add directly to graph
// Add two odometry factors
Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta
graph.add(BetweenFactor<Pose2>(x1, x2, odometry, odometryNoise));
graph.add(BetweenFactor<Pose2>(x2, x3, odometry, odometryNoise));
graph.push_back(BetweenFactor<Pose2>(x1, x2, odometry, odometryNoise));
graph.push_back(BetweenFactor<Pose2>(x2, x3, odometry, odometryNoise));
// Add Range-Bearing measurements to two different landmarks
// create a noise model for the landmark measurements
@ -101,9 +101,9 @@ int main(int argc, char** argv) {
range32 = 2.0;
// Add Bearing-Range factors
graph.add(BearingRangeFactor<Pose2, Point2>(x1, l1, bearing11, range11, measurementNoise));
graph.add(BearingRangeFactor<Pose2, Point2>(x2, l1, bearing21, range21, measurementNoise));
graph.add(BearingRangeFactor<Pose2, Point2>(x3, l2, bearing32, range32, measurementNoise));
graph.push_back(BearingRangeFactor<Pose2, Point2>(x1, l1, bearing11, range11, measurementNoise));
graph.push_back(BearingRangeFactor<Pose2, Point2>(x2, l1, bearing21, range21, measurementNoise));
graph.push_back(BearingRangeFactor<Pose2, Point2>(x3, l2, bearing32, range32, measurementNoise));
// Print
graph.print("Factor Graph:\n");

14
examples/Pose2SLAMExample.cpp
View File

@ -29,7 +29,7 @@
#include <gtsam/geometry/Pose2.h>
// We will use simple integer Keys to refer to the robot poses.
#include <gtsam/nonlinear/Key.h>
#include <gtsam/inference/Key.h>
// In GTSAM, measurement functions are represented as 'factors'. Several common factors
// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
@ -72,23 +72,23 @@ int main(int argc, char** argv) {
// 2a. Add a prior on the first pose, setting it to the origin
// A prior factor consists of a mean and a noise model (covariance matrix)
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1));
graph.add(PriorFactor<Pose2>(1, Pose2(0, 0, 0), priorNoise));
graph.push_back(PriorFactor<Pose2>(1, Pose2(0, 0, 0), priorNoise));
// For simplicity, we will use the same noise model for odometry and loop closures
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1));
// 2b. Add odometry factors
// Create odometry (Between) factors between consecutive poses
graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, 0 ), model));
graph.add(BetweenFactor<Pose2>(2, 3, Pose2(2, 0, M_PI_2), model));
graph.add(BetweenFactor<Pose2>(3, 4, Pose2(2, 0, M_PI_2), model));
graph.add(BetweenFactor<Pose2>(4, 5, Pose2(2, 0, M_PI_2), model));
graph.push_back(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, 0 ), model));
graph.push_back(BetweenFactor<Pose2>(2, 3, Pose2(2, 0, M_PI_2), model));
graph.push_back(BetweenFactor<Pose2>(3, 4, Pose2(2, 0, M_PI_2), model));
graph.push_back(BetweenFactor<Pose2>(4, 5, Pose2(2, 0, M_PI_2), model));
// 2c. Add the loop closure constraint
// This factor encodes the fact that we have returned to the same pose. In real systems,
// these constraints may be identified in many ways, such as appearance-based techniques
// with camera images. We will use another Between Factor to enforce this constraint:
graph.add(BetweenFactor<Pose2>(5, 2, Pose2(2, 0, M_PI_2), model));
graph.push_back(BetweenFactor<Pose2>(5, 2, Pose2(2, 0, M_PI_2), model));
graph.print("\nFactor Graph:\n"); // print
// 3. Create the data structure to hold the initialEstimate estimate to the solution

2
examples/Pose2SLAMExample_graph.cpp
View File

@ -38,7 +38,7 @@ int main (int argc, char** argv) {
// Add a Gaussian prior on first poses
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.01, 0.01, 0.01));
graph->add(PriorFactor<Pose2>(0, priorMean, priorNoise));
graph->push_back(PriorFactor<Pose2>(0, priorMean, priorNoise));
// Single Step Optimization using Levenberg-Marquardt
Values result = LevenbergMarquardtOptimizer(*graph, *initial).optimize();

12
examples/Pose2SLAMExample_graphviz.cpp
View File

@ -33,19 +33,19 @@ int main (int argc, char** argv) {
// 2a. Add a prior on the first pose, setting it to the origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1));
graph.add(PriorFactor<Pose2>(1, Pose2(0, 0, 0), priorNoise));
graph.push_back(PriorFactor<Pose2>(1, Pose2(0, 0, 0), priorNoise));
// For simplicity, we will use the same noise model for odometry and loop closures
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1));
// 2b. Add odometry factors
graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, 0 ), model));
graph.add(BetweenFactor<Pose2>(2, 3, Pose2(2, 0, M_PI_2), model));
graph.add(BetweenFactor<Pose2>(3, 4, Pose2(2, 0, M_PI_2), model));
graph.add(BetweenFactor<Pose2>(4, 5, Pose2(2, 0, M_PI_2), model));
graph.push_back(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, 0 ), model));
graph.push_back(BetweenFactor<Pose2>(2, 3, Pose2(2, 0, M_PI_2), model));
graph.push_back(BetweenFactor<Pose2>(3, 4, Pose2(2, 0, M_PI_2), model));
graph.push_back(BetweenFactor<Pose2>(4, 5, Pose2(2, 0, M_PI_2), model));
// 2c. Add the loop closure constraint
graph.add(BetweenFactor<Pose2>(5, 2, Pose2(2, 0, M_PI_2), model));
graph.push_back(BetweenFactor<Pose2>(5, 2, Pose2(2, 0, M_PI_2), model));
// 3. Create the data structure to hold the initial estimate to the solution
// For illustrative purposes, these have been deliberately set to incorrect values

16
examples/Pose2SLAMwSPCG.cpp
View File

@ -33,7 +33,7 @@
// Each variable in the system (poses) must be identified with a unique key.
// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
// Here we will use simple integer keys
#include <gtsam/nonlinear/Key.h>
#include <gtsam/inference/Key.h>
// In GTSAM, measurement functions are represented as 'factors'. Several common factors
// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
@ -69,16 +69,16 @@ int main(int argc, char** argv) {
// A prior factor consists of a mean and a noise model (covariance matrix)
Pose2 prior(0.0, 0.0, 0.0); // prior at origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1));
graph.add(PriorFactor<Pose2>(1, prior, priorNoise));
graph.push_back(PriorFactor<Pose2>(1, prior, priorNoise));
// 2b. Add odometry factors
// For simplicity, we will use the same noise model for each odometry factor
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1));
// Create odometry (Between) factors between consecutive poses
graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
graph.add(BetweenFactor<Pose2>(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
graph.add(BetweenFactor<Pose2>(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
graph.add(BetweenFactor<Pose2>(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
graph.push_back(BetweenFactor<Pose2>(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
graph.push_back(BetweenFactor<Pose2>(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
graph.push_back(BetweenFactor<Pose2>(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
graph.push_back(BetweenFactor<Pose2>(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
// 2c. Add the loop closure constraint
// This factor encodes the fact that we have returned to the same pose. In real systems,
@ -86,7 +86,7 @@ int main(int argc, char** argv) {
// with camera images.
// We will use another Between Factor to enforce this constraint, with the distance set to zero,
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1));
graph.add(BetweenFactor<Pose2>(5, 1, Pose2(0.0, 0.0, 0.0), model));
graph.push_back(BetweenFactor<Pose2>(5, 1, Pose2(0.0, 0.0, 0.0), model));
graph.print("\nFactor Graph:\n"); // print
@ -104,7 +104,7 @@ int main(int argc, char** argv) {
LevenbergMarquardtParams parameters;
parameters.verbosity = NonlinearOptimizerParams::ERROR;
parameters.verbosityLM = LevenbergMarquardtParams::LAMBDA;
parameters.linearSolverType = SuccessiveLinearizationParams::CONJUGATE_GRADIENT;
parameters.linearSolverType = NonlinearOptimizerParams::CONJUGATE_GRADIENT;
{
parameters.iterativeParams = boost::make_shared<SubgraphSolverParameters>();

8
examples/RangeISAMExample_plaza2.cpp
View File

@ -22,7 +22,7 @@
// Each variable in the system (poses and landmarks) must be identified with a unique key.
// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
// Here we will use Symbols
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/inference/Symbol.h>
// We want to use iSAM2 to solve the range-SLAM problem incrementally
#include <gtsam/nonlinear/ISAM2.h>
@ -125,7 +125,7 @@ int main (int argc, char** argv) {
Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120,
M_PI - 2.02108900000000);
NonlinearFactorGraph newFactors;
newFactors.add(PriorFactor<Pose2>(0, pose0, priorNoise));
newFactors.push_back(PriorFactor<Pose2>(0, pose0, priorNoise));
Values initial;
initial.insert(0, pose0);
@ -158,7 +158,7 @@ int main (int argc, char** argv) {
boost::tie(t, odometry) = timedOdometry;
// add odometry factor
newFactors.add(BetweenFactor<Pose2>(i-1, i, odometry,NM::Diagonal::Sigmas(odoSigmas)));
newFactors.push_back(BetweenFactor<Pose2>(i-1, i, odometry,NM::Diagonal::Sigmas(odoSigmas)));
// predict pose and add as initial estimate
Pose2 predictedPose = lastPose.compose(odometry);
@ -169,7 +169,7 @@ int main (int argc, char** argv) {
while (k < K && t >= boost::get<0>(triples[k])) {
size_t j = boost::get<1>(triples[k]);
double range = boost::get<2>(triples[k]);
newFactors.add(RangeFactor<Pose2, Point2>(i, symbol('L', j), range,rangeNoise));
newFactors.push_back(RangeFactor<Pose2, Point2>(i, symbol('L', j), range,rangeNoise));
k = k + 1;
countK = countK + 1;
}

8
examples/SFMExample.cpp
View File

@ -30,7 +30,7 @@
// Each variable in the system (poses and landmarks) must be identified with a unique key.
// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
// Here we will use Symbols
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/inference/Symbol.h>
// In GTSAM, measurement functions are represented as 'factors'. Several common factors
// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
@ -81,14 +81,14 @@ int main(int argc, char* argv[]) {
// Add a prior on pose x1. This indirectly specifies where the origin is.
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
graph.add(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise)); // add directly to graph
graph.push_back(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise)); // add directly to graph
// Simulated measurements from each camera pose, adding them to the factor graph
for (size_t i = 0; i < poses.size(); ++i) {
for (size_t j = 0; j < points.size(); ++j) {
SimpleCamera camera(poses[i], *K);
Point2 measurement = camera.project(points[j]);
graph.add(GenericProjectionFactor<Pose3, Point3, Cal3_S2>(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K));
graph.push_back(GenericProjectionFactor<Pose3, Point3, Cal3_S2>(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K));
}
}
@ -96,7 +96,7 @@ int main(int argc, char* argv[]) {
// Here we add a prior on the position of the first landmark. This fixes the scale by indicating the distance
// between the first camera and the first landmark. All other landmark positions are interpreted using this scale.
noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
graph.add(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise)); // add directly to graph
graph.push_back(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise)); // add directly to graph
graph.print("Factor Graph:\n");
// Create the data structure to hold the initial estimate to the solution

8
examples/SFMExample_bal.cpp
View File

@ -16,7 +16,7 @@
*/
// For an explanation of headers, see SFMExample.cpp
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/PriorFactor.h>
@ -60,15 +60,15 @@ int main (int argc, char* argv[]) {
BOOST_FOREACH(const SfM_Measurement& m, track.measurements) {
size_t i = m.first;
Point2 uv = m.second;
graph.add(MyFactor(uv, noise, C(i), P(j))); // note use of shorthand symbols C and P
graph.push_back(MyFactor(uv, noise, C(i), P(j))); // note use of shorthand symbols C and P
}
j += 1;
}
// Add a prior on pose x1. This indirectly specifies where the origin is.
// and a prior on the position of the first landmark to fix the scale
graph.add(PriorFactor<SfM_Camera>(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1)));
graph.add(PriorFactor<Point3> (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1)));
graph.push_back(PriorFactor<SfM_Camera>(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1)));
graph.push_back(PriorFactor<Point3> (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1)));
// Create initial estimate
Values initial;

10
examples/SelfCalibrationExample.cpp
View File

@ -27,7 +27,7 @@
#include <gtsam/geometry/Point2.h>
// Inference and optimization
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/DoglegOptimizer.h>
#include <gtsam/nonlinear/Values.h>
@ -54,7 +54,7 @@ int main(int argc, char* argv[]) {
// Add a prior on pose x1.
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
graph.add(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
graph.push_back(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
// Simulated measurements from each camera pose, adding them to the factor graph
Cal3_S2 K(50.0, 50.0, 0.0, 50.0, 50.0);
@ -65,17 +65,17 @@ int main(int argc, char* argv[]) {
Point2 measurement = camera.project(points[j]);
// The only real difference with the Visual SLAM example is that here we use a
// different factor type, that also calculates the Jacobian with respect to calibration
graph.add(GeneralSFMFactor2<Cal3_S2>(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), Symbol('K', 0)));
graph.push_back(GeneralSFMFactor2<Cal3_S2>(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), Symbol('K', 0)));
}
}
// Add a prior on the position of the first landmark.
noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
graph.add(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise)); // add directly to graph
graph.push_back(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise)); // add directly to graph
// Add a prior on the calibration.
noiseModel::Diagonal::shared_ptr calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 0.1, 100, 100));
graph.add(PriorFactor<Cal3_S2>(Symbol('K', 0), K, calNoise));
graph.push_back(PriorFactor<Cal3_S2>(Symbol('K', 0), K, calNoise));
// Create the initial estimate to the solution
// now including an estimate on the camera calibration parameters

4
examples/SimpleRotation.cpp
View File

@ -28,7 +28,7 @@
// Each variable in the system (poses) must be identified with a unique key.
// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
// Here we will use symbols
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/inference/Symbol.h>
// In GTSAM, measurement functions are represented as 'factors'. Several common factors
// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
@ -90,7 +90,7 @@ int main() {
* many more factors would be added.
*/
NonlinearFactorGraph graph;
graph.add(factor);
graph.push_back(factor);
graph.print("full graph");
/**

86
examples/SolverComparer.cpp
View File

@ -9,23 +9,28 @@
* -------------------------------------------------------------------------- */
/**
* @file timeIncremental.cpp
* @file SolverComparer.cpp
* @brief Incremental and batch solving, timing, and accuracy comparisons
* @author Richard Roberts
* @date August, 2013
*/
#include <gtsam/base/timing.h>
#include <gtsam/base/treeTraversal-inst.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/BearingRangeFactor.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/GaussianJunctionTree.h>
#include <gtsam/linear/GaussianEliminationTree.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <fstream>
#include <iostream>
#include <boost/archive/binary_oarchive.hpp>
#include <boost/archive/binary_iarchive.hpp>
#include <boost/serialization/export.hpp>
@ -33,6 +38,12 @@
#include <boost/range/algorithm/set_algorithm.hpp>
#include <boost/random.hpp>
#ifdef GTSAM_USE_TBB
#include <tbb/tbb.h>
#undef max // TBB seems to include windows.h and we don't want these macros
#undef min
#endif
using namespace std;
using namespace gtsam;
using namespace gtsam::symbol_shorthand;
@ -81,11 +92,14 @@ string inputFile;
string datasetName;
int firstStep;
int lastStep;
int nThreads;
int relinSkip;
bool incremental;
bool dogleg;
bool batch;
bool compare;
bool perturb;
bool stats;
double perturbationNoise;
string compareFile1, compareFile2;
@ -97,6 +111,7 @@ void runIncremental();
void runBatch();
void runCompare();
void runPerturb();
void runStats();
/* ************************************************************************* */
int main(int argc, char *argv[]) {
@ -109,11 +124,14 @@ int main(int argc, char *argv[]) {
("dataset,d", po::value<string>(&datasetName)->default_value(""), "Read a dataset file (if and only if --incremental is used)")
("first-step,f", po::value<int>(&firstStep)->default_value(0), "First step to process from the dataset file")
("last-step,l", po::value<int>(&lastStep)->default_value(-1), "Last step to process, or -1 to process until the end of the dataset")
("threads", po::value<int>(&nThreads)->default_value(-1), "Number of threads, or -1 to use all processors")
("relinSkip", po::value<int>(&relinSkip)->default_value(10), "Fluid relinearization check every arg steps")
("incremental", "Run in incremental mode using ISAM2 (default)")
("dogleg", "When in incremental mode, solve with Dogleg instead of Gauss-Newton in iSAM2")
("batch", "Run in batch mode, requires an initialization from --read-solution")
("compare", po::value<vector<string> >()->multitoken(), "Compare two solution files")
("perturb", po::value<double>(&perturbationNoise), "Perturb a solution file with the specified noise")
("stats", "Gather factorization statistics about the dataset, writes text-file histograms")
;
po::variables_map vm;
po::store(po::command_line_parser(argc, argv).options(desc).run(), vm);
@ -122,7 +140,10 @@ int main(int argc, char *argv[]) {
batch = (vm.count("batch") > 0);
compare = (vm.count("compare") > 0);
perturb = (vm.count("perturb") > 0);
incremental = (vm.count("incremental") > 0 || (!batch && !compare && !perturb));
stats = (vm.count("stats") > 0);
const int modesSpecified = int(batch) + int(compare) + int(perturb) + int(stats);
incremental = (vm.count("incremental") > 0 || modesSpecified == 0);
dogleg = (vm.count("dogleg") > 0);
if(compare) {
const vector<string>& compareFiles = vm["compare"].as<vector<string> >();
if(compareFiles.size() != 2) {
@ -133,15 +154,15 @@ int main(int argc, char *argv[]) {
compareFile2 = compareFiles[1];
}
if((batch && incremental) || (batch && compare) || (incremental && compare)) {
cout << "Only one of --incremental, --batch, and --compare may be specified\n" << desc << endl;
if(modesSpecified > 1) {
cout << "Only one of --incremental, --batch, --compare, --perturb, and --stats may be specified\n" << desc << endl;
exit(1);
}
if((incremental || batch) && datasetName.empty()) {
cout << "In incremental and batch modes, a dataset must be specified\n" << desc << endl;
exit(1);
}
if(!(incremental || batch) && !datasetName.empty()) {
if(!(incremental || batch || stats) && !datasetName.empty()) {
cout << "A dataset may only be specified in incremental or batch modes\n" << desc << endl;
exit(1);
}
@ -153,6 +174,10 @@ int main(int argc, char *argv[]) {
cout << "In perturb mode, specify input and output files\n" << desc << endl;
exit(1);
}
if(stats && (datasetName.empty() || inputFile.empty())) {
cout << "In stats mode, specify dataset and input file\n" << desc << endl;
exit(1);
}
// Read input file
if(!inputFile.empty())
@ -178,6 +203,19 @@ int main(int argc, char *argv[]) {
}
}
#ifdef GTSAM_USE_TBB
std::auto_ptr<tbb::task_scheduler_init> init;
if(nThreads > 0) {
cout << "Using " << nThreads << " threads" << endl;
init.reset(new tbb::task_scheduler_init(nThreads));
} else
cout << "Using threads for all processors" << endl;
#else
if(nThreads > 0) {
std::cout << "GTSAM is not compiled with TBB, so threading is disabled and the --threads option cannot be used." << endl;
exit(1);
}
#endif
// Run mode
if(incremental)
@ -188,6 +226,8 @@ int main(int argc, char *argv[]) {
runCompare();
else if(perturb)
runPerturb();
else if(stats)
runStats();
return 0;
}
@ -196,6 +236,8 @@ int main(int argc, char *argv[]) {
void runIncremental()
{
ISAM2Params params;
if(dogleg)
params.optimizationParams = ISAM2DoglegParams();
params.relinearizeSkip = relinSkip;
params.enablePartialRelinearizationCheck = true;
ISAM2 isam2(params);
@ -420,7 +462,7 @@ void runBatch()
gttic_(Create_optimizer);
GaussNewtonParams params;
params.linearSolverType = SuccessiveLinearizationParams::MULTIFRONTAL_CHOLESKY;
params.linearSolverType = NonlinearOptimizerParams::MULTIFRONTAL_CHOLESKY;
GaussNewtonOptimizer optimizer(measurements, initial, params);
gttoc_(Create_optimizer);
double lastError;
@ -504,15 +546,14 @@ void runPerturb()
// Perturb values
VectorValues noise;
Ordering ordering = *initial.orderingArbitrary();
BOOST_FOREACH(const Values::KeyValuePair& key_val, initial)
{
Vector noisev(key_val.value.dim());
for(Vector::Index i = 0; i < noisev.size(); ++i)
noisev(i) = normal(rng);
noise.insert(ordering[key_val.key], noisev);
noise.insert(key_val.key, noisev);
}
Values perturbed = initial.retract(noise, ordering);
Values perturbed = initial.retract(noise);
// Write results
try {
@ -527,3 +568,28 @@ void runPerturb()
}
/* ************************************************************************* */
void runStats()
{
cout << "Gathering statistics..." << endl;
GaussianFactorGraph linear = *datasetMeasurements.linearize(initial);
GaussianJunctionTree jt(GaussianEliminationTree(linear, Ordering::COLAMD(linear)));
treeTraversal::ForestStatistics statistics = treeTraversal::GatherStatistics(jt);
ofstream file;
cout << "Writing SolverComparer_Stats_problemSizeHistogram.txt..." << endl;
file.open("SolverComparer_Stats_problemSizeHistogram.txt");
treeTraversal::ForestStatistics::Write(file, statistics.problemSizeHistogram);
file.close();
cout << "Writing SolverComparer_Stats_numberOfChildrenHistogram.txt..." << endl;
file.open("SolverComparer_Stats_numberOfChildrenHistogram.txt");
treeTraversal::ForestStatistics::Write(file, statistics.numberOfChildrenHistogram);
file.close();
cout << "Writing SolverComparer_Stats_problemSizeOfSecondLargestChildHistogram.txt..." << endl;
file.open("SolverComparer_Stats_problemSizeOfSecondLargestChildHistogram.txt");
treeTraversal::ForestStatistics::Write(file, statistics.problemSizeOfSecondLargestChildHistogram);
file.close();
}

195
examples/TimeTBB.cpp Normal file
View File

@ -0,0 +1,195 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file TimeTBB.cpp
* @brief Measure task scheduling overhead in TBB
* @author Richard Roberts
* @date November 6, 2013
*/
#include <gtsam/global_includes.h>
#include <gtsam/base/Matrix.h>
#include <boost/assign/list_of.hpp>
#include <boost/foreach.hpp>
#include <map>
using namespace std;
using namespace gtsam;
using boost::assign::list_of;
#ifdef GTSAM_USE_TBB
#include <tbb/tbb.h>
#undef max // TBB seems to include windows.h and we don't want these macros
#undef min
static const DenseIndex numberOfProblems = 1000000;
static const DenseIndex problemSize = 4;
typedef Eigen::Matrix<double, problemSize, problemSize> FixedMatrix;
/* ************************************************************************* */
struct ResultWithThreads
{
typedef map<int, double>::value_type value_type;
map<int, double> grainSizesWithoutAllocation;
map<int, double> grainSizesWithAllocation;
};
typedef map<int, ResultWithThreads> Results;
/* ************************************************************************* */
struct WorkerWithoutAllocation
{
vector<double>& results;
WorkerWithoutAllocation(vector<double>& results) : results(results) {}
void operator()(const tbb::blocked_range<size_t>& r) const
{
for(size_t i = r.begin(); i != r.end(); ++i)
{
FixedMatrix m1 = FixedMatrix::Random();
FixedMatrix m2 = FixedMatrix::Random();
FixedMatrix prod = m1 * m2;
results[i] = prod.norm();
}
}
};
/* ************************************************************************* */
map<int, double> testWithoutMemoryAllocation()
{
// A function to do some matrix operations without allocating any memory
// Now call it
vector<double> results(numberOfProblems);
const vector<size_t> grainSizes = list_of(1)(10)(100)(1000);
map<int, double> timingResults;
BOOST_FOREACH(size_t grainSize, grainSizes)
{
tbb::tick_count t0 = tbb::tick_count::now();
tbb::parallel_for(tbb::blocked_range<size_t>(0, numberOfProblems), WorkerWithoutAllocation(results));
tbb::tick_count t1 = tbb::tick_count::now();
cout << "Without memory allocation, grain size = " << grainSize << ", time = " << (t1 - t0).seconds() << endl;
timingResults[(int)grainSize] = (t1 - t0).seconds();
}
return timingResults;
}
/* ************************************************************************* */
struct WorkerWithAllocation
{
vector<double>& results;
WorkerWithAllocation(vector<double>& results) : results(results) {}
void operator()(const tbb::blocked_range<size_t>& r) const
{
tbb::cache_aligned_allocator<double> allocator;
for(size_t i = r.begin(); i != r.end(); ++i)
{
double *m1data = allocator.allocate(problemSize * problemSize);
Eigen::Map<Matrix> m1(m1data, problemSize, problemSize);
double *m2data = allocator.allocate(problemSize * problemSize);
Eigen::Map<Matrix> m2(m2data, problemSize, problemSize);
double *proddata = allocator.allocate(problemSize * problemSize);
Eigen::Map<Matrix> prod(proddata, problemSize, problemSize);
m1 = Eigen::Matrix4d::Random(problemSize, problemSize);
m2 = Eigen::Matrix4d::Random(problemSize, problemSize);
prod = m1 * m2;
results[i] = prod.norm();
allocator.deallocate(m1data, problemSize * problemSize);
allocator.deallocate(m2data, problemSize * problemSize);
allocator.deallocate(proddata, problemSize * problemSize);
}
}
};
/* ************************************************************************* */
map<int, double> testWithMemoryAllocation()
{
// A function to do some matrix operations with allocating memory
// Now call it
vector<double> results(numberOfProblems);
const vector<size_t> grainSizes = list_of(1)(10)(100)(1000);
map<int, double> timingResults;
BOOST_FOREACH(size_t grainSize, grainSizes)
{
tbb::tick_count t0 = tbb::tick_count::now();
tbb::parallel_for(tbb::blocked_range<size_t>(0, numberOfProblems), WorkerWithAllocation(results));
tbb::tick_count t1 = tbb::tick_count::now();
cout << "With memory allocation, grain size = " << grainSize << ", time = " << (t1 - t0).seconds() << endl;
timingResults[grainSize] = (t1 - t0).seconds();
}
return timingResults;
}
/* ************************************************************************* */
int main(int argc, char* argv[])
{
cout << "numberOfProblems = " << numberOfProblems << endl;
cout << "problemSize = " << problemSize << endl;
const vector<int> numThreads = list_of(1)(4)(8);
Results results;
BOOST_FOREACH(size_t n, numThreads)
{
cout << "With " << n << " threads:" << endl;
tbb::task_scheduler_init init(n);
results[n].grainSizesWithoutAllocation = testWithoutMemoryAllocation();
results[n].grainSizesWithAllocation = testWithMemoryAllocation();
cout << endl;
}
cout << "Summary of results:" << endl;
BOOST_FOREACH(const Results::value_type& threads_result, results)
{
const int threads = threads_result.first;
const ResultWithThreads& result = threads_result.second;
if(threads != 1)
{
BOOST_FOREACH(const ResultWithThreads::value_type& grainsize_time, result.grainSizesWithoutAllocation)
{
const int grainsize = grainsize_time.first;
const double speedup = results[1].grainSizesWithoutAllocation[grainsize] / grainsize_time.second;
cout << threads << " threads, without allocation, grain size = " << grainsize << ", speedup = " << speedup << endl;
}
BOOST_FOREACH(const ResultWithThreads::value_type& grainsize_time, result.grainSizesWithAllocation)
{
const int grainsize = grainsize_time.first;
const double speedup = results[1].grainSizesWithAllocation[grainsize] / grainsize_time.second;
cout << threads << " threads, with allocation, grain size = " << grainsize << ", speedup = " << speedup << endl;
}
}
}
return 0;
}
#else
/* ************************************************************************* */
int main(int argc, char* argv [])
{
cout << "GTSAM is compiled without TBB, please compile with TBB to use this program." << endl;
return 0;
}
#endif

8
examples/VisualISAM2Example.cpp
View File

@ -31,7 +31,7 @@
// Each variable in the system (poses and landmarks) must be identified with a unique key.
// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
// Here we will use Symbols
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/inference/Symbol.h>
// We want to use iSAM2 to solve the structure-from-motion problem incrementally, so
// include iSAM2 here
@ -90,7 +90,7 @@ int main(int argc, char* argv[]) {
for (size_t j = 0; j < points.size(); ++j) {
SimpleCamera camera(poses[i], *K);
Point2 measurement = camera.project(points[j]);
graph.add(GenericProjectionFactor<Pose3, Point3, Cal3_S2>(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K));
graph.push_back(GenericProjectionFactor<Pose3, Point3, Cal3_S2>(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K));
}
// Add an initial guess for the current pose
@ -104,11 +104,11 @@ int main(int argc, char* argv[]) {
if( i == 0) {
// Add a prior on pose x0
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3),Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
graph.add(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
graph.push_back(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
// Add a prior on landmark l0
noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
graph.add(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise)); // add directly to graph
graph.push_back(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise)); // add directly to graph
// Add initial guesses to all observed landmarks
// Intentionally initialize the variables off from the ground truth

8
examples/VisualISAMExample.cpp
View File

@ -31,7 +31,7 @@
// Each variable in the system (poses and landmarks) must be identified with a unique key.
// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
// Here we will use Symbols
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/inference/Symbol.h>
// In GTSAM, measurement functions are represented as 'factors'. Several common factors
// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
@ -84,7 +84,7 @@ int main(int argc, char* argv[]) {
for (size_t j = 0; j < points.size(); ++j) {
SimpleCamera camera(poses[i], *K);
Point2 measurement = camera.project(points[j]);
graph.add(GenericProjectionFactor<Pose3, Point3, Cal3_S2>(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K));
graph.push_back(GenericProjectionFactor<Pose3, Point3, Cal3_S2>(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K));
}
// Add an initial guess for the current pose
@ -98,11 +98,11 @@ int main(int argc, char* argv[]) {
if( i == 0) {
// Add a prior on pose x0
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
graph.add(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
graph.push_back(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
// Add a prior on landmark l0
noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
graph.add(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise)); // add directly to graph
graph.push_back(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise)); // add directly to graph
// Add initial guesses to all observed landmarks
// Intentionally initialize the variables off from the ground truth

139
examples/VisualSLAMExample.cpp
View File

@ -1,139 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file VisualSLAMExample.cpp
* @brief A visualSLAM example for the structure-from-motion problem on a simulated dataset
* @author Duy-Nguyen Ta
*/
/**
* A structure-from-motion example with landmarks
* - The landmarks form a 10 meter cube
* - The robot rotates around the landmarks, always facing towards the cube
*/
// As this is a full 3D problem, we will use Pose3 variables to represent the camera
// positions and Point3 variables (x, y, z) to represent the landmark coordinates.
// Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).
// We will also need a camera object to hold calibration information and perform projections.
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/SimpleCamera.h>
// Each variable in the system (poses and landmarks) must be identified with a unique key.
// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
// Here we will use Symbols
#include <gtsam/nonlinear/Symbol.h>
// In GTSAM, measurement functions are represented as 'factors'. Several common factors
// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
// Here we will use Projection factors to model the camera's landmark observations.
// Also, we will initialize the robot at some location using a Prior factor.
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/ProjectionFactor.h>
// When the factors are created, we will add them to a Factor Graph. As the factors we are using
// are nonlinear factors, we will need a Nonlinear Factor Graph.
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
// Finally, once all of the factors have been added to our factor graph, we will want to
// solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values.
// GTSAM includes several nonlinear optimizers to perform this step. Here we will use a
// trust-region method known as Powell's Degleg
#include <gtsam/nonlinear/DoglegOptimizer.h>
// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the
// nonlinear functions around an initial linearization point, then solve the linear system
// to update the linearization point. This happens repeatedly until the solver converges
// to a consistent set of variable values. This requires us to specify an initial guess
// for each variable, held in a Values container.
#include <gtsam/nonlinear/Values.h>
#include <vector>
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
int main(int argc, char* argv[]) {
// Define the camera calibration parameters
Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
// Define the camera observation noise model
noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
// Create the set of ground-truth landmarks
std::vector<gtsam::Point3> points;
points.push_back(gtsam::Point3(10.0,10.0,10.0));
points.push_back(gtsam::Point3(-10.0,10.0,10.0));
points.push_back(gtsam::Point3(-10.0,-10.0,10.0));
points.push_back(gtsam::Point3(10.0,-10.0,10.0));
points.push_back(gtsam::Point3(10.0,10.0,-10.0));
points.push_back(gtsam::Point3(-10.0,10.0,-10.0));
points.push_back(gtsam::Point3(-10.0,-10.0,-10.0));
points.push_back(gtsam::Point3(10.0,-10.0,-10.0));
// Create the set of ground-truth poses
std::vector<gtsam::Pose3> poses;
double radius = 30.0;
int i = 0;
double theta = 0.0;
gtsam::Point3 up(0,0,1);
gtsam::Point3 target(0,0,0);
for(; i < 8; ++i, theta += 2*M_PI/8) {
gtsam::Point3 position = Point3(radius*cos(theta), radius*sin(theta), 0.0);
gtsam::SimpleCamera camera = SimpleCamera::Lookat(position, target, up);
poses.push_back(camera.pose());
}
// Create a factor graph
NonlinearFactorGraph graph;
// Add a prior on pose x1. This indirectly specifies where the origin is.
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(Vector_(6, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1)); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
graph.add(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise)); // add directly to graph
// Simulated measurements from each camera pose, adding them to the factor graph
for (size_t i = 0; i < poses.size(); ++i) {
for (size_t j = 0; j < points.size(); ++j) {
SimpleCamera camera(poses[i], *K);
Point2 measurement = camera.project(points[j]);
graph.add(GenericProjectionFactor<Pose3, Point3, Cal3_S2>(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K));
}
}
// Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained
// Here we add a prior on the position of the first landmark. This fixes the scale by indicating the distance
// between the first camera and the first landmark. All other landmark positions are interpreted using this scale.
noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
graph.add(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise)); // add directly to graph
graph.print("Factor Graph:\n");
// Create the data structure to hold the initialEstimate estimate to the solution
// Intentionally initialize the variables off from the ground truth
Values initialEstimate;
for (size_t i = 0; i < poses.size(); ++i)
initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-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.print("Initial Estimates:\n");
/* Optimize the graph and print results */
Values result = DoglegOptimizer(graph, initialEstimate).optimize();
result.print("Final results:\n");
return 0;
}
/* ************************************************************************* */

21
examples/easyPoint2KalmanFilter.cpp
View File

@ -22,7 +22,7 @@
*/
#include <gtsam/nonlinear/ExtendedKalmanFilter.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/geometry/Point2.h>
@ -37,12 +37,13 @@ int main() {
// Create the Kalman Filter initialization point
Point2 x_initial(0.0, 0.0);
SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1));
// Create Key for initial pose
Symbol x0('x',0);
// Create an ExtendedKalmanFilter object
ExtendedKalmanFilter<Point2> ekf(x_initial, P_initial);
ExtendedKalmanFilter<Point2> ekf(x0, x_initial, P_initial);
// Now predict the state at t=1, i.e. argmax_{x1} P(x1) = P(x1|x0) P(x0)
// In Kalman Filter notation, this is x_{t+1|t} and P_{t+1|t}
@ -56,12 +57,12 @@ int main() {
// For the purposes of this example, let us assume we are using a constant-position model and
// the controls are driving the point to the right at 1 m/s. Then, F = [1 0 ; 0 1], B = [1 0 ; 0 1]
// and u = [1 ; 0]. Let us also assume that the process noise Q = [0.1 0 ; 0 0.1].
Vector u = Vector_(2, 1.0, 0.0);
SharedDiagonal Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1), true);
Vector u = (Vector(2) << 1.0, 0.0);
SharedDiagonal Q = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1), true);
// This simple motion can be modeled with a BetweenFactor
// Create Keys
Symbol x0('x',0), x1('x',1);
// Create Key for next pose
Symbol x1('x',1);
// Predict delta based on controls
Point2 difference(1,0);
// Create Factor
@ -82,7 +83,7 @@ int main() {
// For the purposes of this example, let us assume we have something like a GPS that returns
// the current position of the robot. Then H = [1 0 ; 0 1]. Let us also assume that the measurement noise
// R = [0.25 0 ; 0 0.25].
SharedDiagonal R = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25), true);
SharedDiagonal R = noiseModel::Diagonal::Sigmas((Vector(2) << 0.25, 0.25), true);
// This simple measurement can be modeled with a PriorFactor
Point2 z1(1.0, 0.0);

19
examples/elaboratePoint2KalmanFilter.cpp
View File

@ -22,9 +22,8 @@
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
//#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/NoiseModel.h>
@ -59,7 +58,7 @@ int main() {
// Initialize state x0 (2D point) at origin by adding a prior factor, i.e., Bayes net P(x0)
// This is equivalent to x_0 and P_0
Point2 x_initial(0,0);
SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas((Vec(2) << 0.1, 0.1));
PriorFactor<Point2> factor1(x0, x_initial, P_initial);
// Linearize the factor and add it to the linear factor graph
linearizationPoints.insert(x0, x_initial);
@ -90,7 +89,7 @@ int main() {
ordering->insert(x1, 1);
Point2 difference(1,0);
SharedDiagonal Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
SharedDiagonal Q = noiseModel::Diagonal::Sigmas((Vec(2) << 0.1, 0.1));
BetweenFactor<Point2> factor2(x0, x1, difference, Q);
// Linearize the factor and add it to the linear factor graph
linearizationPoints.insert(x1, x_initial);
@ -168,7 +167,7 @@ int main() {
// = (x_{t} - z_{t}) * R^-1 * (x_{t} - z_{t})^T
// This can be modeled using the PriorFactor, where the mean is z_{t} and the covariance is R.
Point2 z1(1.0, 0.0);
SharedDiagonal R1 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25));
SharedDiagonal R1 = noiseModel::Diagonal::Sigmas((Vec(2) << 0.25, 0.25));
PriorFactor<Point2> factor4(x1, z1, R1);
// Linearize the factor and add it to the linear factor graph
linearFactorGraph->push_back(factor4.linearize(linearizationPoints, *ordering));
@ -220,7 +219,7 @@ int main() {
// Create a nonlinear factor describing the motion model
difference = Point2(1,0);
Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
Q = noiseModel::Diagonal::Sigmas((Vec(2) <, 0.1, 0.1));
BetweenFactor<Point2> factor6(x1, x2, difference, Q);
// Linearize the factor and add it to the linear factor graph
@ -258,7 +257,7 @@ int main() {
// And update using z2 ...
Point2 z2(2.0, 0.0);
SharedDiagonal R2 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25));
SharedDiagonal R2 = noiseModel::Diagonal::Sigmas((Vec(2) << 0.25, 0.25));
PriorFactor<Point2> factor8(x2, z2, R2);
// Linearize the factor and add it to the linear factor graph
@ -309,7 +308,7 @@ int main() {
// Create a nonlinear factor describing the motion model
difference = Point2(1,0);
Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
Q = noiseModel::Diagonal::Sigmas((Vec(2) << 0.1, 0.1));
BetweenFactor<Point2> factor10(x2, x3, difference, Q);
// Linearize the factor and add it to the linear factor graph
@ -347,7 +346,7 @@ int main() {
// And update using z3 ...
Point2 z3(3.0, 0.0);
SharedDiagonal R3 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25));
SharedDiagonal R3 = noiseModel::Diagonal::Sigmas((Vec(2) << 0.25, 0.25));
PriorFactor<Point2> factor12(x3, z3, R3);
// Linearize the factor and add it to the linear factor graph

833
gtsam.h
View File

File diff suppressed because it is too large Load Diff

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

@ -15,16 +15,39 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN)
install(FILES Eigen/Eigen/${filename} DESTINATION include/gtsam/3rdparty/Eigen/Eigen)
endif()
endforeach(eigen_dir)
# Add to project source
set(eigen_headers ${eigen_headers} PARENT_SCOPE)
# install Eigen - only the headers in our 3rdparty directory
install(DIRECTORY Eigen/Eigen
install(DIRECTORY Eigen/Eigen
DESTINATION include/gtsam/3rdparty/Eigen
FILES_MATCHING PATTERN "*.h")
endif()
############ NOTE: When updating GeographicLib be sure to disable building their examples
############ by commenting out their line add_subdirectory (examples).
add_subdirectory(GeographicLib)
############ and unit tests by commenting out their lines:
# add_subdirectory (examples)
# set (TOOLS CartConvert ConicProj GeodesicProj GeoConvert GeodSolve
# GeoidEval Gravity MagneticField Planimeter TransverseMercatorProj)
# add_subdirectory (tools)
# Find GeographicLib using the find script distributed with it
include(GeographicLib/cmake/FindGeographicLib.cmake)
# Set up the option to install GeographicLib
if(GEOGRAPHICLIB_FOUND)
set(install_geographiclib_default OFF)
else()
set(install_geographiclib_default ON)
endif()
option(GTSAM_INSTALL_GEOGRAPHICLIB "Build and install the 3rd-party library GeographicLib" ${install_geographiclib_default})
# Print warning if we'll overwrite GeographicLib
if(GEOGRAPHICLIB_FOUND AND GTSAM_INSTALL_GEOGRAPHICLIB)
message(WARNING "GeographicLib is installed on your system and GTSAM_INSTALL_GEOGRAPHICLIB is enabled. Installing gtsam will either overwrite the installed GeographicLib or install a second version that may conflict. You may want to disable GTSAM_INSTALL_GEOGRAPHICLIB if the installed version was not installed by GTSAM.")
endif()
if(GTSAM_INSTALL_GEOGRAPHICLIB)
add_subdirectory(GeographicLib)
endif()

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

@ -204,7 +204,7 @@ if(NOT MSVC)
option(EIGEN_TEST_NEON "Enable/Disable Neon in tests/examples" OFF)
if(EIGEN_TEST_NEON)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mfpu=neon -mcpu=cortex-a"8)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mfpu=neon -mcpu=cortex-a8")
message(STATUS "Enabling NEON in tests/examples")
endif()

15
gtsam/3rdparty/Eigen/Eigen/Eigen2Support vendored
View File

@ -14,12 +14,25 @@
#error Eigen2 support must be enabled by defining EIGEN2_SUPPORT before including any Eigen header
#endif
#ifndef EIGEN_NO_EIGEN2_DEPRECATED_WARNING
#if defined(__GNUC__) || defined(__INTEL_COMPILER) || defined(__clang__)
#warning "Eigen2 support is deprecated in Eigen 3.2.x and it will be removed in Eigen 3.3. (Define EIGEN_NO_EIGEN2_DEPRECATED_WARNING to disable this warning)"
#else
#pragma message ("Eigen2 support is deprecated in Eigen 3.2.x and it will be removed in Eigen 3.3. (Define EIGEN_NO_EIGEN2_DEPRECATED_WARNING to disable this warning)")
#endif
#endif // EIGEN_NO_EIGEN2_DEPRECATED_WARNING
#include "src/Core/util/DisableStupidWarnings.h"
/** \ingroup Support_modules
* \defgroup Eigen2Support_Module Eigen2 support module
* This module provides a couple of deprecated functions improving the compatibility with Eigen2.
*
* \warning Eigen2 support is deprecated in Eigen 3.2.x and it will be removed in Eigen 3.3.
*
* This module provides a couple of deprecated functions improving the compatibility with Eigen2.
*
* To use it, define EIGEN2_SUPPORT before including any Eigen header
* \code
* #define EIGEN2_SUPPORT

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

@ -16,7 +16,10 @@
namespace Eigen {
namespace internal {
template<typename MatrixType, int UpLo> struct LDLT_Traits;
template<typename MatrixType, int UpLo> struct LDLT_Traits;
// PositiveSemiDef means positive semi-definite and non-zero; same for NegativeSemiDef
enum SignMatrix { PositiveSemiDef, NegativeSemiDef, ZeroSign, Indefinite };
}
/** \ingroup Cholesky_Module
@ -69,7 +72,12 @@ template<typename _MatrixType, int _UpLo> class LDLT
* The default constructor is useful in cases in which the user intends to
* perform decompositions via LDLT::compute(const MatrixType&).
*/
LDLT() : m_matrix(), m_transpositions(), m_isInitialized(false) {}
LDLT()
: m_matrix(),
m_transpositions(),
m_sign(internal::ZeroSign),
m_isInitialized(false)
{}
/** \brief Default Constructor with memory preallocation
*
@ -81,6 +89,7 @@ template<typename _MatrixType, int _UpLo> class LDLT
: m_matrix(size, size),
m_transpositions(size),
m_temporary(size),
m_sign(internal::ZeroSign),
m_isInitialized(false)
{}
@ -93,6 +102,7 @@ template<typename _MatrixType, int _UpLo> class LDLT
: m_matrix(matrix.rows(), matrix.cols()),
m_transpositions(matrix.rows()),
m_temporary(matrix.rows()),
m_sign(internal::ZeroSign),
m_isInitialized(false)
{
compute(matrix);
@ -139,7 +149,7 @@ template<typename _MatrixType, int _UpLo> class LDLT
inline bool isPositive() const
{
eigen_assert(m_isInitialized && "LDLT is not initialized.");
return m_sign == 1;
return m_sign == internal::PositiveSemiDef || m_sign == internal::ZeroSign;
}
#ifdef EIGEN2_SUPPORT
@ -153,7 +163,7 @@ template<typename _MatrixType, int _UpLo> class LDLT
inline bool isNegative(void) const
{
eigen_assert(m_isInitialized && "LDLT is not initialized.");
return m_sign == -1;
return m_sign == internal::NegativeSemiDef || m_sign == internal::ZeroSign;
}
/** \returns a solution x of \f$ A x = b \f$ using the current decomposition of A.
@ -235,7 +245,7 @@ template<typename _MatrixType, int _UpLo> class LDLT
MatrixType m_matrix;
TranspositionType m_transpositions;
TmpMatrixType m_temporary;
int m_sign;
internal::SignMatrix m_sign;
bool m_isInitialized;
};
@ -246,7 +256,7 @@ template<int UpLo> struct ldlt_inplace;
template<> struct ldlt_inplace<Lower>
{
template<typename MatrixType, typename TranspositionType, typename Workspace>
static bool unblocked(MatrixType& mat, TranspositionType& transpositions, Workspace& temp, int* sign=0)
static bool unblocked(MatrixType& mat, TranspositionType& transpositions, Workspace& temp, SignMatrix& sign)
{
using std::abs;
typedef typename MatrixType::Scalar Scalar;
@ -258,8 +268,9 @@ template<> struct ldlt_inplace<Lower>
if (size <= 1)
{
transpositions.setIdentity();
if(sign)
*sign = numext::real(mat.coeff(0,0))>0 ? 1:-1;
if (numext::real(mat.coeff(0,0)) > 0) sign = PositiveSemiDef;
else if (numext::real(mat.coeff(0,0)) < 0) sign = NegativeSemiDef;
else sign = ZeroSign;
return true;
}
@ -284,7 +295,6 @@ template<> struct ldlt_inplace<Lower>
if(biggest_in_corner < cutoff)
{
for(Index i = k; i < size; i++) transpositions.coeffRef(i) = i;
if(sign) *sign = 0;
break;
}
@ -325,15 +335,15 @@ template<> struct ldlt_inplace<Lower>
}
if((rs>0) && (abs(mat.coeffRef(k,k)) > cutoff))
A21 /= mat.coeffRef(k,k);
if(sign)
{
// LDLT is not guaranteed to work for indefinite matrices, but let's try to get the sign right
int newSign = numext::real(mat.diagonal().coeff(index_of_biggest_in_corner)) > 0;
if(k == 0)
*sign = newSign;
else if(*sign != newSign)
*sign = 0;
RealScalar realAkk = numext::real(mat.coeffRef(k,k));
if (sign == PositiveSemiDef) {
if (realAkk < 0) sign = Indefinite;
} else if (sign == NegativeSemiDef) {
if (realAkk > 0) sign = Indefinite;
} else if (sign == ZeroSign) {
if (realAkk > 0) sign = PositiveSemiDef;
else if (realAkk < 0) sign = NegativeSemiDef;
}
}
@ -399,7 +409,7 @@ template<> struct ldlt_inplace<Lower>
template<> struct ldlt_inplace<Upper>
{
template<typename MatrixType, typename TranspositionType, typename Workspace>
static EIGEN_STRONG_INLINE bool unblocked(MatrixType& mat, TranspositionType& transpositions, Workspace& temp, int* sign=0)
static EIGEN_STRONG_INLINE bool unblocked(MatrixType& mat, TranspositionType& transpositions, Workspace& temp, SignMatrix& sign)
{
Transpose<MatrixType> matt(mat);
return ldlt_inplace<Lower>::unblocked(matt, transpositions, temp, sign);
@ -445,7 +455,7 @@ LDLT<MatrixType,_UpLo>& LDLT<MatrixType,_UpLo>::compute(const MatrixType& a)
m_isInitialized = false;
m_temporary.resize(size);
internal::ldlt_inplace<UpLo>::unblocked(m_matrix, m_transpositions, m_temporary, &m_sign);
internal::ldlt_inplace<UpLo>::unblocked(m_matrix, m_transpositions, m_temporary, m_sign);
m_isInitialized = true;
return *this;
@ -473,7 +483,7 @@ LDLT<MatrixType,_UpLo>& LDLT<MatrixType,_UpLo>::rankUpdate(const MatrixBase<Deri
for (Index i = 0; i < size; i++)
m_transpositions.coeffRef(i) = i;
m_temporary.resize(size);
m_sign = sigma>=0 ? 1 : -1;
m_sign = sigma>=0 ? internal::PositiveSemiDef : internal::NegativeSemiDef;
m_isInitialized = true;
}

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

@ -58,10 +58,12 @@ cholmod_sparse viewAsCholmod(SparseMatrix<_Scalar,_Options,_Index>& mat)
res.p = mat.outerIndexPtr();
res.i = mat.innerIndexPtr();
res.x = mat.valuePtr();
res.z = 0;
res.sorted = 1;
if(mat.isCompressed())
{
res.packed = 1;
res.nz = 0;
}
else
{
@ -170,6 +172,7 @@ class CholmodBase : internal::noncopyable
CholmodBase()
: m_cholmodFactor(0), m_info(Success), m_isInitialized(false)
{
m_shiftOffset[0] = m_shiftOffset[1] = RealScalar(0.0);
cholmod_start(&m_cholmod);
}
@ -241,7 +244,7 @@ class CholmodBase : internal::noncopyable
return internal::sparse_solve_retval<CholmodBase, Rhs>(*this, b.derived());
}
/** Performs a symbolic decomposition on the sparcity of \a matrix.
/** Performs a symbolic decomposition on the sparsity pattern of \a matrix.
*
* This function is particularly useful when solving for several problems having the same structure.
*
@ -265,7 +268,7 @@ class CholmodBase : internal::noncopyable
/** Performs a numeric decomposition of \a matrix
*
* The given matrix must has the same sparcity than the matrix on which the symbolic decomposition has been performed.
* The given matrix must have the same sparsity pattern as the matrix on which the symbolic decomposition has been performed.
*
* \sa analyzePattern()
*/
@ -302,7 +305,7 @@ class CholmodBase : internal::noncopyable
{
this->m_info = NumericalIssue;
}
// TODO optimize this copy by swapping when possible (be carreful with alignment, etc.)
// TODO optimize this copy by swapping when possible (be careful with alignment, etc.)
dest = Matrix<Scalar,Dest::RowsAtCompileTime,Dest::ColsAtCompileTime>::Map(reinterpret_cast<Scalar*>(x_cd->x),b.rows(),b.cols());
cholmod_free_dense(&x_cd, &m_cholmod);
}
@ -323,7 +326,7 @@ class CholmodBase : internal::noncopyable
{
this->m_info = NumericalIssue;
}
// TODO optimize this copy by swapping when possible (be carreful with alignment, etc.)
// TODO optimize this copy by swapping when possible (be careful with alignment, etc.)
dest = viewAsEigen<DestScalar,DestOptions,DestIndex>(*x_cs);
cholmod_free_sparse(&x_cs, &m_cholmod);
}
@ -365,8 +368,8 @@ class CholmodBase : internal::noncopyable
*
* This class allows to solve for A.X = B sparse linear problems via a simplicial LL^T Cholesky factorization
* using the Cholmod library.
* This simplicial variant is equivalent to Eigen's built-in SimplicialLLT class. Thefore, it has little practical interest.
* The sparse matrix A must be selfajoint and positive definite. The vectors or matrices
* This simplicial variant is equivalent to Eigen's built-in SimplicialLLT class. Therefore, it has little practical interest.
* The sparse matrix A must be selfadjoint and positive definite. The vectors or matrices
* X and B can be either dense or sparse.
*
* \tparam _MatrixType the type of the sparse matrix A, it must be a SparseMatrix<>
@ -412,8 +415,8 @@ class CholmodSimplicialLLT : public CholmodBase<_MatrixType, _UpLo, CholmodSimpl
*
* This class allows to solve for A.X = B sparse linear problems via a simplicial LDL^T Cholesky factorization
* using the Cholmod library.
* This simplicial variant is equivalent to Eigen's built-in SimplicialLDLT class. Thefore, it has little practical interest.
* The sparse matrix A must be selfajoint and positive definite. The vectors or matrices
* This simplicial variant is equivalent to Eigen's built-in SimplicialLDLT class. Therefore, it has little practical interest.
* The sparse matrix A must be selfadjoint and positive definite. The vectors or matrices
* X and B can be either dense or sparse.
*
* \tparam _MatrixType the type of the sparse matrix A, it must be a SparseMatrix<>
@ -458,7 +461,7 @@ class CholmodSimplicialLDLT : public CholmodBase<_MatrixType, _UpLo, CholmodSimp
* This class allows to solve for A.X = B sparse linear problems via a supernodal LL^T Cholesky factorization
* using the Cholmod library.
* This supernodal variant performs best on dense enough problems, e.g., 3D FEM, or very high order 2D FEM.
* The sparse matrix A must be selfajoint and positive definite. The vectors or matrices
* The sparse matrix A must be selfadjoint and positive definite. The vectors or matrices
* X and B can be either dense or sparse.
*
* \tparam _MatrixType the type of the sparse matrix A, it must be a SparseMatrix<>
@ -501,7 +504,7 @@ class CholmodSupernodalLLT : public CholmodBase<_MatrixType, _UpLo, CholmodSuper
* \brief A general Cholesky factorization and solver based on Cholmod
*
* This class allows to solve for A.X = B sparse linear problems via a LL^T or LDL^T Cholesky factorization
* using the Cholmod library. The sparse matrix A must be selfajoint and positive definite. The vectors or matrices
* using the Cholmod library. The sparse matrix A must be selfadjoint and positive definite. The vectors or matrices
* X and B can be either dense or sparse.
*
* This variant permits to change the underlying Cholesky method at runtime.

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

@ -210,7 +210,7 @@ class Array
: Base(other.derived().rows() * other.derived().cols(), other.derived().rows(), other.derived().cols())
{
Base::_check_template_params();
Base::resize(other.rows(), other.cols());
Base::_resize_to_match(other);
*this = other;
}

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

@ -29,9 +29,9 @@ struct all_unroller
};
template<typename Derived>
struct all_unroller<Derived, 1>
struct all_unroller<Derived, 0>
{
static inline bool run(const Derived &mat) { return mat.coeff(0, 0); }
static inline bool run(const Derived &/*mat*/) { return true; }
};
template<typename Derived>
@ -55,9 +55,9 @@ struct any_unroller
};
template<typename Derived>
struct any_unroller<Derived, 1>
struct any_unroller<Derived, 0>
{
static inline bool run(const Derived &mat) { return mat.coeff(0, 0); }
static inline bool run(const Derived & /*mat*/) { return false; }
};
template<typename Derived>

140
gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h vendored
View File

@ -30,9 +30,9 @@ struct CommaInitializer :
{
typedef typename internal::dense_xpr_base<CommaInitializer<XprType> >::type Base;
EIGEN_DENSE_PUBLIC_INTERFACE(CommaInitializer)
typedef typename internal::conditional<internal::must_nest_by_value<XprType>::ret,
XprType, const XprType&>::type ExpressionTypeNested;
typedef typename XprType::InnerIterator InnerIterator;
typedef typename internal::conditional<internal::must_nest_by_value<XprType>::ret,
XprType, const XprType&>::type ExpressionTypeNested;
typedef typename XprType::InnerIterator InnerIterator;
inline CommaInitializer(XprType& xpr, const Scalar& s)
: m_xpr(xpr), m_row(0), m_col(1), m_currentBlockRows(1)
@ -108,67 +108,67 @@ struct CommaInitializer :
*/
inline XprType& finished() { return m_xpr; }
// The following implement the DenseBase interface
inline Index rows() const { return m_xpr.rows(); }
inline Index cols() const { return m_xpr.cols(); }
inline Index outerStride() const { return m_xpr.outerStride(); }
inline Index innerStride() const { return m_xpr.innerStride(); }
inline CoeffReturnType coeff(Index row, Index col) const
{
return m_xpr.coeff(row, col);
}
inline CoeffReturnType coeff(Index index) const
{
return m_xpr.coeff(index);
}
inline const Scalar& coeffRef(Index row, Index col) const
{
return m_xpr.const_cast_derived().coeffRef(row, col);
}
inline const Scalar& coeffRef(Index index) const
{
return m_xpr.const_cast_derived().coeffRef(index);
}
inline Scalar& coeffRef(Index row, Index col)
{
return m_xpr.const_cast_derived().coeffRef(row, col);
}
inline Scalar& coeffRef(Index index)
{
return m_xpr.const_cast_derived().coeffRef(index);
}
template<int LoadMode>
inline const PacketScalar packet(Index row, Index col) const
{
return m_xpr.template packet<LoadMode>(row, col);
}
template<int LoadMode>
inline void writePacket(Index row, Index col, const PacketScalar& x)
{
m_xpr.const_cast_derived().template writePacket<LoadMode>(row, col, x);
}
template<int LoadMode>
inline const PacketScalar packet(Index index) const
{
return m_xpr.template packet<LoadMode>(index);
}
template<int LoadMode>
inline void writePacket(Index index, const PacketScalar& x)
{
m_xpr.const_cast_derived().template writePacket<LoadMode>(index, x);
}
// The following implement the DenseBase interface
inline Index rows() const { return m_xpr.rows(); }
inline Index cols() const { return m_xpr.cols(); }
inline Index outerStride() const { return m_xpr.outerStride(); }
inline Index innerStride() const { return m_xpr.innerStride(); }
inline CoeffReturnType coeff(Index row, Index col) const
{
return m_xpr.coeff(row, col);
}
inline CoeffReturnType coeff(Index index) const
{
return m_xpr.coeff(index);
}
inline const Scalar& coeffRef(Index row, Index col) const
{
return m_xpr.const_cast_derived().coeffRef(row, col);
}
inline const Scalar& coeffRef(Index index) const
{
return m_xpr.const_cast_derived().coeffRef(index);
}
inline Scalar& coeffRef(Index row, Index col)
{
return m_xpr.const_cast_derived().coeffRef(row, col);
}
inline Scalar& coeffRef(Index index)
{
return m_xpr.const_cast_derived().coeffRef(index);
}
template<int LoadMode>
inline const PacketScalar packet(Index row, Index col) const
{
return m_xpr.template packet<LoadMode>(row, col);
}
template<int LoadMode>
inline void writePacket(Index row, Index col, const PacketScalar& x)
{
m_xpr.const_cast_derived().template writePacket<LoadMode>(row, col, x);
}
template<int LoadMode>
inline const PacketScalar packet(Index index) const
{
return m_xpr.template packet<LoadMode>(index);
}
template<int LoadMode>
inline void writePacket(Index index, const PacketScalar& x)
{
m_xpr.const_cast_derived().template writePacket<LoadMode>(index, x);
}
const XprType& _expression() const { return m_xpr; }
XprType& m_xpr; // target expression
@ -176,12 +176,12 @@ struct CommaInitializer :
Index m_col; // current col id
Index m_currentBlockRows; // current block height
};
namespace internal {
template<typename XprType>
struct traits<CommaInitializer<XprType> > : traits<XprType>
{
};
namespace internal {
template<typename XprType>
struct traits<CommaInitializer<XprType> > : traits<XprType>
{
};
}
/** \anchor MatrixBaseCommaInitRef

30
gtsam/3rdparty/Eigen/Eigen/src/Core/EigenBase.h vendored
View File

@ -126,36 +126,6 @@ Derived& DenseBase<Derived>::operator-=(const EigenBase<OtherDerived> &other)
return derived();
}
/** replaces \c *this by \c *this * \a other.
*
* \returns a reference to \c *this
*/
template<typename Derived>
template<typename OtherDerived>
inline Derived&
MatrixBase<Derived>::operator*=(const EigenBase<OtherDerived> &other)
{
other.derived().applyThisOnTheRight(derived());
return derived();
}
/** replaces \c *this by \c *this * \a other. It is equivalent to MatrixBase::operator*=().
*/
template<typename Derived>
template<typename OtherDerived>
inline void MatrixBase<Derived>::applyOnTheRight(const EigenBase<OtherDerived> &other)
{
other.derived().applyThisOnTheRight(derived());
}
/** replaces \c *this by \c *this * \a other. */
template<typename Derived>
template<typename OtherDerived>
inline void MatrixBase<Derived>::applyOnTheLeft(const EigenBase<OtherDerived> &other)
{
other.derived().applyThisOnTheLeft(derived());
}
} // end namespace Eigen
#endif // EIGEN_EIGENBASE_H

9
gtsam/3rdparty/Eigen/Eigen/src/Core/IO.h vendored
View File

@ -185,21 +185,22 @@ std::ostream & print_matrix(std::ostream & s, const Derived& _m, const IOFormat&
explicit_precision = fmt.precision;
}
std::streamsize old_precision = 0;
if(explicit_precision) old_precision = s.precision(explicit_precision);
bool align_cols = !(fmt.flags & DontAlignCols);
if(align_cols)
{
// compute the largest width
for(Index j = 1; j < m.cols(); ++j)
for(Index j = 0; j < m.cols(); ++j)
for(Index i = 0; i < m.rows(); ++i)
{
std::stringstream sstr;
if(explicit_precision) sstr.precision(explicit_precision);
sstr.copyfmt(s);
sstr << m.coeff(i,j);
width = std::max<Index>(width, Index(sstr.str().length()));
}
}
std::streamsize old_precision = 0;
if(explicit_precision) old_precision = s.precision(explicit_precision);
s << fmt.matPrefix;
for(Index i = 0; i < m.rows(); ++i)
{

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

@ -304,7 +304,7 @@ class Matrix
: Base(other.derived().rows() * other.derived().cols(), other.derived().rows(), other.derived().cols())
{
Base::_check_template_params();
Base::resize(other.rows(), other.cols());
Base::_resize_to_match(other);
// FIXME/CHECK: isn't *this = other.derived() more efficient. it allows to
// go for pure _set() implementations, right?
*this = other;

45
gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h vendored
View File

@ -510,6 +510,51 @@ template<typename Derived> class MatrixBase
{EIGEN_STATIC_ASSERT(std::ptrdiff_t(sizeof(typename OtherDerived::Scalar))==-1,YOU_CANNOT_MIX_ARRAYS_AND_MATRICES); return *this;}
};
/***************************************************************************
* Implementation of matrix base methods
***************************************************************************/
/** replaces \c *this by \c *this * \a other.
*
* \returns a reference to \c *this
*
* Example: \include MatrixBase_applyOnTheRight.cpp
* Output: \verbinclude MatrixBase_applyOnTheRight.out
*/
template<typename Derived>
template<typename OtherDerived>
inline Derived&
MatrixBase<Derived>::operator*=(const EigenBase<OtherDerived> &other)
{
other.derived().applyThisOnTheRight(derived());
return derived();
}
/** replaces \c *this by \c *this * \a other. It is equivalent to MatrixBase::operator*=().
*
* Example: \include MatrixBase_applyOnTheRight.cpp
* Output: \verbinclude MatrixBase_applyOnTheRight.out
*/
template<typename Derived>
template<typename OtherDerived>
inline void MatrixBase<Derived>::applyOnTheRight(const EigenBase<OtherDerived> &other)
{
other.derived().applyThisOnTheRight(derived());
}
/** replaces \c *this by \a other * \c *this.
*
* Example: \include MatrixBase_applyOnTheLeft.cpp
* Output: \verbinclude MatrixBase_applyOnTheLeft.out
*/
template<typename Derived>
template<typename OtherDerived>
inline void MatrixBase<Derived>::applyOnTheLeft(const EigenBase<OtherDerived> &other)
{
other.derived().applyThisOnTheLeft(derived());
}
} // end namespace Eigen
#endif // EIGEN_MATRIXBASE_H

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

@ -553,7 +553,8 @@ struct permut_matrix_product_retval
template<typename Dest> inline void evalTo(Dest& dst) const
{
const Index n = Side==OnTheLeft ? rows() : cols();
// FIXME we need an is_same for expression that is not sensitive to constness. For instance
// is_same_xpr<Block<const Matrix>, Block<Matrix> >::value should be true.
if(is_same<MatrixTypeNestedCleaned,Dest>::value && extract_data(dst) == extract_data(m_matrix))
{
// apply the permutation inplace

16
gtsam/3rdparty/Eigen/Eigen/src/Core/PlainObjectBase.h vendored
View File

@ -47,7 +47,10 @@ template<> struct check_rows_cols_for_overflow<Dynamic> {
}
};
template <typename Derived, typename OtherDerived = Derived, bool IsVector = bool(Derived::IsVectorAtCompileTime)> struct conservative_resize_like_impl;
template <typename Derived,
typename OtherDerived = Derived,
bool IsVector = bool(Derived::IsVectorAtCompileTime) && bool(OtherDerived::IsVectorAtCompileTime)>
struct conservative_resize_like_impl;
template<typename MatrixTypeA, typename MatrixTypeB, bool SwapPointers> struct matrix_swap_impl;
@ -668,8 +671,10 @@ private:
enum { ThisConstantIsPrivateInPlainObjectBase };
};
namespace internal {
template <typename Derived, typename OtherDerived, bool IsVector>
struct internal::conservative_resize_like_impl
struct conservative_resize_like_impl
{
typedef typename Derived::Index Index;
static void run(DenseBase<Derived>& _this, Index rows, Index cols)
@ -729,11 +734,14 @@ struct internal::conservative_resize_like_impl
}
};
namespace internal {
// Here, the specialization for vectors inherits from the general matrix case
// to allow calling .conservativeResize(rows,cols) on vectors.
template <typename Derived, typename OtherDerived>
struct conservative_resize_like_impl<Derived,OtherDerived,true>
: conservative_resize_like_impl<Derived,OtherDerived,false>
{
using conservative_resize_like_impl<Derived,OtherDerived,false>::run;
typedef typename Derived::Index Index;
static void run(DenseBase<Derived>& _this, Index size)
{

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

@ -94,7 +94,8 @@ struct traits<Ref<_PlainObjectType, _Options, _StrideType> >
typedef _PlainObjectType PlainObjectType;
typedef _StrideType StrideType;
enum {
Options = _Options
Options = _Options,
Flags = traits<Map<_PlainObjectType, _Options, _StrideType> >::Flags | NestByRefBit
};
template<typename Derived> struct match {
@ -111,7 +112,7 @@ struct traits<Ref<_PlainObjectType, _Options, _StrideType> >
};
typedef typename internal::conditional<MatchAtCompileTime,internal::true_type,internal::false_type>::type type;
};
};
template<typename Derived>

27
gtsam/3rdparty/Eigen/Eigen/src/Core/StableNorm.h vendored
View File

@ -17,16 +17,29 @@ namespace internal {
template<typename ExpressionType, typename Scalar>
inline void stable_norm_kernel(const ExpressionType& bl, Scalar& ssq, Scalar& scale, Scalar& invScale)
{
Scalar max = bl.cwiseAbs().maxCoeff();
if (max>scale)
using std::max;
Scalar maxCoeff = bl.cwiseAbs().maxCoeff();
if (maxCoeff>scale)
{
ssq = ssq * numext::abs2(scale/max);
scale = max;
invScale = Scalar(1)/scale;
ssq = ssq * numext::abs2(scale/maxCoeff);
Scalar tmp = Scalar(1)/maxCoeff;
if(tmp > NumTraits<Scalar>::highest())
{
invScale = NumTraits<Scalar>::highest();
scale = Scalar(1)/invScale;
}
else
{
scale = maxCoeff;
invScale = tmp;
}
}
// TODO if the max is much much smaller than the current scale,
// TODO if the maxCoeff is much much smaller than the current scale,
// then we can neglect this sub vector
ssq += (bl*invScale).squaredNorm();
if(scale>Scalar(0)) // if scale==0, then bl is 0
ssq += (bl*invScale).squaredNorm();
}
template<typename Derived>

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

@ -284,7 +284,8 @@ struct inplace_transpose_selector<MatrixType,false> { // non square matrix
* Notice however that this method is only useful if you want to replace a matrix by its own transpose.
* If you just need the transpose of a matrix, use transpose().
*
* \note if the matrix is not square, then \c *this must be a resizable matrix.
* \note if the matrix is not square, then \c *this must be a resizable matrix.
* This excludes (non-square) fixed-size matrices, block-expressions and maps.
*
* \sa transpose(), adjoint(), adjointInPlace() */
template<typename Derived>
@ -315,6 +316,7 @@ inline void DenseBase<Derived>::transposeInPlace()
* If you just need the adjoint of a matrix, use adjoint().
*
* \note if the matrix is not square, then \c *this must be a resizable matrix.
* This excludes (non-square) fixed-size matrices, block-expressions and maps.
*
* \sa transpose(), adjoint(), transposeInPlace() */
template<typename Derived>

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

@ -50,7 +50,7 @@ struct traits<PartialReduxExpr<MatrixType, MemberOp, Direction> >
MaxColsAtCompileTime = Direction==Horizontal ? 1 : MatrixType::MaxColsAtCompileTime,
Flags0 = (unsigned int)_MatrixTypeNested::Flags & HereditaryBits,
Flags = (Flags0 & ~RowMajorBit) | (RowsAtCompileTime == 1 ? RowMajorBit : 0),
TraversalSize = Direction==Vertical ? RowsAtCompileTime : ColsAtCompileTime
TraversalSize = Direction==Vertical ? MatrixType::RowsAtCompileTime : MatrixType::ColsAtCompileTime
};
#if EIGEN_GNUC_AT_LEAST(3,4)
typedef typename MemberOp::template Cost<InputScalar,int(TraversalSize)> CostOpType;
@ -58,7 +58,8 @@ struct traits<PartialReduxExpr<MatrixType, MemberOp, Direction> >
typedef typename MemberOp::template Cost<InputScalar,TraversalSize> CostOpType;
#endif
enum {
CoeffReadCost = TraversalSize * traits<_MatrixTypeNested>::CoeffReadCost + int(CostOpType::value)
CoeffReadCost = TraversalSize==Dynamic ? Dynamic
: TraversalSize * traits<_MatrixTypeNested>::CoeffReadCost + int(CostOpType::value)
};
};
}

11
gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/MathFunctions.h vendored
View File

@ -442,8 +442,11 @@ Packet4f pcos<Packet4f>(const Packet4f& _x)
return _mm_xor_ps(y, sign_bit);
}
#if EIGEN_FAST_MATH
// This is based on Quake3's fast inverse square root.
// For detail see here: http://www.beyond3d.com/content/articles/8/
// It lacks 1 (or 2 bits in some rare cases) of precision, and does not handle negative, +inf, or denormalized numbers correctly.
template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
Packet4f psqrt<Packet4f>(const Packet4f& _x)
{
@ -457,6 +460,14 @@ Packet4f psqrt<Packet4f>(const Packet4f& _x)
return pmul(_x,x);
}
#else
template<> EIGEN_STRONG_INLINE Packet4f psqrt<Packet4f>(const Packet4f& x) { return _mm_sqrt_ps(x); }
#endif
template<> EIGEN_STRONG_INLINE Packet2d psqrt<Packet2d>(const Packet2d& x) { return _mm_sqrt_pd(x); }
} // end namespace internal
} // end namespace Eigen

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

@ -83,7 +83,8 @@ template<> struct packet_traits<double> : default_packet_traits
size=2,
HasDiv = 1,
HasExp = 1
HasExp = 1,
HasSqrt = 1
};
};
template<> struct packet_traits<int> : default_packet_traits
@ -507,8 +508,8 @@ template<> EIGEN_STRONG_INLINE int predux_min<Packet4i>(const Packet4i& a)
// for GCC (eg., it does not like using std::min after the pstore !!)
EIGEN_ALIGN16 int aux[4];
pstore(aux, a);
register int aux0 = aux[0]<aux[1] ? aux[0] : aux[1];
register int aux2 = aux[2]<aux[3] ? aux[2] : aux[3];
int aux0 = aux[0]<aux[1] ? aux[0] : aux[1];
int aux2 = aux[2]<aux[3] ? aux[2] : aux[3];
return aux0<aux2 ? aux0 : aux2;
}
@ -528,8 +529,8 @@ template<> EIGEN_STRONG_INLINE int predux_max<Packet4i>(const Packet4i& a)
// for GCC (eg., it does not like using std::min after the pstore !!)
EIGEN_ALIGN16 int aux[4];
pstore(aux, a);
register int aux0 = aux[0]>aux[1] ? aux[0] : aux[1];
register int aux2 = aux[2]>aux[3] ? aux[2] : aux[3];
int aux0 = aux[0]>aux[1] ? aux[0] : aux[1];
int aux2 = aux[2]>aux[3] ? aux[2] : aux[3];
return aux0>aux2 ? aux0 : aux2;
}

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

@ -1128,6 +1128,8 @@ EIGEN_DONT_INLINE void gemm_pack_lhs<Scalar, Index, Pack1, Pack2, StorageOrder,
enum { PacketSize = packet_traits<Scalar>::size };
EIGEN_ASM_COMMENT("EIGEN PRODUCT PACK LHS");
EIGEN_UNUSED_VARIABLE(stride)
EIGEN_UNUSED_VARIABLE(offset)
eigen_assert(((!PanelMode) && stride==0 && offset==0) || (PanelMode && stride>=depth && offset<=stride));
eigen_assert( (StorageOrder==RowMajor) || ((Pack1%PacketSize)==0 && Pack1<=4*PacketSize) );
conj_if<NumTraits<Scalar>::IsComplex && Conjugate> cj;
@ -1215,6 +1217,8 @@ EIGEN_DONT_INLINE void gemm_pack_rhs<Scalar, Index, nr, ColMajor, Conjugate, Pan
::operator()(Scalar* blockB, const Scalar* rhs, Index rhsStride, Index depth, Index cols, Index stride, Index offset)
{
EIGEN_ASM_COMMENT("EIGEN PRODUCT PACK RHS COLMAJOR");
EIGEN_UNUSED_VARIABLE(stride)
EIGEN_UNUSED_VARIABLE(offset)
eigen_assert(((!PanelMode) && stride==0 && offset==0) || (PanelMode && stride>=depth && offset<=stride));
conj_if<NumTraits<Scalar>::IsComplex && Conjugate> cj;
Index packet_cols = (cols/nr) * nr;
@ -1266,6 +1270,8 @@ EIGEN_DONT_INLINE void gemm_pack_rhs<Scalar, Index, nr, RowMajor, Conjugate, Pan
::operator()(Scalar* blockB, const Scalar* rhs, Index rhsStride, Index depth, Index cols, Index stride, Index offset)
{
EIGEN_ASM_COMMENT("EIGEN PRODUCT PACK RHS ROWMAJOR");
EIGEN_UNUSED_VARIABLE(stride)
EIGEN_UNUSED_VARIABLE(offset)
eigen_assert(((!PanelMode) && stride==0 && offset==0) || (PanelMode && stride>=depth && offset<=stride));
conj_if<NumTraits<Scalar>::IsComplex && Conjugate> cj;
Index packet_cols = (cols/nr) * nr;

15
gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixVector.h vendored
View File

@ -52,11 +52,7 @@ EIGEN_DONT_INLINE static void run(
Index rows, Index cols,
const LhsScalar* lhs, Index lhsStride,
const RhsScalar* rhs, Index rhsIncr,
ResScalar* res, Index
#ifdef EIGEN_INTERNAL_DEBUGGING
resIncr
#endif
, RhsScalar alpha);
ResScalar* res, Index resIncr, RhsScalar alpha);
};
template<typename Index, typename LhsScalar, bool ConjugateLhs, typename RhsScalar, bool ConjugateRhs, int Version>
@ -64,12 +60,9 @@ EIGEN_DONT_INLINE void general_matrix_vector_product<Index,LhsScalar,ColMajor,Co
Index rows, Index cols,
const LhsScalar* lhs, Index lhsStride,
const RhsScalar* rhs, Index rhsIncr,
ResScalar* res, Index
#ifdef EIGEN_INTERNAL_DEBUGGING
resIncr
#endif
, RhsScalar alpha)
ResScalar* res, Index resIncr, RhsScalar alpha)
{
EIGEN_UNUSED_VARIABLE(resIncr)
eigen_internal_assert(resIncr==1);
#ifdef _EIGEN_ACCUMULATE_PACKETS
#error _EIGEN_ACCUMULATE_PACKETS has already been defined
@ -265,7 +258,7 @@ EIGEN_DONT_INLINE void general_matrix_vector_product<Index,LhsScalar,ColMajor,Co
// process aligned result's coeffs
if ((size_t(lhs0+alignedStart)%sizeof(LhsPacket))==0)
for (Index i = alignedStart;i<alignedSize;i+=ResPacketSize)
pstore(&res[i], pcj.pmadd(ploadu<LhsPacket>(&lhs0[i]), ptmp0, pload<ResPacket>(&res[i])));
pstore(&res[i], pcj.pmadd(pload<LhsPacket>(&lhs0[i]), ptmp0, pload<ResPacket>(&res[i])));
else
for (Index i = alignedStart;i<alignedSize;i+=ResPacketSize)
pstore(&res[i], pcj.pmadd(ploadu<LhsPacket>(&lhs0[i]), ptmp0, pload<ResPacket>(&res[i])));

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

@ -79,8 +79,8 @@ EIGEN_DONT_INLINE void selfadjoint_matrix_vector_product<Scalar,Index,StorageOrd
for (Index j=FirstTriangular ? bound : 0;
j<(FirstTriangular ? size : bound);j+=2)
{
register const Scalar* EIGEN_RESTRICT A0 = lhs + j*lhsStride;
register const Scalar* EIGEN_RESTRICT A1 = lhs + (j+1)*lhsStride;
const Scalar* EIGEN_RESTRICT A0 = lhs + j*lhsStride;
const Scalar* EIGEN_RESTRICT A1 = lhs + (j+1)*lhsStride;
Scalar t0 = cjAlpha * rhs[j];
Packet ptmp0 = pset1<Packet>(t0);
@ -147,7 +147,7 @@ EIGEN_DONT_INLINE void selfadjoint_matrix_vector_product<Scalar,Index,StorageOrd
}
for (Index j=FirstTriangular ? 0 : bound;j<(FirstTriangular ? bound : size);j++)
{
register const Scalar* EIGEN_RESTRICT A0 = lhs + j*lhsStride;
const Scalar* EIGEN_RESTRICT A0 = lhs + j*lhsStride;
Scalar t1 = cjAlpha * rhs[j];
Scalar t2(0);

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

@ -13,7 +13,7 @@
#define EIGEN_WORLD_VERSION 3
#define EIGEN_MAJOR_VERSION 2
#define EIGEN_MINOR_VERSION 0
#define EIGEN_MINOR_VERSION 1
#define EIGEN_VERSION_AT_LEAST(x,y,z) (EIGEN_WORLD_VERSION>x || (EIGEN_WORLD_VERSION>=x && \
(EIGEN_MAJOR_VERSION>y || (EIGEN_MAJOR_VERSION>=y && \
@ -238,7 +238,12 @@
#endif
// Suppresses 'unused variable' warnings.
#define EIGEN_UNUSED_VARIABLE(var) (void)var;
namespace Eigen {
namespace internal {
template<typename T> void ignore_unused_variable(const T&) {}
}
}
#define EIGEN_UNUSED_VARIABLE(var) Eigen::internal::ignore_unused_variable(var);
#if !defined(EIGEN_ASM_COMMENT)
#if (defined __GNUC__) && ( defined(__i386__) || defined(__x86_64__) )

13
gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h vendored
View File

@ -578,7 +578,7 @@ template<typename T> class aligned_stack_memory_handler
*/
#ifdef EIGEN_ALLOCA
#ifdef __arm__
#if defined(__arm__) || defined(_WIN32)
#define EIGEN_ALIGNED_ALLOCA(SIZE) reinterpret_cast<void*>((reinterpret_cast<size_t>(EIGEN_ALLOCA(SIZE+16)) & ~(size_t(15))) + 16)
#else
#define EIGEN_ALIGNED_ALLOCA EIGEN_ALLOCA
@ -634,7 +634,9 @@ template<typename T> class aligned_stack_memory_handler
/* memory allocated we can safely let the default implementation handle */ \
/* this particular case. */ \
static void *operator new(size_t size, void *ptr) { return ::operator new(size,ptr); } \
static void *operator new[](size_t size, void* ptr) { return ::operator new[](size,ptr); } \
void operator delete(void * memory, void *ptr) throw() { return ::operator delete(memory,ptr); } \
void operator delete[](void * memory, void *ptr) throw() { return ::operator delete[](memory,ptr); } \
/* nothrow-new (returns zero instead of std::bad_alloc) */ \
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_NOTHROW(NeedsToAlign) \
void operator delete(void *ptr, const std::nothrow_t&) throw() { \
@ -729,15 +731,6 @@ public:
::new( p ) T( value );
}
// Support for c++11
#if (__cplusplus >= 201103L)
template<typename... Args>
void construct(pointer p, Args&&... args)
{
::new(p) T(std::forward<Args>(args)...);
}
#endif
void destroy( pointer p )
{
p->~T();

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

@ -512,8 +512,7 @@ template<typename MatrixType>
template<typename OtherDerived, typename ResultType>
bool SVD<MatrixType>::solve(const MatrixBase<OtherDerived> &b, ResultType* result) const
{
const int rows = m_matU.rows();
ei_assert(b.rows() == rows);
ei_assert(b.rows() == m_matU.rows());
Scalar maxVal = m_sigma.cwise().abs().maxCoeff();
for (int j=0; j<b.cols(); ++j)

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

@ -28,7 +28,7 @@ namespace Eigen {
* * AngleAxisf(ea[2], Vector3f::UnitZ()); \endcode
* This corresponds to the right-multiply conventions (with right hand side frames).
*
* The returned angles are in the ranges [0:pi]x[0:pi]x[-pi:pi].
* The returned angles are in the ranges [0:pi]x[-pi:pi]x[-pi:pi].
*
* \sa class AngleAxis
*/

27
gtsam/3rdparty/Eigen/Eigen/src/Geometry/Quaternion.h vendored
View File

@ -150,10 +150,6 @@ public:
/** \returns the conjugated quaternion */
Quaternion<Scalar> conjugate() const;
/** \returns an interpolation for a constant motion between \a other and \c *this
* \a t in [0;1]
* see http://en.wikipedia.org/wiki/Slerp
*/
template<class OtherDerived> Quaternion<Scalar> slerp(const Scalar& t, const QuaternionBase<OtherDerived>& other) const;
/** \returns \c true if \c *this is approximately equal to \a other, within the precision
@ -194,11 +190,11 @@ public:
* \brief The quaternion class used to represent 3D orientations and rotations
*
* \tparam _Scalar the scalar type, i.e., the type of the coefficients
* \tparam _Options controls the memory alignement of the coeffecients. Can be \# AutoAlign or \# DontAlign. Default is AutoAlign.
* \tparam _Options controls the memory alignment of the coefficients. Can be \# AutoAlign or \# DontAlign. Default is AutoAlign.
*
* This class represents a quaternion \f$ w+xi+yj+zk \f$ that is a convenient representation of
* orientations and rotations of objects in three dimensions. Compared to other representations
* like Euler angles or 3x3 matrices, quatertions offer the following advantages:
* like Euler angles or 3x3 matrices, quaternions offer the following advantages:
* \li \b compact storage (4 scalars)
* \li \b efficient to compose (28 flops),
* \li \b stable spherical interpolation
@ -385,7 +381,7 @@ class Map<Quaternion<_Scalar>, _Options >
/** Constructs a Mapped Quaternion object from the pointer \a coeffs
*
* The pointer \a coeffs must reference the four coeffecients of Quaternion in the following order:
* The pointer \a coeffs must reference the four coefficients of Quaternion in the following order:
* \code *coeffs == {x, y, z, w} \endcode
*
* If the template parameter _Options is set to #Aligned, then the pointer coeffs must be aligned. */
@ -399,16 +395,16 @@ class Map<Quaternion<_Scalar>, _Options >
};
/** \ingroup Geometry_Module
* Map an unaligned array of single precision scalar as a quaternion */
* Map an unaligned array of single precision scalars as a quaternion */
typedef Map<Quaternion<float>, 0> QuaternionMapf;
/** \ingroup Geometry_Module
* Map an unaligned array of double precision scalar as a quaternion */
* Map an unaligned array of double precision scalars as a quaternion */
typedef Map<Quaternion<double>, 0> QuaternionMapd;
/** \ingroup Geometry_Module
* Map a 16-bits aligned array of double precision scalars as a quaternion */
* Map a 16-byte aligned array of single precision scalars as a quaternion */
typedef Map<Quaternion<float>, Aligned> QuaternionMapAlignedf;
/** \ingroup Geometry_Module
* Map a 16-bits aligned array of double precision scalars as a quaternion */
* Map a 16-byte aligned array of double precision scalars as a quaternion */
typedef Map<Quaternion<double>, Aligned> QuaternionMapAlignedd;
/***************************************************************************
@ -579,7 +575,7 @@ inline Derived& QuaternionBase<Derived>::setFromTwoVectors(const MatrixBase<Deri
Scalar c = v1.dot(v0);
// if dot == -1, vectors are nearly opposites
// => accuraletly compute the rotation axis by computing the
// => accurately compute the rotation axis by computing the
// intersection of the two planes. This is done by solving:
// x^T v0 = 0
// x^T v1 = 0
@ -677,8 +673,13 @@ QuaternionBase<Derived>::angularDistance(const QuaternionBase<OtherDerived>& oth
return static_cast<Scalar>(2 * acos(d));
}
/** \returns the spherical linear interpolation between the two quaternions
* \c *this and \a other at the parameter \a t
* \c *this and \a other at the parameter \a t in [0;1].
*
* This represents an interpolation for a constant motion between \c *this and \a other,
* see also http://en.wikipedia.org/wiki/Slerp.
*/
template <class Derived>
template <class OtherDerived>

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

@ -530,9 +530,9 @@ public:
inline Transform& operator=(const UniformScaling<Scalar>& t);
inline Transform& operator*=(const UniformScaling<Scalar>& s) { return scale(s.factor()); }
inline Transform<Scalar,Dim,(int(Mode)==int(Isometry)?Affine:Isometry)> operator*(const UniformScaling<Scalar>& s) const
inline Transform<Scalar,Dim,(int(Mode)==int(Isometry)?int(Affine):int(Mode))> operator*(const UniformScaling<Scalar>& s) const
{
Transform<Scalar,Dim,(int(Mode)==int(Isometry)?Affine:Isometry),Options> res = *this;
Transform<Scalar,Dim,(int(Mode)==int(Isometry)?int(Affine):int(Mode)),Options> res = *this;
res.scale(s.factor());
return res;
}

2
gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR.h vendored
View File

@ -349,7 +349,7 @@ template<typename _MatrixType> class ColPivHouseholderQR
return m_usePrescribedThreshold ? m_prescribedThreshold
// this formula comes from experimenting (see "LU precision tuning" thread on the list)
// and turns out to be identical to Higham's formula used already in LDLt.
: NumTraits<Scalar>::epsilon() * m_qr.diagonalSize();
: NumTraits<Scalar>::epsilon() * RealScalar(m_qr.diagonalSize());
}
/** \returns the number of nonzero pivots in the QR decomposition.

53
gtsam/3rdparty/Eigen/Eigen/src/QR/FullPivHouseholderQR.h vendored
View File

@ -63,9 +63,10 @@ template<typename _MatrixType> class FullPivHouseholderQR
typedef typename MatrixType::Index Index;
typedef internal::FullPivHouseholderQRMatrixQReturnType<MatrixType> MatrixQReturnType;
typedef typename internal::plain_diag_type<MatrixType>::type HCoeffsType;
typedef Matrix<Index, 1, ColsAtCompileTime, RowMajor, 1, MaxColsAtCompileTime> IntRowVectorType;
typedef Matrix<Index, 1,
EIGEN_SIZE_MIN_PREFER_DYNAMIC(ColsAtCompileTime,RowsAtCompileTime), RowMajor, 1,
EIGEN_SIZE_MIN_PREFER_FIXED(MaxColsAtCompileTime,MaxRowsAtCompileTime)> IntDiagSizeVectorType;
typedef PermutationMatrix<ColsAtCompileTime, MaxColsAtCompileTime> PermutationType;
typedef typename internal::plain_col_type<MatrixType, Index>::type IntColVectorType;
typedef typename internal::plain_row_type<MatrixType>::type RowVectorType;
typedef typename internal::plain_col_type<MatrixType>::type ColVectorType;
@ -93,10 +94,10 @@ template<typename _MatrixType> class FullPivHouseholderQR
FullPivHouseholderQR(Index rows, Index cols)
: m_qr(rows, cols),
m_hCoeffs((std::min)(rows,cols)),
m_rows_transpositions(rows),
m_cols_transpositions(cols),
m_rows_transpositions((std::min)(rows,cols)),
m_cols_transpositions((std::min)(rows,cols)),
m_cols_permutation(cols),
m_temp((std::min)(rows,cols)),
m_temp(cols),
m_isInitialized(false),
m_usePrescribedThreshold(false) {}
@ -115,10 +116,10 @@ template<typename _MatrixType> class FullPivHouseholderQR
FullPivHouseholderQR(const MatrixType& matrix)
: m_qr(matrix.rows(), matrix.cols()),
m_hCoeffs((std::min)(matrix.rows(), matrix.cols())),
m_rows_transpositions(matrix.rows()),
m_cols_transpositions(matrix.cols()),
m_rows_transpositions((std::min)(matrix.rows(), matrix.cols())),
m_cols_transpositions((std::min)(matrix.rows(), matrix.cols())),
m_cols_permutation(matrix.cols()),
m_temp((std::min)(matrix.rows(), matrix.cols())),
m_temp(matrix.cols()),
m_isInitialized(false),
m_usePrescribedThreshold(false)
{
@ -126,11 +127,12 @@ template<typename _MatrixType> class FullPivHouseholderQR
}
/** This method finds a solution x to the equation Ax=b, where A is the matrix of which
* *this is the QR decomposition, if any exists.
* \c *this is the QR decomposition.
*
* \param b the right-hand-side of the equation to solve.
*
* \returns a solution.
* \returns the exact or least-square solution if the rank is greater or equal to the number of columns of A,
* and an arbitrary solution otherwise.
*
* \note The case where b is a matrix is not yet implemented. Also, this
* code is space inefficient.
@ -172,7 +174,7 @@ template<typename _MatrixType> class FullPivHouseholderQR
}
/** \returns a const reference to the vector of indices representing the rows transpositions */
const IntColVectorType& rowsTranspositions() const
const IntDiagSizeVectorType& rowsTranspositions() const
{
eigen_assert(m_isInitialized && "FullPivHouseholderQR is not initialized.");
return m_rows_transpositions;
@ -344,7 +346,7 @@ template<typename _MatrixType> class FullPivHouseholderQR
return m_usePrescribedThreshold ? m_prescribedThreshold
// this formula comes from experimenting (see "LU precision tuning" thread on the list)
// and turns out to be identical to Higham's formula used already in LDLt.
: NumTraits<Scalar>::epsilon() * m_qr.diagonalSize();
: NumTraits<Scalar>::epsilon() * RealScalar(m_qr.diagonalSize());
}
/** \returns the number of nonzero pivots in the QR decomposition.
@ -368,8 +370,8 @@ template<typename _MatrixType> class FullPivHouseholderQR
protected:
MatrixType m_qr;
HCoeffsType m_hCoeffs;
IntColVectorType m_rows_transpositions;
IntRowVectorType m_cols_transpositions;
IntDiagSizeVectorType m_rows_transpositions;
IntDiagSizeVectorType m_cols_transpositions;
PermutationType m_cols_permutation;
RowVectorType m_temp;
bool m_isInitialized, m_usePrescribedThreshold;
@ -415,10 +417,10 @@ FullPivHouseholderQR<MatrixType>& FullPivHouseholderQR<MatrixType>::compute(cons
m_temp.resize(cols);
m_precision = NumTraits<Scalar>::epsilon() * size;
m_precision = NumTraits<Scalar>::epsilon() * RealScalar(size);
m_rows_transpositions.resize(matrix.rows());
m_cols_transpositions.resize(matrix.cols());
m_rows_transpositions.resize(size);
m_cols_transpositions.resize(size);
Index number_of_transpositions = 0;
RealScalar biggest(0);
@ -516,17 +518,6 @@ struct solve_retval<FullPivHouseholderQR<_MatrixType>, Rhs>
dec().hCoeffs().coeff(k), &temp.coeffRef(0));
}
if(!dec().isSurjective())
{
// is c is in the image of R ?
RealScalar biggest_in_upper_part_of_c = c.topRows( dec().rank() ).cwiseAbs().maxCoeff();
RealScalar biggest_in_lower_part_of_c = c.bottomRows(rows-dec().rank()).cwiseAbs().maxCoeff();
// FIXME brain dead
const RealScalar m_precision = NumTraits<Scalar>::epsilon() * (std::min)(rows,cols);
// this internal:: prefix is needed by at least gcc 3.4 and ICC
if(!internal::isMuchSmallerThan(biggest_in_lower_part_of_c, biggest_in_upper_part_of_c, m_precision))
return;
}
dec().matrixQR()
.topLeftCorner(dec().rank(), dec().rank())
.template triangularView<Upper>()
@ -548,14 +539,14 @@ template<typename MatrixType> struct FullPivHouseholderQRMatrixQReturnType
{
public:
typedef typename MatrixType::Index Index;
typedef typename internal::plain_col_type<MatrixType, Index>::type IntColVectorType;
typedef typename FullPivHouseholderQR<MatrixType>::IntDiagSizeVectorType IntDiagSizeVectorType;
typedef typename internal::plain_diag_type<MatrixType>::type HCoeffsType;
typedef Matrix<typename MatrixType::Scalar, 1, MatrixType::RowsAtCompileTime, RowMajor, 1,
MatrixType::MaxRowsAtCompileTime> WorkVectorType;
FullPivHouseholderQRMatrixQReturnType(const MatrixType& qr,
const HCoeffsType& hCoeffs,
const IntColVectorType& rowsTranspositions)
const IntDiagSizeVectorType& rowsTranspositions)
: m_qr(qr),
m_hCoeffs(hCoeffs),
m_rowsTranspositions(rowsTranspositions)
@ -595,7 +586,7 @@ public:
protected:
typename MatrixType::Nested m_qr;
typename HCoeffsType::Nested m_hCoeffs;
typename IntColVectorType::Nested m_rowsTranspositions;
typename IntDiagSizeVectorType::Nested m_rowsTranspositions;
};
} // end namespace internal

18
gtsam/3rdparty/Eigen/Eigen/src/SPQRSupport/SuiteSparseQRSupport.h vendored
View File

@ -64,7 +64,8 @@ class SPQR
typedef PermutationMatrix<Dynamic, Dynamic> PermutationType;
public:
SPQR()
: m_ordering(SPQR_ORDERING_DEFAULT),
: m_isInitialized(false),
m_ordering(SPQR_ORDERING_DEFAULT),
m_allow_tol(SPQR_DEFAULT_TOL),
m_tolerance (NumTraits<Scalar>::epsilon())
{
@ -72,7 +73,8 @@ class SPQR
}
SPQR(const _MatrixType& matrix)
: m_ordering(SPQR_ORDERING_DEFAULT),
: m_isInitialized(false),
m_ordering(SPQR_ORDERING_DEFAULT),
m_allow_tol(SPQR_DEFAULT_TOL),
m_tolerance (NumTraits<Scalar>::epsilon())
{
@ -82,16 +84,22 @@ class SPQR
~SPQR()
{
// Calls SuiteSparseQR_free()
SPQR_free();
cholmod_l_finish(&m_cc);
}
void SPQR_free()
{
cholmod_l_free_sparse(&m_H, &m_cc);
cholmod_l_free_sparse(&m_cR, &m_cc);
cholmod_l_free_dense(&m_HTau, &m_cc);
std::free(m_E);
std::free(m_HPinv);
cholmod_l_finish(&m_cc);
}
void compute(const _MatrixType& matrix)
{
if(m_isInitialized) SPQR_free();
MatrixType mat(matrix);
cholmod_sparse A;
A = viewAsCholmod(mat);
@ -139,7 +147,7 @@ class SPQR
eigen_assert(b.cols()==1 && "This method is for vectors only");
//Compute Q^T * b
Dest y;
typename Dest::PlainObject y;
y = matrixQ().transpose() * b;
// Solves with the triangular matrix R
Index rk = this->rank();

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

@ -380,7 +380,10 @@ struct svd_precondition_2x2_block_to_be_real<MatrixType, QRPreconditioner, true>
z = abs(work_matrix.coeff(p,q)) / work_matrix.coeff(p,q);
work_matrix.row(p) *= z;
if(svd.computeU()) svd.m_matrixU.col(p) *= conj(z);
z = abs(work_matrix.coeff(q,q)) / work_matrix.coeff(q,q);
if(work_matrix.coeff(q,q)!=Scalar(0))
z = abs(work_matrix.coeff(q,q)) / work_matrix.coeff(q,q);
else
z = Scalar(0);
work_matrix.row(q) *= z;
if(svd.computeU()) svd.m_matrixU.col(q) *= conj(z);
}

34
gtsam/3rdparty/Eigen/Eigen/src/SparseCore/ConservativeSparseSparseProduct.h vendored
View File

@ -66,9 +66,9 @@ static void conservative_sparse_sparse_product_impl(const Lhs& lhs, const Rhs& r
}
// unordered insertion
for(int k=0; k<nnz; ++k)
for(Index k=0; k<nnz; ++k)
{
int i = indices[k];
Index i = indices[k];
res.insertBackByOuterInnerUnordered(j,i) = values[i];
mask[i] = false;
}
@ -76,8 +76,8 @@ static void conservative_sparse_sparse_product_impl(const Lhs& lhs, const Rhs& r
#if 0
// alternative ordered insertion code:
int t200 = rows/(log2(200)*1.39);
int t = (rows*100)/139;
Index t200 = rows/(log2(200)*1.39);
Index t = (rows*100)/139;
// FIXME reserve nnz non zeros
// FIXME implement fast sort algorithms for very small nnz
@ -90,9 +90,9 @@ static void conservative_sparse_sparse_product_impl(const Lhs& lhs, const Rhs& r
if(true)
{
if(nnz>1) std::sort(indices.data(),indices.data()+nnz);
for(int k=0; k<nnz; ++k)
for(Index k=0; k<nnz; ++k)
{
int i = indices[k];
Index i = indices[k];
res.insertBackByOuterInner(j,i) = values[i];
mask[i] = false;
}
@ -100,7 +100,7 @@ static void conservative_sparse_sparse_product_impl(const Lhs& lhs, const Rhs& r
else
{
// dense path
for(int i=0; i<rows; ++i)
for(Index i=0; i<rows; ++i)
{
if(mask[i])
{
@ -134,8 +134,8 @@ struct conservative_sparse_sparse_product_selector<Lhs,Rhs,ResultType,ColMajor,C
static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
{
typedef SparseMatrix<typename ResultType::Scalar,RowMajor> RowMajorMatrix;
typedef SparseMatrix<typename ResultType::Scalar,ColMajor> ColMajorMatrix;
typedef SparseMatrix<typename ResultType::Scalar,RowMajor,typename ResultType::Index> RowMajorMatrix;
typedef SparseMatrix<typename ResultType::Scalar,ColMajor,typename ResultType::Index> ColMajorMatrix;
ColMajorMatrix resCol(lhs.rows(),rhs.cols());
internal::conservative_sparse_sparse_product_impl<Lhs,Rhs,ColMajorMatrix>(lhs, rhs, resCol);
// sort the non zeros:
@ -149,7 +149,7 @@ struct conservative_sparse_sparse_product_selector<Lhs,Rhs,ResultType,RowMajor,C
{
static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
{
typedef SparseMatrix<typename ResultType::Scalar,RowMajor> RowMajorMatrix;
typedef SparseMatrix<typename ResultType::Scalar,RowMajor,typename ResultType::Index> RowMajorMatrix;
RowMajorMatrix rhsRow = rhs;
RowMajorMatrix resRow(lhs.rows(), rhs.cols());
internal::conservative_sparse_sparse_product_impl<RowMajorMatrix,Lhs,RowMajorMatrix>(rhsRow, lhs, resRow);
@ -162,7 +162,7 @@ struct conservative_sparse_sparse_product_selector<Lhs,Rhs,ResultType,ColMajor,R
{
static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
{
typedef SparseMatrix<typename ResultType::Scalar,RowMajor> RowMajorMatrix;
typedef SparseMatrix<typename ResultType::Scalar,RowMajor,typename ResultType::Index> RowMajorMatrix;
RowMajorMatrix lhsRow = lhs;
RowMajorMatrix resRow(lhs.rows(), rhs.cols());
internal::conservative_sparse_sparse_product_impl<Rhs,RowMajorMatrix,RowMajorMatrix>(rhs, lhsRow, resRow);
@ -175,7 +175,7 @@ struct conservative_sparse_sparse_product_selector<Lhs,Rhs,ResultType,RowMajor,R
{
static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
{
typedef SparseMatrix<typename ResultType::Scalar,RowMajor> RowMajorMatrix;
typedef SparseMatrix<typename ResultType::Scalar,RowMajor,typename ResultType::Index> RowMajorMatrix;
RowMajorMatrix resRow(lhs.rows(), rhs.cols());
internal::conservative_sparse_sparse_product_impl<Rhs,Lhs,RowMajorMatrix>(rhs, lhs, resRow);
res = resRow;
@ -190,7 +190,7 @@ struct conservative_sparse_sparse_product_selector<Lhs,Rhs,ResultType,ColMajor,C
static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
{
typedef SparseMatrix<typename ResultType::Scalar,ColMajor> ColMajorMatrix;
typedef SparseMatrix<typename ResultType::Scalar,ColMajor,typename ResultType::Index> ColMajorMatrix;
ColMajorMatrix resCol(lhs.rows(), rhs.cols());
internal::conservative_sparse_sparse_product_impl<Lhs,Rhs,ColMajorMatrix>(lhs, rhs, resCol);
res = resCol;
@ -202,7 +202,7 @@ struct conservative_sparse_sparse_product_selector<Lhs,Rhs,ResultType,RowMajor,C
{
static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
{
typedef SparseMatrix<typename ResultType::Scalar,ColMajor> ColMajorMatrix;
typedef SparseMatrix<typename ResultType::Scalar,ColMajor,typename ResultType::Index> ColMajorMatrix;
ColMajorMatrix lhsCol = lhs;
ColMajorMatrix resCol(lhs.rows(), rhs.cols());
internal::conservative_sparse_sparse_product_impl<ColMajorMatrix,Rhs,ColMajorMatrix>(lhsCol, rhs, resCol);
@ -215,7 +215,7 @@ struct conservative_sparse_sparse_product_selector<Lhs,Rhs,ResultType,ColMajor,R
{
static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
{
typedef SparseMatrix<typename ResultType::Scalar,ColMajor> ColMajorMatrix;
typedef SparseMatrix<typename ResultType::Scalar,ColMajor,typename ResultType::Index> ColMajorMatrix;
ColMajorMatrix rhsCol = rhs;
ColMajorMatrix resCol(lhs.rows(), rhs.cols());
internal::conservative_sparse_sparse_product_impl<Lhs,ColMajorMatrix,ColMajorMatrix>(lhs, rhsCol, resCol);
@ -228,8 +228,8 @@ struct conservative_sparse_sparse_product_selector<Lhs,Rhs,ResultType,RowMajor,R
{
static void run(const Lhs& lhs, const Rhs& rhs, ResultType& res)
{
typedef SparseMatrix<typename ResultType::Scalar,RowMajor> RowMajorMatrix;
typedef SparseMatrix<typename ResultType::Scalar,ColMajor> ColMajorMatrix;
typedef SparseMatrix<typename ResultType::Scalar,RowMajor,typename ResultType::Index> RowMajorMatrix;
typedef SparseMatrix<typename ResultType::Scalar,ColMajor,typename ResultType::Index> ColMajorMatrix;
RowMajorMatrix resRow(lhs.rows(),rhs.cols());
internal::conservative_sparse_sparse_product_impl<Rhs,Lhs,RowMajorMatrix>(rhs, lhs, resRow);
// sort the non zeros:

2
gtsam/3rdparty/Eigen/Eigen/src/SparseCore/MappedSparseMatrix.h vendored
View File

@ -50,6 +50,8 @@ class MappedSparseMatrix
inline Index cols() const { return IsRowMajor ? m_innerSize : m_outerSize; }
inline Index innerSize() const { return m_innerSize; }
inline Index outerSize() const { return m_outerSize; }
bool isCompressed() const { return true; }
//----------------------------------------
// direct access interface

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

@ -66,6 +66,8 @@ public:
typename XprType::Nested m_matrix;
Index m_outerStart;
const internal::variable_if_dynamic<Index, OuterSize> m_outerSize;
EIGEN_INHERIT_ASSIGNMENT_OPERATORS(BlockImpl)
};
@ -391,6 +393,8 @@ public:
protected:
friend class InnerIterator;
friend class ReverseInnerIterator;
EIGEN_INHERIT_ASSIGNMENT_OPERATORS(BlockImpl)
typename XprType::Nested m_matrix;
const internal::variable_if_dynamic<Index, XprType::RowsAtCompileTime == 1 ? 0 : Dynamic> m_startRow;

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

@ -63,6 +63,7 @@ int coletree(const MatrixType& mat, IndexVector& parent, IndexVector& firstRowEl
typedef typename MatrixType::Index Index;
Index nc = mat.cols(); // Number of columns
Index m = mat.rows();
Index diagSize = (std::min)(nc,m);
IndexVector root(nc); // root of subtree of etree
root.setZero();
IndexVector pp(nc); // disjoint sets
@ -72,7 +73,7 @@ int coletree(const MatrixType& mat, IndexVector& parent, IndexVector& firstRowEl
Index row,col;
firstRowElt.resize(m);
firstRowElt.setConstant(nc);
firstRowElt.segment(0, nc).setLinSpaced(nc, 0, nc-1);
firstRowElt.segment(0, diagSize).setLinSpaced(diagSize, 0, diagSize-1);
bool found_diag;
for (col = 0; col < nc; col++)
{
@ -91,7 +92,7 @@ int coletree(const MatrixType& mat, IndexVector& parent, IndexVector& firstRowEl
Index rset, cset, rroot;
for (col = 0; col < nc; col++)
{
found_diag = false;
found_diag = col>=m;
pp(col) = col;
cset = col;
root(cset) = col;
@ -105,6 +106,7 @@ int coletree(const MatrixType& mat, IndexVector& parent, IndexVector& firstRowEl
Index i = col;
if(it) i = it.index();
if (i == col) found_diag = true;
row = firstRowElt(i);
if (row >= col) continue;
rset = internal::etree_find(row, pp); // Find the name of the set containing row

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