Merge branch 'develop' of bitbucket.org:gtborg/gtsam into feature/partition
Andrew Melim
commit
2e1e4951e1
|
|
@ -52,8 +52,7 @@ option(GTSAM_BUILD_EXAMPLES "Enable/Disable building of examples"
|
|||
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)
|
||||
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)
|
||||
|
|
@ -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")
|
||||
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)
|
||||
|
|
@ -171,23 +169,27 @@ find_package(GooglePerfTools)
|
|||
|
||||
###############################################################################
|
||||
# Find MKL
|
||||
find_package(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})
|
||||
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
|
||||
find_package(OpenMP)
|
||||
# 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}")
|
||||
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()
|
||||
|
||||
|
||||
|
|
@ -286,24 +288,11 @@ 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)
|
||||
|
||||
|
|
@ -351,7 +340,7 @@ endif()
|
|||
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})
|
||||
|
|
@ -381,8 +370,7 @@ print_config_flag(${GTSAM_BUILD_TESTS} "Build Tests
|
|||
if (DOXYGEN_FOUND)
|
||||
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 ")
|
||||
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 ")
|
||||
|
|
@ -390,9 +378,11 @@ endif()
|
|||
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 ")
|
||||
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)
|
||||
|
|
|
|||
|
|
@ -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
|
||||
----------------------------
|
||||
|
||||
|
|
@ -64,15 +25,17 @@ Optional dependent libraries:
|
|||
may be downloaded from https://www.threadingbuildingblocks.org/
|
||||
|
||||
Tested compilers
|
||||
- GCC 4.2-4.7
|
||||
- Clang 2.9-3.2
|
||||
- OSX GCC 4.2
|
||||
- MSVC 2010, 2012
|
||||
|
||||
|
||||
- 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
|
||||
|
||||
2)
|
||||
GTSAM makes extensive use of debug assertions, and we highly recommend you work
|
||||
43
LICENSE
43
LICENSE
|
|
@ -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
|
||||
26
LICENSE.BSD
26
LICENSE.BSD
|
|
@ -2,28 +2,12 @@ Copyright (c) 2010, Georgia Tech Research Corporation
|
|||
Atlanta, Georgia 30332-0415
|
||||
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,
|
||||
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.
|
||||
|
||||
* 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.
|
||||
|
||||
* 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.
|
||||
|
||||
* 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.
|
||||
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
64
README.md
|
|
@ -1,7 +1,20 @@
|
|||
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:
|
||||
|
||||
|
|
@ -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 Math Kernel Library (MKL)](http://software.intel.com/en-us/intel-mkl)
|
||||
|
||||
Tested compilers
|
||||
Additional Information
|
||||
----------------------
|
||||
|
||||
- GCC 4.2-4.7
|
||||
- OSX Clang 2.9-5.0
|
||||
- OSX GCC 4.2
|
||||
- MSVC 2010, 2012
|
||||
See the [`INSTALL`](https://bitbucket.org/gtborg/gtsam/src/develop/INSTALL) file for more detailed installation instructions.
|
||||
|
||||
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
|
||||
- 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)
|
||||
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
11
USAGE
|
|
@ -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
|
||||
---------------------------------------------------
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -155,7 +155,7 @@ macro(gtsam_add_grouped_scripts group pattern target_prefix pretty_prefix_name l
|
|||
endif()
|
||||
|
||||
# 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})
|
||||
endif()
|
||||
|
||||
|
|
|
|||
|
|
@ -28,8 +28,8 @@ foreach(example_src ${example_srcs} )
|
|||
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)
|
||||
target_link_libraries(${example_bin} gtsam ${Boost_PROGRAM_OPTIONS_LIBRARY})
|
||||
if(NOT MSVC AND NOT XCODE_VERSION)
|
||||
add_custom_target(${example_bin}.run ${EXECUTABLE_OUTPUT_PATH}${example_bin} ${ARGN})
|
||||
endif()
|
||||
|
||||
|
|
|
|||
|
|
@ -95,42 +95,40 @@ message(STATUS "Install prefix: ${CMAKE_INSTALL_PREFIX}")
|
|||
# build shared and static versions of the library
|
||||
if (GTSAM_BUILD_STATIC_LIBRARY)
|
||||
message(STATUS "Building GTSAM - static")
|
||||
add_library(gtsam-static STATIC ${gtsam_srcs})
|
||||
target_link_libraries(gtsam-static ${GTSAM_BOOST_LIBRARIES} ${GTSAM_ADDITIONAL_LIBRARIES})
|
||||
set_target_properties(gtsam-static PROPERTIES
|
||||
add_library(gtsam STATIC ${gtsam_srcs})
|
||||
target_link_libraries(gtsam ${GTSAM_BOOST_LIBRARIES} ${GTSAM_ADDITIONAL_LIBRARIES})
|
||||
set_target_properties(gtsam PROPERTIES
|
||||
OUTPUT_NAME gtsam
|
||||
CLEAN_DIRECT_OUTPUT 1
|
||||
VERSION ${gtsam_version}
|
||||
SOVERSION ${gtsam_soversion})
|
||||
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"
|
||||
COMPILE_DEFINITIONS GTSAM_IMPORT_STATIC)
|
||||
endif()
|
||||
install(TARGETS gtsam-static EXPORT GTSAM-exports ARCHIVE DESTINATION lib)
|
||||
list(APPEND GTSAM_EXPORTED_TARGETS gtsam-static)
|
||||
install(TARGETS gtsam EXPORT GTSAM-exports ARCHIVE DESTINATION lib)
|
||||
list(APPEND GTSAM_EXPORTED_TARGETS gtsam)
|
||||
set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
|
||||
endif ()
|
||||
|
||||
if (GTSAM_BUILD_SHARED_LIBRARY)
|
||||
else()
|
||||
message(STATUS "Building GTSAM - shared")
|
||||
add_library(gtsam-shared SHARED ${gtsam_srcs})
|
||||
target_link_libraries(gtsam-shared ${GTSAM_BOOST_LIBRARIES} ${GTSAM_ADDITIONAL_LIBRARIES})
|
||||
set_target_properties(gtsam-shared PROPERTIES
|
||||
add_library(gtsam SHARED ${gtsam_srcs})
|
||||
target_link_libraries(gtsam ${GTSAM_BOOST_LIBRARIES} ${GTSAM_ADDITIONAL_LIBRARIES})
|
||||
set_target_properties(gtsam PROPERTIES
|
||||
OUTPUT_NAME gtsam
|
||||
CLEAN_DIRECT_OUTPUT 1
|
||||
VERSION ${gtsam_version}
|
||||
SOVERSION ${gtsam_soversion})
|
||||
if(WIN32)
|
||||
set_target_properties(gtsam-shared PROPERTIES
|
||||
set_target_properties(gtsam PROPERTIES
|
||||
PREFIX ""
|
||||
DEFINE_SYMBOL GTSAM_EXPORTS
|
||||
RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/bin")
|
||||
endif()
|
||||
install(TARGETS gtsam-shared EXPORT GTSAM-exports LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin)
|
||||
list(APPEND GTSAM_EXPORTED_TARGETS gtsam-shared)
|
||||
install(TARGETS gtsam EXPORT GTSAM-exports LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin)
|
||||
list(APPEND GTSAM_EXPORTED_TARGETS gtsam)
|
||||
set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
|
||||
endif(GTSAM_BUILD_SHARED_LIBRARY)
|
||||
endif()
|
||||
|
||||
# Set dataset paths
|
||||
set_property(SOURCE "${CMAKE_CURRENT_SOURCE_DIR}/slam/dataset.cpp"
|
||||
|
|
@ -153,10 +151,10 @@ if (GTSAM_INSTALL_MATLAB_TOOLBOX)
|
|||
|
||||
# Generate, build and install toolbox
|
||||
set(mexFlags "${GTSAM_BUILD_MEX_BINARY_FLAGS}")
|
||||
if("${gtsam-default}" STREQUAL "gtsam-static")
|
||||
if(GTSAM_BUILD_STATIC_LIBRARY)
|
||||
list(APPEND mexFlags -DGTSAM_IMPORT_STATIC)
|
||||
endif()
|
||||
|
||||
# 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 ()
|
||||
|
|
|
|||
|
|
@ -13,11 +13,11 @@ set(base_excluded_files
|
|||
|
||||
# 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)
|
||||
|
||||
# Build timing scripts
|
||||
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)
|
||||
|
||||
|
|
|
|||
|
|
@ -135,12 +135,12 @@ void deserializeBinary(const std::string& serialized, T& output, const std::stri
|
|||
}
|
||||
|
||||
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());
|
||||
if (!out_archive_stream.is_open())
|
||||
return false;
|
||||
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();
|
||||
return true;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -9,7 +9,7 @@ set (discrete_excluded_tests "")
|
|||
|
||||
# Add all 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()
|
||||
|
||||
# Build timing scripts
|
||||
|
|
|
|||
|
|
@ -10,11 +10,11 @@ set(geometry_excluded_files
|
|||
|
||||
# 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)
|
||||
|
||||
# Build timing scripts
|
||||
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)
|
||||
|
||||
|
|
|
|||
|
|
@ -73,7 +73,7 @@ Point3 Rot3::operator*(const Point3& p) const {
|
|||
/* ************************************************************************* */
|
||||
Sphere2 Rot3::rotate(const Sphere2& p,
|
||||
boost::optional<Matrix&> HR, boost::optional<Matrix&> Hp) const {
|
||||
Sphere2 q = rotate(p.point3(Hp));
|
||||
Sphere2 q = Sphere2(rotate(p.point3(Hp)));
|
||||
if (Hp)
|
||||
(*Hp) = q.basis().transpose() * matrix() * (*Hp);
|
||||
if (HR)
|
||||
|
|
@ -84,7 +84,7 @@ Sphere2 Rot3::rotate(const Sphere2& p,
|
|||
/* ************************************************************************* */
|
||||
Sphere2 Rot3::unrotate(const Sphere2& p,
|
||||
boost::optional<Matrix&> HR, boost::optional<Matrix&> Hp) const {
|
||||
Sphere2 q = unrotate(p.point3(Hp));
|
||||
Sphere2 q = Sphere2(unrotate(p.point3(Hp)));
|
||||
if (Hp)
|
||||
(*Hp) = q.basis().transpose() * matrix().transpose () * (*Hp);
|
||||
if (HR)
|
||||
|
|
|
|||
|
|
@ -60,7 +60,7 @@ public:
|
|||
}
|
||||
|
||||
/// Construct from point
|
||||
Sphere2(const Point3& p) :
|
||||
explicit Sphere2(const Point3& p) :
|
||||
p_(p / p.norm()) {
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -22,17 +22,17 @@ GTSAM_CONCEPT_MANIFOLD_INST(EssentialMatrix)
|
|||
// Create two cameras and corresponding essential matrix E
|
||||
Rot3 c1Rc2 = Rot3::yaw(M_PI_2);
|
||||
Point3 c1Tc2(0.1, 0, 0);
|
||||
EssentialMatrix trueE(c1Rc2, c1Tc2);
|
||||
EssentialMatrix trueE(c1Rc2, Sphere2(c1Tc2));
|
||||
|
||||
//*************************************************************************
|
||||
TEST (EssentialMatrix, equality) {
|
||||
EssentialMatrix actual(c1Rc2, c1Tc2), expected(c1Rc2, c1Tc2);
|
||||
EssentialMatrix actual(c1Rc2, Sphere2(c1Tc2)), expected(c1Rc2, Sphere2(c1Tc2));
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
}
|
||||
|
||||
//*************************************************************************
|
||||
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));
|
||||
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::roll(M_PI / 6.0);
|
||||
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)));
|
||||
static Point3 P(0.2, 0.7, -2);
|
||||
Matrix actH1, actH2;
|
||||
|
|
@ -79,7 +79,7 @@ TEST (EssentialMatrix, rotate) {
|
|||
// Let's compute the ground truth E in body frame:
|
||||
Rot3 b1Rb2 = bRc * c1Rc2 * cRb;
|
||||
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, trueE.rotate(bRc), 1e-8));
|
||||
|
||||
|
|
@ -111,7 +111,7 @@ TEST (EssentialMatrix, FromPose3_b) {
|
|||
Matrix actualH;
|
||||
Rot3 c1Rc2 = Rot3::ypr(0.1, -0.2, 0.3);
|
||||
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
|
||||
EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8));
|
||||
Matrix expectedH = numericalDerivative11<EssentialMatrix, Pose3>(
|
||||
|
|
@ -121,7 +121,7 @@ TEST (EssentialMatrix, FromPose3_b) {
|
|||
|
||||
//*************************************************************************
|
||||
TEST (EssentialMatrix, streaming) {
|
||||
EssentialMatrix expected(c1Rc2, c1Tc2), actual;
|
||||
EssentialMatrix expected(c1Rc2, Sphere2(c1Tc2)), actual;
|
||||
stringstream ss;
|
||||
ss << expected;
|
||||
ss >> actual;
|
||||
|
|
|
|||
|
|
@ -10,11 +10,11 @@ set(inference_excluded_files
|
|||
|
||||
# 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)
|
||||
|
||||
# Build timing scripts
|
||||
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)
|
||||
|
||||
|
|
|
|||
|
|
@ -10,7 +10,7 @@ set(linear_excluded_files
|
|||
|
||||
# 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)
|
||||
|
||||
if(MSVC)
|
||||
|
|
@ -20,5 +20,5 @@ endif()
|
|||
|
||||
# Build timing scripts
|
||||
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)
|
||||
|
|
|
|||
|
|
@ -114,19 +114,19 @@ TEST(GaussianBayesTree, complicatedMarginal) {
|
|||
// Create the conditionals to go in the BayesTree
|
||||
GaussianBayesTree bt;
|
||||
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()),
|
||||
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())
|
||||
(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()),
|
||||
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())
|
||||
(11, (Matrix(3,1) << 0.8407, 0.2543, 0.8143).finished()),
|
||||
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())
|
||||
// NOTE the non-upper-triangular form
|
||||
// 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())
|
||||
(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
|
||||
(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())
|
||||
(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())))
|
||||
(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())
|
||||
(5, (Matrix(3,1) << 0.8258, 0.5383, 0.9961).finished()),
|
||||
2, (Vector(3) << 0.0782, 0.4427, 0.1067).finished())))))))));
|
||||
|
|
|
|||
|
|
@ -7,10 +7,10 @@ set (navigation_excluded_tests "")
|
|||
|
||||
# Add all 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()
|
||||
|
||||
# Build timing scripts
|
||||
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)
|
||||
|
|
|
|||
|
|
@ -11,7 +11,7 @@
|
|||
|
||||
/**
|
||||
* @file CombinedImuFactor.h
|
||||
* @author Luca Carlone, Stephen Williams, Richard Roberts
|
||||
* @author Luca Carlone, Stephen Williams, Richard Roberts, Vadim Indelman
|
||||
**/
|
||||
|
||||
#pragma once
|
||||
|
|
@ -342,11 +342,8 @@ namespace gtsam {
|
|||
public:
|
||||
|
||||
/** Shorthand for a smart pointer to a factor */
|
||||
#ifndef _MSC_VER
|
||||
typedef typename boost::shared_ptr<CombinedImuFactor> shared_ptr;
|
||||
#else
|
||||
typedef boost::shared_ptr<CombinedImuFactor> shared_ptr;
|
||||
#endif
|
||||
typedef boost::shared_ptr<CombinedImuFactor> shared_ptr;
|
||||
|
||||
/** Default constructor - only use for serialization */
|
||||
CombinedImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6)) {}
|
||||
|
||||
|
|
|
|||
|
|
@ -11,7 +11,7 @@
|
|||
|
||||
/**
|
||||
* @file ImuFactor.h
|
||||
* @author Luca Carlone, Stephen Williams, Richard Roberts
|
||||
* @author Luca Carlone, Stephen Williams, Richard Roberts, Vadim Indelman
|
||||
**/
|
||||
|
||||
#pragma once
|
||||
|
|
@ -304,11 +304,8 @@ namespace gtsam {
|
|||
public:
|
||||
|
||||
/** 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;
|
||||
#endif
|
||||
|
||||
|
||||
/** Default constructor - only use for serialization */
|
||||
ImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()) {}
|
||||
|
|
|
|||
|
|
@ -10,11 +10,11 @@ set(nonlinear_excluded_files
|
|||
|
||||
# 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)
|
||||
|
||||
# Build timing scripts
|
||||
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)
|
||||
|
||||
|
|
|
|||
|
|
@ -15,10 +15,10 @@ set(slam_excluded_files
|
|||
|
||||
# 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)
|
||||
|
||||
# Build timing scripts
|
||||
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)
|
||||
|
|
|
|||
|
|
@ -37,7 +37,7 @@ bool readOK = readBAL(filename, data);
|
|||
Rot3 c1Rc2 = data.cameras[1].pose().rotation();
|
||||
Point3 c1Tc2 = data.cameras[1].pose().translation();
|
||||
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
|
||||
|
||||
Point2 pA(size_t i) {
|
||||
|
|
@ -297,7 +297,7 @@ SfM_data data;
|
|||
bool readOK = readBAL(filename, data);
|
||||
Rot3 aRb = data.cameras[1].pose().rotation();
|
||||
Point3 aTb = data.cameras[1].pose().translation();
|
||||
EssentialMatrix trueE(aRb, aTb);
|
||||
EssentialMatrix trueE(aRb, Sphere2(aTb));
|
||||
|
||||
double baseline = 10; // actual baseline of the camera
|
||||
|
||||
|
|
|
|||
|
|
@ -10,11 +10,11 @@ set(symbolic_excluded_files
|
|||
|
||||
# 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)
|
||||
|
||||
# Build timing scripts
|
||||
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)
|
||||
|
||||
|
|
|
|||
|
|
@ -58,40 +58,38 @@ message(STATUS "Install prefix: ${CMAKE_INSTALL_PREFIX}")
|
|||
# build shared and static versions of the library
|
||||
if (GTSAM_BUILD_STATIC_LIBRARY)
|
||||
message(STATUS "Building GTSAM_UNSTABLE - static")
|
||||
add_library(gtsam_unstable-static STATIC ${gtsam_unstable_srcs})
|
||||
set_target_properties(gtsam_unstable-static PROPERTIES
|
||||
add_library(gtsam_unstable STATIC ${gtsam_unstable_srcs})
|
||||
set_target_properties(gtsam_unstable PROPERTIES
|
||||
OUTPUT_NAME gtsam_unstable
|
||||
CLEAN_DIRECT_OUTPUT 1
|
||||
VERSION ${gtsam_unstable_version}
|
||||
SOVERSION ${gtsam_unstable_soversion})
|
||||
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"
|
||||
COMPILE_DEFINITIONS GTSAM_UNSTABLE_IMPORT_STATIC)
|
||||
endif()
|
||||
target_link_libraries(gtsam_unstable-static gtsam-static ${GTSAM_UNSTABLE_BOOST_LIBRARIES})
|
||||
install(TARGETS gtsam_unstable-static EXPORT GTSAM-exports ARCHIVE DESTINATION lib)
|
||||
list(APPEND GTSAM_EXPORTED_TARGETS gtsam_unstable-static)
|
||||
target_link_libraries(gtsam_unstable gtsam ${GTSAM_UNSTABLE_BOOST_LIBRARIES})
|
||||
install(TARGETS gtsam_unstable EXPORT GTSAM-exports ARCHIVE DESTINATION lib)
|
||||
list(APPEND GTSAM_EXPORTED_TARGETS gtsam_unstable)
|
||||
set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
|
||||
endif()
|
||||
|
||||
if (GTSAM_BUILD_SHARED_LIBRARY)
|
||||
else()
|
||||
message(STATUS "Building GTSAM_UNSTABLE - shared")
|
||||
add_library(gtsam_unstable-shared SHARED ${gtsam_unstable_srcs})
|
||||
set_target_properties(gtsam_unstable-shared PROPERTIES
|
||||
add_library(gtsam_unstable SHARED ${gtsam_unstable_srcs})
|
||||
set_target_properties(gtsam_unstable PROPERTIES
|
||||
OUTPUT_NAME gtsam_unstable
|
||||
CLEAN_DIRECT_OUTPUT 1
|
||||
VERSION ${gtsam_unstable_version}
|
||||
SOVERSION ${gtsam_unstable_soversion})
|
||||
if(WIN32)
|
||||
set_target_properties(gtsam_unstable-shared PROPERTIES
|
||||
set_target_properties(gtsam_unstable PROPERTIES
|
||||
PREFIX ""
|
||||
DEFINE_SYMBOL GTSAM_UNSTABLE_EXPORTS
|
||||
RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/bin")
|
||||
endif()
|
||||
target_link_libraries(gtsam_unstable-shared gtsam-shared ${GTSAM_UNSTABLE_BOOST_LIBRARIES})
|
||||
install(TARGETS gtsam_unstable-shared EXPORT GTSAM-exports LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin)
|
||||
list(APPEND GTSAM_EXPORTED_TARGETS gtsam_unstable-shared)
|
||||
target_link_libraries(gtsam_unstable gtsam ${GTSAM_UNSTABLE_BOOST_LIBRARIES})
|
||||
install(TARGETS gtsam_unstable EXPORT GTSAM-exports LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin)
|
||||
list(APPEND GTSAM_EXPORTED_TARGETS gtsam_unstable)
|
||||
set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
|
||||
endif()
|
||||
|
||||
|
|
@ -102,12 +100,12 @@ if (GTSAM_INSTALL_MATLAB_TOOLBOX)
|
|||
|
||||
# Generate, build and install toolbox
|
||||
set(mexFlags "${GTSAM_BUILD_MEX_BINARY_FLAGS}")
|
||||
if("${gtsam-default}" STREQUAL "gtsam-static")
|
||||
if(GTSAM_BUILD_STATIC_LIBRARY)
|
||||
list(APPEND mexFlags -DGTSAM_IMPORT_STATIC)
|
||||
endif()
|
||||
|
||||
# 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)
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -3,8 +3,8 @@ file(GLOB base_headers "*.h")
|
|||
install(FILES ${base_headers} DESTINATION include/gtsam_unstable/base)
|
||||
|
||||
set (base_full_libs
|
||||
${gtsam-default}
|
||||
${gtsam_unstable-default})
|
||||
gtsam
|
||||
gtsam_unstable)
|
||||
|
||||
# Exclude tests that don't work
|
||||
set (base_excluded_tests "")
|
||||
|
|
|
|||
|
|
@ -3,8 +3,8 @@ file(GLOB discrete_headers "*.h")
|
|||
install(FILES ${discrete_headers} DESTINATION include/gtsam_unstable/discrete)
|
||||
|
||||
set (discrete_full_libs
|
||||
${gtsam-default}
|
||||
${gtsam_unstable-default})
|
||||
gtsam
|
||||
gtsam_unstable)
|
||||
|
||||
# Exclude tests that don't work
|
||||
#set (discrete_excluded_tests
|
||||
|
|
@ -32,11 +32,11 @@ if (GTSAM_BUILD_EXAMPLES)
|
|||
set_target_properties(${example} PROPERTIES EXCLUDE_FROM_ALL ON)
|
||||
endif()
|
||||
|
||||
if(NOT MSVC)
|
||||
if(NOT MSVC AND NOT XCODE_VERSION)
|
||||
add_dependencies(examples ${example})
|
||||
add_custom_target(${example}.run ${EXECUTABLE_OUTPUT_PATH}${example} ${ARGN})
|
||||
endif()
|
||||
|
||||
target_link_libraries(${example} ${gtsam-default} ${gtsam_unstable-default})
|
||||
target_link_libraries(${example} gtsam gtsam_unstable)
|
||||
endforeach(example)
|
||||
endif (GTSAM_BUILD_EXAMPLES)
|
||||
|
|
|
|||
|
|
@ -4,8 +4,8 @@ install(FILES ${dynamics_headers} DESTINATION include/gtsam_unstable/dynamics)
|
|||
|
||||
# Components to link tests in this subfolder against
|
||||
set (dynamics_full_libs
|
||||
${gtsam-default}
|
||||
${gtsam_unstable-default})
|
||||
gtsam
|
||||
gtsam_unstable)
|
||||
|
||||
# Exclude tests that don't work
|
||||
set (dynamics_excluded_tests "")
|
||||
|
|
|
|||
|
|
@ -18,8 +18,8 @@ foreach(example_src ${example_srcs} )
|
|||
set_target_properties(${example_bin} PROPERTIES EXCLUDE_FROM_ALL ON)
|
||||
endif()
|
||||
|
||||
target_link_libraries(${example_bin} ${gtsam-default} ${gtsam_unstable-default})
|
||||
if(NOT MSVC)
|
||||
target_link_libraries(${example_bin} gtsam gtsam_unstable)
|
||||
if(NOT MSVC AND NOT XCODE_VERSION)
|
||||
add_custom_target(${example_bin}.run ${EXECUTABLE_OUTPUT_PATH}${example_bin} ${ARGN})
|
||||
endif()
|
||||
|
||||
|
|
|
|||
|
|
@ -4,8 +4,8 @@ install(FILES ${geometry_headers} DESTINATION include/gtsam_unstable/geometry)
|
|||
|
||||
# Components to link tests in this subfolder against
|
||||
set (geometry_full_libs
|
||||
${gtsam-default}
|
||||
${gtsam_unstable-default})
|
||||
gtsam
|
||||
gtsam_unstable)
|
||||
|
||||
# Exclude tests that don't work
|
||||
set (geometry_excluded_tests "")
|
||||
|
|
|
|||
|
|
@ -352,6 +352,20 @@ virtual class TransformBtwRobotsUnaryFactorEM : gtsam::NonlinearFactor {
|
|||
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>
|
||||
virtual class SmartRangeFactor : gtsam::NoiseModelFactor {
|
||||
SmartRangeFactor(double s);
|
||||
|
|
|
|||
|
|
@ -4,8 +4,8 @@ install(FILES ${nonlinear_headers} DESTINATION include/gtsam_unstable/nonlinear)
|
|||
|
||||
# Components to link tests in this subfolder against
|
||||
set (nonlinear_full_libs
|
||||
${gtsam-default}
|
||||
${gtsam_unstable-default})
|
||||
gtsam
|
||||
gtsam_unstable)
|
||||
|
||||
# Exclude tests that don't work
|
||||
set (nonlinear_excluded_tests #"")
|
||||
|
|
|
|||
|
|
@ -9,8 +9,8 @@ install(FILES ${slam_headers} DESTINATION include/gtsam_unstable/slam)
|
|||
|
||||
# Components to link tests in this subfolder against
|
||||
set (slam_full_libs
|
||||
${gtsam-default}
|
||||
${gtsam_unstable-default})
|
||||
gtsam
|
||||
gtsam_unstable)
|
||||
|
||||
# Exclude tests that don't work
|
||||
set (slam_excluded_tests
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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);}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -1,6 +1,6 @@
|
|||
# Assemble local libraries
|
||||
set (tests_full_libs
|
||||
${gtsam-default}
|
||||
gtsam
|
||||
CppUnitLite)
|
||||
|
||||
# exclude certain files
|
||||
|
|
|
|||
Loading…
Reference in New Issue