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

Merge branch 'develop' of bitbucket.org:gtborg/gtsam into feature/partition

release/4.3a0
Andrew Melim 2014-02-09 13:45:41 -05:00
parent 1d9aa38a62 c1a018a1fd
commit 2e1e4951e1
39 changed files with 758 additions and 287 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

66
CMakeLists.txt
View File

@ -52,8 +52,7 @@ option(GTSAM_BUILD_EXAMPLES "Enable/Disable building of examples"
if(GTSAM_UNSTABLE_AVAILABLE) if(GTSAM_UNSTABLE_AVAILABLE)
option(GTSAM_BUILD_UNSTABLE "Enable/Disable libgtsam_unstable" ON) option(GTSAM_BUILD_UNSTABLE "Enable/Disable libgtsam_unstable" ON)
endif() endif()
option(GTSAM_BUILD_SHARED_LIBRARY "Enable/Disable building of a shared version of gtsam" ON) option(GTSAM_BUILD_STATIC_LIBRARY "Build a static gtsam library, instead of shared" OFF)
option(GTSAM_BUILD_STATIC_LIBRARY "Enable/Disable building of a static version of gtsam" 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) option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF)
option(GTSAM_POSE3_EXPMAP "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." OFF) option(GTSAM_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_ROT3_EXPMAP "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." OFF)
@ -76,15 +75,14 @@ if(GTSAM_INSTALL_WRAP AND NOT GTSAM_BUILD_WRAP)
message(FATAL_ERROR "GTSAM_INSTALL_WRAP is enabled, please also enable GTSAM_BUILD_WRAP") message(FATAL_ERROR "GTSAM_INSTALL_WRAP is enabled, please also enable GTSAM_BUILD_WRAP")
endif() 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 # Flags for choosing default packaging tools
set(CPACK_SOURCE_GENERATOR "TGZ" CACHE STRING "CPack Default Source Generator") set(CPACK_SOURCE_GENERATOR "TGZ" CACHE STRING "CPack Default Source Generator")
set(CPACK_GENERATOR "TGZ" CACHE STRING "CPack Default Binary 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' # Flags to determine whether tests and examples are build during 'make install'
# Note that these remove the targets from the 'all' # Note that these remove the targets from the 'all'
option(GTSAM_DISABLE_TESTS_ON_INSTALL "Disables building tests during install" ON) option(GTSAM_DISABLE_TESTS_ON_INSTALL "Disables building tests during install" ON)
@ -171,23 +169,27 @@ find_package(GooglePerfTools)
############################################################################### ###############################################################################
# Find MKL # Find MKL
find_package(MKL) if(GTSAM_USE_EIGEN_MKL)
find_package(MKL)
if(MKL_FOUND AND GTSAM_WITH_EIGEN_MKL) if(MKL_FOUND AND GTSAM_WITH_EIGEN_MKL)
set(GTSAM_USE_EIGEN_MKL 1) # This will go into config.h 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 set(EIGEN_USE_MKL_ALL 1) # This will go into config.h - it makes Eigen use MKL
include_directories(${MKL_INCLUDE_DIR}) include_directories(${MKL_INCLUDE_DIR})
list(APPEND GTSAM_ADDITIONAL_LIBRARIES ${MKL_LIBRARIES}) list(APPEND GTSAM_ADDITIONAL_LIBRARIES ${MKL_LIBRARIES})
endif()
endif() endif()
############################################################################### ###############################################################################
# Find OpenMP # Find OpenMP (if we're also using MKL)
find_package(OpenMP) 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) 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(GTSAM_USE_EIGEN_MKL_OPENMP 1) # This will go into config.h
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
endif()
endif() endif()
@ -286,24 +288,11 @@ endif()
if(GTSAM_ENABLE_CONSISTENCY_CHECKS) if(GTSAM_ENABLE_CONSISTENCY_CHECKS)
add_definitions(-DGTSAM_EXTRA_CONSISTENCY_CHECKS) add_definitions(-DGTSAM_EXTRA_CONSISTENCY_CHECKS)
endif() endif()
############################################################################### ###############################################################################
# Add components # 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 # Build CppUnitLite
add_subdirectory(CppUnitLite) add_subdirectory(CppUnitLite)
@ -351,7 +340,7 @@ endif()
set(CPACK_PACKAGE_DESCRIPTION_SUMMARY "GTSAM") set(CPACK_PACKAGE_DESCRIPTION_SUMMARY "GTSAM")
set(CPACK_PACKAGE_VENDOR "Frank Dellaert, Georgia Institute of Technology") set(CPACK_PACKAGE_VENDOR "Frank Dellaert, Georgia Institute of Technology")
set(CPACK_PACKAGE_CONTACT "Frank Dellaert, dellaert@cc.gatech.edu") 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_RESOURCE_FILE_LICENSE "${CMAKE_CURRENT_SOURCE_DIR}/LICENSE")
set(CPACK_PACKAGE_VERSION_MAJOR ${GTSAM_VERSION_MAJOR}) set(CPACK_PACKAGE_VERSION_MAJOR ${GTSAM_VERSION_MAJOR})
set(CPACK_PACKAGE_VERSION_MINOR ${GTSAM_VERSION_MINOR}) set(CPACK_PACKAGE_VERSION_MINOR ${GTSAM_VERSION_MINOR})
@ -381,8 +370,7 @@ print_config_flag(${GTSAM_BUILD_TESTS} "Build Tests
if (DOXYGEN_FOUND) if (DOXYGEN_FOUND)
print_config_flag(${GTSAM_BUILD_DOCS} "Build Docs ") print_config_flag(${GTSAM_BUILD_DOCS} "Build Docs ")
endif() endif()
print_config_flag(${GTSAM_BUILD_SHARED_LIBRARY} "Build shared GTSAM Library ") print_config_flag(${GTSAM_BUILD_STATIC_LIBRARY} "Build static GTSAM library instead of shared")
print_config_flag(${GTSAM_BUILD_STATIC_LIBRARY} "Build static GTSAM Library ")
print_config_flag(${GTSAM_BUILD_TYPE_POSTFIXES} "Put build type in library name ") print_config_flag(${GTSAM_BUILD_TYPE_POSTFIXES} "Put build type in library name ")
if(GTSAM_UNSTABLE_AVAILABLE) if(GTSAM_UNSTABLE_AVAILABLE)
print_config_flag(${GTSAM_BUILD_UNSTABLE} "Build libgtsam_unstable ") print_config_flag(${GTSAM_BUILD_UNSTABLE} "Build libgtsam_unstable ")
@ -390,9 +378,11 @@ endif()
print_config_flag(${GTSAM_DISABLE_TESTS_ON_INSTALL} "No tests in all or install ") print_config_flag(${GTSAM_DISABLE_TESTS_ON_INSTALL} "No tests in all or install ")
print_config_flag(${GTSAM_DISABLE_EXAMPLES_ON_INSTALL} "No examples in all or install ") print_config_flag(${GTSAM_DISABLE_EXAMPLES_ON_INSTALL} "No examples in all or install ")
string(TOUPPER "${CMAKE_BUILD_TYPE}" cmake_build_type_toupper) string(TOUPPER "${CMAKE_BUILD_TYPE}" cmake_build_type_toupper)
message(STATUS " Build type : ${CMAKE_BUILD_TYPE}") if(NOT MSVC AND NOT XCODE_VERSION)
message(STATUS " C compilation flags : ${CMAKE_C_FLAGS} ${CMAKE_C_FLAGS_${cmake_build_type_toupper}}") message(STATUS " Build type : ${CMAKE_BUILD_TYPE}")
message(STATUS " C++ compilation flags : ${CMAKE_CXX_FLAGS} ${CMAKE_CXX_FLAGS_${cmake_build_type_toupper}}") 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) if(GTSAM_USE_TBB)
message(STATUS " Use Intel TBB : Yes") message(STATUS " Use Intel TBB : Yes")
elseif(TBB_FOUND) elseif(TBB_FOUND)

57
README → INSTALL
View File

@ -1,6 +1,3 @@
README - Georgia Tech Smoothing and Mapping library
---------------------------------------------------
Quickstart Quickstart
In the root library folder execute: In the root library folder execute:
@ -11,42 +8,6 @@ $] cmake ..
$] make check (optional, runs unit tests) $] make check (optional, runs unit tests)
$] make install $] 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 Important Installation Notes
---------------------------- ----------------------------
@ -64,15 +25,17 @@ Optional dependent libraries:
may be downloaded from https://www.threadingbuildingblocks.org/ may be downloaded from https://www.threadingbuildingblocks.org/
Tested compilers Tested compilers
- GCC 4.2-4.7
- Clang 2.9-3.2 - GCC 4.2-4.7
- OSX GCC 4.2 - OSX Clang 2.9-5.0
- MSVC 2010, 2012 - OSX GCC 4.2
- MSVC 2010, 2012
Tested systems: Tested systems:
- Ubuntu 11.04, 11.10, 12.04, 12.10, 13.04
- MacOS 10.6, 10.7 - Ubuntu 11.04 - 13.10
- Windows 7 - MacOS 10.6 - 10.9
- Windows 7, 8
2) 2)
GTSAM makes extensive use of debug assertions, and we highly recommend you work 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 GTSAM is released under the simplified BSD license, reproduced in the file
Atlanta, Georgia 30332-0415 LICENSE.BSD in this directory.
All Rights Reserved
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, - CCOLAMD 2.73: Tim Davis' constrained column approximate minimum degree
are permitted provided that the following conditions are met: ordering library
- Included unmodified in gtsam/3rdparty/CCOLAMD and gtsam/3rdparty/UFconfig
* Redistributions of source code must retain the above copyright notice, - http://www.cise.ufl.edu/research/sparse
this list of conditions and the following disclaimer. - Licenced under LGPL v2.1, provided in gtsam/3rdparty/CCOLAMD/Doc/lesser.txt
- Eigen 3.2: General C++ matrix and linear algebra library
* Redistributions in binary form must reproduce the above copyright notice, - Modified with 3 patches that have been contributed back to the Eigen team:
this list of conditions and the following disclaimer in the documentation - http://eigen.tuxfamily.org/bz/show_bug.cgi?id=704 (Householder QR MKL selection)
and/or other materials provided with the distribution. - 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)
* Neither the name of the copyright holders nor the names of its contributors - Licenced under MPL2, provided in gtsam/3rdparty/Eigen/COPYING.README
may be used to endorse or promote products derived from this software without - Some code that is 3rd-party to Eigen is BSD and LGPL
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.

