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

Merge branch 'develop' of github.com:borglab/gtsam into feature/1dsfm_mfas

release/4.3a0
akrishnan86 2020-07-21 08:00:34 -07:00
parent 0eec6fbeec c8ddd43f97
commit 62ba551830
98 changed files with 3546 additions and 1317 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.travis.python.sh
View File

@ -34,7 +34,7 @@ cmake $CURRDIR -DCMAKE_BUILD_TYPE=Release \
make -j$(nproc) install
cd $CURRDIR/../gtsam_install/cython
cd cython
sudo $PYTHON setup.py install

61
CMakeLists.txt
View File

@ -454,12 +454,11 @@ endif()
if (GTSAM_INSTALL_CYTHON_TOOLBOX)
set(GTSAM_INSTALL_CYTHON_TOOLBOX 1)
# Set up cache options
set(GTSAM_CYTHON_INSTALL_PATH "" CACHE PATH "Cython toolbox destination, blank defaults to CMAKE_INSTALL_PREFIX/cython")
if(NOT GTSAM_CYTHON_INSTALL_PATH)
set(GTSAM_CYTHON_INSTALL_PATH "${CMAKE_INSTALL_PREFIX}/cython")
endif()
# Cython install path appended with Build type (e.g. cython, cythonDebug, etc).
# This does not override custom values set from the command line
set(GTSAM_CYTHON_INSTALL_PATH "${PROJECT_BINARY_DIR}/cython${GTSAM_BUILD_TAG}" CACHE PATH "Cython toolbox destination, blank defaults to PROJECT_BINARY_DIR/cython<GTSAM_BUILD_TAG>")
set(GTSAM_EIGENCY_INSTALL_PATH ${GTSAM_CYTHON_INSTALL_PATH}/gtsam_eigency)
add_subdirectory(cython)
add_subdirectory(cython ${GTSAM_CYTHON_INSTALL_PATH})
else()
set(GTSAM_INSTALL_CYTHON_TOOLBOX 0) # This will go into config.h
endif()
@ -537,54 +536,54 @@ endif()
print_build_options_for_target(gtsam)
message(STATUS " Use System Eigen : ${GTSAM_USE_SYSTEM_EIGEN} (Using version: ${GTSAM_EIGEN_VERSION})")
message(STATUS " Use System Eigen : ${GTSAM_USE_SYSTEM_EIGEN} (Using version: ${GTSAM_EIGEN_VERSION})")
if(GTSAM_USE_TBB)
message(STATUS " Use Intel TBB : Yes")
message(STATUS " Use Intel TBB : Yes")
elseif(TBB_FOUND)
message(STATUS " Use Intel TBB : TBB found but GTSAM_WITH_TBB is disabled")
message(STATUS " Use Intel TBB : TBB found but GTSAM_WITH_TBB is disabled")
else()
message(STATUS " Use Intel TBB : TBB not found")
message(STATUS " Use Intel TBB : TBB not found")
endif()
if(GTSAM_USE_EIGEN_MKL)
message(STATUS " Eigen will use MKL : Yes")
message(STATUS " Eigen will use MKL : Yes")
elseif(MKL_FOUND)
message(STATUS " Eigen will use MKL : MKL found but GTSAM_WITH_EIGEN_MKL is disabled")
message(STATUS " Eigen will use MKL : MKL found but GTSAM_WITH_EIGEN_MKL is disabled")
else()
message(STATUS " Eigen will use MKL : MKL not found")
message(STATUS " Eigen will use MKL : MKL not found")
endif()
if(GTSAM_USE_EIGEN_MKL_OPENMP)
message(STATUS " Eigen will use MKL and OpenMP : Yes")
message(STATUS " Eigen will use MKL and OpenMP : Yes")
elseif(OPENMP_FOUND AND NOT GTSAM_WITH_EIGEN_MKL)
message(STATUS " Eigen will use MKL and OpenMP : OpenMP found but GTSAM_WITH_EIGEN_MKL is disabled")
message(STATUS " Eigen will use MKL and OpenMP : OpenMP found but GTSAM_WITH_EIGEN_MKL is disabled")
elseif(OPENMP_FOUND AND NOT MKL_FOUND)
message(STATUS " Eigen will use MKL and OpenMP : OpenMP found but MKL not found")
message(STATUS " Eigen will use MKL and OpenMP : OpenMP found but MKL not found")
elseif(OPENMP_FOUND)
message(STATUS " Eigen will use MKL and OpenMP : OpenMP found but GTSAM_WITH_EIGEN_MKL_OPENMP is disabled")
message(STATUS " Eigen will use MKL and OpenMP : OpenMP found but GTSAM_WITH_EIGEN_MKL_OPENMP is disabled")
else()
message(STATUS " Eigen will use MKL and OpenMP : OpenMP not found")
message(STATUS " Eigen will use MKL and OpenMP : OpenMP not found")
endif()
message(STATUS " Default allocator : ${GTSAM_DEFAULT_ALLOCATOR}")
message(STATUS " Default allocator : ${GTSAM_DEFAULT_ALLOCATOR}")
if(GTSAM_THROW_CHEIRALITY_EXCEPTION)
message(STATUS " Cheirality exceptions enabled : YES")
message(STATUS " Cheirality exceptions enabled : YES")
else()
message(STATUS " Cheirality exceptions enabled : NO")
message(STATUS " Cheirality exceptions enabled : NO")
endif()
if(NOT MSVC AND NOT XCODE_VERSION)
if(CCACHE_FOUND AND GTSAM_BUILD_WITH_CCACHE)
message(STATUS " Build with ccache : Yes")
message(STATUS " Build with ccache : Yes")
elseif(CCACHE_FOUND)
message(STATUS " Build with ccache : ccache found but GTSAM_BUILD_WITH_CCACHE is disabled")
message(STATUS " Build with ccache : ccache found but GTSAM_BUILD_WITH_CCACHE is disabled")
else()
message(STATUS " Build with ccache : No")
message(STATUS " Build with ccache : No")
endif()
endif()
message(STATUS "Packaging flags ")
message(STATUS " CPack Source Generator : ${CPACK_SOURCE_GENERATOR}")
message(STATUS " CPack Generator : ${CPACK_GENERATOR}")
message(STATUS " CPack Source Generator : ${CPACK_SOURCE_GENERATOR}")
message(STATUS " CPack Generator : ${CPACK_GENERATOR}")
message(STATUS "GTSAM flags ")
print_config_flag(${GTSAM_USE_QUATERNIONS} "Quaternions as default Rot3 ")
@ -595,15 +594,19 @@ print_config_flag(${GTSAM_ALLOW_DEPRECATED_SINCE_V4} "Deprecated in GTSAM 4 al
print_config_flag(${GTSAM_TYPEDEF_POINTS_TO_VECTORS} "Point3 is typedef to Vector3 ")
print_config_flag(${GTSAM_SUPPORT_NESTED_DISSECTION} "Metis-based Nested Dissection ")
print_config_flag(${GTSAM_TANGENT_PREINTEGRATION} "Use tangent-space preintegration")
print_config_flag(${GTSAM_BUILD_WRAP} "Build Wrap ")
print_config_flag(${GTSAM_BUILD_WRAP} "Build Wrap ")
message(STATUS "MATLAB toolbox flags ")
print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install matlab toolbox ")
print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install MATLAB toolbox ")
if (${GTSAM_INSTALL_MATLAB_TOOLBOX})
message(STATUS " MATLAB root : ${MATLAB_ROOT}")
message(STATUS " MEX binary : ${MEX_COMMAND}")
endif()
message(STATUS "Cython toolbox flags ")
print_config_flag(${GTSAM_INSTALL_CYTHON_TOOLBOX} "Install Cython toolbox ")
print_config_flag(${GTSAM_INSTALL_CYTHON_TOOLBOX} "Install Cython toolbox ")
if(GTSAM_INSTALL_CYTHON_TOOLBOX)
message(STATUS " Python version : ${GTSAM_PYTHON_VERSION}")
message(STATUS " Python version : ${GTSAM_PYTHON_VERSION}")
endif()
message(STATUS "===============================================================")

5
cmake/GtsamBuildTypes.cmake
View File

@ -1,3 +1,8 @@
# Set cmake policy to recognize the AppleClang compiler
# independently from the Clang compiler.
if(POLICY CMP0025)
cmake_policy(SET CMP0025 NEW)
endif()
# function: list_append_cache(var [new_values ...])
# Like "list(APPEND ...)" but working for CACHE variables.

101
cmake/GtsamCythonWrap.cmake
View File

@ -41,9 +41,8 @@ execute_process(COMMAND "${PYTHON_EXECUTABLE}" "-c"
function(wrap_and_install_library_cython interface_header extra_imports install_path libs dependencies)
# Paths for generated files
get_filename_component(module_name "${interface_header}" NAME_WE)
set(generated_files_path "${PROJECT_BINARY_DIR}/cython/${module_name}")
set(generated_files_path "${install_path}")
wrap_library_cython("${interface_header}" "${generated_files_path}" "${extra_imports}" "${libs}" "${dependencies}")
install_cython_wrapped_library("${interface_header}" "${generated_files_path}" "${install_path}")
endfunction()
function(set_up_required_cython_packages)
@ -138,6 +137,10 @@ function(cythonize target pyx_file output_lib_we output_dir include_dirs libs in
target_link_libraries(${target} "${libs}")
endif()
add_dependencies(${target} ${target}_pyx2cpp)
if(TARGET ${python_install_target})
add_dependencies(${python_install_target} ${target})
endif()
endfunction()
# Internal function that wraps a library and compiles the wrapper
@ -150,9 +153,12 @@ function(wrap_library_cython interface_header generated_files_path extra_imports
get_filename_component(module_name "${interface_header}" NAME_WE)
# Wrap module to Cython pyx
message(STATUS "Cython wrapper generating ${module_name}.pyx")
message(STATUS "Cython wrapper generating ${generated_files_path}/${module_name}.pyx")
set(generated_pyx "${generated_files_path}/${module_name}.pyx")
file(MAKE_DIRECTORY "${generated_files_path}")
if(NOT EXISTS ${generated_files_path})
file(MAKE_DIRECTORY "${generated_files_path}")
endif()
add_custom_command(
OUTPUT ${generated_pyx}
DEPENDS ${interface_header} wrap
@ -175,46 +181,6 @@ function(wrap_library_cython interface_header generated_files_path extra_imports
COMMAND cmake -E remove_directory ${generated_files_path})
endfunction()
# Internal function that installs a wrap toolbox
function(install_cython_wrapped_library interface_header generated_files_path install_path)
get_filename_component(module_name "${interface_header}" NAME_WE)
# NOTE: only installs .pxd and .pyx and binary files (not .cpp) - the trailing slash on the directory name
# here prevents creating the top-level module name directory in the destination.
# Split up filename to strip trailing '/' in GTSAM_CYTHON_INSTALL_PATH/subdirectory if there is one
get_filename_component(location "${install_path}" PATH)
get_filename_component(name "${install_path}" NAME)
message(STATUS "Installing Cython Toolbox to ${location}${GTSAM_BUILD_TAG}/${name}") #${GTSAM_CYTHON_INSTALL_PATH}"
if(GTSAM_BUILD_TYPE_POSTFIXES)
foreach(build_type ${CMAKE_CONFIGURATION_TYPES})
string(TOUPPER "${build_type}" build_type_upper)
if(${build_type_upper} STREQUAL "RELEASE")
set(build_type_tag "") # Don't create release mode tag on installed directory
else()
set(build_type_tag "${build_type}")
endif()
install(DIRECTORY "${generated_files_path}/" DESTINATION "${location}${build_type_tag}/${name}"
CONFIGURATIONS "${build_type}"
PATTERN "build" EXCLUDE
PATTERN "CMakeFiles" EXCLUDE
PATTERN "Makefile" EXCLUDE
PATTERN "*.cmake" EXCLUDE
PATTERN "*.cpp" EXCLUDE
PATTERN "*.py" EXCLUDE)
endforeach()
else()
install(DIRECTORY "${generated_files_path}/" DESTINATION ${install_path}
PATTERN "build" EXCLUDE
PATTERN "CMakeFiles" EXCLUDE
PATTERN "Makefile" EXCLUDE
PATTERN "*.cmake" EXCLUDE
PATTERN "*.cpp" EXCLUDE
PATTERN "*.py" EXCLUDE)
endif()
endfunction()
# Helper function to install Cython scripts and handle multiple build types where the scripts
# should be installed to all build type toolboxes
#
@ -232,50 +198,7 @@ function(install_cython_scripts source_directory dest_directory patterns)
foreach(pattern ${patterns})
list(APPEND patterns_args PATTERN "${pattern}")
endforeach()
if(GTSAM_BUILD_TYPE_POSTFIXES)
foreach(build_type ${CMAKE_CONFIGURATION_TYPES})
string(TOUPPER "${build_type}" build_type_upper)
if(${build_type_upper} STREQUAL "RELEASE")
set(build_type_tag "") # Don't create release mode tag on installed directory
else()
set(build_type_tag "${build_type}")
endif()
# Split up filename to strip trailing '/' in GTSAM_CYTHON_INSTALL_PATH if there is one
get_filename_component(location "${dest_directory}" PATH)
get_filename_component(name "${dest_directory}" NAME)
install(DIRECTORY "${source_directory}" DESTINATION "${location}/${name}${build_type_tag}" CONFIGURATIONS "${build_type}"
file(COPY "${source_directory}" DESTINATION "${dest_directory}"
FILES_MATCHING ${patterns_args} PATTERN "${exclude_patterns}" EXCLUDE)
endforeach()
else()
install(DIRECTORY "${source_directory}" DESTINATION "${dest_directory}" FILES_MATCHING ${patterns_args} PATTERN "${exclude_patterns}" EXCLUDE)
endif()
endfunction()
# Helper function to install specific files and handle multiple build types where the scripts
# should be installed to all build type toolboxes
#
# Arguments:
# source_files: The source files to be installed.
# dest_directory: The destination directory to install to.
function(install_cython_files source_files dest_directory)
if(GTSAM_BUILD_TYPE_POSTFIXES)
foreach(build_type ${CMAKE_CONFIGURATION_TYPES})
string(TOUPPER "${build_type}" build_type_upper)
if(${build_type_upper} STREQUAL "RELEASE")
set(build_type_tag "") # Don't create release mode tag on installed directory
else()
set(build_type_tag "${build_type}")
endif()
# Split up filename to strip trailing '/' in GTSAM_CYTHON_INSTALL_PATH if there is one
get_filename_component(location "${dest_directory}" PATH)
get_filename_component(name "${dest_directory}" NAME)
install(FILES "${source_files}" DESTINATION "${location}/${name}${build_type_tag}" CONFIGURATIONS "${build_type}")
endforeach()
else()
install(FILES "${source_files}" DESTINATION "${dest_directory}")
endif()
endfunction()

33
cython/CMakeLists.txt
View File

@ -3,16 +3,32 @@ include(GtsamCythonWrap)
# Create the cython toolbox for the gtsam library
if (GTSAM_INSTALL_CYTHON_TOOLBOX)
# Add the new make target command
set(python_install_target python-install)
add_custom_target(${python_install_target}
COMMAND ${PYTHON_EXECUTABLE} ${GTSAM_CYTHON_INSTALL_PATH}/setup.py install
WORKING_DIRECTORY ${GTSAM_CYTHON_INSTALL_PATH})
# build and include the eigency version of eigency
add_subdirectory(gtsam_eigency)
include_directories(${PROJECT_BINARY_DIR}/cython/gtsam_eigency)
include_directories(${GTSAM_EIGENCY_INSTALL_PATH})
# Fix for error "C1128: number of sections exceeded object file format limit"
if(MSVC)
add_compile_options(/bigobj)
endif()
# wrap gtsam
# First set up all the package related files.
# This also ensures the below wrap operations work correctly.
set(CYTHON_INSTALL_REQUIREMENTS_FILE "${PROJECT_SOURCE_DIR}/cython/requirements.txt")
# Install the custom-generated __init__.py
# This makes the cython (sub-)directories into python packages, so gtsam can be found while wrapping gtsam_unstable
configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam/__init__.py ${GTSAM_CYTHON_INSTALL_PATH}/gtsam/__init__.py COPYONLY)
configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam_unstable/__init__.py ${GTSAM_CYTHON_INSTALL_PATH}/gtsam_unstable/__init__.py COPYONLY)
configure_file(${PROJECT_SOURCE_DIR}/cython/setup.py.in ${GTSAM_CYTHON_INSTALL_PATH}/setup.py)
# Wrap gtsam
add_custom_target(gtsam_header DEPENDS "../gtsam.h")
wrap_and_install_library_cython("../gtsam.h" # interface_header
"" # extra imports
@ -20,8 +36,9 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX)
gtsam # library to link with
"wrap;cythonize_eigency;gtsam;gtsam_header" # dependencies which need to be built before wrapping
)
add_dependencies(${python_install_target} gtsam gtsam_header)
# wrap gtsam_unstable
# Wrap gtsam_unstable
if(GTSAM_BUILD_UNSTABLE)
add_custom_target(gtsam_unstable_header DEPENDS "../gtsam_unstable/gtsam_unstable.h")
wrap_and_install_library_cython("../gtsam_unstable/gtsam_unstable.h" # interface_header
@ -30,17 +47,9 @@ if (GTSAM_INSTALL_CYTHON_TOOLBOX)
gtsam_unstable # library to link with
"gtsam_unstable;gtsam_unstable_header;cythonize_gtsam" # dependencies to be built before wrapping
)
add_dependencies(${python_install_target} gtsam_unstable gtsam_unstable_header)
endif()
file(READ "${PROJECT_SOURCE_DIR}/cython/requirements.txt" CYTHON_INSTALL_REQUIREMENTS)
file(READ "${PROJECT_SOURCE_DIR}/README.md" README_CONTENTS)
# Install the custom-generated __init__.py
# This is to make the build/cython/gtsam folder a python package, so gtsam can be found while wrapping gtsam_unstable
configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam/__init__.py ${PROJECT_BINARY_DIR}/cython/gtsam/__init__.py COPYONLY)
configure_file(${PROJECT_SOURCE_DIR}/cython/gtsam_unstable/__init__.py ${PROJECT_BINARY_DIR}/cython/gtsam_unstable/__init__.py COPYONLY)
configure_file(${PROJECT_SOURCE_DIR}/cython/setup.py.in ${PROJECT_BINARY_DIR}/cython/setup.py)
install_cython_files("${PROJECT_BINARY_DIR}/cython/setup.py" "${GTSAM_CYTHON_INSTALL_PATH}")
# install scripts and tests
install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam" "${GTSAM_CYTHON_INSTALL_PATH}" "*.py")
install_cython_scripts("${PROJECT_SOURCE_DIR}/cython/gtsam_unstable" "${GTSAM_CYTHON_INSTALL_PATH}" "*.py")

134
cython/README.md
View File

@ -1,43 +1,51 @@
# Python Wrapper
This is the Cython/Python wrapper around the GTSAM C++ library.
This is the Python wrapper around the GTSAM C++ library. We use Cython to generate the bindings to the underlying C++ code.
## Requirements
- If you want to build the GTSAM python library for a specific python version (eg 3.6),
use the `-DGTSAM_PYTHON_VERSION=3.6` option when running `cmake` otherwise the default interpreter will be used.
- If the interpreter is inside an environment (such as an anaconda environment or virtualenv environment),
then the environment should be active while building GTSAM.
- This wrapper needs `Cython(>=0.25.2)`, `backports_abc(>=0.5)`, and `numpy(>=1.11.0)`. These can be installed as follows:
```bash
pip install -r <gtsam_folder>/cython/requirements.txt
```
- For compatibility with GTSAM's Eigen version, it contains its own cloned version of [Eigency](https://github.com/wouterboomsma/eigency.git),
named `gtsam_eigency`, to interface between C++'s Eigen and Python's numpy.
## Install
- if you want to build the gtsam python library for a specific python version (eg 2.7), use the `-DGTSAM_PYTHON_VERSION=2.7` option when running `cmake` otherwise the default interpreter will be used.
- If the interpreter is inside an environment (such as an anaconda environment or virtualenv environment) then the environment should be active while building gtsam.
- This wrapper needs Cython(>=0.25.2), backports_abc>=0.5, and numpy. These can be installed as follows:
- Run cmake with the `GTSAM_INSTALL_CYTHON_TOOLBOX` cmake flag enabled to configure building the wrapper. The wrapped module will be built and copied to the directory defined by `GTSAM_CYTHON_INSTALL_PATH`, which is by default `<PROJECT_BINARY_DIR>/cython` in Release mode and `<PROJECT_BINARY_DIR>/cython<CMAKE_BUILD_TYPE>` for other modes.
```bash
pip install -r <gtsam_folder>/cython/requirements.txt
```
- Build GTSAM and the wrapper with `make`.
- For compatibility with gtsam's Eigen version, it contains its own cloned version of [Eigency](https://github.com/wouterboomsma/eigency.git),
named **gtsam_eigency**, to interface between C++'s Eigen and Python's numpy.
- To install, simply run `make python-install`.
- The same command can be used to install into a virtual environment if it is active.
- **NOTE**: if you don't want GTSAM to install to a system directory such as `/usr/local`, pass `-DCMAKE_INSTALL_PREFIX="./install"` to cmake to install GTSAM to a subdirectory of the build directory.
- Build and install gtsam using cmake with `GTSAM_INSTALL_CYTHON_TOOLBOX` enabled.
The wrapped module will be installed to `GTSAM_CYTHON_INSTALL_PATH`, which is
by default: `<your CMAKE_INSTALL_PREFIX>/cython`
- To use the library without installing system-wide: modify your `PYTHONPATH` to include the `GTSAM_CYTHON_INSTALL_PATH`:
```bash
export PYTHONPATH=$PYTHONPATH:<GTSAM_CYTHON_INSTALL_PATH>
```
- To install system-wide: run `make install` then navigate to `GTSAM_CYTHON_INSTALL_PATH` and run `python setup.py install`
- (the same command can be used to install into a virtual environment if it is active)
- note: if you don't want gtsam to install to a system directory such as `/usr/local`, pass `-DCMAKE_INSTALL_PREFIX="./install"` to cmake to install gtsam to a subdirectory of the build directory.
- if you run `setup.py` from the build directory rather than the installation directory, the script will warn you with the message: `setup.py is being run from an unexpected location`.
Before `make install` is run, not all the components of the package have been copied across, so running `setup.py` from the build directory would result in an incomplete package.
- You can also directly run `make python-install` without running `make`, and it will compile all the dependencies accordingly.
## Unit Tests
The Cython toolbox also has a small set of unit tests located in the
test directory. To run them:
```bash
cd <your GTSAM_CYTHON_INSTALL_PATH>
python -m unittest discover
```
```bash
cd <GTSAM_CYTHON_INSTALL_PATH>
python -m unittest discover
```
## Utils
TODO
## Examples
TODO
## Writing Your Own Scripts
@ -46,79 +54,63 @@ See the tests for examples.
### Some Important Notes:
- Vector/Matrix:
+ GTSAM expects double-precision floating point vectors and matrices.
Hence, you should pass numpy matrices with dtype=float, or 'float64'.
+ Also, GTSAM expects *column-major* matrices, unlike the default storage
scheme in numpy. Hence, you should pass column-major matrices to gtsam using
- GTSAM expects double-precision floating point vectors and matrices.
Hence, you should pass numpy matrices with `dtype=float`, or `float64`.
- Also, GTSAM expects _column-major_ matrices, unlike the default storage
scheme in numpy. Hence, you should pass column-major matrices to GTSAM using
the flag order='F'. And you always get column-major matrices back.
For more details, see: https://github.com/wouterboomsma/eigency#storage-layout---why-arrays-are-sometimes-transposed
+ Passing row-major matrices of different dtype, e.g. 'int', will also work
For more details, see [this link](https://github.com/wouterboomsma/eigency#storage-layout---why-arrays-are-sometimes-transposed).
- Passing row-major matrices of different dtype, e.g. `int`, will also work
as the wrapper converts them to column-major and dtype float for you,
using numpy.array.astype(float, order='F', copy=False).
However, this will result a copy if your matrix is not in the expected type
and storage order.
- Inner namespace: Classes in inner namespace will be prefixed by <innerNamespace>_ in Python.
Examples: noiseModel_Gaussian, noiseModel_mEstimator_Tukey
- Inner namespace: Classes in inner namespace will be prefixed by <innerNamespace>\_ in Python.
Examples: `noiseModel_Gaussian`, `noiseModel_mEstimator_Tukey`
- Casting from a base class to a derive class must be done explicitly.
Examples:
```Python
noiseBase = factor.noiseModel()
noiseGaussian = dynamic_cast_noiseModel_Gaussian_noiseModel_Base(noiseBase)
```
## Wrapping Your Own Project That Uses GTSAM
Examples:
- Set PYTHONPATH to include ${GTSAM_CYTHON_INSTALL_PATH}
+ so that it can find gtsam Cython header: gtsam/gtsam.pxd
```python
noiseBase = factor.noiseModel()
noiseGaussian = dynamic_cast_noiseModel_Gaussian_noiseModel_Base(noiseBase)
```
- In your CMakeList.txt
```cmake
find_package(GTSAM REQUIRED) # Make sure gtsam's install folder is in your PATH
set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH}" "${GTSAM_DIR}/../GTSAMCMakeTools")
## Wrapping Custom GTSAM-based Project
# Wrap
include(GtsamCythonWrap)
include_directories(${GTSAM_EIGENCY_INSTALL_PATH})
wrap_and_install_library_cython("your_project_interface.h"
"from gtsam.gtsam cimport *" # extra import of gtsam/gtsam.pxd Cython header
"your_install_path"
"libraries_to_link_with_the_cython_module"
"dependencies_which_need_to_be_built_before_the_wrapper"
)
#Optional: install_cython_scripts and install_cython_files. See GtsamCythonWrap.cmake.
```
Please refer to the template project and the corresponding tutorial available [here](https://github.com/borglab/GTSAM-project-python).
## KNOWN ISSUES
- Doesn't work with python3 installed from homebrew
- size-related issue: can only wrap up to a certain number of classes: up to mEstimator!
- Guess: 64 vs 32b? disutils Compiler flags?
- Bug with Cython 0.24: instantiated factor classes return FastVector<size_t> for keys(), which can't be casted to FastVector<Key>
- Upgrading to 0.25 solves the problem
- Need default constructor and default copy constructor for almost every classes... :(
- support these constructors by default and declare "delete" for special classes?
- Doesn't work with python3 installed from homebrew
- size-related issue: can only wrap up to a certain number of classes: up to mEstimator!
- Guess: 64 vs 32b? disutils Compiler flags?
- Bug with Cython 0.24: instantiated factor classes return FastVector<size_t> for keys(), which can't be casted to FastVector<Key>
- Upgrading to 0.25 solves the problem
- Need default constructor and default copy constructor for almost every classes... :(
- support these constructors by default and declare "delete" for special classes?
### TODO
- [ ] allow duplication of parent' functions in child classes. Not allowed for now due to conflicts in Cython.
- [ ] a common header for boost shared_ptr? (Or wait until everything is switched to std::shared_ptr in gtsam?)
- [ ] a common header for boost shared_ptr? (Or wait until everything is switched to std::shared_ptr in GTSAM?)
- [ ] inner namespaces ==> inner packages?
- [ ] Wrap fixed-size Matrices/Vectors?
### Completed/Cancelled:
- [x] Fix Python tests: don't use " import <package> * ": Bad style!!! (18-03-17 19:50)
- [x] Fix Python tests: don't use " import <package> \* ": Bad style!!! (18-03-17 19:50)
- [x] Unit tests for cython wrappers @done (18-03-17 18:45) -- simply compare generated files
- [x] Wrap unstable @done (18-03-17 15:30)
- [x] Unify cython/gtsam.h and the original gtsam.h @done (18-03-17 15:30)
- [x] Unify cython/GTSAM.h and the original GTSAM.h @done (18-03-17 15:30)
- [x] 18-03-17: manage to unify the two versions by removing std container stubs from the matlab version,and keeping KeyList/KeyVector/KeySet as in the matlab version. Probably Cython 0.25 fixes the casting problem.
- [x] 06-03-17: manage to remove the requirements for default and copy constructors
- [ ] 25-11-16: Try to unify but failed. Main reasons are: Key/size_t, std containers, KeyVector/KeyList/KeySet. Matlab doesn't need to know about Key, but I can't make Cython to ignore Key as it couldn't cast KeyVector, i.e. FastVector<Key>, to FastVector<size_t>.
- [ ] Marginal and JointMarginal: revert changes @failed (17-03-17 11:00) -- Cython does need a default constructor! It produces cpp code like this: ```gtsam::JointMarginal __pyx_t_1;``` Users don't have to wrap this constructor, however.
- [ ] Marginal and JointMarginal: revert changes @failed (17-03-17 11:00) -- Cython does need a default constructor! It produces cpp code like this: `GTSAM::JointMarginal __pyx_t_1;` Users don't have to wrap this constructor, however.
- [x] Convert input numpy Matrix/Vector to float dtype and storage order 'F' automatically, cannot crash! @done (15-03-17 13:00)
- [x] Remove requirements.txt - Frank: don't bother with only 2 packages and a special case for eigency! @done (08-03-17 10:30)
- [x] CMake install script @done (25-11-16 02:30)
@ -132,7 +124,7 @@ wrap_and_install_library_cython("your_project_interface.h"
- [x] Casting from parent and grandparents @done (16-11-16 17:00)
- [x] Allow overloading constructors. The current solution is annoying!!! @done (16-11-16 17:00)
- [x] Support "print obj" @done (16-11-16 17:00)
- [x] methods for FastVector: at, [], ... @done (16-11-16 17:00)
- [x] methods for FastVector: at, [], ... @done (16-11-16 17:00)
- [x] Cython: Key and size_t: traits<size_t> doesn't exist @done (16-09-12 18:34)
- [x] KeyVector, KeyList, KeySet... @done (16-09-13 17:19)
- [x] [Nice to have] parse typedef @done (16-09-13 17:19)

56
cython/gtsam/tests/test_FrobeniusFactor.py Normal file
View File

@ -0,0 +1,56 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
FrobeniusFactor unit tests.
Author: Frank Dellaert
"""
# pylint: disable=no-name-in-module, import-error, invalid-name
import unittest
import numpy as np
from gtsam import (Rot3, SO3, SO4, FrobeniusBetweenFactorSO4, FrobeniusFactorSO4,
FrobeniusWormholeFactor, SOn)
id = SO4()
v1 = np.array([0, 0, 0, 0.1, 0, 0])
Q1 = SO4.Expmap(v1)
v2 = np.array([0, 0, 0, 0.01, 0.02, 0.03])
Q2 = SO4.Expmap(v2)
class TestFrobeniusFactorSO4(unittest.TestCase):
"""Test FrobeniusFactor factors."""
def test_frobenius_factor(self):
"""Test creation of a factor that calculates the Frobenius norm."""
factor = FrobeniusFactorSO4(1, 2)
actual = factor.evaluateError(Q1, Q2)
expected = (Q2.matrix()-Q1.matrix()).transpose().reshape((16,))
np.testing.assert_array_equal(actual, expected)
def test_frobenius_between_factor(self):
"""Test creation of a Frobenius BetweenFactor."""
factor = FrobeniusBetweenFactorSO4(1, 2, Q1.between(Q2))
actual = factor.evaluateError(Q1, Q2)
expected = np.zeros((16,))
np.testing.assert_array_almost_equal(actual, expected)
def test_frobenius_wormhole_factor(self):
"""Test creation of a factor that calculates Shonan error."""
R1 = SO3.Expmap(v1[3:])
R2 = SO3.Expmap(v2[3:])
factor = FrobeniusWormholeFactor(1, 2, Rot3(R1.between(R2).matrix()), p=4)
I4 = SOn(4)
Q1 = I4.retract(v1)
Q2 = I4.retract(v2)
actual = factor.evaluateError(Q1, Q2)
expected = np.zeros((12,))
np.testing.assert_array_almost_equal(actual, expected, decimal=4)
if __name__ == "__main__":
unittest.main()

13
cython/gtsam/tests/test_NonlinearOptimizer.py
View File

@ -17,7 +17,8 @@ import unittest
import gtsam
from gtsam import (DoglegOptimizer, DoglegParams, GaussNewtonOptimizer,
GaussNewtonParams, LevenbergMarquardtOptimizer,
LevenbergMarquardtParams, NonlinearFactorGraph, Ordering,
LevenbergMarquardtParams, PCGSolverParameters,
DummyPreconditionerParameters, NonlinearFactorGraph, Ordering,
Point2, PriorFactorPoint2, Values)
from gtsam.utils.test_case import GtsamTestCase
@ -61,6 +62,16 @@ class TestScenario(GtsamTestCase):
fg, initial_values, lmParams).optimize()
self.assertAlmostEqual(0, fg.error(actual2))
# Levenberg-Marquardt
lmParams = LevenbergMarquardtParams.CeresDefaults()
lmParams.setLinearSolverType("ITERATIVE")
cgParams = PCGSolverParameters()
cgParams.setPreconditionerParams(DummyPreconditionerParameters())
lmParams.setIterativeParams(cgParams)
actual2 = LevenbergMarquardtOptimizer(
fg, initial_values, lmParams).optimize()
self.assertAlmostEqual(0, fg.error(actual2))
# Dogleg
dlParams = DoglegParams()
dlParams.setOrdering(ordering)

35
cython/gtsam/tests/test_logging_optimizer.py
View File

@ -4,6 +4,12 @@ Author: Jing Wu and Frank Dellaert
"""
# pylint: disable=invalid-name
import sys
if sys.version_info.major >= 3:
from io import StringIO
else:
from cStringIO import StringIO
import unittest
from datetime import datetime
@ -37,11 +43,24 @@ class TestOptimizeComet(GtsamTestCase):
self.optimizer = gtsam.GaussNewtonOptimizer(
graph, initial, self.params)
self.lmparams = gtsam.LevenbergMarquardtParams()
self.lmoptimizer = gtsam.LevenbergMarquardtOptimizer(
graph, initial, self.lmparams
)
# setup output capture
self.capturedOutput = StringIO()
sys.stdout = self.capturedOutput
def tearDown(self):
"""Reset print capture."""
sys.stdout = sys.__stdout__
def test_simple_printing(self):
"""Test with a simple hook."""
# Provide a hook that just prints
def hook(_, error: float):
def hook(_, error):
print(error)
# Only thing we require from optimizer is an iterate method
@ -51,6 +70,16 @@ class TestOptimizeComet(GtsamTestCase):
actual = self.optimizer.values()
self.gtsamAssertEquals(actual.atRot3(KEY), self.expected, tol=1e-6)
def test_lm_simple_printing(self):
"""Make sure we are properly terminating LM"""
def hook(_, error):
print(error)
gtsam_optimize(self.lmoptimizer, self.lmparams, hook)
actual = self.lmoptimizer.values()
self.gtsamAssertEquals(actual.atRot3(KEY), self.expected, tol=1e-6)
@unittest.skip("Not a test we want run every time, as needs comet.ml account")
def test_comet(self):
"""Test with a comet hook."""
@ -65,7 +94,7 @@ class TestOptimizeComet(GtsamTestCase):
+ str(time.hour)+":"+str(time.minute)+":"+str(time.second))
# I want to do some comet thing here
def hook(optimizer, error: float):
def hook(optimizer, error):
comet.log_metric("Karcher error",
error, optimizer.iterations())
@ -76,4 +105,4 @@ class TestOptimizeComet(GtsamTestCase):
self.gtsamAssertEquals(actual.atRot3(KEY), self.expected)
if __name__ == "__main__":
unittest.main()
unittest.main()

14
cython/gtsam/utils/logging_optimizer.py
View File

@ -4,15 +4,11 @@ Author: Jing Wu and Frank Dellaert
"""
# pylint: disable=invalid-name
from typing import TypeVar
from gtsam import NonlinearOptimizer, NonlinearOptimizerParams
import gtsam
T = TypeVar('T')
def optimize(optimizer: T, check_convergence, hook):
def optimize(optimizer, check_convergence, hook):
""" Given an optimizer and a convergence check, iterate until convergence.
After each iteration, hook(optimizer, error) is called.
After the function, use values and errors to get the result.
@ -36,8 +32,8 @@ def optimize(optimizer: T, check_convergence, hook):
current_error = new_error
def gtsam_optimize(optimizer: NonlinearOptimizer,
params: NonlinearOptimizerParams,
def gtsam_optimize(optimizer,
params,
hook):
""" Given an optimizer and params, iterate until convergence.
After each iteration, hook(optimizer) is called.
@ -50,5 +46,7 @@ def gtsam_optimize(optimizer: NonlinearOptimizer,
def check_convergence(optimizer, current_error, new_error):
return (optimizer.iterations() >= params.getMaxIterations()) or (
gtsam.checkConvergence(params.getRelativeErrorTol(), params.getAbsoluteErrorTol(), params.getErrorTol(),
current_error, new_error))
current_error, new_error)) or (
isinstance(optimizer, gtsam.LevenbergMarquardtOptimizer) and optimizer.lambda_() > params.getlambdaUpperBound())
optimize(optimizer, check_convergence, hook)
return optimizer.values()

21
cython/gtsam_eigency/CMakeLists.txt
View File

@ -4,11 +4,11 @@ include(GtsamCythonWrap)
# so that the cython-generated header "conversions_api.h" can be found when cythonizing eigency's core
# and eigency's cython pxd headers can be found when cythonizing gtsam
file(COPY "." DESTINATION ".")
set(OUTPUT_DIR "${PROJECT_BINARY_DIR}/cython/gtsam_eigency")
set(OUTPUT_DIR "${GTSAM_CYTHON_INSTALL_PATH}/gtsam_eigency")
set(EIGENCY_INCLUDE_DIR ${OUTPUT_DIR})
# This is to make the build/cython/gtsam_eigency folder a python package
configure_file(__init__.py.in ${PROJECT_BINARY_DIR}/cython/gtsam_eigency/__init__.py)
configure_file(__init__.py.in ${OUTPUT_DIR}/__init__.py)
# include eigency headers
include_directories(${EIGENCY_INCLUDE_DIR})
@ -16,8 +16,8 @@ include_directories(${EIGENCY_INCLUDE_DIR})
# Cythonize and build eigency
message(STATUS "Cythonize and build eigency")
# Important trick: use "../gtsam_eigency/conversions.pyx" to let cython know that the conversions module is
# a part of the gtsam_eigency package and generate the function call import_gtsam_igency__conversions()
# in conversions_api.h correctly!!!
# a part of the gtsam_eigency package and generate the function call import_gtsam_eigency__conversions()
# in conversions_api.h correctly!
cythonize(cythonize_eigency_conversions "../gtsam_eigency/conversions.pyx" "conversions"
"${OUTPUT_DIR}" "${EIGENCY_INCLUDE_DIR}" "" "" "")
cythonize(cythonize_eigency_core "../gtsam_eigency/core.pyx" "core"
@ -37,13 +37,6 @@ add_dependencies(cythonize_eigency_core cythonize_eigency_conversions)
add_custom_target(cythonize_eigency)
add_dependencies(cythonize_eigency cythonize_eigency_conversions cythonize_eigency_core)
# install
install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
DESTINATION "${GTSAM_CYTHON_INSTALL_PATH}${GTSAM_BUILD_TAG}"
PATTERN "CMakeLists.txt" EXCLUDE
PATTERN "__init__.py.in" EXCLUDE)
install(TARGETS cythonize_eigency_core cythonize_eigency_conversions
DESTINATION "${GTSAM_CYTHON_INSTALL_PATH}${GTSAM_BUILD_TAG}/gtsam_eigency")
install(FILES ${OUTPUT_DIR}/conversions_api.h DESTINATION ${GTSAM_CYTHON_INSTALL_PATH}${GTSAM_BUILD_TAG}/gtsam_eigency)
configure_file(__init__.py.in ${OUTPUT_DIR}/__init__.py)
install(FILES ${OUTPUT_DIR}/__init__.py DESTINATION ${GTSAM_CYTHON_INSTALL_PATH}${GTSAM_BUILD_TAG}/gtsam_eigency)
if(TARGET ${python_install_target})
add_dependencies(${python_install_target} cythonize_eigency)
endif()

27
cython/setup.py.in
View File

@ -7,6 +7,22 @@ except ImportError:
packages = find_packages()
package_data = {
package:
[f for f in os.listdir(package.replace('.', os.path.sep)) if os.path.splitext(f)[1] in ('.so', '.pyd')]
for package in packages
}
cython_install_requirements = open("${CYTHON_INSTALL_REQUIREMENTS_FILE}").readlines()
install_requires = [line.strip() \
for line in cython_install_requirements \
if len(line.strip()) > 0 and not line.strip().startswith('#')
]
# Cleaner to read in the contents rather than copy them over.
readme_contents = open("${PROJECT_SOURCE_DIR}/README.md").read()
setup(
name='gtsam',
description='Georgia Tech Smoothing And Mapping library',
@ -16,7 +32,7 @@ setup(
author_email='frank.dellaert@gtsam.org',
license='Simplified BSD license',
keywords='slam sam robotics localization mapping optimization',
long_description='''${README_CONTENTS}''',
long_description=readme_contents,
long_description_content_type='text/markdown',
python_requires='>=2.7',
# https://pypi.org/pypi?%3Aaction=list_classifiers
@ -34,11 +50,6 @@ setup(
],
packages=packages,
package_data={package:
[f for f in os.listdir(package.replace('.', os.path.sep)) if os.path.splitext(f)[1] in ('.so', '.pyd')]
for package in packages
},
install_requires=[line.strip() for line in '''
${CYTHON_INSTALL_REQUIREMENTS}
'''.splitlines() if len(line.strip()) > 0 and not line.strip().startswith('#')]
package_data=package_data,
install_requires=install_requires
)

BIN
doc/robust.pdf Normal file
View File

Binary file not shown.

21
docker/README.md Normal file
View File

@ -0,0 +1,21 @@
# Instructions
Build all docker images, in order:
```bash
(cd ubuntu-boost-tbb && ./build.sh)
(cd ubuntu-gtsam && ./build.sh)
(cd ubuntu-gtsam-python && ./build.sh)
(cd ubuntu-gtsam-python-vnc && ./build.sh)
```
Then launch with:
docker run -p 5900:5900 dellaert/ubuntu-gtsam-python-vnc:bionic
Then open a remote VNC X client, for example:
sudo apt-get install tigervnc-viewer
xtigervncviewer :5900

18
docker/ubuntu-boost-tbb-eigen3/Dockerfile
View File

@ -1,18 +0,0 @@
# Get the base Ubuntu image from Docker Hub
FROM ubuntu:bionic
# Update apps on the base image
RUN apt-get -y update && apt-get install -y
# Install C++
RUN apt-get -y install build-essential
# Install boost and cmake
RUN apt-get -y install libboost-all-dev cmake
# Install TBB
RUN apt-get -y install libtbb-dev
# Install latest Eigen
RUN apt-get install -y libeigen3-dev

19
docker/ubuntu-boost-tbb/Dockerfile Normal file
View File

@ -0,0 +1,19 @@
# Basic Ubuntu 18.04 image with Boost and TBB installed. To be used for building further downstream packages.
# Get the base Ubuntu image from Docker Hub
FROM ubuntu:bionic
# Disable GUI prompts
ENV DEBIAN_FRONTEND noninteractive
# Update apps on the base image
RUN apt-get -y update && apt-get -y install
# Install C++
RUN apt-get -y install build-essential apt-utils
# Install boost and cmake
RUN apt-get -y install libboost-all-dev cmake
# Install TBB
RUN apt-get -y install libtbb-dev

3
docker/ubuntu-boost-tbb/build.sh Executable file
View File

@ -0,0 +1,3 @@
# Build command for Docker image
# TODO(dellaert): use docker compose and/or cmake
docker build --no-cache -t dellaert/ubuntu-boost-tbb:bionic .

20
docker/ubuntu-gtsam-python-vnc/Dockerfile Normal file
View File

@ -0,0 +1,20 @@
# This GTSAM image connects to the host X-server via VNC to provide a Graphical User Interface for interaction.
# Get the base Ubuntu/GTSAM image from Docker Hub
FROM dellaert/ubuntu-gtsam-python:bionic
# Things needed to get a python GUI
ENV DEBIAN_FRONTEND noninteractive
RUN apt install -y python-tk
RUN python3 -m pip install matplotlib
# Install a VNC X-server, Frame buffer, and windows manager
RUN apt install -y x11vnc xvfb fluxbox
# Finally, install wmctrl needed for bootstrap script
RUN apt install -y wmctrl
# Copy bootstrap script and make sure it runs
COPY bootstrap.sh /
CMD '/bootstrap.sh'

111
docker/ubuntu-gtsam-python-vnc/bootstrap.sh Executable file
View File

@ -0,0 +1,111 @@
#!/bin/bash
# Based on: http://www.richud.com/wiki/Ubuntu_Fluxbox_GUI_with_x11vnc_and_Xvfb
main() {
log_i "Starting xvfb virtual display..."
launch_xvfb
log_i "Starting window manager..."
launch_window_manager
log_i "Starting VNC server..."
run_vnc_server
}
launch_xvfb() {
local xvfbLockFilePath="/tmp/.X1-lock"
if [ -f "${xvfbLockFilePath}" ]
then
log_i "Removing xvfb lock file '${xvfbLockFilePath}'..."
if ! rm -v "${xvfbLockFilePath}"
then
log_e "Failed to remove xvfb lock file"
exit 1
fi
fi
# Set defaults if the user did not specify envs.
export DISPLAY=${XVFB_DISPLAY:-:1}
local screen=${XVFB_SCREEN:-0}
local resolution=${XVFB_RESOLUTION:-1280x960x24}
local timeout=${XVFB_TIMEOUT:-5}
# Start and wait for either Xvfb to be fully up or we hit the timeout.
Xvfb ${DISPLAY} -screen ${screen} ${resolution} &
local loopCount=0
until xdpyinfo -display ${DISPLAY} > /dev/null 2>&1
do
loopCount=$((loopCount+1))
sleep 1
if [ ${loopCount} -gt ${timeout} ]
then
log_e "xvfb failed to start"
exit 1
fi
done
}
launch_window_manager() {
local timeout=${XVFB_TIMEOUT:-5}
# Start and wait for either fluxbox to be fully up or we hit the timeout.
fluxbox &
local loopCount=0
until wmctrl -m > /dev/null 2>&1
do
loopCount=$((loopCount+1))
sleep 1
if [ ${loopCount} -gt ${timeout} ]
then
log_e "fluxbox failed to start"
exit 1
fi
done
}
run_vnc_server() {
local passwordArgument='-nopw'
if [ -n "${VNC_SERVER_PASSWORD}" ]
then
local passwordFilePath="${HOME}/.x11vnc.pass"
if ! x11vnc -storepasswd "${VNC_SERVER_PASSWORD}" "${passwordFilePath}"
then
log_e "Failed to store x11vnc password"
exit 1
fi
passwordArgument=-"-rfbauth ${passwordFilePath}"
log_i "The VNC server will ask for a password"
else
log_w "The VNC server will NOT ask for a password"
fi
x11vnc -ncache 10 -ncache_cr -display ${DISPLAY} -forever ${passwordArgument} &
wait $!
}
log_i() {
log "[INFO] ${@}"
}
log_w() {
log "[WARN] ${@}"
}
log_e() {
log "[ERROR] ${@}"
}
log() {
echo "[$(date '+%Y-%m-%d %H:%M:%S')] ${@}"
}
control_c() {
echo ""
exit
}
trap control_c SIGINT SIGTERM SIGHUP
main
exit

4
docker/ubuntu-gtsam-python-vnc/build.sh Executable file
View File

@ -0,0 +1,4 @@
# Build command for Docker image
# TODO(dellaert): use docker compose and/or cmake
# Needs to be run in docker/ubuntu-gtsam-python-vnc directory
docker build -t dellaert/ubuntu-gtsam-python-vnc:bionic .

5
docker/ubuntu-gtsam-python-vnc/vnc.sh Executable file
View File

@ -0,0 +1,5 @@
# After running this script, connect VNC client to 0.0.0.0:5900
docker run -it \
--workdir="/usr/src/gtsam" \
-p 5900:5900 \
dellaert/ubuntu-gtsam-python-vnc:bionic

31
docker/ubuntu-gtsam-python/Dockerfile Normal file
View File

@ -0,0 +1,31 @@
# GTSAM Ubuntu image with Python wrapper support.
# Get the base Ubuntu/GTSAM image from Docker Hub
FROM dellaert/ubuntu-gtsam:bionic
# Install pip
RUN apt-get install -y python3-pip python3-dev
# Install python wrapper requirements
RUN python3 -m pip install -U -r /usr/src/gtsam/cython/requirements.txt
# Run cmake again, now with cython toolbox on
WORKDIR /usr/src/gtsam/build
RUN cmake \
-DCMAKE_BUILD_TYPE=Release \
-DGTSAM_WITH_EIGEN_MKL=OFF \
-DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \
-DGTSAM_BUILD_TIMING_ALWAYS=OFF \
-DGTSAM_BUILD_TESTS=OFF \
-DGTSAM_INSTALL_CYTHON_TOOLBOX=ON \
-DGTSAM_PYTHON_VERSION=3\
..
# Build again, as ubuntu-gtsam image cleaned
RUN make -j4 install && make clean
# Needed to run python wrapper:
RUN echo 'export PYTHONPATH=/usr/local/cython/:$PYTHONPATH' >> /root/.bashrc
# Run bash
CMD ["bash"]

3
docker/ubuntu-gtsam-python/build.sh Executable file
View File

@ -0,0 +1,3 @@
# Build command for Docker image
# TODO(dellaert): use docker compose and/or cmake
docker build --no-cache -t dellaert/ubuntu-gtsam-python:bionic .

36
docker/ubuntu-gtsam/Dockerfile Normal file
View File

@ -0,0 +1,36 @@
# Ubuntu image with GTSAM installed. Configured with Boost and TBB support.
# Get the base Ubuntu image from Docker Hub
FROM dellaert/ubuntu-boost-tbb:bionic
# Install git
RUN apt-get update && \
apt-get install -y git
# Install compiler
RUN apt-get install -y build-essential
# Clone GTSAM (develop branch)
WORKDIR /usr/src/
RUN git clone --single-branch --branch develop https://github.com/borglab/gtsam.git
# Change to build directory. Will be created automatically.
WORKDIR /usr/src/gtsam/build
# Run cmake
RUN cmake \
-DCMAKE_BUILD_TYPE=Release \
-DGTSAM_WITH_EIGEN_MKL=OFF \
-DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \
-DGTSAM_BUILD_TIMING_ALWAYS=OFF \
-DGTSAM_BUILD_TESTS=OFF \
-DGTSAM_INSTALL_CYTHON_TOOLBOX=OFF \
..
# Build
RUN make -j4 install && make clean
# Needed to link with GTSAM
RUN echo 'export LD_LIBRARY_PATH=/usr/local/lib:LD_LIBRARY_PATH' >> /root/.bashrc
# Run bash
CMD ["bash"]

3
docker/ubuntu-gtsam/build.sh Executable file
View File

@ -0,0 +1,3 @@
# Build command for Docker image
# TODO(dellaert): use docker compose and/or cmake
docker build --no-cache -t dellaert/ubuntu-gtsam:bionic .

3
examples/CMakeLists.txt
View File

@ -1,7 +1,4 @@
set (excluded_examples
DiscreteBayesNet_FG.cpp
UGM_chain.cpp
UGM_small.cpp
elaboratePoint2KalmanFilter.cpp
)

6
examples/Data/Klaus3.g2o Normal file
View File

@ -0,0 +1,6 @@
VERTEX_SE3:QUAT 0 -3.865747774038187 0.06639337702667497 -0.16064874691945374 0.024595211709139555 0.49179523413089893 -0.06279232989379242 0.8680954132776109
VERTEX_SE3:QUAT 1 -3.614793159814815 0.04774490041587656 -0.2837650367985949 0.00991721787943912 0.4854918961891193 -0.042343290945895576 0.8731588132957809
VERTEX_SE3:QUAT 2 -3.255096913553434 0.013296754286114112 -0.5339792269680574 -0.027851108010665374 0.585478168397957 -0.05088341463532465 0.8086102325762403
EDGE_SE3:QUAT 0 1 0.2509546142233723 -0.01864847661079841 -0.12311628987914114 -0.022048798853273946 -0.01796327847857683 0.010210006313668573 0.9995433591728293 100.0 0.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 25.0 0.0 0.0 25.0 0.0 25.0
EDGE_SE3:QUAT 0 2 0.6106508604847534 -0.05309662274056086 -0.3733304800486037 -0.054972994022992064 0.10432547598981769 -0.02221474884651081 0.9927742290779572 100.0 0.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 25.0 0.0 0.0 25.0 0.0 25.0
EDGE_SE3:QUAT 1 2 0.3596962462613811 -0.03444814612976245 -0.25021419016946256 -0.03174661848656213 0.11646825423134777 -0.02951742735854383 0.9922479626852876 100.0 0.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 25.0 0.0 0.0 25.0 0.0 25.0

11
examples/Data/toyExample.g2o Executable file
View File

@ -0,0 +1,11 @@
VERTEX_SE3:QUAT 0 0 0 0 0 0 0 1
VERTEX_SE3:QUAT 1 0 0 0 0 0 0 1
VERTEX_SE3:QUAT 2 0 0 0 0.00499994 0.00499994 0.00499994 0.999963
VERTEX_SE3:QUAT 3 0 0 0 -0.00499994 -0.00499994 -0.00499994 0.999963
VERTEX_SE3:QUAT 4 0 0 0 0.00499994 0.00499994 0.00499994 0.999963
EDGE_SE3:QUAT 1 2 1 2 0 0 0 0.707107 0.707107 100 0 0 0 0 0 100 0 0 0 0 100 0 0 0 100 0 0 100 0 100
EDGE_SE3:QUAT 2 3 -3.26795e-07 1 0 0 0 0.707107 0.707107 100 0 0 0 0 0 100 0 0 0 0 100 0 0 0 100 0 0 100 0 100
EDGE_SE3:QUAT 3 4 1 1 0 0 0 0.707107 0.707107 100 0 0 0 0 0 100 0 0 0 0 100 0 0 0 100 0 0 100 0 100
EDGE_SE3:QUAT 3 1 6.9282e-07 2 0 0 0 1 1.73205e-07 100 0 0 0 0 0 100 0 0 0 0 100 0 0 0 100 0 0 100 0 100
EDGE_SE3:QUAT 1 4 -1 1 0 0 0 -0.707107 0.707107 100 0 0 0 0 0 100 0 0 0 0 100 0 0 0 100 0 0 100 0 100
EDGE_SE3:QUAT 0 1 0 0 0 0 0 0 1 100 0 0 0 0 0 100 0 0 0 0 100 0 0 0 100 0 0 100 0 100

83
examples/DiscreteBayesNetExample.cpp Normal file
View File

@ -0,0 +1,83 @@
/* ----------------------------------------------------------------------------
* 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 DiscreteBayesNetExample.cpp
* @brief Discrete Bayes Net example with famous Asia Bayes Network
* @author Frank Dellaert
* @date JULY 10, 2020
*/
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/discrete/DiscreteMarginals.h>
#include <gtsam/inference/BayesNet.h>
#include <iomanip>
using namespace std;
using namespace gtsam;
int main(int argc, char **argv) {
DiscreteBayesNet asia;
DiscreteKey Asia(0, 2), Smoking(4, 2), Tuberculosis(3, 2), LungCancer(6, 2),
Bronchitis(7, 2), Either(5, 2), XRay(2, 2), Dyspnea(1, 2);
asia.add(Asia % "99/1");
asia.add(Smoking % "50/50");
asia.add(Tuberculosis | Asia = "99/1 95/5");
asia.add(LungCancer | Smoking = "99/1 90/10");
asia.add(Bronchitis | Smoking = "70/30 40/60");
asia.add((Either | Tuberculosis, LungCancer) = "F T T T");
asia.add(XRay | Either = "95/5 2/98");
asia.add((Dyspnea | Either, Bronchitis) = "9/1 2/8 3/7 1/9");
// print
vector<string> pretty = {"Asia", "Dyspnea", "XRay", "Tuberculosis",
"Smoking", "Either", "LungCancer", "Bronchitis"};
auto formatter = [pretty](Key key) { return pretty[key]; };
asia.print("Asia", formatter);
// Convert to factor graph
DiscreteFactorGraph fg(asia);
// Create solver and eliminate
Ordering ordering;
ordering += Key(0), Key(1), Key(2), Key(3), Key(4), Key(5), Key(6), Key(7);
DiscreteBayesNet::shared_ptr chordal = fg.eliminateSequential(ordering);
// solve
DiscreteFactor::sharedValues mpe = chordal->optimize();
GTSAM_PRINT(*mpe);
// We can also build a Bayes tree (directed junction tree).
// The elimination order above will do fine:
auto bayesTree = fg.eliminateMultifrontal(ordering);
bayesTree->print("bayesTree", formatter);
// add evidence, we were in Asia and we have dyspnea
fg.add(Asia, "0 1");
fg.add(Dyspnea, "0 1");
// solve again, now with evidence
DiscreteBayesNet::shared_ptr chordal2 = fg.eliminateSequential(ordering);
DiscreteFactor::sharedValues mpe2 = chordal2->optimize();
GTSAM_PRINT(*mpe2);
// We can also sample from it
cout << "\n10 samples:" << endl;
for (size_t i = 0; i < 10; i++) {
DiscreteFactor::sharedValues sample = chordal2->sample();
GTSAM_PRINT(*sample);
}
return 0;
}

117
examples/DiscreteBayesNet_FG.cpp
View File

@ -15,105 +15,106 @@
* @author Abhijit
* @date Jun 4, 2012
*
* We use the famous Rain/Cloudy/Sprinkler Example of [Russell & Norvig, 2009, p529]
* You may be familiar with other graphical model packages like BNT (available
* at http://bnt.googlecode.com/svn/trunk/docs/usage.html) where this is used as an
* example. The following demo is same as that in the above link, except that
* everything is using GTSAM.
* We use the famous Rain/Cloudy/Sprinkler Example of [Russell & Norvig, 2009,
* p529] You may be familiar with other graphical model packages like BNT
* (available at http://bnt.googlecode.com/svn/trunk/docs/usage.html) where this
* is used as an example. The following demo is same as that in the above link,
* except that everything is using GTSAM.
*/
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/discrete/DiscreteSequentialSolver.h>
#include <gtsam/discrete/DiscreteMarginals.h>
#include <iomanip>
using namespace std;
using namespace gtsam;
int main(int argc, char **argv) {
// Define keys and a print function
Key C(1), S(2), R(3), W(4);
auto print = [=](DiscreteFactor::sharedValues values) {
cout << boolalpha << "Cloudy = " << static_cast<bool>((*values)[C])
<< " Sprinkler = " << static_cast<bool>((*values)[S])
<< " Rain = " << boolalpha << static_cast<bool>((*values)[R])
<< " WetGrass = " << static_cast<bool>((*values)[W]) << endl;
};
// We assume binary state variables
// we have 0 == "False" and 1 == "True"
const size_t nrStates = 2;
// define variables
DiscreteKey Cloudy(1, nrStates), Sprinkler(2, nrStates), Rain(3, nrStates),
WetGrass(4, nrStates);
DiscreteKey Cloudy(C, nrStates), Sprinkler(S, nrStates), Rain(R, nrStates),
WetGrass(W, nrStates);
// create Factor Graph of the bayes net
DiscreteFactorGraph graph;
// add factors
graph.add(Cloudy, "0.5 0.5"); //P(Cloudy)
graph.add(Cloudy & Sprinkler, "0.5 0.5 0.9 0.1"); //P(Sprinkler | Cloudy)
graph.add(Cloudy & Rain, "0.8 0.2 0.2 0.8"); //P(Rain | Cloudy)
graph.add(Cloudy, "0.5 0.5"); // P(Cloudy)
graph.add(Cloudy & Sprinkler, "0.5 0.5 0.9 0.1"); // P(Sprinkler | Cloudy)
graph.add(Cloudy & Rain, "0.8 0.2 0.2 0.8"); // P(Rain | Cloudy)
graph.add(Sprinkler & Rain & WetGrass,
"1 0 0.1 0.9 0.1 0.9 0.001 0.99"); //P(WetGrass | Sprinkler, Rain)
"1 0 0.1 0.9 0.1 0.9 0.001 0.99"); // P(WetGrass | Sprinkler, Rain)
// Alternatively we can also create a DiscreteBayesNet, add DiscreteConditional
// factors and create a FactorGraph from it. (See testDiscreteBayesNet.cpp)
// Alternatively we can also create a DiscreteBayesNet, add
// DiscreteConditional factors and create a FactorGraph from it. (See
// testDiscreteBayesNet.cpp)
// Since this is a relatively small distribution, we can as well print
// the whole distribution..
cout << "Distribution of Example: " << endl;
cout << setw(11) << "Cloudy(C)" << setw(14) << "Sprinkler(S)" << setw(10)
<< "Rain(R)" << setw(14) << "WetGrass(W)" << setw(15) << "P(C,S,R,W)"
<< endl;
<< "Rain(R)" << setw(14) << "WetGrass(W)" << setw(15) << "P(C,S,R,W)"
<< endl;
for (size_t a = 0; a < nrStates; a++)
for (size_t m = 0; m < nrStates; m++)
for (size_t h = 0; h < nrStates; h++)
for (size_t c = 0; c < nrStates; c++) {
DiscreteFactor::Values values;
values[Cloudy.first] = c;
values[Sprinkler.first] = h;
values[Rain.first] = m;
values[WetGrass.first] = a;
values[C] = c;
values[S] = h;
values[R] = m;
values[W] = a;
double prodPot = graph(values);
cout << boolalpha << setw(8) << (bool) c << setw(14)
<< (bool) h << setw(12) << (bool) m << setw(13)
<< (bool) a << setw(16) << prodPot << endl;
cout << setw(8) << static_cast<bool>(c) << setw(14)
<< static_cast<bool>(h) << setw(12) << static_cast<bool>(m)
<< setw(13) << static_cast<bool>(a) << setw(16) << prodPot
<< endl;
}
// "Most Probable Explanation", i.e., configuration with largest value
DiscreteSequentialSolver solver(graph);
DiscreteFactor::sharedValues optimalDecoding = solver.optimize();
cout <<"\nMost Probable Explanation (MPE):" << endl;
cout << boolalpha << "Cloudy = " << (bool)(*optimalDecoding)[Cloudy.first]
<< " Sprinkler = " << (bool)(*optimalDecoding)[Sprinkler.first]
<< " Rain = " << boolalpha << (bool)(*optimalDecoding)[Rain.first]
<< " WetGrass = " << (bool)(*optimalDecoding)[WetGrass.first]<< endl;
DiscreteFactor::sharedValues mpe = graph.eliminateSequential()->optimize();
cout << "\nMost Probable Explanation (MPE):" << endl;
print(mpe);
// "Inference" We show an inference query like: probability that the Sprinkler
// was on; given that the grass is wet i.e. P( S | C=0) = ?
// "Inference" We show an inference query like: probability that the Sprinkler was on;
// given that the grass is wet i.e. P( S | W=1) =?
cout << "\nInference Query: Probability of Sprinkler being on given Grass is Wet" << endl;
// add evidence that it is not Cloudy
graph.add(Cloudy, "1 0");
// Method 1: we can compute the joint marginal P(S,W) and from that we can compute
// P(S | W=1) = P(S,W=1)/P(W=1) We do this in following three steps..
// solve again, now with evidence
DiscreteBayesNet::shared_ptr chordal = graph.eliminateSequential();
DiscreteFactor::sharedValues mpe_with_evidence = chordal->optimize();
//Step1: Compute P(S,W)
DiscreteFactorGraph jointFG;
jointFG = *solver.jointFactorGraph(DiscreteKeys(Sprinkler & WetGrass).indices());
DecisionTreeFactor probSW = jointFG.product();
cout << "\nMPE given C=0:" << endl;
print(mpe_with_evidence);
//Step2: Compute P(W)
DiscreteFactor::shared_ptr probW = solver.marginalFactor(WetGrass.first);
//Step3: Computer P(S | W=1) = P(S,W=1)/P(W=1)
DiscreteFactor::Values values;
values[WetGrass.first] = 1;
//print P(S=0|W=1)
values[Sprinkler.first] = 0;
cout << "P(S=0|W=1) = " << probSW(values)/(*probW)(values) << endl;
//print P(S=1|W=1)
values[Sprinkler.first] = 1;
cout << "P(S=1|W=1) = " << probSW(values)/(*probW)(values) << endl;
// TODO: Method 2 : One way is to modify the factor graph to
// incorporate the evidence node and compute the marginal
// TODO: graph.addEvidence(Cloudy,0);
// we can also calculate arbitrary marginals:
DiscreteMarginals marginals(graph);
cout << "\nP(S=1|C=0):" << marginals.marginalProbabilities(Sprinkler)[1]
<< endl;
cout << "\nP(R=0|C=0):" << marginals.marginalProbabilities(Rain)[0] << endl;
cout << "\nP(W=1|C=0):" << marginals.marginalProbabilities(WetGrass)[1]
<< endl;
// We can also sample from it
cout << "\n10 samples:" << endl;
for (size_t i = 0; i < 10; i++) {
DiscreteFactor::sharedValues sample = chordal->sample();
print(sample);
}
return 0;
}

94
examples/HMMExample.cpp Normal file
View File

@ -0,0 +1,94 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2020, 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 DiscreteBayesNetExample.cpp
* @brief Hidden Markov Model example, discrete.
* @author Frank Dellaert
* @date July 12, 2020
*/
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/discrete/DiscreteMarginals.h>
#include <gtsam/inference/BayesNet.h>
#include <iomanip>
#include <sstream>
using namespace std;
using namespace gtsam;
int main(int argc, char **argv) {
const int nrNodes = 4;
const size_t nrStates = 3;
// Define variables as well as ordering
Ordering ordering;
vector<DiscreteKey> keys;
for (int k = 0; k < nrNodes; k++) {
DiscreteKey key_i(k, nrStates);
keys.push_back(key_i);
ordering.emplace_back(k);
}
// Create HMM as a DiscreteBayesNet
DiscreteBayesNet hmm;
// Define backbone
const string transition = "8/1/1 1/8/1 1/1/8";
for (int k = 1; k < nrNodes; k++) {
hmm.add(keys[k] | keys[k - 1] = transition);
}
// Add some measurements, not needed for all time steps!
hmm.add(keys[0] % "7/2/1");
hmm.add(keys[1] % "1/9/0");
hmm.add(keys.back() % "5/4/1");
// print
hmm.print("HMM");
// Convert to factor graph
DiscreteFactorGraph factorGraph(hmm);
// Create solver and eliminate
// This will create a DAG ordered with arrow of time reversed
DiscreteBayesNet::shared_ptr chordal =
factorGraph.eliminateSequential(ordering);
chordal->print("Eliminated");
// solve
DiscreteFactor::sharedValues mpe = chordal->optimize();
GTSAM_PRINT(*mpe);
// We can also sample from it
cout << "\n10 samples:" << endl;
for (size_t k = 0; k < 10; k++) {
DiscreteFactor::sharedValues sample = chordal->sample();
GTSAM_PRINT(*sample);
}
// Or compute the marginals. This re-eliminates the FG into a Bayes tree
cout << "\nComputing Node Marginals .." << endl;
DiscreteMarginals marginals(factorGraph);
for (int k = 0; k < nrNodes; k++) {
Vector margProbs = marginals.marginalProbabilities(keys[k]);
stringstream ss;
ss << "marginal " << k;
print(margProbs, ss.str());
}
// TODO(frank): put in the glue to have DiscreteMarginals produce *arbitrary*
// joints efficiently, by the Bayes tree shortcut magic. All the code is there
// but it's not yet connected.
return 0;
}

359
examples/IMUKittiExampleGPS.cpp Normal file
View File

@ -0,0 +1,359 @@
/* ----------------------------------------------------------------------------
* 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 IMUKittiExampleGPS
* @brief Example of application of ISAM2 for GPS-aided navigation on the KITTI VISION BENCHMARK SUITE
* @author Ported by Thomas Jespersen (thomasj@tkjelectronics.dk), TKJ Electronics
*/
// GTSAM related includes.
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/navigation/GPSFactor.h>
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/ISAM2Params.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/inference/Symbol.h>
#include <cstring>
#include <fstream>
#include <iostream>
using namespace std;
using namespace gtsam;
using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
using symbol_shorthand::V; // Vel (xdot,ydot,zdot)
using symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz)
struct KittiCalibration {
double body_ptx;
double body_pty;
double body_ptz;
double body_prx;
double body_pry;
double body_prz;
double accelerometer_sigma;
double gyroscope_sigma;
double integration_sigma;
double accelerometer_bias_sigma;
double gyroscope_bias_sigma;
double average_delta_t;
};
struct ImuMeasurement {
double time;
double dt;
Vector3 accelerometer;
Vector3 gyroscope; // omega
};
struct GpsMeasurement {
double time;
Vector3 position; // x,y,z
};
const string output_filename = "IMUKittiExampleGPSResults.csv";
void loadKittiData(KittiCalibration& kitti_calibration,
vector<ImuMeasurement>& imu_measurements,
vector<GpsMeasurement>& gps_measurements) {
string line;
// Read IMU metadata and compute relative sensor pose transforms
// BodyPtx BodyPty BodyPtz BodyPrx BodyPry BodyPrz AccelerometerSigma GyroscopeSigma IntegrationSigma
// AccelerometerBiasSigma GyroscopeBiasSigma AverageDeltaT
string imu_metadata_file = findExampleDataFile("KittiEquivBiasedImu_metadata.txt");
ifstream imu_metadata(imu_metadata_file.c_str());
printf("-- Reading sensor metadata\n");
getline(imu_metadata, line, '\n'); // ignore the first line
// Load Kitti calibration
getline(imu_metadata, line, '\n');
sscanf(line.c_str(), "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
&kitti_calibration.body_ptx,
&kitti_calibration.body_pty,
&kitti_calibration.body_ptz,
&kitti_calibration.body_prx,
&kitti_calibration.body_pry,
&kitti_calibration.body_prz,
&kitti_calibration.accelerometer_sigma,
&kitti_calibration.gyroscope_sigma,
&kitti_calibration.integration_sigma,
&kitti_calibration.accelerometer_bias_sigma,
&kitti_calibration.gyroscope_bias_sigma,
&kitti_calibration.average_delta_t);
printf("IMU metadata: %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf\n",
kitti_calibration.body_ptx,
kitti_calibration.body_pty,
kitti_calibration.body_ptz,
kitti_calibration.body_prx,
kitti_calibration.body_pry,
kitti_calibration.body_prz,
kitti_calibration.accelerometer_sigma,
kitti_calibration.gyroscope_sigma,
kitti_calibration.integration_sigma,
kitti_calibration.accelerometer_bias_sigma,
kitti_calibration.gyroscope_bias_sigma,
kitti_calibration.average_delta_t);
// Read IMU data
// Time dt accelX accelY accelZ omegaX omegaY omegaZ
string imu_data_file = findExampleDataFile("KittiEquivBiasedImu.txt");
printf("-- Reading IMU measurements from file\n");
{
ifstream imu_data(imu_data_file.c_str());
getline(imu_data, line, '\n'); // ignore the first line
double time = 0, dt = 0, acc_x = 0, acc_y = 0, acc_z = 0, gyro_x = 0, gyro_y = 0, gyro_z = 0;
while (!imu_data.eof()) {
getline(imu_data, line, '\n');
sscanf(line.c_str(), "%lf %lf %lf %lf %lf %lf %lf %lf",
&time, &dt,
&acc_x, &acc_y, &acc_z,
&gyro_x, &gyro_y, &gyro_z);
ImuMeasurement measurement;
measurement.time = time;
measurement.dt = dt;
measurement.accelerometer = Vector3(acc_x, acc_y, acc_z);
measurement.gyroscope = Vector3(gyro_x, gyro_y, gyro_z);
imu_measurements.push_back(measurement);
}
}
// Read GPS data
// Time,X,Y,Z
string gps_data_file = findExampleDataFile("KittiGps_converted.txt");
printf("-- Reading GPS measurements from file\n");
{
ifstream gps_data(gps_data_file.c_str());
getline(gps_data, line, '\n'); // ignore the first line
double time = 0, gps_x = 0, gps_y = 0, gps_z = 0;
while (!gps_data.eof()) {
getline(gps_data, line, '\n');
sscanf(line.c_str(), "%lf,%lf,%lf,%lf", &time, &gps_x, &gps_y, &gps_z);
GpsMeasurement measurement;
measurement.time = time;
measurement.position = Vector3(gps_x, gps_y, gps_z);
gps_measurements.push_back(measurement);
}
}
}
int main(int argc, char* argv[]) {
KittiCalibration kitti_calibration;
vector<ImuMeasurement> imu_measurements;
vector<GpsMeasurement> gps_measurements;
loadKittiData(kitti_calibration, imu_measurements, gps_measurements);
Vector6 BodyP = (Vector6() << kitti_calibration.body_ptx, kitti_calibration.body_pty, kitti_calibration.body_ptz,
kitti_calibration.body_prx, kitti_calibration.body_pry, kitti_calibration.body_prz)
.finished();
auto body_T_imu = Pose3::Expmap(BodyP);
if (!body_T_imu.equals(Pose3(), 1e-5)) {
printf("Currently only support IMUinBody is identity, i.e. IMU and body frame are the same");
exit(-1);
}
// Configure different variables
// double t_offset = gps_measurements[0].time;
size_t first_gps_pose = 1;
size_t gps_skip = 10; // Skip this many GPS measurements each time
double g = 9.8;
auto w_coriolis = Vector3::Zero(); // zero vector
// Configure noise models
auto noise_model_gps = noiseModel::Diagonal::Precisions((Vector6() << Vector3::Constant(0),
Vector3::Constant(1.0/0.07))
.finished());
// Set initial conditions for the estimated trajectory
// initial pose is the reference frame (navigation frame)
auto current_pose_global = Pose3(Rot3(), gps_measurements[first_gps_pose].position);
// the vehicle is stationary at the beginning at position 0,0,0
Vector3 current_velocity_global = Vector3::Zero();
auto current_bias = imuBias::ConstantBias(); // init with zero bias
auto sigma_init_x = noiseModel::Diagonal::Precisions((Vector6() << Vector3::Constant(0),
Vector3::Constant(1.0))
.finished());
auto sigma_init_v = noiseModel::Diagonal::Sigmas(Vector3::Constant(1000.0));
auto sigma_init_b = noiseModel::Diagonal::Sigmas((Vector6() << Vector3::Constant(0.100),
Vector3::Constant(5.00e-05))
.finished());
// Set IMU preintegration parameters
Matrix33 measured_acc_cov = I_3x3 * pow(kitti_calibration.accelerometer_sigma, 2);
Matrix33 measured_omega_cov = I_3x3 * pow(kitti_calibration.gyroscope_sigma, 2);
// error committed in integrating position from velocities
Matrix33 integration_error_cov = I_3x3 * pow(kitti_calibration.integration_sigma, 2);
auto imu_params = PreintegratedImuMeasurements::Params::MakeSharedU(g);
imu_params->accelerometerCovariance = measured_acc_cov; // acc white noise in continuous
imu_params->integrationCovariance = integration_error_cov; // integration uncertainty continuous
imu_params->gyroscopeCovariance = measured_omega_cov; // gyro white noise in continuous
imu_params->omegaCoriolis = w_coriolis;
std::shared_ptr<PreintegratedImuMeasurements> current_summarized_measurement = nullptr;
// Set ISAM2 parameters and create ISAM2 solver object
ISAM2Params isam_params;
isam_params.factorization = ISAM2Params::CHOLESKY;
isam_params.relinearizeSkip = 10;
ISAM2 isam(isam_params);
// Create the factor graph and values object that will store new factors and values to add to the incremental graph
NonlinearFactorGraph new_factors;
Values new_values; // values storing the initial estimates of new nodes in the factor graph
/// Main loop:
/// (1) we read the measurements
/// (2) we create the corresponding factors in the graph
/// (3) we solve the graph to obtain and optimal estimate of robot trajectory
printf("-- Starting main loop: inference is performed at each time step, but we plot trajectory every 10 steps\n");
size_t j = 0;
for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) {
// At each non=IMU measurement we initialize a new node in the graph
auto current_pose_key = X(i);
auto current_vel_key = V(i);
auto current_bias_key = B(i);
double t = gps_measurements[i].time;
if (i == first_gps_pose) {
// Create initial estimate and prior on initial pose, velocity, and biases
new_values.insert(current_pose_key, current_pose_global);
new_values.insert(current_vel_key, current_velocity_global);
new_values.insert(current_bias_key, current_bias);
new_factors.emplace_shared<PriorFactor<Pose3>>(current_pose_key, current_pose_global, sigma_init_x);
new_factors.emplace_shared<PriorFactor<Vector3>>(current_vel_key, current_velocity_global, sigma_init_v);
new_factors.emplace_shared<PriorFactor<imuBias::ConstantBias>>(current_bias_key, current_bias, sigma_init_b);
} else {
double t_previous = gps_measurements[i-1].time;
// Summarize IMU data between the previous GPS measurement and now
current_summarized_measurement = std::make_shared<PreintegratedImuMeasurements>(imu_params, current_bias);
static size_t included_imu_measurement_count = 0;
while (j < imu_measurements.size() && imu_measurements[j].time <= t) {
if (imu_measurements[j].time >= t_previous) {
current_summarized_measurement->integrateMeasurement(imu_measurements[j].accelerometer,
imu_measurements[j].gyroscope,
imu_measurements[j].dt);
included_imu_measurement_count++;
}
j++;
}
// Create IMU factor
auto previous_pose_key = X(i-1);
auto previous_vel_key = V(i-1);
auto previous_bias_key = B(i-1);
new_factors.emplace_shared<ImuFactor>(previous_pose_key, previous_vel_key,
current_pose_key, current_vel_key,
previous_bias_key, *current_summarized_measurement);
// Bias evolution as given in the IMU metadata
auto sigma_between_b = noiseModel::Diagonal::Sigmas((Vector6() <<
Vector3::Constant(sqrt(included_imu_measurement_count) * kitti_calibration.accelerometer_bias_sigma),
Vector3::Constant(sqrt(included_imu_measurement_count) * kitti_calibration.gyroscope_bias_sigma))
.finished());
new_factors.emplace_shared<BetweenFactor<imuBias::ConstantBias>>(previous_bias_key,
current_bias_key,
imuBias::ConstantBias(),
sigma_between_b);
// Create GPS factor
auto gps_pose = Pose3(current_pose_global.rotation(), gps_measurements[i].position);
if ((i % gps_skip) == 0) {
new_factors.emplace_shared<PriorFactor<Pose3>>(current_pose_key, gps_pose, noise_model_gps);
new_values.insert(current_pose_key, gps_pose);
printf("################ POSE INCLUDED AT TIME %lf ################\n", t);
gps_pose.translation().print();
printf("\n\n");
} else {
new_values.insert(current_pose_key, current_pose_global);
}
// Add initial values for velocity and bias based on the previous estimates
new_values.insert(current_vel_key, current_velocity_global);
new_values.insert(current_bias_key, current_bias);
// Update solver
// =======================================================================
// We accumulate 2*GPSskip GPS measurements before updating the solver at
// first so that the heading becomes observable.
if (i > (first_gps_pose + 2*gps_skip)) {
printf("################ NEW FACTORS AT TIME %lf ################\n", t);
new_factors.print();
isam.update(new_factors, new_values);
// Reset the newFactors and newValues list
new_factors.resize(0);
new_values.clear();
// Extract the result/current estimates
Values result = isam.calculateEstimate();
current_pose_global = result.at<Pose3>(current_pose_key);
current_velocity_global = result.at<Vector3>(current_vel_key);
current_bias = result.at<imuBias::ConstantBias>(current_bias_key);
printf("\n################ POSE AT TIME %lf ################\n", t);
current_pose_global.print();
printf("\n\n");
}
}
}
// Save results to file
printf("\nWriting results to file...\n");
FILE* fp_out = fopen(output_filename.c_str(), "w+");
fprintf(fp_out, "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m)\n");
Values result = isam.calculateEstimate();
for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) {
auto pose_key = X(i);
auto vel_key = V(i);
auto bias_key = B(i);
auto pose = result.at<Pose3>(pose_key);
auto velocity = result.at<Vector3>(vel_key);
auto bias = result.at<imuBias::ConstantBias>(bias_key);
auto pose_quat = pose.rotation().toQuaternion();
auto gps = gps_measurements[i].position;
cout << "State at #" << i << endl;
cout << "Pose:" << endl << pose << endl;
cout << "Velocity:" << endl << velocity << endl;
cout << "Bias:" << endl << bias << endl;
fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",
gps_measurements[i].time,
pose.x(), pose.y(), pose.z(),
pose_quat.x(), pose_quat.y(), pose_quat.z(), pose_quat.w(),
gps(0), gps(1), gps(2));
}
fclose(fp_out);
}

35
examples/ImuFactorsExample.cpp
View File

@ -35,22 +35,28 @@
* optional arguments:
* data_csv_path path to the CSV file with the IMU data.
* -c use CombinedImuFactor
* Note: Define USE_LM to use Levenberg Marquardt Optimizer
* By default ISAM2 is used
*/
// GTSAM related includes.
#include <gtsam/inference/Symbol.h>
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/navigation/GPSFactor.h>
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/inference/Symbol.h>
#include <cstring>
#include <fstream>
#include <iostream>
// Uncomment the following to use Levenberg Marquardt Optimizer
// #define USE_LM
using namespace gtsam;
using namespace std;
@ -64,6 +70,17 @@ static const char use_combined_imu_flag[3] = "-c";
int main(int argc, char* argv[]) {
string data_filename;
bool use_combined_imu = false;
#ifndef USE_LM
printf("Using ISAM2\n");
ISAM2Params parameters;
parameters.relinearizeThreshold = 0.01;
parameters.relinearizeSkip = 1;
ISAM2 isam2(parameters);
#else
printf("Using Levenberg Marquardt Optimizer\n");
#endif
if (argc < 2) {
printf("using default CSV file\n");
data_filename = findExampleDataFile("imuAndGPSdata.csv");
@ -252,9 +269,19 @@ int main(int argc, char* argv[]) {
initial_values.insert(V(correction_count), prop_state.v());
initial_values.insert(B(correction_count), prev_bias);
Values result;
#ifdef USE_LM
LevenbergMarquardtOptimizer optimizer(*graph, initial_values);
Values result = optimizer.optimize();
result = optimizer.optimize();
#else
isam2.update(*graph, initial_values);
isam2.update();
result = isam2.calculateEstimate();
// reset the graph
graph->resize(0);
initial_values.clear();
#endif
// Overwrite the beginning of the preintegration for the next step.
prev_state = NavState(result.at<Pose3>(X(correction_count)),
result.at<Vector3>(V(correction_count)));

2
examples/SFMExample.cpp
View File

@ -109,7 +109,7 @@ int main(int argc, char* argv[]) {
Symbol('x', i), corrupted_pose);
}
for (size_t j = 0; j < points.size(); ++j) {
auto corrupted_point = points[j] + Point3(-0.25, 0.20, 0.15);
Point3 corrupted_point = points[j] + Point3(-0.25, 0.20, 0.15);
initialEstimate.insert<Point3>(Symbol('l', j), corrupted_point);
}
initialEstimate.print("Initial Estimates:\n");

74
examples/UGM_chain.cpp
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */
/**
* @file small.cpp
* @file UGM_chain.cpp
* @brief UGM (undirected graphical model) examples: chain
* @author Frank Dellaert
* @author Abhijit Kundu
@ -19,10 +19,9 @@
* for more explanation. This code demos the same example using GTSAM.
*/
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/discrete/DiscreteSequentialSolver.h>
#include <gtsam/discrete/DiscreteMarginals.h>
#include <gtsam/base/timing.h>
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/discrete/DiscreteMarginals.h>
#include <iomanip>
@ -30,9 +29,8 @@ using namespace std;
using namespace gtsam;
int main(int argc, char** argv) {
// Set Number of Nodes in the Graph
const int nrNodes = 60;
// Set Number of Nodes in the Graph
const int nrNodes = 60;
// Each node takes 1 of 7 possible states denoted by 0-6 in following order:
// ["VideoGames" "Industry" "GradSchool" "VideoGames(with PhD)"
@ -40,70 +38,55 @@ int main(int argc, char** argv) {
const size_t nrStates = 7;
// define variables
vector<DiscreteKey> nodes;
for (int i = 0; i < nrNodes; i++){
DiscreteKey dk(i, nrStates);
nodes.push_back(dk);
}
vector<DiscreteKey> nodes;
for (int i = 0; i < nrNodes; i++) {
DiscreteKey dk(i, nrStates);
nodes.push_back(dk);
}
// create graph
DiscreteFactorGraph graph;
// add node potentials
graph.add(nodes[0], ".3 .6 .1 0 0 0 0");
for (int i = 1; i < nrNodes; i++)
graph.add(nodes[i], "1 1 1 1 1 1 1");
for (int i = 1; i < nrNodes; i++) graph.add(nodes[i], "1 1 1 1 1 1 1");
const std::string edgePotential = ".08 .9 .01 0 0 0 .01 "
".03 .95 .01 0 0 0 .01 "
".06 .06 .75 .05 .05 .02 .01 "
"0 0 0 .3 .6 .09 .01 "
"0 0 0 .02 .95 .02 .01 "
"0 0 0 .01 .01 .97 .01 "
"0 0 0 0 0 0 1";
const std::string edgePotential =
".08 .9 .01 0 0 0 .01 "
".03 .95 .01 0 0 0 .01 "
".06 .06 .75 .05 .05 .02 .01 "
"0 0 0 .3 .6 .09 .01 "
"0 0 0 .02 .95 .02 .01 "
"0 0 0 .01 .01 .97 .01 "
"0 0 0 0 0 0 1";
// add edge potentials
for (int i = 0; i < nrNodes - 1; i++)
graph.add(nodes[i] & nodes[i + 1], edgePotential);
cout << "Created Factor Graph with " << nrNodes << " variable nodes and "
<< graph.size() << " factors (Unary+Edge).";
<< graph.size() << " factors (Unary+Edge).";
// "Decoding", i.e., configuration with largest value
// We use sequential variable elimination
DiscreteSequentialSolver solver(graph);
DiscreteFactor::sharedValues optimalDecoding = solver.optimize();
DiscreteBayesNet::shared_ptr chordal = graph.eliminateSequential();
DiscreteFactor::sharedValues optimalDecoding = chordal->optimize();
optimalDecoding->print("\nMost Probable Explanation (optimalDecoding)\n");
// "Inference" Computing marginals for each node
cout << "\nComputing Node Marginals ..(Sequential Elimination)" << endl;
gttic_(Sequential);
for (vector<DiscreteKey>::iterator itr = nodes.begin(); itr != nodes.end();
++itr) {
//Compute the marginal
Vector margProbs = solver.marginalProbabilities(*itr);
//Print the marginals
cout << "Node#" << setw(4) << itr->first << " : ";
print(margProbs);
}
gttoc_(Sequential);
// Here we'll make use of DiscreteMarginals class, which makes use of
// bayes-tree based shortcut evaluation of marginals
DiscreteMarginals marginals(graph);
cout << "\nComputing Node Marginals ..(BayesTree based)" << endl;
gttic_(Multifrontal);
for (vector<DiscreteKey>::iterator itr = nodes.begin(); itr != nodes.end();
++itr) {
//Compute the marginal
Vector margProbs = marginals.marginalProbabilities(*itr);
for (vector<DiscreteKey>::iterator it = nodes.begin(); it != nodes.end();
++it) {
// Compute the marginal
Vector margProbs = marginals.marginalProbabilities(*it);
//Print the marginals
cout << "Node#" << setw(4) << itr->first << " : ";
// Print the marginals
cout << "Node#" << setw(4) << it->first << " : ";
print(margProbs);
}
gttoc_(Multifrontal);
@ -111,4 +94,3 @@ int main(int argc, char** argv) {
tictoc_print_();
return 0;
}

19
examples/UGM_small.cpp
View File

@ -10,15 +10,16 @@
* -------------------------------------------------------------------------- */
/**
* @file small.cpp
* @file UGM_small.cpp
* @brief UGM (undirected graphical model) examples: small
* @author Frank Dellaert
*
* See http://www.di.ens.fr/~mschmidt/Software/UGM/small.html
*/
#include <gtsam/base/Vector.h>
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/discrete/DiscreteSequentialSolver.h>
#include <gtsam/discrete/DiscreteMarginals.h>
using namespace std;
using namespace gtsam;
@ -61,24 +62,24 @@ int main(int argc, char** argv) {
// "Decoding", i.e., configuration with largest value (MPE)
// We use sequential variable elimination
DiscreteSequentialSolver solver(graph);
DiscreteFactor::sharedValues optimalDecoding = solver.optimize();
DiscreteBayesNet::shared_ptr chordal = graph.eliminateSequential();
DiscreteFactor::sharedValues optimalDecoding = chordal->optimize();
optimalDecoding->print("\noptimalDecoding");
// "Inference" Computing marginals
cout << "\nComputing Node Marginals .." << endl;
Vector margProbs;
DiscreteMarginals marginals(graph);
margProbs = solver.marginalProbabilities(Cathy);
Vector margProbs = marginals.marginalProbabilities(Cathy);
print(margProbs, "Cathy's Node Marginal:");
margProbs = solver.marginalProbabilities(Heather);
margProbs = marginals.marginalProbabilities(Heather);
print(margProbs, "Heather's Node Marginal");
margProbs = solver.marginalProbabilities(Mark);
margProbs = marginals.marginalProbabilities(Mark);
print(margProbs, "Mark's Node Marginal");
margProbs = solver.marginalProbabilities(Allison);
margProbs = marginals.marginalProbabilities(Allison);
print(margProbs, "Allison's Node Marginal");
return 0;

63
gtsam.h
View File

@ -598,6 +598,7 @@ class SOn {
// Standard Constructors
SOn(size_t n);
static gtsam::SOn FromMatrix(Matrix R);
static gtsam::SOn Lift(size_t n, Matrix R);
// Testable
void print(string s) const;
@ -1458,7 +1459,7 @@ virtual class Null: gtsam::noiseModel::mEstimator::Base {
void serializable() const;
double weight(double error) const;
double residual(double error) const;
double loss(double error) const;
};
virtual class Fair: gtsam::noiseModel::mEstimator::Base {
@ -1469,7 +1470,7 @@ virtual class Fair: gtsam::noiseModel::mEstimator::Base {
void serializable() const;
double weight(double error) const;
double residual(double error) const;
double loss(double error) const;
};
virtual class Huber: gtsam::noiseModel::mEstimator::Base {
@ -1480,7 +1481,7 @@ virtual class Huber: gtsam::noiseModel::mEstimator::Base {
void serializable() const;
double weight(double error) const;
double residual(double error) const;
double loss(double error) const;
};
virtual class Cauchy: gtsam::noiseModel::mEstimator::Base {
@ -1491,7 +1492,7 @@ virtual class Cauchy: gtsam::noiseModel::mEstimator::Base {
void serializable() const;
double weight(double error) const;
double residual(double error) const;
double loss(double error) const;
};
virtual class Tukey: gtsam::noiseModel::mEstimator::Base {
@ -1502,7 +1503,7 @@ virtual class Tukey: gtsam::noiseModel::mEstimator::Base {
void serializable() const;
double weight(double error) const;
double residual(double error) const;
double loss(double error) const;
};
virtual class Welsch: gtsam::noiseModel::mEstimator::Base {
@ -1513,7 +1514,7 @@ virtual class Welsch: gtsam::noiseModel::mEstimator::Base {
void serializable() const;
double weight(double error) const;
double residual(double error) const;
double loss(double error) const;
};
virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base {
@ -1524,7 +1525,7 @@ virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base {
void serializable() const;
double weight(double error) const;
double residual(double error) const;
double loss(double error) const;
};
virtual class DCS: gtsam::noiseModel::mEstimator::Base {
@ -1535,7 +1536,7 @@ virtual class DCS: gtsam::noiseModel::mEstimator::Base {
void serializable() const;
double weight(double error) const;
double residual(double error) const;
double loss(double error) const;
};
virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base {
@ -1546,7 +1547,7 @@ virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base {
void serializable() const;
double weight(double error) const;
double residual(double error) const;
double loss(double error) const;
};
}///\namespace mEstimator
@ -1937,6 +1938,22 @@ virtual class ConjugateGradientParameters : gtsam::IterativeOptimizationParamete
void print() const;
};
#include <gtsam/linear/Preconditioner.h>
virtual class PreconditionerParameters {
PreconditionerParameters();
};
virtual class DummyPreconditionerParameters : gtsam::PreconditionerParameters {
DummyPreconditionerParameters();
};
#include <gtsam/linear/PCGSolver.h>
virtual class PCGSolverParameters : gtsam::ConjugateGradientParameters {
PCGSolverParameters();
void print(string s);
void setPreconditionerParams(gtsam::PreconditionerParameters* preconditioner);
};
#include <gtsam/linear/SubgraphSolver.h>
virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters {
SubgraphSolverParameters();
@ -2835,6 +2852,34 @@ virtual class KarcherMeanFactor : gtsam::NonlinearFactor {
KarcherMeanFactor(const gtsam::KeyVector& keys);
};
#include <gtsam/slam/FrobeniusFactor.h>
gtsam::noiseModel::Isotropic* ConvertPose3NoiseModel(
gtsam::noiseModel::Base* model, size_t d);
template<T = {gtsam::SO3, gtsam::SO4}>
virtual class FrobeniusFactor : gtsam::NoiseModelFactor {
FrobeniusFactor(size_t key1, size_t key2);
FrobeniusFactor(size_t key1, size_t key2, gtsam::noiseModel::Base* model);
Vector evaluateError(const T& R1, const T& R2);
};
template<T = {gtsam::SO3, gtsam::SO4}>
virtual class FrobeniusBetweenFactor : gtsam::NoiseModelFactor {
FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12);
FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12, gtsam::noiseModel::Base* model);
Vector evaluateError(const T& R1, const T& R2);
};
virtual class FrobeniusWormholeFactor : gtsam::NoiseModelFactor {
FrobeniusWormholeFactor(size_t key1, size_t key2, const gtsam::Rot3& R12,
size_t p);
FrobeniusWormholeFactor(size_t key1, size_t key2, const gtsam::Rot3& R12,
size_t p, gtsam::noiseModel::Base* model);
Vector evaluateError(const gtsam::SOn& Q1, const gtsam::SOn& Q2);
};
//*************************************************************************
// Navigation
//*************************************************************************

12
gtsam/CMakeLists.txt
View File

@ -15,7 +15,7 @@ set (gtsam_subdirs
sfm
slam
smart
navigation
navigation
)
set(gtsam_srcs)
@ -186,11 +186,17 @@ install(
list(APPEND GTSAM_EXPORTED_TARGETS gtsam)
set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
# make sure that ccolamd compiles even in face of warnings
# Make sure that ccolamd compiles even in face of warnings
# and suppress all warnings from 3rd party code if Release build
if(WIN32)
set_source_files_properties(${3rdparty_srcs} PROPERTIES COMPILE_FLAGS "-w")
set_source_files_properties(${3rdparty_srcs} PROPERTIES COMPILE_FLAGS "/w")
else()
if("${CMAKE_BUILD_TYPE}" STREQUAL "Release")
# Suppress all warnings from 3rd party sources.
set_source_files_properties(${3rdparty_srcs} PROPERTIES COMPILE_FLAGS "-w")
else()
set_source_files_properties(${3rdparty_srcs} PROPERTIES COMPILE_FLAGS "-Wno-error")
endif()
endif()
# Create the matlab toolbox for the gtsam library

61
gtsam/base/Matrix.h
View File

@ -548,17 +548,47 @@ GTSAM_EXPORT Vector columnNormSquare(const Matrix &A);
namespace boost {
namespace serialization {
/**
* Ref. https://stackoverflow.com/questions/18382457/eigen-and-boostserialize/22903063#22903063
*
* Eigen supports calling resize() on both static and dynamic matrices.
* This allows for a uniform API, with resize having no effect if the static matrix
* is already the correct size.
* https://eigen.tuxfamily.org/dox/group__TutorialMatrixClass.html#TutorialMatrixSizesResizing
*
* We use all the Matrix template parameters to ensure wide compatibility.
*
* eigen_typekit in ROS uses the same code
* http://docs.ros.org/lunar/api/eigen_typekit/html/eigen__mqueue_8cpp_source.html
*/
// split version - sends sizes ahead
template<class Archive>
void save(Archive & ar, const gtsam::Matrix & m, unsigned int /*version*/) {
template<class Archive,
typename Scalar_,
int Rows_,
int Cols_,
int Ops_,
int MaxRows_,
int MaxCols_>
void save(Archive & ar,
const Eigen::Matrix<Scalar_, Rows_, Cols_, Ops_, MaxRows_, MaxCols_> & m,
const unsigned int /*version*/) {
const size_t rows = m.rows(), cols = m.cols();
ar << BOOST_SERIALIZATION_NVP(rows);
ar << BOOST_SERIALIZATION_NVP(cols);
ar << make_nvp("data", make_array(m.data(), m.size()));
}
template<class Archive>
void load(Archive & ar, gtsam::Matrix & m, unsigned int /*version*/) {
template<class Archive,
typename Scalar_,
int Rows_,
int Cols_,
int Ops_,
int MaxRows_,
int MaxCols_>
void load(Archive & ar,
Eigen::Matrix<Scalar_, Rows_, Cols_, Ops_, MaxRows_, MaxCols_> & m,
const unsigned int /*version*/) {
size_t rows, cols;
ar >> BOOST_SERIALIZATION_NVP(rows);
ar >> BOOST_SERIALIZATION_NVP(cols);
@ -566,8 +596,25 @@ namespace boost {
ar >> make_nvp("data", make_array(m.data(), m.size()));
}
// templated version of BOOST_SERIALIZATION_SPLIT_FREE(Eigen::Matrix);
template<class Archive,
typename Scalar_,
int Rows_,
int Cols_,
int Ops_,
int MaxRows_,
int MaxCols_>
void serialize(Archive & ar,
Eigen::Matrix<Scalar_, Rows_, Cols_, Ops_, MaxRows_, MaxCols_> & m,
const unsigned int version) {
split_free(ar, m, version);
}
// specialized to Matrix for MATLAB wrapper
template <class Archive>
void serialize(Archive& ar, gtsam::Matrix& m, const unsigned int version) {
split_free(ar, m, version);
}
} // namespace serialization
} // namespace boost
BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Matrix);

3
gtsam/base/types.h
View File

@ -28,6 +28,7 @@
#include <cstdint>
#include <exception>
#include <string>
#ifdef GTSAM_USE_TBB
#include <tbb/scalable_allocator.h>
@ -54,7 +55,7 @@
namespace gtsam {
/// Function to demangle type name of variable, e.g. demangle(typeid(x).name())
std::string demangle(const char* name);
std::string GTSAM_EXPORT demangle(const char* name);
/// Integer nonlinear key type
typedef std::uint64_t Key;

3
gtsam/discrete/DiscreteBayesNet.h
View File

@ -20,13 +20,14 @@
#include <vector>
#include <map>
#include <boost/shared_ptr.hpp>
#include <gtsam/inference/BayesNet.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/discrete/DiscreteConditional.h>
namespace gtsam {
/** A Bayes net made from linear-Discrete densities */
class GTSAM_EXPORT DiscreteBayesNet: public FactorGraph<DiscreteConditional>
class GTSAM_EXPORT DiscreteBayesNet: public BayesNet<DiscreteConditional>
{
public:

23
gtsam/discrete/DiscreteBayesTree.cpp
View File

@ -29,13 +29,32 @@ namespace gtsam {
template class BayesTreeCliqueBase<DiscreteBayesTreeClique, DiscreteFactorGraph>;
template class BayesTree<DiscreteBayesTreeClique>;
/* ************************************************************************* */
double DiscreteBayesTreeClique::evaluate(
const DiscreteConditional::Values& values) const {
// evaluate all conditionals and multiply
double result = (*conditional_)(values);
for (const auto& child : children) {
result *= child->evaluate(values);
}
return result;
}
/* ************************************************************************* */
bool DiscreteBayesTree::equals(const This& other, double tol) const
{
bool DiscreteBayesTree::equals(const This& other, double tol) const {
return Base::equals(other, tol);
}
/* ************************************************************************* */
double DiscreteBayesTree::evaluate(
const DiscreteConditional::Values& values) const {
double result = 1.0;
for (const auto& root : roots_) {
result *= root->evaluate(values);
}
return result;
}
} // \namespace gtsam

84
gtsam/discrete/DiscreteBayesTree.h
View File

@ -11,7 +11,8 @@
/**
* @file DiscreteBayesTree.h
* @brief Discrete Bayes Tree, the result of eliminating a DiscreteJunctionTree
* @brief Discrete Bayes Tree, the result of eliminating a
* DiscreteJunctionTree
* @brief DiscreteBayesTree
* @author Frank Dellaert
* @author Richard Roberts
@ -22,45 +23,62 @@
#include <gtsam/discrete/DiscreteBayesNet.h>
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/inference/BayesTree.h>
#include <gtsam/inference/Conditional.h>
#include <gtsam/inference/BayesTreeCliqueBase.h>
#include <string>
namespace gtsam {
// Forward declarations
class DiscreteConditional;
class VectorValues;
// Forward declarations
class DiscreteConditional;
class VectorValues;
/* ************************************************************************* */
/** A clique in a DiscreteBayesTree */
class GTSAM_EXPORT DiscreteBayesTreeClique :
public BayesTreeCliqueBase<DiscreteBayesTreeClique, DiscreteFactorGraph>
{
public:
typedef DiscreteBayesTreeClique This;
typedef BayesTreeCliqueBase<DiscreteBayesTreeClique, DiscreteFactorGraph> Base;
typedef boost::shared_ptr<This> shared_ptr;
typedef boost::weak_ptr<This> weak_ptr;
DiscreteBayesTreeClique() {}
DiscreteBayesTreeClique(const boost::shared_ptr<DiscreteConditional>& conditional) : Base(conditional) {}
};
/* ************************************************************************* */
/** A clique in a DiscreteBayesTree */
class GTSAM_EXPORT DiscreteBayesTreeClique
: public BayesTreeCliqueBase<DiscreteBayesTreeClique, DiscreteFactorGraph> {
public:
typedef DiscreteBayesTreeClique This;
typedef BayesTreeCliqueBase<DiscreteBayesTreeClique, DiscreteFactorGraph>
Base;
typedef boost::shared_ptr<This> shared_ptr;
typedef boost::weak_ptr<This> weak_ptr;
DiscreteBayesTreeClique() {}
DiscreteBayesTreeClique(
const boost::shared_ptr<DiscreteConditional>& conditional)
: Base(conditional) {}
/* ************************************************************************* */
/** A Bayes tree representing a Discrete density */
class GTSAM_EXPORT DiscreteBayesTree :
public BayesTree<DiscreteBayesTreeClique>
{
private:
typedef BayesTree<DiscreteBayesTreeClique> Base;
/// print index signature only
void printSignature(
const std::string& s = "Clique: ",
const KeyFormatter& formatter = DefaultKeyFormatter) const {
conditional_->printSignature(s, formatter);
}
public:
typedef DiscreteBayesTree This;
typedef boost::shared_ptr<This> shared_ptr;
//** evaluate conditional probability of subtree for given Values */
double evaluate(const DiscreteConditional::Values& values) const;
};
/** Default constructor, creates an empty Bayes tree */
DiscreteBayesTree() {}
/* ************************************************************************* */
/** A Bayes tree representing a Discrete density */
class GTSAM_EXPORT DiscreteBayesTree
: public BayesTree<DiscreteBayesTreeClique> {
private:
typedef BayesTree<DiscreteBayesTreeClique> Base;
/** Check equality */
bool equals(const This& other, double tol = 1e-9) const;
};
public:
typedef DiscreteBayesTree This;
typedef boost::shared_ptr<This> shared_ptr;
}
/** Default constructor, creates an empty Bayes tree */
DiscreteBayesTree() {}
/** Check equality */
bool equals(const This& other, double tol = 1e-9) const;
//** evaluate probability for given Values */
double evaluate(const DiscreteConditional::Values& values) const;
};
} // namespace gtsam

78
gtsam/discrete/DiscreteConditional.cpp
View File

@ -27,6 +27,7 @@
#include <algorithm>
#include <random>
#include <stdexcept>
#include <string>
#include <vector>
using namespace std;
@ -61,16 +62,26 @@ DiscreteConditional::DiscreteConditional(const DecisionTreeFactor& joint,
}
/* ******************************************************************************** */
DiscreteConditional::DiscreteConditional(const Signature& signature) :
BaseFactor(signature.discreteKeysParentsFirst(), signature.cpt()), BaseConditional(
1) {
}
DiscreteConditional::DiscreteConditional(const Signature& signature)
: BaseFactor(signature.discreteKeys(), signature.cpt()),
BaseConditional(1) {}
/* ******************************************************************************** */
void DiscreteConditional::print(const std::string& s,
const KeyFormatter& formatter) const {
std::cout << s << std::endl;
Potentials::print(s);
void DiscreteConditional::print(const string& s,
const KeyFormatter& formatter) const {
cout << s << " P( ";
for (const_iterator it = beginFrontals(); it != endFrontals(); ++it) {
cout << formatter(*it) << " ";
}
if (nrParents()) {
cout << "| ";
for (const_iterator it = beginParents(); it != endParents(); ++it) {
cout << formatter(*it) << " ";
}
}
cout << ")";
Potentials::print("");
cout << endl;
}
/* ******************************************************************************** */
@ -173,55 +184,28 @@ size_t DiscreteConditional::solve(const Values& parentsValues) const {
/* ******************************************************************************** */
size_t DiscreteConditional::sample(const Values& parentsValues) const {
static mt19937 rng(2); // random number generator
bool debug = ISDEBUG("DiscreteConditional::sample");
static mt19937 rng(2); // random number generator
// Get the correct conditional density
ADT pFS = choose(parentsValues); // P(F|S=parentsValues)
if (debug)
GTSAM_PRINT(pFS);
ADT pFS = choose(parentsValues); // P(F|S=parentsValues)
// get cumulative distribution function (cdf)
// TODO, only works for one key now, seems horribly slow this way
// TODO(Duy): only works for one key now, seems horribly slow this way
assert(nrFrontals() == 1);
Key j = (firstFrontalKey());
size_t nj = cardinality(j);
vector<double> cdf(nj);
Key key = firstFrontalKey();
size_t nj = cardinality(key);
vector<double> p(nj);
Values frontals;
double sum = 0;
for (size_t value = 0; value < nj; value++) {
frontals[j] = value;
double pValueS = pFS(frontals); // P(F=value|S=parentsValues)
sum += pValueS; // accumulate
if (debug)
cout << sum << " ";
if (pValueS == 1) {
if (debug)
cout << "--> " << value << endl;
return value; // shortcut exit
frontals[key] = value;
p[value] = pFS(frontals); // P(F=value|S=parentsValues)
if (p[value] == 1.0) {
return value; // shortcut exit
}
cdf[value] = sum;
}
// inspired by http://www.boost.org/doc/libs/1_46_1/doc/html/boost_random/tutorial.html
uniform_real_distribution<double> dist(0, cdf.back());
size_t sampled = lower_bound(cdf.begin(), cdf.end(), dist(rng)) - cdf.begin();
if (debug)
cout << "-> " << sampled << endl;
return sampled;
return 0;
std::discrete_distribution<size_t> distribution(p.begin(), p.end());
return distribution(rng);
}
/* ******************************************************************************** */
//void DiscreteConditional::permuteWithInverse(
// const Permutation& inversePermutation) {
// IndexConditionalOrdered::permuteWithInverse(inversePermutation);
// Potentials::permuteWithInverse(inversePermutation);
//}
/* ******************************************************************************** */
}// namespace

9
gtsam/discrete/DiscreteConditional.h
View File

@ -24,6 +24,8 @@
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include <string>
namespace gtsam {
/**
@ -92,6 +94,13 @@ public:
/// @name Standard Interface
/// @{
/// print index signature only
void printSignature(
const std::string& s = "Discrete Conditional: ",
const KeyFormatter& formatter = DefaultKeyFormatter) const {
static_cast<const BaseConditional*>(this)->print(s, formatter);
}
/// Evaluate, just look up in AlgebraicDecisonTree
virtual double operator()(const Values& values) const {
return Potentials::operator()(values);

68
gtsam/discrete/Potentials.cpp
View File

@ -15,50 +15,52 @@
* @author Frank Dellaert
*/
#include <gtsam/discrete/Potentials.h>
#include <gtsam/discrete/DecisionTree-inl.h>
#include <gtsam/discrete/Potentials.h>
#include <boost/format.hpp>
#include <string>
using namespace std;
namespace gtsam {
// explicit instantiation
template class DecisionTree<Key, double> ;
template class AlgebraicDecisionTree<Key> ;
// explicit instantiation
template class DecisionTree<Key, double>;
template class AlgebraicDecisionTree<Key>;
/* ************************************************************************* */
double Potentials::safe_div(const double& a, const double& b) {
// cout << boost::format("%g / %g = %g\n") % a % b % ((a == 0) ? 0 : (a / b));
// The use for safe_div is when we divide the product factor by the sum factor.
// If the product or sum is zero, we accord zero probability to the event.
return (a == 0 || b == 0) ? 0 : (a / b);
}
/* ************************************************************************* */
double Potentials::safe_div(const double& a, const double& b) {
// cout << boost::format("%g / %g = %g\n") % a % b % ((a == 0) ? 0 : (a / b));
// The use for safe_div is when we divide the product factor by the sum
// factor. If the product or sum is zero, we accord zero probability to the
// event.
return (a == 0 || b == 0) ? 0 : (a / b);
}
/* ******************************************************************************** */
Potentials::Potentials() :
ADT(1.0) {
}
/* ********************************************************************************
*/
Potentials::Potentials() : ADT(1.0) {}
/* ******************************************************************************** */
Potentials::Potentials(const DiscreteKeys& keys, const ADT& decisionTree) :
ADT(decisionTree), cardinalities_(keys.cardinalities()) {
}
/* ********************************************************************************
*/
Potentials::Potentials(const DiscreteKeys& keys, const ADT& decisionTree)
: ADT(decisionTree), cardinalities_(keys.cardinalities()) {}
/* ************************************************************************* */
bool Potentials::equals(const Potentials& other, double tol) const {
return ADT::equals(other, tol);
}
/* ************************************************************************* */
bool Potentials::equals(const Potentials& other, double tol) const {
return ADT::equals(other, tol);
}
/* ************************************************************************* */
void Potentials::print(const string& s,
const KeyFormatter& formatter) const {
cout << s << "\n Cardinalities: ";
for(const DiscreteKey& key: cardinalities_)
cout << formatter(key.first) << "=" << formatter(key.second) << " ";
cout << endl;
ADT::print(" ");
}
/* ************************************************************************* */
void Potentials::print(const string& s, const KeyFormatter& formatter) const {
cout << s << "\n Cardinalities: {";
for (const DiscreteKey& key : cardinalities_)
cout << formatter(key.first) << ":" << key.second << ", ";
cout << "}" << endl;
ADT::print(" ");
}
//
// /* ************************************************************************* */
// template<class P>
@ -95,4 +97,4 @@ namespace gtsam {
/* ************************************************************************* */
} // namespace gtsam
} // namespace gtsam

18
gtsam/discrete/Signature.cpp
View File

@ -122,28 +122,30 @@ namespace gtsam {
key_(key) {
}
DiscreteKeys Signature::discreteKeysParentsFirst() const {
DiscreteKeys Signature::discreteKeys() const {
DiscreteKeys keys;
for(const DiscreteKey& key: parents_)
keys.push_back(key);
keys.push_back(key_);
for (const DiscreteKey& key : parents_) keys.push_back(key);
return keys;
}
KeyVector Signature::indices() const {
KeyVector js;
js.push_back(key_.first);
for(const DiscreteKey& key: parents_)
js.push_back(key.first);
for (const DiscreteKey& key : parents_) js.push_back(key.first);
return js;
}
vector<double> Signature::cpt() const {
vector<double> cpt;
if (table_) {
for(const Row& row: *table_)
for(const double& x: row)
cpt.push_back(x);
const size_t nrStates = table_->at(0).size();
for (size_t j = 0; j < nrStates; j++) {
for (const Row& row : *table_) {
assert(row.size() == nrStates);
cpt.push_back(row[j]);
}
}
}
return cpt;
}

4
gtsam/discrete/Signature.h
View File

@ -86,8 +86,8 @@ namespace gtsam {
return parents_;
}
/** All keys, with variable key last */
DiscreteKeys discreteKeysParentsFirst() const;
/** All keys, with variable key first */
DiscreteKeys discreteKeys() const;
/** All key indices, with variable key first */
KeyVector indices() const;

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

@ -132,7 +132,7 @@ TEST(ADT, example3)
/** Convert Signature into CPT */
ADT create(const Signature& signature) {
ADT p(signature.discreteKeysParentsFirst(), signature.cpt());
ADT p(signature.discreteKeys(), signature.cpt());
static size_t count = 0;
const DiscreteKey& key = signature.key();
string dotfile = (boost::format("CPT-%03d-%d") % ++count % key.first).str();
@ -181,19 +181,20 @@ TEST(ADT, joint)
dot(joint, "Asia-ASTLBEX");
joint = apply(joint, pD, &mul);
dot(joint, "Asia-ASTLBEXD");
EXPECT_LONGS_EQUAL(346, (long)muls);
EXPECT_LONGS_EQUAL(346, muls);
gttoc_(asiaJoint);
tictoc_getNode(asiaJointNode, asiaJoint);
elapsed = asiaJointNode->secs() + asiaJointNode->wall();
tictoc_reset_();
printCounts("Asia joint");
// Form P(A,S,T,L) = P(A) P(S) P(T|A) P(L|S)
ADT pASTL = pA;
pASTL = apply(pASTL, pS, &mul);
pASTL = apply(pASTL, pT, &mul);
pASTL = apply(pASTL, pL, &mul);
// test combine
// test combine to check that P(A) = \sum_{S,T,L} P(A,S,T,L)
ADT fAa = pASTL.combine(L, &add_).combine(T, &add_).combine(S, &add_);
EXPECT(assert_equal(pA, fAa));
ADT fAb = pASTL.combine(S, &add_).combine(T, &add_).combine(L, &add_);

126
gtsam/discrete/tests/testDiscreteBayesNet.cpp
View File

@ -18,110 +18,135 @@
#include <gtsam/discrete/DiscreteBayesNet.h>
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/base/Testable.h>
#include <gtsam/discrete/DiscreteMarginals.h>
#include <gtsam/base/debug.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Vector.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/map.hpp>
#include <boost/assign/list_inserter.hpp>
#include <boost/assign/std/map.hpp>
using namespace boost::assign;
#include <iostream>
#include <string>
#include <vector>
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
TEST(DiscreteBayesNet, Asia)
{
TEST(DiscreteBayesNet, bayesNet) {
DiscreteBayesNet bayesNet;
DiscreteKey Parent(0, 2), Child(1, 2);
auto prior = boost::make_shared<DiscreteConditional>(Parent % "6/4");
CHECK(assert_equal(Potentials::ADT({Parent}, "0.6 0.4"),
(Potentials::ADT)*prior));
bayesNet.push_back(prior);
auto conditional =
boost::make_shared<DiscreteConditional>(Child | Parent = "7/3 8/2");
EXPECT_LONGS_EQUAL(1, *(conditional->beginFrontals()));
Potentials::ADT expected(Child & Parent, "0.7 0.8 0.3 0.2");
CHECK(assert_equal(expected, (Potentials::ADT)*conditional));
bayesNet.push_back(conditional);
DiscreteFactorGraph fg(bayesNet);
LONGS_EQUAL(2, fg.back()->size());
// Check the marginals
const double expectedMarginal[2]{0.4, 0.6 * 0.3 + 0.4 * 0.2};
DiscreteMarginals marginals(fg);
for (size_t j = 0; j < 2; j++) {
Vector FT = marginals.marginalProbabilities(DiscreteKey(j, 2));
EXPECT_DOUBLES_EQUAL(expectedMarginal[j], FT[1], 1e-3);
EXPECT_DOUBLES_EQUAL(FT[0], 1.0 - FT[1], 1e-9);
}
}
/* ************************************************************************* */
TEST(DiscreteBayesNet, Asia) {
DiscreteBayesNet asia;
// DiscreteKey A("Asia"), S("Smoking"), T("Tuberculosis"), L("LungCancer"), B(
// "Bronchitis"), E("Either"), X("XRay"), D("Dyspnoea");
DiscreteKey A(0,2), S(4,2), T(3,2), L(6,2), B(7,2), E(5,2), X(2,2), D(1,2);
DiscreteKey Asia(0, 2), Smoking(4, 2), Tuberculosis(3, 2), LungCancer(6, 2),
Bronchitis(7, 2), Either(5, 2), XRay(2, 2), Dyspnea(1, 2);
// TODO: make a version that doesn't use the parser
asia.add(A % "99/1");
asia.add(S % "50/50");
asia.add(Asia % "99/1");
asia.add(Smoking % "50/50");
asia.add(T | A = "99/1 95/5");
asia.add(L | S = "99/1 90/10");
asia.add(B | S = "70/30 40/60");
asia.add(Tuberculosis | Asia = "99/1 95/5");
asia.add(LungCancer | Smoking = "99/1 90/10");
asia.add(Bronchitis | Smoking = "70/30 40/60");
asia.add((E | T, L) = "F T T T");
asia.add((Either | Tuberculosis, LungCancer) = "F T T T");
asia.add(X | E = "95/5 2/98");
// next lines are same as asia.add((D | E, B) = "9/1 2/8 3/7 1/9");
DiscreteConditional::shared_ptr actual =
boost::make_shared<DiscreteConditional>((D | E, B) = "9/1 2/8 3/7 1/9");
asia.push_back(actual);
// GTSAM_PRINT(asia);
asia.add(XRay | Either = "95/5 2/98");
asia.add((Dyspnea | Either, Bronchitis) = "9/1 2/8 3/7 1/9");
// Convert to factor graph
DiscreteFactorGraph fg(asia);
// GTSAM_PRINT(fg);
LONGS_EQUAL(3,fg.back()->size());
Potentials::ADT expected(B & D & E, "0.9 0.3 0.1 0.7 0.2 0.1 0.8 0.9");
CHECK(assert_equal(expected,(Potentials::ADT)*actual));
LONGS_EQUAL(3, fg.back()->size());
// Check the marginals we know (of the parent-less nodes)
DiscreteMarginals marginals(fg);
Vector2 va(0.99, 0.01), vs(0.5, 0.5);
EXPECT(assert_equal(va, marginals.marginalProbabilities(Asia)));
EXPECT(assert_equal(vs, marginals.marginalProbabilities(Smoking)));
// Create solver and eliminate
Ordering ordering;
ordering += Key(0),Key(1),Key(2),Key(3),Key(4),Key(5),Key(6),Key(7);
ordering += Key(0), Key(1), Key(2), Key(3), Key(4), Key(5), Key(6), Key(7);
DiscreteBayesNet::shared_ptr chordal = fg.eliminateSequential(ordering);
// GTSAM_PRINT(*chordal);
DiscreteConditional expected2(B % "11/9");
CHECK(assert_equal(expected2,*chordal->back()));
DiscreteConditional expected2(Bronchitis % "11/9");
EXPECT(assert_equal(expected2, *chordal->back()));
// solve
DiscreteFactor::sharedValues actualMPE = chordal->optimize();
DiscreteFactor::Values expectedMPE;
insert(expectedMPE)(A.first, 0)(D.first, 0)(X.first, 0)(T.first, 0)(S.first,
0)(E.first, 0)(L.first, 0)(B.first, 0);
insert(expectedMPE)(Asia.first, 0)(Dyspnea.first, 0)(XRay.first, 0)(
Tuberculosis.first, 0)(Smoking.first, 0)(Either.first, 0)(
LungCancer.first, 0)(Bronchitis.first, 0);
EXPECT(assert_equal(expectedMPE, *actualMPE));
// add evidence, we were in Asia and we have Dispnoea
fg.add(A, "0 1");
fg.add(D, "0 1");
// fg.product().dot("fg");
// add evidence, we were in Asia and we have dyspnea
fg.add(Asia, "0 1");
fg.add(Dyspnea, "0 1");
// solve again, now with evidence
DiscreteBayesNet::shared_ptr chordal2 = fg.eliminateSequential(ordering);
// GTSAM_PRINT(*chordal2);
DiscreteFactor::sharedValues actualMPE2 = chordal2->optimize();
DiscreteFactor::Values expectedMPE2;
insert(expectedMPE2)(A.first, 1)(D.first, 1)(X.first, 0)(T.first, 0)(S.first,
1)(E.first, 0)(L.first, 0)(B.first, 1);
insert(expectedMPE2)(Asia.first, 1)(Dyspnea.first, 1)(XRay.first, 0)(
Tuberculosis.first, 0)(Smoking.first, 1)(Either.first, 0)(
LungCancer.first, 0)(Bronchitis.first, 1);
EXPECT(assert_equal(expectedMPE2, *actualMPE2));
// now sample from it
DiscreteFactor::Values expectedSample;
SETDEBUG("DiscreteConditional::sample", false);
insert(expectedSample)(A.first, 1)(D.first, 1)(X.first, 1)(T.first, 0)(
S.first, 1)(E.first, 1)(L.first, 1)(B.first, 0);
insert(expectedSample)(Asia.first, 1)(Dyspnea.first, 1)(XRay.first, 1)(
Tuberculosis.first, 0)(Smoking.first, 1)(Either.first, 1)(
LungCancer.first, 1)(Bronchitis.first, 0);
DiscreteFactor::sharedValues actualSample = chordal2->sample();
EXPECT(assert_equal(expectedSample, *actualSample));
}
/* ************************************************************************* */
TEST_UNSAFE(DiscreteBayesNet, Sugar)
{
DiscreteKey T(0,2), L(1,2), E(2,2), D(3,2), C(8,3), S(7,2);
TEST_UNSAFE(DiscreteBayesNet, Sugar) {
DiscreteKey T(0, 2), L(1, 2), E(2, 2), C(8, 3), S(7, 2);
DiscreteBayesNet bn;
// test some mistakes
// add(bn, D);
// add(bn, D | E);
// add(bn, D | E = "blah");
// try logic
bn.add((E | T, L) = "OR");
bn.add((E | T, L) = "AND");
// // try multivalued
bn.add(C % "1/1/2");
bn.add(C | S = "1/1/2 5/2/3");
// try multivalued
bn.add(C % "1/1/2");
bn.add(C | S = "1/1/2 5/2/3");
}
/* ************************************************************************* */
@ -130,4 +155,3 @@ int main() {
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

482
gtsam/discrete/tests/testDiscreteBayesTree.cpp
View File

@ -1,261 +1,228 @@
///* ----------------------------------------------------------------------------
//
// * 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 testDiscreteBayesTree.cpp
// * @date sept 15, 2012
// * @author Frank Dellaert
// */
//
//#include <gtsam/discrete/DiscreteBayesNet.h>
//#include <gtsam/discrete/DiscreteBayesTree.h>
//#include <gtsam/discrete/DiscreteFactorGraph.h>
//
//#include <boost/assign/std/vector.hpp>
//using namespace boost::assign;
//
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2020, 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 testDiscreteBayesTree.cpp
* @date sept 15, 2012
* @author Frank Dellaert
*/
#include <gtsam/base/Vector.h>
#include <gtsam/discrete/DiscreteBayesNet.h>
#include <gtsam/discrete/DiscreteBayesTree.h>
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/inference/BayesNet.h>
#include <boost/assign/std/vector.hpp>
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
//
//using namespace std;
//using namespace gtsam;
//
//static bool debug = false;
//
///**
// * Custom clique class to debug shortcuts
// */
////class Clique: public BayesTreeCliqueBaseOrdered<Clique, DiscreteConditional> {
////
////protected:
////
////public:
////
//// typedef BayesTreeCliqueBaseOrdered<Clique, DiscreteConditional> Base;
//// typedef boost::shared_ptr<Clique> shared_ptr;
////
//// // Constructors
//// Clique() {
//// }
//// Clique(const DiscreteConditional::shared_ptr& conditional) :
//// Base(conditional) {
//// }
//// Clique(
//// const std::pair<DiscreteConditional::shared_ptr,
//// DiscreteConditional::FactorType::shared_ptr>& result) :
//// Base(result) {
//// }
////
//// /// print index signature only
//// void printSignature(const std::string& s = "Clique: ",
//// const KeyFormatter& indexFormatter = DefaultKeyFormatter) const {
//// ((IndexConditionalOrdered::shared_ptr) conditional_)->print(s, indexFormatter);
//// }
////
//// /// evaluate value of sub-tree
//// double evaluate(const DiscreteConditional::Values & values) {
//// double result = (*(this->conditional_))(values);
//// // evaluate all children and multiply into result
//// for(boost::shared_ptr<Clique> c: children_)
//// result *= c->evaluate(values);
//// return result;
//// }
////
////};
//
////typedef BayesTreeOrdered<DiscreteConditional, Clique> DiscreteBayesTree;
////
/////* ************************************************************************* */
////double evaluate(const DiscreteBayesTree& tree,
//// const DiscreteConditional::Values & values) {
//// return tree.root()->evaluate(values);
////}
//
///* ************************************************************************* */
//
//TEST_UNSAFE( DiscreteBayesTree, thinTree ) {
//
// const int nrNodes = 15;
// const size_t nrStates = 2;
//
// // define variables
// vector<DiscreteKey> key;
// for (int i = 0; i < nrNodes; i++) {
// DiscreteKey key_i(i, nrStates);
// key.push_back(key_i);
// }
//
// // create a thin-tree Bayesnet, a la Jean-Guillaume
// DiscreteBayesNet bayesNet;
// bayesNet.add(key[14] % "1/3");
//
// bayesNet.add(key[13] | key[14] = "1/3 3/1");
// bayesNet.add(key[12] | key[14] = "3/1 3/1");
//
// bayesNet.add((key[11] | key[13], key[14]) = "1/4 2/3 3/2 4/1");
// bayesNet.add((key[10] | key[13], key[14]) = "1/4 3/2 2/3 4/1");
// bayesNet.add((key[9] | key[12], key[14]) = "4/1 2/3 F 1/4");
// bayesNet.add((key[8] | key[12], key[14]) = "T 1/4 3/2 4/1");
//
// bayesNet.add((key[7] | key[11], key[13]) = "1/4 2/3 3/2 4/1");
// bayesNet.add((key[6] | key[11], key[13]) = "1/4 3/2 2/3 4/1");
// bayesNet.add((key[5] | key[10], key[13]) = "4/1 2/3 3/2 1/4");
// bayesNet.add((key[4] | key[10], key[13]) = "2/3 1/4 3/2 4/1");
//
// bayesNet.add((key[3] | key[9], key[12]) = "1/4 2/3 3/2 4/1");
// bayesNet.add((key[2] | key[9], key[12]) = "1/4 8/2 2/3 4/1");
// bayesNet.add((key[1] | key[8], key[12]) = "4/1 2/3 3/2 1/4");
// bayesNet.add((key[0] | key[8], key[12]) = "2/3 1/4 3/2 4/1");
//
//// if (debug) {
//// GTSAM_PRINT(bayesNet);
//// bayesNet.saveGraph("/tmp/discreteBayesNet.dot");
//// }
//
// // create a BayesTree out of a Bayes net
// DiscreteBayesTree bayesTree(bayesNet);
// if (debug) {
// GTSAM_PRINT(bayesTree);
// bayesTree.saveGraph("/tmp/discreteBayesTree.dot");
// }
//
// // Check whether BN and BT give the same answer on all configurations
// // Also calculate all some marginals
// Vector marginals = zero(15);
// double joint_12_14 = 0, joint_9_12_14 = 0, joint_8_12_14 = 0, joint_8_12 = 0,
// joint82 = 0, joint12 = 0, joint24 = 0, joint45 = 0, joint46 = 0,
// joint_4_11 = 0;
// vector<DiscreteFactor::Values> allPosbValues = cartesianProduct(
// key[0] & key[1] & key[2] & key[3] & key[4] & key[5] & key[6] & key[7]
// & key[8] & key[9] & key[10] & key[11] & key[12] & key[13] & key[14]);
// for (size_t i = 0; i < allPosbValues.size(); ++i) {
// DiscreteFactor::Values x = allPosbValues[i];
// double expected = evaluate(bayesNet, x);
// double actual = evaluate(bayesTree, x);
// DOUBLES_EQUAL(expected, actual, 1e-9);
// // collect marginals
// for (size_t i = 0; i < 15; i++)
// if (x[i])
// marginals[i] += actual;
// // calculate shortcut 8 and 0
// if (x[12] && x[14])
// joint_12_14 += actual;
// if (x[9] && x[12] & x[14])
// joint_9_12_14 += actual;
// if (x[8] && x[12] & x[14])
// joint_8_12_14 += actual;
// if (x[8] && x[12])
// joint_8_12 += actual;
// if (x[8] && x[2])
// joint82 += actual;
// if (x[1] && x[2])
// joint12 += actual;
// if (x[2] && x[4])
// joint24 += actual;
// if (x[4] && x[5])
// joint45 += actual;
// if (x[4] && x[6])
// joint46 += actual;
// if (x[4] && x[11])
// joint_4_11 += actual;
// }
// DiscreteFactor::Values all1 = allPosbValues.back();
//
// Clique::shared_ptr R = bayesTree.root();
//
// // check separator marginal P(S0)
// Clique::shared_ptr c = bayesTree[0];
// DiscreteFactorGraph separatorMarginal0 = c->separatorMarginal(R,
// EliminateDiscrete);
// EXPECT_DOUBLES_EQUAL(joint_8_12, separatorMarginal0(all1), 1e-9);
//
// // check separator marginal P(S9), should be P(14)
// c = bayesTree[9];
// DiscreteFactorGraph separatorMarginal9 = c->separatorMarginal(R,
// EliminateDiscrete);
// EXPECT_DOUBLES_EQUAL(marginals[14], separatorMarginal9(all1), 1e-9);
//
// // check separator marginal of root, should be empty
// c = bayesTree[11];
// DiscreteFactorGraph separatorMarginal11 = c->separatorMarginal(R,
// EliminateDiscrete);
// EXPECT_LONGS_EQUAL(0, separatorMarginal11.size());
//
// // check shortcut P(S9||R) to root
// c = bayesTree[9];
// DiscreteBayesNet shortcut = c->shortcut(R, EliminateDiscrete);
// EXPECT_LONGS_EQUAL(0, shortcut.size());
//
// // check shortcut P(S8||R) to root
// c = bayesTree[8];
// shortcut = c->shortcut(R, EliminateDiscrete);
// EXPECT_DOUBLES_EQUAL(joint_12_14/marginals[14], evaluate(shortcut,all1),
// 1e-9);
//
// // check shortcut P(S2||R) to root
// c = bayesTree[2];
// shortcut = c->shortcut(R, EliminateDiscrete);
// EXPECT_DOUBLES_EQUAL(joint_9_12_14/marginals[14], evaluate(shortcut,all1),
// 1e-9);
//
// // check shortcut P(S0||R) to root
// c = bayesTree[0];
// shortcut = c->shortcut(R, EliminateDiscrete);
// EXPECT_DOUBLES_EQUAL(joint_8_12_14/marginals[14], evaluate(shortcut,all1),
// 1e-9);
//
// // calculate all shortcuts to root
// DiscreteBayesTree::Nodes cliques = bayesTree.nodes();
// for(Clique::shared_ptr c: cliques) {
// DiscreteBayesNet shortcut = c->shortcut(R, EliminateDiscrete);
// if (debug) {
// c->printSignature();
// shortcut.print("shortcut:");
// }
// }
//
// // Check all marginals
// DiscreteFactor::shared_ptr marginalFactor;
// for (size_t i = 0; i < 15; i++) {
// marginalFactor = bayesTree.marginalFactor(i, EliminateDiscrete);
// double actual = (*marginalFactor)(all1);
// EXPECT_DOUBLES_EQUAL(marginals[i], actual, 1e-9);
// }
//
// DiscreteBayesNet::shared_ptr actualJoint;
//
// // Check joint P(8,2) TODO: not disjoint !
//// actualJoint = bayesTree.jointBayesNet(8, 2, EliminateDiscrete);
//// EXPECT_DOUBLES_EQUAL(joint82, evaluate(*actualJoint,all1), 1e-9);
//
// // Check joint P(1,2) TODO: not disjoint !
//// actualJoint = bayesTree.jointBayesNet(1, 2, EliminateDiscrete);
//// EXPECT_DOUBLES_EQUAL(joint12, evaluate(*actualJoint,all1), 1e-9);
//
// // Check joint P(2,4)
// actualJoint = bayesTree.jointBayesNet(2, 4, EliminateDiscrete);
// EXPECT_DOUBLES_EQUAL(joint24, evaluate(*actualJoint,all1), 1e-9);
//
// // Check joint P(4,5) TODO: not disjoint !
//// actualJoint = bayesTree.jointBayesNet(4, 5, EliminateDiscrete);
//// EXPECT_DOUBLES_EQUAL(joint46, evaluate(*actualJoint,all1), 1e-9);
//
// // Check joint P(4,6) TODO: not disjoint !
//// actualJoint = bayesTree.jointBayesNet(4, 6, EliminateDiscrete);
//// EXPECT_DOUBLES_EQUAL(joint46, evaluate(*actualJoint,all1), 1e-9);
//
// // Check joint P(4,11)
// actualJoint = bayesTree.jointBayesNet(4, 11, EliminateDiscrete);
// EXPECT_DOUBLES_EQUAL(joint_4_11, evaluate(*actualJoint,all1), 1e-9);
//
//}
#include <vector>
using namespace std;
using namespace gtsam;
static bool debug = false;
/* ************************************************************************* */
TEST_UNSAFE(DiscreteBayesTree, ThinTree) {
const int nrNodes = 15;
const size_t nrStates = 2;
// define variables
vector<DiscreteKey> key;
for (int i = 0; i < nrNodes; i++) {
DiscreteKey key_i(i, nrStates);
key.push_back(key_i);
}
// create a thin-tree Bayesnet, a la Jean-Guillaume
DiscreteBayesNet bayesNet;
bayesNet.add(key[14] % "1/3");
bayesNet.add(key[13] | key[14] = "1/3 3/1");
bayesNet.add(key[12] | key[14] = "3/1 3/1");
bayesNet.add((key[11] | key[13], key[14]) = "1/4 2/3 3/2 4/1");
bayesNet.add((key[10] | key[13], key[14]) = "1/4 3/2 2/3 4/1");
bayesNet.add((key[9] | key[12], key[14]) = "4/1 2/3 F 1/4");
bayesNet.add((key[8] | key[12], key[14]) = "T 1/4 3/2 4/1");
bayesNet.add((key[7] | key[11], key[13]) = "1/4 2/3 3/2 4/1");
bayesNet.add((key[6] | key[11], key[13]) = "1/4 3/2 2/3 4/1");
bayesNet.add((key[5] | key[10], key[13]) = "4/1 2/3 3/2 1/4");
bayesNet.add((key[4] | key[10], key[13]) = "2/3 1/4 3/2 4/1");
bayesNet.add((key[3] | key[9], key[12]) = "1/4 2/3 3/2 4/1");
bayesNet.add((key[2] | key[9], key[12]) = "1/4 8/2 2/3 4/1");
bayesNet.add((key[1] | key[8], key[12]) = "4/1 2/3 3/2 1/4");
bayesNet.add((key[0] | key[8], key[12]) = "2/3 1/4 3/2 4/1");
if (debug) {
GTSAM_PRINT(bayesNet);
bayesNet.saveGraph("/tmp/discreteBayesNet.dot");
}
// create a BayesTree out of a Bayes net
auto bayesTree = DiscreteFactorGraph(bayesNet).eliminateMultifrontal();
if (debug) {
GTSAM_PRINT(*bayesTree);
bayesTree->saveGraph("/tmp/discreteBayesTree.dot");
}
// Check frontals and parents
for (size_t i : {13, 14, 9, 3, 2, 8, 1, 0, 10, 5, 4}) {
auto clique_i = (*bayesTree)[i];
EXPECT_LONGS_EQUAL(i, *(clique_i->conditional_->beginFrontals()));
}
auto R = bayesTree->roots().front();
// Check whether BN and BT give the same answer on all configurations
vector<DiscreteFactor::Values> allPosbValues = cartesianProduct(
key[0] & key[1] & key[2] & key[3] & key[4] & key[5] & key[6] & key[7] &
key[8] & key[9] & key[10] & key[11] & key[12] & key[13] & key[14]);
for (size_t i = 0; i < allPosbValues.size(); ++i) {
DiscreteFactor::Values x = allPosbValues[i];
double expected = bayesNet.evaluate(x);
double actual = bayesTree->evaluate(x);
DOUBLES_EQUAL(expected, actual, 1e-9);
}
// Calculate all some marginals for Values==all1
Vector marginals = Vector::Zero(15);
double joint_12_14 = 0, joint_9_12_14 = 0, joint_8_12_14 = 0, joint_8_12 = 0,
joint82 = 0, joint12 = 0, joint24 = 0, joint45 = 0, joint46 = 0,
joint_4_11 = 0, joint_11_13 = 0, joint_11_13_14 = 0,
joint_11_12_13_14 = 0, joint_9_11_12_13 = 0, joint_8_11_12_13 = 0;
for (size_t i = 0; i < allPosbValues.size(); ++i) {
DiscreteFactor::Values x = allPosbValues[i];
double px = bayesTree->evaluate(x);
for (size_t i = 0; i < 15; i++)
if (x[i]) marginals[i] += px;
if (x[12] && x[14]) {
joint_12_14 += px;
if (x[9]) joint_9_12_14 += px;
if (x[8]) joint_8_12_14 += px;
}
if (x[8] && x[12]) joint_8_12 += px;
if (x[2]) {
if (x[8]) joint82 += px;
if (x[1]) joint12 += px;
}
if (x[4]) {
if (x[2]) joint24 += px;
if (x[5]) joint45 += px;
if (x[6]) joint46 += px;
if (x[11]) joint_4_11 += px;
}
if (x[11] && x[13]) {
joint_11_13 += px;
if (x[8] && x[12]) joint_8_11_12_13 += px;
if (x[9] && x[12]) joint_9_11_12_13 += px;
if (x[14]) {
joint_11_13_14 += px;
if (x[12]) {
joint_11_12_13_14 += px;
}
}
}
}
DiscreteFactor::Values all1 = allPosbValues.back();
// check separator marginal P(S0)
auto clique = (*bayesTree)[0];
DiscreteFactorGraph separatorMarginal0 =
clique->separatorMarginal(EliminateDiscrete);
DOUBLES_EQUAL(joint_8_12, separatorMarginal0(all1), 1e-9);
// check separator marginal P(S9), should be P(14)
clique = (*bayesTree)[9];
DiscreteFactorGraph separatorMarginal9 =
clique->separatorMarginal(EliminateDiscrete);
DOUBLES_EQUAL(marginals[14], separatorMarginal9(all1), 1e-9);
// check separator marginal of root, should be empty
clique = (*bayesTree)[11];
DiscreteFactorGraph separatorMarginal11 =
clique->separatorMarginal(EliminateDiscrete);
LONGS_EQUAL(0, separatorMarginal11.size());
// check shortcut P(S9||R) to root
clique = (*bayesTree)[9];
DiscreteBayesNet shortcut = clique->shortcut(R, EliminateDiscrete);
LONGS_EQUAL(1, shortcut.size());
DOUBLES_EQUAL(joint_11_13_14 / joint_11_13, shortcut.evaluate(all1), 1e-9);
// check shortcut P(S8||R) to root
clique = (*bayesTree)[8];
shortcut = clique->shortcut(R, EliminateDiscrete);
DOUBLES_EQUAL(joint_11_12_13_14 / joint_11_13, shortcut.evaluate(all1), 1e-9);
// check shortcut P(S2||R) to root
clique = (*bayesTree)[2];
shortcut = clique->shortcut(R, EliminateDiscrete);
DOUBLES_EQUAL(joint_9_11_12_13 / joint_11_13, shortcut.evaluate(all1), 1e-9);
// check shortcut P(S0||R) to root
clique = (*bayesTree)[0];
shortcut = clique->shortcut(R, EliminateDiscrete);
DOUBLES_EQUAL(joint_8_11_12_13 / joint_11_13, shortcut.evaluate(all1), 1e-9);
// calculate all shortcuts to root
DiscreteBayesTree::Nodes cliques = bayesTree->nodes();
for (auto clique : cliques) {
DiscreteBayesNet shortcut = clique.second->shortcut(R, EliminateDiscrete);
if (debug) {
clique.second->conditional_->printSignature();
shortcut.print("shortcut:");
}
}
// Check all marginals
DiscreteFactor::shared_ptr marginalFactor;
for (size_t i = 0; i < 15; i++) {
marginalFactor = bayesTree->marginalFactor(i, EliminateDiscrete);
double actual = (*marginalFactor)(all1);
DOUBLES_EQUAL(marginals[i], actual, 1e-9);
}
DiscreteBayesNet::shared_ptr actualJoint;
// Check joint P(8, 2)
actualJoint = bayesTree->jointBayesNet(8, 2, EliminateDiscrete);
DOUBLES_EQUAL(joint82, actualJoint->evaluate(all1), 1e-9);
// Check joint P(1, 2)
actualJoint = bayesTree->jointBayesNet(1, 2, EliminateDiscrete);
DOUBLES_EQUAL(joint12, actualJoint->evaluate(all1), 1e-9);
// Check joint P(2, 4)
actualJoint = bayesTree->jointBayesNet(2, 4, EliminateDiscrete);
DOUBLES_EQUAL(joint24, actualJoint->evaluate(all1), 1e-9);
// Check joint P(4, 5)
actualJoint = bayesTree->jointBayesNet(4, 5, EliminateDiscrete);
DOUBLES_EQUAL(joint45, actualJoint->evaluate(all1), 1e-9);
// Check joint P(4, 6)
actualJoint = bayesTree->jointBayesNet(4, 6, EliminateDiscrete);
DOUBLES_EQUAL(joint46, actualJoint->evaluate(all1), 1e-9);
// Check joint P(4, 11)
actualJoint = bayesTree->jointBayesNet(4, 11, EliminateDiscrete);
DOUBLES_EQUAL(joint_4_11, actualJoint->evaluate(all1), 1e-9);
}
/* ************************************************************************* */
int main() {
@ -263,4 +230,3 @@ int main() {
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

BIN
gtsam/discrete/tests/testDiscreteBayesTree.pdf Normal file
View File

Binary file not shown.

76
gtsam/discrete/tests/testDiscreteConditional.cpp
View File

@ -16,9 +16,9 @@
* @date Feb 14, 2011
*/
#include <boost/make_shared.hpp>
#include <boost/assign/std/map.hpp>
#include <boost/assign/std/vector.hpp>
#include <boost/make_shared.hpp>
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
@ -36,6 +36,11 @@ TEST( DiscreteConditional, constructors)
DiscreteConditional::shared_ptr expected1 = //
boost::make_shared<DiscreteConditional>(X | Y = "1/1 2/3 1/4");
EXPECT(expected1);
EXPECT_LONGS_EQUAL(0, *(expected1->beginFrontals()));
EXPECT_LONGS_EQUAL(2, *(expected1->beginParents()));
EXPECT(expected1->endParents() == expected1->end());
EXPECT(expected1->endFrontals() == expected1->beginParents());
DecisionTreeFactor f1(X & Y, "0.5 0.4 0.2 0.5 0.6 0.8");
DiscreteConditional actual1(1, f1);
EXPECT(assert_equal(*expected1, actual1, 1e-9));
@ -43,71 +48,68 @@ TEST( DiscreteConditional, constructors)
DecisionTreeFactor f2(X & Y & Z,
"0.2 0.5 0.3 0.6 0.4 0.7 0.25 0.55 0.35 0.65 0.45 0.75");
DiscreteConditional actual2(1, f2);
DecisionTreeFactor::shared_ptr actual2factor = actual2.toFactor();
// EXPECT(assert_equal(f2, *actual2factor, 1e-9));
EXPECT(assert_equal(f2 / *f2.sum(1), *actual2.toFactor(), 1e-9));
}
/* ************************************************************************* */
TEST( DiscreteConditional, constructors_alt_interface)
{
DiscreteKey X(0, 2), Y(2, 3), Z(1, 2); // watch ordering !
TEST(DiscreteConditional, constructors_alt_interface) {
DiscreteKey X(0, 2), Y(2, 3), Z(1, 2); // watch ordering !
Signature::Table table;
Signature::Row r1, r2, r3;
r1 += 1.0, 1.0; r2 += 2.0, 3.0; r3 += 1.0, 4.0;
r1 += 1.0, 1.0;
r2 += 2.0, 3.0;
r3 += 1.0, 4.0;
table += r1, r2, r3;
DiscreteConditional::shared_ptr expected1 = //
boost::make_shared<DiscreteConditional>(X | Y = table);
EXPECT(expected1);
auto actual1 = boost::make_shared<DiscreteConditional>(X | Y = table);
EXPECT(actual1);
DecisionTreeFactor f1(X & Y, "0.5 0.4 0.2 0.5 0.6 0.8");
DiscreteConditional actual1(1, f1);
EXPECT(assert_equal(*expected1, actual1, 1e-9));
DiscreteConditional expected1(1, f1);
EXPECT(assert_equal(expected1, *actual1, 1e-9));
DecisionTreeFactor f2(X & Y & Z,
"0.2 0.5 0.3 0.6 0.4 0.7 0.25 0.55 0.35 0.65 0.45 0.75");
DecisionTreeFactor f2(
X & Y & Z, "0.2 0.5 0.3 0.6 0.4 0.7 0.25 0.55 0.35 0.65 0.45 0.75");
DiscreteConditional actual2(1, f2);
DecisionTreeFactor::shared_ptr actual2factor = actual2.toFactor();
// EXPECT(assert_equal(f2, *actual2factor, 1e-9));
EXPECT(assert_equal(f2 / *f2.sum(1), *actual2.toFactor(), 1e-9));
}
/* ************************************************************************* */
TEST( DiscreteConditional, constructors2)
{
TEST(DiscreteConditional, constructors2) {
// Declare keys and ordering
DiscreteKey C(0,2), B(1,2);
DecisionTreeFactor expected(C & B, "0.8 0.75 0.2 0.25");
DiscreteKey C(0, 2), B(1, 2);
DecisionTreeFactor actual(C & B, "0.8 0.75 0.2 0.25");
Signature signature((C | B) = "4/1 3/1");
DiscreteConditional actual(signature);
DecisionTreeFactor::shared_ptr actualFactor = actual.toFactor();
EXPECT(assert_equal(expected, *actualFactor));
DiscreteConditional expected(signature);
DecisionTreeFactor::shared_ptr expectedFactor = expected.toFactor();
EXPECT(assert_equal(*expectedFactor, actual));
}
/* ************************************************************************* */
TEST( DiscreteConditional, constructors3)
{
TEST(DiscreteConditional, constructors3) {
// Declare keys and ordering
DiscreteKey C(0,2), B(1,2), A(2,2);
DecisionTreeFactor expected(C & B & A, "0.8 0.5 0.5 0.2 0.2 0.5 0.5 0.8");
DiscreteKey C(0, 2), B(1, 2), A(2, 2);
DecisionTreeFactor actual(C & B & A, "0.8 0.5 0.5 0.2 0.2 0.5 0.5 0.8");
Signature signature((C | B, A) = "4/1 1/1 1/1 1/4");
DiscreteConditional actual(signature);
DecisionTreeFactor::shared_ptr actualFactor = actual.toFactor();
EXPECT(assert_equal(expected, *actualFactor));
DiscreteConditional expected(signature);
DecisionTreeFactor::shared_ptr expectedFactor = expected.toFactor();
EXPECT(assert_equal(*expectedFactor, actual));
}
/* ************************************************************************* */
TEST( DiscreteConditional, Combine) {
TEST(DiscreteConditional, Combine) {
DiscreteKey A(0, 2), B(1, 2);
vector<DiscreteConditional::shared_ptr> c;
c.push_back(boost::make_shared<DiscreteConditional>(A | B = "1/2 2/1"));
c.push_back(boost::make_shared<DiscreteConditional>(B % "1/2"));
DecisionTreeFactor factor(A & B, "0.111111 0.444444 0.222222 0.222222");
DiscreteConditional expected(2, factor);
DiscreteConditional::shared_ptr actual = DiscreteConditional::Combine(
c.begin(), c.end());
EXPECT(assert_equal(expected, *actual,1e-5));
DiscreteConditional actual(2, factor);
auto expected = DiscreteConditional::Combine(c.begin(), c.end());
EXPECT(assert_equal(*expected, actual, 1e-5));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

1
gtsam/discrete/tests/testDiscreteFactorGraph.cpp
View File

@ -19,6 +19,7 @@
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/discrete/DiscreteEliminationTree.h>
#include <gtsam/discrete/DiscreteBayesTree.h>
#include <gtsam/inference/BayesNet.h>
#include <CppUnitLite/TestHarness.h>

42
gtsam/discrete/tests/testDiscreteMarginals.cpp
View File

@ -146,8 +146,7 @@ TEST_UNSAFE( DiscreteMarginals, truss ) {
/* ************************************************************************* */
// Second truss example with non-trivial factors
TEST_UNSAFE( DiscreteMarginals, truss2 ) {
TEST_UNSAFE(DiscreteMarginals, truss2) {
const int nrNodes = 5;
const size_t nrStates = 2;
@ -160,40 +159,39 @@ TEST_UNSAFE( DiscreteMarginals, truss2 ) {
// create graph and add three truss potentials
DiscreteFactorGraph graph;
graph.add(key[0] & key[2] & key[4],"1 2 3 4 5 6 7 8");
graph.add(key[1] & key[3] & key[4],"1 2 3 4 5 6 7 8");
graph.add(key[2] & key[3] & key[4],"1 2 3 4 5 6 7 8");
graph.add(key[0] & key[2] & key[4], "1 2 3 4 5 6 7 8");
graph.add(key[1] & key[3] & key[4], "1 2 3 4 5 6 7 8");
graph.add(key[2] & key[3] & key[4], "1 2 3 4 5 6 7 8");
// Calculate the marginals by brute force
vector<DiscreteFactor::Values> allPosbValues = cartesianProduct(
key[0] & key[1] & key[2] & key[3] & key[4]);
vector<DiscreteFactor::Values> allPosbValues =
cartesianProduct(key[0] & key[1] & key[2] & key[3] & key[4]);
Vector T = Z_5x1, F = Z_5x1;
for (size_t i = 0; i < allPosbValues.size(); ++i) {
DiscreteFactor::Values x = allPosbValues[i];
double px = graph(x);
for (size_t j=0;j<5;j++)
if (x[j]) T[j]+=px; else F[j]+=px;
// cout << x[0] << " " << x[1] << " "<< x[2] << " " << x[3] << " " << x[4] << " :\t" << px << endl;
for (size_t j = 0; j < 5; j++)
if (x[j])
T[j] += px;
else
F[j] += px;
}
// Check all marginals given by a sequential solver and Marginals
// DiscreteSequentialSolver solver(graph);
// DiscreteSequentialSolver solver(graph);
DiscreteMarginals marginals(graph);
for (size_t j=0;j<5;j++) {
double sum = T[j]+F[j];
T[j]/=sum;
F[j]/=sum;
// // solver
// Vector actualV = solver.marginalProbabilities(key[j]);
// EXPECT(assert_equal((Vector(2) << F[j], T[j]), actualV));
for (size_t j = 0; j < 5; j++) {
double sum = T[j] + F[j];
T[j] /= sum;
F[j] /= sum;
// Marginals
vector<double> table;
table += F[j],T[j];
DecisionTreeFactor expectedM(key[j],table);
table += F[j], T[j];
DecisionTreeFactor expectedM(key[j], table);
DiscreteFactor::shared_ptr actualM = marginals(j);
EXPECT(assert_equal(expectedM, *boost::dynamic_pointer_cast<DecisionTreeFactor>(actualM)));
EXPECT(assert_equal(
expectedM, *boost::dynamic_pointer_cast<DecisionTreeFactor>(actualM)));
}
}

44
gtsam/discrete/tests/testSignature.cpp
View File

@ -11,36 +11,43 @@
/**
* @file testSignature
* @brief Tests focusing on the details of Signatures to evaluate boost compliance
* @brief Tests focusing on the details of Signatures to evaluate boost
* compliance
* @author Alex Cunningham
* @date Sept 19th 2011
*/
#include <boost/assign/std/vector.hpp>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/discrete/Signature.h>
#include <boost/assign/std/vector.hpp>
#include <vector>
using namespace std;
using namespace gtsam;
using namespace boost::assign;
DiscreteKey X(0,2), Y(1,3), Z(2,2);
DiscreteKey X(0, 2), Y(1, 3), Z(2, 2);
/* ************************************************************************* */
TEST(testSignature, simple_conditional) {
Signature sig(X | Y = "1/1 2/3 1/4");
Signature::Table table = *sig.table();
vector<double> row[3]{{0.5, 0.5}, {0.4, 0.6}, {0.2, 0.8}};
CHECK(row[0] == table[0]);
CHECK(row[1] == table[1]);
CHECK(row[2] == table[2]);
DiscreteKey actKey = sig.key();
LONGS_EQUAL((long)X.first, (long)actKey.first);
LONGS_EQUAL(X.first, actKey.first);
DiscreteKeys actKeys = sig.discreteKeysParentsFirst();
LONGS_EQUAL(2, (long)actKeys.size());
LONGS_EQUAL((long)Y.first, (long)actKeys.front().first);
LONGS_EQUAL((long)X.first, (long)actKeys.back().first);
DiscreteKeys actKeys = sig.discreteKeys();
LONGS_EQUAL(2, actKeys.size());
LONGS_EQUAL(X.first, actKeys.front().first);
LONGS_EQUAL(Y.first, actKeys.back().first);
vector<double> actCpt = sig.cpt();
EXPECT_LONGS_EQUAL(6, (long)actCpt.size());
EXPECT_LONGS_EQUAL(6, actCpt.size());
}
/* ************************************************************************* */
@ -54,17 +61,20 @@ TEST(testSignature, simple_conditional_nonparser) {
Signature sig(X | Y = table);
DiscreteKey actKey = sig.key();
EXPECT_LONGS_EQUAL((long)X.first, (long)actKey.first);
EXPECT_LONGS_EQUAL(X.first, actKey.first);
DiscreteKeys actKeys = sig.discreteKeysParentsFirst();
LONGS_EQUAL(2, (long)actKeys.size());
LONGS_EQUAL((long)Y.first, (long)actKeys.front().first);
LONGS_EQUAL((long)X.first, (long)actKeys.back().first);
DiscreteKeys actKeys = sig.discreteKeys();
LONGS_EQUAL(2, actKeys.size());
LONGS_EQUAL(X.first, actKeys.front().first);
LONGS_EQUAL(Y.first, actKeys.back().first);
vector<double> actCpt = sig.cpt();
EXPECT_LONGS_EQUAL(6, (long)actCpt.size());
EXPECT_LONGS_EQUAL(6, actCpt.size());
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

4
gtsam/geometry/PinholePose.h
View File

@ -107,9 +107,9 @@ public:
// If needed, apply chain rule
if (Dpose)
*Dpose = Dpi_pn * *Dpose;
*Dpose = Dpi_pn * *Dpose;
if (Dpoint)
*Dpoint = Dpi_pn * *Dpoint;
*Dpoint = Dpi_pn * *Dpoint;
return pi;
}

14
gtsam/geometry/SOn.h
View File

@ -292,6 +292,10 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
boost::none) const;
/// @}
template <class Archive>
friend void save(Archive&, SO&, const unsigned int);
template <class Archive>
friend void load(Archive&, SO&, const unsigned int);
template <class Archive>
friend void serialize(Archive&, SO&, const unsigned int);
friend class boost::serialization::access;
@ -329,6 +333,16 @@ template <>
SOn LieGroup<SOn, Eigen::Dynamic>::between(const SOn& g, DynamicJacobian H1,
DynamicJacobian H2) const;
/** Serialization function */
template<class Archive>
void serialize(
Archive& ar, SOn& Q,
const unsigned int file_version
) {
Matrix& M = Q.matrix_;
ar& BOOST_SERIALIZATION_NVP(M);
}
/*
* Define the traits. internal::LieGroup provides both Lie group and Testable
*/

2
gtsam/geometry/Unit3.h
View File

@ -90,6 +90,8 @@ public:
/// Copy assignment
Unit3& operator=(const Unit3 & u) {
p_ = u.p_;
B_ = u.B_;
H_B_ = u.H_B_;
return *this;
}

9
gtsam/geometry/tests/testUnit3.cpp
View File

@ -484,6 +484,15 @@ TEST(Unit3, ErrorBetweenFactor) {
}
}
TEST(Unit3, CopyAssign) {
Unit3 p{1, 0.2, 0.3};
EXPECT(p.error(p).isZero());
p = Unit3{-1, 2, 8};
EXPECT(p.error(p).isZero());
}
/* ************************************************************************* */
TEST(actualH, Serialization) {
Unit3 p(0, 1, 0);

4
gtsam/inference/BayesNet.h
View File

@ -69,4 +69,6 @@ namespace gtsam {
void saveGraph(const std::string &s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
};
}
}
#include <gtsam/inference/BayesNet-inst.h>

54
gtsam/inference/BayesTreeCliqueBase-inst.h
View File

@ -136,57 +136,61 @@ namespace gtsam {
}
}
/* ************************************************************************* */
/* *********************************************************************** */
// separator marginal, uses separator marginal of parent recursively
// P(C) = P(F|S) P(S)
/* ************************************************************************* */
template<class DERIVED, class FACTORGRAPH>
/* *********************************************************************** */
template <class DERIVED, class FACTORGRAPH>
typename BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::FactorGraphType
BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::separatorMarginal(Eliminate function) const
{
BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::separatorMarginal(
Eliminate function) const {
gttic(BayesTreeCliqueBase_separatorMarginal);
// Check if the Separator marginal was already calculated
if (!cachedSeparatorMarginal_)
{
if (!cachedSeparatorMarginal_) {
gttic(BayesTreeCliqueBase_separatorMarginal_cachemiss);
// If this is the root, there is no separator
if (parent_.expired() /*(if we're the root)*/)
{
if (parent_.expired() /*(if we're the root)*/) {
// we are root, return empty
FactorGraphType empty;
cachedSeparatorMarginal_ = empty;
}
else
{
} else {
// Flatten recursion in timing outline
gttoc(BayesTreeCliqueBase_separatorMarginal_cachemiss);
gttoc(BayesTreeCliqueBase_separatorMarginal);
// Obtain P(S) = \int P(Cp) = \int P(Fp|Sp) P(Sp)
// initialize P(Cp) with the parent separator marginal
derived_ptr parent(parent_.lock());
gttoc(BayesTreeCliqueBase_separatorMarginal_cachemiss); // Flatten recursion in timing outline
gttoc(BayesTreeCliqueBase_separatorMarginal);
FactorGraphType p_Cp(parent->separatorMarginal(function)); // P(Sp)
FactorGraphType p_Cp(parent->separatorMarginal(function)); // P(Sp)
gttic(BayesTreeCliqueBase_separatorMarginal);
gttic(BayesTreeCliqueBase_separatorMarginal_cachemiss);
// now add the parent conditional
p_Cp += parent->conditional_; // P(Fp|Sp)
p_Cp += parent->conditional_; // P(Fp|Sp)
// The variables we want to keepSet are exactly the ones in S
KeyVector indicesS(this->conditional()->beginParents(), this->conditional()->endParents());
cachedSeparatorMarginal_ = *p_Cp.marginalMultifrontalBayesNet(Ordering(indicesS), function);
KeyVector indicesS(this->conditional()->beginParents(),
this->conditional()->endParents());
auto separatorMarginal =
p_Cp.marginalMultifrontalBayesNet(Ordering(indicesS), function);
cachedSeparatorMarginal_.reset(*separatorMarginal);
}
}
// return the shortcut P(S||B)
return *cachedSeparatorMarginal_; // return the cached version
return *cachedSeparatorMarginal_; // return the cached version
}
/* ************************************************************************* */
// marginal2, uses separator marginal of parent recursively
/* *********************************************************************** */
// marginal2, uses separator marginal of parent
// P(C) = P(F|S) P(S)
/* ************************************************************************* */
template<class DERIVED, class FACTORGRAPH>
/* *********************************************************************** */
template <class DERIVED, class FACTORGRAPH>
typename BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::FactorGraphType
BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::marginal2(Eliminate function) const
{
BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::marginal2(
Eliminate function) const {
gttic(BayesTreeCliqueBase_marginal2);
// initialize with separator marginal P(S)
FactorGraphType p_C = this->separatorMarginal(function);

3
gtsam/inference/Conditional.h
View File

@ -65,6 +65,8 @@ namespace gtsam {
Conditional(size_t nrFrontals) : nrFrontals_(nrFrontals) {}
/// @}
public:
/// @name Testable
/// @{
@ -76,7 +78,6 @@ namespace gtsam {
/// @}
public:
/// @name Standard Interface
/// @{

2
gtsam/linear/JacobianFactor.cpp
View File

@ -515,7 +515,7 @@ Vector JacobianFactor::error_vector(const VectorValues& c) const {
double JacobianFactor::error(const VectorValues& c) const {
Vector e = unweighted_error(c);
// Use the noise model distance function to get the correct error if available.
if (model_) return 0.5 * model_->distance(e);
if (model_) return 0.5 * model_->squaredMahalanobisDistance(e);
return 0.5 * e.dot(e);
}

74
gtsam/linear/LossFunctions.cpp
View File

@ -137,12 +137,12 @@ Fair::Fair(double c, const ReweightScheme reweight) : Base(reweight), c_(c) {
}
}
double Fair::weight(double error) const {
return 1.0 / (1.0 + std::abs(error) / c_);
double Fair::weight(double distance) const {
return 1.0 / (1.0 + std::abs(distance) / c_);
}
double Fair::residual(double error) const {
const double absError = std::abs(error);
double Fair::loss(double distance) const {
const double absError = std::abs(distance);
const double normalizedError = absError / c_;
const double c_2 = c_ * c_;
return c_2 * (normalizedError - std::log1p(normalizedError));
@ -170,15 +170,15 @@ Huber::Huber(double k, const ReweightScheme reweight) : Base(reweight), k_(k) {
}
}
double Huber::weight(double error) const {
const double absError = std::abs(error);
double Huber::weight(double distance) const {
const double absError = std::abs(distance);
return (absError <= k_) ? (1.0) : (k_ / absError);
}
double Huber::residual(double error) const {
const double absError = std::abs(error);
double Huber::loss(double distance) const {
const double absError = std::abs(distance);
if (absError <= k_) { // |x| <= k
return error*error / 2;
return distance*distance / 2;
} else { // |x| > k
return k_ * (absError - (k_/2));
}
@ -208,12 +208,12 @@ Cauchy::Cauchy(double k, const ReweightScheme reweight) : Base(reweight), k_(k),
}
}
double Cauchy::weight(double error) const {
return ksquared_ / (ksquared_ + error*error);
double Cauchy::weight(double distance) const {
return ksquared_ / (ksquared_ + distance*distance);
}
double Cauchy::residual(double error) const {
const double val = std::log1p(error * error / ksquared_);
double Cauchy::loss(double distance) const {
const double val = std::log1p(distance * distance / ksquared_);
return ksquared_ * val * 0.5;
}
@ -241,18 +241,18 @@ Tukey::Tukey(double c, const ReweightScheme reweight) : Base(reweight), c_(c), c
}
}
double Tukey::weight(double error) const {
if (std::abs(error) <= c_) {
const double one_minus_xc2 = 1.0 - error*error/csquared_;
double Tukey::weight(double distance) const {
if (std::abs(distance) <= c_) {
const double one_minus_xc2 = 1.0 - distance*distance/csquared_;
return one_minus_xc2 * one_minus_xc2;
}
return 0.0;
}
double Tukey::residual(double error) const {
double absError = std::abs(error);
double Tukey::loss(double distance) const {
double absError = std::abs(distance);
if (absError <= c_) {
const double one_minus_xc2 = 1.0 - error*error/csquared_;
const double one_minus_xc2 = 1.0 - distance*distance/csquared_;
const double t = one_minus_xc2*one_minus_xc2*one_minus_xc2;
return csquared_ * (1 - t) / 6.0;
} else {
@ -280,13 +280,13 @@ Tukey::shared_ptr Tukey::Create(double c, const ReweightScheme reweight) {
Welsch::Welsch(double c, const ReweightScheme reweight) : Base(reweight), c_(c), csquared_(c * c) {}
double Welsch::weight(double error) const {
const double xc2 = (error*error)/csquared_;
double Welsch::weight(double distance) const {
const double xc2 = (distance*distance)/csquared_;
return std::exp(-xc2);
}
double Welsch::residual(double error) const {
const double xc2 = (error*error)/csquared_;
double Welsch::loss(double distance) const {
const double xc2 = (distance*distance)/csquared_;
return csquared_ * 0.5 * -std::expm1(-xc2);
}
@ -311,16 +311,16 @@ GemanMcClure::GemanMcClure(double c, const ReweightScheme reweight)
: Base(reweight), c_(c) {
}
double GemanMcClure::weight(double error) const {
double GemanMcClure::weight(double distance) const {
const double c2 = c_*c_;
const double c4 = c2*c2;
const double c2error = c2 + error*error;
const double c2error = c2 + distance*distance;
return c4/(c2error*c2error);
}
double GemanMcClure::residual(double error) const {
double GemanMcClure::loss(double distance) const {
const double c2 = c_*c_;
const double error2 = error*error;
const double error2 = distance*distance;
return 0.5 * (c2 * error2) / (c2 + error2);
}
@ -345,8 +345,8 @@ DCS::DCS(double c, const ReweightScheme reweight)
: Base(reweight), c_(c) {
}
double DCS::weight(double error) const {
const double e2 = error*error;
double DCS::weight(double distance) const {
const double e2 = distance*distance;
if (e2 > c_)
{
const double w = 2.0*c_/(c_ + e2);
@ -356,10 +356,10 @@ double DCS::weight(double error) const {
return 1.0;
}
double DCS::residual(double error) const {
double DCS::loss(double distance) const {
// This is the simplified version of Eq 9 from (Agarwal13icra)
// after you simplify and cancel terms.
const double e2 = error*error;
const double e2 = distance*distance;
const double e4 = e2*e2;
const double c2 = c_*c_;
@ -391,17 +391,17 @@ L2WithDeadZone::L2WithDeadZone(double k, const ReweightScheme reweight)
}
}
double L2WithDeadZone::weight(double error) const {
double L2WithDeadZone::weight(double distance) const {
// note that this code is slightly uglier than residual, because there are three distinct
// cases to handle (left of deadzone, deadzone, right of deadzone) instead of the two
// cases (deadzone, non-deadzone) in residual.
if (std::abs(error) <= k_) return 0.0;
else if (error > k_) return (-k_+error)/error;
else return (k_+error)/error;
if (std::abs(distance) <= k_) return 0.0;
else if (distance > k_) return (-k_+distance)/distance;
else return (k_+distance)/distance;
}
double L2WithDeadZone::residual(double error) const {
const double abs_error = std::abs(error);
double L2WithDeadZone::loss(double distance) const {
const double abs_error = std::abs(distance);
return (abs_error < k_) ? 0.0 : 0.5*(k_-abs_error)*(k_-abs_error);
}

56
gtsam/linear/LossFunctions.h
View File

@ -36,12 +36,12 @@ namespace noiseModel {
* The mEstimator name space contains all robust error functions.
* It mirrors the exposition at
* https://members.loria.fr/MOBerger/Enseignement/Master2/Documents/ZhangIVC-97-01.pdf
* which talks about minimizing \sum \rho(r_i), where \rho is a residual function of choice.
* which talks about minimizing \sum \rho(r_i), where \rho is a loss function of choice.
*
* To illustrate, let's consider the least-squares (L2), L1, and Huber estimators as examples:
*
* Name Symbol Least-Squares L1-norm Huber
* Residual \rho(x) 0.5*x^2 |x| 0.5*x^2 if |x|<k, 0.5*k^2 + k|x-k| otherwise
* Loss \rho(x) 0.5*x^2 |x| 0.5*x^2 if |x|<k, 0.5*k^2 + k|x-k| otherwise
* Derivative \phi(x) x sgn(x) x if |x|<k, k sgn(x) otherwise
* Weight w(x)=\phi(x)/x 1 1/|x| 1 if |x|<k, k/|x| otherwise
*
@ -75,27 +75,31 @@ class GTSAM_EXPORT Base {
* the quadratic function for an L2 penalty, the absolute value function for
* an L1 penalty, etc.
*
* TODO(mikebosse): When the residual function has as input the norm of the
* residual vector, then it prevents implementations of asymmeric loss
* TODO(mikebosse): When the loss function has as input the norm of the
* error vector, then it prevents implementations of asymmeric loss
* functions. It would be better for this function to accept the vector and
* internally call the norm if necessary.
*/
virtual double residual(double error) const { return 0; };
virtual double loss(double distance) const { return 0; };
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
virtual double residual(double distance) const { return loss(distance); };
#endif
/*
* This method is responsible for returning the weight function for a given
* amount of error. The weight function is related to the analytic derivative
* of the residual function. See
* of the loss function. See
* https://members.loria.fr/MOBerger/Enseignement/Master2/Documents/ZhangIVC-97-01.pdf
* for details. This method is required when optimizing cost functions with
* robust penalties using iteratively re-weighted least squares.
*/
virtual double weight(double error) const = 0;
virtual double weight(double distance) const = 0;
virtual void print(const std::string &s) const = 0;
virtual bool equals(const Base &expected, double tol = 1e-8) const = 0;
double sqrtWeight(double error) const { return std::sqrt(weight(error)); }
double sqrtWeight(double distance) const { return std::sqrt(weight(distance)); }
/** produce a weight vector according to an error vector and the implemented
* robust function */
@ -123,7 +127,7 @@ class GTSAM_EXPORT Base {
}
};
/// Null class is not robust so is a Gaussian ?
/// Null class should behave as Gaussian
class GTSAM_EXPORT Null : public Base {
public:
typedef boost::shared_ptr<Null> shared_ptr;
@ -131,7 +135,7 @@ class GTSAM_EXPORT Null : public Base {
Null(const ReweightScheme reweight = Block) : Base(reweight) {}
~Null() {}
double weight(double /*error*/) const { return 1.0; }
double residual(double error) const { return error; }
double loss(double distance) const { return 0.5 * distance * distance; }
void print(const std::string &s) const;
bool equals(const Base & /*expected*/, double /*tol*/) const { return true; }
static shared_ptr Create();
@ -154,8 +158,8 @@ class GTSAM_EXPORT Fair : public Base {
typedef boost::shared_ptr<Fair> shared_ptr;
Fair(double c = 1.3998, const ReweightScheme reweight = Block);
double weight(double error) const override;
double residual(double error) const override;
double weight(double distance) const override;
double loss(double distance) const override;
void print(const std::string &s) const override;
bool equals(const Base &expected, double tol = 1e-8) const override;
static shared_ptr Create(double c, const ReweightScheme reweight = Block);
@ -179,8 +183,8 @@ class GTSAM_EXPORT Huber : public Base {
typedef boost::shared_ptr<Huber> shared_ptr;
Huber(double k = 1.345, const ReweightScheme reweight = Block);
double weight(double error) const override;
double residual(double error) const override;
double weight(double distance) const override;
double loss(double distance) const override;
void print(const std::string &s) const override;
bool equals(const Base &expected, double tol = 1e-8) const override;
static shared_ptr Create(double k, const ReweightScheme reweight = Block);
@ -209,8 +213,8 @@ class GTSAM_EXPORT Cauchy : public Base {
typedef boost::shared_ptr<Cauchy> shared_ptr;
Cauchy(double k = 0.1, const ReweightScheme reweight = Block);
double weight(double error) const override;
double residual(double error) const override;
double weight(double distance) const override;
double loss(double distance) const override;
void print(const std::string &s) const override;
bool equals(const Base &expected, double tol = 1e-8) const override;
static shared_ptr Create(double k, const ReweightScheme reweight = Block);
@ -234,8 +238,8 @@ class GTSAM_EXPORT Tukey : public Base {
typedef boost::shared_ptr<Tukey> shared_ptr;
Tukey(double c = 4.6851, const ReweightScheme reweight = Block);
double weight(double error) const override;
double residual(double error) const override;
double weight(double distance) const override;
double loss(double distance) const override;
void print(const std::string &s) const override;
bool equals(const Base &expected, double tol = 1e-8) const override;
static shared_ptr Create(double k, const ReweightScheme reweight = Block);
@ -259,8 +263,8 @@ class GTSAM_EXPORT Welsch : public Base {
typedef boost::shared_ptr<Welsch> shared_ptr;
Welsch(double c = 2.9846, const ReweightScheme reweight = Block);
double weight(double error) const override;
double residual(double error) const override;
double weight(double distance) const override;
double loss(double distance) const override;
void print(const std::string &s) const override;
bool equals(const Base &expected, double tol = 1e-8) const override;
static shared_ptr Create(double k, const ReweightScheme reweight = Block);
@ -295,8 +299,8 @@ class GTSAM_EXPORT GemanMcClure : public Base {
GemanMcClure(double c = 1.0, const ReweightScheme reweight = Block);
~GemanMcClure() {}
double weight(double error) const override;
double residual(double error) const override;
double weight(double distance) const override;
double loss(double distance) const override;
void print(const std::string &s) const override;
bool equals(const Base &expected, double tol = 1e-8) const override;
static shared_ptr Create(double k, const ReweightScheme reweight = Block);
@ -325,8 +329,8 @@ class GTSAM_EXPORT DCS : public Base {
DCS(double c = 1.0, const ReweightScheme reweight = Block);
~DCS() {}
double weight(double error) const override;
double residual(double error) const override;
double weight(double distance) const override;
double loss(double distance) const override;
void print(const std::string &s) const override;
bool equals(const Base &expected, double tol = 1e-8) const override;
static shared_ptr Create(double k, const ReweightScheme reweight = Block);
@ -358,8 +362,8 @@ class GTSAM_EXPORT L2WithDeadZone : public Base {
typedef boost::shared_ptr<L2WithDeadZone> shared_ptr;
L2WithDeadZone(double k = 1.0, const ReweightScheme reweight = Block);
double weight(double error) const override;
double residual(double error) const override;
double weight(double distance) const override;
double loss(double distance) const override;
void print(const std::string &s) const override;
bool equals(const Base &expected, double tol = 1e-8) const override;
static shared_ptr Create(double k, const ReweightScheme reweight = Block);

29
gtsam/linear/NoiseModel.cpp
View File

@ -74,6 +74,13 @@ Vector Base::sigmas() const {
throw("Base::sigmas: sigmas() not implemented for this noise model");
}
/* ************************************************************************* */
double Base::squaredMahalanobisDistance(const Vector& v) const {
// Note: for Diagonal, which does ediv_, will be correct for constraints
Vector w = whiten(v);
return w.dot(w);
}
/* ************************************************************************* */
Gaussian::shared_ptr Gaussian::SqrtInformation(const Matrix& R, bool smart) {
size_t m = R.rows(), n = R.cols();
@ -164,13 +171,6 @@ Vector Gaussian::unwhiten(const Vector& v) const {
return backSubstituteUpper(thisR(), v);
}
/* ************************************************************************* */
double Gaussian::squaredMahalanobisDistance(const Vector& v) const {
// Note: for Diagonal, which does ediv_, will be correct for constraints
Vector w = whiten(v);
return w.dot(w);
}
/* ************************************************************************* */
Matrix Gaussian::Whiten(const Matrix& H) const {
return thisR() * H;
@ -376,8 +376,19 @@ Vector Constrained::whiten(const Vector& v) const {
return c;
}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/* ************************************************************************* */
double Constrained::distance(const Vector& v) const {
double Constrained::error(const Vector& v) const {
Vector w = Diagonal::whiten(v); // get noisemodel for constrained elements
for (size_t i=0; i<dim_; ++i) // add mu weights on constrained variables
if (constrained(i)) // whiten makes constrained variables zero
w[i] = v[i] * sqrt(mu_[i]); // TODO: may want to store sqrt rather than rebuild
return 0.5 * w.dot(w);
}
#endif
/* ************************************************************************* */
double Constrained::squaredMahalanobisDistance(const Vector& v) const {
Vector w = Diagonal::whiten(v); // get noisemodel for constrained elements
for (size_t i=0; i<dim_; ++i) // add mu weights on constrained variables
if (constrained(i)) // whiten makes constrained variables zero
@ -663,7 +674,7 @@ void Robust::WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const{
}
Robust::shared_ptr Robust::Create(
const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise){
const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise){
return shared_ptr(new Robust(robust,noise));
}

187
gtsam/linear/NoiseModel.h
View File

@ -90,7 +90,27 @@ namespace gtsam {
/// Unwhiten an error vector.
virtual Vector unwhiten(const Vector& v) const = 0;
virtual double distance(const Vector& v) const = 0;
/// Squared Mahalanobis distance v'*R'*R*v = <R*v,R*v>
virtual double squaredMahalanobisDistance(const Vector& v) const;
/// Mahalanobis distance
virtual double mahalanobisDistance(const Vector& v) const {
return std::sqrt(squaredMahalanobisDistance(v));
}
/// loss function, input is Mahalanobis distance
virtual double loss(const double squared_distance) const {
return 0.5 * squared_distance;
}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// calculate the error value given measurement error vector
virtual double error(const Vector& v) const = 0;
virtual double distance(const Vector& v) {
return error(v) * 2;
}
#endif
virtual void WhitenSystem(std::vector<Matrix>& A, Vector& b) const = 0;
virtual void WhitenSystem(Matrix& A, Vector& b) const = 0;
@ -200,39 +220,30 @@ namespace gtsam {
*/
static shared_ptr Covariance(const Matrix& covariance, bool smart = true);
virtual void print(const std::string& name) const;
virtual bool equals(const Base& expected, double tol=1e-9) const;
virtual Vector sigmas() const;
virtual Vector whiten(const Vector& v) const;
virtual Vector unwhiten(const Vector& v) const;
/**
* Squared Mahalanobis distance v'*R'*R*v = <R*v,R*v>
*/
virtual double squaredMahalanobisDistance(const Vector& v) const;
/**
* Mahalanobis distance
*/
virtual double mahalanobisDistance(const Vector& v) const {
return std::sqrt(squaredMahalanobisDistance(v));
}
void print(const std::string& name) const override;
bool equals(const Base& expected, double tol=1e-9) const override;
Vector sigmas() const override;
Vector whiten(const Vector& v) const override;
Vector unwhiten(const Vector& v) const override;
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
virtual double Mahalanobis(const Vector& v) const {
return squaredMahalanobisDistance(v);
}
#endif
inline virtual double distance(const Vector& v) const {
return squaredMahalanobisDistance(v);
/**
* error value 0.5 * v'*R'*R*v
*/
inline double error(const Vector& v) const override {
return 0.5 * squaredMahalanobisDistance(v);
}
#endif
/**
* Multiply a derivative with R (derivative of whiten)
* Equivalent to whitening each column of the input matrix.
*/
virtual Matrix Whiten(const Matrix& H) const;
Matrix Whiten(const Matrix& H) const override;
/**
* In-place version
@ -247,10 +258,10 @@ namespace gtsam {
/**
* Whiten a system, in place as well
*/
virtual void WhitenSystem(std::vector<Matrix>& A, Vector& b) const;
virtual void WhitenSystem(Matrix& A, Vector& b) const;
virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const;
virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const;
void WhitenSystem(std::vector<Matrix>& A, Vector& b) const override;
void WhitenSystem(Matrix& A, Vector& b) const override;
void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const override;
void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const override;
/**
* Apply appropriately weighted QR factorization to the system [A b]
@ -335,13 +346,13 @@ namespace gtsam {
return Variances(precisions.array().inverse(), smart);
}
virtual void print(const std::string& name) const;
virtual Vector sigmas() const { return sigmas_; }
virtual Vector whiten(const Vector& v) const;
virtual Vector unwhiten(const Vector& v) const;
virtual Matrix Whiten(const Matrix& H) const;
virtual void WhitenInPlace(Matrix& H) const;
virtual void WhitenInPlace(Eigen::Block<Matrix> H) const;
void print(const std::string& name) const override;
Vector sigmas() const override { return sigmas_; }
Vector whiten(const Vector& v) const override;
Vector unwhiten(const Vector& v) const override;
Matrix Whiten(const Matrix& H) const override;
void WhitenInPlace(Matrix& H) const override;
void WhitenInPlace(Eigen::Block<Matrix> H) const override;
/**
* Return standard deviations (sqrt of diagonal)
@ -363,7 +374,7 @@ namespace gtsam {
/**
* Return R itself, but note that Whiten(H) is cheaper than R*H
*/
virtual Matrix R() const {
Matrix R() const override {
return invsigmas().asDiagonal();
}
@ -417,10 +428,10 @@ namespace gtsam {
typedef boost::shared_ptr<Constrained> shared_ptr;
virtual ~Constrained() {}
~Constrained() {}
/// true if a constrained noise mode, saves slow/clumsy dynamic casting
virtual bool isConstrained() const { return true; }
bool isConstrained() const override { return true; }
/// Return true if a particular dimension is free or constrained
bool constrained(size_t i) const;
@ -472,12 +483,16 @@ namespace gtsam {
return MixedVariances(precisions.array().inverse());
}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/**
* The distance function for a constrained noisemodel,
* The error function for a constrained noisemodel,
* for non-constrained versions, uses sigmas, otherwise
* uses the penalty function with mu
*/
virtual double distance(const Vector& v) const;
double error(const Vector& v) const override;
#endif
double squaredMahalanobisDistance(const Vector& v) const override;
/** Fully constrained variations */
static shared_ptr All(size_t dim) {
@ -494,16 +509,16 @@ namespace gtsam {
return shared_ptr(new Constrained(Vector::Constant(dim, mu), Vector::Constant(dim,0)));
}
virtual void print(const std::string& name) const;
void print(const std::string& name) const override;
/// Calculates error vector with weights applied
virtual Vector whiten(const Vector& v) const;
Vector whiten(const Vector& v) const override;
/// Whitening functions will perform partial whitening on rows
/// with a non-zero sigma. Other rows remain untouched.
virtual Matrix Whiten(const Matrix& H) const;
virtual void WhitenInPlace(Matrix& H) const;
virtual void WhitenInPlace(Eigen::Block<Matrix> H) const;
Matrix Whiten(const Matrix& H) const override;
void WhitenInPlace(Matrix& H) const override;
void WhitenInPlace(Eigen::Block<Matrix> H) const override;
/**
* Apply QR factorization to the system [A b], taking into account constraints
@ -514,7 +529,7 @@ namespace gtsam {
* @param Ab is the m*(n+1) augmented system matrix [A b]
* @return diagonal noise model can be all zeros, mixed, or not-constrained
*/
virtual Diagonal::shared_ptr QR(Matrix& Ab) const;
Diagonal::shared_ptr QR(Matrix& Ab) const override;
/**
* Returns a Unit version of a constrained noisemodel in which
@ -576,14 +591,14 @@ namespace gtsam {
return Variance(dim, 1.0/precision, smart);
}
virtual void print(const std::string& name) const;
virtual double squaredMahalanobisDistance(const Vector& v) const;
virtual Vector whiten(const Vector& v) const;
virtual Vector unwhiten(const Vector& v) const;
virtual Matrix Whiten(const Matrix& H) const;
virtual void WhitenInPlace(Matrix& H) const;
virtual void whitenInPlace(Vector& v) const;
virtual void WhitenInPlace(Eigen::Block<Matrix> H) const;
void print(const std::string& name) const override;
double squaredMahalanobisDistance(const Vector& v) const override;
Vector whiten(const Vector& v) const override;
Vector unwhiten(const Vector& v) const override;
Matrix Whiten(const Matrix& H) const override;
void WhitenInPlace(Matrix& H) const override;
void whitenInPlace(Vector& v) const override;
void WhitenInPlace(Eigen::Block<Matrix> H) const override;
/**
* Return standard deviation
@ -616,7 +631,7 @@ namespace gtsam {
typedef boost::shared_ptr<Unit> shared_ptr;
virtual ~Unit() {}
~Unit() {}
/**
* Create a unit covariance noise model
@ -626,19 +641,19 @@ namespace gtsam {
}
/// true if a unit noise model, saves slow/clumsy dynamic casting
virtual bool isUnit() const { return true; }
bool isUnit() const override { return true; }
virtual void print(const std::string& name) const;
virtual double squaredMahalanobisDistance(const Vector& v) const {return v.dot(v); }
virtual Vector whiten(const Vector& v) const { return v; }
virtual Vector unwhiten(const Vector& v) const { return v; }
virtual Matrix Whiten(const Matrix& H) const { return H; }
virtual void WhitenInPlace(Matrix& /*H*/) const {}
virtual void WhitenInPlace(Eigen::Block<Matrix> /*H*/) const {}
virtual void whitenInPlace(Vector& /*v*/) const {}
virtual void unwhitenInPlace(Vector& /*v*/) const {}
virtual void whitenInPlace(Eigen::Block<Vector>& /*v*/) const {}
virtual void unwhitenInPlace(Eigen::Block<Vector>& /*v*/) const {}
void print(const std::string& name) const override;
double squaredMahalanobisDistance(const Vector& v) const override {return v.dot(v); }
Vector whiten(const Vector& v) const override { return v; }
Vector unwhiten(const Vector& v) const override { return v; }
Matrix Whiten(const Matrix& H) const override { return H; }
void WhitenInPlace(Matrix& /*H*/) const override {}
void WhitenInPlace(Eigen::Block<Matrix> /*H*/) const override {}
void whitenInPlace(Vector& /*v*/) const override {}
void unwhitenInPlace(Vector& /*v*/) const override {}
void whitenInPlace(Eigen::Block<Vector>& /*v*/) const override {}
void unwhitenInPlace(Eigen::Block<Vector>& /*v*/) const override {}
private:
/** Serialization function */
@ -687,10 +702,10 @@ namespace gtsam {
: Base(noise->dim()), robust_(robust), noise_(noise) {}
/// Destructor
virtual ~Robust() {}
~Robust() {}
virtual void print(const std::string& name) const;
virtual bool equals(const Base& expected, double tol=1e-9) const;
void print(const std::string& name) const override;
bool equals(const Base& expected, double tol=1e-9) const override;
/// Return the contained robust error function
const RobustModel::shared_ptr& robust() const { return robust_; }
@ -699,28 +714,36 @@ namespace gtsam {
const NoiseModel::shared_ptr& noise() const { return noise_; }
// TODO: functions below are dummy but necessary for the noiseModel::Base
inline virtual Vector whiten(const Vector& v) const
inline Vector whiten(const Vector& v) const override
{ Vector r = v; this->WhitenSystem(r); return r; }
inline virtual Matrix Whiten(const Matrix& A) const
inline Matrix Whiten(const Matrix& A) const override
{ Vector b; Matrix B=A; this->WhitenSystem(B,b); return B; }
inline virtual Vector unwhiten(const Vector& /*v*/) const
inline Vector unwhiten(const Vector& /*v*/) const override
{ throw std::invalid_argument("unwhiten is not currently supported for robust noise models."); }
// Fold the use of the m-estimator residual(...) function into distance(...)
inline virtual double distance(const Vector& v) const
{ return robust_->residual(this->unweightedWhiten(v).norm()); }
inline virtual double distance_non_whitened(const Vector& v) const
{ return robust_->residual(v.norm()); }
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
inline double distance(const Vector& v) override {
return robust_->loss(this->unweightedWhiten(v).norm());
}
// Fold the use of the m-estimator loss(...) function into error(...)
inline double error(const Vector& v) const override
{ return robust_->loss(noise_->mahalanobisDistance(v)); }
#endif
double loss(const double squared_distance) const override {
return robust_->loss(std::sqrt(squared_distance));
}
// TODO: these are really robust iterated re-weighting support functions
virtual void WhitenSystem(Vector& b) const;
virtual void WhitenSystem(std::vector<Matrix>& A, Vector& b) const;
virtual void WhitenSystem(Matrix& A, Vector& b) const;
virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const;
virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const;
void WhitenSystem(std::vector<Matrix>& A, Vector& b) const override;
void WhitenSystem(Matrix& A, Vector& b) const override;
void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const override;
void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const override;
virtual Vector unweightedWhiten(const Vector& v) const {
Vector unweightedWhiten(const Vector& v) const override {
return noise_->unweightedWhiten(v);
}
virtual double weight(const Vector& v) const {
double weight(const Vector& v) const override {
// Todo(mikebosse): make the robust weight function input a vector.
return robust_->weight(v.norm());
}

11
gtsam/linear/PCGSolver.cpp
View File

@ -45,6 +45,17 @@ PCGSolver::PCGSolver(const PCGSolverParameters &p) {
preconditioner_ = createPreconditioner(p.preconditioner_);
}
void PCGSolverParameters::setPreconditionerParams(const boost::shared_ptr<PreconditionerParameters> preconditioner) {
preconditioner_ = preconditioner;
}
void PCGSolverParameters::print(const std::string &s) const {
std::cout << s << std::endl;;
std::ostringstream os;
print(os);
std::cout << os.str() << std::endl;
}
/*****************************************************************************/
VectorValues PCGSolver::optimize(const GaussianFactorGraph &gfg,
const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda,

5
gtsam/linear/PCGSolver.h
View File

@ -48,7 +48,12 @@ public:
return *preconditioner_;
}
// needed for python wrapper
void print(const std::string &s) const;
boost::shared_ptr<PreconditionerParameters> preconditioner_;
void setPreconditionerParams(const boost::shared_ptr<PreconditionerParameters> preconditioner);
};
/**

115
gtsam/linear/tests/testNoiseModel.cpp
View File

@ -182,8 +182,9 @@ TEST(NoiseModel, ConstrainedMixed )
EXPECT(assert_equal(Vector3(0.5, 1.0, 0.5),d->whiten(infeasible)));
EXPECT(assert_equal(Vector3(0.5, 0.0, 0.5),d->whiten(feasible)));
DOUBLES_EQUAL(1000.0 + 0.25 + 0.25,d->distance(infeasible),1e-9);
DOUBLES_EQUAL(0.5,d->distance(feasible),1e-9);
DOUBLES_EQUAL(0.5 * (1000.0 + 0.25 + 0.25),d->loss(d->squaredMahalanobisDistance(infeasible)),1e-9);
DOUBLES_EQUAL(0.5, d->squaredMahalanobisDistance(feasible),1e-9);
DOUBLES_EQUAL(0.5 * 0.5, d->loss(0.5),1e-9);
}
/* ************************************************************************* */
@ -197,8 +198,9 @@ TEST(NoiseModel, ConstrainedAll )
EXPECT(assert_equal(Vector3(1.0, 1.0, 1.0),i->whiten(infeasible)));
EXPECT(assert_equal(Vector3(0.0, 0.0, 0.0),i->whiten(feasible)));
DOUBLES_EQUAL(1000.0 * 3.0,i->distance(infeasible),1e-9);
DOUBLES_EQUAL(0.0,i->distance(feasible),1e-9);
DOUBLES_EQUAL(0.5 * 1000.0 * 3.0,i->loss(i->squaredMahalanobisDistance(infeasible)),1e-9);
DOUBLES_EQUAL(0.0, i->squaredMahalanobisDistance(feasible), 1e-9);
DOUBLES_EQUAL(0.0, i->loss(0.0),1e-9);
}
/* ************************************************************************* */
@ -451,7 +453,7 @@ TEST(NoiseModel, WhitenInPlace)
/*
* These tests are responsible for testing the weight functions for the m-estimators in GTSAM.
* The weight function is related to the analytic derivative of the residual function. See
* The weight function is related to the analytic derivative of the loss function. See
* https://members.loria.fr/MOBerger/Enseignement/Master2/Documents/ZhangIVC-97-01.pdf
* for details. This weight function is required when optimizing cost functions with robust
* penalties using iteratively re-weighted least squares.
@ -467,10 +469,10 @@ TEST(NoiseModel, robustFunctionFair)
DOUBLES_EQUAL(0.3333333333333333, fair->weight(error3), 1e-8);
DOUBLES_EQUAL(0.8333333333333333, fair->weight(error4), 1e-8);
DOUBLES_EQUAL(0.441961080151135, fair->residual(error1), 1e-8);
DOUBLES_EQUAL(22.534692783297260, fair->residual(error2), 1e-8);
DOUBLES_EQUAL(22.534692783297260, fair->residual(error3), 1e-8);
DOUBLES_EQUAL(0.441961080151135, fair->residual(error4), 1e-8);
DOUBLES_EQUAL(0.441961080151135, fair->loss(error1), 1e-8);
DOUBLES_EQUAL(22.534692783297260, fair->loss(error2), 1e-8);
DOUBLES_EQUAL(22.534692783297260, fair->loss(error3), 1e-8);
DOUBLES_EQUAL(0.441961080151135, fair->loss(error4), 1e-8);
}
TEST(NoiseModel, robustFunctionHuber)
@ -483,10 +485,10 @@ TEST(NoiseModel, robustFunctionHuber)
DOUBLES_EQUAL(0.5, huber->weight(error3), 1e-8);
DOUBLES_EQUAL(1.0, huber->weight(error4), 1e-8);
DOUBLES_EQUAL(0.5000, huber->residual(error1), 1e-8);
DOUBLES_EQUAL(37.5000, huber->residual(error2), 1e-8);
DOUBLES_EQUAL(37.5000, huber->residual(error3), 1e-8);
DOUBLES_EQUAL(0.5000, huber->residual(error4), 1e-8);
DOUBLES_EQUAL(0.5000, huber->loss(error1), 1e-8);
DOUBLES_EQUAL(37.5000, huber->loss(error2), 1e-8);
DOUBLES_EQUAL(37.5000, huber->loss(error3), 1e-8);
DOUBLES_EQUAL(0.5000, huber->loss(error4), 1e-8);
}
TEST(NoiseModel, robustFunctionCauchy)
@ -499,10 +501,10 @@ TEST(NoiseModel, robustFunctionCauchy)
DOUBLES_EQUAL(0.2000, cauchy->weight(error3), 1e-8);
DOUBLES_EQUAL(0.961538461538461, cauchy->weight(error4), 1e-8);
DOUBLES_EQUAL(0.490258914416017, cauchy->residual(error1), 1e-8);
DOUBLES_EQUAL(20.117973905426254, cauchy->residual(error2), 1e-8);
DOUBLES_EQUAL(20.117973905426254, cauchy->residual(error3), 1e-8);
DOUBLES_EQUAL(0.490258914416017, cauchy->residual(error4), 1e-8);
DOUBLES_EQUAL(0.490258914416017, cauchy->loss(error1), 1e-8);
DOUBLES_EQUAL(20.117973905426254, cauchy->loss(error2), 1e-8);
DOUBLES_EQUAL(20.117973905426254, cauchy->loss(error3), 1e-8);
DOUBLES_EQUAL(0.490258914416017, cauchy->loss(error4), 1e-8);
}
TEST(NoiseModel, robustFunctionGemanMcClure)
@ -514,10 +516,10 @@ TEST(NoiseModel, robustFunctionGemanMcClure)
DOUBLES_EQUAL(9.80296e-5, gmc->weight(error3), 1e-8);
DOUBLES_EQUAL(0.25 , gmc->weight(error4), 1e-8);
DOUBLES_EQUAL(0.2500, gmc->residual(error1), 1e-8);
DOUBLES_EQUAL(0.495049504950495, gmc->residual(error2), 1e-8);
DOUBLES_EQUAL(0.495049504950495, gmc->residual(error3), 1e-8);
DOUBLES_EQUAL(0.2500, gmc->residual(error4), 1e-8);
DOUBLES_EQUAL(0.2500, gmc->loss(error1), 1e-8);
DOUBLES_EQUAL(0.495049504950495, gmc->loss(error2), 1e-8);
DOUBLES_EQUAL(0.495049504950495, gmc->loss(error3), 1e-8);
DOUBLES_EQUAL(0.2500, gmc->loss(error4), 1e-8);
}
TEST(NoiseModel, robustFunctionWelsch)
@ -530,10 +532,10 @@ TEST(NoiseModel, robustFunctionWelsch)
DOUBLES_EQUAL(0.018315638888734, welsch->weight(error3), 1e-8);
DOUBLES_EQUAL(0.960789439152323, welsch->weight(error4), 1e-8);
DOUBLES_EQUAL(0.490132010595960, welsch->residual(error1), 1e-8);
DOUBLES_EQUAL(12.271054513890823, welsch->residual(error2), 1e-8);
DOUBLES_EQUAL(12.271054513890823, welsch->residual(error3), 1e-8);
DOUBLES_EQUAL(0.490132010595960, welsch->residual(error4), 1e-8);
DOUBLES_EQUAL(0.490132010595960, welsch->loss(error1), 1e-8);
DOUBLES_EQUAL(12.271054513890823, welsch->loss(error2), 1e-8);
DOUBLES_EQUAL(12.271054513890823, welsch->loss(error3), 1e-8);
DOUBLES_EQUAL(0.490132010595960, welsch->loss(error4), 1e-8);
}
TEST(NoiseModel, robustFunctionTukey)
@ -546,10 +548,10 @@ TEST(NoiseModel, robustFunctionTukey)
DOUBLES_EQUAL(0.0, tukey->weight(error3), 1e-8);
DOUBLES_EQUAL(0.9216, tukey->weight(error4), 1e-8);
DOUBLES_EQUAL(0.480266666666667, tukey->residual(error1), 1e-8);
DOUBLES_EQUAL(4.166666666666667, tukey->residual(error2), 1e-8);
DOUBLES_EQUAL(4.166666666666667, tukey->residual(error3), 1e-8);
DOUBLES_EQUAL(0.480266666666667, tukey->residual(error4), 1e-8);
DOUBLES_EQUAL(0.480266666666667, tukey->loss(error1), 1e-8);
DOUBLES_EQUAL(4.166666666666667, tukey->loss(error2), 1e-8);
DOUBLES_EQUAL(4.166666666666667, tukey->loss(error3), 1e-8);
DOUBLES_EQUAL(0.480266666666667, tukey->loss(error4), 1e-8);
}
TEST(NoiseModel, robustFunctionDCS)
@ -560,8 +562,8 @@ TEST(NoiseModel, robustFunctionDCS)
DOUBLES_EQUAL(1.0 , dcs->weight(error1), 1e-8);
DOUBLES_EQUAL(0.00039211, dcs->weight(error2), 1e-8);
DOUBLES_EQUAL(0.5 , dcs->residual(error1), 1e-8);
DOUBLES_EQUAL(0.9900990099, dcs->residual(error2), 1e-8);
DOUBLES_EQUAL(0.5 , dcs->loss(error1), 1e-8);
DOUBLES_EQUAL(0.9900990099, dcs->loss(error2), 1e-8);
}
TEST(NoiseModel, robustFunctionL2WithDeadZone)
@ -576,12 +578,12 @@ TEST(NoiseModel, robustFunctionL2WithDeadZone)
DOUBLES_EQUAL(0.00990099009, lsdz->weight(e4), 1e-8);
DOUBLES_EQUAL(0.9, lsdz->weight(e5), 1e-8);
DOUBLES_EQUAL(40.5, lsdz->residual(e0), 1e-8);
DOUBLES_EQUAL(0.00005, lsdz->residual(e1), 1e-8);
DOUBLES_EQUAL(0.0, lsdz->residual(e2), 1e-8);
DOUBLES_EQUAL(0.0, lsdz->residual(e3), 1e-8);
DOUBLES_EQUAL(0.00005, lsdz->residual(e4), 1e-8);
DOUBLES_EQUAL(40.5, lsdz->residual(e5), 1e-8);
DOUBLES_EQUAL(40.5, lsdz->loss(e0), 1e-8);
DOUBLES_EQUAL(0.00005, lsdz->loss(e1), 1e-8);
DOUBLES_EQUAL(0.0, lsdz->loss(e2), 1e-8);
DOUBLES_EQUAL(0.0, lsdz->loss(e3), 1e-8);
DOUBLES_EQUAL(0.00005, lsdz->loss(e4), 1e-8);
DOUBLES_EQUAL(40.5, lsdz->loss(e5), 1e-8);
}
/* ************************************************************************* */
@ -665,11 +667,11 @@ TEST(NoiseModel, robustNoiseL2WithDeadZone)
/*
* TODO(mike): There is currently a bug in GTSAM, where none of the mEstimator classes
* implement a residual function, and GTSAM calls the weight function to evaluate the
* total penalty, rather than calling the residual function. The weight function should be
* implement a loss function, and GTSAM calls the weight function to evaluate the
* total penalty, rather than calling the loss function. The weight function should be
* used during iteratively reweighted least squares optimization, but should not be used to
* evaluate the total penalty. The long-term solution is for all mEstimators to implement
* both a weight and a residual function, and for GTSAM to call the residual function when
* both a weight and a loss function, and for GTSAM to call the loss function when
* evaluating the total penalty. This bug causes the test below to fail, so I'm leaving it
* commented out until the underlying bug in GTSAM is fixed.
*
@ -681,13 +683,44 @@ TEST(NoiseModel, robustNoiseL2WithDeadZone)
}
TEST(NoiseModel, lossFunctionAtZero)
{
const double k = 5.0;
auto fair = mEstimator::Fair::Create(k);
DOUBLES_EQUAL(fair->loss(0), 0, 1e-8);
DOUBLES_EQUAL(fair->weight(0), 1, 1e-8);
auto huber = mEstimator::Huber::Create(k);
DOUBLES_EQUAL(huber->loss(0), 0, 1e-8);
DOUBLES_EQUAL(huber->weight(0), 1, 1e-8);
auto cauchy = mEstimator::Cauchy::Create(k);
DOUBLES_EQUAL(cauchy->loss(0), 0, 1e-8);
DOUBLES_EQUAL(cauchy->weight(0), 1, 1e-8);
auto gmc = mEstimator::GemanMcClure::Create(k);
DOUBLES_EQUAL(gmc->loss(0), 0, 1e-8);
DOUBLES_EQUAL(gmc->weight(0), 1, 1e-8);
auto welsch = mEstimator::Welsch::Create(k);
DOUBLES_EQUAL(welsch->loss(0), 0, 1e-8);
DOUBLES_EQUAL(welsch->weight(0), 1, 1e-8);
auto tukey = mEstimator::Tukey::Create(k);
DOUBLES_EQUAL(tukey->loss(0), 0, 1e-8);
DOUBLES_EQUAL(tukey->weight(0), 1, 1e-8);
auto dcs = mEstimator::DCS::Create(k);
DOUBLES_EQUAL(dcs->loss(0), 0, 1e-8);
DOUBLES_EQUAL(dcs->weight(0), 1, 1e-8);
// auto lsdz = mEstimator::L2WithDeadZone::Create(k);
// DOUBLES_EQUAL(lsdz->loss(0), 0, 1e-8);
// DOUBLES_EQUAL(lsdz->weight(0), 1, 1e-8);
}
/* ************************************************************************* */
#define TEST_GAUSSIAN(gaussian)\
EQUALITY(info, gaussian->information());\
EQUALITY(cov, gaussian->covariance());\
EXPECT(assert_equal(white, gaussian->whiten(e)));\
EXPECT(assert_equal(e, gaussian->unwhiten(white)));\
EXPECT_DOUBLES_EQUAL(251, gaussian->distance(e), 1e-9);\
EXPECT_DOUBLES_EQUAL(251.0, gaussian->squaredMahalanobisDistance(e), 1e-9);\
EXPECT_DOUBLES_EQUAL(0.5 * 251.0, gaussian->loss(251.0), 1e-9);\
Matrix A = R.inverse(); Vector b = e;\
gaussian->WhitenSystem(A, b);\
EXPECT(assert_equal(I, A));\

37
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -17,9 +17,11 @@
* @author Vadim Indelman
* @author David Jensen
* @author Frank Dellaert
* @author Varun Agrawal
**/
#include <gtsam/navigation/CombinedImuFactor.h>
#include <boost/serialization/export.hpp>
/* External or standard includes */
#include <ostream>
@ -28,6 +30,31 @@ namespace gtsam {
using namespace std;
//------------------------------------------------------------------------------
// Inner class PreintegrationCombinedParams
//------------------------------------------------------------------------------
void PreintegrationCombinedParams::print(const string& s) const {
PreintegrationParams::print(s);
cout << "biasAccCovariance:\n[\n" << biasAccCovariance << "\n]"
<< endl;
cout << "biasOmegaCovariance:\n[\n" << biasOmegaCovariance << "\n]"
<< endl;
cout << "biasAccOmegaInt:\n[\n" << biasAccOmegaInt << "\n]"
<< endl;
}
//------------------------------------------------------------------------------
bool PreintegrationCombinedParams::equals(const PreintegratedRotationParams& other,
double tol) const {
auto e = dynamic_cast<const PreintegrationCombinedParams*>(&other);
return e != nullptr && PreintegrationParams::equals(other, tol) &&
equal_with_abs_tol(biasAccCovariance, e->biasAccCovariance,
tol) &&
equal_with_abs_tol(biasOmegaCovariance, e->biasOmegaCovariance,
tol) &&
equal_with_abs_tol(biasAccOmegaInt, e->biasAccOmegaInt, tol);
}
//------------------------------------------------------------------------------
// Inner class PreintegratedCombinedMeasurements
//------------------------------------------------------------------------------
@ -242,6 +269,13 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i,
return r;
}
//------------------------------------------------------------------------------
std::ostream& operator<<(std::ostream& os, const CombinedImuFactor& f) {
f._PIM_.print("combined preintegrated measurements:\n");
os << " noise model sigmas: " << f.noiseModel_->sigmas().transpose();
return os;
}
//------------------------------------------------------------------------------
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
CombinedImuFactor::CombinedImuFactor(
@ -279,3 +313,6 @@ void CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
}
/// namespace gtsam
/// Boost serialization export definition for derived class
BOOST_CLASS_EXPORT_IMPLEMENT(gtsam::PreintegrationCombinedParams);

101
gtsam/navigation/CombinedImuFactor.h
View File

@ -17,6 +17,7 @@
* @author Vadim Indelman
* @author David Jensen
* @author Frank Dellaert
* @author Varun Agrawal
**/
#pragma once
@ -26,6 +27,7 @@
#include <gtsam/navigation/TangentPreintegration.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/serialization.h>
namespace gtsam {
@ -61,10 +63,18 @@ struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams {
Matrix3 biasOmegaCovariance; ///< continuous-time "Covariance" describing gyroscope bias random walk
Matrix6 biasAccOmegaInt; ///< covariance of bias used for pre-integration
/// Default constructor makes uninitialized params struct.
/// Used for serialization.
PreintegrationCombinedParams()
: biasAccCovariance(I_3x3),
biasOmegaCovariance(I_3x3),
biasAccOmegaInt(I_6x6) {}
/// See two named constructors below for good values of n_gravity in body frame
PreintegrationCombinedParams(const Vector3& n_gravity) :
PreintegrationParams(n_gravity), biasAccCovariance(I_3x3), biasOmegaCovariance(
I_3x3), biasAccOmegaInt(I_6x6) {
PreintegrationCombinedParams(const Vector3& n_gravity) :
PreintegrationParams(n_gravity), biasAccCovariance(I_3x3),
biasOmegaCovariance(I_3x3), biasAccOmegaInt(I_6x6) {
}
// Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis
@ -77,6 +87,9 @@ PreintegrationCombinedParams(const Vector3& n_gravity) :
return boost::shared_ptr<PreintegrationCombinedParams>(new PreintegrationCombinedParams(Vector3(0, 0, -g)));
}
void print(const std::string& s="") const;
bool equals(const PreintegratedRotationParams& other, double tol) const;
void setBiasAccCovariance(const Matrix3& cov) { biasAccCovariance=cov; }
void setBiasOmegaCovariance(const Matrix3& cov) { biasOmegaCovariance=cov; }
void setBiasAccOmegaInt(const Matrix6& cov) { biasAccOmegaInt=cov; }
@ -86,24 +99,22 @@ PreintegrationCombinedParams(const Vector3& n_gravity) :
const Matrix6& getBiasAccOmegaInt() const { return biasAccOmegaInt; }
private:
/// Default constructor makes unitialized params struct
PreintegrationCombinedParams() {}
/** Serialization function */
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation::Params);
ar& BOOST_SERIALIZATION_NVP(biasAccCovariance);
ar& BOOST_SERIALIZATION_NVP(biasOmegaCovariance);
ar& BOOST_SERIALIZATION_NVP(biasAccOmegaInt);
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
namespace bs = ::boost::serialization;
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationParams);
ar & BOOST_SERIALIZATION_NVP(biasAccCovariance);
ar & BOOST_SERIALIZATION_NVP(biasOmegaCovariance);
ar & BOOST_SERIALIZATION_NVP(biasAccOmegaInt);
}
public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
/**
* PreintegratedCombinedMeasurements integrates the IMU measurements
* (rotation rates and accelerations) and the corresponding covariance matrix.
@ -128,7 +139,6 @@ public:
*/
Eigen::Matrix<double, 15, 15> preintMeasCov_;
friend class CombinedImuFactor;
public:
@ -136,11 +146,14 @@ public:
/// @{
/// Default constructor only for serialization and Cython wrapper
PreintegratedCombinedMeasurements() {}
PreintegratedCombinedMeasurements() {
preintMeasCov_.setZero();
}
/**
* Default constructor, initializes the class with no measurements
* @param bias Current estimate of acceleration and rotation rate biases
* @param p Parameters, typically fixed in a single application
* @param biasHat Current estimate of acceleration and rotation rate biases
*/
PreintegratedCombinedMeasurements(
const boost::shared_ptr<Params>& p,
@ -149,6 +162,19 @@ public:
preintMeasCov_.setZero();
}
/**
* Construct preintegrated directly from members: base class and preintMeasCov
* @param base PreintegrationType instance
* @param preintMeasCov Covariance matrix used in noise model.
*/
PreintegratedCombinedMeasurements(const PreintegrationType& base, const Eigen::Matrix<double, 15, 15>& preintMeasCov)
: PreintegrationType(base),
preintMeasCov_(preintMeasCov) {
}
/// Virtual destructor
virtual ~PreintegratedCombinedMeasurements() {}
/// @}
/// @name Basic utilities
@ -158,20 +184,25 @@ public:
void resetIntegration() override;
/// const reference to params, shadows definition in base class
Params& p() const { return *boost::static_pointer_cast<Params>(this->p_);}
Params& p() const { return *boost::static_pointer_cast<Params>(this->p_); }
/// @}
/// @name Access instance variables
/// @{
/// Return pre-integrated measurement covariance
Matrix preintMeasCov() const { return preintMeasCov_; }
/// @}
/// @name Testable
/// @{
/// print
void print(const std::string& s = "Preintegrated Measurements:") const override;
bool equals(const PreintegratedCombinedMeasurements& expected, double tol = 1e-9) const;
/// equals
bool equals(const PreintegratedCombinedMeasurements& expected,
double tol = 1e-9) const;
/// @}
/// @name Main functionality
/// @{
@ -205,6 +236,7 @@ public:
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
namespace bs = ::boost::serialization;
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationType);
ar& BOOST_SERIALIZATION_NVP(preintMeasCov_);
}
@ -244,9 +276,6 @@ private:
PreintegratedCombinedMeasurements _PIM_;
/** Default constructor - only use for serialization */
CombinedImuFactor() {}
public:
/** Shorthand for a smart pointer to a factor */
@ -256,6 +285,9 @@ public:
typedef boost::shared_ptr<CombinedImuFactor> shared_ptr;
#endif
/** Default constructor - only use for serialization */
CombinedImuFactor() {}
/**
* Constructor
* @param pose_i Previous pose key
@ -277,12 +309,17 @@ public:
/** implement functions needed for Testable */
/// @name Testable
/// @{
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
const CombinedImuFactor&);
/// print
virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const;
/// equals
virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
/// @}
/** Access the preintegrated measurements. */
@ -321,14 +358,12 @@ public:
#endif
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & boost::serialization::make_nvp("NoiseModelFactor6",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(_PIM_);
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(NoiseModelFactor6);
ar& BOOST_SERIALIZATION_NVP(_PIM_);
}
public:
@ -336,4 +371,18 @@ public:
};
// class CombinedImuFactor
} /// namespace gtsam
template <>
struct traits<PreintegrationCombinedParams>
: public Testable<PreintegrationCombinedParams> {};
template <>
struct traits<PreintegratedCombinedMeasurements>
: public Testable<PreintegratedCombinedMeasurements> {};
template <>
struct traits<CombinedImuFactor> : public Testable<CombinedImuFactor> {};
} // namespace gtsam
/// Add Boost serialization export key (declaration) for derived class
BOOST_CLASS_EXPORT_KEY(gtsam::PreintegrationCombinedParams);

8
gtsam/navigation/ImuFactor.h
View File

@ -59,7 +59,7 @@ typedef ManifoldPreintegration PreintegrationType;
*/
/**
* PreintegratedIMUMeasurements accumulates (integrates) the IMU measurements
* PreintegratedImuMeasurements accumulates (integrates) the IMU measurements
* (rotation rates and accelerations) and the corresponding covariance matrix.
* The measurements are then used to build the Preintegrated IMU factor.
* Integration is done incrementally (ideally, one integrates the measurement
@ -87,8 +87,8 @@ public:
/**
* Constructor, initializes the class with no measurements
* @param bias Current estimate of acceleration and rotation rate biases
* @param p Parameters, typically fixed in a single application
* @param p Parameters, typically fixed in a single application
* @param biasHat Current estimate of acceleration and rotation rate biases
*/
PreintegratedImuMeasurements(const boost::shared_ptr<PreintegrationParams>& p,
const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) :
@ -164,7 +164,7 @@ private:
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
namespace bs = ::boost::serialization;
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationType);
ar & bs::make_nvp("preintMeasCov_", bs::make_array(preintMeasCov_.data(), preintMeasCov_.size()));
ar & BOOST_SERIALIZATION_NVP(preintMeasCov_);
}
};

14
gtsam/navigation/ManifoldPreintegration.h
View File

@ -118,15 +118,13 @@ private:
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
namespace bs = ::boost::serialization;
ar & BOOST_SERIALIZATION_NVP(p_);
ar & BOOST_SERIALIZATION_NVP(deltaTij_);
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase);
ar & BOOST_SERIALIZATION_NVP(deltaXij_);
ar & BOOST_SERIALIZATION_NVP(biasHat_);
ar & bs::make_nvp("delRdelBiasOmega_", bs::make_array(delRdelBiasOmega_.data(), delRdelBiasOmega_.size()));
ar & bs::make_nvp("delPdelBiasAcc_", bs::make_array(delPdelBiasAcc_.data(), delPdelBiasAcc_.size()));
ar & bs::make_nvp("delPdelBiasOmega_", bs::make_array(delPdelBiasOmega_.data(), delPdelBiasOmega_.size()));
ar & bs::make_nvp("delVdelBiasAcc_", bs::make_array(delVdelBiasAcc_.data(), delVdelBiasAcc_.size()));
ar & bs::make_nvp("delVdelBiasOmega_", bs::make_array(delVdelBiasOmega_.data(), delVdelBiasOmega_.size()));
ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_);
ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_);
ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_);
ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_);
}
};

6
gtsam/navigation/PreintegratedRotation.cpp
View File

@ -25,7 +25,7 @@ using namespace std;
namespace gtsam {
void PreintegratedRotation::Params::print(const string& s) const {
void PreintegratedRotationParams::print(const string& s) const {
cout << s << endl;
cout << "gyroscopeCovariance:\n[\n" << gyroscopeCovariance << "\n]" << endl;
if (omegaCoriolis)
@ -34,8 +34,8 @@ void PreintegratedRotation::Params::print(const string& s) const {
body_P_sensor->print("body_P_sensor");
}
bool PreintegratedRotation::Params::equals(
const PreintegratedRotation::Params& other, double tol) const {
bool PreintegratedRotationParams::equals(
const PreintegratedRotationParams& other, double tol) const {
if (body_P_sensor) {
if (!other.body_P_sensor
|| !assert_equal(*body_P_sensor, *other.body_P_sensor, tol))

10
gtsam/navigation/PreintegratedRotation.h
View File

@ -61,9 +61,15 @@ struct GTSAM_EXPORT PreintegratedRotationParams {
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
namespace bs = ::boost::serialization;
ar & bs::make_nvp("gyroscopeCovariance", bs::make_array(gyroscopeCovariance.data(), gyroscopeCovariance.size()));
ar & BOOST_SERIALIZATION_NVP(omegaCoriolis);
ar & BOOST_SERIALIZATION_NVP(gyroscopeCovariance);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor);
// Provide support for Eigen::Matrix in boost::optional
bool omegaCoriolisFlag = omegaCoriolis.is_initialized();
ar & boost::serialization::make_nvp("omegaCoriolisFlag", omegaCoriolisFlag);
if (omegaCoriolisFlag) {
ar & BOOST_SERIALIZATION_NVP(*omegaCoriolis);
}
}
#ifdef GTSAM_USE_QUATERNIONS

10
gtsam/navigation/PreintegrationBase.h
View File

@ -213,6 +213,16 @@ class GTSAM_EXPORT PreintegrationBase {
/// @}
#endif
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_NVP(p_);
ar & BOOST_SERIALIZATION_NVP(biasHat_);
ar & BOOST_SERIALIZATION_NVP(deltaTij_);
}
public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};

6
gtsam/navigation/PreintegrationParams.cpp
View File

@ -27,7 +27,7 @@ namespace gtsam {
//------------------------------------------------------------------------------
void PreintegrationParams::print(const string& s) const {
PreintegratedRotation::Params::print(s);
PreintegratedRotationParams::print(s);
cout << "accelerometerCovariance:\n[\n" << accelerometerCovariance << "\n]"
<< endl;
cout << "integrationCovariance:\n[\n" << integrationCovariance << "\n]"
@ -39,10 +39,10 @@ void PreintegrationParams::print(const string& s) const {
}
//------------------------------------------------------------------------------
bool PreintegrationParams::equals(const PreintegratedRotation::Params& other,
bool PreintegrationParams::equals(const PreintegratedRotationParams& other,
double tol) const {
auto e = dynamic_cast<const PreintegrationParams*>(&other);
return e != nullptr && PreintegratedRotation::Params::equals(other, tol) &&
return e != nullptr && PreintegratedRotationParams::equals(other, tol) &&
use2ndOrderCoriolis == e->use2ndOrderCoriolis &&
equal_with_abs_tol(accelerometerCovariance, e->accelerometerCovariance,
tol) &&

17
gtsam/navigation/PreintegrationParams.h
View File

@ -31,7 +31,8 @@ struct GTSAM_EXPORT PreintegrationParams: PreintegratedRotationParams {
/// Default constructor for serialization only
PreintegrationParams()
: accelerometerCovariance(I_3x3),
: PreintegratedRotationParams(),
accelerometerCovariance(I_3x3),
integrationCovariance(I_3x3),
use2ndOrderCoriolis(false),
n_gravity(0, 0, -1) {}
@ -39,7 +40,8 @@ struct GTSAM_EXPORT PreintegrationParams: PreintegratedRotationParams {
/// The Params constructor insists on getting the navigation frame gravity vector
/// For convenience, two commonly used conventions are provided by named constructors below
PreintegrationParams(const Vector3& n_gravity)
: accelerometerCovariance(I_3x3),
: PreintegratedRotationParams(),
accelerometerCovariance(I_3x3),
integrationCovariance(I_3x3),
use2ndOrderCoriolis(false),
n_gravity(n_gravity) {}
@ -54,8 +56,8 @@ struct GTSAM_EXPORT PreintegrationParams: PreintegratedRotationParams {
return boost::shared_ptr<PreintegrationParams>(new PreintegrationParams(Vector3(0, 0, -g)));
}
void print(const std::string& s) const;
bool equals(const PreintegratedRotation::Params& other, double tol) const;
void print(const std::string& s="") const;
bool equals(const PreintegratedRotationParams& other, double tol) const;
void setAccelerometerCovariance(const Matrix3& cov) { accelerometerCovariance = cov; }
void setIntegrationCovariance(const Matrix3& cov) { integrationCovariance = cov; }
@ -73,10 +75,9 @@ protected:
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
namespace bs = ::boost::serialization;
ar & boost::serialization::make_nvp("PreintegratedRotation_Params",
boost::serialization::base_object<PreintegratedRotationParams>(*this));
ar & bs::make_nvp("accelerometerCovariance", bs::make_array(accelerometerCovariance.data(), accelerometerCovariance.size()));
ar & bs::make_nvp("integrationCovariance", bs::make_array(integrationCovariance.data(), integrationCovariance.size()));
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotationParams);
ar & BOOST_SERIALIZATION_NVP(accelerometerCovariance);
ar & BOOST_SERIALIZATION_NVP(integrationCovariance);
ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis);
ar & BOOST_SERIALIZATION_NVP(n_gravity);
}

10
gtsam/navigation/TangentPreintegration.h
View File

@ -132,12 +132,10 @@ private:
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
namespace bs = ::boost::serialization;
ar & BOOST_SERIALIZATION_NVP(p_);
ar & BOOST_SERIALIZATION_NVP(biasHat_);
ar & BOOST_SERIALIZATION_NVP(deltaTij_);
ar & bs::make_nvp("preintegrated_", bs::make_array(preintegrated_.data(), preintegrated_.size()));
ar & bs::make_nvp("preintegrated_H_biasAcc_", bs::make_array(preintegrated_H_biasAcc_.data(), preintegrated_H_biasAcc_.size()));
ar & bs::make_nvp("preintegrated_H_biasOmega_", bs::make_array(preintegrated_H_biasOmega_.data(), preintegrated_H_biasOmega_.size()));
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase);
ar & BOOST_SERIALIZATION_NVP(preintegrated_);
ar & BOOST_SERIALIZATION_NVP(preintegrated_H_biasAcc_);
ar & BOOST_SERIALIZATION_NVP(preintegrated_H_biasOmega_);
}
public:

60
gtsam/navigation/tests/testImuFactorSerialization.cpp
View File

@ -16,15 +16,19 @@
* @author Frank Dellaert
* @author Richard Roberts
* @author Stephen Williams
* @author Varun Agrawal
*/
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/navigation/ImuFactor.h>
#include <fstream>
using namespace std;
using namespace gtsam;
using namespace gtsam::serializationTestHelpers;
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained,
"gtsam_noiseModel_Constrained");
@ -38,23 +42,23 @@ BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic,
BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel");
BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
TEST(ImuFactor, serialization) {
using namespace gtsam::serializationTestHelpers;
template <typename P>
P getPreintegratedMeasurements() {
// Create default parameters with Z-down and above noise paramaters
auto p = PreintegrationParams::MakeSharedD(9.81);
p->body_P_sensor = Pose3(Rot3::Ypr(0, 0, M_PI), Point3(0,0,0));
auto p = P::Params::MakeSharedD(9.81);
p->body_P_sensor = Pose3(Rot3::Ypr(0, 0, M_PI), Point3(0, 0, 0));
p->accelerometerCovariance = 1e-7 * I_3x3;
p->gyroscopeCovariance = 1e-8 * I_3x3;
p->integrationCovariance = 1e-9 * I_3x3;
const double deltaT = 0.005;
const imuBias::ConstantBias priorBias(
Vector3(0, 0, 0), Vector3(0, 0.01, 0)); // Biases (acc, rot)
PreintegratedImuMeasurements pim(p, priorBias);
// Biases (acc, rot)
const imuBias::ConstantBias priorBias(Vector3(0, 0, 0), Vector3(0, 0.01, 0));
// measurements are needed for non-inf noise model, otherwise will throw err
P pim(p, priorBias);
// measurements are needed for non-inf noise model, otherwise will throw error
// when deserialize
const Vector3 measuredOmega(0, 0.01, 0);
const Vector3 measuredAcc(0, 0, -9.81);
@ -62,6 +66,16 @@ TEST(ImuFactor, serialization) {
for (int j = 0; j < 200; ++j)
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
return pim;
}
TEST(ImuFactor, serialization) {
auto pim = getPreintegratedMeasurements<PreintegratedImuMeasurements>();
EXPECT(equalsObj<PreintegratedImuMeasurements>(pim));
EXPECT(equalsXML<PreintegratedImuMeasurements>(pim));
EXPECT(equalsBinary<PreintegratedImuMeasurements>(pim));
ImuFactor factor(1, 2, 3, 4, 5, pim);
EXPECT(equalsObj<ImuFactor>(factor));
@ -69,6 +83,30 @@ TEST(ImuFactor, serialization) {
EXPECT(equalsBinary<ImuFactor>(factor));
}
TEST(ImuFactor2, serialization) {
auto pim = getPreintegratedMeasurements<PreintegratedImuMeasurements>();
ImuFactor2 factor(1, 2, 3, pim);
EXPECT(equalsObj<ImuFactor2>(factor));
EXPECT(equalsXML<ImuFactor2>(factor));
EXPECT(equalsBinary<ImuFactor2>(factor));
}
TEST(CombinedImuFactor, Serialization) {
auto pim = getPreintegratedMeasurements<PreintegratedCombinedMeasurements>();
EXPECT(equalsObj<PreintegratedCombinedMeasurements>(pim));
EXPECT(equalsXML<PreintegratedCombinedMeasurements>(pim));
EXPECT(equalsBinary<PreintegratedCombinedMeasurements>(pim));
const CombinedImuFactor factor(1, 2, 3, 4, 5, 6, pim);
EXPECT(equalsObj<CombinedImuFactor>(factor));
EXPECT(equalsXML<CombinedImuFactor>(factor));
EXPECT(equalsBinary<CombinedImuFactor>(factor));
}
/* ************************************************************************* */
int main() {
TestResult tr;

2
gtsam/nonlinear/AdaptAutoDiff.h
View File

@ -57,7 +57,7 @@ class AdaptAutoDiff {
if (H1 || H2) {
// Get derivatives with AutoDiff
const double* parameters[] = {v1.data(), v2.data()};
double rowMajor1[M * N1], rowMajor2[M * N2]; // on the stack
double rowMajor1[M * N1] = {}, rowMajor2[M * N2] = {}; // on the stack
double* jacobians[] = {rowMajor1, rowMajor2};
success = AutoDiff<FUNCTOR, double, N1, N2>::Differentiate(
f, parameters, M, result.data(), jacobians);

148
gtsam/nonlinear/FunctorizedFactor.h Normal file
View File

@ -0,0 +1,148 @@
/* ----------------------------------------------------------------------------
* 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 FunctorizedFactor.h
* @date May 31, 2020
* @author Varun Agrawal
**/
#pragma once
#include <gtsam/base/Testable.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <cmath>
namespace gtsam {
/**
* Factor which evaluates provided unary functor and uses the result to compute
* error with respect to the provided measurement.
*
* Template parameters are
* @param R: The return type of the functor after evaluation.
* @param T: The argument type for the functor.
*
* Example:
* Key key = Symbol('X', 0);
* auto model = noiseModel::Isotropic::Sigma(9, 1);
*
* /// Functor that takes a matrix and multiplies every element by m
* class MultiplyFunctor {
* double m_; ///< simple multiplier
* public:
* MultiplyFunctor(double m) : m_(m) {}
* Matrix operator()(const Matrix &X,
* OptionalJacobian<-1, -1> H = boost::none) const {
* if (H)
* *H = m_ * Matrix::Identity(X.rows()*X.cols(), X.rows()*X.cols());
* return m_ * X;
* }
* };
*
* Matrix measurement = Matrix::Identity(3, 3);
* double multiplier = 2.0;
*
* FunctorizedFactor<Matrix, Matrix> factor(keyX, measurement, model,
* MultiplyFunctor(multiplier));
*/
template <typename R, typename T>
class GTSAM_EXPORT FunctorizedFactor : public NoiseModelFactor1<T> {
private:
using Base = NoiseModelFactor1<T>;
R measured_; ///< value that is compared with functor return value
SharedNoiseModel noiseModel_; ///< noise model
std::function<R(T, boost::optional<Matrix &>)> func_; ///< functor instance
public:
/** default constructor - only use for serialization */
FunctorizedFactor() {}
/** Construct with given x and the parameters of the basis
*
* @param key: Factor key
* @param z: Measurement object of same type as that returned by functor
* @param model: Noise model
* @param func: The instance of the functor object
*/
FunctorizedFactor(Key key, const R &z, const SharedNoiseModel &model,
const std::function<R(T, boost::optional<Matrix &>)> func)
: Base(model, key), measured_(z), noiseModel_(model), func_(func) {}
virtual ~FunctorizedFactor() {}
/// @return a deep copy of this factor
virtual NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<NonlinearFactor>(
NonlinearFactor::shared_ptr(new FunctorizedFactor<R, T>(*this)));
}
Vector evaluateError(const T &params,
boost::optional<Matrix &> H = boost::none) const {
R x = func_(params, H);
Vector error = traits<R>::Local(measured_, x);
return error;
}
/// @name Testable
/// @{
void print(const std::string &s = "",
const KeyFormatter &keyFormatter = DefaultKeyFormatter) const {
Base::print(s, keyFormatter);
std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor("
<< keyFormatter(this->key()) << ")" << std::endl;
traits<R>::Print(measured_, " measurement: ");
std::cout << " noise model sigmas: " << noiseModel_->sigmas().transpose()
<< std::endl;
}
virtual bool equals(const NonlinearFactor &other, double tol = 1e-9) const {
const FunctorizedFactor<R, T> *e =
dynamic_cast<const FunctorizedFactor<R, T> *>(&other);
const bool base = Base::equals(*e, tol);
return e && Base::equals(other, tol) &&
traits<R>::Equals(this->measured_, e->measured_, tol);
}
/// @}
private:
/** Serialization function */
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
ar &boost::serialization::make_nvp(
"NoiseModelFactor1", boost::serialization::base_object<Base>(*this));
ar &BOOST_SERIALIZATION_NVP(measured_);
ar &BOOST_SERIALIZATION_NVP(func_);
}
};
/// traits
template <typename R, typename T>
struct traits<FunctorizedFactor<R, T>>
: public Testable<FunctorizedFactor<R, T>> {};
/**
* Helper function to create a functorized factor.
*
* Uses function template deduction to identify return type and functor type, so
* template list only needs the functor argument type.
*/
template <typename T, typename R, typename FUNC>
FunctorizedFactor<R, T> MakeFunctorizedFactor(Key key, const R &z,
const SharedNoiseModel &model,
const FUNC func) {
return FunctorizedFactor<R, T>(key, z, model, func);
}
} // namespace gtsam

2
gtsam/nonlinear/NonlinearFactor.cpp
View File

@ -121,7 +121,7 @@ double NoiseModelFactor::error(const Values& c) const {
const Vector b = unwhitenedError(c);
check(noiseModel_, b.size());
if (noiseModel_)
return 0.5 * noiseModel_->distance(b);
return noiseModel_->loss(noiseModel_->squaredMahalanobisDistance(b));
else
return 0.5 * b.squaredNorm();
} else {

6
gtsam/nonlinear/internal/ExecutionTrace.h
View File

@ -169,6 +169,12 @@ class ExecutionTrace {
content.ptr->reverseAD2(dTdA, jacobians);
}
~ExecutionTrace() {
if (kind == Function) {
content.ptr->~CallRecord<Dim>();
}
}
/// Define type so we can apply it as a meta-function
typedef ExecutionTrace<T> type;
};

1
gtsam/nonlinear/tests/testExecutionTrace.cpp
View File

@ -16,6 +16,7 @@
* @brief unit tests for Expression internals
*/
#include <gtsam/nonlinear/internal/CallRecord.h>
#include <gtsam/nonlinear/internal/ExecutionTrace.h>
#include <gtsam/geometry/Point2.h>

185
gtsam/nonlinear/tests/testFunctorizedFactor.cpp Normal file
View File

@ -0,0 +1,185 @@
/* ----------------------------------------------------------------------------
* 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
* -------------------------------1-------------------------------------------
*/
/**
* @file testFunctorizedFactor.cpp
* @date May 31, 2020
* @author Varun Agrawal
* @brief unit tests for FunctorizedFactor class
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/FunctorizedFactor.h>
#include <gtsam/nonlinear/factorTesting.h>
using namespace std;
using namespace gtsam;
Key key = Symbol('X', 0);
auto model = noiseModel::Isotropic::Sigma(9, 1);
/// Functor that takes a matrix and multiplies every element by m
class MultiplyFunctor {
double m_; ///< simple multiplier
public:
MultiplyFunctor(double m) : m_(m) {}
Matrix operator()(const Matrix &X,
OptionalJacobian<-1, -1> H = boost::none) const {
if (H) *H = m_ * Matrix::Identity(X.rows() * X.cols(), X.rows() * X.cols());
return m_ * X;
}
};
/* ************************************************************************* */
// Test identity operation for FunctorizedFactor.
TEST(FunctorizedFactor, Identity) {
Matrix X = Matrix::Identity(3, 3), measurement = Matrix::Identity(3, 3);
double multiplier = 1.0;
auto functor = MultiplyFunctor(multiplier);
auto factor = MakeFunctorizedFactor<Matrix>(key, measurement, model, functor);
Vector error = factor.evaluateError(X);
EXPECT(assert_equal(Vector::Zero(9), error, 1e-9));
}
/* ************************************************************************* */
// Test FunctorizedFactor with multiplier value of 2.
TEST(FunctorizedFactor, Multiply2) {
double multiplier = 2.0;
Matrix X = Matrix::Identity(3, 3);
Matrix measurement = multiplier * Matrix::Identity(3, 3);
auto factor = MakeFunctorizedFactor<Matrix>(key, measurement, model,
MultiplyFunctor(multiplier));
Vector error = factor.evaluateError(X);
EXPECT(assert_equal(Vector::Zero(9), error, 1e-9));
}
/* ************************************************************************* */
// Test equality function for FunctorizedFactor.
TEST(FunctorizedFactor, Equality) {
Matrix measurement = Matrix::Identity(2, 2);
double multiplier = 2.0;
auto factor1 = MakeFunctorizedFactor<Matrix>(key, measurement, model,
MultiplyFunctor(multiplier));
auto factor2 = MakeFunctorizedFactor<Matrix>(key, measurement, model,
MultiplyFunctor(multiplier));
EXPECT(factor1.equals(factor2));
}
/* *************************************************************************** */
// Test Jacobians of FunctorizedFactor.
TEST(FunctorizedFactor, Jacobians) {
Matrix X = Matrix::Identity(3, 3);
Matrix actualH;
double multiplier = 2.0;
auto factor =
MakeFunctorizedFactor<Matrix>(key, X, model, MultiplyFunctor(multiplier));
Values values;
values.insert<Matrix>(key, X);
// Check Jacobians
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
}
/* ************************************************************************* */
// Test print result of FunctorizedFactor.
TEST(FunctorizedFactor, Print) {
Matrix X = Matrix::Identity(2, 2);
double multiplier = 2.0;
auto factor =
MakeFunctorizedFactor<Matrix>(key, X, model, MultiplyFunctor(multiplier));
// redirect output to buffer so we can compare
stringstream buffer;
streambuf *old = cout.rdbuf(buffer.rdbuf());
factor.print();
// get output string and reset stdout
string actual = buffer.str();
cout.rdbuf(old);
string expected =
" keys = { X0 }\n"
" noise model: unit (9) \n"
"FunctorizedFactor(X0)\n"
" measurement: [\n"
" 1, 0;\n"
" 0, 1\n"
"]\n"
" noise model sigmas: 1 1 1 1 1 1 1 1 1\n";
CHECK_EQUAL(expected, actual);
}
/* ************************************************************************* */
// Test FunctorizedFactor using a std::function type.
TEST(FunctorizedFactor, Functional) {
double multiplier = 2.0;
Matrix X = Matrix::Identity(3, 3);
Matrix measurement = multiplier * Matrix::Identity(3, 3);
std::function<Matrix(Matrix, boost::optional<Matrix &>)> functional =
MultiplyFunctor(multiplier);
auto factor =
MakeFunctorizedFactor<Matrix>(key, measurement, model, functional);
Vector error = factor.evaluateError(X);
EXPECT(assert_equal(Vector::Zero(9), error, 1e-9));
}
/* ************************************************************************* */
// Test FunctorizedFactor with a lambda function.
TEST(FunctorizedFactor, Lambda) {
double multiplier = 2.0;
Matrix X = Matrix::Identity(3, 3);
Matrix measurement = multiplier * Matrix::Identity(3, 3);
auto lambda = [multiplier](const Matrix &X,
OptionalJacobian<-1, -1> H = boost::none) {
if (H)
*H = multiplier *
Matrix::Identity(X.rows() * X.cols(), X.rows() * X.cols());
return multiplier * X;
};
// FunctorizedFactor<Matrix> factor(key, measurement, model, lambda);
auto factor = MakeFunctorizedFactor<Matrix>(key, measurement, model, lambda);
Vector error = factor.evaluateError(X);
EXPECT(assert_equal(Vector::Zero(9), error, 1e-9));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

3
gtsam/sfm/TranslationFactor.h
View File

@ -67,8 +67,7 @@ class TranslationFactor : public NoiseModelFactor2<Point3, Point3> {
boost::optional<Matrix&> H2 = boost::none) const override {
const Point3 dir = Tb - Ta;
Matrix33 H_predicted_dir;
const Point3 predicted =
dir.normalized(H1 || H2 ? &H_predicted_dir : nullptr);
const Point3 predicted = normalize(dir, H1 || H2 ? &H_predicted_dir : nullptr);
if (H1) *H1 = -H_predicted_dir;
if (H2) *H2 = H_predicted_dir;
return predicted - measured_w_aZb_;

117
gtsam/slam/FrobeniusFactor.cpp Normal file
View File

@ -0,0 +1,117 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2019, 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 FrobeniusFactor.cpp
* @date March 2019
* @author Frank Dellaert
* @brief Various factors that minimize some Frobenius norm
*/
#include <gtsam/slam/FrobeniusFactor.h>
#include <gtsam/base/timing.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <cmath>
#include <iostream>
#include <vector>
using namespace std;
namespace gtsam {
//******************************************************************************
boost::shared_ptr<noiseModel::Isotropic> ConvertPose3NoiseModel(
const SharedNoiseModel& model, size_t d, bool defaultToUnit) {
double sigma = 1.0;
if (model != nullptr) {
if (model->dim() != 6) {
if (!defaultToUnit)
throw std::runtime_error("Can only convert Pose3 noise models");
} else {
auto sigmas = model->sigmas().head(3).eval();
if (sigmas(1) != sigmas(0) || sigmas(2) != sigmas(0)) {
if (!defaultToUnit)
throw std::runtime_error("Can only convert isotropic rotation noise");
} else {
sigma = sigmas(0);
}
}
}
return noiseModel::Isotropic::Sigma(d, sigma);
}
//******************************************************************************
FrobeniusWormholeFactor::FrobeniusWormholeFactor(Key j1, Key j2, const Rot3& R12,
size_t p,
const SharedNoiseModel& model)
: NoiseModelFactor2<SOn, SOn>(ConvertPose3NoiseModel(model, p * 3), j1, j2),
M_(R12.matrix()), // 3*3 in all cases
p_(p), // 4 for SO(4)
pp_(p * p), // 16 for SO(4)
dimension_(SOn::Dimension(p)), // 6 for SO(4)
G_(pp_, dimension_) // 16*6 for SO(4)
{
// Calculate G matrix of vectorized generators
Matrix Z = Matrix::Zero(p, p);
for (size_t j = 0; j < dimension_; j++) {
const auto X = SOn::Hat(Eigen::VectorXd::Unit(dimension_, j));
G_.col(j) = Eigen::Map<const Matrix>(X.data(), pp_, 1);
}
assert(noiseModel()->dim() == 3 * p_);
}
//******************************************************************************
Vector FrobeniusWormholeFactor::evaluateError(
const SOn& Q1, const SOn& Q2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
gttic(FrobeniusWormholeFactorP_evaluateError);
const Matrix& M1 = Q1.matrix();
const Matrix& M2 = Q2.matrix();
assert(M1.rows() == p_ && M2.rows() == p_);
const size_t dim = 3 * p_; // Stiefel manifold dimension
Vector fQ2(dim), hQ1(dim);
// Vectorize and extract only d leftmost columns, i.e. vec(M2*P)
fQ2 << Eigen::Map<const Matrix>(M2.data(), dim, 1);
// Vectorize M1*P*R12
const Matrix Q1PR12 = M1.leftCols<3>() * M_;
hQ1 << Eigen::Map<const Matrix>(Q1PR12.data(), dim, 1);
// If asked, calculate Jacobian as (M \otimes M1) * G
if (H1) {
const size_t p2 = 2 * p_;
Matrix RPxQ = Matrix::Zero(dim, pp_);
RPxQ.block(0, 0, p_, dim) << M1 * M_(0, 0), M1 * M_(1, 0), M1 * M_(2, 0);
RPxQ.block(p_, 0, p_, dim) << M1 * M_(0, 1), M1 * M_(1, 1), M1 * M_(2, 1);
RPxQ.block(p2, 0, p_, dim) << M1 * M_(0, 2), M1 * M_(1, 2), M1 * M_(2, 2);
*H1 = -RPxQ * G_;
}
if (H2) {
const size_t p2 = 2 * p_;
Matrix PxQ = Matrix::Zero(dim, pp_);
PxQ.block(0, 0, p_, p_) = M2;
PxQ.block(p_, p_, p_, p_) = M2;
PxQ.block(p2, p2, p_, p_) = M2;
*H2 = PxQ * G_;
}
return fQ2 - hQ1;
}
//******************************************************************************
} // namespace gtsam

145
gtsam/slam/FrobeniusFactor.h Normal file
View File

@ -0,0 +1,145 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2019, 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 FrobeniusFactor.h
* @date March 2019
* @author Frank Dellaert
* @brief Various factors that minimize some Frobenius norm
*/
#pragma once
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/SOn.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam {
/**
* When creating (any) FrobeniusFactor we convert a 6-dimensional Pose3
* BetweenFactor noise model into an 9 or 16-dimensional isotropic noise
* model used to weight the Frobenius norm. If the noise model passed is
* null we return a Dim-dimensional isotropic noise model with sigma=1.0. If
* not, we we check if the 3-dimensional noise model on rotations is
* isotropic. If it is, we extend to 'Dim' dimensions, otherwise we throw an
* error. If defaultToUnit == false throws an exception on unexepcted input.
*/
GTSAM_EXPORT boost::shared_ptr<noiseModel::Isotropic> ConvertPose3NoiseModel(
const SharedNoiseModel& model, size_t d, bool defaultToUnit = true);
/**
* FrobeniusPrior calculates the Frobenius norm between a given matrix and an
* element of SO(3) or SO(4).
*/
template <class Rot>
class FrobeniusPrior : public NoiseModelFactor1<Rot> {
enum { Dim = Rot::VectorN2::RowsAtCompileTime };
using MatrixNN = typename Rot::MatrixNN;
Eigen::Matrix<double, Dim, 1> vecM_; ///< vectorized matrix to approximate
public:
/// Constructor
FrobeniusPrior(Key j, const MatrixNN& M,
const SharedNoiseModel& model = nullptr)
: NoiseModelFactor1<Rot>(ConvertPose3NoiseModel(model, Dim), j) {
vecM_ << Eigen::Map<const Matrix>(M.data(), Dim, 1);
}
/// Error is just Frobenius norm between Rot element and vectorized matrix M.
Vector evaluateError(const Rot& R,
boost::optional<Matrix&> H = boost::none) const {
return R.vec(H) - vecM_; // Jacobian is computed only when needed.
}
};
/**
* FrobeniusFactor calculates the Frobenius norm between rotation matrices.
* The template argument can be any fixed-size SO<N>.
*/
template <class Rot>
class FrobeniusFactor : public NoiseModelFactor2<Rot, Rot> {
enum { Dim = Rot::VectorN2::RowsAtCompileTime };
public:
/// Constructor
FrobeniusFactor(Key j1, Key j2, const SharedNoiseModel& model = nullptr)
: NoiseModelFactor2<Rot, Rot>(ConvertPose3NoiseModel(model, Dim), j1,
j2) {}
/// Error is just Frobenius norm between rotation matrices.
Vector evaluateError(const Rot& R1, const Rot& R2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
Vector error = R2.vec(H2) - R1.vec(H1);
if (H1) *H1 = -*H1;
return error;
}
};
/**
* FrobeniusBetweenFactor is a BetweenFactor that evaluates the Frobenius norm
* of the rotation error between measured and predicted (rather than the
* Logmap of the error). This factor is only defined for fixed-dimension types,
* and in fact only SO3 and SO4 really work, as we need SO<N>::AdjointMap.
*/
template <class Rot>
class FrobeniusBetweenFactor : public NoiseModelFactor2<Rot, Rot> {
Rot R12_; ///< measured rotation between R1 and R2
Eigen::Matrix<double, Rot::dimension, Rot::dimension>
R2hat_H_R1_; ///< fixed derivative of R2hat wrpt R1
enum { Dim = Rot::VectorN2::RowsAtCompileTime };
public:
/// Constructor
FrobeniusBetweenFactor(Key j1, Key j2, const Rot& R12,
const SharedNoiseModel& model = nullptr)
: NoiseModelFactor2<Rot, Rot>(
ConvertPose3NoiseModel(model, Dim), j1, j2),
R12_(R12),
R2hat_H_R1_(R12.inverse().AdjointMap()) {}
/// Error is Frobenius norm between R1*R12 and R2.
Vector evaluateError(const Rot& R1, const Rot& R2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
const Rot R2hat = R1.compose(R12_);
Eigen::Matrix<double, Dim, Rot::dimension> vec_H_R2hat;
Vector error = R2.vec(H2) - R2hat.vec(H1 ? &vec_H_R2hat : nullptr);
if (H1) *H1 = -vec_H_R2hat * R2hat_H_R1_;
return error;
}
};
/**
* FrobeniusWormholeFactor is a BetweenFactor that moves in SO(p), but will
* land on the SO(3) sub-manifold of SO(p) at the global minimum. It projects
* the SO(p) matrices down to a Stiefel manifold of p*d matrices.
* TODO(frank): template on D=2 or 3
*/
class GTSAM_EXPORT FrobeniusWormholeFactor : public NoiseModelFactor2<SOn, SOn> {
Matrix M_; ///< measured rotation between R1 and R2
size_t p_, pp_, dimension_; ///< dimensionality constants
Matrix G_; ///< matrix of vectorized generators
public:
/// Constructor. Note we convert to 3*p-dimensional noise model.
FrobeniusWormholeFactor(Key j1, Key j2, const Rot3& R12, size_t p = 4,
const SharedNoiseModel& model = nullptr);
/// Error is Frobenius norm between Q1*P*R12 and Q2*P, where P=[I_3x3;0]
/// projects down from SO(p) to the Stiefel manifold of px3 matrices.
Vector evaluateError(const SOn& Q1, const SOn& Q2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const;
};
} // namespace gtsam

14
gtsam/slam/SmartFactorBase.h
View File

@ -207,10 +207,18 @@ protected:
Vector ue = cameras.reprojectionError(point, measured_, Fs, E);
if (body_P_sensor_ && Fs) {
const Pose3 sensor_P_body = body_P_sensor_->inverse();
constexpr int camera_dim = traits<CAMERA>::dimension;
constexpr int pose_dim = traits<Pose3>::dimension;
for (size_t i = 0; i < Fs->size(); i++) {
const Pose3 w_Pose_body = cameras[i].pose() * sensor_P_body;
Matrix J(6, 6);
const Pose3 world_P_body = w_Pose_body.compose(*body_P_sensor_, J);
const Pose3 world_P_body = cameras[i].pose() * sensor_P_body;
Eigen::Matrix<double, camera_dim, camera_dim> J;
J.setZero();
Eigen::Matrix<double, pose_dim, pose_dim> H;
// Call compose to compute Jacobian for camera extrinsics
world_P_body.compose(*body_P_sensor_, H);
// Assign extrinsics part of the Jacobian
J.template block<pose_dim, pose_dim>(0, 0) = H;
Fs->at(i) = Fs->at(i) * J;
}
}

35
gtsam/slam/dataset.cpp
View File

@ -442,11 +442,11 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
auto p = dynamic_cast<const GenericValue<Pose3>*>(&key_value.value);
if (!p) continue;
const Pose3& pose = p->value();
Point3 t = pose.translation();
Rot3 R = pose.rotation();
stream << "VERTEX_SE3:QUAT " << key_value.key << " " << t.x() << " " << t.y() << " " << t.z()
<< " " << R.toQuaternion().x() << " " << R.toQuaternion().y() << " " << R.toQuaternion().z()
<< " " << R.toQuaternion().w() << endl;
const Point3 t = pose.translation();
const auto q = pose.rotation().toQuaternion();
stream << "VERTEX_SE3:QUAT " << key_value.key << " " << t.x() << " "
<< t.y() << " " << t.z() << " " << q.x() << " " << q.y() << " "
<< q.z() << " " << q.w() << endl;
}
// save edges (2D or 3D)
@ -486,13 +486,12 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
throw invalid_argument("writeG2o: invalid noise model!");
}
Matrix Info = gaussianModel->R().transpose() * gaussianModel->R();
Pose3 pose3D = factor3D->measured();
Point3 p = pose3D.translation();
Rot3 R = pose3D.rotation();
stream << "EDGE_SE3:QUAT " << factor3D->key1() << " " << factor3D->key2() << " "
<< p.x() << " " << p.y() << " " << p.z() << " " << R.toQuaternion().x()
<< " " << R.toQuaternion().y() << " " << R.toQuaternion().z() << " " << R.toQuaternion().w();
const Pose3 pose3D = factor3D->measured();
const Point3 p = pose3D.translation();
const auto q = pose3D.rotation().toQuaternion();
stream << "EDGE_SE3:QUAT " << factor3D->key1() << " " << factor3D->key2()
<< " " << p.x() << " " << p.y() << " " << p.z() << " " << q.x()
<< " " << q.y() << " " << q.z() << " " << q.w();
Matrix InfoG2o = I_6x6;
InfoG2o.block(0,0,3,3) = Info.block(3,3,3,3); // cov translation
@ -511,6 +510,11 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
stream.close();
}
/* ************************************************************************* */
static Rot3 NormalizedRot3(double w, double x, double y, double z) {
const double norm = sqrt(w * w + x * x + y * y + z * z), f = 1.0 / norm;
return Rot3::Quaternion(f * w, f * x, f * y, f * z);
}
/* ************************************************************************* */
std::map<Key, Pose3> parse3DPoses(const string& filename) {
ifstream is(filename.c_str());
@ -535,14 +539,15 @@ std::map<Key, Pose3> parse3DPoses(const string& filename) {
Key id;
double x, y, z, qx, qy, qz, qw;
ls >> id >> x >> y >> z >> qx >> qy >> qz >> qw;
poses.emplace(id, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z}));
poses.emplace(id, Pose3(NormalizedRot3(qw, qx, qy, qz), {x, y, z}));
}
}
return poses;
}
/* ************************************************************************* */
BetweenFactorPose3s parse3DFactors(const string& filename,
BetweenFactorPose3s parse3DFactors(
const string& filename,
const noiseModel::Diagonal::shared_ptr& corruptingNoise) {
ifstream is(filename.c_str());
if (!is) throw invalid_argument("parse3DFactors: can not find file " + filename);
@ -592,7 +597,7 @@ BetweenFactorPose3s parse3DFactors(const string& filename,
mgtsam.block<3, 3>(3, 0) = m.block<3, 3>(3, 0); // off diagonal
SharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam);
auto R12 = Rot3::Quaternion(qw, qx, qy, qz);
auto R12 = NormalizedRot3(qw, qx, qy, qz);
if (sampler) {
R12 = R12.retract(sampler->sample());
}

169
gtsam/slam/tests/testDataset.cpp
View File

@ -122,45 +122,6 @@ TEST( dataSet, Balbianello)
EXPECT(assert_equal(expected,actual,1));
}
/* ************************************************************************* */
TEST( dataSet, readG2o)
{
const string g2oFile = findExampleDataFile("pose2example");
NonlinearFactorGraph::shared_ptr actualGraph;
Values::shared_ptr actualValues;
boost::tie(actualGraph, actualValues) = readG2o(g2oFile);
Values expectedValues;
expectedValues.insert(0, Pose2(0.000000, 0.000000, 0.000000));
expectedValues.insert(1, Pose2(1.030390, 0.011350, -0.081596));
expectedValues.insert(2, Pose2(2.036137, -0.129733, -0.301887));
expectedValues.insert(3, Pose2(3.015097, -0.442395, -0.345514));
expectedValues.insert(4, Pose2(3.343949, 0.506678, 1.214715));
expectedValues.insert(5, Pose2(3.684491, 1.464049, 1.183785));
expectedValues.insert(6, Pose2(4.064626, 2.414783, 1.176333));
expectedValues.insert(7, Pose2(4.429778, 3.300180, 1.259169));
expectedValues.insert(8, Pose2(4.128877, 2.321481, -1.825391));
expectedValues.insert(9, Pose2(3.884653, 1.327509, -1.953016));
expectedValues.insert(10, Pose2(3.531067, 0.388263, -2.148934));
EXPECT(assert_equal(expectedValues,*actualValues,1e-5));
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions(Vector3(44.721360, 44.721360, 30.901699));
NonlinearFactorGraph expectedGraph;
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(0, 1, Pose2(1.030390, 0.011350, -0.081596), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(1.013900, -0.058639, -0.220291), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(2, 3, Pose2(1.027650, -0.007456, -0.043627), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(4, 5, Pose2(1.016030, 0.014565, -0.030930), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(5, 6, Pose2(1.023890, 0.006808, -0.007452), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(6, 7, Pose2(0.957734, 0.003159, 0.082836), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(8, 9, Pose2(1.023440, 0.013984, -0.127624), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(9,10, Pose2(1.003350, 0.022250, -0.195918), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(5, 9, Pose2(0.033943, 0.032439, 3.073637), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(3,10, Pose2(0.044020, 0.988477, -1.553511), model);
EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5));
}
/* ************************************************************************* */
TEST(dataSet, readG2o3D) {
const string g2oFile = findExampleDataFile("pose3example");
@ -273,59 +234,103 @@ TEST( dataSet, readG2o3DNonDiagonalNoise)
}
/* ************************************************************************* */
TEST( dataSet, readG2oHuber)
{
const string g2oFile = findExampleDataFile("pose2example");
NonlinearFactorGraph::shared_ptr actualGraph;
Values::shared_ptr actualValues;
bool is3D = false;
boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D, KernelFunctionTypeHUBER);
TEST(dataSet, readG2oCheckDeterminants) {
const string g2oFile = findExampleDataFile("toyExample.g2o");
noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions(Vector3(44.721360, 44.721360, 30.901699));
SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345), baseModel);
// Check determinants in factors
auto factors = parse3DFactors(g2oFile);
EXPECT_LONGS_EQUAL(6, factors.size());
for (const auto& factor : factors) {
const Rot3 R = factor->measured().rotation();
EXPECT_DOUBLES_EQUAL(1.0, R.matrix().determinant(), 1e-9);
}
NonlinearFactorGraph expectedGraph;
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(0, 1, Pose2(1.030390, 0.011350, -0.081596), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(1.013900, -0.058639, -0.220291), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(2, 3, Pose2(1.027650, -0.007456, -0.043627), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(4, 5, Pose2(1.016030, 0.014565, -0.030930), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(5, 6, Pose2(1.023890, 0.006808, -0.007452), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(6, 7, Pose2(0.957734, 0.003159, 0.082836), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(8, 9, Pose2(1.023440, 0.013984, -0.127624), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(9,10, Pose2(1.003350, 0.022250, -0.195918), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(5, 9, Pose2(0.033943, 0.032439, 3.073637), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(3,10, Pose2(0.044020, 0.988477, -1.553511), model);
EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5));
// Check determinants in initial values
const map<Key, Pose3> poses = parse3DPoses(g2oFile);
EXPECT_LONGS_EQUAL(5, poses.size());
for (const auto& key_value : poses) {
const Rot3 R = key_value.second.rotation();
EXPECT_DOUBLES_EQUAL(1.0, R.matrix().determinant(), 1e-9);
}
}
/* ************************************************************************* */
TEST( dataSet, readG2oTukey)
{
static NonlinearFactorGraph expectedGraph(const SharedNoiseModel& model) {
NonlinearFactorGraph g;
using Factor = BetweenFactor<Pose2>;
g.emplace_shared<Factor>(0, 1, Pose2(1.030390, 0.011350, -0.081596), model);
g.emplace_shared<Factor>(1, 2, Pose2(1.013900, -0.058639, -0.220291), model);
g.emplace_shared<Factor>(2, 3, Pose2(1.027650, -0.007456, -0.043627), model);
g.emplace_shared<Factor>(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model);
g.emplace_shared<Factor>(4, 5, Pose2(1.016030, 0.014565, -0.030930), model);
g.emplace_shared<Factor>(5, 6, Pose2(1.023890, 0.006808, -0.007452), model);
g.emplace_shared<Factor>(6, 7, Pose2(0.957734, 0.003159, 0.082836), model);
g.emplace_shared<Factor>(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model);
g.emplace_shared<Factor>(8, 9, Pose2(1.023440, 0.013984, -0.127624), model);
g.emplace_shared<Factor>(9, 10, Pose2(1.003350, 0.022250, -0.195918), model);
g.emplace_shared<Factor>(5, 9, Pose2(0.033943, 0.032439, 3.073637), model);
g.emplace_shared<Factor>(3, 10, Pose2(0.044020, 0.988477, -1.553511), model);
return g;
}
/* ************************************************************************* */
TEST(dataSet, readG2o) {
const string g2oFile = findExampleDataFile("pose2example");
NonlinearFactorGraph::shared_ptr actualGraph;
Values::shared_ptr actualValues;
boost::tie(actualGraph, actualValues) = readG2o(g2oFile);
auto model = noiseModel::Diagonal::Precisions(
Vector3(44.721360, 44.721360, 30.901699));
EXPECT(assert_equal(expectedGraph(model), *actualGraph, 1e-5));
Values expectedValues;
expectedValues.insert(0, Pose2(0.000000, 0.000000, 0.000000));
expectedValues.insert(1, Pose2(1.030390, 0.011350, -0.081596));
expectedValues.insert(2, Pose2(2.036137, -0.129733, -0.301887));
expectedValues.insert(3, Pose2(3.015097, -0.442395, -0.345514));
expectedValues.insert(4, Pose2(3.343949, 0.506678, 1.214715));
expectedValues.insert(5, Pose2(3.684491, 1.464049, 1.183785));
expectedValues.insert(6, Pose2(4.064626, 2.414783, 1.176333));
expectedValues.insert(7, Pose2(4.429778, 3.300180, 1.259169));
expectedValues.insert(8, Pose2(4.128877, 2.321481, -1.825391));
expectedValues.insert(9, Pose2(3.884653, 1.327509, -1.953016));
expectedValues.insert(10, Pose2(3.531067, 0.388263, -2.148934));
EXPECT(assert_equal(expectedValues, *actualValues, 1e-5));
}
/* ************************************************************************* */
TEST(dataSet, readG2oHuber) {
const string g2oFile = findExampleDataFile("pose2example");
NonlinearFactorGraph::shared_ptr actualGraph;
Values::shared_ptr actualValues;
bool is3D = false;
boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D, KernelFunctionTypeTUKEY);
boost::tie(actualGraph, actualValues) =
readG2o(g2oFile, is3D, KernelFunctionTypeHUBER);
noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions(Vector3(44.721360, 44.721360, 30.901699));
SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Tukey::Create(4.6851), baseModel);
auto baseModel = noiseModel::Diagonal::Precisions(
Vector3(44.721360, 44.721360, 30.901699));
auto model = noiseModel::Robust::Create(
noiseModel::mEstimator::Huber::Create(1.345), baseModel);
NonlinearFactorGraph expectedGraph;
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(0, 1, Pose2(1.030390, 0.011350, -0.081596), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(1.013900, -0.058639, -0.220291), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(2, 3, Pose2(1.027650, -0.007456, -0.043627), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(4, 5, Pose2(1.016030, 0.014565, -0.030930), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(5, 6, Pose2(1.023890, 0.006808, -0.007452), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(6, 7, Pose2(0.957734, 0.003159, 0.082836), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(8, 9, Pose2(1.023440, 0.013984, -0.127624), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(9,10, Pose2(1.003350, 0.022250, -0.195918), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(5, 9, Pose2(0.033943, 0.032439, 3.073637), model);
expectedGraph.emplace_shared<BetweenFactor<Pose2> >(3,10, Pose2(0.044020, 0.988477, -1.553511), model);
EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5));
EXPECT(assert_equal(expectedGraph(model), *actualGraph, 1e-5));
}
/* ************************************************************************* */
TEST(dataSet, readG2oTukey) {
const string g2oFile = findExampleDataFile("pose2example");
NonlinearFactorGraph::shared_ptr actualGraph;
Values::shared_ptr actualValues;
bool is3D = false;
boost::tie(actualGraph, actualValues) =
readG2o(g2oFile, is3D, KernelFunctionTypeTUKEY);
auto baseModel = noiseModel::Diagonal::Precisions(
Vector3(44.721360, 44.721360, 30.901699));
auto model = noiseModel::Robust::Create(
noiseModel::mEstimator::Tukey::Create(4.6851), baseModel);
EXPECT(assert_equal(expectedGraph(model), *actualGraph, 1e-5));
}
/* ************************************************************************* */
@ -495,7 +500,7 @@ TEST( dataSet, writeBALfromValues_Dubrovnik){
SfmData readData;
readBAL(filenameToRead, readData);
Pose3 poseChange = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.3,0.1,0.3));
Pose3 poseChange = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.3,0.1,0.3));
Values value;
for(size_t i=0; i < readData.number_cameras(); i++){ // for each camera

244
gtsam/slam/tests/testFrobeniusFactor.cpp Normal file
View File

@ -0,0 +1,244 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010-2019, 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
* -------------------------------------------------------------------------- */
/**
* testFrobeniusFactor.cpp
*
* @file testFrobeniusFactor.cpp
* @date March 2019
* @author Frank Dellaert
* @brief Check evaluateError for various Frobenius norm
*/
#include <gtsam/base/lieProxies.h>
#include <gtsam/base/testLie.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/SO3.h>
#include <gtsam/geometry/SO4.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/factorTesting.h>
#include <gtsam/slam/FrobeniusFactor.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
//******************************************************************************
namespace so3 {
SO3 id;
Vector3 v1 = (Vector(3) << 0.1, 0, 0).finished();
SO3 R1 = SO3::Expmap(v1);
Vector3 v2 = (Vector(3) << 0.01, 0.02, 0.03).finished();
SO3 R2 = SO3::Expmap(v2);
SO3 R12 = R1.between(R2);
} // namespace so3
/* ************************************************************************* */
TEST(FrobeniusPriorSO3, evaluateError) {
using namespace ::so3;
auto factor = FrobeniusPrior<SO3>(1, R2.matrix());
Vector actual = factor.evaluateError(R1);
Vector expected = R1.vec() - R2.vec();
EXPECT(assert_equal(expected, actual, 1e-9));
Values values;
values.insert(1, R1);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
}
/* ************************************************************************* */
TEST(FrobeniusPriorSO3, ClosestTo) {
// Example top-left of SO(4) matrix not quite on SO(3) manifold
Matrix3 M;
M << 0.79067393, 0.6051136, -0.0930814, //
0.4155925, -0.64214347, -0.64324489, //
-0.44948549, 0.47046326, -0.75917576;
SO3 expected = SO3::ClosestTo(M);
// manifold optimization gets same result as SVD solution in ClosestTo
NonlinearFactorGraph graph;
graph.emplace_shared<FrobeniusPrior<SO3> >(1, M);
Values initial;
initial.insert(1, SO3(I_3x3));
auto result = GaussNewtonOptimizer(graph, initial).optimize();
EXPECT_DOUBLES_EQUAL(0.0, graph.error(result), 1e-6);
EXPECT(assert_equal(expected, result.at<SO3>(1), 1e-6));
}
/* ************************************************************************* */
TEST(FrobeniusPriorSO3, ChordalL2mean) {
// See Hartley13ijcv:
// Cost function C(R) = \sum FrobeniusPrior(R,R_i)
// Closed form solution = ClosestTo(C_e), where
// C_e = \sum R_i !!!!
// We will test by computing mean of R1=exp(v1) R1^T=exp(-v1):
using namespace ::so3;
SO3 expected; // identity
Matrix3 M = R1.matrix() + R1.matrix().transpose();
EXPECT(assert_equal(expected, SO3::ClosestTo(M), 1e-6));
EXPECT(assert_equal(expected, SO3::ChordalMean({R1, R1.inverse()}), 1e-6));
// manifold optimization gets same result as ChordalMean
NonlinearFactorGraph graph;
graph.emplace_shared<FrobeniusPrior<SO3> >(1, R1.matrix());
graph.emplace_shared<FrobeniusPrior<SO3> >(1, R1.matrix().transpose());
Values initial;
initial.insert<SO3>(1, R1.inverse());
auto result = GaussNewtonOptimizer(graph, initial).optimize();
EXPECT_DOUBLES_EQUAL(0.0, graph.error(result), 0.1); // Why so loose?
EXPECT(assert_equal(expected, result.at<SO3>(1), 1e-5));
}
/* ************************************************************************* */
TEST(FrobeniusFactorSO3, evaluateError) {
using namespace ::so3;
auto factor = FrobeniusFactor<SO3>(1, 2);
Vector actual = factor.evaluateError(R1, R2);
Vector expected = R2.vec() - R1.vec();
EXPECT(assert_equal(expected, actual, 1e-9));
Values values;
values.insert(1, R1);
values.insert(2, R2);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
}
/* ************************************************************************* */
// Commented out as SO(n) not yet supported (and might never be)
// TEST(FrobeniusBetweenFactorSOn, evaluateError) {
// using namespace ::so3;
// auto factor =
// FrobeniusBetweenFactor<SOn>(1, 2, SOn::FromMatrix(R12.matrix()));
// Vector actual = factor.evaluateError(SOn::FromMatrix(R1.matrix()),
// SOn::FromMatrix(R2.matrix()));
// Vector expected = Vector9::Zero();
// EXPECT(assert_equal(expected, actual, 1e-9));
// Values values;
// values.insert(1, R1);
// values.insert(2, R2);
// EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
// }
/* ************************************************************************* */
TEST(FrobeniusBetweenFactorSO3, evaluateError) {
using namespace ::so3;
auto factor = FrobeniusBetweenFactor<SO3>(1, 2, R12);
Vector actual = factor.evaluateError(R1, R2);
Vector expected = Vector9::Zero();
EXPECT(assert_equal(expected, actual, 1e-9));
Values values;
values.insert(1, R1);
values.insert(2, R2);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
}
//******************************************************************************
namespace so4 {
SO4 id;
Vector6 v1 = (Vector(6) << 0.1, 0, 0, 0, 0, 0).finished();
SO4 Q1 = SO4::Expmap(v1);
Vector6 v2 = (Vector(6) << 0.01, 0.02, 0.03, 0.04, 0.05, 0.06).finished();
SO4 Q2 = SO4::Expmap(v2);
} // namespace so4
/* ************************************************************************* */
TEST(FrobeniusFactorSO4, evaluateError) {
using namespace ::so4;
auto factor = FrobeniusFactor<SO4>(1, 2, noiseModel::Unit::Create(6));
Vector actual = factor.evaluateError(Q1, Q2);
Vector expected = Q2.vec() - Q1.vec();
EXPECT(assert_equal(expected, actual, 1e-9));
Values values;
values.insert(1, Q1);
values.insert(2, Q2);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
}
/* ************************************************************************* */
TEST(FrobeniusBetweenFactorSO4, evaluateError) {
using namespace ::so4;
Matrix4 M{I_4x4};
M.topLeftCorner<3, 3>() = ::so3::R12.matrix();
auto factor = FrobeniusBetweenFactor<SO4>(1, 2, Q1.between(Q2));
Matrix H1, H2;
Vector actual = factor.evaluateError(Q1, Q2, H1, H2);
Vector expected = SO4::VectorN2::Zero();
EXPECT(assert_equal(expected, actual, 1e-9));
Values values;
values.insert(1, Q1);
values.insert(2, Q2);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
}
//******************************************************************************
namespace submanifold {
SO4 id;
Vector6 v1 = (Vector(6) << 0, 0, 0, 0.1, 0, 0).finished();
SO3 R1 = SO3::Expmap(v1.tail<3>());
SO4 Q1 = SO4::Expmap(v1);
Vector6 v2 = (Vector(6) << 0, 0, 0, 0.01, 0.02, 0.03).finished();
SO3 R2 = SO3::Expmap(v2.tail<3>());
SO4 Q2 = SO4::Expmap(v2);
SO3 R12 = R1.between(R2);
} // namespace submanifold
/* ************************************************************************* */
TEST(FrobeniusWormholeFactor, evaluateError) {
auto model = noiseModel::Isotropic::Sigma(6, 1.2); // dimension = 6 not 16
for (const size_t p : {5, 4, 3}) {
Matrix M = Matrix::Identity(p, p);
M.topLeftCorner(3, 3) = submanifold::R1.matrix();
SOn Q1(M);
M.topLeftCorner(3, 3) = submanifold::R2.matrix();
SOn Q2(M);
auto factor =
FrobeniusWormholeFactor(1, 2, Rot3(::so3::R12.matrix()), p, model);
Matrix H1, H2;
factor.evaluateError(Q1, Q2, H1, H2);
// Test derivatives
Values values;
values.insert(1, Q1);
values.insert(2, Q2);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
}
}
/* ************************************************************************* */
TEST(FrobeniusWormholeFactor, equivalenceToSO3) {
using namespace ::submanifold;
auto R12 = ::so3::R12.retract(Vector3(0.1, 0.2, -0.1));
auto model = noiseModel::Isotropic::Sigma(6, 1.2); // wrong dimension
auto factor3 = FrobeniusBetweenFactor<SO3>(1, 2, R12, model);
auto factor4 = FrobeniusWormholeFactor(1, 2, Rot3(R12.matrix()), 4, model);
const Matrix3 E3(factor3.evaluateError(R1, R2).data());
const Matrix43 E4(
factor4.evaluateError(SOn(Q1.matrix()), SOn(Q2.matrix())).data());
EXPECT(assert_equal((Matrix)E4.topLeftCorner<3, 3>(), E3, 1e-9));
EXPECT(assert_equal((Matrix)E4.row(3), Matrix13::Zero(), 1e-9));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

44
gtsam/slam/tests/testSmartFactorBase.cpp
View File

@ -37,11 +37,11 @@ class PinholeFactor: public SmartFactorBase<PinholeCamera<Cal3Bundler> > {
public:
typedef SmartFactorBase<PinholeCamera<Cal3Bundler> > Base;
PinholeFactor() {}
PinholeFactor(const SharedNoiseModel& sharedNoiseModel): Base(sharedNoiseModel) {
}
virtual double error(const Values& values) const {
return 0.0;
}
PinholeFactor(const SharedNoiseModel& sharedNoiseModel,
boost::optional<Pose3> body_P_sensor = boost::none,
size_t expectedNumberCameras = 10)
: Base(sharedNoiseModel, body_P_sensor, expectedNumberCameras) {}
virtual double error(const Values& values) const { return 0.0; }
virtual boost::shared_ptr<GaussianFactor> linearize(
const Values& values) const {
return boost::shared_ptr<GaussianFactor>(new JacobianFactor());
@ -60,6 +60,40 @@ TEST(SmartFactorBase, Pinhole) {
EXPECT_LONGS_EQUAL(2 * 2, f.dim());
}
TEST(SmartFactorBase, PinholeWithSensor) {
Pose3 body_P_sensor(Rot3(), Point3(1, 0, 0));
PinholeFactor f = PinholeFactor(unit2, body_P_sensor);
EXPECT(assert_equal<Pose3>(f.body_P_sensor(), body_P_sensor));
PinholeFactor::Cameras cameras;
// Assume body at origin.
Pose3 world_P_body = Pose3();
// Camera coordinates in world frame.
Pose3 wTc = world_P_body * body_P_sensor;
cameras.push_back(PinholeCamera<Cal3Bundler>(wTc));
// Simple point to project slightly off image center
Point3 p(0, 0, 10);
Point2 measurement = cameras[0].project(p);
f.add(measurement, 1);
PinholeFactor::Cameras::FBlocks Fs;
Matrix E;
Vector error = f.unwhitenedError<Point3>(cameras, p, Fs, E);
Vector expectedError = Vector::Zero(2);
Matrix29 expectedFs;
expectedFs << -0.001, -1.00001, 0, -0.1, 0, -0.01, 0, 0, 0, 1, 0, 0, 0, -0.1, 0, 0, 0, 0;
Matrix23 expectedE;
expectedE << 0.1, 0, 0.01, 0, 0.1, 0;
EXPECT(assert_equal(error, expectedError));
// We only have the jacobian for the 1 camera
// Use of a lower tolerance value due to compiler precision mismatch.
EXPECT(assert_equal(expectedFs, Fs[0], 1e-3));
EXPECT(assert_equal(expectedE, E));
}
/* ************************************************************************* */
#include <gtsam/geometry/StereoCamera.h>

7
tests/testNonlinearOptimizer.cpp
View File

@ -459,11 +459,12 @@ TEST(NonlinearOptimizer, RobustMeanCalculation) {
init.insert(0, 100.0);
expected.insert(0, 3.33333333);
LevenbergMarquardtParams params;
DoglegParams params_dl;
params_dl.setRelativeErrorTol(1e-10);
auto gn_result = GaussNewtonOptimizer(fg, init).optimize();
auto lm_result = LevenbergMarquardtOptimizer(fg, init, params).optimize();
auto dl_result = DoglegOptimizer(fg, init).optimize();
auto lm_result = LevenbergMarquardtOptimizer(fg, init).optimize();
auto dl_result = DoglegOptimizer(fg, init, params_dl).optimize();
EXPECT(assert_equal(expected, gn_result, tol));
EXPECT(assert_equal(expected, lm_result, tol));

110
timing/timeFrobeniusFactor.cpp Normal file
View File

@ -0,0 +1,110 @@
/* ----------------------------------------------------------------------------
* 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 timeFrobeniusFactor.cpp
* @brief time FrobeniusFactor with BAL file
* @author Frank Dellaert
* @date June 6, 2015
*/
#include <gtsam/base/timing.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/SO4.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/linear/PCGSolver.h>
#include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/FrobeniusFactor.h>
#include <iostream>
#include <string>
#include <vector>
using namespace std;
using namespace gtsam;
static SharedNoiseModel gNoiseModel = noiseModel::Unit::Create(2);
int main(int argc, char* argv[]) {
// primitive argument parsing:
if (argc > 3) {
throw runtime_error("Usage: timeFrobeniusFactor [g2oFile]");
}
string g2oFile;
try {
if (argc > 1)
g2oFile = argv[argc - 1];
else
g2oFile =
"/Users/dellaert/git/2019c-notes-shonanrotationaveraging/matlabCode/"
"datasets/randomTorus3D.g2o";
// g2oFile = findExampleDataFile("sphere_smallnoise.graph");
} catch (const exception& e) {
cerr << e.what() << '\n';
exit(1);
}
// Read G2O file
const auto factors = parse3DFactors(g2oFile);
const auto poses = parse3DPoses(g2oFile);
// Build graph
NonlinearFactorGraph graph;
// graph.add(NonlinearEquality<SO4>(0, SO4()));
auto priorModel = noiseModel::Isotropic::Sigma(6, 10000);
graph.add(PriorFactor<SO4>(0, SO4(), priorModel));
for (const auto& factor : factors) {
const auto& keys = factor->keys();
const auto& Tij = factor->measured();
const auto& model = factor->noiseModel();
graph.emplace_shared<FrobeniusWormholeFactor>(
keys[0], keys[1], SO3(Tij.rotation().matrix()), model);
}
boost::mt19937 rng(42);
// Set parameters to be similar to ceres
LevenbergMarquardtParams params;
LevenbergMarquardtParams::SetCeresDefaults(&params);
params.setLinearSolverType("MULTIFRONTAL_QR");
// params.setVerbosityLM("SUMMARY");
// params.linearSolverType = LevenbergMarquardtParams::Iterative;
// auto pcg = boost::make_shared<PCGSolverParameters>();
// pcg->preconditioner_ =
// boost::make_shared<SubgraphPreconditionerParameters>();
// boost::make_shared<BlockJacobiPreconditionerParameters>();
// params.iterativeParams = pcg;
// Optimize
for (size_t i = 0; i < 100; i++) {
gttic_(optimize);
Values initial;
initial.insert(0, SO4());
for (size_t j = 1; j < poses.size(); j++) {
initial.insert(j, SO4::Random(rng));
}
LevenbergMarquardtOptimizer lm(graph, initial, params);
Values result = lm.optimize();
cout << "cost = " << graph.error(result) << endl;
}
tictoc_finishedIteration_();
tictoc_print_();
return 0;
}