diff --git a/.travis.python.sh b/.travis.python.sh index 1ef5799aa..772311f38 100644 --- a/.travis.python.sh +++ b/.travis.python.sh @@ -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 diff --git a/CMakeLists.txt b/CMakeLists.txt index a810ac9df..12cb6882e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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") set(GTSAM_EIGENCY_INSTALL_PATH ${GTSAM_CYTHON_INSTALL_PATH}/gtsam_eigency) - add_subdirectory(cython) + add_subdirectory(cython ${GTSAM_CYTHON_INSTALL_PATH}) else() set(GTSAM_INSTALL_CYTHON_TOOLBOX 0) # This will go into config.h endif() @@ -537,54 +536,54 @@ endif() print_build_options_for_target(gtsam) -message(STATUS " Use System Eigen : ${GTSAM_USE_SYSTEM_EIGEN} (Using version: ${GTSAM_EIGEN_VERSION})") +message(STATUS " Use System Eigen : ${GTSAM_USE_SYSTEM_EIGEN} (Using version: ${GTSAM_EIGEN_VERSION})") if(GTSAM_USE_TBB) - message(STATUS " Use Intel TBB : Yes") + message(STATUS " Use Intel TBB : Yes") elseif(TBB_FOUND) - message(STATUS " Use Intel TBB : TBB found but GTSAM_WITH_TBB is disabled") + message(STATUS " Use Intel TBB : TBB found but GTSAM_WITH_TBB is disabled") else() - message(STATUS " Use Intel TBB : TBB not found") + message(STATUS " Use Intel TBB : TBB not found") endif() if(GTSAM_USE_EIGEN_MKL) - message(STATUS " Eigen will use MKL : Yes") + message(STATUS " Eigen will use MKL : Yes") elseif(MKL_FOUND) - message(STATUS " Eigen will use MKL : MKL found but GTSAM_WITH_EIGEN_MKL is disabled") + message(STATUS " Eigen will use MKL : MKL found but GTSAM_WITH_EIGEN_MKL is disabled") else() - message(STATUS " Eigen will use MKL : MKL not found") + message(STATUS " Eigen will use MKL : MKL not found") endif() if(GTSAM_USE_EIGEN_MKL_OPENMP) - message(STATUS " Eigen will use MKL and OpenMP : Yes") + message(STATUS " Eigen will use MKL and OpenMP : Yes") elseif(OPENMP_FOUND AND NOT GTSAM_WITH_EIGEN_MKL) - message(STATUS " Eigen will use MKL and OpenMP : OpenMP found but GTSAM_WITH_EIGEN_MKL is disabled") + message(STATUS " Eigen will use MKL and OpenMP : OpenMP found but GTSAM_WITH_EIGEN_MKL is disabled") elseif(OPENMP_FOUND AND NOT MKL_FOUND) - message(STATUS " Eigen will use MKL and OpenMP : OpenMP found but MKL not found") + message(STATUS " Eigen will use MKL and OpenMP : OpenMP found but MKL not found") elseif(OPENMP_FOUND) - message(STATUS " Eigen will use MKL and OpenMP : OpenMP found but GTSAM_WITH_EIGEN_MKL_OPENMP is disabled") + message(STATUS " Eigen will use MKL and OpenMP : OpenMP found but GTSAM_WITH_EIGEN_MKL_OPENMP is disabled") else() - message(STATUS " Eigen will use MKL and OpenMP : OpenMP not found") + message(STATUS " Eigen will use MKL and OpenMP : OpenMP not found") endif() -message(STATUS " Default allocator : ${GTSAM_DEFAULT_ALLOCATOR}") +message(STATUS " Default allocator : ${GTSAM_DEFAULT_ALLOCATOR}") if(GTSAM_THROW_CHEIRALITY_EXCEPTION) - message(STATUS " Cheirality exceptions enabled : YES") + message(STATUS " Cheirality exceptions enabled : YES") else() - message(STATUS " Cheirality exceptions enabled : NO") + message(STATUS " Cheirality exceptions enabled : NO") endif() if(NOT MSVC AND NOT XCODE_VERSION) if(CCACHE_FOUND AND GTSAM_BUILD_WITH_CCACHE) - message(STATUS " Build with ccache : Yes") + message(STATUS " Build with ccache : Yes") elseif(CCACHE_FOUND) - message(STATUS " Build with ccache : ccache found but GTSAM_BUILD_WITH_CCACHE is disabled") + message(STATUS " Build with ccache : ccache found but GTSAM_BUILD_WITH_CCACHE is disabled") else() - message(STATUS " Build with ccache : No") + message(STATUS " Build with ccache : No") endif() endif() message(STATUS "Packaging flags ") -message(STATUS " CPack Source Generator : ${CPACK_SOURCE_GENERATOR}") -message(STATUS " CPack Generator : ${CPACK_GENERATOR}") +message(STATUS " CPack Source Generator : ${CPACK_SOURCE_GENERATOR}") +message(STATUS " CPack Generator : ${CPACK_GENERATOR}") message(STATUS "GTSAM flags ") print_config_flag(${GTSAM_USE_QUATERNIONS} "Quaternions as default Rot3 ") @@ -595,15 +594,19 @@ print_config_flag(${GTSAM_ALLOW_DEPRECATED_SINCE_V4} "Deprecated in GTSAM 4 al print_config_flag(${GTSAM_TYPEDEF_POINTS_TO_VECTORS} "Point3 is typedef to Vector3 ") print_config_flag(${GTSAM_SUPPORT_NESTED_DISSECTION} "Metis-based Nested Dissection ") print_config_flag(${GTSAM_TANGENT_PREINTEGRATION} "Use tangent-space preintegration") -print_config_flag(${GTSAM_BUILD_WRAP} "Build Wrap ") +print_config_flag(${GTSAM_BUILD_WRAP} "Build Wrap ") message(STATUS "MATLAB toolbox flags ") -print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install matlab toolbox ") +print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install MATLAB toolbox ") +if (${GTSAM_INSTALL_MATLAB_TOOLBOX}) + message(STATUS " MATLAB root : ${MATLAB_ROOT}") + message(STATUS " MEX binary : ${MEX_COMMAND}") +endif() message(STATUS "Cython toolbox flags ") -print_config_flag(${GTSAM_INSTALL_CYTHON_TOOLBOX} "Install Cython toolbox ") +print_config_flag(${GTSAM_INSTALL_CYTHON_TOOLBOX} "Install Cython toolbox ") if(GTSAM_INSTALL_CYTHON_TOOLBOX) - message(STATUS " Python version : ${GTSAM_PYTHON_VERSION}") + message(STATUS " Python version : ${GTSAM_PYTHON_VERSION}") endif() message(STATUS "===============================================================") diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index 15a02b6e8..088ba7ad2 100644 --- a/cmake/GtsamBuildTypes.cmake +++ b/cmake/GtsamBuildTypes.cmake @@ -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. diff --git a/cmake/GtsamCythonWrap.cmake b/cmake/GtsamCythonWrap.cmake index f1382729f..c8f876895 100644 --- a/cmake/GtsamCythonWrap.cmake +++ b/cmake/GtsamCythonWrap.cmake @@ -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() - diff --git a/cython/CMakeLists.txt b/cython/CMakeLists.txt index 4cc9d2f5d..221025575 100644 --- a/cython/CMakeLists.txt +++ b/cython/CMakeLists.txt @@ -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") diff --git a/cython/README.md b/cython/README.md index bc6e346d9..f69b7a5a6 100644 --- a/cython/README.md +++ b/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 /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 `/cython` in Release mode and `/cython` for other modes. -```bash - pip install -r /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: `/cython` - -- To use the library without installing system-wide: modify your `PYTHONPATH` to include the `GTSAM_CYTHON_INSTALL_PATH`: -```bash -export PYTHONPATH=$PYTHONPATH: -``` -- 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 - python -m unittest discover -``` + ```bash + cd + 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 _ in Python. -Examples: noiseModel_Gaussian, noiseModel_mEstimator_Tukey +- Inner namespace: Classes in inner namespace will be prefixed by \_ 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 for keys(), which can't be casted to FastVector - - 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 for keys(), which can't be casted to FastVector + - 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 * ": Bad style!!! (18-03-17 19:50) +- [x] Fix Python tests: don't use " import \* ": 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, to FastVector. -- [ ] 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 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) diff --git a/cython/gtsam/tests/test_FrobeniusFactor.py b/cython/gtsam/tests/test_FrobeniusFactor.py new file mode 100644 index 000000000..f3f5354bb --- /dev/null +++ b/cython/gtsam/tests/test_FrobeniusFactor.py @@ -0,0 +1,56 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +FrobeniusFactor unit tests. +Author: Frank Dellaert +""" +# pylint: disable=no-name-in-module, import-error, invalid-name +import unittest + +import numpy as np +from gtsam import (Rot3, SO3, SO4, FrobeniusBetweenFactorSO4, FrobeniusFactorSO4, + FrobeniusWormholeFactor, SOn) + +id = SO4() +v1 = np.array([0, 0, 0, 0.1, 0, 0]) +Q1 = SO4.Expmap(v1) +v2 = np.array([0, 0, 0, 0.01, 0.02, 0.03]) +Q2 = SO4.Expmap(v2) + + +class TestFrobeniusFactorSO4(unittest.TestCase): + """Test FrobeniusFactor factors.""" + + def test_frobenius_factor(self): + """Test creation of a factor that calculates the Frobenius norm.""" + factor = FrobeniusFactorSO4(1, 2) + actual = factor.evaluateError(Q1, Q2) + expected = (Q2.matrix()-Q1.matrix()).transpose().reshape((16,)) + np.testing.assert_array_equal(actual, expected) + + def test_frobenius_between_factor(self): + """Test creation of a Frobenius BetweenFactor.""" + factor = FrobeniusBetweenFactorSO4(1, 2, Q1.between(Q2)) + actual = factor.evaluateError(Q1, Q2) + expected = np.zeros((16,)) + np.testing.assert_array_almost_equal(actual, expected) + + def test_frobenius_wormhole_factor(self): + """Test creation of a factor that calculates Shonan error.""" + R1 = SO3.Expmap(v1[3:]) + R2 = SO3.Expmap(v2[3:]) + factor = FrobeniusWormholeFactor(1, 2, Rot3(R1.between(R2).matrix()), p=4) + I4 = SOn(4) + Q1 = I4.retract(v1) + Q2 = I4.retract(v2) + actual = factor.evaluateError(Q1, Q2) + expected = np.zeros((12,)) + np.testing.assert_array_almost_equal(actual, expected, decimal=4) + + +if __name__ == "__main__": + unittest.main() diff --git a/cython/gtsam/tests/test_NonlinearOptimizer.py b/cython/gtsam/tests/test_NonlinearOptimizer.py index efefb218a..985dc30a2 100644 --- a/cython/gtsam/tests/test_NonlinearOptimizer.py +++ b/cython/gtsam/tests/test_NonlinearOptimizer.py @@ -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) diff --git a/cython/gtsam/tests/test_logging_optimizer.py b/cython/gtsam/tests/test_logging_optimizer.py index c857a6f7d..2560a72a2 100644 --- a/cython/gtsam/tests/test_logging_optimizer.py +++ b/cython/gtsam/tests/test_logging_optimizer.py @@ -4,6 +4,12 @@ Author: Jing Wu and Frank Dellaert """ # pylint: disable=invalid-name +import sys +if sys.version_info.major >= 3: + from io import StringIO +else: + from cStringIO import StringIO + import unittest from datetime import datetime @@ -37,11 +43,24 @@ class TestOptimizeComet(GtsamTestCase): self.optimizer = gtsam.GaussNewtonOptimizer( graph, initial, self.params) + self.lmparams = gtsam.LevenbergMarquardtParams() + self.lmoptimizer = gtsam.LevenbergMarquardtOptimizer( + graph, initial, self.lmparams + ) + + # setup output capture + self.capturedOutput = StringIO() + sys.stdout = self.capturedOutput + + def tearDown(self): + """Reset print capture.""" + sys.stdout = sys.__stdout__ + def test_simple_printing(self): """Test with a simple hook.""" # Provide a hook that just prints - def hook(_, error: float): + def hook(_, error): print(error) # Only thing we require from optimizer is an iterate method @@ -51,6 +70,16 @@ class TestOptimizeComet(GtsamTestCase): actual = self.optimizer.values() self.gtsamAssertEquals(actual.atRot3(KEY), self.expected, tol=1e-6) + def test_lm_simple_printing(self): + """Make sure we are properly terminating LM""" + def hook(_, error): + print(error) + + gtsam_optimize(self.lmoptimizer, self.lmparams, hook) + + actual = self.lmoptimizer.values() + self.gtsamAssertEquals(actual.atRot3(KEY), self.expected, tol=1e-6) + @unittest.skip("Not a test we want run every time, as needs comet.ml account") def test_comet(self): """Test with a comet hook.""" @@ -65,7 +94,7 @@ class TestOptimizeComet(GtsamTestCase): + str(time.hour)+":"+str(time.minute)+":"+str(time.second)) # I want to do some comet thing here - def hook(optimizer, error: float): + def hook(optimizer, error): comet.log_metric("Karcher error", error, optimizer.iterations()) @@ -76,4 +105,4 @@ class TestOptimizeComet(GtsamTestCase): self.gtsamAssertEquals(actual.atRot3(KEY), self.expected) if __name__ == "__main__": - unittest.main() \ No newline at end of file + unittest.main() diff --git a/cython/gtsam/utils/logging_optimizer.py b/cython/gtsam/utils/logging_optimizer.py index b201bb8aa..3d9175951 100644 --- a/cython/gtsam/utils/logging_optimizer.py +++ b/cython/gtsam/utils/logging_optimizer.py @@ -4,15 +4,11 @@ Author: Jing Wu and Frank Dellaert """ # pylint: disable=invalid-name -from typing import TypeVar - from gtsam import NonlinearOptimizer, NonlinearOptimizerParams import gtsam -T = TypeVar('T') - -def optimize(optimizer: T, check_convergence, hook): +def optimize(optimizer, check_convergence, hook): """ Given an optimizer and a convergence check, iterate until convergence. After each iteration, hook(optimizer, error) is called. After the function, use values and errors to get the result. @@ -36,8 +32,8 @@ def optimize(optimizer: T, check_convergence, hook): current_error = new_error -def gtsam_optimize(optimizer: NonlinearOptimizer, - params: NonlinearOptimizerParams, +def gtsam_optimize(optimizer, + params, hook): """ Given an optimizer and params, iterate until convergence. After each iteration, hook(optimizer) is called. @@ -50,5 +46,7 @@ def gtsam_optimize(optimizer: NonlinearOptimizer, def check_convergence(optimizer, current_error, new_error): return (optimizer.iterations() >= params.getMaxIterations()) or ( gtsam.checkConvergence(params.getRelativeErrorTol(), params.getAbsoluteErrorTol(), params.getErrorTol(), - current_error, new_error)) + current_error, new_error)) or ( + isinstance(optimizer, gtsam.LevenbergMarquardtOptimizer) and optimizer.lambda_() > params.getlambdaUpperBound()) optimize(optimizer, check_convergence, hook) + return optimizer.values() diff --git a/cython/gtsam_eigency/CMakeLists.txt b/cython/gtsam_eigency/CMakeLists.txt index 77bead834..a0cf0fbde 100644 --- a/cython/gtsam_eigency/CMakeLists.txt +++ b/cython/gtsam_eigency/CMakeLists.txt @@ -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() diff --git a/cython/setup.py.in b/cython/setup.py.in index df92b564c..98a05c9f6 100644 --- a/cython/setup.py.in +++ b/cython/setup.py.in @@ -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 ) diff --git a/doc/robust.pdf b/doc/robust.pdf new file mode 100644 index 000000000..67b853f44 Binary files /dev/null and b/doc/robust.pdf differ diff --git a/docker/README.md b/docker/README.md new file mode 100644 index 000000000..0c136f94c --- /dev/null +++ b/docker/README.md @@ -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 + + diff --git a/docker/ubuntu-boost-tbb-eigen3/Dockerfile b/docker/ubuntu-boost-tbb-eigen3/Dockerfile deleted file mode 100644 index 33aa1ab96..000000000 --- a/docker/ubuntu-boost-tbb-eigen3/Dockerfile +++ /dev/null @@ -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 - diff --git a/docker/ubuntu-boost-tbb/Dockerfile b/docker/ubuntu-boost-tbb/Dockerfile new file mode 100644 index 000000000..9f6eea3b8 --- /dev/null +++ b/docker/ubuntu-boost-tbb/Dockerfile @@ -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 diff --git a/docker/ubuntu-boost-tbb/build.sh b/docker/ubuntu-boost-tbb/build.sh new file mode 100755 index 000000000..2dac4c3db --- /dev/null +++ b/docker/ubuntu-boost-tbb/build.sh @@ -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 . diff --git a/docker/ubuntu-gtsam-python-vnc/Dockerfile b/docker/ubuntu-gtsam-python-vnc/Dockerfile new file mode 100644 index 000000000..61ecd9b9a --- /dev/null +++ b/docker/ubuntu-gtsam-python-vnc/Dockerfile @@ -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' diff --git a/docker/ubuntu-gtsam-python-vnc/bootstrap.sh b/docker/ubuntu-gtsam-python-vnc/bootstrap.sh new file mode 100755 index 000000000..21356138f --- /dev/null +++ b/docker/ubuntu-gtsam-python-vnc/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 diff --git a/docker/ubuntu-gtsam-python-vnc/build.sh b/docker/ubuntu-gtsam-python-vnc/build.sh new file mode 100755 index 000000000..8d280252f --- /dev/null +++ b/docker/ubuntu-gtsam-python-vnc/build.sh @@ -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 . diff --git a/docker/ubuntu-gtsam-python-vnc/vnc.sh b/docker/ubuntu-gtsam-python-vnc/vnc.sh new file mode 100755 index 000000000..c0ab692c6 --- /dev/null +++ b/docker/ubuntu-gtsam-python-vnc/vnc.sh @@ -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 \ No newline at end of file diff --git a/docker/ubuntu-gtsam-python/Dockerfile b/docker/ubuntu-gtsam-python/Dockerfile new file mode 100644 index 000000000..c733ceb19 --- /dev/null +++ b/docker/ubuntu-gtsam-python/Dockerfile @@ -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"] diff --git a/docker/ubuntu-gtsam-python/build.sh b/docker/ubuntu-gtsam-python/build.sh new file mode 100755 index 000000000..1696f6c61 --- /dev/null +++ b/docker/ubuntu-gtsam-python/build.sh @@ -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 . diff --git a/docker/ubuntu-gtsam/Dockerfile b/docker/ubuntu-gtsam/Dockerfile new file mode 100644 index 000000000..187c76314 --- /dev/null +++ b/docker/ubuntu-gtsam/Dockerfile @@ -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"] diff --git a/docker/ubuntu-gtsam/build.sh b/docker/ubuntu-gtsam/build.sh new file mode 100755 index 000000000..bf545e9c2 --- /dev/null +++ b/docker/ubuntu-gtsam/build.sh @@ -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 . diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 7251c2b6f..476f4ae21 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -1,7 +1,4 @@ set (excluded_examples - DiscreteBayesNet_FG.cpp - UGM_chain.cpp - UGM_small.cpp elaboratePoint2KalmanFilter.cpp ) diff --git a/examples/Data/Klaus3.g2o b/examples/Data/Klaus3.g2o new file mode 100644 index 000000000..4c7233fa7 --- /dev/null +++ b/examples/Data/Klaus3.g2o @@ -0,0 +1,6 @@ +VERTEX_SE3:QUAT 0 -3.865747774038187 0.06639337702667497 -0.16064874691945374 0.024595211709139555 0.49179523413089893 -0.06279232989379242 0.8680954132776109 +VERTEX_SE3:QUAT 1 -3.614793159814815 0.04774490041587656 -0.2837650367985949 0.00991721787943912 0.4854918961891193 -0.042343290945895576 0.8731588132957809 +VERTEX_SE3:QUAT 2 -3.255096913553434 0.013296754286114112 -0.5339792269680574 -0.027851108010665374 0.585478168397957 -0.05088341463532465 0.8086102325762403 +EDGE_SE3:QUAT 0 1 0.2509546142233723 -0.01864847661079841 -0.12311628987914114 -0.022048798853273946 -0.01796327847857683 0.010210006313668573 0.9995433591728293 100.0 0.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 25.0 0.0 0.0 25.0 0.0 25.0 +EDGE_SE3:QUAT 0 2 0.6106508604847534 -0.05309662274056086 -0.3733304800486037 -0.054972994022992064 0.10432547598981769 -0.02221474884651081 0.9927742290779572 100.0 0.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 25.0 0.0 0.0 25.0 0.0 25.0 +EDGE_SE3:QUAT 1 2 0.3596962462613811 -0.03444814612976245 -0.25021419016946256 -0.03174661848656213 0.11646825423134777 -0.02951742735854383 0.9922479626852876 100.0 0.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 0.0 100.0 0.0 0.0 0.0 25.0 0.0 0.0 25.0 0.0 25.0 diff --git a/examples/Data/toyExample.g2o b/examples/Data/toyExample.g2o new file mode 100755 index 000000000..5ff1ba74a --- /dev/null +++ b/examples/Data/toyExample.g2o @@ -0,0 +1,11 @@ +VERTEX_SE3:QUAT 0 0 0 0 0 0 0 1 +VERTEX_SE3:QUAT 1 0 0 0 0 0 0 1 +VERTEX_SE3:QUAT 2 0 0 0 0.00499994 0.00499994 0.00499994 0.999963 +VERTEX_SE3:QUAT 3 0 0 0 -0.00499994 -0.00499994 -0.00499994 0.999963 +VERTEX_SE3:QUAT 4 0 0 0 0.00499994 0.00499994 0.00499994 0.999963 +EDGE_SE3:QUAT 1 2 1 2 0 0 0 0.707107 0.707107 100 0 0 0 0 0 100 0 0 0 0 100 0 0 0 100 0 0 100 0 100 +EDGE_SE3:QUAT 2 3 -3.26795e-07 1 0 0 0 0.707107 0.707107 100 0 0 0 0 0 100 0 0 0 0 100 0 0 0 100 0 0 100 0 100 +EDGE_SE3:QUAT 3 4 1 1 0 0 0 0.707107 0.707107 100 0 0 0 0 0 100 0 0 0 0 100 0 0 0 100 0 0 100 0 100 +EDGE_SE3:QUAT 3 1 6.9282e-07 2 0 0 0 1 1.73205e-07 100 0 0 0 0 0 100 0 0 0 0 100 0 0 0 100 0 0 100 0 100 +EDGE_SE3:QUAT 1 4 -1 1 0 0 0 -0.707107 0.707107 100 0 0 0 0 0 100 0 0 0 0 100 0 0 0 100 0 0 100 0 100 +EDGE_SE3:QUAT 0 1 0 0 0 0 0 0 1 100 0 0 0 0 0 100 0 0 0 0 100 0 0 0 100 0 0 100 0 100 diff --git a/examples/DiscreteBayesNetExample.cpp b/examples/DiscreteBayesNetExample.cpp new file mode 100644 index 000000000..5dca116c3 --- /dev/null +++ b/examples/DiscreteBayesNetExample.cpp @@ -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 +#include +#include + +#include + +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 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; +} diff --git a/examples/DiscreteBayesNet_FG.cpp b/examples/DiscreteBayesNet_FG.cpp index 6eb08c12e..121df4bef 100644 --- a/examples/DiscreteBayesNet_FG.cpp +++ b/examples/DiscreteBayesNet_FG.cpp @@ -15,105 +15,106 @@ * @author Abhijit * @date Jun 4, 2012 * - * We use the famous Rain/Cloudy/Sprinkler Example of [Russell & Norvig, 2009, p529] - * You may be familiar with other graphical model packages like BNT (available - * at http://bnt.googlecode.com/svn/trunk/docs/usage.html) where this is used as an - * example. The following demo is same as that in the above link, except that - * everything is using GTSAM. + * We use the famous Rain/Cloudy/Sprinkler Example of [Russell & Norvig, 2009, + * p529] You may be familiar with other graphical model packages like BNT + * (available at http://bnt.googlecode.com/svn/trunk/docs/usage.html) where this + * is used as an example. The following demo is same as that in the above link, + * except that everything is using GTSAM. */ #include -#include +#include + #include using namespace std; using namespace gtsam; int main(int argc, char **argv) { + // Define keys and a print function + Key C(1), S(2), R(3), W(4); + auto print = [=](DiscreteFactor::sharedValues values) { + cout << boolalpha << "Cloudy = " << static_cast((*values)[C]) + << " Sprinkler = " << static_cast((*values)[S]) + << " Rain = " << boolalpha << static_cast((*values)[R]) + << " WetGrass = " << static_cast((*values)[W]) << endl; + }; // We assume binary state variables // we have 0 == "False" and 1 == "True" const size_t nrStates = 2; // define variables - DiscreteKey Cloudy(1, nrStates), Sprinkler(2, nrStates), Rain(3, nrStates), - WetGrass(4, nrStates); + DiscreteKey Cloudy(C, nrStates), Sprinkler(S, nrStates), Rain(R, nrStates), + WetGrass(W, nrStates); // create Factor Graph of the bayes net DiscreteFactorGraph graph; // add factors - graph.add(Cloudy, "0.5 0.5"); //P(Cloudy) - graph.add(Cloudy & Sprinkler, "0.5 0.5 0.9 0.1"); //P(Sprinkler | Cloudy) - graph.add(Cloudy & Rain, "0.8 0.2 0.2 0.8"); //P(Rain | Cloudy) + graph.add(Cloudy, "0.5 0.5"); // P(Cloudy) + graph.add(Cloudy & Sprinkler, "0.5 0.5 0.9 0.1"); // P(Sprinkler | Cloudy) + graph.add(Cloudy & Rain, "0.8 0.2 0.2 0.8"); // P(Rain | Cloudy) graph.add(Sprinkler & Rain & WetGrass, - "1 0 0.1 0.9 0.1 0.9 0.001 0.99"); //P(WetGrass | Sprinkler, Rain) + "1 0 0.1 0.9 0.1 0.9 0.001 0.99"); // P(WetGrass | Sprinkler, Rain) - // Alternatively we can also create a DiscreteBayesNet, add DiscreteConditional - // factors and create a FactorGraph from it. (See testDiscreteBayesNet.cpp) + // Alternatively we can also create a DiscreteBayesNet, add + // DiscreteConditional factors and create a FactorGraph from it. (See + // testDiscreteBayesNet.cpp) // Since this is a relatively small distribution, we can as well print // the whole distribution.. cout << "Distribution of Example: " << endl; cout << setw(11) << "Cloudy(C)" << setw(14) << "Sprinkler(S)" << setw(10) - << "Rain(R)" << setw(14) << "WetGrass(W)" << setw(15) << "P(C,S,R,W)" - << endl; + << "Rain(R)" << setw(14) << "WetGrass(W)" << setw(15) << "P(C,S,R,W)" + << endl; for (size_t a = 0; a < nrStates; a++) for (size_t m = 0; m < nrStates; m++) for (size_t h = 0; h < nrStates; h++) for (size_t c = 0; c < nrStates; c++) { DiscreteFactor::Values values; - values[Cloudy.first] = c; - values[Sprinkler.first] = h; - values[Rain.first] = m; - values[WetGrass.first] = a; + values[C] = c; + values[S] = h; + values[R] = m; + values[W] = a; double prodPot = graph(values); - cout << boolalpha << setw(8) << (bool) c << setw(14) - << (bool) h << setw(12) << (bool) m << setw(13) - << (bool) a << setw(16) << prodPot << endl; + cout << setw(8) << static_cast(c) << setw(14) + << static_cast(h) << setw(12) << static_cast(m) + << setw(13) << static_cast(a) << setw(16) << prodPot + << endl; } - // "Most Probable Explanation", i.e., configuration with largest value - DiscreteSequentialSolver solver(graph); - DiscreteFactor::sharedValues optimalDecoding = solver.optimize(); - cout <<"\nMost Probable Explanation (MPE):" << endl; - cout << boolalpha << "Cloudy = " << (bool)(*optimalDecoding)[Cloudy.first] - << " Sprinkler = " << (bool)(*optimalDecoding)[Sprinkler.first] - << " Rain = " << boolalpha << (bool)(*optimalDecoding)[Rain.first] - << " WetGrass = " << (bool)(*optimalDecoding)[WetGrass.first]<< endl; + DiscreteFactor::sharedValues mpe = graph.eliminateSequential()->optimize(); + cout << "\nMost Probable Explanation (MPE):" << endl; + print(mpe); + // "Inference" We show an inference query like: probability that the Sprinkler + // was on; given that the grass is wet i.e. P( S | C=0) = ? - // "Inference" We show an inference query like: probability that the Sprinkler was on; - // given that the grass is wet i.e. P( S | W=1) =? - cout << "\nInference Query: Probability of Sprinkler being on given Grass is Wet" << endl; + // add evidence that it is not Cloudy + graph.add(Cloudy, "1 0"); - // Method 1: we can compute the joint marginal P(S,W) and from that we can compute - // P(S | W=1) = P(S,W=1)/P(W=1) We do this in following three steps.. + // solve again, now with evidence + DiscreteBayesNet::shared_ptr chordal = graph.eliminateSequential(); + DiscreteFactor::sharedValues mpe_with_evidence = chordal->optimize(); - //Step1: Compute P(S,W) - DiscreteFactorGraph jointFG; - jointFG = *solver.jointFactorGraph(DiscreteKeys(Sprinkler & WetGrass).indices()); - DecisionTreeFactor probSW = jointFG.product(); + cout << "\nMPE given C=0:" << endl; + print(mpe_with_evidence); - //Step2: Compute P(W) - DiscreteFactor::shared_ptr probW = solver.marginalFactor(WetGrass.first); - - //Step3: Computer P(S | W=1) = P(S,W=1)/P(W=1) - DiscreteFactor::Values values; - values[WetGrass.first] = 1; - - //print P(S=0|W=1) - values[Sprinkler.first] = 0; - cout << "P(S=0|W=1) = " << probSW(values)/(*probW)(values) << endl; - - //print P(S=1|W=1) - values[Sprinkler.first] = 1; - cout << "P(S=1|W=1) = " << probSW(values)/(*probW)(values) << endl; - - // TODO: Method 2 : One way is to modify the factor graph to - // incorporate the evidence node and compute the marginal - // TODO: graph.addEvidence(Cloudy,0); + // we can also calculate arbitrary marginals: + DiscreteMarginals marginals(graph); + cout << "\nP(S=1|C=0):" << marginals.marginalProbabilities(Sprinkler)[1] + << endl; + cout << "\nP(R=0|C=0):" << marginals.marginalProbabilities(Rain)[0] << endl; + cout << "\nP(W=1|C=0):" << marginals.marginalProbabilities(WetGrass)[1] + << endl; + // We can also sample from it + cout << "\n10 samples:" << endl; + for (size_t i = 0; i < 10; i++) { + DiscreteFactor::sharedValues sample = chordal->sample(); + print(sample); + } return 0; } diff --git a/examples/HMMExample.cpp b/examples/HMMExample.cpp new file mode 100644 index 000000000..ee861e381 --- /dev/null +++ b/examples/HMMExample.cpp @@ -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 +#include +#include + +#include +#include + +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 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; +} diff --git a/examples/IMUKittiExampleGPS.cpp b/examples/IMUKittiExampleGPS.cpp new file mode 100644 index 000000000..f1d89b47a --- /dev/null +++ b/examples/IMUKittiExampleGPS.cpp @@ -0,0 +1,359 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file IMUKittiExampleGPS + * @brief Example of application of ISAM2 for GPS-aided navigation on the KITTI VISION BENCHMARK SUITE + * @author Ported by Thomas Jespersen (thomasj@tkjelectronics.dk), TKJ Electronics + */ + +// GTSAM related includes. +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y) +using symbol_shorthand::V; // Vel (xdot,ydot,zdot) +using symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz) + +struct KittiCalibration { + double body_ptx; + double body_pty; + double body_ptz; + double body_prx; + double body_pry; + double body_prz; + double accelerometer_sigma; + double gyroscope_sigma; + double integration_sigma; + double accelerometer_bias_sigma; + double gyroscope_bias_sigma; + double average_delta_t; +}; + +struct ImuMeasurement { + double time; + double dt; + Vector3 accelerometer; + Vector3 gyroscope; // omega +}; + +struct GpsMeasurement { + double time; + Vector3 position; // x,y,z +}; + +const string output_filename = "IMUKittiExampleGPSResults.csv"; + +void loadKittiData(KittiCalibration& kitti_calibration, + vector& imu_measurements, + vector& gps_measurements) { + string line; + + // Read IMU metadata and compute relative sensor pose transforms + // BodyPtx BodyPty BodyPtz BodyPrx BodyPry BodyPrz AccelerometerSigma GyroscopeSigma IntegrationSigma + // AccelerometerBiasSigma GyroscopeBiasSigma AverageDeltaT + string imu_metadata_file = findExampleDataFile("KittiEquivBiasedImu_metadata.txt"); + ifstream imu_metadata(imu_metadata_file.c_str()); + + printf("-- Reading sensor metadata\n"); + + getline(imu_metadata, line, '\n'); // ignore the first line + + // Load Kitti calibration + getline(imu_metadata, line, '\n'); + sscanf(line.c_str(), "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf", + &kitti_calibration.body_ptx, + &kitti_calibration.body_pty, + &kitti_calibration.body_ptz, + &kitti_calibration.body_prx, + &kitti_calibration.body_pry, + &kitti_calibration.body_prz, + &kitti_calibration.accelerometer_sigma, + &kitti_calibration.gyroscope_sigma, + &kitti_calibration.integration_sigma, + &kitti_calibration.accelerometer_bias_sigma, + &kitti_calibration.gyroscope_bias_sigma, + &kitti_calibration.average_delta_t); + printf("IMU metadata: %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf\n", + kitti_calibration.body_ptx, + kitti_calibration.body_pty, + kitti_calibration.body_ptz, + kitti_calibration.body_prx, + kitti_calibration.body_pry, + kitti_calibration.body_prz, + kitti_calibration.accelerometer_sigma, + kitti_calibration.gyroscope_sigma, + kitti_calibration.integration_sigma, + kitti_calibration.accelerometer_bias_sigma, + kitti_calibration.gyroscope_bias_sigma, + kitti_calibration.average_delta_t); + + // Read IMU data + // Time dt accelX accelY accelZ omegaX omegaY omegaZ + string imu_data_file = findExampleDataFile("KittiEquivBiasedImu.txt"); + printf("-- Reading IMU measurements from file\n"); + { + ifstream imu_data(imu_data_file.c_str()); + getline(imu_data, line, '\n'); // ignore the first line + + double time = 0, dt = 0, acc_x = 0, acc_y = 0, acc_z = 0, gyro_x = 0, gyro_y = 0, gyro_z = 0; + while (!imu_data.eof()) { + getline(imu_data, line, '\n'); + sscanf(line.c_str(), "%lf %lf %lf %lf %lf %lf %lf %lf", + &time, &dt, + &acc_x, &acc_y, &acc_z, + &gyro_x, &gyro_y, &gyro_z); + + ImuMeasurement measurement; + measurement.time = time; + measurement.dt = dt; + measurement.accelerometer = Vector3(acc_x, acc_y, acc_z); + measurement.gyroscope = Vector3(gyro_x, gyro_y, gyro_z); + imu_measurements.push_back(measurement); + } + } + + // Read GPS data + // Time,X,Y,Z + string gps_data_file = findExampleDataFile("KittiGps_converted.txt"); + printf("-- Reading GPS measurements from file\n"); + { + ifstream gps_data(gps_data_file.c_str()); + getline(gps_data, line, '\n'); // ignore the first line + + double time = 0, gps_x = 0, gps_y = 0, gps_z = 0; + while (!gps_data.eof()) { + getline(gps_data, line, '\n'); + sscanf(line.c_str(), "%lf,%lf,%lf,%lf", &time, &gps_x, &gps_y, &gps_z); + + GpsMeasurement measurement; + measurement.time = time; + measurement.position = Vector3(gps_x, gps_y, gps_z); + gps_measurements.push_back(measurement); + } + } +} + +int main(int argc, char* argv[]) { + KittiCalibration kitti_calibration; + vector imu_measurements; + vector gps_measurements; + loadKittiData(kitti_calibration, imu_measurements, gps_measurements); + + Vector6 BodyP = (Vector6() << kitti_calibration.body_ptx, kitti_calibration.body_pty, kitti_calibration.body_ptz, + kitti_calibration.body_prx, kitti_calibration.body_pry, kitti_calibration.body_prz) + .finished(); + auto body_T_imu = Pose3::Expmap(BodyP); + if (!body_T_imu.equals(Pose3(), 1e-5)) { + printf("Currently only support IMUinBody is identity, i.e. IMU and body frame are the same"); + exit(-1); + } + + // Configure different variables + // double t_offset = gps_measurements[0].time; + size_t first_gps_pose = 1; + size_t gps_skip = 10; // Skip this many GPS measurements each time + double g = 9.8; + auto w_coriolis = Vector3::Zero(); // zero vector + + // Configure noise models + auto noise_model_gps = noiseModel::Diagonal::Precisions((Vector6() << Vector3::Constant(0), + Vector3::Constant(1.0/0.07)) + .finished()); + + // Set initial conditions for the estimated trajectory + // initial pose is the reference frame (navigation frame) + auto current_pose_global = Pose3(Rot3(), gps_measurements[first_gps_pose].position); + // the vehicle is stationary at the beginning at position 0,0,0 + Vector3 current_velocity_global = Vector3::Zero(); + auto current_bias = imuBias::ConstantBias(); // init with zero bias + + auto sigma_init_x = noiseModel::Diagonal::Precisions((Vector6() << Vector3::Constant(0), + Vector3::Constant(1.0)) + .finished()); + auto sigma_init_v = noiseModel::Diagonal::Sigmas(Vector3::Constant(1000.0)); + auto sigma_init_b = noiseModel::Diagonal::Sigmas((Vector6() << Vector3::Constant(0.100), + Vector3::Constant(5.00e-05)) + .finished()); + + // Set IMU preintegration parameters + Matrix33 measured_acc_cov = I_3x3 * pow(kitti_calibration.accelerometer_sigma, 2); + Matrix33 measured_omega_cov = I_3x3 * pow(kitti_calibration.gyroscope_sigma, 2); + // error committed in integrating position from velocities + Matrix33 integration_error_cov = I_3x3 * pow(kitti_calibration.integration_sigma, 2); + + auto imu_params = PreintegratedImuMeasurements::Params::MakeSharedU(g); + imu_params->accelerometerCovariance = measured_acc_cov; // acc white noise in continuous + imu_params->integrationCovariance = integration_error_cov; // integration uncertainty continuous + imu_params->gyroscopeCovariance = measured_omega_cov; // gyro white noise in continuous + imu_params->omegaCoriolis = w_coriolis; + + std::shared_ptr current_summarized_measurement = nullptr; + + // Set ISAM2 parameters and create ISAM2 solver object + ISAM2Params isam_params; + isam_params.factorization = ISAM2Params::CHOLESKY; + isam_params.relinearizeSkip = 10; + + ISAM2 isam(isam_params); + + // Create the factor graph and values object that will store new factors and values to add to the incremental graph + NonlinearFactorGraph new_factors; + Values new_values; // values storing the initial estimates of new nodes in the factor graph + + /// Main loop: + /// (1) we read the measurements + /// (2) we create the corresponding factors in the graph + /// (3) we solve the graph to obtain and optimal estimate of robot trajectory + printf("-- Starting main loop: inference is performed at each time step, but we plot trajectory every 10 steps\n"); + size_t j = 0; + for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) { + // At each non=IMU measurement we initialize a new node in the graph + auto current_pose_key = X(i); + auto current_vel_key = V(i); + auto current_bias_key = B(i); + double t = gps_measurements[i].time; + + if (i == first_gps_pose) { + // Create initial estimate and prior on initial pose, velocity, and biases + new_values.insert(current_pose_key, current_pose_global); + new_values.insert(current_vel_key, current_velocity_global); + new_values.insert(current_bias_key, current_bias); + new_factors.emplace_shared>(current_pose_key, current_pose_global, sigma_init_x); + new_factors.emplace_shared>(current_vel_key, current_velocity_global, sigma_init_v); + new_factors.emplace_shared>(current_bias_key, current_bias, sigma_init_b); + } else { + double t_previous = gps_measurements[i-1].time; + + // Summarize IMU data between the previous GPS measurement and now + current_summarized_measurement = std::make_shared(imu_params, current_bias); + static size_t included_imu_measurement_count = 0; + while (j < imu_measurements.size() && imu_measurements[j].time <= t) { + if (imu_measurements[j].time >= t_previous) { + current_summarized_measurement->integrateMeasurement(imu_measurements[j].accelerometer, + imu_measurements[j].gyroscope, + imu_measurements[j].dt); + included_imu_measurement_count++; + } + j++; + } + + // Create IMU factor + auto previous_pose_key = X(i-1); + auto previous_vel_key = V(i-1); + auto previous_bias_key = B(i-1); + + new_factors.emplace_shared(previous_pose_key, previous_vel_key, + current_pose_key, current_vel_key, + previous_bias_key, *current_summarized_measurement); + + // Bias evolution as given in the IMU metadata + auto sigma_between_b = noiseModel::Diagonal::Sigmas((Vector6() << + Vector3::Constant(sqrt(included_imu_measurement_count) * kitti_calibration.accelerometer_bias_sigma), + Vector3::Constant(sqrt(included_imu_measurement_count) * kitti_calibration.gyroscope_bias_sigma)) + .finished()); + new_factors.emplace_shared>(previous_bias_key, + current_bias_key, + imuBias::ConstantBias(), + sigma_between_b); + + // Create GPS factor + auto gps_pose = Pose3(current_pose_global.rotation(), gps_measurements[i].position); + if ((i % gps_skip) == 0) { + new_factors.emplace_shared>(current_pose_key, gps_pose, noise_model_gps); + new_values.insert(current_pose_key, gps_pose); + + printf("################ POSE INCLUDED AT TIME %lf ################\n", t); + gps_pose.translation().print(); + printf("\n\n"); + } else { + new_values.insert(current_pose_key, current_pose_global); + } + + // Add initial values for velocity and bias based on the previous estimates + new_values.insert(current_vel_key, current_velocity_global); + new_values.insert(current_bias_key, current_bias); + + // Update solver + // ======================================================================= + // We accumulate 2*GPSskip GPS measurements before updating the solver at + // first so that the heading becomes observable. + if (i > (first_gps_pose + 2*gps_skip)) { + printf("################ NEW FACTORS AT TIME %lf ################\n", t); + new_factors.print(); + + isam.update(new_factors, new_values); + + // Reset the newFactors and newValues list + new_factors.resize(0); + new_values.clear(); + + // Extract the result/current estimates + Values result = isam.calculateEstimate(); + + current_pose_global = result.at(current_pose_key); + current_velocity_global = result.at(current_vel_key); + current_bias = result.at(current_bias_key); + + printf("\n################ POSE AT TIME %lf ################\n", t); + current_pose_global.print(); + printf("\n\n"); + } + } + } + + // Save results to file + printf("\nWriting results to file...\n"); + FILE* fp_out = fopen(output_filename.c_str(), "w+"); + fprintf(fp_out, "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m)\n"); + + Values result = isam.calculateEstimate(); + for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) { + auto pose_key = X(i); + auto vel_key = V(i); + auto bias_key = B(i); + + auto pose = result.at(pose_key); + auto velocity = result.at(vel_key); + auto bias = result.at(bias_key); + + auto pose_quat = pose.rotation().toQuaternion(); + auto gps = gps_measurements[i].position; + + cout << "State at #" << i << endl; + cout << "Pose:" << endl << pose << endl; + cout << "Velocity:" << endl << velocity << endl; + cout << "Bias:" << endl << bias << endl; + + fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n", + gps_measurements[i].time, + pose.x(), pose.y(), pose.z(), + pose_quat.x(), pose_quat.y(), pose_quat.z(), pose_quat.w(), + gps(0), gps(1), gps(2)); + } + + fclose(fp_out); +} diff --git a/examples/ImuFactorsExample.cpp b/examples/ImuFactorsExample.cpp index a4707ea46..63355631b 100644 --- a/examples/ImuFactorsExample.cpp +++ b/examples/ImuFactorsExample.cpp @@ -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 #include #include #include -#include -#include #include #include +#include +#include +#include +#include #include #include #include +// 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(X(correction_count)), result.at(V(correction_count))); diff --git a/examples/SFMExample.cpp b/examples/SFMExample.cpp index 7f0c79e0e..fddca8169 100644 --- a/examples/SFMExample.cpp +++ b/examples/SFMExample.cpp @@ -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(Symbol('l', j), corrupted_point); } initialEstimate.print("Initial Estimates:\n"); diff --git a/examples/UGM_chain.cpp b/examples/UGM_chain.cpp index 4ce4e7af4..3a885a844 100644 --- a/examples/UGM_chain.cpp +++ b/examples/UGM_chain.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file small.cpp + * @file UGM_chain.cpp * @brief UGM (undirected graphical model) examples: chain * @author Frank Dellaert * @author Abhijit Kundu @@ -19,10 +19,9 @@ * for more explanation. This code demos the same example using GTSAM. */ -#include -#include -#include #include +#include +#include #include @@ -30,9 +29,8 @@ using namespace std; using namespace gtsam; int main(int argc, char** argv) { - - // Set Number of Nodes in the Graph - const int nrNodes = 60; + // Set Number of Nodes in the Graph + const int nrNodes = 60; // Each node takes 1 of 7 possible states denoted by 0-6 in following order: // ["VideoGames" "Industry" "GradSchool" "VideoGames(with PhD)" @@ -40,70 +38,55 @@ int main(int argc, char** argv) { const size_t nrStates = 7; // define variables - vector nodes; - for (int i = 0; i < nrNodes; i++){ - DiscreteKey dk(i, nrStates); - nodes.push_back(dk); - } + vector nodes; + for (int i = 0; i < nrNodes; i++) { + DiscreteKey dk(i, nrStates); + nodes.push_back(dk); + } // create graph DiscreteFactorGraph graph; // add node potentials graph.add(nodes[0], ".3 .6 .1 0 0 0 0"); - for (int i = 1; i < nrNodes; i++) - graph.add(nodes[i], "1 1 1 1 1 1 1"); + for (int i = 1; i < nrNodes; i++) graph.add(nodes[i], "1 1 1 1 1 1 1"); - const std::string edgePotential = ".08 .9 .01 0 0 0 .01 " - ".03 .95 .01 0 0 0 .01 " - ".06 .06 .75 .05 .05 .02 .01 " - "0 0 0 .3 .6 .09 .01 " - "0 0 0 .02 .95 .02 .01 " - "0 0 0 .01 .01 .97 .01 " - "0 0 0 0 0 0 1"; + const std::string edgePotential = + ".08 .9 .01 0 0 0 .01 " + ".03 .95 .01 0 0 0 .01 " + ".06 .06 .75 .05 .05 .02 .01 " + "0 0 0 .3 .6 .09 .01 " + "0 0 0 .02 .95 .02 .01 " + "0 0 0 .01 .01 .97 .01 " + "0 0 0 0 0 0 1"; // add edge potentials for (int i = 0; i < nrNodes - 1; i++) graph.add(nodes[i] & nodes[i + 1], edgePotential); cout << "Created Factor Graph with " << nrNodes << " variable nodes and " - << graph.size() << " factors (Unary+Edge)."; + << graph.size() << " factors (Unary+Edge)."; // "Decoding", i.e., configuration with largest value // We use sequential variable elimination - DiscreteSequentialSolver solver(graph); - DiscreteFactor::sharedValues optimalDecoding = solver.optimize(); + DiscreteBayesNet::shared_ptr chordal = graph.eliminateSequential(); + DiscreteFactor::sharedValues optimalDecoding = chordal->optimize(); optimalDecoding->print("\nMost Probable Explanation (optimalDecoding)\n"); // "Inference" Computing marginals for each node - - - cout << "\nComputing Node Marginals ..(Sequential Elimination)" << endl; - gttic_(Sequential); - for (vector::iterator itr = nodes.begin(); itr != nodes.end(); - ++itr) { - //Compute the marginal - Vector margProbs = solver.marginalProbabilities(*itr); - - //Print the marginals - cout << "Node#" << setw(4) << itr->first << " : "; - print(margProbs); - } - gttoc_(Sequential); - // Here we'll make use of DiscreteMarginals class, which makes use of // bayes-tree based shortcut evaluation of marginals DiscreteMarginals marginals(graph); cout << "\nComputing Node Marginals ..(BayesTree based)" << endl; gttic_(Multifrontal); - for (vector::iterator itr = nodes.begin(); itr != nodes.end(); - ++itr) { - //Compute the marginal - Vector margProbs = marginals.marginalProbabilities(*itr); + for (vector::iterator it = nodes.begin(); it != nodes.end(); + ++it) { + // Compute the marginal + Vector margProbs = marginals.marginalProbabilities(*it); - //Print the marginals - cout << "Node#" << setw(4) << itr->first << " : "; + // Print the marginals + cout << "Node#" << setw(4) << it->first << " : "; print(margProbs); } gttoc_(Multifrontal); @@ -111,4 +94,3 @@ int main(int argc, char** argv) { tictoc_print_(); return 0; } - diff --git a/examples/UGM_small.cpp b/examples/UGM_small.cpp index f5338bc67..27a6205a3 100644 --- a/examples/UGM_small.cpp +++ b/examples/UGM_small.cpp @@ -10,15 +10,16 @@ * -------------------------------------------------------------------------- */ /** - * @file small.cpp + * @file UGM_small.cpp * @brief UGM (undirected graphical model) examples: small * @author Frank Dellaert * * See http://www.di.ens.fr/~mschmidt/Software/UGM/small.html */ +#include #include -#include +#include using namespace std; using namespace gtsam; @@ -61,24 +62,24 @@ int main(int argc, char** argv) { // "Decoding", i.e., configuration with largest value (MPE) // We use sequential variable elimination - DiscreteSequentialSolver solver(graph); - DiscreteFactor::sharedValues optimalDecoding = solver.optimize(); + DiscreteBayesNet::shared_ptr chordal = graph.eliminateSequential(); + DiscreteFactor::sharedValues optimalDecoding = chordal->optimize(); optimalDecoding->print("\noptimalDecoding"); // "Inference" Computing marginals cout << "\nComputing Node Marginals .." << endl; - Vector margProbs; + DiscreteMarginals marginals(graph); - margProbs = solver.marginalProbabilities(Cathy); + Vector margProbs = marginals.marginalProbabilities(Cathy); print(margProbs, "Cathy's Node Marginal:"); - margProbs = solver.marginalProbabilities(Heather); + margProbs = marginals.marginalProbabilities(Heather); print(margProbs, "Heather's Node Marginal"); - margProbs = solver.marginalProbabilities(Mark); + margProbs = marginals.marginalProbabilities(Mark); print(margProbs, "Mark's Node Marginal"); - margProbs = solver.marginalProbabilities(Allison); + margProbs = marginals.marginalProbabilities(Allison); print(margProbs, "Allison's Node Marginal"); return 0; diff --git a/gtsam.h b/gtsam.h index cf6f658a1..2cd30be42 100644 --- a/gtsam.h +++ b/gtsam.h @@ -598,6 +598,7 @@ class SOn { // Standard Constructors SOn(size_t n); static gtsam::SOn FromMatrix(Matrix R); + static gtsam::SOn Lift(size_t n, Matrix R); // Testable void print(string s) const; @@ -1458,7 +1459,7 @@ virtual class Null: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double residual(double error) const; + double loss(double error) const; }; virtual class Fair: gtsam::noiseModel::mEstimator::Base { @@ -1469,7 +1470,7 @@ virtual class Fair: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double residual(double error) const; + double loss(double error) const; }; virtual class Huber: gtsam::noiseModel::mEstimator::Base { @@ -1480,7 +1481,7 @@ virtual class Huber: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double residual(double error) const; + double loss(double error) const; }; virtual class Cauchy: gtsam::noiseModel::mEstimator::Base { @@ -1491,7 +1492,7 @@ virtual class Cauchy: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double residual(double error) const; + double loss(double error) const; }; virtual class Tukey: gtsam::noiseModel::mEstimator::Base { @@ -1502,7 +1503,7 @@ virtual class Tukey: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double residual(double error) const; + double loss(double error) const; }; virtual class Welsch: gtsam::noiseModel::mEstimator::Base { @@ -1513,7 +1514,7 @@ virtual class Welsch: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double residual(double error) const; + double loss(double error) const; }; virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base { @@ -1524,7 +1525,7 @@ virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double residual(double error) const; + double loss(double error) const; }; virtual class DCS: gtsam::noiseModel::mEstimator::Base { @@ -1535,7 +1536,7 @@ virtual class DCS: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double residual(double error) const; + double loss(double error) const; }; virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base { @@ -1546,7 +1547,7 @@ virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double residual(double error) const; + double loss(double error) const; }; }///\namespace mEstimator @@ -1937,6 +1938,22 @@ virtual class ConjugateGradientParameters : gtsam::IterativeOptimizationParamete void print() const; }; +#include +virtual class PreconditionerParameters { + PreconditionerParameters(); +}; + +virtual class DummyPreconditionerParameters : gtsam::PreconditionerParameters { + DummyPreconditionerParameters(); +}; + +#include +virtual class PCGSolverParameters : gtsam::ConjugateGradientParameters { + PCGSolverParameters(); + void print(string s); + void setPreconditionerParams(gtsam::PreconditionerParameters* preconditioner); +}; + #include virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters { SubgraphSolverParameters(); @@ -2835,6 +2852,34 @@ virtual class KarcherMeanFactor : gtsam::NonlinearFactor { KarcherMeanFactor(const gtsam::KeyVector& keys); }; +#include +gtsam::noiseModel::Isotropic* ConvertPose3NoiseModel( + gtsam::noiseModel::Base* model, size_t d); + +template +virtual class FrobeniusFactor : gtsam::NoiseModelFactor { + FrobeniusFactor(size_t key1, size_t key2); + FrobeniusFactor(size_t key1, size_t key2, gtsam::noiseModel::Base* model); + + Vector evaluateError(const T& R1, const T& R2); +}; + +template +virtual class FrobeniusBetweenFactor : gtsam::NoiseModelFactor { + FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12); + FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12, gtsam::noiseModel::Base* model); + + Vector evaluateError(const T& R1, const T& R2); +}; + +virtual class FrobeniusWormholeFactor : gtsam::NoiseModelFactor { + FrobeniusWormholeFactor(size_t key1, size_t key2, const gtsam::Rot3& R12, + size_t p); + FrobeniusWormholeFactor(size_t key1, size_t key2, const gtsam::Rot3& R12, + size_t p, gtsam::noiseModel::Base* model); + Vector evaluateError(const gtsam::SOn& Q1, const gtsam::SOn& Q2); +}; + //************************************************************************* // Navigation //************************************************************************* diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 3d1bbd2a7..16dca6736 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -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 diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index fa70e5b00..1c1138438 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -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 - void save(Archive & ar, const gtsam::Matrix & m, unsigned int /*version*/) { + template + void save(Archive & ar, + const Eigen::Matrix & 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 - void load(Archive & ar, gtsam::Matrix & m, unsigned int /*version*/) { + template + void load(Archive & ar, + Eigen::Matrix & 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 + void serialize(Archive & ar, + Eigen::Matrix & m, + const unsigned int version) { + split_free(ar, m, version); + } + + // specialized to Matrix for MATLAB wrapper + template + 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); - diff --git a/gtsam/base/types.h b/gtsam/base/types.h index c5dac9ab7..aaada3cee 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -28,6 +28,7 @@ #include #include +#include #ifdef GTSAM_USE_TBB #include @@ -54,7 +55,7 @@ namespace gtsam { /// Function to demangle type name of variable, e.g. demangle(typeid(x).name()) - std::string demangle(const char* name); + std::string GTSAM_EXPORT demangle(const char* name); /// Integer nonlinear key type typedef std::uint64_t Key; diff --git a/gtsam/discrete/DiscreteBayesNet.h b/gtsam/discrete/DiscreteBayesNet.h index dcc336f89..237caf745 100644 --- a/gtsam/discrete/DiscreteBayesNet.h +++ b/gtsam/discrete/DiscreteBayesNet.h @@ -20,13 +20,14 @@ #include #include #include +#include #include #include namespace gtsam { /** A Bayes net made from linear-Discrete densities */ - class GTSAM_EXPORT DiscreteBayesNet: public FactorGraph + class GTSAM_EXPORT DiscreteBayesNet: public BayesNet { public: diff --git a/gtsam/discrete/DiscreteBayesTree.cpp b/gtsam/discrete/DiscreteBayesTree.cpp index bed50a470..990d10dbe 100644 --- a/gtsam/discrete/DiscreteBayesTree.cpp +++ b/gtsam/discrete/DiscreteBayesTree.cpp @@ -29,13 +29,32 @@ namespace gtsam { template class BayesTreeCliqueBase; template class BayesTree; + /* ************************************************************************* */ + 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 diff --git a/gtsam/discrete/DiscreteBayesTree.h b/gtsam/discrete/DiscreteBayesTree.h index 0df6ab476..3d6e016fd 100644 --- a/gtsam/discrete/DiscreteBayesTree.h +++ b/gtsam/discrete/DiscreteBayesTree.h @@ -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 #include #include +#include #include +#include + 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 - { - public: - typedef DiscreteBayesTreeClique This; - typedef BayesTreeCliqueBase Base; - typedef boost::shared_ptr shared_ptr; - typedef boost::weak_ptr weak_ptr; - DiscreteBayesTreeClique() {} - DiscreteBayesTreeClique(const boost::shared_ptr& conditional) : Base(conditional) {} - }; +/* ************************************************************************* */ +/** A clique in a DiscreteBayesTree */ +class GTSAM_EXPORT DiscreteBayesTreeClique + : public BayesTreeCliqueBase { + public: + typedef DiscreteBayesTreeClique This; + typedef BayesTreeCliqueBase + Base; + typedef boost::shared_ptr shared_ptr; + typedef boost::weak_ptr weak_ptr; + DiscreteBayesTreeClique() {} + DiscreteBayesTreeClique( + const boost::shared_ptr& conditional) + : Base(conditional) {} - /* ************************************************************************* */ - /** A Bayes tree representing a Discrete density */ - class GTSAM_EXPORT DiscreteBayesTree : - public BayesTree - { - private: - typedef BayesTree 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 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 { + private: + typedef BayesTree Base; - /** Check equality */ - bool equals(const This& other, double tol = 1e-9) const; - }; + public: + typedef DiscreteBayesTree This; + typedef boost::shared_ptr 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 diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp index 2ab3054a8..ac7c58405 100644 --- a/gtsam/discrete/DiscreteConditional.cpp +++ b/gtsam/discrete/DiscreteConditional.cpp @@ -27,6 +27,7 @@ #include #include #include +#include #include 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 cdf(nj); + Key key = firstFrontalKey(); + size_t nj = cardinality(key); + vector 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 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 distribution(p.begin(), p.end()); + return distribution(rng); } -/* ******************************************************************************** */ -//void DiscreteConditional::permuteWithInverse( -// const Permutation& inversePermutation) { -// IndexConditionalOrdered::permuteWithInverse(inversePermutation); -// Potentials::permuteWithInverse(inversePermutation); -//} /* ******************************************************************************** */ }// namespace diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h index 3da8d0a82..225e6e1d3 100644 --- a/gtsam/discrete/DiscreteConditional.h +++ b/gtsam/discrete/DiscreteConditional.h @@ -24,6 +24,8 @@ #include #include +#include + 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(this)->print(s, formatter); + } + /// Evaluate, just look up in AlgebraicDecisonTree virtual double operator()(const Values& values) const { return Potentials::operator()(values); diff --git a/gtsam/discrete/Potentials.cpp b/gtsam/discrete/Potentials.cpp index c4cdbe0ef..fe99ea975 100644 --- a/gtsam/discrete/Potentials.cpp +++ b/gtsam/discrete/Potentials.cpp @@ -15,50 +15,52 @@ * @author Frank Dellaert */ -#include #include +#include + #include +#include + using namespace std; namespace gtsam { - // explicit instantiation - template class DecisionTree ; - template class AlgebraicDecisionTree ; +// explicit instantiation +template class DecisionTree; +template class AlgebraicDecisionTree; - /* ************************************************************************* */ - 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 @@ -95,4 +97,4 @@ namespace gtsam { /* ************************************************************************* */ -} // namespace gtsam +} // namespace gtsam diff --git a/gtsam/discrete/Signature.cpp b/gtsam/discrete/Signature.cpp index 89e763703..94b160a29 100644 --- a/gtsam/discrete/Signature.cpp +++ b/gtsam/discrete/Signature.cpp @@ -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 Signature::cpt() const { vector 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; } diff --git a/gtsam/discrete/Signature.h b/gtsam/discrete/Signature.h index 587cd6b30..6c59b5bff 100644 --- a/gtsam/discrete/Signature.h +++ b/gtsam/discrete/Signature.h @@ -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; diff --git a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp index 753af16d8..0261ef833 100644 --- a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp +++ b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp @@ -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_); diff --git a/gtsam/discrete/tests/testDiscreteBayesNet.cpp b/gtsam/discrete/tests/testDiscreteBayesNet.cpp index 5ed662332..2b440e5a0 100644 --- a/gtsam/discrete/tests/testDiscreteBayesNet.cpp +++ b/gtsam/discrete/tests/testDiscreteBayesNet.cpp @@ -18,110 +18,135 @@ #include #include -#include +#include #include +#include +#include #include -#include + #include +#include using namespace boost::assign; #include +#include +#include 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(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(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((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); } /* ************************************************************************* */ - diff --git a/gtsam/discrete/tests/testDiscreteBayesTree.cpp b/gtsam/discrete/tests/testDiscreteBayesTree.cpp index 93126f642..ecf485036 100644 --- a/gtsam/discrete/tests/testDiscreteBayesTree.cpp +++ b/gtsam/discrete/tests/testDiscreteBayesTree.cpp @@ -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 -//#include -//#include -// -//#include -//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 +#include +#include +#include +#include + +#include +using namespace boost::assign; + #include -// -//using namespace std; -//using namespace gtsam; -// -//static bool debug = false; -// -///** -// * Custom clique class to debug shortcuts -// */ -////class Clique: public BayesTreeCliqueBaseOrdered { -//// -////protected: -//// -////public: -//// -//// typedef BayesTreeCliqueBaseOrdered Base; -//// typedef boost::shared_ptr shared_ptr; -//// -//// // Constructors -//// Clique() { -//// } -//// Clique(const DiscreteConditional::shared_ptr& conditional) : -//// Base(conditional) { -//// } -//// Clique( -//// const std::pair& 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 c: children_) -//// result *= c->evaluate(values); -//// return result; -//// } -//// -////}; -// -////typedef BayesTreeOrdered 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 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 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 + +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 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 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); } /* ************************************************************************* */ - diff --git a/gtsam/discrete/tests/testDiscreteBayesTree.pdf b/gtsam/discrete/tests/testDiscreteBayesTree.pdf new file mode 100644 index 000000000..e8167d455 Binary files /dev/null and b/gtsam/discrete/tests/testDiscreteBayesTree.pdf differ diff --git a/gtsam/discrete/tests/testDiscreteConditional.cpp b/gtsam/discrete/tests/testDiscreteConditional.cpp index 888bf76df..3ac3ffc9e 100644 --- a/gtsam/discrete/tests/testDiscreteConditional.cpp +++ b/gtsam/discrete/tests/testDiscreteConditional.cpp @@ -16,9 +16,9 @@ * @date Feb 14, 2011 */ -#include #include #include +#include using namespace boost::assign; #include @@ -36,6 +36,11 @@ TEST( DiscreteConditional, constructors) DiscreteConditional::shared_ptr expected1 = // boost::make_shared(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(X | Y = table); - EXPECT(expected1); + auto actual1 = boost::make_shared(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 c; c.push_back(boost::make_shared(A | B = "1/2 2/1")); c.push_back(boost::make_shared(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); +} /* ************************************************************************* */ - diff --git a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp index 0fbf44097..1defd5acf 100644 --- a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp +++ b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include diff --git a/gtsam/discrete/tests/testDiscreteMarginals.cpp b/gtsam/discrete/tests/testDiscreteMarginals.cpp index 4e9f956b6..e1eb92af3 100644 --- a/gtsam/discrete/tests/testDiscreteMarginals.cpp +++ b/gtsam/discrete/tests/testDiscreteMarginals.cpp @@ -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 allPosbValues = cartesianProduct( - key[0] & key[1] & key[2] & key[3] & key[4]); + vector 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 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(actualM))); + EXPECT(assert_equal( + expectedM, *boost::dynamic_pointer_cast(actualM))); } } diff --git a/gtsam/discrete/tests/testSignature.cpp b/gtsam/discrete/tests/testSignature.cpp index de47a00f3..049c455f7 100644 --- a/gtsam/discrete/tests/testSignature.cpp +++ b/gtsam/discrete/tests/testSignature.cpp @@ -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 #include - #include #include +#include +#include + 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 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 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 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); +} /* ************************************************************************* */ diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index 60088577c..79dbb9ce9 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -107,9 +107,9 @@ public: // If needed, apply chain rule if (Dpose) - *Dpose = Dpi_pn * *Dpose; + *Dpose = Dpi_pn * *Dpose; if (Dpoint) - *Dpoint = Dpi_pn * *Dpoint; + *Dpoint = Dpi_pn * *Dpoint; return pi; } diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h index a08f87783..a6392c2f9 100644 --- a/gtsam/geometry/SOn.h +++ b/gtsam/geometry/SOn.h @@ -292,6 +292,10 @@ class SO : public LieGroup, internal::DimensionSO(N)> { boost::none) const; /// @} + template + friend void save(Archive&, SO&, const unsigned int); + template + friend void load(Archive&, SO&, const unsigned int); template friend void serialize(Archive&, SO&, const unsigned int); friend class boost::serialization::access; @@ -329,6 +333,16 @@ template <> SOn LieGroup::between(const SOn& g, DynamicJacobian H1, DynamicJacobian H2) const; +/** Serialization function */ +template +void serialize( + Archive& ar, SOn& Q, + const unsigned int file_version +) { + Matrix& M = Q.matrix_; + ar& BOOST_SERIALIZATION_NVP(M); +} + /* * Define the traits. internal::LieGroup provides both Lie group and Testable */ diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index b80e6e4d8..27d41a014 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -90,6 +90,8 @@ public: /// Copy assignment Unit3& operator=(const Unit3 & u) { p_ = u.p_; + B_ = u.B_; + H_B_ = u.H_B_; return *this; } diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index a25ab25c7..ddf60a256 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -484,6 +484,15 @@ TEST(Unit3, ErrorBetweenFactor) { } } +TEST(Unit3, CopyAssign) { + Unit3 p{1, 0.2, 0.3}; + + EXPECT(p.error(p).isZero()); + + p = Unit3{-1, 2, 8}; + EXPECT(p.error(p).isZero()); +} + /* ************************************************************************* */ TEST(actualH, Serialization) { Unit3 p(0, 1, 0); diff --git a/gtsam/inference/BayesNet.h b/gtsam/inference/BayesNet.h index a69fb9b8c..0597ece98 100644 --- a/gtsam/inference/BayesNet.h +++ b/gtsam/inference/BayesNet.h @@ -69,4 +69,6 @@ namespace gtsam { void saveGraph(const std::string &s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; }; -} \ No newline at end of file +} + +#include diff --git a/gtsam/inference/BayesTreeCliqueBase-inst.h b/gtsam/inference/BayesTreeCliqueBase-inst.h index e762786f5..a02fe274e 100644 --- a/gtsam/inference/BayesTreeCliqueBase-inst.h +++ b/gtsam/inference/BayesTreeCliqueBase-inst.h @@ -136,57 +136,61 @@ namespace gtsam { } } - /* ************************************************************************* */ + /* *********************************************************************** */ // separator marginal, uses separator marginal of parent recursively // P(C) = P(F|S) P(S) - /* ************************************************************************* */ - template + /* *********************************************************************** */ + template typename BayesTreeCliqueBase::FactorGraphType - BayesTreeCliqueBase::separatorMarginal(Eliminate function) const - { + BayesTreeCliqueBase::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 + /* *********************************************************************** */ + template typename BayesTreeCliqueBase::FactorGraphType - BayesTreeCliqueBase::marginal2(Eliminate function) const - { + BayesTreeCliqueBase::marginal2( + Eliminate function) const { gttic(BayesTreeCliqueBase_marginal2); // initialize with separator marginal P(S) FactorGraphType p_C = this->separatorMarginal(function); diff --git a/gtsam/inference/Conditional.h b/gtsam/inference/Conditional.h index 1d486030c..295122879 100644 --- a/gtsam/inference/Conditional.h +++ b/gtsam/inference/Conditional.h @@ -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 /// @{ diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 09a9a6103..b8a08be9e 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -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); } diff --git a/gtsam/linear/LossFunctions.cpp b/gtsam/linear/LossFunctions.cpp index de2a5142d..bf799a2ba 100644 --- a/gtsam/linear/LossFunctions.cpp +++ b/gtsam/linear/LossFunctions.cpp @@ -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); } diff --git a/gtsam/linear/LossFunctions.h b/gtsam/linear/LossFunctions.h index 1f7cc1377..6a5dc5a26 100644 --- a/gtsam/linear/LossFunctions.h +++ b/gtsam/linear/LossFunctions.h @@ -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| 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 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 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 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 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 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 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); diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index d7fd2d1ea..f5ec95696 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -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 + 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& 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 = - */ - 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& 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& 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 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 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 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 H) const; + Matrix Whiten(const Matrix& H) const override; + void WhitenInPlace(Matrix& H) const override; + void WhitenInPlace(Eigen::Block 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 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 H) const override; /** * Return standard deviation @@ -616,7 +631,7 @@ namespace gtsam { typedef boost::shared_ptr 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 /*H*/) const {} - virtual void whitenInPlace(Vector& /*v*/) const {} - virtual void unwhitenInPlace(Vector& /*v*/) const {} - virtual void whitenInPlace(Eigen::Block& /*v*/) const {} - virtual void unwhitenInPlace(Eigen::Block& /*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 /*H*/) const override {} + void whitenInPlace(Vector& /*v*/) const override {} + void unwhitenInPlace(Vector& /*v*/) const override {} + void whitenInPlace(Eigen::Block& /*v*/) const override {} + void unwhitenInPlace(Eigen::Block& /*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& 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& 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()); } diff --git a/gtsam/linear/PCGSolver.cpp b/gtsam/linear/PCGSolver.cpp index 08307c5ab..a7af7d8d8 100644 --- a/gtsam/linear/PCGSolver.cpp +++ b/gtsam/linear/PCGSolver.cpp @@ -45,6 +45,17 @@ PCGSolver::PCGSolver(const PCGSolverParameters &p) { preconditioner_ = createPreconditioner(p.preconditioner_); } +void PCGSolverParameters::setPreconditionerParams(const boost::shared_ptr 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 &lambda, diff --git a/gtsam/linear/PCGSolver.h b/gtsam/linear/PCGSolver.h index f5b278ae5..7752902ba 100644 --- a/gtsam/linear/PCGSolver.h +++ b/gtsam/linear/PCGSolver.h @@ -48,7 +48,12 @@ public: return *preconditioner_; } + // needed for python wrapper + void print(const std::string &s) const; + boost::shared_ptr preconditioner_; + + void setPreconditionerParams(const boost::shared_ptr preconditioner); }; /** diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 3f6550b9f..42d68a603 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -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));\ diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 149067269..d7b4b7bf1 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -17,9 +17,11 @@ * @author Vadim Indelman * @author David Jensen * @author Frank Dellaert + * @author Varun Agrawal **/ #include +#include /* External or standard includes */ #include @@ -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(&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); + diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 6b3bf979a..a89568433 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -17,6 +17,7 @@ * @author Vadim Indelman * @author David Jensen * @author Frank Dellaert + * @author Varun Agrawal **/ #pragma once @@ -26,6 +27,7 @@ #include #include #include +#include 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(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 - 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 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& 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& 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(this->p_);} + Params& p() const { return *boost::static_pointer_cast(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 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 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 - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & boost::serialization::make_nvp("NoiseModelFactor6", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(_PIM_); + template + 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 + : public Testable {}; + +template <> +struct traits + : public Testable {}; + +template <> +struct traits : public Testable {}; + +} // namespace gtsam + +/// Add Boost serialization export key (declaration) for derived class +BOOST_CLASS_EXPORT_KEY(gtsam::PreintegrationCombinedParams); diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 8e3f8f0a4..7e080ffd5 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -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& 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_); } }; diff --git a/gtsam/navigation/ManifoldPreintegration.h b/gtsam/navigation/ManifoldPreintegration.h index 22897b9d4..a290972e4 100644 --- a/gtsam/navigation/ManifoldPreintegration.h +++ b/gtsam/navigation/ManifoldPreintegration.h @@ -118,15 +118,13 @@ private: template 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_); } }; diff --git a/gtsam/navigation/PreintegratedRotation.cpp b/gtsam/navigation/PreintegratedRotation.cpp index 8c29d85dd..c5d48b734 100644 --- a/gtsam/navigation/PreintegratedRotation.cpp +++ b/gtsam/navigation/PreintegratedRotation.cpp @@ -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)) diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index 12938a625..0e0559a32 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -61,9 +61,15 @@ struct GTSAM_EXPORT PreintegratedRotationParams { template 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 diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index eb30c1f13..29d7814b5 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -213,6 +213,16 @@ class GTSAM_EXPORT PreintegrationBase { /// @} #endif + private: + /** Serialization function */ + friend class boost::serialization::access; + template + 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 }; diff --git a/gtsam/navigation/PreintegrationParams.cpp b/gtsam/navigation/PreintegrationParams.cpp index 61cd1617c..2298bb696 100644 --- a/gtsam/navigation/PreintegrationParams.cpp +++ b/gtsam/navigation/PreintegrationParams.cpp @@ -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(&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) && diff --git a/gtsam/navigation/PreintegrationParams.h b/gtsam/navigation/PreintegrationParams.h index 4bff625ca..9ae66e678 100644 --- a/gtsam/navigation/PreintegrationParams.h +++ b/gtsam/navigation/PreintegrationParams.h @@ -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(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 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { namespace bs = ::boost::serialization; - ar & boost::serialization::make_nvp("PreintegratedRotation_Params", - boost::serialization::base_object(*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); } diff --git a/gtsam/navigation/TangentPreintegration.h b/gtsam/navigation/TangentPreintegration.h index edf76e562..1b51b4e1e 100644 --- a/gtsam/navigation/TangentPreintegration.h +++ b/gtsam/navigation/TangentPreintegration.h @@ -132,12 +132,10 @@ private: template 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: diff --git a/gtsam/navigation/tests/testImuFactorSerialization.cpp b/gtsam/navigation/tests/testImuFactorSerialization.cpp index 59d0ac199..ed72e18e9 100644 --- a/gtsam/navigation/tests/testImuFactorSerialization.cpp +++ b/gtsam/navigation/tests/testImuFactorSerialization.cpp @@ -16,15 +16,19 @@ * @author Frank Dellaert * @author Richard Roberts * @author Stephen Williams + * @author Varun Agrawal */ -#include -#include #include +#include +#include +#include + #include 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 +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(); + + EXPECT(equalsObj(pim)); + EXPECT(equalsXML(pim)); + EXPECT(equalsBinary(pim)); + ImuFactor factor(1, 2, 3, 4, 5, pim); EXPECT(equalsObj(factor)); @@ -69,6 +83,30 @@ TEST(ImuFactor, serialization) { EXPECT(equalsBinary(factor)); } +TEST(ImuFactor2, serialization) { + auto pim = getPreintegratedMeasurements(); + + ImuFactor2 factor(1, 2, 3, pim); + + EXPECT(equalsObj(factor)); + EXPECT(equalsXML(factor)); + EXPECT(equalsBinary(factor)); +} + +TEST(CombinedImuFactor, Serialization) { + auto pim = getPreintegratedMeasurements(); + + EXPECT(equalsObj(pim)); + EXPECT(equalsXML(pim)); + EXPECT(equalsBinary(pim)); + + const CombinedImuFactor factor(1, 2, 3, 4, 5, 6, pim); + + EXPECT(equalsObj(factor)); + EXPECT(equalsXML(factor)); + EXPECT(equalsBinary(factor)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/nonlinear/AdaptAutoDiff.h b/gtsam/nonlinear/AdaptAutoDiff.h index ff059ef78..682cca29a 100644 --- a/gtsam/nonlinear/AdaptAutoDiff.h +++ b/gtsam/nonlinear/AdaptAutoDiff.h @@ -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::Differentiate( f, parameters, M, result.data(), jacobians); diff --git a/gtsam/nonlinear/FunctorizedFactor.h b/gtsam/nonlinear/FunctorizedFactor.h new file mode 100644 index 000000000..a83198967 --- /dev/null +++ b/gtsam/nonlinear/FunctorizedFactor.h @@ -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 +#include + +#include + +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 factor(keyX, measurement, model, + * MultiplyFunctor(multiplier)); + */ +template +class GTSAM_EXPORT FunctorizedFactor : public NoiseModelFactor1 { + private: + using Base = NoiseModelFactor1; + + R measured_; ///< value that is compared with functor return value + SharedNoiseModel noiseModel_; ///< noise model + std::function)> 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)> 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::shared_ptr(new FunctorizedFactor(*this))); + } + + Vector evaluateError(const T ¶ms, + boost::optional H = boost::none) const { + R x = func_(params, H); + Vector error = traits::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::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 *e = + dynamic_cast *>(&other); + const bool base = Base::equals(*e, tol); + return e && Base::equals(other, tol) && + traits::Equals(this->measured_, e->measured_, tol); + } + /// @} + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE &ar, const unsigned int /*version*/) { + ar &boost::serialization::make_nvp( + "NoiseModelFactor1", boost::serialization::base_object(*this)); + ar &BOOST_SERIALIZATION_NVP(measured_); + ar &BOOST_SERIALIZATION_NVP(func_); + } +}; + +/// traits +template +struct traits> + : public Testable> {}; + +/** + * 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 +FunctorizedFactor MakeFunctorizedFactor(Key key, const R &z, + const SharedNoiseModel &model, + const FUNC func) { + return FunctorizedFactor(key, z, model, func); +} + +} // namespace gtsam diff --git a/gtsam/nonlinear/NonlinearFactor.cpp b/gtsam/nonlinear/NonlinearFactor.cpp index ee14e8073..1cfcba274 100644 --- a/gtsam/nonlinear/NonlinearFactor.cpp +++ b/gtsam/nonlinear/NonlinearFactor.cpp @@ -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 { diff --git a/gtsam/nonlinear/internal/ExecutionTrace.h b/gtsam/nonlinear/internal/ExecutionTrace.h index ace0aaea8..2943b5e68 100644 --- a/gtsam/nonlinear/internal/ExecutionTrace.h +++ b/gtsam/nonlinear/internal/ExecutionTrace.h @@ -169,6 +169,12 @@ class ExecutionTrace { content.ptr->reverseAD2(dTdA, jacobians); } + ~ExecutionTrace() { + if (kind == Function) { + content.ptr->~CallRecord(); + } + } + /// Define type so we can apply it as a meta-function typedef ExecutionTrace type; }; diff --git a/gtsam/nonlinear/tests/testExecutionTrace.cpp b/gtsam/nonlinear/tests/testExecutionTrace.cpp index c2b245780..58f76089a 100644 --- a/gtsam/nonlinear/tests/testExecutionTrace.cpp +++ b/gtsam/nonlinear/tests/testExecutionTrace.cpp @@ -16,6 +16,7 @@ * @brief unit tests for Expression internals */ +#include #include #include diff --git a/gtsam/nonlinear/tests/testFunctorizedFactor.cpp b/gtsam/nonlinear/tests/testFunctorizedFactor.cpp new file mode 100644 index 000000000..12dd6b91c --- /dev/null +++ b/gtsam/nonlinear/tests/testFunctorizedFactor.cpp @@ -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 +#include +#include +#include +#include + +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(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(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(key, measurement, model, + MultiplyFunctor(multiplier)); + auto factor2 = MakeFunctorizedFactor(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(key, X, model, MultiplyFunctor(multiplier)); + + Values values; + values.insert(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(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)> functional = + MultiplyFunctor(multiplier); + auto factor = + MakeFunctorizedFactor(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 factor(key, measurement, model, lambda); + auto factor = MakeFunctorizedFactor(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); +} +/* ************************************************************************* */ diff --git a/gtsam/sfm/TranslationFactor.h b/gtsam/sfm/TranslationFactor.h index 9a4bd68a7..d63633d7e 100644 --- a/gtsam/sfm/TranslationFactor.h +++ b/gtsam/sfm/TranslationFactor.h @@ -67,8 +67,7 @@ class TranslationFactor : public NoiseModelFactor2 { boost::optional H2 = boost::none) const override { const Point3 dir = Tb - Ta; Matrix33 H_predicted_dir; - const Point3 predicted = - dir.normalized(H1 || H2 ? &H_predicted_dir : nullptr); + const Point3 predicted = normalize(dir, H1 || H2 ? &H_predicted_dir : nullptr); if (H1) *H1 = -H_predicted_dir; if (H2) *H2 = H_predicted_dir; return predicted - measured_w_aZb_; diff --git a/gtsam/slam/FrobeniusFactor.cpp b/gtsam/slam/FrobeniusFactor.cpp new file mode 100644 index 000000000..904addb03 --- /dev/null +++ b/gtsam/slam/FrobeniusFactor.cpp @@ -0,0 +1,117 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file FrobeniusFactor.cpp + * @date March 2019 + * @author Frank Dellaert + * @brief Various factors that minimize some Frobenius norm + */ + +#include + +#include +#include +#include + +#include +#include +#include + +using namespace std; + +namespace gtsam { + +//****************************************************************************** +boost::shared_ptr ConvertPose3NoiseModel( + const SharedNoiseModel& model, size_t d, bool defaultToUnit) { + double sigma = 1.0; + if (model != nullptr) { + if (model->dim() != 6) { + if (!defaultToUnit) + throw std::runtime_error("Can only convert Pose3 noise models"); + } else { + auto sigmas = model->sigmas().head(3).eval(); + if (sigmas(1) != sigmas(0) || sigmas(2) != sigmas(0)) { + if (!defaultToUnit) + throw std::runtime_error("Can only convert isotropic rotation noise"); + } else { + sigma = sigmas(0); + } + } + } + return noiseModel::Isotropic::Sigma(d, sigma); +} + +//****************************************************************************** +FrobeniusWormholeFactor::FrobeniusWormholeFactor(Key j1, Key j2, const Rot3& R12, + size_t p, + const SharedNoiseModel& model) + : NoiseModelFactor2(ConvertPose3NoiseModel(model, p * 3), j1, j2), + M_(R12.matrix()), // 3*3 in all cases + p_(p), // 4 for SO(4) + pp_(p * p), // 16 for SO(4) + dimension_(SOn::Dimension(p)), // 6 for SO(4) + G_(pp_, dimension_) // 16*6 for SO(4) +{ + // Calculate G matrix of vectorized generators + Matrix Z = Matrix::Zero(p, p); + for (size_t j = 0; j < dimension_; j++) { + const auto X = SOn::Hat(Eigen::VectorXd::Unit(dimension_, j)); + G_.col(j) = Eigen::Map(X.data(), pp_, 1); + } + assert(noiseModel()->dim() == 3 * p_); +} + +//****************************************************************************** +Vector FrobeniusWormholeFactor::evaluateError( + const SOn& Q1, const SOn& Q2, boost::optional H1, + boost::optional H2) const { + gttic(FrobeniusWormholeFactorP_evaluateError); + + const Matrix& M1 = Q1.matrix(); + const Matrix& M2 = Q2.matrix(); + assert(M1.rows() == p_ && M2.rows() == p_); + + const size_t dim = 3 * p_; // Stiefel manifold dimension + Vector fQ2(dim), hQ1(dim); + + // Vectorize and extract only d leftmost columns, i.e. vec(M2*P) + fQ2 << Eigen::Map(M2.data(), dim, 1); + + // Vectorize M1*P*R12 + const Matrix Q1PR12 = M1.leftCols<3>() * M_; + hQ1 << Eigen::Map(Q1PR12.data(), dim, 1); + + // If asked, calculate Jacobian as (M \otimes M1) * G + if (H1) { + const size_t p2 = 2 * p_; + Matrix RPxQ = Matrix::Zero(dim, pp_); + RPxQ.block(0, 0, p_, dim) << M1 * M_(0, 0), M1 * M_(1, 0), M1 * M_(2, 0); + RPxQ.block(p_, 0, p_, dim) << M1 * M_(0, 1), M1 * M_(1, 1), M1 * M_(2, 1); + RPxQ.block(p2, 0, p_, dim) << M1 * M_(0, 2), M1 * M_(1, 2), M1 * M_(2, 2); + *H1 = -RPxQ * G_; + } + if (H2) { + const size_t p2 = 2 * p_; + Matrix PxQ = Matrix::Zero(dim, pp_); + PxQ.block(0, 0, p_, p_) = M2; + PxQ.block(p_, p_, p_, p_) = M2; + PxQ.block(p2, p2, p_, p_) = M2; + *H2 = PxQ * G_; + } + + return fQ2 - hQ1; +} + +//****************************************************************************** + +} // namespace gtsam diff --git a/gtsam/slam/FrobeniusFactor.h b/gtsam/slam/FrobeniusFactor.h new file mode 100644 index 000000000..a73445ae0 --- /dev/null +++ b/gtsam/slam/FrobeniusFactor.h @@ -0,0 +1,145 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file FrobeniusFactor.h + * @date March 2019 + * @author Frank Dellaert + * @brief Various factors that minimize some Frobenius norm + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +/** + * When creating (any) FrobeniusFactor we convert a 6-dimensional Pose3 + * BetweenFactor noise model into an 9 or 16-dimensional isotropic noise + * model used to weight the Frobenius norm. If the noise model passed is + * null we return a Dim-dimensional isotropic noise model with sigma=1.0. If + * not, we we check if the 3-dimensional noise model on rotations is + * isotropic. If it is, we extend to 'Dim' dimensions, otherwise we throw an + * error. If defaultToUnit == false throws an exception on unexepcted input. + */ + GTSAM_EXPORT boost::shared_ptr ConvertPose3NoiseModel( + const SharedNoiseModel& model, size_t d, bool defaultToUnit = true); + +/** + * FrobeniusPrior calculates the Frobenius norm between a given matrix and an + * element of SO(3) or SO(4). + */ +template +class FrobeniusPrior : public NoiseModelFactor1 { + enum { Dim = Rot::VectorN2::RowsAtCompileTime }; + using MatrixNN = typename Rot::MatrixNN; + Eigen::Matrix vecM_; ///< vectorized matrix to approximate + + public: + /// Constructor + FrobeniusPrior(Key j, const MatrixNN& M, + const SharedNoiseModel& model = nullptr) + : NoiseModelFactor1(ConvertPose3NoiseModel(model, Dim), j) { + vecM_ << Eigen::Map(M.data(), Dim, 1); + } + + /// Error is just Frobenius norm between Rot element and vectorized matrix M. + Vector evaluateError(const Rot& R, + boost::optional H = boost::none) const { + return R.vec(H) - vecM_; // Jacobian is computed only when needed. + } +}; + +/** + * FrobeniusFactor calculates the Frobenius norm between rotation matrices. + * The template argument can be any fixed-size SO. + */ +template +class FrobeniusFactor : public NoiseModelFactor2 { + enum { Dim = Rot::VectorN2::RowsAtCompileTime }; + + public: + /// Constructor + FrobeniusFactor(Key j1, Key j2, const SharedNoiseModel& model = nullptr) + : NoiseModelFactor2(ConvertPose3NoiseModel(model, Dim), j1, + j2) {} + + /// Error is just Frobenius norm between rotation matrices. + Vector evaluateError(const Rot& R1, const Rot& R2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const { + Vector error = R2.vec(H2) - R1.vec(H1); + if (H1) *H1 = -*H1; + return error; + } +}; + +/** + * FrobeniusBetweenFactor is a BetweenFactor that evaluates the Frobenius norm + * of the rotation error between measured and predicted (rather than the + * Logmap of the error). This factor is only defined for fixed-dimension types, + * and in fact only SO3 and SO4 really work, as we need SO::AdjointMap. + */ +template +class FrobeniusBetweenFactor : public NoiseModelFactor2 { + Rot R12_; ///< measured rotation between R1 and R2 + Eigen::Matrix + R2hat_H_R1_; ///< fixed derivative of R2hat wrpt R1 + enum { Dim = Rot::VectorN2::RowsAtCompileTime }; + + public: + /// Constructor + FrobeniusBetweenFactor(Key j1, Key j2, const Rot& R12, + const SharedNoiseModel& model = nullptr) + : NoiseModelFactor2( + ConvertPose3NoiseModel(model, Dim), j1, j2), + R12_(R12), + R2hat_H_R1_(R12.inverse().AdjointMap()) {} + + /// Error is Frobenius norm between R1*R12 and R2. + Vector evaluateError(const Rot& R1, const Rot& R2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const { + const Rot R2hat = R1.compose(R12_); + Eigen::Matrix vec_H_R2hat; + Vector error = R2.vec(H2) - R2hat.vec(H1 ? &vec_H_R2hat : nullptr); + if (H1) *H1 = -vec_H_R2hat * R2hat_H_R1_; + return error; + } +}; + +/** + * FrobeniusWormholeFactor is a BetweenFactor that moves in SO(p), but will + * land on the SO(3) sub-manifold of SO(p) at the global minimum. It projects + * the SO(p) matrices down to a Stiefel manifold of p*d matrices. + * TODO(frank): template on D=2 or 3 + */ +class GTSAM_EXPORT FrobeniusWormholeFactor : public NoiseModelFactor2 { + Matrix M_; ///< measured rotation between R1 and R2 + size_t p_, pp_, dimension_; ///< dimensionality constants + Matrix G_; ///< matrix of vectorized generators + + public: + /// Constructor. Note we convert to 3*p-dimensional noise model. + FrobeniusWormholeFactor(Key j1, Key j2, const Rot3& R12, size_t p = 4, + const SharedNoiseModel& model = nullptr); + + /// Error is Frobenius norm between Q1*P*R12 and Q2*P, where P=[I_3x3;0] + /// projects down from SO(p) to the Stiefel manifold of px3 matrices. + Vector evaluateError(const SOn& Q1, const SOn& Q2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const; +}; + +} // namespace gtsam diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 717a9c1f2..1c80b8744 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -207,10 +207,18 @@ protected: Vector ue = cameras.reprojectionError(point, measured_, Fs, E); if (body_P_sensor_ && Fs) { const Pose3 sensor_P_body = body_P_sensor_->inverse(); + constexpr int camera_dim = traits::dimension; + constexpr int pose_dim = traits::dimension; + for (size_t i = 0; i < Fs->size(); i++) { - const Pose3 w_Pose_body = cameras[i].pose() * sensor_P_body; - Matrix J(6, 6); - const Pose3 world_P_body = w_Pose_body.compose(*body_P_sensor_, J); + const Pose3 world_P_body = cameras[i].pose() * sensor_P_body; + Eigen::Matrix J; + J.setZero(); + Eigen::Matrix H; + // Call compose to compute Jacobian for camera extrinsics + world_P_body.compose(*body_P_sensor_, H); + // Assign extrinsics part of the Jacobian + J.template block(0, 0) = H; Fs->at(i) = Fs->at(i) * J; } } diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 66e8fc4c0..669935994 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -442,11 +442,11 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, auto p = dynamic_cast*>(&key_value.value); if (!p) continue; const Pose3& pose = p->value(); - Point3 t = pose.translation(); - Rot3 R = pose.rotation(); - stream << "VERTEX_SE3:QUAT " << key_value.key << " " << t.x() << " " << t.y() << " " << t.z() - << " " << R.toQuaternion().x() << " " << R.toQuaternion().y() << " " << R.toQuaternion().z() - << " " << R.toQuaternion().w() << endl; + const Point3 t = pose.translation(); + const auto q = pose.rotation().toQuaternion(); + stream << "VERTEX_SE3:QUAT " << key_value.key << " " << t.x() << " " + << t.y() << " " << t.z() << " " << q.x() << " " << q.y() << " " + << q.z() << " " << q.w() << endl; } // save edges (2D or 3D) @@ -486,13 +486,12 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, throw invalid_argument("writeG2o: invalid noise model!"); } Matrix Info = gaussianModel->R().transpose() * gaussianModel->R(); - Pose3 pose3D = factor3D->measured(); - Point3 p = pose3D.translation(); - Rot3 R = pose3D.rotation(); - - stream << "EDGE_SE3:QUAT " << factor3D->key1() << " " << factor3D->key2() << " " - << p.x() << " " << p.y() << " " << p.z() << " " << R.toQuaternion().x() - << " " << R.toQuaternion().y() << " " << R.toQuaternion().z() << " " << R.toQuaternion().w(); + const Pose3 pose3D = factor3D->measured(); + const Point3 p = pose3D.translation(); + const auto q = pose3D.rotation().toQuaternion(); + stream << "EDGE_SE3:QUAT " << factor3D->key1() << " " << factor3D->key2() + << " " << p.x() << " " << p.y() << " " << p.z() << " " << q.x() + << " " << q.y() << " " << q.z() << " " << q.w(); Matrix InfoG2o = I_6x6; InfoG2o.block(0,0,3,3) = Info.block(3,3,3,3); // cov translation @@ -511,6 +510,11 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, stream.close(); } +/* ************************************************************************* */ +static Rot3 NormalizedRot3(double w, double x, double y, double z) { + const double norm = sqrt(w * w + x * x + y * y + z * z), f = 1.0 / norm; + return Rot3::Quaternion(f * w, f * x, f * y, f * z); +} /* ************************************************************************* */ std::map parse3DPoses(const string& filename) { ifstream is(filename.c_str()); @@ -535,14 +539,15 @@ std::map parse3DPoses(const string& filename) { Key id; double x, y, z, qx, qy, qz, qw; ls >> id >> x >> y >> z >> qx >> qy >> qz >> qw; - poses.emplace(id, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z})); + poses.emplace(id, Pose3(NormalizedRot3(qw, qx, qy, qz), {x, y, z})); } } return poses; } /* ************************************************************************* */ -BetweenFactorPose3s parse3DFactors(const string& filename, +BetweenFactorPose3s parse3DFactors( + const string& filename, const noiseModel::Diagonal::shared_ptr& corruptingNoise) { ifstream is(filename.c_str()); if (!is) throw invalid_argument("parse3DFactors: can not find file " + filename); @@ -592,7 +597,7 @@ BetweenFactorPose3s parse3DFactors(const string& filename, mgtsam.block<3, 3>(3, 0) = m.block<3, 3>(3, 0); // off diagonal SharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam); - auto R12 = Rot3::Quaternion(qw, qx, qy, qz); + auto R12 = NormalizedRot3(qw, qx, qy, qz); if (sampler) { R12 = R12.retract(sampler->sample()); } diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 9a3c797b2..8088ab18a 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -122,45 +122,6 @@ TEST( dataSet, Balbianello) EXPECT(assert_equal(expected,actual,1)); } -/* ************************************************************************* */ -TEST( dataSet, readG2o) -{ - const string g2oFile = findExampleDataFile("pose2example"); - NonlinearFactorGraph::shared_ptr actualGraph; - Values::shared_ptr actualValues; - boost::tie(actualGraph, actualValues) = readG2o(g2oFile); - - Values expectedValues; - expectedValues.insert(0, Pose2(0.000000, 0.000000, 0.000000)); - expectedValues.insert(1, Pose2(1.030390, 0.011350, -0.081596)); - expectedValues.insert(2, Pose2(2.036137, -0.129733, -0.301887)); - expectedValues.insert(3, Pose2(3.015097, -0.442395, -0.345514)); - expectedValues.insert(4, Pose2(3.343949, 0.506678, 1.214715)); - expectedValues.insert(5, Pose2(3.684491, 1.464049, 1.183785)); - expectedValues.insert(6, Pose2(4.064626, 2.414783, 1.176333)); - expectedValues.insert(7, Pose2(4.429778, 3.300180, 1.259169)); - expectedValues.insert(8, Pose2(4.128877, 2.321481, -1.825391)); - expectedValues.insert(9, Pose2(3.884653, 1.327509, -1.953016)); - expectedValues.insert(10, Pose2(3.531067, 0.388263, -2.148934)); - EXPECT(assert_equal(expectedValues,*actualValues,1e-5)); - - noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions(Vector3(44.721360, 44.721360, 30.901699)); - NonlinearFactorGraph expectedGraph; - expectedGraph.emplace_shared >(0, 1, Pose2(1.030390, 0.011350, -0.081596), model); - expectedGraph.emplace_shared >(1, 2, Pose2(1.013900, -0.058639, -0.220291), model); - expectedGraph.emplace_shared >(2, 3, Pose2(1.027650, -0.007456, -0.043627), model); - expectedGraph.emplace_shared >(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model); - expectedGraph.emplace_shared >(4, 5, Pose2(1.016030, 0.014565, -0.030930), model); - expectedGraph.emplace_shared >(5, 6, Pose2(1.023890, 0.006808, -0.007452), model); - expectedGraph.emplace_shared >(6, 7, Pose2(0.957734, 0.003159, 0.082836), model); - expectedGraph.emplace_shared >(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model); - expectedGraph.emplace_shared >(8, 9, Pose2(1.023440, 0.013984, -0.127624), model); - expectedGraph.emplace_shared >(9,10, Pose2(1.003350, 0.022250, -0.195918), model); - expectedGraph.emplace_shared >(5, 9, Pose2(0.033943, 0.032439, 3.073637), model); - expectedGraph.emplace_shared >(3,10, Pose2(0.044020, 0.988477, -1.553511), model); - EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5)); -} - /* ************************************************************************* */ TEST(dataSet, readG2o3D) { const string g2oFile = findExampleDataFile("pose3example"); @@ -273,59 +234,103 @@ TEST( dataSet, readG2o3DNonDiagonalNoise) } /* ************************************************************************* */ -TEST( dataSet, readG2oHuber) -{ - const string g2oFile = findExampleDataFile("pose2example"); - NonlinearFactorGraph::shared_ptr actualGraph; - Values::shared_ptr actualValues; - bool is3D = false; - boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D, KernelFunctionTypeHUBER); +TEST(dataSet, readG2oCheckDeterminants) { + const string g2oFile = findExampleDataFile("toyExample.g2o"); - noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions(Vector3(44.721360, 44.721360, 30.901699)); - SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345), baseModel); + // Check determinants in factors + auto factors = parse3DFactors(g2oFile); + EXPECT_LONGS_EQUAL(6, factors.size()); + for (const auto& factor : factors) { + const Rot3 R = factor->measured().rotation(); + EXPECT_DOUBLES_EQUAL(1.0, R.matrix().determinant(), 1e-9); + } - NonlinearFactorGraph expectedGraph; - expectedGraph.emplace_shared >(0, 1, Pose2(1.030390, 0.011350, -0.081596), model); - expectedGraph.emplace_shared >(1, 2, Pose2(1.013900, -0.058639, -0.220291), model); - expectedGraph.emplace_shared >(2, 3, Pose2(1.027650, -0.007456, -0.043627), model); - expectedGraph.emplace_shared >(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model); - expectedGraph.emplace_shared >(4, 5, Pose2(1.016030, 0.014565, -0.030930), model); - expectedGraph.emplace_shared >(5, 6, Pose2(1.023890, 0.006808, -0.007452), model); - expectedGraph.emplace_shared >(6, 7, Pose2(0.957734, 0.003159, 0.082836), model); - expectedGraph.emplace_shared >(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model); - expectedGraph.emplace_shared >(8, 9, Pose2(1.023440, 0.013984, -0.127624), model); - expectedGraph.emplace_shared >(9,10, Pose2(1.003350, 0.022250, -0.195918), model); - expectedGraph.emplace_shared >(5, 9, Pose2(0.033943, 0.032439, 3.073637), model); - expectedGraph.emplace_shared >(3,10, Pose2(0.044020, 0.988477, -1.553511), model); - EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5)); + // Check determinants in initial values + const map poses = parse3DPoses(g2oFile); + EXPECT_LONGS_EQUAL(5, poses.size()); + for (const auto& key_value : poses) { + const Rot3 R = key_value.second.rotation(); + EXPECT_DOUBLES_EQUAL(1.0, R.matrix().determinant(), 1e-9); + } } /* ************************************************************************* */ -TEST( dataSet, readG2oTukey) -{ +static NonlinearFactorGraph expectedGraph(const SharedNoiseModel& model) { + NonlinearFactorGraph g; + using Factor = BetweenFactor; + g.emplace_shared(0, 1, Pose2(1.030390, 0.011350, -0.081596), model); + g.emplace_shared(1, 2, Pose2(1.013900, -0.058639, -0.220291), model); + g.emplace_shared(2, 3, Pose2(1.027650, -0.007456, -0.043627), model); + g.emplace_shared(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model); + g.emplace_shared(4, 5, Pose2(1.016030, 0.014565, -0.030930), model); + g.emplace_shared(5, 6, Pose2(1.023890, 0.006808, -0.007452), model); + g.emplace_shared(6, 7, Pose2(0.957734, 0.003159, 0.082836), model); + g.emplace_shared(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model); + g.emplace_shared(8, 9, Pose2(1.023440, 0.013984, -0.127624), model); + g.emplace_shared(9, 10, Pose2(1.003350, 0.022250, -0.195918), model); + g.emplace_shared(5, 9, Pose2(0.033943, 0.032439, 3.073637), model); + g.emplace_shared(3, 10, Pose2(0.044020, 0.988477, -1.553511), model); + return g; +} + +/* ************************************************************************* */ +TEST(dataSet, readG2o) { + const string g2oFile = findExampleDataFile("pose2example"); + NonlinearFactorGraph::shared_ptr actualGraph; + Values::shared_ptr actualValues; + boost::tie(actualGraph, actualValues) = readG2o(g2oFile); + + auto model = noiseModel::Diagonal::Precisions( + Vector3(44.721360, 44.721360, 30.901699)); + EXPECT(assert_equal(expectedGraph(model), *actualGraph, 1e-5)); + + Values expectedValues; + expectedValues.insert(0, Pose2(0.000000, 0.000000, 0.000000)); + expectedValues.insert(1, Pose2(1.030390, 0.011350, -0.081596)); + expectedValues.insert(2, Pose2(2.036137, -0.129733, -0.301887)); + expectedValues.insert(3, Pose2(3.015097, -0.442395, -0.345514)); + expectedValues.insert(4, Pose2(3.343949, 0.506678, 1.214715)); + expectedValues.insert(5, Pose2(3.684491, 1.464049, 1.183785)); + expectedValues.insert(6, Pose2(4.064626, 2.414783, 1.176333)); + expectedValues.insert(7, Pose2(4.429778, 3.300180, 1.259169)); + expectedValues.insert(8, Pose2(4.128877, 2.321481, -1.825391)); + expectedValues.insert(9, Pose2(3.884653, 1.327509, -1.953016)); + expectedValues.insert(10, Pose2(3.531067, 0.388263, -2.148934)); + EXPECT(assert_equal(expectedValues, *actualValues, 1e-5)); +} + +/* ************************************************************************* */ +TEST(dataSet, readG2oHuber) { const string g2oFile = findExampleDataFile("pose2example"); NonlinearFactorGraph::shared_ptr actualGraph; Values::shared_ptr actualValues; bool is3D = false; - boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D, KernelFunctionTypeTUKEY); + boost::tie(actualGraph, actualValues) = + readG2o(g2oFile, is3D, KernelFunctionTypeHUBER); - noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions(Vector3(44.721360, 44.721360, 30.901699)); - SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Tukey::Create(4.6851), baseModel); + auto baseModel = noiseModel::Diagonal::Precisions( + Vector3(44.721360, 44.721360, 30.901699)); + auto model = noiseModel::Robust::Create( + noiseModel::mEstimator::Huber::Create(1.345), baseModel); - NonlinearFactorGraph expectedGraph; - expectedGraph.emplace_shared >(0, 1, Pose2(1.030390, 0.011350, -0.081596), model); - expectedGraph.emplace_shared >(1, 2, Pose2(1.013900, -0.058639, -0.220291), model); - expectedGraph.emplace_shared >(2, 3, Pose2(1.027650, -0.007456, -0.043627), model); - expectedGraph.emplace_shared >(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model); - expectedGraph.emplace_shared >(4, 5, Pose2(1.016030, 0.014565, -0.030930), model); - expectedGraph.emplace_shared >(5, 6, Pose2(1.023890, 0.006808, -0.007452), model); - expectedGraph.emplace_shared >(6, 7, Pose2(0.957734, 0.003159, 0.082836), model); - expectedGraph.emplace_shared >(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model); - expectedGraph.emplace_shared >(8, 9, Pose2(1.023440, 0.013984, -0.127624), model); - expectedGraph.emplace_shared >(9,10, Pose2(1.003350, 0.022250, -0.195918), model); - expectedGraph.emplace_shared >(5, 9, Pose2(0.033943, 0.032439, 3.073637), model); - expectedGraph.emplace_shared >(3,10, Pose2(0.044020, 0.988477, -1.553511), model); - EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5)); + EXPECT(assert_equal(expectedGraph(model), *actualGraph, 1e-5)); +} + +/* ************************************************************************* */ +TEST(dataSet, readG2oTukey) { + const string g2oFile = findExampleDataFile("pose2example"); + NonlinearFactorGraph::shared_ptr actualGraph; + Values::shared_ptr actualValues; + bool is3D = false; + boost::tie(actualGraph, actualValues) = + readG2o(g2oFile, is3D, KernelFunctionTypeTUKEY); + + auto baseModel = noiseModel::Diagonal::Precisions( + Vector3(44.721360, 44.721360, 30.901699)); + auto model = noiseModel::Robust::Create( + noiseModel::mEstimator::Tukey::Create(4.6851), baseModel); + + EXPECT(assert_equal(expectedGraph(model), *actualGraph, 1e-5)); } /* ************************************************************************* */ @@ -495,7 +500,7 @@ TEST( dataSet, writeBALfromValues_Dubrovnik){ SfmData readData; readBAL(filenameToRead, readData); - Pose3 poseChange = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.3,0.1,0.3)); + Pose3 poseChange = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.3,0.1,0.3)); Values value; for(size_t i=0; i < readData.number_cameras(); i++){ // for each camera diff --git a/gtsam/slam/tests/testFrobeniusFactor.cpp b/gtsam/slam/tests/testFrobeniusFactor.cpp new file mode 100644 index 000000000..9cb0c19fa --- /dev/null +++ b/gtsam/slam/tests/testFrobeniusFactor.cpp @@ -0,0 +1,244 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * testFrobeniusFactor.cpp + * + * @file testFrobeniusFactor.cpp + * @date March 2019 + * @author Frank Dellaert + * @brief Check evaluateError for various Frobenius norm + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +//****************************************************************************** +namespace so3 { +SO3 id; +Vector3 v1 = (Vector(3) << 0.1, 0, 0).finished(); +SO3 R1 = SO3::Expmap(v1); +Vector3 v2 = (Vector(3) << 0.01, 0.02, 0.03).finished(); +SO3 R2 = SO3::Expmap(v2); +SO3 R12 = R1.between(R2); +} // namespace so3 + +/* ************************************************************************* */ +TEST(FrobeniusPriorSO3, evaluateError) { + using namespace ::so3; + auto factor = FrobeniusPrior(1, R2.matrix()); + Vector actual = factor.evaluateError(R1); + Vector expected = R1.vec() - R2.vec(); + EXPECT(assert_equal(expected, actual, 1e-9)); + + Values values; + values.insert(1, R1); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +} + +/* ************************************************************************* */ +TEST(FrobeniusPriorSO3, ClosestTo) { + // Example top-left of SO(4) matrix not quite on SO(3) manifold + Matrix3 M; + M << 0.79067393, 0.6051136, -0.0930814, // + 0.4155925, -0.64214347, -0.64324489, // + -0.44948549, 0.47046326, -0.75917576; + + SO3 expected = SO3::ClosestTo(M); + + // manifold optimization gets same result as SVD solution in ClosestTo + NonlinearFactorGraph graph; + graph.emplace_shared >(1, M); + + Values initial; + initial.insert(1, SO3(I_3x3)); + auto result = GaussNewtonOptimizer(graph, initial).optimize(); + EXPECT_DOUBLES_EQUAL(0.0, graph.error(result), 1e-6); + EXPECT(assert_equal(expected, result.at(1), 1e-6)); +} + +/* ************************************************************************* */ +TEST(FrobeniusPriorSO3, ChordalL2mean) { + // See Hartley13ijcv: + // Cost function C(R) = \sum FrobeniusPrior(R,R_i) + // Closed form solution = ClosestTo(C_e), where + // C_e = \sum R_i !!!! + + // We will test by computing mean of R1=exp(v1) R1^T=exp(-v1): + using namespace ::so3; + SO3 expected; // identity + Matrix3 M = R1.matrix() + R1.matrix().transpose(); + EXPECT(assert_equal(expected, SO3::ClosestTo(M), 1e-6)); + EXPECT(assert_equal(expected, SO3::ChordalMean({R1, R1.inverse()}), 1e-6)); + + // manifold optimization gets same result as ChordalMean + NonlinearFactorGraph graph; + graph.emplace_shared >(1, R1.matrix()); + graph.emplace_shared >(1, R1.matrix().transpose()); + + Values initial; + initial.insert(1, R1.inverse()); + auto result = GaussNewtonOptimizer(graph, initial).optimize(); + EXPECT_DOUBLES_EQUAL(0.0, graph.error(result), 0.1); // Why so loose? + EXPECT(assert_equal(expected, result.at(1), 1e-5)); +} + +/* ************************************************************************* */ +TEST(FrobeniusFactorSO3, evaluateError) { + using namespace ::so3; + auto factor = FrobeniusFactor(1, 2); + Vector actual = factor.evaluateError(R1, R2); + Vector expected = R2.vec() - R1.vec(); + EXPECT(assert_equal(expected, actual, 1e-9)); + + Values values; + values.insert(1, R1); + values.insert(2, R2); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +} + +/* ************************************************************************* */ +// Commented out as SO(n) not yet supported (and might never be) +// TEST(FrobeniusBetweenFactorSOn, evaluateError) { +// using namespace ::so3; +// auto factor = +// FrobeniusBetweenFactor(1, 2, SOn::FromMatrix(R12.matrix())); +// Vector actual = factor.evaluateError(SOn::FromMatrix(R1.matrix()), +// SOn::FromMatrix(R2.matrix())); +// Vector expected = Vector9::Zero(); +// EXPECT(assert_equal(expected, actual, 1e-9)); + +// Values values; +// values.insert(1, R1); +// values.insert(2, R2); +// EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +// } + +/* ************************************************************************* */ +TEST(FrobeniusBetweenFactorSO3, evaluateError) { + using namespace ::so3; + auto factor = FrobeniusBetweenFactor(1, 2, R12); + Vector actual = factor.evaluateError(R1, R2); + Vector expected = Vector9::Zero(); + EXPECT(assert_equal(expected, actual, 1e-9)); + + Values values; + values.insert(1, R1); + values.insert(2, R2); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +} + +//****************************************************************************** +namespace so4 { +SO4 id; +Vector6 v1 = (Vector(6) << 0.1, 0, 0, 0, 0, 0).finished(); +SO4 Q1 = SO4::Expmap(v1); +Vector6 v2 = (Vector(6) << 0.01, 0.02, 0.03, 0.04, 0.05, 0.06).finished(); +SO4 Q2 = SO4::Expmap(v2); +} // namespace so4 + +/* ************************************************************************* */ +TEST(FrobeniusFactorSO4, evaluateError) { + using namespace ::so4; + auto factor = FrobeniusFactor(1, 2, noiseModel::Unit::Create(6)); + Vector actual = factor.evaluateError(Q1, Q2); + Vector expected = Q2.vec() - Q1.vec(); + EXPECT(assert_equal(expected, actual, 1e-9)); + + Values values; + values.insert(1, Q1); + values.insert(2, Q2); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +} + +/* ************************************************************************* */ +TEST(FrobeniusBetweenFactorSO4, evaluateError) { + using namespace ::so4; + Matrix4 M{I_4x4}; + M.topLeftCorner<3, 3>() = ::so3::R12.matrix(); + auto factor = FrobeniusBetweenFactor(1, 2, Q1.between(Q2)); + Matrix H1, H2; + Vector actual = factor.evaluateError(Q1, Q2, H1, H2); + Vector expected = SO4::VectorN2::Zero(); + EXPECT(assert_equal(expected, actual, 1e-9)); + + Values values; + values.insert(1, Q1); + values.insert(2, Q2); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +} + +//****************************************************************************** +namespace submanifold { +SO4 id; +Vector6 v1 = (Vector(6) << 0, 0, 0, 0.1, 0, 0).finished(); +SO3 R1 = SO3::Expmap(v1.tail<3>()); +SO4 Q1 = SO4::Expmap(v1); +Vector6 v2 = (Vector(6) << 0, 0, 0, 0.01, 0.02, 0.03).finished(); +SO3 R2 = SO3::Expmap(v2.tail<3>()); +SO4 Q2 = SO4::Expmap(v2); +SO3 R12 = R1.between(R2); +} // namespace submanifold + +/* ************************************************************************* */ +TEST(FrobeniusWormholeFactor, evaluateError) { + auto model = noiseModel::Isotropic::Sigma(6, 1.2); // dimension = 6 not 16 + for (const size_t p : {5, 4, 3}) { + Matrix M = Matrix::Identity(p, p); + M.topLeftCorner(3, 3) = submanifold::R1.matrix(); + SOn Q1(M); + M.topLeftCorner(3, 3) = submanifold::R2.matrix(); + SOn Q2(M); + auto factor = + FrobeniusWormholeFactor(1, 2, Rot3(::so3::R12.matrix()), p, model); + Matrix H1, H2; + factor.evaluateError(Q1, Q2, H1, H2); + + // Test derivatives + Values values; + values.insert(1, Q1); + values.insert(2, Q2); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); + } +} + +/* ************************************************************************* */ +TEST(FrobeniusWormholeFactor, equivalenceToSO3) { + using namespace ::submanifold; + auto R12 = ::so3::R12.retract(Vector3(0.1, 0.2, -0.1)); + auto model = noiseModel::Isotropic::Sigma(6, 1.2); // wrong dimension + auto factor3 = FrobeniusBetweenFactor(1, 2, R12, model); + auto factor4 = FrobeniusWormholeFactor(1, 2, Rot3(R12.matrix()), 4, model); + const Matrix3 E3(factor3.evaluateError(R1, R2).data()); + const Matrix43 E4( + factor4.evaluateError(SOn(Q1.matrix()), SOn(Q2.matrix())).data()); + EXPECT(assert_equal((Matrix)E4.topLeftCorner<3, 3>(), E3, 1e-9)); + EXPECT(assert_equal((Matrix)E4.row(3), Matrix13::Zero(), 1e-9)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/slam/tests/testSmartFactorBase.cpp b/gtsam/slam/tests/testSmartFactorBase.cpp index f69f4c113..fd771f102 100644 --- a/gtsam/slam/tests/testSmartFactorBase.cpp +++ b/gtsam/slam/tests/testSmartFactorBase.cpp @@ -37,11 +37,11 @@ class PinholeFactor: public SmartFactorBase > { public: typedef SmartFactorBase > Base; PinholeFactor() {} - PinholeFactor(const SharedNoiseModel& sharedNoiseModel): Base(sharedNoiseModel) { - } - virtual double error(const Values& values) const { - return 0.0; - } + PinholeFactor(const SharedNoiseModel& sharedNoiseModel, + boost::optional body_P_sensor = boost::none, + size_t expectedNumberCameras = 10) + : Base(sharedNoiseModel, body_P_sensor, expectedNumberCameras) {} + virtual double error(const Values& values) const { return 0.0; } virtual boost::shared_ptr linearize( const Values& values) const { return boost::shared_ptr(new JacobianFactor()); @@ -60,6 +60,40 @@ TEST(SmartFactorBase, Pinhole) { EXPECT_LONGS_EQUAL(2 * 2, f.dim()); } +TEST(SmartFactorBase, PinholeWithSensor) { + Pose3 body_P_sensor(Rot3(), Point3(1, 0, 0)); + PinholeFactor f = PinholeFactor(unit2, body_P_sensor); + EXPECT(assert_equal(f.body_P_sensor(), body_P_sensor)); + + PinholeFactor::Cameras cameras; + // Assume body at origin. + Pose3 world_P_body = Pose3(); + // Camera coordinates in world frame. + Pose3 wTc = world_P_body * body_P_sensor; + cameras.push_back(PinholeCamera(wTc)); + + // Simple point to project slightly off image center + Point3 p(0, 0, 10); + Point2 measurement = cameras[0].project(p); + f.add(measurement, 1); + + PinholeFactor::Cameras::FBlocks Fs; + Matrix E; + Vector error = f.unwhitenedError(cameras, p, Fs, E); + + Vector expectedError = Vector::Zero(2); + Matrix29 expectedFs; + expectedFs << -0.001, -1.00001, 0, -0.1, 0, -0.01, 0, 0, 0, 1, 0, 0, 0, -0.1, 0, 0, 0, 0; + Matrix23 expectedE; + expectedE << 0.1, 0, 0.01, 0, 0.1, 0; + + EXPECT(assert_equal(error, expectedError)); + // We only have the jacobian for the 1 camera + // Use of a lower tolerance value due to compiler precision mismatch. + EXPECT(assert_equal(expectedFs, Fs[0], 1e-3)); + EXPECT(assert_equal(expectedE, E)); +} + /* ************************************************************************* */ #include diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index 5d4c3f844..a549dc726 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -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)); diff --git a/timing/timeFrobeniusFactor.cpp b/timing/timeFrobeniusFactor.cpp new file mode 100644 index 000000000..c8bdd8fec --- /dev/null +++ b/timing/timeFrobeniusFactor.cpp @@ -0,0 +1,110 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file timeFrobeniusFactor.cpp + * @brief time FrobeniusFactor with BAL file + * @author Frank Dellaert + * @date June 6, 2015 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +static SharedNoiseModel gNoiseModel = noiseModel::Unit::Create(2); + +int main(int argc, char* argv[]) { + // primitive argument parsing: + if (argc > 3) { + throw runtime_error("Usage: timeFrobeniusFactor [g2oFile]"); + } + + string g2oFile; + try { + if (argc > 1) + g2oFile = argv[argc - 1]; + else + g2oFile = + "/Users/dellaert/git/2019c-notes-shonanrotationaveraging/matlabCode/" + "datasets/randomTorus3D.g2o"; + // g2oFile = findExampleDataFile("sphere_smallnoise.graph"); + } catch (const exception& e) { + cerr << e.what() << '\n'; + exit(1); + } + + // Read G2O file + const auto factors = parse3DFactors(g2oFile); + const auto poses = parse3DPoses(g2oFile); + + // Build graph + NonlinearFactorGraph graph; + // graph.add(NonlinearEquality(0, SO4())); + auto priorModel = noiseModel::Isotropic::Sigma(6, 10000); + graph.add(PriorFactor(0, SO4(), priorModel)); + for (const auto& factor : factors) { + const auto& keys = factor->keys(); + const auto& Tij = factor->measured(); + const auto& model = factor->noiseModel(); + graph.emplace_shared( + keys[0], keys[1], SO3(Tij.rotation().matrix()), model); + } + + boost::mt19937 rng(42); + + // Set parameters to be similar to ceres + LevenbergMarquardtParams params; + LevenbergMarquardtParams::SetCeresDefaults(¶ms); + params.setLinearSolverType("MULTIFRONTAL_QR"); + // params.setVerbosityLM("SUMMARY"); + // params.linearSolverType = LevenbergMarquardtParams::Iterative; + // auto pcg = boost::make_shared(); + // pcg->preconditioner_ = + // boost::make_shared(); + // boost::make_shared(); + // params.iterativeParams = pcg; + + // Optimize + for (size_t i = 0; i < 100; i++) { + gttic_(optimize); + Values initial; + initial.insert(0, SO4()); + for (size_t j = 1; j < poses.size(); j++) { + initial.insert(j, SO4::Random(rng)); + } + LevenbergMarquardtOptimizer lm(graph, initial, params); + Values result = lm.optimize(); + cout << "cost = " << graph.error(result) << endl; + } + + tictoc_finishedIteration_(); + tictoc_print_(); + + return 0; +}