26
LICENSE.BSD
View File

@ -2,28 +2,12 @@ Copyright (c) 2010, Georgia Tech Research Corporation
Atlanta, Georgia 30332-0415 Atlanta, Georgia 30332-0415
All Rights Reserved All Rights Reserved
See also README for licensing of 3rd-party code included in GTSAM. Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
Redistribution and use in source and binary forms, with or without modification, 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice, 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.
this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice, 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 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 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.
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.

64
README.md
View File

@ -1,7 +1,20 @@
README - Georgia Tech Smoothing and Mapping library 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 Quickstart
----------
In the root library folder execute: In the root library folder execute:
@ -24,52 +37,11 @@ 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 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) - [Intel Math Kernel Library (MKL)](http://software.intel.com/en-us/intel-mkl)
Tested compilers Additional Information
----------------------
- GCC 4.2-4.7 See the [`INSTALL`](https://bitbucket.org/gtborg/gtsam/src/develop/INSTALL) file for more detailed installation instructions.
- OSX Clang 2.9-5.0
- OSX GCC 4.2
- MSVC 2010, 2012
Tested systems: 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.
- Ubuntu 11.04 - 13.10 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.
- MacOS 10.6 - 10.9
- Windows 7, 8
See the `INSTALL` file for more detailed installation instructions.
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.
GTSAM is open source under the BSD license, see the `LICENSE.BSD` file.
Please see the `examples/` directory and the `USAGE` file for examples 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)

11
USAGE
View File

@ -47,6 +47,17 @@ Factors:
SLAM example, is a measurement such as a visual reading on a landmark or SLAM example, is a measurement such as a visual reading on a landmark or
odometry. 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 VSLAM Example
--------------------------------------------------- ---------------------------------------------------

6
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() endif()
# Add option for using build type postfixes to allow installing multiple build modes # 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)
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()
# Add debugging flags but only on the first pass # Add debugging flags but only on the first pass
if(NOT FIRST_PASS_DONE) if(NOT FIRST_PASS_DONE)

2
cmake/GtsamTesting.cmake
View File

@ -155,7 +155,7 @@ macro(gtsam_add_grouped_scripts group pattern target_prefix pretty_prefix_name l
endif() endif()
# Add .run target # Add .run target
if(NOT MSVC) if(NOT MSVC AND NOT XCODE_VERSION)
add_custom_target(${script_bin}.run ${EXECUTABLE_OUTPUT_PATH}${script_bin} ${ARGN}) add_custom_target(${script_bin}.run ${EXECUTABLE_OUTPUT_PATH}${script_bin} ${ARGN})
endif() endif()

4
examples/CMakeLists.txt
View File

@ -28,8 +28,8 @@ foreach(example_src ${example_srcs} )
set_target_properties(${example_bin} PROPERTIES EXCLUDE_FROM_ALL ON) set_target_properties(${example_bin} PROPERTIES EXCLUDE_FROM_ALL ON)
endif() endif()
target_link_libraries(${example_bin} ${gtsam-default} ${Boost_PROGRAM_OPTIONS_LIBRARY}) target_link_libraries(${example_bin} gtsam ${Boost_PROGRAM_OPTIONS_LIBRARY})
if(NOT MSVC) if(NOT MSVC AND NOT XCODE_VERSION)
add_custom_target(${example_bin}.run ${EXECUTABLE_OUTPUT_PATH}${example_bin} ${ARGN}) add_custom_target(${example_bin}.run ${EXECUTABLE_OUTPUT_PATH}${example_bin} ${ARGN})
endif() endif()

34
gtsam/CMakeLists.txt
View File

@ -95,42 +95,40 @@ message(STATUS "Install prefix: ${CMAKE_INSTALL_PREFIX}")
# build shared and static versions of the library # build shared and static versions of the library
if (GTSAM_BUILD_STATIC_LIBRARY) if (GTSAM_BUILD_STATIC_LIBRARY)
message(STATUS "Building GTSAM - static") message(STATUS "Building GTSAM - static")
add_library(gtsam-static STATIC ${gtsam_srcs}) add_library(gtsam STATIC ${gtsam_srcs})
target_link_libraries(gtsam-static ${GTSAM_BOOST_LIBRARIES} ${GTSAM_ADDITIONAL_LIBRARIES}) target_link_libraries(gtsam ${GTSAM_BOOST_LIBRARIES} ${GTSAM_ADDITIONAL_LIBRARIES})
set_target_properties(gtsam-static PROPERTIES set_target_properties(gtsam PROPERTIES
OUTPUT_NAME gtsam OUTPUT_NAME gtsam
CLEAN_DIRECT_OUTPUT 1 CLEAN_DIRECT_OUTPUT 1
VERSION ${gtsam_version} VERSION ${gtsam_version}
SOVERSION ${gtsam_soversion}) SOVERSION ${gtsam_soversion})
if(WIN32) # Add 'lib' prefix to static library to avoid filename collision with shared library if(WIN32) # Add 'lib' prefix to static library to avoid filename collision with shared library
set_target_properties(gtsam-static PROPERTIES set_target_properties(gtsam PROPERTIES
PREFIX "lib" PREFIX "lib"
COMPILE_DEFINITIONS GTSAM_IMPORT_STATIC) COMPILE_DEFINITIONS GTSAM_IMPORT_STATIC)
endif() endif()
install(TARGETS gtsam-static EXPORT GTSAM-exports ARCHIVE DESTINATION lib) install(TARGETS gtsam EXPORT GTSAM-exports ARCHIVE DESTINATION lib)
list(APPEND GTSAM_EXPORTED_TARGETS gtsam-static) list(APPEND GTSAM_EXPORTED_TARGETS gtsam)
set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE) set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
endif () else()
if (GTSAM_BUILD_SHARED_LIBRARY)
message(STATUS "Building GTSAM - shared") message(STATUS "Building GTSAM - shared")
add_library(gtsam-shared SHARED ${gtsam_srcs}) add_library(gtsam SHARED ${gtsam_srcs})
target_link_libraries(gtsam-shared ${GTSAM_BOOST_LIBRARIES} ${GTSAM_ADDITIONAL_LIBRARIES}) target_link_libraries(gtsam ${GTSAM_BOOST_LIBRARIES} ${GTSAM_ADDITIONAL_LIBRARIES})
set_target_properties(gtsam-shared PROPERTIES set_target_properties(gtsam PROPERTIES
OUTPUT_NAME gtsam OUTPUT_NAME gtsam
CLEAN_DIRECT_OUTPUT 1 CLEAN_DIRECT_OUTPUT 1
VERSION ${gtsam_version} VERSION ${gtsam_version}
SOVERSION ${gtsam_soversion}) SOVERSION ${gtsam_soversion})
if(WIN32) if(WIN32)
set_target_properties(gtsam-shared PROPERTIES set_target_properties(gtsam PROPERTIES
PREFIX "" PREFIX ""
DEFINE_SYMBOL GTSAM_EXPORTS DEFINE_SYMBOL GTSAM_EXPORTS
RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/bin") RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/bin")
endif() endif()
install(TARGETS gtsam-shared EXPORT GTSAM-exports LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin) install(TARGETS gtsam EXPORT GTSAM-exports LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin)
list(APPEND GTSAM_EXPORTED_TARGETS gtsam-shared) list(APPEND GTSAM_EXPORTED_TARGETS gtsam)
set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE) set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
endif(GTSAM_BUILD_SHARED_LIBRARY) endif()
# Set dataset paths # Set dataset paths
set_property(SOURCE "${CMAKE_CURRENT_SOURCE_DIR}/slam/dataset.cpp" set_property(SOURCE "${CMAKE_CURRENT_SOURCE_DIR}/slam/dataset.cpp"
@ -153,10 +151,10 @@ if (GTSAM_INSTALL_MATLAB_TOOLBOX)
# Generate, build and install toolbox # Generate, build and install toolbox
set(mexFlags "${GTSAM_BUILD_MEX_BINARY_FLAGS}") set(mexFlags "${GTSAM_BUILD_MEX_BINARY_FLAGS}")
if("${gtsam-default}" STREQUAL "gtsam-static") if(GTSAM_BUILD_STATIC_LIBRARY)
list(APPEND mexFlags -DGTSAM_IMPORT_STATIC) list(APPEND mexFlags -DGTSAM_IMPORT_STATIC)
endif() endif()
# Wrap # Wrap
wrap_and_install_library(../gtsam.h "${gtsam-default};${GTSAM_ADDITIONAL_LIBRARIES}" "" "${mexFlags}") wrap_and_install_library(../gtsam.h "gtsam;${GTSAM_ADDITIONAL_LIBRARIES}" "" "${mexFlags}")
endif () endif ()

4
gtsam/base/CMakeLists.txt
View File

@ -13,11 +13,11 @@ set(base_excluded_files
# Build tests # Build tests
if (GTSAM_BUILD_TESTS) if (GTSAM_BUILD_TESTS)
gtsam_add_subdir_tests(base "${gtsam-default}" "${gtsam-default}" "${base_excluded_files}") gtsam_add_subdir_tests(base "gtsam" "gtsam" "${base_excluded_files}")
endif(GTSAM_BUILD_TESTS) endif(GTSAM_BUILD_TESTS)
# Build timing scripts # Build timing scripts
if (GTSAM_BUILD_TIMING) if (GTSAM_BUILD_TIMING)
gtsam_add_subdir_timing(base "${gtsam-default}" "${gtsam-default}" "${base_excluded_files}") gtsam_add_subdir_timing(base "gtsam" "gtsam" "${base_excluded_files}")
endif(GTSAM_BUILD_TIMING) endif(GTSAM_BUILD_TIMING)

4
gtsam/base/serialization.h
View File

@ -135,12 +135,12 @@ void deserializeBinary(const std::string& serialized, T& output, const std::stri
} }
template<class T> template<class T>
bool deserializeToBinaryFile(const T& input, const std::string& filename, const std::string& name="data") { bool serializeToBinaryFile(const T& input, const std::string& filename, const std::string& name="data") {
std::ofstream out_archive_stream(filename.c_str()); std::ofstream out_archive_stream(filename.c_str());
if (!out_archive_stream.is_open()) if (!out_archive_stream.is_open())
return false; return false;
boost::archive::binary_oarchive out_archive(out_archive_stream); boost::archive::binary_oarchive out_archive(out_archive_stream);
out_archive << boost::serialization::make_nvp(name.c_str(), input);; out_archive << boost::serialization::make_nvp(name.c_str(), input);
out_archive_stream.close(); out_archive_stream.close();
return true; return true;
} }

2
gtsam/discrete/CMakeLists.txt
View File

@ -9,7 +9,7 @@ set (discrete_excluded_tests "")
# Add all tests # Add all tests
if (GTSAM_BUILD_TESTS) if (GTSAM_BUILD_TESTS)
gtsam_add_subdir_tests(discrete "${gtsam-default}" "${gtsam-default}" "${discrete_excluded_tests}") gtsam_add_subdir_tests(discrete "gtsam" "gtsam" "${discrete_excluded_tests}")
endif() endif()
# Build timing scripts # Build timing scripts

4
gtsam/geometry/CMakeLists.txt
View File

@ -10,11 +10,11 @@ set(geometry_excluded_files
# Build tests # Build tests
if (GTSAM_BUILD_TESTS) if (GTSAM_BUILD_TESTS)
gtsam_add_subdir_tests(geometry "${gtsam-default}" "${gtsam-default}" "${geometry_excluded_files}") gtsam_add_subdir_tests(geometry "gtsam" "gtsam" "${geometry_excluded_files}")
endif(GTSAM_BUILD_TESTS) endif(GTSAM_BUILD_TESTS)
# Build timing scripts # Build timing scripts
if (GTSAM_BUILD_TIMING) if (GTSAM_BUILD_TIMING)
gtsam_add_subdir_timing(geometry "${gtsam-default}" "${gtsam-default}" "${geometry_excluded_files}") gtsam_add_subdir_timing(geometry "gtsam" "gtsam" "${geometry_excluded_files}")
endif(GTSAM_BUILD_TIMING) endif(GTSAM_BUILD_TIMING)

4
gtsam/geometry/Rot3.cpp
View File

@ -73,7 +73,7 @@ Point3 Rot3::operator*(const Point3& p) const {
/* ************************************************************************* */ /* ************************************************************************* */
Sphere2 Rot3::rotate(const Sphere2& p, Sphere2 Rot3::rotate(const Sphere2& p,
boost::optional<Matrix&> HR, boost::optional<Matrix&> Hp) const { boost::optional<Matrix&> HR, boost::optional<Matrix&> Hp) const {
Sphere2 q = rotate(p.point3(Hp)); Sphere2 q = Sphere2(rotate(p.point3(Hp)));
if (Hp) if (Hp)
(*Hp) = q.basis().transpose() * matrix() * (*Hp); (*Hp) = q.basis().transpose() * matrix() * (*Hp);
if (HR) if (HR)
@ -84,7 +84,7 @@ Sphere2 Rot3::rotate(const Sphere2& p,
/* ************************************************************************* */ /* ************************************************************************* */
Sphere2 Rot3::unrotate(const Sphere2& p, Sphere2 Rot3::unrotate(const Sphere2& p,
boost::optional<Matrix&> HR, boost::optional<Matrix&> Hp) const { boost::optional<Matrix&> HR, boost::optional<Matrix&> Hp) const {
Sphere2 q = unrotate(p.point3(Hp)); Sphere2 q = Sphere2(unrotate(p.point3(Hp)));
if (Hp) if (Hp)
(*Hp) = q.basis().transpose() * matrix().transpose () * (*Hp); (*Hp) = q.basis().transpose() * matrix().transpose () * (*Hp);
if (HR) if (HR)

2
gtsam/geometry/Sphere2.h
View File

@ -60,7 +60,7 @@ public:
} }
/// Construct from point /// Construct from point
Sphere2(const Point3& p) : explicit Sphere2(const Point3& p) :
p_(p / p.norm()) { p_(p / p.norm()) {
} }

14
gtsam/geometry/tests/testEssentialMatrix.cpp
View File

@ -22,17 +22,17 @@ GTSAM_CONCEPT_MANIFOLD_INST(EssentialMatrix)
// Create two cameras and corresponding essential matrix E // Create two cameras and corresponding essential matrix E
Rot3 c1Rc2 = Rot3::yaw(M_PI_2); Rot3 c1Rc2 = Rot3::yaw(M_PI_2);
Point3 c1Tc2(0.1, 0, 0); Point3 c1Tc2(0.1, 0, 0);
EssentialMatrix trueE(c1Rc2, c1Tc2); EssentialMatrix trueE(c1Rc2, Sphere2(c1Tc2));
//************************************************************************* //*************************************************************************
TEST (EssentialMatrix, equality) { TEST (EssentialMatrix, equality) {
EssentialMatrix actual(c1Rc2, c1Tc2), expected(c1Rc2, c1Tc2); EssentialMatrix actual(c1Rc2, Sphere2(c1Tc2)), expected(c1Rc2, Sphere2(c1Tc2));
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
} }
//************************************************************************* //*************************************************************************
TEST (EssentialMatrix, retract1) { TEST (EssentialMatrix, retract1) {
EssentialMatrix expected(c1Rc2.retract((Vector(3) << 0.1, 0, 0)), c1Tc2); EssentialMatrix expected(c1Rc2.retract((Vector(3) << 0.1, 0, 0)), Sphere2(c1Tc2));
EssentialMatrix actual = trueE.retract((Vector(5) << 0.1, 0, 0, 0, 0)); EssentialMatrix actual = trueE.retract((Vector(5) << 0.1, 0, 0, 0, 0));
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
} }
@ -54,7 +54,7 @@ TEST (EssentialMatrix, transform_to) {
Rot3 aRb2 = Rot3::yaw(M_PI / 3.0) * Rot3::pitch(M_PI_4) Rot3 aRb2 = Rot3::yaw(M_PI / 3.0) * Rot3::pitch(M_PI_4)
* Rot3::roll(M_PI / 6.0); * Rot3::roll(M_PI / 6.0);
Point3 aTb2(19.2, 3.7, 5.9); Point3 aTb2(19.2, 3.7, 5.9);
EssentialMatrix E(aRb2, aTb2); EssentialMatrix E(aRb2, Sphere2(aTb2));
//EssentialMatrix E(aRb, Sphere2(aTb).retract((Vector(2) << 0.1, 0))); //EssentialMatrix E(aRb, Sphere2(aTb).retract((Vector(2) << 0.1, 0)));
static Point3 P(0.2, 0.7, -2); static Point3 P(0.2, 0.7, -2);
Matrix actH1, actH2; Matrix actH1, actH2;
@ -79,7 +79,7 @@ TEST (EssentialMatrix, rotate) {
// Let's compute the ground truth E in body frame: // Let's compute the ground truth E in body frame:
Rot3 b1Rb2 = bRc * c1Rc2 * cRb; Rot3 b1Rb2 = bRc * c1Rc2 * cRb;
Point3 b1Tb2 = bRc * c1Tc2; Point3 b1Tb2 = bRc * c1Tc2;
EssentialMatrix bodyE(b1Rb2, b1Tb2); EssentialMatrix bodyE(b1Rb2, Sphere2(b1Tb2));
EXPECT(assert_equal(bodyE, bRc * trueE, 1e-8)); EXPECT(assert_equal(bodyE, bRc * trueE, 1e-8));
EXPECT(assert_equal(bodyE, trueE.rotate(bRc), 1e-8)); EXPECT(assert_equal(bodyE, trueE.rotate(bRc), 1e-8));
@ -111,7 +111,7 @@ TEST (EssentialMatrix, FromPose3_b) {
Matrix actualH; Matrix actualH;
Rot3 c1Rc2 = Rot3::ypr(0.1, -0.2, 0.3); Rot3 c1Rc2 = Rot3::ypr(0.1, -0.2, 0.3);
Point3 c1Tc2(0.4, 0.5, 0.6); Point3 c1Tc2(0.4, 0.5, 0.6);
EssentialMatrix trueE(c1Rc2, c1Tc2); EssentialMatrix trueE(c1Rc2, Sphere2(c1Tc2));
Pose3 pose(c1Rc2, c1Tc2); // Pose between two cameras Pose3 pose(c1Rc2, c1Tc2); // Pose between two cameras
EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8)); EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8));
Matrix expectedH = numericalDerivative11<EssentialMatrix, Pose3>( Matrix expectedH = numericalDerivative11<EssentialMatrix, Pose3>(
@ -121,7 +121,7 @@ TEST (EssentialMatrix, FromPose3_b) {
//************************************************************************* //*************************************************************************
TEST (EssentialMatrix, streaming) { TEST (EssentialMatrix, streaming) {
EssentialMatrix expected(c1Rc2, c1Tc2), actual; EssentialMatrix expected(c1Rc2, Sphere2(c1Tc2)), actual;
stringstream ss; stringstream ss;
ss << expected; ss << expected;
ss >> actual; ss >> actual;

4
gtsam/inference/CMakeLists.txt
View File

@ -10,11 +10,11 @@ set(inference_excluded_files
# Build tests # Build tests
if (GTSAM_BUILD_TESTS) if (GTSAM_BUILD_TESTS)
gtsam_add_subdir_tests(inference "${gtsam-default}" "${gtsam-default}" "${inference_excluded_files}") gtsam_add_subdir_tests(inference "gtsam" "gtsam" "${inference_excluded_files}")
endif(GTSAM_BUILD_TESTS) endif(GTSAM_BUILD_TESTS)
# Build timing scripts # Build timing scripts
if (GTSAM_BUILD_TIMING) if (GTSAM_BUILD_TIMING)
gtsam_add_subdir_timing(inference "${gtsam-default}" "${gtsam-default}" "${inference_excluded_files}") gtsam_add_subdir_timing(inference "gtsam" "gtsam" "${inference_excluded_files}")
endif(GTSAM_BUILD_TIMING) endif(GTSAM_BUILD_TIMING)

4
gtsam/linear/CMakeLists.txt
View File

@ -10,7 +10,7 @@ set(linear_excluded_files
# Build tests # Build tests
if (GTSAM_BUILD_TESTS) if (GTSAM_BUILD_TESTS)
gtsam_add_subdir_tests(linear "${gtsam-default}" "${gtsam-default}" "${linear_excluded_files}") gtsam_add_subdir_tests(linear "gtsam" "gtsam" "${linear_excluded_files}")
endif(GTSAM_BUILD_TESTS) endif(GTSAM_BUILD_TESTS)
if(MSVC) if(MSVC)
@ -20,5 +20,5 @@ endif()
# Build timing scripts # Build timing scripts
if (GTSAM_BUILD_TIMING) if (GTSAM_BUILD_TIMING)
gtsam_add_subdir_timing(linear "${gtsam-default}" "${gtsam-default}" "${linear_excluded_files}") gtsam_add_subdir_timing(linear "gtsam" "gtsam" "${linear_excluded_files}")
endif(GTSAM_BUILD_TIMING) endif(GTSAM_BUILD_TIMING)

12
gtsam/linear/tests/testGaussianBayesTreeUnordered.cpp
View File

@ -114,19 +114,19 @@ TEST(GaussianBayesTree, complicatedMarginal) {
// Create the conditionals to go in the BayesTree // Create the conditionals to go in the BayesTree
GaussianBayesTree bt; GaussianBayesTree bt;
bt.insertRoot( bt.insertRoot(
MakeClique(GaussianConditional(pair_list_of (11, (Matrix(3,1) << 0.0971, 0, 0).finished()) MakeClique(GaussianConditional(pair_list_of<Key, Matrix> (11, (Matrix(3,1) << 0.0971, 0, 0).finished())
(12, (Matrix(3,2) << 0.3171, 0.4387, 0.9502, 0.3816, 0, 0.7655).finished()), (12, (Matrix(3,2) << 0.3171, 0.4387, 0.9502, 0.3816, 0, 0.7655).finished()),
2, (Vector(3) << 0.2638, 0.1455, 0.1361).finished()), list_of 2, (Vector(3) << 0.2638, 0.1455, 0.1361).finished()), list_of
(MakeClique(GaussianConditional(pair_list_of (9, (Matrix(3,1) << 0.7952, 0, 0).finished()) (MakeClique(GaussianConditional(pair_list_of<Key, Matrix> (9, (Matrix(3,1) << 0.7952, 0, 0).finished())
(10, (Matrix(3,2) << 0.4456, 0.7547, 0.6463, 0.2760, 0, 0.6797).finished()) (10, (Matrix(3,2) << 0.4456, 0.7547, 0.6463, 0.2760, 0, 0.6797).finished())
(11, (Matrix(3,1) << 0.6551, 0.1626, 0.1190).finished()) (11, (Matrix(3,1) << 0.6551, 0.1626, 0.1190).finished())
(12, (Matrix(3,2) << 0.4984, 0.5853, 0.9597, 0.2238, 0.3404, 0.7513).finished()), (12, (Matrix(3,2) << 0.4984, 0.5853, 0.9597, 0.2238, 0.3404, 0.7513).finished()),
2, (Vector(3) << 0.4314, 0.9106, 0.1818).finished()))) 2, (Vector(3) << 0.4314, 0.9106, 0.1818).finished())))
(MakeClique(GaussianConditional(pair_list_of (7, (Matrix(3,1) << 0.2551, 0, 0).finished()) (MakeClique(GaussianConditional(pair_list_of<Key, Matrix> (7, (Matrix(3,1) << 0.2551, 0, 0).finished())
(8, (Matrix(3,2) << 0.8909, 0.1386, 0.9593, 0.1493, 0, 0.2575).finished()) (8, (Matrix(3,2) << 0.8909, 0.1386, 0.9593, 0.1493, 0, 0.2575).finished())
(11, (Matrix(3,1) << 0.8407, 0.2543, 0.8143).finished()), (11, (Matrix(3,1) << 0.8407, 0.2543, 0.8143).finished()),
2, (Vector(3) << 0.3998, 0.2599, 0.8001).finished()), list_of 2, (Vector(3) << 0.3998, 0.2599, 0.8001).finished()), list_of
(MakeClique(GaussianConditional(pair_list_of (5, (Matrix(3,1) << 0.2435, 0, 0).finished()) (MakeClique(GaussianConditional(pair_list_of<Key, Matrix> (5, (Matrix(3,1) << 0.2435, 0, 0).finished())
(6, (Matrix(3,2) << 0.4733, 0.1966, 0.3517, 0.2511, 0.8308, 0.0).finished()) (6, (Matrix(3,2) << 0.4733, 0.1966, 0.3517, 0.2511, 0.8308, 0.0).finished())
// NOTE the non-upper-triangular form // NOTE the non-upper-triangular form
// here since this test was written when we had column permutations // here since this test was written when we had column permutations
@ -136,11 +136,11 @@ TEST(GaussianBayesTree, complicatedMarginal) {
(7, (Matrix(3,1) << 0.5853, 0.5497, 0.9172).finished()) (7, (Matrix(3,1) << 0.5853, 0.5497, 0.9172).finished())
(8, (Matrix(3,2) << 0.2858, 0.3804, 0.7572, 0.5678, 0.7537, 0.0759).finished()), (8, (Matrix(3,2) << 0.2858, 0.3804, 0.7572, 0.5678, 0.7537, 0.0759).finished()),
2, (Vector(3) << 0.8173, 0.8687, 0.0844).finished()), list_of 2, (Vector(3) << 0.8173, 0.8687, 0.0844).finished()), list_of
(MakeClique(GaussianConditional(pair_list_of (3, (Matrix(3,1) << 0.0540, 0, 0).finished()) (MakeClique(GaussianConditional(pair_list_of<Key, Matrix> (3, (Matrix(3,1) << 0.0540, 0, 0).finished())
(4, (Matrix(3,2) << 0.9340, 0.4694, 0.1299, 0.0119, 0, 0.3371).finished()) (4, (Matrix(3,2) << 0.9340, 0.4694, 0.1299, 0.0119, 0, 0.3371).finished())
(6, (Matrix(3,2) << 0.1622, 0.5285, 0.7943, 0.1656, 0.3112, 0.6020).finished()), (6, (Matrix(3,2) << 0.1622, 0.5285, 0.7943, 0.1656, 0.3112, 0.6020).finished()),
2, (Vector(3) << 0.9619, 0.0046, 0.7749).finished()))) 2, (Vector(3) << 0.9619, 0.0046, 0.7749).finished())))
(MakeClique(GaussianConditional(pair_list_of (1, (Matrix(3,1) << 0.2630, 0, 0).finished()) (MakeClique(GaussianConditional(pair_list_of<Key, Matrix> (1, (Matrix(3,1) << 0.2630, 0, 0).finished())
(2, (Matrix(3,2) << 0.7482, 0.2290, 0.4505, 0.9133, 0, 0.1524).finished()) (2, (Matrix(3,2) << 0.7482, 0.2290, 0.4505, 0.9133, 0, 0.1524).finished())
(5, (Matrix(3,1) << 0.8258, 0.5383, 0.9961).finished()), (5, (Matrix(3,1) << 0.8258, 0.5383, 0.9961).finished()),
2, (Vector(3) << 0.0782, 0.4427, 0.1067).finished()))))))))); 2, (Vector(3) << 0.0782, 0.4427, 0.1067).finished())))))))));

4
gtsam/navigation/CMakeLists.txt
View File

@ -7,10 +7,10 @@ set (navigation_excluded_tests "")
# Add all tests # Add all tests
if (GTSAM_BUILD_TESTS) if (GTSAM_BUILD_TESTS)
gtsam_add_subdir_tests(navigation "${gtsam-default}" "${gtsam-default}" "${navigation_excluded_tests}") gtsam_add_subdir_tests(navigation "gtsam" "gtsam" "${navigation_excluded_tests}")
endif() endif()
# Build timing scripts # Build timing scripts
if (GTSAM_BUILD_TIMING) if (GTSAM_BUILD_TIMING)
gtsam_add_subdir_timing(navigation "${gtsam-default}" "${gtsam-default}" "${navigation_excluded_files}") gtsam_add_subdir_timing(navigation "gtsam" "gtsam" "${navigation_excluded_files}")
endif(GTSAM_BUILD_TIMING) endif(GTSAM_BUILD_TIMING)

9
gtsam/navigation/CombinedImuFactor.h
View File

@ -11,7 +11,7 @@
/** /**
* @file CombinedImuFactor.h * @file CombinedImuFactor.h
* @author Luca Carlone, Stephen Williams, Richard Roberts * @author Luca Carlone, Stephen Williams, Richard Roberts, Vadim Indelman
**/ **/
#pragma once #pragma once
@ -342,11 +342,8 @@ namespace gtsam {
public: public:
/** Shorthand for a smart pointer to a factor */ /** Shorthand for a smart pointer to a factor */
#ifndef _MSC_VER typedef boost::shared_ptr<CombinedImuFactor> shared_ptr;
typedef typename boost::shared_ptr<CombinedImuFactor> shared_ptr;
#else
typedef boost::shared_ptr<CombinedImuFactor> shared_ptr;
#endif
/** Default constructor - only use for serialization */ /** Default constructor - only use for serialization */
CombinedImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6)) {} CombinedImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6)) {}

7
gtsam/navigation/ImuFactor.h
View File

@ -11,7 +11,7 @@
/** /**
* @file ImuFactor.h * @file ImuFactor.h
* @author Luca Carlone, Stephen Williams, Richard Roberts * @author Luca Carlone, Stephen Williams, Richard Roberts, Vadim Indelman
**/ **/
#pragma once #pragma once
@ -304,11 +304,8 @@ namespace gtsam {
public: public:
/** Shorthand for a smart pointer to a factor */ /** Shorthand for a smart pointer to a factor */
#ifndef _MSC_VER
typedef typename boost::shared_ptr<ImuFactor> shared_ptr;
#else
typedef boost::shared_ptr<ImuFactor> shared_ptr; typedef boost::shared_ptr<ImuFactor> shared_ptr;
#endif
/** Default constructor - only use for serialization */ /** Default constructor - only use for serialization */
ImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()) {} ImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()) {}

4
gtsam/nonlinear/CMakeLists.txt
View File

@ -10,11 +10,11 @@ set(nonlinear_excluded_files
# Build tests # Build tests
if (GTSAM_BUILD_TESTS) if (GTSAM_BUILD_TESTS)
gtsam_add_subdir_tests(nonlinear "${gtsam-default}" "${gtsam-default}" "${nonlinear_excluded_files}") gtsam_add_subdir_tests(nonlinear "gtsam" "gtsam" "${nonlinear_excluded_files}")
endif(GTSAM_BUILD_TESTS) endif(GTSAM_BUILD_TESTS)
# Build timing scripts # Build timing scripts
if (GTSAM_BUILD_TIMING) if (GTSAM_BUILD_TIMING)
gtsam_add_subdir_timing(nonlinear "${gtsam-default}" "${gtsam-default}" "${nonlinear_excluded_files}") gtsam_add_subdir_timing(nonlinear "gtsam" "gtsam" "${nonlinear_excluded_files}")
endif(GTSAM_BUILD_TIMING) endif(GTSAM_BUILD_TIMING)

4
gtsam/slam/CMakeLists.txt
View File

@ -15,10 +15,10 @@ set(slam_excluded_files
# Build tests # Build tests
if (GTSAM_BUILD_TESTS) if (GTSAM_BUILD_TESTS)
gtsam_add_subdir_tests(slam "${gtsam-default}" "${gtsam-default}" "${slam_excluded_files}") gtsam_add_subdir_tests(slam "gtsam" "gtsam" "${slam_excluded_files}")
endif(GTSAM_BUILD_TESTS) endif(GTSAM_BUILD_TESTS)
# Build timing scripts # Build timing scripts
if (GTSAM_BUILD_TIMING) if (GTSAM_BUILD_TIMING)
gtsam_add_subdir_timing(slam "${gtsam-default}" "${gtsam-default}" "${slam_excluded_files}") gtsam_add_subdir_timing(slam "gtsam" "gtsam" "${slam_excluded_files}")
endif(GTSAM_BUILD_TIMING) endif(GTSAM_BUILD_TIMING)

4
gtsam/slam/tests/testEssentialMatrixFactor.cpp
View File

@ -37,7 +37,7 @@ bool readOK = readBAL(filename, data);
Rot3 c1Rc2 = data.cameras[1].pose().rotation(); Rot3 c1Rc2 = data.cameras[1].pose().rotation();
Point3 c1Tc2 = data.cameras[1].pose().translation(); Point3 c1Tc2 = data.cameras[1].pose().translation();
PinholeCamera<Cal3_S2> camera2(data.cameras[1].pose(),Cal3_S2()); PinholeCamera<Cal3_S2> camera2(data.cameras[1].pose(),Cal3_S2());
EssentialMatrix trueE(c1Rc2, c1Tc2); EssentialMatrix trueE(c1Rc2, Sphere2(c1Tc2));
double baseline = 0.1; // actual baseline of the camera double baseline = 0.1; // actual baseline of the camera
Point2 pA(size_t i) { Point2 pA(size_t i) {
@ -297,7 +297,7 @@ SfM_data data;
bool readOK = readBAL(filename, data); bool readOK = readBAL(filename, data);
Rot3 aRb = data.cameras[1].pose().rotation(); Rot3 aRb = data.cameras[1].pose().rotation();
Point3 aTb = data.cameras[1].pose().translation(); Point3 aTb = data.cameras[1].pose().translation();
EssentialMatrix trueE(aRb, aTb); EssentialMatrix trueE(aRb, Sphere2(aTb));
double baseline = 10; // actual baseline of the camera double baseline = 10; // actual baseline of the camera

4
gtsam/symbolic/CMakeLists.txt
View File

@ -10,11 +10,11 @@ set(symbolic_excluded_files
# Build tests # Build tests
if (GTSAM_BUILD_TESTS) if (GTSAM_BUILD_TESTS)
gtsam_add_subdir_tests(symbolic "${gtsam-default}" "${gtsam-default}" "${symbolic_excluded_files}") gtsam_add_subdir_tests(symbolic "gtsam" "gtsam" "${symbolic_excluded_files}")
endif(GTSAM_BUILD_TESTS) endif(GTSAM_BUILD_TESTS)
# Build timing scripts # Build timing scripts
if (GTSAM_BUILD_TIMING) if (GTSAM_BUILD_TIMING)
gtsam_add_subdir_timing(symbolic "${gtsam-default}" "${gtsam-default}" "${symbolic_excluded_files}") gtsam_add_subdir_timing(symbolic "gtsam" "gtsam" "${symbolic_excluded_files}")
endif(GTSAM_BUILD_TIMING) endif(GTSAM_BUILD_TIMING)

32
gtsam_unstable/CMakeLists.txt
View File

@ -58,40 +58,38 @@ message(STATUS "Install prefix: ${CMAKE_INSTALL_PREFIX}")
# build shared and static versions of the library # build shared and static versions of the library
if (GTSAM_BUILD_STATIC_LIBRARY) if (GTSAM_BUILD_STATIC_LIBRARY)
message(STATUS "Building GTSAM_UNSTABLE - static") message(STATUS "Building GTSAM_UNSTABLE - static")
add_library(gtsam_unstable-static STATIC ${gtsam_unstable_srcs}) add_library(gtsam_unstable STATIC ${gtsam_unstable_srcs})
set_target_properties(gtsam_unstable-static PROPERTIES set_target_properties(gtsam_unstable PROPERTIES
OUTPUT_NAME gtsam_unstable OUTPUT_NAME gtsam_unstable
CLEAN_DIRECT_OUTPUT 1 CLEAN_DIRECT_OUTPUT 1
VERSION ${gtsam_unstable_version} VERSION ${gtsam_unstable_version}
SOVERSION ${gtsam_unstable_soversion}) SOVERSION ${gtsam_unstable_soversion})
if(WIN32) # Add 'lib' prefix to static library to avoid filename collision with shared library if(WIN32) # Add 'lib' prefix to static library to avoid filename collision with shared library
set_target_properties(gtsam_unstable-static PROPERTIES set_target_properties(gtsam_unstable PROPERTIES
PREFIX "lib" PREFIX "lib"
COMPILE_DEFINITIONS GTSAM_UNSTABLE_IMPORT_STATIC) COMPILE_DEFINITIONS GTSAM_UNSTABLE_IMPORT_STATIC)
endif() endif()
target_link_libraries(gtsam_unstable-static gtsam-static ${GTSAM_UNSTABLE_BOOST_LIBRARIES}) target_link_libraries(gtsam_unstable gtsam ${GTSAM_UNSTABLE_BOOST_LIBRARIES})
install(TARGETS gtsam_unstable-static EXPORT GTSAM-exports ARCHIVE DESTINATION lib) install(TARGETS gtsam_unstable EXPORT GTSAM-exports ARCHIVE DESTINATION lib)
list(APPEND GTSAM_EXPORTED_TARGETS gtsam_unstable-static) list(APPEND GTSAM_EXPORTED_TARGETS gtsam_unstable)
set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE) set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
endif() else()
if (GTSAM_BUILD_SHARED_LIBRARY)
message(STATUS "Building GTSAM_UNSTABLE - shared") message(STATUS "Building GTSAM_UNSTABLE - shared")
add_library(gtsam_unstable-shared SHARED ${gtsam_unstable_srcs}) add_library(gtsam_unstable SHARED ${gtsam_unstable_srcs})
set_target_properties(gtsam_unstable-shared PROPERTIES set_target_properties(gtsam_unstable PROPERTIES
OUTPUT_NAME gtsam_unstable OUTPUT_NAME gtsam_unstable
CLEAN_DIRECT_OUTPUT 1 CLEAN_DIRECT_OUTPUT 1
VERSION ${gtsam_unstable_version} VERSION ${gtsam_unstable_version}
SOVERSION ${gtsam_unstable_soversion}) SOVERSION ${gtsam_unstable_soversion})
if(WIN32) if(WIN32)
set_target_properties(gtsam_unstable-shared PROPERTIES set_target_properties(gtsam_unstable PROPERTIES
PREFIX "" PREFIX ""
DEFINE_SYMBOL GTSAM_UNSTABLE_EXPORTS DEFINE_SYMBOL GTSAM_UNSTABLE_EXPORTS
RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/bin") RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/bin")
endif() endif()
target_link_libraries(gtsam_unstable-shared gtsam-shared ${GTSAM_UNSTABLE_BOOST_LIBRARIES}) target_link_libraries(gtsam_unstable gtsam ${GTSAM_UNSTABLE_BOOST_LIBRARIES})
install(TARGETS gtsam_unstable-shared EXPORT GTSAM-exports LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin) install(TARGETS gtsam_unstable EXPORT GTSAM-exports LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin)
list(APPEND GTSAM_EXPORTED_TARGETS gtsam_unstable-shared) list(APPEND GTSAM_EXPORTED_TARGETS gtsam_unstable)
set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE) set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
endif() endif()
@ -102,12 +100,12 @@ if (GTSAM_INSTALL_MATLAB_TOOLBOX)
# Generate, build and install toolbox # Generate, build and install toolbox
set(mexFlags "${GTSAM_BUILD_MEX_BINARY_FLAGS}") set(mexFlags "${GTSAM_BUILD_MEX_BINARY_FLAGS}")
if("${gtsam-default}" STREQUAL "gtsam-static") if(GTSAM_BUILD_STATIC_LIBRARY)
list(APPEND mexFlags -DGTSAM_IMPORT_STATIC) list(APPEND mexFlags -DGTSAM_IMPORT_STATIC)
endif() endif()
# Wrap # Wrap
wrap_and_install_library(gtsam_unstable.h "${gtsam-default};${gtsam_unstable-default}" "" "${mexFlags}") wrap_and_install_library(gtsam_unstable.h "gtsam;gtsam_unstable" "" "${mexFlags}")
endif(GTSAM_INSTALL_MATLAB_TOOLBOX) endif(GTSAM_INSTALL_MATLAB_TOOLBOX)

4
gtsam_unstable/base/CMakeLists.txt
View File

@ -3,8 +3,8 @@ file(GLOB base_headers "*.h")
install(FILES ${base_headers} DESTINATION include/gtsam_unstable/base) install(FILES ${base_headers} DESTINATION include/gtsam_unstable/base)
set (base_full_libs set (base_full_libs
${gtsam-default} gtsam
${gtsam_unstable-default}) gtsam_unstable)
# Exclude tests that don't work # Exclude tests that don't work
set (base_excluded_tests "") set (base_excluded_tests "")

8
gtsam_unstable/discrete/CMakeLists.txt
View File

@ -3,8 +3,8 @@ file(GLOB discrete_headers "*.h")
install(FILES ${discrete_headers} DESTINATION include/gtsam_unstable/discrete) install(FILES ${discrete_headers} DESTINATION include/gtsam_unstable/discrete)
set (discrete_full_libs set (discrete_full_libs
${gtsam-default} gtsam
${gtsam_unstable-default}) gtsam_unstable)
# Exclude tests that don't work # Exclude tests that don't work
#set (discrete_excluded_tests #set (discrete_excluded_tests
@ -32,11 +32,11 @@ if (GTSAM_BUILD_EXAMPLES)
set_target_properties(${example} PROPERTIES EXCLUDE_FROM_ALL ON) set_target_properties(${example} PROPERTIES EXCLUDE_FROM_ALL ON)
endif() endif()
if(NOT MSVC) if(NOT MSVC AND NOT XCODE_VERSION)
add_dependencies(examples ${example}) add_dependencies(examples ${example})
add_custom_target(${example}.run ${EXECUTABLE_OUTPUT_PATH}${example} ${ARGN}) add_custom_target(${example}.run ${EXECUTABLE_OUTPUT_PATH}${example} ${ARGN})
endif() endif()
target_link_libraries(${example} ${gtsam-default} ${gtsam_unstable-default}) target_link_libraries(${example} gtsam gtsam_unstable)
endforeach(example) endforeach(example)
endif (GTSAM_BUILD_EXAMPLES) endif (GTSAM_BUILD_EXAMPLES)

4
gtsam_unstable/dynamics/CMakeLists.txt
View File

@ -4,8 +4,8 @@ install(FILES ${dynamics_headers} DESTINATION include/gtsam_unstable/dynamics)
# Components to link tests in this subfolder against # Components to link tests in this subfolder against
set (dynamics_full_libs set (dynamics_full_libs
${gtsam-default} gtsam
${gtsam_unstable-default}) gtsam_unstable)
# Exclude tests that don't work # Exclude tests that don't work
set (dynamics_excluded_tests "") set (dynamics_excluded_tests "")

4
gtsam_unstable/examples/CMakeLists.txt
View File

@ -18,8 +18,8 @@ foreach(example_src ${example_srcs} )
set_target_properties(${example_bin} PROPERTIES EXCLUDE_FROM_ALL ON) set_target_properties(${example_bin} PROPERTIES EXCLUDE_FROM_ALL ON)
endif() endif()
target_link_libraries(${example_bin} ${gtsam-default} ${gtsam_unstable-default}) target_link_libraries(${example_bin} gtsam gtsam_unstable)
if(NOT MSVC) if(NOT MSVC AND NOT XCODE_VERSION)
add_custom_target(${example_bin}.run ${EXECUTABLE_OUTPUT_PATH}${example_bin} ${ARGN}) add_custom_target(${example_bin}.run ${EXECUTABLE_OUTPUT_PATH}${example_bin} ${ARGN})
endif() endif()

4
gtsam_unstable/geometry/CMakeLists.txt
View File

@ -4,8 +4,8 @@ install(FILES ${geometry_headers} DESTINATION include/gtsam_unstable/geometry)
# Components to link tests in this subfolder against # Components to link tests in this subfolder against
set (geometry_full_libs set (geometry_full_libs
${gtsam-default} gtsam
${gtsam_unstable-default}) gtsam_unstable)
# Exclude tests that don't work # Exclude tests that don't work
set (geometry_excluded_tests "") set (geometry_excluded_tests "")

14
gtsam_unstable/gtsam_unstable.h
View File

@ -352,6 +352,20 @@ virtual class TransformBtwRobotsUnaryFactorEM : gtsam::NonlinearFactor {
void serializable() const; // enabling serialization functionality void serializable() const; // enabling serialization functionality
}; };
#include <gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h>
template<T = {gtsam::Pose2}>
virtual class TransformBtwRobotsUnaryFactor : gtsam::NonlinearFactor {
TransformBtwRobotsUnaryFactor(size_t key, const T& relativePose, size_t keyA, size_t keyB,
const gtsam::Values& valA, const gtsam::Values& valB,
const gtsam::noiseModel::Gaussian* model);
Vector whitenedError(const gtsam::Values& x);
Vector unwhitenedError(const gtsam::Values& x);
void setValAValB(const gtsam::Values valA, const gtsam::Values valB);
void serializable() const; // enabling serialization functionality
};
#include <gtsam_unstable/slam/SmartRangeFactor.h> #include <gtsam_unstable/slam/SmartRangeFactor.h>
virtual class SmartRangeFactor : gtsam::NoiseModelFactor { virtual class SmartRangeFactor : gtsam::NoiseModelFactor {
SmartRangeFactor(double s); SmartRangeFactor(double s);

4
gtsam_unstable/nonlinear/CMakeLists.txt
View File

@ -4,8 +4,8 @@ install(FILES ${nonlinear_headers} DESTINATION include/gtsam_unstable/nonlinear)
# Components to link tests in this subfolder against # Components to link tests in this subfolder against
set (nonlinear_full_libs set (nonlinear_full_libs
${gtsam-default} gtsam
${gtsam_unstable-default}) gtsam_unstable)
# Exclude tests that don't work # Exclude tests that don't work
set (nonlinear_excluded_tests #"") set (nonlinear_excluded_tests #"")

4
gtsam_unstable/slam/CMakeLists.txt
View File

@ -9,8 +9,8 @@ install(FILES ${slam_headers} DESTINATION include/gtsam_unstable/slam)
# Components to link tests in this subfolder against # Components to link tests in this subfolder against
set (slam_full_libs set (slam_full_libs
${gtsam-default} gtsam
${gtsam_unstable-default}) gtsam_unstable)
# Exclude tests that don't work # Exclude tests that don't work
set (slam_excluded_tests set (slam_excluded_tests

245
gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h Normal file
View File

@ -0,0 +1,245 @@
/* ----------------------------------------------------------------------------
* 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 TransformBtwRobotsUnaryFactor.h
* @brief Unary factor for determining transformation between given trajectories of two robots
* @author Vadim Indelman
**/
#pragma once
#include <ostream>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Lie.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/slam/BetweenFactor.h>
namespace gtsam {
/**
* A class for a measurement predicted by "between(config[key1],config[key2])"
* @tparam VALUE the Value type
* @addtogroup SLAM
*/
template<class VALUE>
class TransformBtwRobotsUnaryFactor: public NonlinearFactor {
public:
typedef VALUE T;
private:
typedef TransformBtwRobotsUnaryFactor<VALUE> This;
typedef gtsam::NonlinearFactor Base;
gtsam::Key key_;
VALUE measured_; /** The measurement */
gtsam::Values valA_; // given values for robot A map\trajectory
gtsam::Values valB_; // given values for robot B map\trajectory
gtsam::Key keyA_; // key of robot A to which the measurement refers
gtsam::Key keyB_; // key of robot B to which the measurement refers
SharedGaussian model_;
/** concept check by type */
GTSAM_CONCEPT_LIE_TYPE(T)
GTSAM_CONCEPT_TESTABLE_TYPE(T)
public:
// shorthand for a smart pointer to a factor
typedef typename boost::shared_ptr<TransformBtwRobotsUnaryFactor> shared_ptr;
/** default constructor - only use for serialization */
TransformBtwRobotsUnaryFactor() {}
/** Constructor */
TransformBtwRobotsUnaryFactor(Key key, const VALUE& measured, Key keyA, Key keyB,
const gtsam::Values valA, const gtsam::Values valB,
const SharedGaussian& model) :
Base(cref_list_of<1>(key)), key_(key), measured_(measured), keyA_(keyA), keyB_(keyB),
model_(model){
setValAValB(valA, valB);
}
virtual ~TransformBtwRobotsUnaryFactor() {}
/** Clone */
virtual gtsam::NonlinearFactor::shared_ptr clone() const { return boost::make_shared<This>(*this); }
/** implement functions needed for Testable */
/** print */
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "TransformBtwRobotsUnaryFactor("
<< keyFormatter(key_) << ")\n";
std::cout << "MR between factor keys: "
<< keyFormatter(keyA_) << ","
<< keyFormatter(keyB_) << "\n";
measured_.print(" measured: ");
model_->print(" noise model: ");
// Base::print(s, keyFormatter);
}
/** equals */
virtual bool equals(const NonlinearFactor& f, double tol=1e-9) const {
const This *t = dynamic_cast<const This*> (&f);
if(t && Base::equals(f))
return key_ == t->key_ && measured_.equals(t->measured_);
else
return false;
}
/** implement functions needed to derive from Factor */
/* ************************************************************************* */
void setValAValB(const gtsam::Values valA, const gtsam::Values valB){
if ( (!valA.exists(keyA_)) && (!valB.exists(keyA_)) && (!valA.exists(keyB_)) && (!valB.exists(keyB_)) )
throw("something is wrong!");
// TODO: make sure the two keys belong to different robots
if (valA.exists(keyA_)){
valA_ = valA;
valB_ = valB;
}
else {
valA_ = valB;
valB_ = valA;
}
}
/* ************************************************************************* */
virtual double error(const gtsam::Values& x) const {
return whitenedError(x).squaredNorm();
}
/* ************************************************************************* */
/**
* Linearize a non-linearFactorN to get a gtsam::GaussianFactor,
* \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$
* Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$
*/
/* This version of linearize recalculates the noise model each time */
virtual boost::shared_ptr<gtsam::GaussianFactor> linearize(const gtsam::Values& x) const {
// Only linearize if the factor is active
if (!this->active(x))
return boost::shared_ptr<gtsam::JacobianFactor>();
//std::cout<<"About to linearize"<<std::endl;
gtsam::Matrix A1;
std::vector<gtsam::Matrix> A(this->size());
gtsam::Vector b = -whitenedError(x, A);
A1 = A[0];
return gtsam::GaussianFactor::shared_ptr(
new gtsam::JacobianFactor(key_, A1, b, gtsam::noiseModel::Unit::Create(b.size())));
}
/* ************************************************************************* */
gtsam::Vector whitenedError(const gtsam::Values& x,
boost::optional<std::vector<gtsam::Matrix>&> H = boost::none) const {
bool debug = true;
Matrix H_compose, H_between1, H_dummy;
T orgA_T_currA = valA_.at<T>(keyA_);
T orgB_T_currB = valB_.at<T>(keyB_);
T orgA_T_orgB = x.at<T>(key_);
T orgA_T_currB = orgA_T_orgB.compose(orgB_T_currB, H_compose, H_dummy);
T currA_T_currB_pred = orgA_T_currA.between(orgA_T_currB, H_dummy, H_between1);
T currA_T_currB_msr = measured_;
Vector err_unw = currA_T_currB_msr.localCoordinates(currA_T_currB_pred);
Vector err_wh = err_unw;
if (H) {
(*H)[0] = H_compose * H_between1;
model_->WhitenSystem(*H, err_wh);
}
else {
model_->whitenInPlace(err_wh);
}
Vector err_wh2 = model_->whiten(err_wh);
if (debug){
// std::cout<<"err_wh: "<<err_wh[0]<<err_wh[1]<<err_wh[2]<<std::endl;
// std::cout<<"err_wh2: "<<err_wh2[0]<<err_wh2[1]<<err_wh2[2]<<std::endl;
// std::cout<<"H_compose - rows, cols, : "<<H_compose.rows()<<", "<< H_compose.cols()<<std::endl;
// std::cout<<"H_between1 - rows, cols, : "<<H_between1.rows()<<", "<< H_between1.cols()<<std::endl;
// std::cout<<"H_unwh - rows, cols, : "<<H_unwh.rows()<<", "<< H_unwh.cols()<<std::endl;
// std::cout<<"H_unwh: "<<std:endl<<H_unwh[0]
}
return err_wh;
}
/* ************************************************************************* */
gtsam::Vector unwhitenedError(const gtsam::Values& x) const {
T orgA_T_currA = valA_.at<T>(keyA_);
T orgB_T_currB = valB_.at<T>(keyB_);
T orgA_T_orgB = x.at<T>(key_);
T orgA_T_currB = orgA_T_orgB.compose(orgB_T_currB);
T currA_T_currB_pred = orgA_T_currA.between(orgA_T_currB);
T currA_T_currB_msr = measured_;
return currA_T_currB_msr.localCoordinates(currA_T_currB_pred);
}
/* ************************************************************************* */
/** number of variables attached to this factor */
std::size_t size() const {
return 1;
}
virtual size_t dim() const {
return model_->R().rows() + model_->R().cols();
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearFactor",
boost::serialization::base_object<Base>(*this));
//ar & BOOST_SERIALIZATION_NVP(measured_);
}
}; // \class TransformBtwRobotsUnaryFactor
} /// namespace gtsam

317
gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp Normal file
View File

@ -0,0 +1,317 @@
/**
* @file testBTransformBtwRobotsUnaryFactor.cpp
* @brief Unit test for the TransformBtwRobotsUnaryFactor
* @author Vadim Indelman
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
//#include <gtsam/linear/GaussianSequentialSolver.h>
using namespace std;
using namespace gtsam;
// Disabled this test because it is currently failing - remove the lines "#if 0" and "#endif" below
// to reenable the test.
//#if 0
/* ************************************************************************* */
LieVector predictionError(const Pose2& org1_T_org2, const gtsam::Key& key, const TransformBtwRobotsUnaryFactor<gtsam::Pose2>& factor){
gtsam::Values values;
values.insert(key, org1_T_org2);
// LieVector err = factor.whitenedError(values);
// return err;
return LieVector::Expmap(factor.whitenedError(values));
}
/* ************************************************************************* */
//LieVector predictionError_standard(const Pose2& p1, const Pose2& p2, const gtsam::Key& keyA, const gtsam::Key& keyB, const BetweenFactor<gtsam::Pose2>& factor){
// gtsam::Values values;
// values.insert(keyA, p1);
// values.insert(keyB, p2);
// // LieVector err = factor.whitenedError(values);
// // return err;
// return LieVector::Expmap(factor.whitenedError(values));
//}
/* ************************************************************************* */
TEST( TransformBtwRobotsUnaryFactor, ConstructorAndEquals)
{
gtsam::Key key(0);
gtsam::Key keyA(1);
gtsam::Key keyB(2);
gtsam::Pose2 p1(10.0, 15.0, 0.1);
gtsam::Pose2 p2(15.0, 15.0, 0.3);
gtsam::Pose2 noise(0.5, 0.4, 0.01);
gtsam::Pose2 rel_pose_ideal = p1.between(p2);
gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise);
SharedGaussian model(noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.05)));
gtsam::Values valA, valB;
valA.insert(keyA, p1);
valB.insert(keyB, p2);
// Constructor
TransformBtwRobotsUnaryFactor<gtsam::Pose2> g(key, rel_pose_msr, keyA, keyB, valA, valB, model);
TransformBtwRobotsUnaryFactor<gtsam::Pose2> h(key, rel_pose_msr, keyA, keyB, valA, valB, model);
// Equals
CHECK(assert_equal(g, h, 1e-5));
}
/* ************************************************************************* */
TEST( TransformBtwRobotsUnaryFactor, unwhitenedError)
{
gtsam::Key key(0);
gtsam::Key keyA(1);
gtsam::Key keyB(2);
gtsam::Pose2 orgA_T_1(10.0, 15.0, 0.1);
gtsam::Pose2 orgB_T_2(15.0, 15.0, 0.3);
gtsam::Pose2 orgA_T_orgB(100.0, 45.0, 1.8);
gtsam::Pose2 orgA_T_2 = orgA_T_orgB.compose(orgB_T_2);
gtsam::Pose2 rel_pose_ideal = orgA_T_1.between(orgA_T_2);
gtsam::Pose2 rel_pose_msr = rel_pose_ideal;
SharedGaussian model(noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.05)));
gtsam::Values valA, valB;
valA.insert(keyA, orgA_T_1);
valB.insert(keyB, orgB_T_2);
// Constructor
TransformBtwRobotsUnaryFactor<gtsam::Pose2> g(key, rel_pose_msr, keyA, keyB, valA, valB, model);
gtsam::Values values;
values.insert(key, orgA_T_orgB);
Vector err = g.unwhitenedError(values);
// Equals
CHECK(assert_equal(err, zero(3), 1e-5));
}
/* ************************************************************************* */
TEST( TransformBtwRobotsUnaryFactor, unwhitenedError2)
{
gtsam::Key key(0);
gtsam::Key keyA(1);
gtsam::Key keyB(2);
gtsam::Pose2 orgA_T_currA(0.0, 0.0, 0.0);
gtsam::Pose2 orgB_T_currB(-10.0, 15.0, 0.1);
gtsam::Pose2 orgA_T_orgB(0.0, 0.0, 0.0);
gtsam::Pose2 orgA_T_currB = orgA_T_orgB.compose(orgB_T_currB);
gtsam::Pose2 rel_pose_ideal = orgA_T_currA.between(orgA_T_currB);
gtsam::Pose2 rel_pose_msr = rel_pose_ideal;
SharedGaussian model(noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.05)));
double prior_outlier = 0.01;
double prior_inlier = 0.99;
gtsam::Values valA, valB;
valA.insert(keyA, orgA_T_currA);
valB.insert(keyB, orgB_T_currB);
// Constructor
TransformBtwRobotsUnaryFactor<gtsam::Pose2> g(key, rel_pose_msr, keyA, keyB, valA, valB, model);
gtsam::Values values;
values.insert(key, orgA_T_orgB);
Vector err = g.unwhitenedError(values);
// Equals
CHECK(assert_equal(err, zero(3), 1e-5));
}
/* ************************************************************************* */
TEST( TransformBtwRobotsUnaryFactor, Optimize)
{
gtsam::Key key(0);
gtsam::Key keyA(1);
gtsam::Key keyB(2);
gtsam::Pose2 orgA_T_currA(0.0, 0.0, 0.0);
gtsam::Pose2 orgB_T_currB(1.0, 2.0, 0.05);
gtsam::Pose2 orgA_T_orgB_tr(10.0, -15.0, 0.0);
gtsam::Pose2 orgA_T_currB_tr = orgA_T_orgB_tr.compose(orgB_T_currB);
gtsam::Pose2 currA_T_currB_tr = orgA_T_currA.between(orgA_T_currB_tr);
// some error in measurements
// gtsam::Pose2 currA_Tmsr_currB1 = currA_T_currB_tr.compose(gtsam::Pose2(0.1, 0.02, 0.01));
// gtsam::Pose2 currA_Tmsr_currB2 = currA_T_currB_tr.compose(gtsam::Pose2(-0.1, 0.02, 0.01));
// gtsam::Pose2 currA_Tmsr_currB3 = currA_T_currB_tr.compose(gtsam::Pose2(0.1, -0.02, 0.01));
// gtsam::Pose2 currA_Tmsr_currB4 = currA_T_currB_tr.compose(gtsam::Pose2(0.1, 0.02, -0.01));
// ideal measurements
gtsam::Pose2 currA_Tmsr_currB1 = currA_T_currB_tr.compose(gtsam::Pose2(0.0, 0.0, 0.0));
gtsam::Pose2 currA_Tmsr_currB2 = currA_Tmsr_currB1;
gtsam::Pose2 currA_Tmsr_currB3 = currA_Tmsr_currB1;
gtsam::Pose2 currA_Tmsr_currB4 = currA_Tmsr_currB1;
SharedGaussian model(noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.05)));
gtsam::Values valA, valB;
valA.insert(keyA, orgA_T_currA);
valB.insert(keyB, orgB_T_currB);
// Constructor
TransformBtwRobotsUnaryFactor<gtsam::Pose2> g1(key, currA_Tmsr_currB1, keyA, keyB, valA, valB, model);
TransformBtwRobotsUnaryFactor<gtsam::Pose2> g2(key, currA_Tmsr_currB2, keyA, keyB, valA, valB, model);
TransformBtwRobotsUnaryFactor<gtsam::Pose2> g3(key, currA_Tmsr_currB3, keyA, keyB, valA, valB, model);
TransformBtwRobotsUnaryFactor<gtsam::Pose2> g4(key, currA_Tmsr_currB4, keyA, keyB, valA, valB, model);
gtsam::Values values;
values.insert(key, gtsam::Pose2());
gtsam::NonlinearFactorGraph graph;
graph.push_back(g1);
graph.push_back(g2);
graph.push_back(g3);
graph.push_back(g4);
gtsam::GaussNewtonParams params;
gtsam::GaussNewtonOptimizer optimizer(graph, values, params);
gtsam::Values result = optimizer.optimize();
gtsam::Pose2 orgA_T_orgB_opt = result.at<gtsam::Pose2>(key);
CHECK(assert_equal(orgA_T_orgB_opt, orgA_T_orgB_tr, 1e-5));
}
/* ************************************************************************* */
TEST( TransformBtwRobotsUnaryFactor, Jacobian)
{
gtsam::Key key(0);
gtsam::Key keyA(1);
gtsam::Key keyB(2);
gtsam::Pose2 orgA_T_1(10.0, 15.0, 0.1);
gtsam::Pose2 orgB_T_2(15.0, 15.0, 0.3);
gtsam::Pose2 orgA_T_orgB(100.0, 45.0, 1.8);
gtsam::Pose2 orgA_T_2 = orgA_T_orgB.compose(orgB_T_2);
gtsam::Pose2 noise(0.5, 0.4, 0.01);
gtsam::Pose2 rel_pose_ideal = orgA_T_1.between(orgA_T_2);
gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise);
SharedGaussian model(noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 0.05)));
gtsam::Values valA, valB;
valA.insert(keyA, orgA_T_1);
valB.insert(keyB, orgB_T_2);
// Constructor
TransformBtwRobotsUnaryFactor<gtsam::Pose2> g(key, rel_pose_msr, keyA, keyB, valA, valB, model);
gtsam::Values values;
values.insert(key, orgA_T_orgB);
std::vector<gtsam::Matrix> H_actual(1);
Vector actual_err_wh = g.whitenedError(values, H_actual);
Matrix H1_actual = H_actual[0];
double stepsize = 1.0e-9;
Matrix H1_expected = gtsam::numericalDerivative11<LieVector, Pose2>(boost::bind(&predictionError, _1, key, g), orgA_T_orgB, stepsize);
// CHECK( assert_equal(H1_expected, H1_actual, 1e-5));
}
/////* ************************************************************************** */
//TEST (TransformBtwRobotsUnaryFactor, jacobian ) {
//
// gtsam::Key keyA(1);
// gtsam::Key keyB(2);
//
// // Inlier test
// gtsam::Pose2 p1(10.0, 15.0, 0.1);
// gtsam::Pose2 p2(15.0, 15.0, 0.3);
// gtsam::Pose2 noise(0.5, 0.4, 0.01);
// gtsam::Pose2 rel_pose_ideal = p1.between(p2);
// gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise);
//
// SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas(gtsam::(Vector(3) << 0.5, 0.5, 0.05)));
// SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas(gtsam::(Vector(3) << 50.0, 50.0, 10.0)));
//
// gtsam::Values values;
// values.insert(keyA, p1);
// values.insert(keyB, p2);
//
// double prior_outlier = 0.0;
// double prior_inlier = 1.0;
//
// TransformBtwRobotsUnaryFactor<gtsam::Pose2> f(keyA, keyB, rel_pose_msr, model_inlier, model_outlier,
// prior_inlier, prior_outlier);
//
// std::vector<gtsam::Matrix> H_actual(2);
// Vector actual_err_wh = f.whitenedError(values, H_actual);
//
// Matrix H1_actual = H_actual[0];
// Matrix H2_actual = H_actual[1];
//
// // compare to standard between factor
// BetweenFactor<gtsam::Pose2> h(keyA, keyB, rel_pose_msr, model_inlier );
// Vector actual_err_wh_stnd = h.whitenedError(values);
// Vector actual_err_wh_inlier = (Vector(3) << actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]);
// CHECK( assert_equal(actual_err_wh_stnd, actual_err_wh_inlier, 1e-8));
// std::vector<gtsam::Matrix> H_actual_stnd_unwh(2);
// (void)h.unwhitenedError(values, H_actual_stnd_unwh);
// Matrix H1_actual_stnd_unwh = H_actual_stnd_unwh[0];
// Matrix H2_actual_stnd_unwh = H_actual_stnd_unwh[1];
// Matrix H1_actual_stnd = model_inlier->Whiten(H1_actual_stnd_unwh);
// Matrix H2_actual_stnd = model_inlier->Whiten(H2_actual_stnd_unwh);
//// CHECK( assert_equal(H1_actual_stnd, H1_actual, 1e-8));
//// CHECK( assert_equal(H2_actual_stnd, H2_actual, 1e-8));
//
// double stepsize = 1.0e-9;
// Matrix H1_expected = gtsam::numericalDerivative11<LieVector, Pose2>(boost::bind(&predictionError, _1, p2, keyA, keyB, f), p1, stepsize);
// Matrix H2_expected = gtsam::numericalDerivative11<LieVector, Pose2>(boost::bind(&predictionError, p1, _1, keyA, keyB, f), p2, stepsize);
//
//
// // try to check numerical derivatives of a standard between factor
// Matrix H1_expected_stnd = gtsam::numericalDerivative11<LieVector, Pose2>(boost::bind(&predictionError_standard, _1, p2, keyA, keyB, h), p1, stepsize);
// CHECK( assert_equal(H1_expected_stnd, H1_actual_stnd, 1e-5));
//
//
// CHECK( assert_equal(H1_expected, H1_actual, 1e-8));
// CHECK( assert_equal(H2_expected, H2_actual, 1e-8));
//
//}
//#endif
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */

2
tests/CMakeLists.txt
View File

@ -1,6 +1,6 @@
# Assemble local libraries # Assemble local libraries
set (tests_full_libs set (tests_full_libs
${gtsam-default} gtsam
CppUnitLite) CppUnitLite)
# exclude certain files # exclude certain files