Merge branch 'develop' into feature/g2o-vertices
commit
f4c8e7580c
|
@ -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
|
||||
|
||||
|
|
|
@ -10,7 +10,7 @@ endif()
|
|||
# Set the version number for the library
|
||||
set (GTSAM_VERSION_MAJOR 4)
|
||||
set (GTSAM_VERSION_MINOR 0)
|
||||
set (GTSAM_VERSION_PATCH 2)
|
||||
set (GTSAM_VERSION_PATCH 3)
|
||||
math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}")
|
||||
set (GTSAM_VERSION_STRING "${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}")
|
||||
|
||||
|
@ -68,8 +68,8 @@ if(GTSAM_UNSTABLE_AVAILABLE)
|
|||
endif()
|
||||
option(BUILD_SHARED_LIBS "Build shared gtsam library, instead of static" ON)
|
||||
option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF)
|
||||
option(GTSAM_POSE3_EXPMAP "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." OFF)
|
||||
option(GTSAM_ROT3_EXPMAP "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." OFF)
|
||||
option(GTSAM_POSE3_EXPMAP "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." ON)
|
||||
option(GTSAM_ROT3_EXPMAP "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." ON)
|
||||
option(GTSAM_ENABLE_CONSISTENCY_CHECKS "Enable/Disable expensive consistency checks" OFF)
|
||||
option(GTSAM_WITH_TBB "Use Intel Threaded Building Blocks (TBB) if available" ON)
|
||||
option(GTSAM_WITH_EIGEN_MKL "Eigen will use Intel MKL if available" OFF)
|
||||
|
@ -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()
|
||||
|
@ -607,7 +606,7 @@ endif()
|
|||
message(STATUS "Cython toolbox flags ")
|
||||
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 "===============================================================")
|
||||
|
||||
|
|
|
@ -72,9 +72,9 @@ A Lie group is both a manifold *and* a group. Hence, a LIE_GROUP type should imp
|
|||
However, we now also need to be able to evaluate the derivatives of compose and inverse.
|
||||
Hence, we have the following extra valid static functions defined in the struct `gtsam::traits<T>`:
|
||||
|
||||
* `r = traits<T>::Compose(p,q,Hq,Hp)`
|
||||
* `r = traits<T>::Compose(p,q,Hp,Hq)`
|
||||
* `q = traits<T>::Inverse(p,Hp)`
|
||||
* `r = traits<T>::Between(p,q,Hq,H2p)`
|
||||
* `r = traits<T>::Between(p,q,Hp,Hq)`
|
||||
|
||||
where above the *H* arguments stand for optional Jacobian arguments.
|
||||
That makes it possible to create factors implementing priors (PriorFactor) or relations between two instances of a Lie group type (BetweenFactor).
|
||||
|
|
|
@ -1,5 +1,5 @@
|
|||
# version format
|
||||
version: 4.0.2-{branch}-build{build}
|
||||
version: 4.0.3-{branch}-build{build}
|
||||
|
||||
os: Visual Studio 2019
|
||||
|
||||
|
|
|
@ -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.
|
||||
|
|
|
@ -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()
|
||||
|
||||
|
|
|
@ -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
134
cython/README.md
|
@ -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)
|
||||
|
|
|
@ -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)
|
||||
|
|
|
@ -43,6 +43,11 @@ 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
|
||||
|
@ -65,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."""
|
||||
|
|
|
@ -46,5 +46,7 @@ def gtsam_optimize(optimizer,
|
|||
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()
|
||||
|
|
|
@ -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()
|
||||
|
|
|
@ -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
|
||||
)
|
||||
|
|
Binary file not shown.
|
@ -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
|
||||
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
|
@ -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 .
|
|
@ -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'
|
|
@ -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
|
|
@ -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 .
|
|
@ -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
|
|
@ -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"]
|
|
@ -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 .
|
|
@ -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"]
|
|
@ -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 .
|
|
@ -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;
|
||||
}
|
|
@ -10,7 +10,7 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file DiscreteBayesNet_graph.cpp
|
||||
* @file DiscreteBayesNet_FG.cpp
|
||||
* @brief Discrete Bayes Net example using Factor Graphs
|
||||
* @author Abhijit
|
||||
* @date Jun 4, 2012
|
||||
|
|
|
@ -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;
|
||||
}
|
|
@ -163,7 +163,7 @@ int main(int argc, char* argv[]) {
|
|||
vector<GpsMeasurement> gps_measurements;
|
||||
loadKittiData(kitti_calibration, imu_measurements, gps_measurements);
|
||||
|
||||
Vector6 BodyP = (Vector(6) << kitti_calibration.body_ptx, kitti_calibration.body_pty, kitti_calibration.body_ptz,
|
||||
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);
|
||||
|
@ -173,28 +173,29 @@ int main(int argc, char* argv[]) {
|
|||
}
|
||||
|
||||
// Configure different variables
|
||||
double t_offset = gps_measurements[0].time;
|
||||
// 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 vector
|
||||
auto w_coriolis = Vector3::Zero(); // zero vector
|
||||
|
||||
// Configure noise models
|
||||
auto noise_model_gps = noiseModel::Diagonal::Precisions((Vector(6) << Vector3::Constant(0),
|
||||
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);
|
||||
auto current_velocity_global = Vector3(); // the vehicle is stationary at the beginning at position 0,0,0
|
||||
// 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((Vector(6) << Vector3::Constant(0),
|
||||
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((Vector(6) << Vector3::Constant(0.100),
|
||||
auto sigma_init_b = noiseModel::Diagonal::Sigmas((Vector6() << Vector3::Constant(0.100),
|
||||
Vector3::Constant(5.00e-05))
|
||||
.finished());
|
||||
|
||||
|
@ -270,7 +271,7 @@ int main(int argc, char* argv[]) {
|
|||
previous_bias_key, *current_summarized_measurement);
|
||||
|
||||
// Bias evolution as given in the IMU metadata
|
||||
auto sigma_between_b = noiseModel::Diagonal::Sigmas((Vector(6) <<
|
||||
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());
|
||||
|
@ -342,6 +343,11 @@ int main(int argc, char* argv[]) {
|
|||
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(),
|
||||
|
|
|
@ -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)));
|
||||
|
|
|
@ -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");
|
||||
|
|
34
gtsam.h
34
gtsam.h
|
@ -1459,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 {
|
||||
|
@ -1470,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 {
|
||||
|
@ -1481,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 {
|
||||
|
@ -1492,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 {
|
||||
|
@ -1503,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 {
|
||||
|
@ -1514,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 {
|
||||
|
@ -1525,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 {
|
||||
|
@ -1536,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 {
|
||||
|
@ -1547,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
|
||||
|
@ -1938,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();
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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);
|
||||
|
||||
|
|
|
@ -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:
|
||||
|
||||
|
|
|
@ -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
|
||||
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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_);
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
Binary file not shown.
|
@ -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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
|
@ -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>
|
||||
|
||||
|
|
|
@ -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)));
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
|
@ -19,8 +19,10 @@
|
|||
#include <gtsam/geometry/concepts.h>
|
||||
#include <gtsam/base/concepts.h>
|
||||
|
||||
#include <iostream>
|
||||
#include <cmath>
|
||||
#include <iostream>
|
||||
#include <limits>
|
||||
#include <string>
|
||||
|
||||
using namespace std;
|
||||
|
||||
|
@ -36,10 +38,10 @@ Pose3::Pose3(const Pose2& pose2) :
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose3 Pose3::Create(const Rot3& R, const Point3& t, OptionalJacobian<6, 3> H1,
|
||||
OptionalJacobian<6, 3> H2) {
|
||||
if (H1) *H1 << I_3x3, Z_3x3;
|
||||
if (H2) *H2 << Z_3x3, R.transpose();
|
||||
Pose3 Pose3::Create(const Rot3& R, const Point3& t, OptionalJacobian<6, 3> HR,
|
||||
OptionalJacobian<6, 3> Ht) {
|
||||
if (HR) *HR << I_3x3, Z_3x3;
|
||||
if (Ht) *Ht << Z_3x3, R.transpose();
|
||||
return Pose3(R, t);
|
||||
}
|
||||
|
||||
|
@ -72,15 +74,15 @@ Matrix6 Pose3::adjointMap(const Vector6& xi) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y,
|
||||
OptionalJacobian<6,6> H) {
|
||||
if (H) {
|
||||
H->setZero();
|
||||
OptionalJacobian<6, 6> Hxi) {
|
||||
if (Hxi) {
|
||||
Hxi->setZero();
|
||||
for (int i = 0; i < 6; ++i) {
|
||||
Vector6 dxi;
|
||||
dxi.setZero();
|
||||
dxi(i) = 1.0;
|
||||
Matrix6 Gi = adjointMap(dxi);
|
||||
H->col(i) = Gi * y;
|
||||
Hxi->col(i) = Gi * y;
|
||||
}
|
||||
}
|
||||
return adjointMap(xi) * y;
|
||||
|
@ -88,15 +90,15 @@ Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y,
|
|||
|
||||
/* ************************************************************************* */
|
||||
Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y,
|
||||
OptionalJacobian<6,6> H) {
|
||||
if (H) {
|
||||
H->setZero();
|
||||
OptionalJacobian<6, 6> Hxi) {
|
||||
if (Hxi) {
|
||||
Hxi->setZero();
|
||||
for (int i = 0; i < 6; ++i) {
|
||||
Vector6 dxi;
|
||||
dxi.setZero();
|
||||
dxi(i) = 1.0;
|
||||
Matrix6 GTi = adjointMap(dxi).transpose();
|
||||
H->col(i) = GTi * y;
|
||||
Hxi->col(i) = GTi * y;
|
||||
}
|
||||
}
|
||||
return adjointMap(xi).transpose() * y;
|
||||
|
@ -116,8 +118,8 @@ bool Pose3::equals(const Pose3& pose, double tol) const {
|
|||
|
||||
/* ************************************************************************* */
|
||||
/** Modified from Murray94book version (which assumes w and v normalized?) */
|
||||
Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> H) {
|
||||
if (H) *H = ExpmapDerivative(xi);
|
||||
Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) {
|
||||
if (Hxi) *Hxi = ExpmapDerivative(xi);
|
||||
|
||||
// get angular velocity omega and translational velocity v from twist xi
|
||||
Vector3 omega(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5));
|
||||
|
@ -125,8 +127,8 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> H) {
|
|||
Rot3 R = Rot3::Expmap(omega);
|
||||
double theta2 = omega.dot(omega);
|
||||
if (theta2 > std::numeric_limits<double>::epsilon()) {
|
||||
Vector3 t_parallel = omega * omega.dot(v); // translation parallel to axis
|
||||
Vector3 omega_cross_v = omega.cross(v); // points towards axis
|
||||
Vector3 t_parallel = omega * omega.dot(v); // translation parallel to axis
|
||||
Vector3 omega_cross_v = omega.cross(v); // points towards axis
|
||||
Vector3 t = (omega_cross_v - R * omega_cross_v + t_parallel) / theta2;
|
||||
return Pose3(R, t);
|
||||
} else {
|
||||
|
@ -135,10 +137,10 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> H) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector6 Pose3::Logmap(const Pose3& p, OptionalJacobian<6, 6> H) {
|
||||
if (H) *H = LogmapDerivative(p);
|
||||
const Vector3 w = Rot3::Logmap(p.rotation());
|
||||
const Vector3 T = p.translation();
|
||||
Vector6 Pose3::Logmap(const Pose3& pose, OptionalJacobian<6, 6> Hpose) {
|
||||
if (Hpose) *Hpose = LogmapDerivative(pose);
|
||||
const Vector3 w = Rot3::Logmap(pose.rotation());
|
||||
const Vector3 T = pose.translation();
|
||||
const double t = w.norm();
|
||||
if (t < 1e-10) {
|
||||
Vector6 log;
|
||||
|
@ -158,33 +160,33 @@ Vector6 Pose3::Logmap(const Pose3& p, OptionalJacobian<6, 6> H) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose3 Pose3::ChartAtOrigin::Retract(const Vector6& xi, ChartJacobian H) {
|
||||
Pose3 Pose3::ChartAtOrigin::Retract(const Vector6& xi, ChartJacobian Hxi) {
|
||||
#ifdef GTSAM_POSE3_EXPMAP
|
||||
return Expmap(xi, H);
|
||||
return Expmap(xi, Hxi);
|
||||
#else
|
||||
Matrix3 DR;
|
||||
Rot3 R = Rot3::Retract(xi.head<3>(), H ? &DR : 0);
|
||||
if (H) {
|
||||
*H = I_6x6;
|
||||
H->topLeftCorner<3,3>() = DR;
|
||||
Rot3 R = Rot3::Retract(xi.head<3>(), Hxi ? &DR : 0);
|
||||
if (Hxi) {
|
||||
*Hxi = I_6x6;
|
||||
Hxi->topLeftCorner<3, 3>() = DR;
|
||||
}
|
||||
return Pose3(R, Point3(xi.tail<3>()));
|
||||
#endif
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector6 Pose3::ChartAtOrigin::Local(const Pose3& T, ChartJacobian H) {
|
||||
Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) {
|
||||
#ifdef GTSAM_POSE3_EXPMAP
|
||||
return Logmap(T, H);
|
||||
return Logmap(pose, Hpose);
|
||||
#else
|
||||
Matrix3 DR;
|
||||
Vector3 omega = Rot3::LocalCoordinates(T.rotation(), H ? &DR : 0);
|
||||
if (H) {
|
||||
*H = I_6x6;
|
||||
H->topLeftCorner<3,3>() = DR;
|
||||
Vector3 omega = Rot3::LocalCoordinates(pose.rotation(), Hpose ? &DR : 0);
|
||||
if (Hpose) {
|
||||
*Hpose = I_6x6;
|
||||
Hpose->topLeftCorner<3, 3>() = DR;
|
||||
}
|
||||
Vector6 xi;
|
||||
xi << omega, T.translation();
|
||||
xi << omega, pose.translation();
|
||||
return xi;
|
||||
#endif
|
||||
}
|
||||
|
@ -261,16 +263,16 @@ Matrix6 Pose3::LogmapDerivative(const Pose3& pose) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
const Point3& Pose3::translation(OptionalJacobian<3, 6> H) const {
|
||||
if (H) *H << Z_3x3, rotation().matrix();
|
||||
const Point3& Pose3::translation(OptionalJacobian<3, 6> Hself) const {
|
||||
if (Hself) *Hself << Z_3x3, rotation().matrix();
|
||||
return t_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
const Rot3& Pose3::rotation(OptionalJacobian<3, 6> H) const {
|
||||
if (H) {
|
||||
*H << I_3x3, Z_3x3;
|
||||
const Rot3& Pose3::rotation(OptionalJacobian<3, 6> Hself) const {
|
||||
if (Hself) {
|
||||
*Hself << I_3x3, Z_3x3;
|
||||
}
|
||||
return R_;
|
||||
}
|
||||
|
@ -284,9 +286,10 @@ Matrix4 Pose3::matrix() const {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose3 Pose3::transformPoseFrom(const Pose3& aTb) const {
|
||||
Pose3 Pose3::transformPoseFrom(const Pose3& aTb, OptionalJacobian<6, 6> Hself,
|
||||
OptionalJacobian<6, 6> HaTb) const {
|
||||
const Pose3& wTa = *this;
|
||||
return wTa * aTb;
|
||||
return wTa.compose(aTb, Hself, HaTb);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
@ -299,101 +302,101 @@ Pose3 Pose3::transform_to(const Pose3& pose) const {
|
|||
#endif
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose3 Pose3::transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> H1,
|
||||
OptionalJacobian<6, 6> H2) const {
|
||||
if (H1) *H1 = -wTb.inverse().AdjointMap() * AdjointMap();
|
||||
if (H2) *H2 = I_6x6;
|
||||
Pose3 Pose3::transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself,
|
||||
OptionalJacobian<6, 6> HwTb) const {
|
||||
if (Hself) *Hself = -wTb.inverse().AdjointMap() * AdjointMap();
|
||||
if (HwTb) *HwTb = I_6x6;
|
||||
const Pose3& wTa = *this;
|
||||
return wTa.inverse() * wTb;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Pose3::transformFrom(const Point3& p, OptionalJacobian<3,6> Dpose,
|
||||
OptionalJacobian<3,3> Dpoint) const {
|
||||
Point3 Pose3::transformFrom(const Point3& point, OptionalJacobian<3, 6> Hself,
|
||||
OptionalJacobian<3, 3> Hpoint) const {
|
||||
// Only get matrix once, to avoid multiple allocations,
|
||||
// as well as multiple conversions in the Quaternion case
|
||||
const Matrix3 R = R_.matrix();
|
||||
if (Dpose) {
|
||||
Dpose->leftCols<3>() = R * skewSymmetric(-p.x(), -p.y(), -p.z());
|
||||
Dpose->rightCols<3>() = R;
|
||||
if (Hself) {
|
||||
Hself->leftCols<3>() = R * skewSymmetric(-point.x(), -point.y(), -point.z());
|
||||
Hself->rightCols<3>() = R;
|
||||
}
|
||||
if (Dpoint) {
|
||||
*Dpoint = R;
|
||||
if (Hpoint) {
|
||||
*Hpoint = R;
|
||||
}
|
||||
return R_ * p + t_;
|
||||
return R_ * point + t_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Pose3::transformTo(const Point3& p, OptionalJacobian<3,6> Dpose,
|
||||
OptionalJacobian<3,3> Dpoint) const {
|
||||
Point3 Pose3::transformTo(const Point3& point, OptionalJacobian<3, 6> Hself,
|
||||
OptionalJacobian<3, 3> Hpoint) const {
|
||||
// Only get transpose once, to avoid multiple allocations,
|
||||
// as well as multiple conversions in the Quaternion case
|
||||
const Matrix3 Rt = R_.transpose();
|
||||
const Point3 q(Rt*(p - t_));
|
||||
if (Dpose) {
|
||||
const Point3 q(Rt*(point - t_));
|
||||
if (Hself) {
|
||||
const double wx = q.x(), wy = q.y(), wz = q.z();
|
||||
(*Dpose) <<
|
||||
(*Hself) <<
|
||||
0.0, -wz, +wy,-1.0, 0.0, 0.0,
|
||||
+wz, 0.0, -wx, 0.0,-1.0, 0.0,
|
||||
-wy, +wx, 0.0, 0.0, 0.0,-1.0;
|
||||
}
|
||||
if (Dpoint) {
|
||||
*Dpoint = Rt;
|
||||
if (Hpoint) {
|
||||
*Hpoint = Rt;
|
||||
}
|
||||
return q;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1,
|
||||
OptionalJacobian<1, 3> H2) const {
|
||||
double Pose3::range(const Point3& point, OptionalJacobian<1, 6> Hself,
|
||||
OptionalJacobian<1, 3> Hpoint) const {
|
||||
Matrix36 D_local_pose;
|
||||
Matrix3 D_local_point;
|
||||
Point3 local = transformTo(point, H1 ? &D_local_pose : 0, H2 ? &D_local_point : 0);
|
||||
if (!H1 && !H2) {
|
||||
Point3 local = transformTo(point, Hself ? &D_local_pose : 0, Hpoint ? &D_local_point : 0);
|
||||
if (!Hself && !Hpoint) {
|
||||
return local.norm();
|
||||
} else {
|
||||
Matrix13 D_r_local;
|
||||
const double r = norm3(local, D_r_local);
|
||||
if (H1) *H1 = D_r_local * D_local_pose;
|
||||
if (H2) *H2 = D_r_local * D_local_point;
|
||||
if (Hself) *Hself = D_r_local * D_local_pose;
|
||||
if (Hpoint) *Hpoint = D_r_local * D_local_point;
|
||||
return r;
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double Pose3::range(const Pose3& pose, OptionalJacobian<1, 6> H1,
|
||||
OptionalJacobian<1, 6> H2) const {
|
||||
double Pose3::range(const Pose3& pose, OptionalJacobian<1, 6> Hself,
|
||||
OptionalJacobian<1, 6> Hpose) const {
|
||||
Matrix13 D_local_point;
|
||||
double r = range(pose.translation(), H1, H2 ? &D_local_point : 0);
|
||||
if (H2) *H2 << Matrix13::Zero(), D_local_point * pose.rotation().matrix();
|
||||
double r = range(pose.translation(), Hself, Hpose ? &D_local_point : 0);
|
||||
if (Hpose) *Hpose << Matrix13::Zero(), D_local_point * pose.rotation().matrix();
|
||||
return r;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Unit3 Pose3::bearing(const Point3& point, OptionalJacobian<2, 6> H1,
|
||||
OptionalJacobian<2, 3> H2) const {
|
||||
Unit3 Pose3::bearing(const Point3& point, OptionalJacobian<2, 6> Hself,
|
||||
OptionalJacobian<2, 3> Hpoint) const {
|
||||
Matrix36 D_local_pose;
|
||||
Matrix3 D_local_point;
|
||||
Point3 local = transformTo(point, H1 ? &D_local_pose : 0, H2 ? &D_local_point : 0);
|
||||
if (!H1 && !H2) {
|
||||
Point3 local = transformTo(point, Hself ? &D_local_pose : 0, Hpoint ? &D_local_point : 0);
|
||||
if (!Hself && !Hpoint) {
|
||||
return Unit3(local);
|
||||
} else {
|
||||
Matrix23 D_b_local;
|
||||
Unit3 b = Unit3::FromPoint3(local, D_b_local);
|
||||
if (H1) *H1 = D_b_local * D_local_pose;
|
||||
if (H2) *H2 = D_b_local * D_local_point;
|
||||
if (Hself) *Hself = D_b_local * D_local_pose;
|
||||
if (Hpoint) *Hpoint = D_b_local * D_local_point;
|
||||
return b;
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Unit3 Pose3::bearing(const Pose3& pose, OptionalJacobian<2, 6> H1,
|
||||
OptionalJacobian<2, 6> H2) const {
|
||||
if (H2) {
|
||||
H2->setZero();
|
||||
return bearing(pose.translation(), H1, H2.cols<3>(3));
|
||||
Unit3 Pose3::bearing(const Pose3& pose, OptionalJacobian<2, 6> Hself,
|
||||
OptionalJacobian<2, 6> Hpose) const {
|
||||
if (Hpose) {
|
||||
Hpose->setZero();
|
||||
return bearing(pose.translation(), Hself, Hpose.cols<3>(3));
|
||||
}
|
||||
return bearing(pose.translation(), H1, boost::none);
|
||||
return bearing(pose.translation(), Hself, boost::none);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
@ -75,8 +75,8 @@ public:
|
|||
|
||||
/// Named constructor with derivatives
|
||||
static Pose3 Create(const Rot3& R, const Point3& t,
|
||||
OptionalJacobian<6, 3> H1 = boost::none,
|
||||
OptionalJacobian<6, 3> H2 = boost::none);
|
||||
OptionalJacobian<6, 3> HR = boost::none,
|
||||
OptionalJacobian<6, 3> Ht = boost::none);
|
||||
|
||||
/**
|
||||
* Create Pose3 by aligning two point pairs
|
||||
|
@ -117,10 +117,10 @@ public:
|
|||
/// @{
|
||||
|
||||
/// Exponential map at identity - create a rotation from canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$
|
||||
static Pose3 Expmap(const Vector6& xi, OptionalJacobian<6, 6> H = boost::none);
|
||||
static Pose3 Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi = boost::none);
|
||||
|
||||
/// Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of this rotation
|
||||
static Vector6 Logmap(const Pose3& p, OptionalJacobian<6, 6> H = boost::none);
|
||||
static Vector6 Logmap(const Pose3& pose, OptionalJacobian<6, 6> Hpose = boost::none);
|
||||
|
||||
/**
|
||||
* Calculate Adjoint map, transforming a twist in the this pose's (i.e, body) frame to the world spatial frame
|
||||
|
@ -157,7 +157,7 @@ public:
|
|||
* Action of the adjointMap on a Lie-algebra vector y, with optional derivatives
|
||||
*/
|
||||
static Vector6 adjoint(const Vector6 &xi, const Vector6 &y,
|
||||
OptionalJacobian<6, 6> = boost::none);
|
||||
OptionalJacobian<6, 6> Hxi = boost::none);
|
||||
|
||||
// temporary fix for wrappers until case issue is resolved
|
||||
static Matrix6 adjointMap_(const Vector6 &xi) { return adjointMap(xi);}
|
||||
|
@ -167,7 +167,7 @@ public:
|
|||
* The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space.
|
||||
*/
|
||||
static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y,
|
||||
OptionalJacobian<6, 6> H = boost::none);
|
||||
OptionalJacobian<6, 6> Hxi = boost::none);
|
||||
|
||||
/// Derivative of Expmap
|
||||
static Matrix6 ExpmapDerivative(const Vector6& xi);
|
||||
|
@ -177,8 +177,8 @@ public:
|
|||
|
||||
// Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP
|
||||
struct ChartAtOrigin {
|
||||
static Pose3 Retract(const Vector6& v, ChartJacobian H = boost::none);
|
||||
static Vector6 Local(const Pose3& r, ChartJacobian H = boost::none);
|
||||
static Pose3 Retract(const Vector6& xi, ChartJacobian Hxi = boost::none);
|
||||
static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = boost::none);
|
||||
};
|
||||
|
||||
using LieGroup<Pose3, 6>::inverse; // version with derivative
|
||||
|
@ -201,38 +201,38 @@ public:
|
|||
|
||||
/**
|
||||
* @brief takes point in Pose coordinates and transforms it to world coordinates
|
||||
* @param p point in Pose coordinates
|
||||
* @param Dpose optional 3*6 Jacobian wrpt this pose
|
||||
* @param Dpoint optional 3*3 Jacobian wrpt point
|
||||
* @param point point in Pose coordinates
|
||||
* @param Hself optional 3*6 Jacobian wrpt this pose
|
||||
* @param Hpoint optional 3*3 Jacobian wrpt point
|
||||
* @return point in world coordinates
|
||||
*/
|
||||
Point3 transformFrom(const Point3& p, OptionalJacobian<3, 6> Dpose =
|
||||
boost::none, OptionalJacobian<3, 3> Dpoint = boost::none) const;
|
||||
Point3 transformFrom(const Point3& point, OptionalJacobian<3, 6> Hself =
|
||||
boost::none, OptionalJacobian<3, 3> Hpoint = boost::none) const;
|
||||
|
||||
/** syntactic sugar for transformFrom */
|
||||
inline Point3 operator*(const Point3& p) const {
|
||||
return transformFrom(p);
|
||||
inline Point3 operator*(const Point3& point) const {
|
||||
return transformFrom(point);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief takes point in world coordinates and transforms it to Pose coordinates
|
||||
* @param p point in world coordinates
|
||||
* @param Dpose optional 3*6 Jacobian wrpt this pose
|
||||
* @param Dpoint optional 3*3 Jacobian wrpt point
|
||||
* @param point point in world coordinates
|
||||
* @param Hself optional 3*6 Jacobian wrpt this pose
|
||||
* @param Hpoint optional 3*3 Jacobian wrpt point
|
||||
* @return point in Pose coordinates
|
||||
*/
|
||||
Point3 transformTo(const Point3& p, OptionalJacobian<3, 6> Dpose =
|
||||
boost::none, OptionalJacobian<3, 3> Dpoint = boost::none) const;
|
||||
Point3 transformTo(const Point3& point, OptionalJacobian<3, 6> Hself =
|
||||
boost::none, OptionalJacobian<3, 3> Hpoint = boost::none) const;
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/// get rotation
|
||||
const Rot3& rotation(OptionalJacobian<3, 6> H = boost::none) const;
|
||||
const Rot3& rotation(OptionalJacobian<3, 6> Hself = boost::none) const;
|
||||
|
||||
/// get translation
|
||||
const Point3& translation(OptionalJacobian<3, 6> H = boost::none) const;
|
||||
const Point3& translation(OptionalJacobian<3, 6> Hself = boost::none) const;
|
||||
|
||||
/// get x
|
||||
double x() const {
|
||||
|
@ -252,36 +252,44 @@ public:
|
|||
/** convert to 4*4 matrix */
|
||||
Matrix4 matrix() const;
|
||||
|
||||
/** receives a pose in local coordinates and transforms it to world coordinates */
|
||||
Pose3 transformPoseFrom(const Pose3& pose) const;
|
||||
/**
|
||||
* Assuming self == wTa, takes a pose aTb in local coordinates
|
||||
* and transforms it to world coordinates wTb = wTa * aTb.
|
||||
* This is identical to compose.
|
||||
*/
|
||||
Pose3 transformPoseFrom(const Pose3& aTb, OptionalJacobian<6, 6> Hself = boost::none,
|
||||
OptionalJacobian<6, 6> HaTb = boost::none) const;
|
||||
|
||||
/** receives a pose in world coordinates and transforms it to local coordinates */
|
||||
Pose3 transformPoseTo(const Pose3& pose, OptionalJacobian<6, 6> H1 = boost::none,
|
||||
OptionalJacobian<6, 6> H2 = boost::none) const;
|
||||
/**
|
||||
* Assuming self == wTa, takes a pose wTb in world coordinates
|
||||
* and transforms it to local coordinates aTb = inv(wTa) * wTb
|
||||
*/
|
||||
Pose3 transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself = boost::none,
|
||||
OptionalJacobian<6, 6> HwTb = boost::none) const;
|
||||
|
||||
/**
|
||||
* Calculate range to a landmark
|
||||
* @param point 3D location of landmark
|
||||
* @return range (double)
|
||||
*/
|
||||
double range(const Point3& point, OptionalJacobian<1, 6> H1 = boost::none,
|
||||
OptionalJacobian<1, 3> H2 = boost::none) const;
|
||||
double range(const Point3& point, OptionalJacobian<1, 6> Hself = boost::none,
|
||||
OptionalJacobian<1, 3> Hpoint = boost::none) const;
|
||||
|
||||
/**
|
||||
* Calculate range to another pose
|
||||
* @param pose Other SO(3) pose
|
||||
* @return range (double)
|
||||
*/
|
||||
double range(const Pose3& pose, OptionalJacobian<1, 6> H1 = boost::none,
|
||||
OptionalJacobian<1, 6> H2 = boost::none) const;
|
||||
double range(const Pose3& pose, OptionalJacobian<1, 6> Hself = boost::none,
|
||||
OptionalJacobian<1, 6> Hpose = boost::none) const;
|
||||
|
||||
/**
|
||||
* Calculate bearing to a landmark
|
||||
* @param point 3D location of landmark
|
||||
* @return bearing (Unit3)
|
||||
*/
|
||||
Unit3 bearing(const Point3& point, OptionalJacobian<2, 6> H1 = boost::none,
|
||||
OptionalJacobian<2, 3> H2 = boost::none) const;
|
||||
Unit3 bearing(const Point3& point, OptionalJacobian<2, 6> Hself = boost::none,
|
||||
OptionalJacobian<2, 3> Hpoint = boost::none) const;
|
||||
|
||||
/**
|
||||
* Calculate bearing to another pose
|
||||
|
@ -289,8 +297,8 @@ public:
|
|||
* information is ignored.
|
||||
* @return bearing (Unit3)
|
||||
*/
|
||||
Unit3 bearing(const Pose3& pose, OptionalJacobian<2, 6> H1 = boost::none,
|
||||
OptionalJacobian<2, 6> H2 = boost::none) const;
|
||||
Unit3 bearing(const Pose3& pose, OptionalJacobian<2, 6> Hself = boost::none,
|
||||
OptionalJacobian<2, 6> Hpose = boost::none) const;
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
|
@ -321,20 +329,20 @@ public:
|
|||
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
|
||||
/// @name Deprecated
|
||||
/// @{
|
||||
Point3 transform_from(const Point3& p,
|
||||
OptionalJacobian<3, 6> Dpose = boost::none,
|
||||
OptionalJacobian<3, 3> Dpoint = boost::none) const {
|
||||
return transformFrom(p, Dpose, Dpoint);
|
||||
Point3 transform_from(const Point3& point,
|
||||
OptionalJacobian<3, 6> Hself = boost::none,
|
||||
OptionalJacobian<3, 3> Hpoint = boost::none) const {
|
||||
return transformFrom(point, Hself, Hpoint);
|
||||
}
|
||||
Point3 transform_to(const Point3& p,
|
||||
OptionalJacobian<3, 6> Dpose = boost::none,
|
||||
OptionalJacobian<3, 3> Dpoint = boost::none) const {
|
||||
return transformTo(p, Dpose, Dpoint);
|
||||
Point3 transform_to(const Point3& point,
|
||||
OptionalJacobian<3, 6> Hself = boost::none,
|
||||
OptionalJacobian<3, 3> Hpoint = boost::none) const {
|
||||
return transformTo(point, Hself, Hpoint);
|
||||
}
|
||||
Pose3 transform_pose_to(const Pose3& pose,
|
||||
OptionalJacobian<6, 6> H1 = boost::none,
|
||||
OptionalJacobian<6, 6> H2 = boost::none) const {
|
||||
return transformPoseTo(pose, H1, H2);
|
||||
Pose3 transform_pose_to(const Pose3& pose,
|
||||
OptionalJacobian<6, 6> Hself = boost::none,
|
||||
OptionalJacobian<6, 6> Hpose = boost::none) const {
|
||||
return transformPoseTo(pose, Hself, Hpose);
|
||||
}
|
||||
/**
|
||||
* @deprecated: this function is neither here not there. */
|
||||
|
|
|
@ -340,7 +340,7 @@ void serialize(
|
|||
const unsigned int file_version
|
||||
) {
|
||||
Matrix& M = Q.matrix_;
|
||||
ar& M;
|
||||
ar& BOOST_SERIALIZATION_NVP(M);
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
|
@ -418,6 +418,29 @@ TEST(Pose3, transform_to_rotate) {
|
|||
EXPECT(assert_equal(expected, actual, 0.001));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Check transformPoseFrom and its pushforward
|
||||
Pose3 transformPoseFrom_(const Pose3& wTa, const Pose3& aTb) {
|
||||
return wTa.transformPoseFrom(aTb);
|
||||
}
|
||||
|
||||
TEST(Pose3, transformPoseFrom)
|
||||
{
|
||||
Matrix actual = (T2*T2).matrix();
|
||||
Matrix expected = T2.matrix()*T2.matrix();
|
||||
EXPECT(assert_equal(actual, expected, 1e-8));
|
||||
|
||||
Matrix H1, H2;
|
||||
T2.transformPoseFrom(T2, H1, H2);
|
||||
|
||||
Matrix numericalH1 = numericalDerivative21(transformPoseFrom_, T2, T2);
|
||||
EXPECT(assert_equal(numericalH1, H1, 5e-3));
|
||||
EXPECT(assert_equal(T2.inverse().AdjointMap(), H1, 5e-3));
|
||||
|
||||
Matrix numericalH2 = numericalDerivative22(transformPoseFrom_, T2, T2);
|
||||
EXPECT(assert_equal(numericalH2, H2, 1e-4));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Pose3, transformTo) {
|
||||
Pose3 transform(Rot3::Rodrigues(0, 0, -1.570796), Point3(2, 4, 0));
|
||||
|
|
|
@ -69,4 +69,6 @@ namespace gtsam {
|
|||
void saveGraph(const std::string &s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
|
||||
};
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
#include <gtsam/inference/BayesNet-inst.h>
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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
|
||||
/// @{
|
||||
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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));
|
||||
}
|
||||
|
||||
|
|
|
@ -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());
|
||||
}
|
||||
|
|
|
@ -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,
|
||||
|
|
|
@ -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);
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
|
@ -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));\
|
||||
|
|
|
@ -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);
|
||||
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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_);
|
||||
}
|
||||
};
|
||||
|
||||
|
|
|
@ -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_);
|
||||
}
|
||||
};
|
||||
|
||||
|
|
|
@ -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))
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
};
|
||||
|
|
|
@ -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) &&
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
|
|
|
@ -68,7 +68,7 @@ Vector9 TangentPreintegration::UpdatePreintegrated(const Vector3& a_body,
|
|||
Matrix3 w_tangent_H_theta, invH;
|
||||
const Vector3 w_tangent = // angular velocity mapped back to tangent space
|
||||
local.applyInvDexp(w_body, A ? &w_tangent_H_theta : 0, C ? &invH : 0);
|
||||
const Rot3 R(local.expmap());
|
||||
const Rot3 R(local.expmap()); // nRb: rotation of body in nav frame
|
||||
const Vector3 a_nav = R * a_body;
|
||||
const double dt22 = 0.5 * dt * dt;
|
||||
|
||||
|
@ -110,7 +110,7 @@ void TangentPreintegration::update(const Vector3& measuredAcc,
|
|||
Vector3 acc = biasHat_.correctAccelerometer(measuredAcc);
|
||||
Vector3 omega = biasHat_.correctGyroscope(measuredOmega);
|
||||
|
||||
// Possibly correct for sensor pose
|
||||
// Possibly correct for sensor pose by converting to body frame
|
||||
Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega;
|
||||
if (p().body_P_sensor)
|
||||
boost::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega,
|
||||
|
|
|
@ -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:
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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 ¶ms,
|
||||
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
|
|
@ -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 {
|
||||
|
|
|
@ -168,7 +168,11 @@ Camera camera(Pose3(Rot3().retract(Vector3(0.1, 0.2, 0.3)), Point3(0, 5, 0)),
|
|||
Point3 point(10, 0, -5); // negative Z-axis convention of Snavely!
|
||||
Vector9 P = Camera().localCoordinates(camera);
|
||||
Vector3 X = point;
|
||||
#ifdef GTSAM_POSE3_EXPMAP
|
||||
Vector2 expectedMeasurement(1.3124675, 1.2057287);
|
||||
#else
|
||||
Vector2 expectedMeasurement(1.2431567, 1.2525694);
|
||||
#endif
|
||||
Matrix E1 = numericalDerivative21<Vector2, Vector9, Vector3>(adapted, P, X);
|
||||
Matrix E2 = numericalDerivative22<Vector2, Vector9, Vector3>(adapted, P, X);
|
||||
}
|
||||
|
@ -177,7 +181,11 @@ Matrix E2 = numericalDerivative22<Vector2, Vector9, Vector3>(adapted, P, X);
|
|||
// Check that Local worked as expected
|
||||
TEST(AdaptAutoDiff, Local) {
|
||||
using namespace example;
|
||||
#ifdef GTSAM_POSE3_EXPMAP
|
||||
Vector9 expectedP = (Vector9() << 0.1, 0.2, 0.3, 0.7583528428, 4.9582357859, -0.224941471539, 1, 0, 0).finished();
|
||||
#else
|
||||
Vector9 expectedP = (Vector9() << 0.1, 0.2, 0.3, 0, 5, 0, 1, 0, 0).finished();
|
||||
#endif
|
||||
EXPECT(equal_with_abs_tol(expectedP, P));
|
||||
Vector3 expectedX(10, 0, -5); // negative Z-axis convention of Snavely!
|
||||
EXPECT(equal_with_abs_tol(expectedX, X));
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
/* ************************************************************************* */
|
|
@ -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));
|
||||
|
|
Loading…
Reference in New Issue