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/.travis.sh b/.travis.sh index 9fc09a3f8..535a72f4b 100755 --- a/.travis.sh +++ b/.travis.sh @@ -63,7 +63,7 @@ function configure() -DGTSAM_BUILD_EXAMPLES_ALWAYS=${GTSAM_BUILD_EXAMPLES_ALWAYS:-ON} \ -DGTSAM_ALLOW_DEPRECATED_SINCE_V4=${GTSAM_ALLOW_DEPRECATED_SINCE_V4:-OFF} \ -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \ - -DCMAKE_VERBOSE_MAKEFILE=ON + -DCMAKE_VERBOSE_MAKEFILE=OFF } diff --git a/.travis.yml b/.travis.yml index ca6a426ea..d8094ef4d 100644 --- a/.travis.yml +++ b/.travis.yml @@ -14,7 +14,8 @@ addons: - clang-9 - build-essential pkg-config - cmake - - libpython-dev python-numpy + - python3-dev libpython-dev + - python3-numpy - libboost-all-dev # before_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/GTSAM-Concepts.md b/GTSAM-Concepts.md index a6cfee984..953357ede 100644 --- a/GTSAM-Concepts.md +++ b/GTSAM-Concepts.md @@ -72,9 +72,9 @@ A Lie group is both a manifold *and* a group. Hence, a LIE_GROUP type should imp However, we now also need to be able to evaluate the derivatives of compose and inverse. Hence, we have the following extra valid static functions defined in the struct `gtsam::traits`: -* `r = traits::Compose(p,q,Hq,Hp)` +* `r = traits::Compose(p,q,Hp,Hq)` * `q = traits::Inverse(p,Hp)` -* `r = traits::Between(p,q,Hq,H2p)` +* `r = traits::Between(p,q,Hp,Hq)` where above the *H* arguments stand for optional Jacobian arguments. That makes it possible to create factors implementing priors (PriorFactor) or relations between two instances of a Lie group type (BetweenFactor). 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/requirements.txt b/cython/requirements.txt index cd77b097d..8d3c7aeb4 100644 --- a/cython/requirements.txt +++ b/cython/requirements.txt @@ -1,3 +1,3 @@ Cython>=0.25.2 backports_abc>=0.5 -numpy>=1.12.0 +numpy>=1.11.0 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/debian/README.md b/debian/README.md deleted file mode 100644 index 74eb351cd..000000000 --- a/debian/README.md +++ /dev/null @@ -1,12 +0,0 @@ -# How to build a GTSAM debian package - -To use the ``debuild`` command, install the ``devscripts`` package - - sudo apt install devscripts - -Change into the gtsam directory, then run: - - debuild -us -uc -j4 - -Adjust the ``-j4`` depending on how many CPUs you want to build on in -parallel. diff --git a/debian/changelog b/debian/changelog deleted file mode 100644 index ef5d5ab97..000000000 --- a/debian/changelog +++ /dev/null @@ -1,5 +0,0 @@ -gtsam (4.0.0-1berndpfrommer) bionic; urgency=medium - - * initial release - - -- Bernd Pfrommer Wed, 18 Jul 2018 20:36:44 -0400 diff --git a/debian/compat b/debian/compat deleted file mode 100644 index ec635144f..000000000 --- a/debian/compat +++ /dev/null @@ -1 +0,0 @@ -9 diff --git a/debian/control b/debian/control deleted file mode 100644 index 9b3ae5308..000000000 --- a/debian/control +++ /dev/null @@ -1,15 +0,0 @@ -Source: gtsam -Section: libs -Priority: optional -Maintainer: Frank Dellaert -Uploaders: Jose Luis Blanco Claraco , Bernd Pfrommer -Build-Depends: cmake, libboost-all-dev (>= 1.58), libeigen3-dev, libtbb-dev, debhelper (>=9) -Standards-Version: 3.9.7 -Homepage: https://github.com/borglab/gtsam -Vcs-Browser: https://github.com/borglab/gtsam - -Package: libgtsam-dev -Architecture: any -Depends: ${shlibs:Depends}, ${misc:Depends}, libboost-serialization-dev, libboost-system-dev, libboost-filesystem-dev, libboost-thread-dev, libboost-program-options-dev, libboost-date-time-dev, libboost-timer-dev, libboost-chrono-dev, libboost-regex-dev -Description: Georgia Tech Smoothing and Mapping Library - gtsam: Georgia Tech Smoothing and Mapping library for SLAM type applications diff --git a/debian/copyright b/debian/copyright deleted file mode 100644 index c2f41d83d..000000000 --- a/debian/copyright +++ /dev/null @@ -1,15 +0,0 @@ -Format: https://www.debian.org/doc/packaging-manuals/copyright-format/1.0/ -Upstream-Name: gtsam -Source: https://bitbucket.org/gtborg/gtsam.git - -Files: * -Copyright: 2017, Frank Dellaert -License: BSD - -Files: gtsam/3rdparty/CCOLAMD/* -Copyright: 2005-2011, Univ. of Florida. Authors: Timothy A. Davis, Sivasankaran Rajamanickam, and Stefan Larimore. Closely based on COLAMD by Davis, Stefan Larimore, in collaboration with Esmond Ng, and John Gilbert. http://www.cise.ufl.edu/research/sparse -License: GNU LESSER GENERAL PUBLIC LICENSE - -Files: gtsam/3rdparty/Eigen/* -Copyright: 2017, Multiple Authors -License: MPL2 diff --git a/debian/gtsam-docs.docs b/debian/gtsam-docs.docs deleted file mode 100644 index e69de29bb..000000000 diff --git a/debian/rules b/debian/rules deleted file mode 100755 index fab798f6e..000000000 --- a/debian/rules +++ /dev/null @@ -1,29 +0,0 @@ -#!/usr/bin/make -f -# See debhelper(7) (uncomment to enable) -# output every command that modifies files on the build system. -export DH_VERBOSE = 1 - -# Makefile target name for running unit tests: -GTSAM_TEST_TARGET = check - -# see FEATURE AREAS in dpkg-buildflags(1) -#export DEB_BUILD_MAINT_OPTIONS = hardening=+all - -# see ENVIRONMENT in dpkg-buildflags(1) -# package maintainers to append CFLAGS -#export DEB_CFLAGS_MAINT_APPEND = -Wall -pedantic -# package maintainers to append LDFLAGS -#export DEB_LDFLAGS_MAINT_APPEND = -Wl,--as-needed - -%: - dh $@ --parallel - -# dh_make generated override targets -# This is example for Cmake (See https://bugs.debian.org/641051 ) -override_dh_auto_configure: - dh_auto_configure -- -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCMAKE_INSTALL_PREFIX=/usr -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DGTSAM_BUILD_TESTS=ON -DGTSAM_BUILD_WRAP=OFF -DGTSAM_BUILD_DOCS=OFF -DGTSAM_INSTALL_CPPUNITLITE=OFF -DGTSAM_INSTALL_GEOGRAPHICLIB=OFF -DGTSAM_BUILD_TYPE_POSTFIXES=OFF -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF - -override_dh_auto_test-arch: - # Tests for arch-dependent : - echo "[override_dh_auto_test-arch]" - dh_auto_build -O--buildsystem=cmake -- $(GTSAM_TEST_TARGET) diff --git a/debian/source/format b/debian/source/format deleted file mode 100644 index 163aaf8d8..000000000 --- a/debian/source/format +++ /dev/null @@ -1 +0,0 @@ -3.0 (quilt) diff --git a/doc/gtsam-coordinate-frames.lyx b/doc/gtsam-coordinate-frames.lyx index 33d0dd977..cfb44696b 100644 --- a/doc/gtsam-coordinate-frames.lyx +++ b/doc/gtsam-coordinate-frames.lyx @@ -2291,15 +2291,11 @@ uncalibration used in the residual). \end_layout -\begin_layout Standard -\begin_inset Note Note -status collapsed - \begin_layout Section Noise models of prior factors \end_layout -\begin_layout Plain Layout +\begin_layout Standard The simplest way to describe noise models is by an example. Let's take a prior factor on a 3D pose \begin_inset Formula $x\in\SE 3$ @@ -2353,7 +2349,7 @@ e\left(x\right)=\norm{h\left(x\right)}_{\Sigma}^{2}=h\left(x\right)^{\t}\Sigma^{ useful answer out quickly ] \end_layout -\begin_layout Plain Layout +\begin_layout Standard The density induced by a noise model on the prior factor is Gaussian in the tangent space about the linearization point. Suppose that the pose is linearized at @@ -2431,7 +2427,7 @@ Here we see that the update . \end_layout -\begin_layout Plain Layout +\begin_layout Standard This means that to draw random pose samples, we actually draw random samples of \begin_inset Formula $\delta x$ @@ -2456,7 +2452,7 @@ This means that to draw random pose samples, we actually draw random samples Noise models of between factors \end_layout -\begin_layout Plain Layout +\begin_layout Standard The noise model of a BetweenFactor is a bit more complicated. The unwhitened error is \begin_inset Formula @@ -2516,11 +2512,6 @@ e\left(\delta x_{1}\right) & \approx\norm{\log\left(z^{-1}\left(x_{1}\exp\delta \end_inset -\end_layout - -\end_inset - - \end_layout \end_body diff --git a/doc/gtsam-coordinate-frames.pdf b/doc/gtsam-coordinate-frames.pdf index 3613ef0ac..77910b4cf 100644 Binary files a/doc/gtsam-coordinate-frames.pdf and b/doc/gtsam-coordinate-frames.pdf differ 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 614db91c7..2cd30be42 100644 --- a/gtsam.h +++ b/gtsam.h @@ -281,7 +281,7 @@ virtual class Value { }; #include -template +template virtual class GenericValue : gtsam::Value { void serializable() const; }; @@ -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 //************************************************************************* @@ -2955,6 +3000,7 @@ class PreintegratedImuMeasurements { gtsam::Rot3 deltaRij() const; Vector deltaPij() const; Vector deltaVij() const; + gtsam::imuBias::ConstantBias biasHat() const; Vector biasHatVector() const; gtsam::NavState predict(const gtsam::NavState& state_i, const gtsam::imuBias::ConstantBias& bias) const; @@ -3016,6 +3062,7 @@ class PreintegratedCombinedMeasurements { gtsam::Rot3 deltaRij() const; Vector deltaPij() const; Vector deltaVij() const; + gtsam::imuBias::ConstantBias biasHat() const; Vector biasHatVector() const; gtsam::NavState predict(const gtsam::NavState& state_i, const gtsam::imuBias::ConstantBias& bias) const; diff --git a/gtsam/3rdparty/CMakeLists.txt b/gtsam/3rdparty/CMakeLists.txt index 89149d964..c8fecc339 100644 --- a/gtsam/3rdparty/CMakeLists.txt +++ b/gtsam/3rdparty/CMakeLists.txt @@ -17,7 +17,7 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN) endforeach(eigen_dir) if(GTSAM_WITH_EIGEN_UNSUPPORTED) - message("-- Installing Eigen Unsupported modules") + message(STATUS "Installing Eigen Unsupported modules") # do the same for the unsupported eigen folder file(GLOB_RECURSE unsupported_eigen_headers "${CMAKE_CURRENT_SOURCE_DIR}/Eigen/unsupported/Eigen/*.h") 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/GenericValue.h b/gtsam/base/GenericValue.h index e1cb3bc2c..2ac3eb80c 100644 --- a/gtsam/base/GenericValue.h +++ b/gtsam/base/GenericValue.h @@ -181,7 +181,7 @@ public: // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html enum { NeedsToAlign = (sizeof(T) % 16) == 0 }; public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) + GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) }; /// use this macro instead of BOOST_CLASS_EXPORT for GenericValues diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index f3653f099..9feb2b451 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -214,7 +214,7 @@ public: enum { NeedsToAlign = (sizeof(M1) % 16) == 0 || (sizeof(M2) % 16) == 0 }; public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) + GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) }; // Define any direct product group to be a model of the multiplicative Group concept diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 776badcd1..21017b575 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -552,17 +552,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); @@ -570,8 +600,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/SymmetricBlockMatrix.h b/gtsam/base/SymmetricBlockMatrix.h index 1ec9a5ad3..4e030606d 100644 --- a/gtsam/base/SymmetricBlockMatrix.h +++ b/gtsam/base/SymmetricBlockMatrix.h @@ -76,7 +76,7 @@ namespace gtsam { blockStart_(0) { fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension); - matrix_.setZero(variableColOffsets_.back(), variableColOffsets_.back()); + matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back()); assertInvariants(); } @@ -86,7 +86,7 @@ namespace gtsam { blockStart_(0) { fillOffsets(firstBlockDim, lastBlockDim, appendOneDimension); - matrix_.setZero(variableColOffsets_.back(), variableColOffsets_.back()); + matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back()); assertInvariants(); } @@ -95,7 +95,7 @@ namespace gtsam { SymmetricBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix, bool appendOneDimension = false) : blockStart_(0) { - matrix_.setZero(matrix.rows(), matrix.cols()); + matrix_.resize(matrix.rows(), matrix.cols()); matrix_.triangularView() = matrix.triangularView(); fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension); if(matrix_.rows() != matrix_.cols()) @@ -416,4 +416,3 @@ namespace gtsam { class CholeskyFailed; } - diff --git a/gtsam/base/make_shared.h b/gtsam/base/make_shared.h new file mode 100644 index 000000000..5281eec6d --- /dev/null +++ b/gtsam/base/make_shared.h @@ -0,0 +1,67 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 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 make_shared.h + * @brief make_shared trampoline function to ensure proper alignment + * @author Fan Jiang + */ + +#pragma once + +#include + +#include + +#include + +#include + +namespace gtsam { + /// An shorthand alias for accessing the ::type inside std::enable_if that can be used in a template directly + template + using enable_if_t = typename std::enable_if::type; +} + +namespace gtsam { + + /** + * Add our own `make_shared` as a layer of wrapping on `boost::make_shared` + * This solves the problem with the stock `make_shared` that custom alignment is not respected, causing SEGFAULTs + * at runtime, which is notoriously hard to debug. + * + * Explanation + * =============== + * The template `needs_eigen_aligned_allocator::value` will evaluate to `std::true_type` if the type alias + * `_eigen_aligned_allocator_trait = void` is present in a class, which is automatically added by the + * `GTSAM_MAKE_ALIGNED_OPERATOR_NEW` macro. + * + * This function declaration will only be taken when the above condition is true, so if some object does not need to + * be aligned, `gtsam::make_shared` will fall back to the next definition, which is a simple wrapper for + * `boost::make_shared`. + * + * @tparam T The type of object being constructed + * @tparam Args Type of the arguments of the constructor + * @param args Arguments of the constructor + * @return The object created as a boost::shared_ptr + */ + template + gtsam::enable_if_t::value, boost::shared_ptr> make_shared(Args &&... args) { + return boost::allocate_shared(Eigen::aligned_allocator(), std::forward(args)...); + } + + /// Fall back to the boost version if no need for alignment + template + gtsam::enable_if_t::value, boost::shared_ptr> make_shared(Args &&... args) { + return boost::make_shared(std::forward(args)...); + } + +} diff --git a/gtsam/base/serialization.h b/gtsam/base/serialization.h index e475465de..f589ecc5e 100644 --- a/gtsam/base/serialization.h +++ b/gtsam/base/serialization.h @@ -42,123 +42,218 @@ namespace gtsam { -// Serialization directly to strings in compressed format -template -std::string serialize(const T& input) { - std::ostringstream out_archive_stream; +/** @name Standard serialization + * Serialization in default compressed format + */ +///@{ +/// serializes to a stream +template +void serializeToStream(const T& input, std::ostream& out_archive_stream) { boost::archive::text_oarchive out_archive(out_archive_stream); out_archive << input; - return out_archive_stream.str(); } -template -void deserialize(const std::string& serialized, T& output) { - std::istringstream in_archive_stream(serialized); +/// deserializes from a stream +template +void deserializeFromStream(std::istream& in_archive_stream, T& output) { boost::archive::text_iarchive in_archive(in_archive_stream); in_archive >> output; } -template +/// serializes to a string +template +std::string serializeToString(const T& input) { + std::ostringstream out_archive_stream; + serializeToStream(input, out_archive_stream); + return out_archive_stream.str(); +} + +/// deserializes from a string +template +void deserializeFromString(const std::string& serialized, T& output) { + std::istringstream in_archive_stream(serialized); + deserializeFromStream(in_archive_stream, output); +} + +/// serializes to a file +template bool serializeToFile(const T& input, const std::string& filename) { std::ofstream out_archive_stream(filename.c_str()); - if (!out_archive_stream.is_open()) - return false; - boost::archive::text_oarchive out_archive(out_archive_stream); - out_archive << input; + if (!out_archive_stream.is_open()) return false; + serializeToStream(input, out_archive_stream); out_archive_stream.close(); return true; } -template +/// deserializes from a file +template bool deserializeFromFile(const std::string& filename, T& output) { std::ifstream in_archive_stream(filename.c_str()); - if (!in_archive_stream.is_open()) - return false; - boost::archive::text_iarchive in_archive(in_archive_stream); - in_archive >> output; + if (!in_archive_stream.is_open()) return false; + deserializeFromStream(in_archive_stream, output); in_archive_stream.close(); return true; } -// Serialization to XML format with named structures -template -std::string serializeXML(const T& input, const std::string& name="data") { - std::ostringstream out_archive_stream; - // braces to flush out_archive as it goes out of scope before taking str() - // fixes crash with boost 1.66-1.68 - // see https://github.com/boostorg/serialization/issues/82 - { - boost::archive::xml_oarchive out_archive(out_archive_stream); - out_archive << boost::serialization::make_nvp(name.c_str(), input); - } - return out_archive_stream.str(); +/// serializes to a string +template +std::string serialize(const T& input) { + return serializeToString(input); } -template -void deserializeXML(const std::string& serialized, T& output, const std::string& name="data") { - std::istringstream in_archive_stream(serialized); - boost::archive::xml_iarchive in_archive(in_archive_stream); - in_archive >> boost::serialization::make_nvp(name.c_str(), output); +/// deserializes from a string +template +void deserialize(const std::string& serialized, T& output) { + deserializeFromString(serialized, output); } +///@} -template -bool serializeToXMLFile(const T& input, const std::string& filename, const std::string& name="data") { - std::ofstream out_archive_stream(filename.c_str()); - if (!out_archive_stream.is_open()) - return false; +/** @name XML Serialization + * Serialization to XML format with named structures + */ +///@{ +/// serializes to a stream in XML +template +void serializeToXMLStream(const T& input, std::ostream& out_archive_stream, + const std::string& name = "data") { boost::archive::xml_oarchive out_archive(out_archive_stream); - out_archive << boost::serialization::make_nvp(name.c_str(), input);; - out_archive_stream.close(); - return true; + out_archive << boost::serialization::make_nvp(name.c_str(), input); } -template -bool deserializeFromXMLFile(const std::string& filename, T& output, const std::string& name="data") { - std::ifstream in_archive_stream(filename.c_str()); - if (!in_archive_stream.is_open()) - return false; +/// deserializes from a stream in XML +template +void deserializeFromXMLStream(std::istream& in_archive_stream, T& output, + const std::string& name = "data") { boost::archive::xml_iarchive in_archive(in_archive_stream); in_archive >> boost::serialization::make_nvp(name.c_str(), output); - in_archive_stream.close(); - return true; } -// Serialization to binary format with named structures -template -std::string serializeBinary(const T& input, const std::string& name="data") { +/// serializes to a string in XML +template +std::string serializeToXMLString(const T& input, + const std::string& name = "data") { std::ostringstream out_archive_stream; - boost::archive::binary_oarchive out_archive(out_archive_stream); - out_archive << boost::serialization::make_nvp(name.c_str(), input); + serializeToXMLStream(input, out_archive_stream, name); return out_archive_stream.str(); } -template -void deserializeBinary(const std::string& serialized, T& output, const std::string& name="data") { +/// deserializes from a string in XML +template +void deserializeFromXMLString(const std::string& serialized, T& output, + const std::string& name = "data") { std::istringstream in_archive_stream(serialized); - boost::archive::binary_iarchive in_archive(in_archive_stream); - in_archive >> boost::serialization::make_nvp(name.c_str(), output); + deserializeFromXMLStream(in_archive_stream, output, name); } -template -bool serializeToBinaryFile(const T& input, const std::string& filename, const std::string& name="data") { +/// serializes to an XML file +template +bool serializeToXMLFile(const T& input, const std::string& filename, + const std::string& name = "data") { std::ofstream out_archive_stream(filename.c_str()); - if (!out_archive_stream.is_open()) - return false; - boost::archive::binary_oarchive out_archive(out_archive_stream); - out_archive << boost::serialization::make_nvp(name.c_str(), input); + if (!out_archive_stream.is_open()) return false; + serializeToXMLStream(input, out_archive_stream, name); out_archive_stream.close(); return true; } -template -bool deserializeFromBinaryFile(const std::string& filename, T& output, const std::string& name="data") { +/// deserializes from an XML file +template +bool deserializeFromXMLFile(const std::string& filename, T& output, + const std::string& name = "data") { std::ifstream in_archive_stream(filename.c_str()); - if (!in_archive_stream.is_open()) - return false; - boost::archive::binary_iarchive in_archive(in_archive_stream); - in_archive >> boost::serialization::make_nvp(name.c_str(), output); + if (!in_archive_stream.is_open()) return false; + deserializeFromXMLStream(in_archive_stream, output, name); in_archive_stream.close(); return true; } -} // \namespace gtsam +/// serializes to a string in XML +template +std::string serializeXML(const T& input, + const std::string& name = "data") { + return serializeToXMLString(input, name); +} + +/// deserializes from a string in XML +template +void deserializeXML(const std::string& serialized, T& output, + const std::string& name = "data") { + deserializeFromXMLString(serialized, output, name); +} +///@} + +/** @name Binary Serialization + * Serialization to binary format with named structures + */ +///@{ +/// serializes to a stream in binary +template +void serializeToBinaryStream(const T& input, std::ostream& out_archive_stream, + const std::string& name = "data") { + boost::archive::binary_oarchive out_archive(out_archive_stream); + out_archive << boost::serialization::make_nvp(name.c_str(), input); +} + +/// deserializes from a stream in binary +template +void deserializeFromBinaryStream(std::istream& in_archive_stream, T& output, + const std::string& name = "data") { + boost::archive::binary_iarchive in_archive(in_archive_stream); + in_archive >> boost::serialization::make_nvp(name.c_str(), output); +} + +/// serializes to a string in binary +template +std::string serializeToBinaryString(const T& input, + const std::string& name = "data") { + std::ostringstream out_archive_stream; + serializeToBinaryStream(input, out_archive_stream, name); + return out_archive_stream.str(); +} + +/// deserializes from a string in binary +template +void deserializeFromBinaryString(const std::string& serialized, T& output, + const std::string& name = "data") { + std::istringstream in_archive_stream(serialized); + deserializeFromBinaryStream(in_archive_stream, output, name); +} + +/// serializes to a binary file +template +bool serializeToBinaryFile(const T& input, const std::string& filename, + const std::string& name = "data") { + std::ofstream out_archive_stream(filename.c_str()); + if (!out_archive_stream.is_open()) return false; + serializeToBinaryStream(input, out_archive_stream, name); + out_archive_stream.close(); + return true; +} + +/// deserializes from a binary file +template +bool deserializeFromBinaryFile(const std::string& filename, T& output, + const std::string& name = "data") { + std::ifstream in_archive_stream(filename.c_str()); + if (!in_archive_stream.is_open()) return false; + deserializeFromBinaryStream(in_archive_stream, output, name); + in_archive_stream.close(); + return true; +} + +/// serializes to a string in binary +template +std::string serializeBinary(const T& input, + const std::string& name = "data") { + return serializeToBinaryString(input, name); +} + +/// deserializes from a string in binary +template +void deserializeBinary(const std::string& serialized, T& output, + const std::string& name = "data") { + deserializeFromBinaryString(serialized, output, name); +} +///@} + +} // namespace gtsam diff --git a/gtsam/base/serializationTestHelpers.h b/gtsam/base/serializationTestHelpers.h index 06b3462e9..5994a5e51 100644 --- a/gtsam/base/serializationTestHelpers.h +++ b/gtsam/base/serializationTestHelpers.h @@ -26,6 +26,7 @@ #include #include +#include // whether to print the serialized text to stdout @@ -40,22 +41,37 @@ T create() { return T(); } +// Creates or empties a folder in the build folder and returns the relative path +boost::filesystem::path resetFilesystem( + boost::filesystem::path folder = "actual") { + boost::filesystem::remove_all(folder); + boost::filesystem::create_directory(folder); + return folder; +} + // Templated round-trip serialization template void roundtrip(const T& input, T& output) { - // Serialize std::string serialized = serialize(input); if (verbose) std::cout << serialized << std::endl << std::endl; - deserialize(serialized, output); } -// This version requires equality operator +// Templated round-trip serialization using a file +template +void roundtripFile(const T& input, T& output) { + boost::filesystem::path path = resetFilesystem()/"graph.dat"; + serializeToFile(input, path.string()); + deserializeFromFile(path.string(), output); +} + +// This version requires equality operator and uses string and file round-trips template bool equality(const T& input = T()) { - T output = create(); + T output = create(), outputf = create(); roundtrip(input,output); - return input==output; + roundtripFile(input,outputf); + return (input==output) && (input==outputf); } // This version requires Testable @@ -77,20 +93,26 @@ bool equalsDereferenced(const T& input) { // Templated round-trip serialization using XML template void roundtripXML(const T& input, T& output) { - // Serialize std::string serialized = serializeXML(input); if (verbose) std::cout << serialized << std::endl << std::endl; - - // De-serialize deserializeXML(serialized, output); } +// Templated round-trip serialization using XML File +template +void roundtripXMLFile(const T& input, T& output) { + boost::filesystem::path path = resetFilesystem()/"graph.xml"; + serializeToXMLFile(input, path.string()); + deserializeFromXMLFile(path.string(), output); +} + // This version requires equality operator template bool equalityXML(const T& input = T()) { - T output = create(); + T output = create(), outputf = create(); roundtripXML(input,output); - return input==output; + roundtripXMLFile(input,outputf); + return (input==output) && (input==outputf); } // This version requires Testable @@ -112,20 +134,26 @@ bool equalsDereferencedXML(const T& input = T()) { // Templated round-trip serialization using XML template void roundtripBinary(const T& input, T& output) { - // Serialize std::string serialized = serializeBinary(input); if (verbose) std::cout << serialized << std::endl << std::endl; - - // De-serialize deserializeBinary(serialized, output); } +// Templated round-trip serialization using Binary file +template +void roundtripBinaryFile(const T& input, T& output) { + boost::filesystem::path path = resetFilesystem()/"graph.bin"; + serializeToBinaryFile(input, path.string()); + deserializeFromBinaryFile(path.string(), output); +} + // This version requires equality operator template bool equalityBinary(const T& input = T()) { - T output = create(); + T output = create(), outputf = create(); roundtripBinary(input,output); - return input==output; + roundtripBinaryFile(input,outputf); + return (input==output) && (input==outputf); } // This version requires Testable diff --git a/gtsam/base/types.h b/gtsam/base/types.h index 2fa6eebb6..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; @@ -230,3 +231,50 @@ namespace std { #ifdef ERROR #undef ERROR #endif + +namespace gtsam { + + /// Convenience void_t as we assume C++11, it will not conflict the std one in C++17 as this is in `gtsam::` + template using void_t = void; + + /** + * A SFINAE trait to mark classes that need special alignment. + * + * This is required to make boost::make_shared and etc respect alignment, which is essential for the Python + * wrappers to work properly. + * + * Explanation + * ============= + * When a GTSAM type is not declared with the type alias `_eigen_aligned_allocator_trait = void`, the first template + * will be taken so `needs_eigen_aligned_allocator` will be resolved to `std::false_type`. + * + * Otherwise, it will resolve to the second template, which will be resolved to `std::true_type`. + * + * Please refer to `gtsam/base/make_shared.h` for an example. + */ + template> + struct needs_eigen_aligned_allocator : std::false_type { + }; + template + struct needs_eigen_aligned_allocator> : std::true_type { + }; + +} + +/** + * This marks a GTSAM object to require alignment. With this macro an object will automatically be allocated in aligned + * memory when one uses `gtsam::make_shared`. It reduces future misalignment problems that is hard to debug. + * See https://eigen.tuxfamily.org/dox/group__DenseMatrixManipulation__Alignement.html for detailed explanation. + */ +#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW \ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW \ + using _eigen_aligned_allocator_trait = void; + +/** + * This marks a GTSAM object to require alignment. With this macro an object will automatically be allocated in aligned + * memory when one uses `gtsam::make_shared`. It reduces future misalignment problems that is hard to debug. + * See https://eigen.tuxfamily.org/dox/group__DenseMatrixManipulation__Alignement.html for detailed explanation. + */ +#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) \ + EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) \ + using _eigen_aligned_allocator_trait = void; 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/BearingRange.h b/gtsam/geometry/BearingRange.h index 7c73f3cbd..8db7abffe 100644 --- a/gtsam/geometry/BearingRange.h +++ b/gtsam/geometry/BearingRange.h @@ -162,7 +162,7 @@ private: NeedsToAlign = (sizeof(B) % 16) == 0 || (sizeof(R) % 16) == 0 }; public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) + GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) }; // Declare this to be both Testable and a Manifold diff --git a/gtsam/geometry/Cal3Fisheye.cpp b/gtsam/geometry/Cal3Fisheye.cpp index c6b43004e..f7794fafb 100644 --- a/gtsam/geometry/Cal3Fisheye.cpp +++ b/gtsam/geometry/Cal3Fisheye.cpp @@ -24,7 +24,7 @@ namespace gtsam { /* ************************************************************************* */ -Cal3Fisheye::Cal3Fisheye(const Vector& v) +Cal3Fisheye::Cal3Fisheye(const Vector9& v) : fx_(v[0]), fy_(v[1]), s_(v[2]), @@ -50,76 +50,73 @@ Matrix3 Cal3Fisheye::K() const { } /* ************************************************************************* */ -static Matrix29 D2dcalibration(const double xd, const double yd, - const double xi, const double yi, - const double t3, const double t5, - const double t7, const double t9, const double r, - Matrix2& DK) { - // order: fx, fy, s, u0, v0 - Matrix25 DR1; - DR1 << xd, 0.0, yd, 1.0, 0.0, 0.0, yd, 0.0, 0.0, 1.0; - - // order: k1, k2, k3, k4 - Matrix24 DR2; - DR2 << t3 * xi, t5 * xi, t7 * xi, t9 * xi, t3 * yi, t5 * yi, t7 * yi, t9 * yi; - DR2 /= r; - Matrix29 D; - D << DR1, DK * DR2; - return D; -} - -/* ************************************************************************* */ -static Matrix2 D2dintrinsic(const double xi, const double yi, const double r, - const double td, const double t, const double tt, - const double t4, const double t6, const double t8, - const double k1, const double k2, const double k3, - const double k4, const Matrix2& DK) { - const double dr_dxi = xi / sqrt(xi * xi + yi * yi); - const double dr_dyi = yi / sqrt(xi * xi + yi * yi); - const double dt_dr = 1 / (1 + r * r); - const double dtd_dt = - 1 + 3 * k1 * tt + 5 * k2 * t4 + 7 * k3 * t6 + 9 * k4 * t8; - const double dtd_dxi = dtd_dt * dt_dr * dr_dxi; - const double dtd_dyi = dtd_dt * dt_dr * dr_dyi; - - const double rinv = 1 / r; - const double rrinv = 1 / (r * r); - const double dxd_dxi = - dtd_dxi * xi * rinv + td * rinv + td * xi * (-rrinv) * dr_dxi; - const double dxd_dyi = dtd_dyi * xi * rinv - td * xi * rrinv * dr_dyi; - const double dyd_dxi = dtd_dxi * yi * rinv - td * yi * rrinv * dr_dxi; - const double dyd_dyi = - dtd_dyi * yi * rinv + td * rinv + td * yi * (-rrinv) * dr_dyi; - - Matrix2 DR; - DR << dxd_dxi, dxd_dyi, dyd_dxi, dyd_dyi; - - return DK * DR; +double Cal3Fisheye::Scaling(double r) { + static constexpr double threshold = 1e-8; + if (r > threshold || r < -threshold) { + return atan(r) / r; + } else { + // Taylor expansion close to 0 + double r2 = r * r, r4 = r2 * r2; + return 1.0 - r2 / 3 + r4 / 5; + } } /* ************************************************************************* */ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1, OptionalJacobian<2, 2> H2) const { const double xi = p.x(), yi = p.y(); - const double r = sqrt(xi * xi + yi * yi); + const double r2 = xi * xi + yi * yi, r = sqrt(r2); const double t = atan(r); - const double tt = t * t, t4 = tt * tt, t6 = tt * t4, t8 = t4 * t4; - const double td = t * (1 + k1_ * tt + k2_ * t4 + k3_ * t6 + k4_ * t8); - const double td_o_r = r > 1e-8 ? td / r : 1; - const double xd = td_o_r * xi, yd = td_o_r * yi; + const double t2 = t * t, t4 = t2 * t2, t6 = t2 * t4, t8 = t4 * t4; + Vector5 K, T; + K << 1, k1_, k2_, k3_, k4_; + T << 1, t2, t4, t6, t8; + const double scaling = Scaling(r); + const double s = scaling * K.dot(T); + const double xd = s * xi, yd = s * yi; Point2 uv(fx_ * xd + s_ * yd + u0_, fy_ * yd + v0_); Matrix2 DK; if (H1 || H2) DK << fx_, s_, 0.0, fy_; // Derivative for calibration parameters (2 by 9) - if (H1) - *H1 = D2dcalibration(xd, yd, xi, yi, t * tt, t * t4, t * t6, t * t8, r, DK); + if (H1) { + Matrix25 DR1; + // order: fx, fy, s, u0, v0 + DR1 << xd, 0.0, yd, 1.0, 0.0, 0.0, yd, 0.0, 0.0, 1.0; + + // order: k1, k2, k3, k4 + Matrix24 DR2; + auto T4 = T.tail<4>().transpose(); + DR2 << xi * T4, yi * T4; + *H1 << DR1, DK * scaling * DR2; + } // Derivative for points in intrinsic coords (2 by 2) - if (H2) - *H2 = - D2dintrinsic(xi, yi, r, td, t, tt, t4, t6, t8, k1_, k2_, k3_, k4_, DK); + if (H2) { + const double dtd_dt = + 1 + 3 * k1_ * t2 + 5 * k2_ * t4 + 7 * k3_ * t6 + 9 * k4_ * t8; + const double dt_dr = 1 / (1 + r2); + const double rinv = 1 / r; + const double dr_dxi = xi * rinv; + const double dr_dyi = yi * rinv; + const double dtd_dxi = dtd_dt * dt_dr * dr_dxi; + const double dtd_dyi = dtd_dt * dt_dr * dr_dyi; + + const double td = t * K.dot(T); + const double rrinv = 1 / r2; + const double dxd_dxi = + dtd_dxi * dr_dxi + td * rinv - td * xi * rrinv * dr_dxi; + const double dxd_dyi = dtd_dyi * dr_dxi - td * xi * rrinv * dr_dyi; + const double dyd_dxi = dtd_dxi * dr_dyi - td * yi * rrinv * dr_dxi; + const double dyd_dyi = + dtd_dyi * dr_dyi + td * rinv - td * yi * rrinv * dr_dyi; + + Matrix2 DR; + DR << dxd_dxi, dxd_dyi, dyd_dxi, dyd_dyi; + + *H2 = DK * DR; + } return uv; } @@ -157,39 +154,10 @@ Point2 Cal3Fisheye::calibrate(const Point2& uv, const double tol) const { return pi; } -/* ************************************************************************* */ -Matrix2 Cal3Fisheye::D2d_intrinsic(const Point2& p) const { - const double xi = p.x(), yi = p.y(); - const double r = sqrt(xi * xi + yi * yi); - const double t = atan(r); - const double tt = t * t, t4 = tt * tt, t6 = t4 * tt, t8 = t4 * t4; - const double td = t * (1 + k1_ * tt + k2_ * t4 + k3_ * t6 + k4_ * t8); - - Matrix2 DK; - DK << fx_, s_, 0.0, fy_; - - return D2dintrinsic(xi, yi, r, td, t, tt, t4, t6, t8, k1_, k2_, k3_, k4_, DK); -} - -/* ************************************************************************* */ -Matrix29 Cal3Fisheye::D2d_calibration(const Point2& p) const { - const double xi = p.x(), yi = p.y(); - const double r = sqrt(xi * xi + yi * yi); - const double t = atan(r); - const double tt = t * t, t4 = tt * tt, t6 = tt * t4, t8 = t4 * t4; - const double td = t * (1 + k1_ * tt + k2_ * t4 + k3_ * t6 + k4_ * t8); - const double xd = td / r * xi, yd = td / r * yi; - - Matrix2 DK; - DK << fx_, s_, 0.0, fy_; - return D2dcalibration(xd, yd, xi, yi, t * tt, t * t4, t * t6, t * t8, r, DK); -} - /* ************************************************************************* */ void Cal3Fisheye::print(const std::string& s_) const { gtsam::print((Matrix)K(), s_ + ".K"); gtsam::print(Vector(k()), s_ + ".k"); - ; } /* ************************************************************************* */ diff --git a/gtsam/geometry/Cal3Fisheye.h b/gtsam/geometry/Cal3Fisheye.h index 5fb196047..e24fe074f 100644 --- a/gtsam/geometry/Cal3Fisheye.h +++ b/gtsam/geometry/Cal3Fisheye.h @@ -20,6 +20,8 @@ #include +#include + namespace gtsam { /** @@ -43,7 +45,7 @@ namespace gtsam { * [u; v; 1] = K*[x_d; y_d; 1] */ class GTSAM_EXPORT Cal3Fisheye { - protected: + private: double fx_, fy_, s_, u0_, v0_; // focal length, skew and principal point double k1_, k2_, k3_, k4_; // fisheye distortion coefficients @@ -78,7 +80,7 @@ class GTSAM_EXPORT Cal3Fisheye { /// @name Advanced Constructors /// @{ - Cal3Fisheye(const Vector& v); + explicit Cal3Fisheye(const Vector9& v); /// @} /// @name Standard Interface @@ -120,6 +122,9 @@ class GTSAM_EXPORT Cal3Fisheye { /// Return all parameters as a vector Vector9 vector() const; + /// Helper function that calculates atan(r)/r + static double Scaling(double r); + /** * @brief convert intrinsic coordinates [x_i; y_i] to (distorted) image * coordinates [u; v] @@ -136,13 +141,6 @@ class GTSAM_EXPORT Cal3Fisheye { /// y_i] Point2 calibrate(const Point2& p, const double tol = 1e-5) const; - /// Derivative of uncalibrate wrpt intrinsic coordinates [x_i; y_i] - Matrix2 D2d_intrinsic(const Point2& p) const; - - /// Derivative of uncalibrate wrpt the calibration parameters - /// [fx, fy, s, u0, v0, k1, k2, k3, k4] - Matrix29 D2d_calibration(const Point2& p) const; - /// @} /// @name Testable /// @{ diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index ecf9a572d..feab8e0fa 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -319,7 +319,7 @@ private: } public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; template diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 3235fdedd..ca719eb37 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -212,7 +212,7 @@ class EssentialMatrix { /// @} public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; template<> diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index c52127a45..9a80b937b 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -325,7 +325,7 @@ private: } public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; // manifold traits diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index 935962423..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; } @@ -222,7 +222,7 @@ private: } public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; // end of class PinholeBaseK @@ -425,7 +425,7 @@ private: } public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; // end of class PinholePose diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 388318827..2a1f108ca 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -317,7 +317,7 @@ public: public: // Align for Point2, which is either derived from, or is typedef, of Vector2 - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; // Pose2 /** specialization for pose2 wedge function (generic template in Lie.h) */ diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index e0fb6e5a5..6361adc90 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -19,8 +19,10 @@ #include #include -#include #include +#include +#include +#include using namespace std; @@ -36,10 +38,10 @@ Pose3::Pose3(const Pose2& pose2) : } /* ************************************************************************* */ -Pose3 Pose3::Create(const Rot3& R, const Point3& t, OptionalJacobian<6, 3> H1, - OptionalJacobian<6, 3> H2) { - if (H1) *H1 << I_3x3, Z_3x3; - if (H2) *H2 << Z_3x3, R.transpose(); +Pose3 Pose3::Create(const Rot3& R, const Point3& t, OptionalJacobian<6, 3> HR, + OptionalJacobian<6, 3> Ht) { + if (HR) *HR << I_3x3, Z_3x3; + if (Ht) *Ht << Z_3x3, R.transpose(); return Pose3(R, t); } @@ -72,15 +74,15 @@ Matrix6 Pose3::adjointMap(const Vector6& xi) { /* ************************************************************************* */ Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y, - OptionalJacobian<6,6> H) { - if (H) { - H->setZero(); + OptionalJacobian<6, 6> Hxi) { + if (Hxi) { + Hxi->setZero(); for (int i = 0; i < 6; ++i) { Vector6 dxi; dxi.setZero(); dxi(i) = 1.0; Matrix6 Gi = adjointMap(dxi); - H->col(i) = Gi * y; + Hxi->col(i) = Gi * y; } } return adjointMap(xi) * y; @@ -88,15 +90,15 @@ Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y, /* ************************************************************************* */ Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y, - OptionalJacobian<6,6> H) { - if (H) { - H->setZero(); + OptionalJacobian<6, 6> Hxi) { + if (Hxi) { + Hxi->setZero(); for (int i = 0; i < 6; ++i) { Vector6 dxi; dxi.setZero(); dxi(i) = 1.0; Matrix6 GTi = adjointMap(dxi).transpose(); - H->col(i) = GTi * y; + Hxi->col(i) = GTi * y; } } return adjointMap(xi).transpose() * y; @@ -114,8 +116,8 @@ bool Pose3::equals(const Pose3& pose, double tol) const { /* ************************************************************************* */ /** Modified from Murray94book version (which assumes w and v normalized?) */ -Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> H) { - if (H) *H = ExpmapDerivative(xi); +Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) { + if (Hxi) *Hxi = ExpmapDerivative(xi); // get angular velocity omega and translational velocity v from twist xi Vector3 omega(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5)); @@ -123,8 +125,8 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> H) { Rot3 R = Rot3::Expmap(omega); double theta2 = omega.dot(omega); if (theta2 > std::numeric_limits::epsilon()) { - Vector3 t_parallel = omega * omega.dot(v); // translation parallel to axis - Vector3 omega_cross_v = omega.cross(v); // points towards axis + Vector3 t_parallel = omega * omega.dot(v); // translation parallel to axis + Vector3 omega_cross_v = omega.cross(v); // points towards axis Vector3 t = (omega_cross_v - R * omega_cross_v + t_parallel) / theta2; return Pose3(R, t); } else { @@ -133,10 +135,10 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> H) { } /* ************************************************************************* */ -Vector6 Pose3::Logmap(const Pose3& p, OptionalJacobian<6, 6> H) { - if (H) *H = LogmapDerivative(p); - const Vector3 w = Rot3::Logmap(p.rotation()); - const Vector3 T = p.translation(); +Vector6 Pose3::Logmap(const Pose3& pose, OptionalJacobian<6, 6> Hpose) { + if (Hpose) *Hpose = LogmapDerivative(pose); + const Vector3 w = Rot3::Logmap(pose.rotation()); + const Vector3 T = pose.translation(); const double t = w.norm(); if (t < 1e-10) { Vector6 log; @@ -156,33 +158,33 @@ Vector6 Pose3::Logmap(const Pose3& p, OptionalJacobian<6, 6> H) { } /* ************************************************************************* */ -Pose3 Pose3::ChartAtOrigin::Retract(const Vector6& xi, ChartJacobian H) { +Pose3 Pose3::ChartAtOrigin::Retract(const Vector6& xi, ChartJacobian Hxi) { #ifdef GTSAM_POSE3_EXPMAP - return Expmap(xi, H); + return Expmap(xi, Hxi); #else Matrix3 DR; - Rot3 R = Rot3::Retract(xi.head<3>(), H ? &DR : 0); - if (H) { - *H = I_6x6; - H->topLeftCorner<3,3>() = DR; + Rot3 R = Rot3::Retract(xi.head<3>(), Hxi ? &DR : 0); + if (Hxi) { + *Hxi = I_6x6; + Hxi->topLeftCorner<3, 3>() = DR; } return Pose3(R, Point3(xi.tail<3>())); #endif } /* ************************************************************************* */ -Vector6 Pose3::ChartAtOrigin::Local(const Pose3& T, ChartJacobian H) { +Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) { #ifdef GTSAM_POSE3_EXPMAP - return Logmap(T, H); + return Logmap(pose, Hpose); #else Matrix3 DR; - Vector3 omega = Rot3::LocalCoordinates(T.rotation(), H ? &DR : 0); - if (H) { - *H = I_6x6; - H->topLeftCorner<3,3>() = DR; + Vector3 omega = Rot3::LocalCoordinates(pose.rotation(), Hpose ? &DR : 0); + if (Hpose) { + *Hpose = I_6x6; + Hpose->topLeftCorner<3, 3>() = DR; } Vector6 xi; - xi << omega, T.translation(); + xi << omega, pose.translation(); return xi; #endif } @@ -259,16 +261,16 @@ Matrix6 Pose3::LogmapDerivative(const Pose3& pose) { } /* ************************************************************************* */ -const Point3& Pose3::translation(OptionalJacobian<3, 6> H) const { - if (H) *H << Z_3x3, rotation().matrix(); +const Point3& Pose3::translation(OptionalJacobian<3, 6> Hself) const { + if (Hself) *Hself << Z_3x3, rotation().matrix(); return t_; } /* ************************************************************************* */ -const Rot3& Pose3::rotation(OptionalJacobian<3, 6> H) const { - if (H) { - *H << I_3x3, Z_3x3; +const Rot3& Pose3::rotation(OptionalJacobian<3, 6> Hself) const { + if (Hself) { + *Hself << I_3x3, Z_3x3; } return R_; } @@ -282,9 +284,10 @@ Matrix4 Pose3::matrix() const { } /* ************************************************************************* */ -Pose3 Pose3::transformPoseFrom(const Pose3& aTb) const { +Pose3 Pose3::transformPoseFrom(const Pose3& aTb, OptionalJacobian<6, 6> Hself, + OptionalJacobian<6, 6> HaTb) const { const Pose3& wTa = *this; - return wTa * aTb; + return wTa.compose(aTb, Hself, HaTb); } /* ************************************************************************* */ @@ -297,101 +300,101 @@ Pose3 Pose3::transform_to(const Pose3& pose) const { #endif /* ************************************************************************* */ -Pose3 Pose3::transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> H1, - OptionalJacobian<6, 6> H2) const { - if (H1) *H1 = -wTb.inverse().AdjointMap() * AdjointMap(); - if (H2) *H2 = I_6x6; +Pose3 Pose3::transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself, + OptionalJacobian<6, 6> HwTb) const { + if (Hself) *Hself = -wTb.inverse().AdjointMap() * AdjointMap(); + if (HwTb) *HwTb = I_6x6; const Pose3& wTa = *this; return wTa.inverse() * wTb; } /* ************************************************************************* */ -Point3 Pose3::transformFrom(const Point3& p, OptionalJacobian<3,6> Dpose, - OptionalJacobian<3,3> Dpoint) const { +Point3 Pose3::transformFrom(const Point3& point, OptionalJacobian<3, 6> Hself, + OptionalJacobian<3, 3> Hpoint) const { // Only get matrix once, to avoid multiple allocations, // as well as multiple conversions in the Quaternion case const Matrix3 R = R_.matrix(); - if (Dpose) { - Dpose->leftCols<3>() = R * skewSymmetric(-p.x(), -p.y(), -p.z()); - Dpose->rightCols<3>() = R; + if (Hself) { + Hself->leftCols<3>() = R * skewSymmetric(-point.x(), -point.y(), -point.z()); + Hself->rightCols<3>() = R; } - if (Dpoint) { - *Dpoint = R; + if (Hpoint) { + *Hpoint = R; } - return R_ * p + t_; + return R_ * point + t_; } /* ************************************************************************* */ -Point3 Pose3::transformTo(const Point3& p, OptionalJacobian<3,6> Dpose, - OptionalJacobian<3,3> Dpoint) const { +Point3 Pose3::transformTo(const Point3& point, OptionalJacobian<3, 6> Hself, + OptionalJacobian<3, 3> Hpoint) const { // Only get transpose once, to avoid multiple allocations, // as well as multiple conversions in the Quaternion case const Matrix3 Rt = R_.transpose(); - const Point3 q(Rt*(p - t_)); - if (Dpose) { + const Point3 q(Rt*(point - t_)); + if (Hself) { const double wx = q.x(), wy = q.y(), wz = q.z(); - (*Dpose) << + (*Hself) << 0.0, -wz, +wy,-1.0, 0.0, 0.0, +wz, 0.0, -wx, 0.0,-1.0, 0.0, -wy, +wx, 0.0, 0.0, 0.0,-1.0; } - if (Dpoint) { - *Dpoint = Rt; + if (Hpoint) { + *Hpoint = Rt; } return q; } /* ************************************************************************* */ -double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1, - OptionalJacobian<1, 3> H2) const { +double Pose3::range(const Point3& point, OptionalJacobian<1, 6> Hself, + OptionalJacobian<1, 3> Hpoint) const { Matrix36 D_local_pose; Matrix3 D_local_point; - Point3 local = transformTo(point, H1 ? &D_local_pose : 0, H2 ? &D_local_point : 0); - if (!H1 && !H2) { + Point3 local = transformTo(point, Hself ? &D_local_pose : 0, Hpoint ? &D_local_point : 0); + if (!Hself && !Hpoint) { return local.norm(); } else { Matrix13 D_r_local; const double r = norm3(local, D_r_local); - if (H1) *H1 = D_r_local * D_local_pose; - if (H2) *H2 = D_r_local * D_local_point; + if (Hself) *Hself = D_r_local * D_local_pose; + if (Hpoint) *Hpoint = D_r_local * D_local_point; return r; } } /* ************************************************************************* */ -double Pose3::range(const Pose3& pose, OptionalJacobian<1, 6> H1, - OptionalJacobian<1, 6> H2) const { +double Pose3::range(const Pose3& pose, OptionalJacobian<1, 6> Hself, + OptionalJacobian<1, 6> Hpose) const { Matrix13 D_local_point; - double r = range(pose.translation(), H1, H2 ? &D_local_point : 0); - if (H2) *H2 << Matrix13::Zero(), D_local_point * pose.rotation().matrix(); + double r = range(pose.translation(), Hself, Hpose ? &D_local_point : 0); + if (Hpose) *Hpose << Matrix13::Zero(), D_local_point * pose.rotation().matrix(); return r; } /* ************************************************************************* */ -Unit3 Pose3::bearing(const Point3& point, OptionalJacobian<2, 6> H1, - OptionalJacobian<2, 3> H2) const { +Unit3 Pose3::bearing(const Point3& point, OptionalJacobian<2, 6> Hself, + OptionalJacobian<2, 3> Hpoint) const { Matrix36 D_local_pose; Matrix3 D_local_point; - Point3 local = transformTo(point, H1 ? &D_local_pose : 0, H2 ? &D_local_point : 0); - if (!H1 && !H2) { + Point3 local = transformTo(point, Hself ? &D_local_pose : 0, Hpoint ? &D_local_point : 0); + if (!Hself && !Hpoint) { return Unit3(local); } else { Matrix23 D_b_local; Unit3 b = Unit3::FromPoint3(local, D_b_local); - if (H1) *H1 = D_b_local * D_local_pose; - if (H2) *H2 = D_b_local * D_local_point; + if (Hself) *Hself = D_b_local * D_local_pose; + if (Hpoint) *Hpoint = D_b_local * D_local_point; return b; } } /* ************************************************************************* */ -Unit3 Pose3::bearing(const Pose3& pose, OptionalJacobian<2, 6> H1, - OptionalJacobian<2, 6> H2) const { - if (H2) { - H2->setZero(); - return bearing(pose.translation(), H1, H2.cols<3>(3)); +Unit3 Pose3::bearing(const Pose3& pose, OptionalJacobian<2, 6> Hself, + OptionalJacobian<2, 6> Hpose) const { + if (Hpose) { + Hpose->setZero(); + return bearing(pose.translation(), Hself, Hpose.cols<3>(3)); } - return bearing(pose.translation(), H1, boost::none); + return bearing(pose.translation(), Hself, boost::none); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index fa55f98de..3825b6241 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -75,8 +75,8 @@ public: /// Named constructor with derivatives static Pose3 Create(const Rot3& R, const Point3& t, - OptionalJacobian<6, 3> H1 = boost::none, - OptionalJacobian<6, 3> H2 = boost::none); + OptionalJacobian<6, 3> HR = boost::none, + OptionalJacobian<6, 3> Ht = boost::none); /** * Create Pose3 by aligning two point pairs @@ -117,10 +117,10 @@ public: /// @{ /// Exponential map at identity - create a rotation from canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ - static Pose3 Expmap(const Vector6& xi, OptionalJacobian<6, 6> H = boost::none); + static Pose3 Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi = boost::none); /// Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of this rotation - static Vector6 Logmap(const Pose3& p, OptionalJacobian<6, 6> H = boost::none); + static Vector6 Logmap(const Pose3& pose, OptionalJacobian<6, 6> Hpose = boost::none); /** * Calculate Adjoint map, transforming a twist in the this pose's (i.e, body) frame to the world spatial frame @@ -157,7 +157,7 @@ public: * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives */ static Vector6 adjoint(const Vector6 &xi, const Vector6 &y, - OptionalJacobian<6, 6> = boost::none); + OptionalJacobian<6, 6> Hxi = boost::none); // temporary fix for wrappers until case issue is resolved static Matrix6 adjointMap_(const Vector6 &xi) { return adjointMap(xi);} @@ -167,7 +167,7 @@ public: * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space. */ static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y, - OptionalJacobian<6, 6> H = boost::none); + OptionalJacobian<6, 6> Hxi = boost::none); /// Derivative of Expmap static Matrix6 ExpmapDerivative(const Vector6& xi); @@ -177,8 +177,8 @@ public: // Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP struct ChartAtOrigin { - static Pose3 Retract(const Vector6& v, ChartJacobian H = boost::none); - static Vector6 Local(const Pose3& r, ChartJacobian H = boost::none); + static Pose3 Retract(const Vector6& xi, ChartJacobian Hxi = boost::none); + static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = boost::none); }; using LieGroup::inverse; // version with derivative @@ -201,38 +201,38 @@ public: /** * @brief takes point in Pose coordinates and transforms it to world coordinates - * @param p point in Pose coordinates - * @param Dpose optional 3*6 Jacobian wrpt this pose - * @param Dpoint optional 3*3 Jacobian wrpt point + * @param point point in Pose coordinates + * @param Hself optional 3*6 Jacobian wrpt this pose + * @param Hpoint optional 3*3 Jacobian wrpt point * @return point in world coordinates */ - Point3 transformFrom(const Point3& p, OptionalJacobian<3, 6> Dpose = - boost::none, OptionalJacobian<3, 3> Dpoint = boost::none) const; + Point3 transformFrom(const Point3& point, OptionalJacobian<3, 6> Hself = + boost::none, OptionalJacobian<3, 3> Hpoint = boost::none) const; /** syntactic sugar for transformFrom */ - inline Point3 operator*(const Point3& p) const { - return transformFrom(p); + inline Point3 operator*(const Point3& point) const { + return transformFrom(point); } /** * @brief takes point in world coordinates and transforms it to Pose coordinates - * @param p point in world coordinates - * @param Dpose optional 3*6 Jacobian wrpt this pose - * @param Dpoint optional 3*3 Jacobian wrpt point + * @param point point in world coordinates + * @param Hself optional 3*6 Jacobian wrpt this pose + * @param Hpoint optional 3*3 Jacobian wrpt point * @return point in Pose coordinates */ - Point3 transformTo(const Point3& p, OptionalJacobian<3, 6> Dpose = - boost::none, OptionalJacobian<3, 3> Dpoint = boost::none) const; + Point3 transformTo(const Point3& point, OptionalJacobian<3, 6> Hself = + boost::none, OptionalJacobian<3, 3> Hpoint = boost::none) const; /// @} /// @name Standard Interface /// @{ /// get rotation - const Rot3& rotation(OptionalJacobian<3, 6> H = boost::none) const; + const Rot3& rotation(OptionalJacobian<3, 6> Hself = boost::none) const; /// get translation - const Point3& translation(OptionalJacobian<3, 6> H = boost::none) const; + const Point3& translation(OptionalJacobian<3, 6> Hself = boost::none) const; /// get x double x() const { @@ -252,36 +252,44 @@ public: /** convert to 4*4 matrix */ Matrix4 matrix() const; - /** receives a pose in local coordinates and transforms it to world coordinates */ - Pose3 transformPoseFrom(const Pose3& pose) const; + /** + * Assuming self == wTa, takes a pose aTb in local coordinates + * and transforms it to world coordinates wTb = wTa * aTb. + * This is identical to compose. + */ + Pose3 transformPoseFrom(const Pose3& aTb, OptionalJacobian<6, 6> Hself = boost::none, + OptionalJacobian<6, 6> HaTb = boost::none) const; - /** receives a pose in world coordinates and transforms it to local coordinates */ - Pose3 transformPoseTo(const Pose3& pose, OptionalJacobian<6, 6> H1 = boost::none, - OptionalJacobian<6, 6> H2 = boost::none) const; + /** + * Assuming self == wTa, takes a pose wTb in world coordinates + * and transforms it to local coordinates aTb = inv(wTa) * wTb + */ + Pose3 transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself = boost::none, + OptionalJacobian<6, 6> HwTb = boost::none) const; /** * Calculate range to a landmark * @param point 3D location of landmark * @return range (double) */ - double range(const Point3& point, OptionalJacobian<1, 6> H1 = boost::none, - OptionalJacobian<1, 3> H2 = boost::none) const; + double range(const Point3& point, OptionalJacobian<1, 6> Hself = boost::none, + OptionalJacobian<1, 3> Hpoint = boost::none) const; /** * Calculate range to another pose * @param pose Other SO(3) pose * @return range (double) */ - double range(const Pose3& pose, OptionalJacobian<1, 6> H1 = boost::none, - OptionalJacobian<1, 6> H2 = boost::none) const; + double range(const Pose3& pose, OptionalJacobian<1, 6> Hself = boost::none, + OptionalJacobian<1, 6> Hpose = boost::none) const; /** * Calculate bearing to a landmark * @param point 3D location of landmark * @return bearing (Unit3) */ - Unit3 bearing(const Point3& point, OptionalJacobian<2, 6> H1 = boost::none, - OptionalJacobian<2, 3> H2 = boost::none) const; + Unit3 bearing(const Point3& point, OptionalJacobian<2, 6> Hself = boost::none, + OptionalJacobian<2, 3> Hpoint = boost::none) const; /** * Calculate bearing to another pose @@ -289,8 +297,8 @@ public: * information is ignored. * @return bearing (Unit3) */ - Unit3 bearing(const Pose3& pose, OptionalJacobian<2, 6> H1 = boost::none, - OptionalJacobian<2, 6> H2 = boost::none) const; + Unit3 bearing(const Pose3& pose, OptionalJacobian<2, 6> Hself = boost::none, + OptionalJacobian<2, 6> Hpose = boost::none) const; /// @} /// @name Advanced Interface @@ -321,20 +329,20 @@ public: #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// @name Deprecated /// @{ - Point3 transform_from(const Point3& p, - OptionalJacobian<3, 6> Dpose = boost::none, - OptionalJacobian<3, 3> Dpoint = boost::none) const { - return transformFrom(p, Dpose, Dpoint); + Point3 transform_from(const Point3& point, + OptionalJacobian<3, 6> Hself = boost::none, + OptionalJacobian<3, 3> Hpoint = boost::none) const { + return transformFrom(point, Hself, Hpoint); } - Point3 transform_to(const Point3& p, - OptionalJacobian<3, 6> Dpose = boost::none, - OptionalJacobian<3, 3> Dpoint = boost::none) const { - return transformTo(p, Dpose, Dpoint); + Point3 transform_to(const Point3& point, + OptionalJacobian<3, 6> Hself = boost::none, + OptionalJacobian<3, 3> Hpoint = boost::none) const { + return transformTo(point, Hself, Hpoint); } - Pose3 transform_pose_to(const Pose3& pose, - OptionalJacobian<6, 6> H1 = boost::none, - OptionalJacobian<6, 6> H2 = boost::none) const { - return transformPoseTo(pose, H1, H2); + Pose3 transform_pose_to(const Pose3& pose, + OptionalJacobian<6, 6> Hself = boost::none, + OptionalJacobian<6, 6> Hpose = boost::none) const { + return transformPoseTo(pose, Hself, Hpose); } /** * @deprecated: this function is neither here not there. */ @@ -355,7 +363,7 @@ public: #ifdef GTSAM_USE_QUATERNIONS // Align if we are using Quaternions public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW #endif }; // Pose3 class diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 8ab7dd377..9ba4e5df8 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -544,7 +544,7 @@ namespace gtsam { #ifdef GTSAM_USE_QUATERNIONS // only align if quaternion, Matrix3 has no alignment requirements public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW #endif }; diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h index 5313d3018..a6392c2f9 100644 --- a/gtsam/geometry/SOn.h +++ b/gtsam/geometry/SOn.h @@ -20,8 +20,8 @@ #include #include +#include #include - #include #include // TODO(frank): how to avoid? @@ -54,7 +54,7 @@ class SO : public LieGroup, internal::DimensionSO(N)> { using VectorN2 = Eigen::Matrix; using MatrixDD = Eigen::Matrix; - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(true) protected: MatrixNN matrix_; ///< Rotation matrix @@ -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 f1a9c1a69..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; } @@ -214,7 +216,7 @@ private: /// @} public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; // Define GTSAM traits diff --git a/gtsam/geometry/tests/testCal3DFisheye.cpp b/gtsam/geometry/tests/testCal3DFisheye.cpp index 9203b5438..9317fb737 100644 --- a/gtsam/geometry/tests/testCal3DFisheye.cpp +++ b/gtsam/geometry/tests/testCal3DFisheye.cpp @@ -10,17 +10,18 @@ * -------------------------------------------------------------------------- */ /** - * @file testCal3Fisheye.cpp + * @file testCal3DFisheye.cpp * @brief Unit tests for fisheye calibration class * @author ghaggin */ -#include #include #include #include #include +#include + using namespace gtsam; GTSAM_CONCEPT_TESTABLE_INST(Cal3Fisheye) @@ -30,12 +31,27 @@ static const double fx = 250, fy = 260, s = 0.1, u0 = 320, v0 = 240; static Cal3Fisheye K(fx, fy, s, u0, v0, -0.013721808247486035, 0.020727425669427896, -0.012786476702685545, 0.0025242267320687625); -static Point2 p(2, 3); +static Point2 kTestPoint2(2, 3); + +/* ************************************************************************* */ +TEST(Cal3Fisheye, assert_equal) { CHECK(assert_equal(K, K, 1e-5)); } + +/* ************************************************************************* */ +TEST(Cal3Fisheye, retract) { + Cal3Fisheye expected(K.fx() + 1, K.fy() + 2, K.skew() + 3, K.px() + 4, + K.py() + 5, K.k1() + 6, K.k2() + 7, K.k3() + 8, + K.k4() + 9); + Vector d(9); + d << 1, 2, 3, 4, 5, 6, 7, 8, 9; + Cal3Fisheye actual = K.retract(d); + CHECK(assert_equal(expected, actual, 1e-7)); + CHECK(assert_equal(d, K.localCoordinates(actual), 1e-7)); +} /* ************************************************************************* */ TEST(Cal3Fisheye, uncalibrate1) { // Calculate the solution - const double xi = p.x(), yi = p.y(); + const double xi = kTestPoint2.x(), yi = kTestPoint2.y(); const double r = sqrt(xi * xi + yi * yi); const double t = atan(r); const double tt = t * t, t4 = tt * tt, t6 = tt * t4, t8 = t4 * t4; @@ -46,32 +62,42 @@ TEST(Cal3Fisheye, uncalibrate1) { Point2 uv_sol(v[0] / v[2], v[1] / v[2]); - Point2 uv = K.uncalibrate(p); + Point2 uv = K.uncalibrate(kTestPoint2); CHECK(assert_equal(uv, uv_sol)); } /* ************************************************************************* */ -/** - * Check that a point at (0,0) projects to the - * image center. - */ -TEST(Cal3Fisheye, uncalibrate2) { - Point2 pz(0, 0); - auto uv = K.uncalibrate(pz); - CHECK(assert_equal(uv, Point2(u0, v0))); +// For numerical derivatives +Point2 f(const Cal3Fisheye& k, const Point2& pt) { return k.uncalibrate(pt); } + +/* ************************************************************************* */ +TEST(Cal3Fisheye, Derivatives) { + Matrix H1, H2; + K.uncalibrate(kTestPoint2, H1, H2); + CHECK(assert_equal(numericalDerivative21(f, K, kTestPoint2, 1e-7), H1, 1e-5)); + CHECK(assert_equal(numericalDerivative22(f, K, kTestPoint2, 1e-7), H2, 1e-5)); } /* ************************************************************************* */ -/** - * This test uses cv2::fisheye::projectPoints to test that uncalibrate - * properly projects a point into the image plane. One notable difference - * between opencv and the Cal3Fisheye::uncalibrate function is the skew - * parameter. The equivalence is alpha = s/fx. - * - * - * Python script to project points with fisheye model in OpenCv - * (script run with OpenCv version 4.2.0 and Numpy version 1.18.2) - */ +// Check that a point at (0,0) projects to the image center. +TEST(Cal3Fisheye, uncalibrate2) { + Point2 pz(0, 0); + Matrix H1, H2; + auto uv = K.uncalibrate(pz, H1, H2); + CHECK(assert_equal(uv, Point2(u0, v0))); + CHECK(assert_equal(numericalDerivative21(f, K, pz, 1e-7), H1, 1e-5)); + // TODO(frank): the second jacobian is all NaN for the image center! + // CHECK(assert_equal(numericalDerivative22(f, K, pz, 1e-7), H2, 1e-5)); +} + +/* ************************************************************************* */ +// This test uses cv2::fisheye::projectPoints to test that uncalibrate +// properly projects a point into the image plane. One notable difference +// between opencv and the Cal3Fisheye::uncalibrate function is the skew +// parameter. The equivalence is alpha = s/fx. +// +// Python script to project points with fisheye model in OpenCv +// (script run with OpenCv version 4.2.0 and Numpy version 1.18.2) // clang-format off /* =========================================================== @@ -94,6 +120,7 @@ tvec = np.float64([[0.,0.,0.]]); imagePoints, jacobian = cv2.fisheye.projectPoints(objpts, rvec, tvec, cameraMatrix, distCoeffs, alpha=alpha) np.set_printoptions(precision=14) print(imagePoints) + =========================================================== * Script output: [[[457.82638130304935 408.18905848512986]]] */ @@ -134,21 +161,18 @@ TEST(Cal3Fisheye, calibrate1) { } /* ************************************************************************* */ -/** - * Check that calibrate returns (0,0) for the image center - */ +// Check that calibrate returns (0,0) for the image center TEST(Cal3Fisheye, calibrate2) { Point2 uv(u0, v0); auto xi_hat = K.calibrate(uv); CHECK(assert_equal(xi_hat, Point2(0, 0))) } -/** - * Run calibrate on OpenCv test from uncalibrate3 - * (script shown above) - * 3d point: (23, 27, 31) - * 2d point in image plane: (457.82638130304935, 408.18905848512986) - */ +/* ************************************************************************* */ +// Run calibrate on OpenCv test from uncalibrate3 +// (script shown above) +// 3d point: (23, 27, 31) +// 2d point in image plane: (457.82638130304935, 408.18905848512986) TEST(Cal3Fisheye, calibrate3) { Point3 p3(23, 27, 31); Point2 xi(p3.x() / p3.z(), p3.y() / p3.z()); @@ -157,47 +181,6 @@ TEST(Cal3Fisheye, calibrate3) { CHECK(assert_equal(xi_hat, xi)); } -/* ************************************************************************* */ -// For numerical derivatives -Point2 uncalibrate_(const Cal3Fisheye& k, const Point2& pt) { - return k.uncalibrate(pt); -} - -/* ************************************************************************* */ -TEST(Cal3Fisheye, Duncalibrate1) { - Matrix computed; - K.uncalibrate(p, computed, boost::none); - Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7); - CHECK(assert_equal(numerical, computed, 1e-5)); - Matrix separate = K.D2d_calibration(p); - CHECK(assert_equal(numerical, separate, 1e-5)); -} - -/* ************************************************************************* */ -TEST(Cal3Fisheye, Duncalibrate2) { - Matrix computed; - K.uncalibrate(p, boost::none, computed); - Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7); - CHECK(assert_equal(numerical, computed, 1e-5)); - Matrix separate = K.D2d_intrinsic(p); - CHECK(assert_equal(numerical, separate, 1e-5)); -} - -/* ************************************************************************* */ -TEST(Cal3Fisheye, assert_equal) { CHECK(assert_equal(K, K, 1e-5)); } - -/* ************************************************************************* */ -TEST(Cal3Fisheye, retract) { - Cal3Fisheye expected(K.fx() + 1, K.fy() + 2, K.skew() + 3, K.px() + 4, - K.py() + 5, K.k1() + 6, K.k2() + 7, K.k3() + 8, - K.k4() + 9); - Vector d(9); - d << 1, 2, 3, 4, 5, 6, 7, 8, 9; - Cal3Fisheye actual = K.retract(d); - CHECK(assert_equal(expected, actual, 1e-7)); - CHECK(assert_equal(d, K.localCoordinates(actual), 1e-7)); -} - /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index caeed5770..bb64192ef 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -418,6 +418,29 @@ TEST(Pose3, transform_to_rotate) { EXPECT(assert_equal(expected, actual, 0.001)); } +/* ************************************************************************* */ +// Check transformPoseFrom and its pushforward +Pose3 transformPoseFrom_(const Pose3& wTa, const Pose3& aTb) { + return wTa.transformPoseFrom(aTb); +} + +TEST(Pose3, transformPoseFrom) +{ + Matrix actual = (T2*T2).matrix(); + Matrix expected = T2.matrix()*T2.matrix(); + EXPECT(assert_equal(actual, expected, 1e-8)); + + Matrix H1, H2; + T2.transformPoseFrom(T2, H1, H2); + + Matrix numericalH1 = numericalDerivative21(transformPoseFrom_, T2, T2); + EXPECT(assert_equal(numericalH1, H1, 5e-3)); + EXPECT(assert_equal(T2.inverse().AdjointMap(), H1, 5e-3)); + + Matrix numericalH2 = numericalDerivative22(transformPoseFrom_, T2, T2); + EXPECT(assert_equal(numericalH2, H2, 1e-4)); +} + /* ************************************************************************* */ TEST(Pose3, transformTo) { Pose3 transform(Rot3::Rodrigues(0, 0, -1.570796), Point3(2, 4, 0)); 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/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 586b7b165..8cdf0fdc0 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -215,7 +215,7 @@ struct CameraProjectionMatrix { private: const Matrix3 K_; public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; /** 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 bb83b672d..2e0ffb034 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -505,7 +505,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/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h index db588008e..5a0031caf 100644 --- a/gtsam/navigation/AttitudeFactor.h +++ b/gtsam/navigation/AttitudeFactor.h @@ -139,7 +139,7 @@ private: } public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; /// traits @@ -219,7 +219,7 @@ private: } public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; /// traits 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 ca9b2ca1a..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: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + 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,12 +236,13 @@ 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_); } public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; /** @@ -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,19 +358,31 @@ 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: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; // 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/ImuBias.h b/gtsam/navigation/ImuBias.h index ff1a53025..d52b4eb29 100644 --- a/gtsam/navigation/ImuBias.h +++ b/gtsam/navigation/ImuBias.h @@ -171,7 +171,7 @@ private: public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW /// @} }; // ConstantBias class 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 71ef5d08f..0e0559a32 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -61,15 +61,21 @@ 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 // Align if we are using Quaternions public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW #endif }; @@ -182,7 +188,7 @@ class GTSAM_EXPORT PreintegratedRotation { #ifdef GTSAM_USE_QUATERNIONS // Align if we are using Quaternions public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW #endif }; diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 9926d207a..29d7814b5 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -213,8 +213,18 @@ 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: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; } /// namespace gtsam 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 962fef277..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); } @@ -84,7 +85,7 @@ protected: #ifdef GTSAM_USE_QUATERNIONS // Align if we are using Quaternions public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW #endif }; diff --git a/gtsam/navigation/TangentPreintegration.h b/gtsam/navigation/TangentPreintegration.h index 11945e53a..1b51b4e1e 100644 --- a/gtsam/navigation/TangentPreintegration.h +++ b/gtsam/navigation/TangentPreintegration.h @@ -132,16 +132,14 @@ 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: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; } /// namespace gtsam 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/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index 04d82fe9a..c42b2bdfc 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -209,7 +209,7 @@ private: // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html enum { NeedsToAlign = (sizeof(T) % 16) == 0 }; public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) + GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) }; // ExpressionFactor 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/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index d4eb655c3..1bba57051 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -175,7 +175,7 @@ public: /// @} - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW private: @@ -265,7 +265,7 @@ public: traits::Print(value_, "Value"); } - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW private: @@ -331,7 +331,7 @@ public: return traits::Local(x1,x2); } - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW private: 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/PriorFactor.h b/gtsam/nonlinear/PriorFactor.h index 8d8c67d5c..0afbaa588 100644 --- a/gtsam/nonlinear/PriorFactor.h +++ b/gtsam/nonlinear/PriorFactor.h @@ -114,7 +114,7 @@ namespace gtsam { // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html enum { NeedsToAlign = (sizeof(T) % 16) == 0 }; public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) + GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) }; } /// namespace gtsam 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/internal/ExpressionNode.h b/gtsam/nonlinear/internal/ExpressionNode.h index 0011efb74..0ae37f130 100644 --- a/gtsam/nonlinear/internal/ExpressionNode.h +++ b/gtsam/nonlinear/internal/ExpressionNode.h @@ -150,7 +150,7 @@ public: return constant_; } - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; //----------------------------------------------------------------------------- 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 new file mode 100644 index 000000000..d63633d7e --- /dev/null +++ b/gtsam/sfm/TranslationFactor.h @@ -0,0 +1,84 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +#pragma once + +/** + * @file TranslationFactor.h + * @author Frank Dellaert + * @date March 2020 + * @brief Binary factor for a relative translation direction measurement. + */ + +#include +#include +#include +#include + +namespace gtsam { + +/** + * Binary factor for a relative translation direction measurement + * w_aZb. The measurement equation is + * w_aZb = Unit3(Tb - Ta) + * i.e., w_aZb is the translation direction from frame A to B, in world + * coordinates. Although Unit3 instances live on a manifold, following + * Wilson14eccv_1DSfM.pdf error we compute the *chordal distance* in the + * ambient world coordinate frame: + * normalized(Tb - Ta) - w_aZb.point3() + * + * @addtogroup SFM + */ +class TranslationFactor : public NoiseModelFactor2 { + private: + typedef NoiseModelFactor2 Base; + Point3 measured_w_aZb_; + + public: + /// default constructor + TranslationFactor() {} + + TranslationFactor(Key a, Key b, const Unit3& w_aZb, + const SharedNoiseModel& noiseModel) + : Base(noiseModel, a, b), measured_w_aZb_(w_aZb.point3()) {} + + /** + * @brief Caclulate error: (norm(Tb - Ta) - measurement) + * where Tb and Ta are Point3 translations and measurement is + * the Unit3 translation direction from a to b. + * + * @param Ta translation for key a + * @param Tb translation for key b + * @param H1 optional jacobian in Ta + * @param H2 optional jacobian in Tb + * @return * Vector + */ + Vector evaluateError( + const Point3& Ta, const Point3& Tb, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const override { + const Point3 dir = Tb - Ta; + Matrix33 H_predicted_dir; + 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_; + } + + private: + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "Base", boost::serialization::base_object(*this)); + } +}; // \ TranslationFactor +} // namespace gtsam diff --git a/gtsam/sfm/TranslationRecovery.cpp b/gtsam/sfm/TranslationRecovery.cpp new file mode 100644 index 000000000..aeeae688f --- /dev/null +++ b/gtsam/sfm/TranslationRecovery.cpp @@ -0,0 +1,103 @@ +/* ---------------------------------------------------------------------------- + + * 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 TranslationRecovery.h + * @author Frank Dellaert + * @date March 2020 + * @brief test recovering translations when rotations are given. + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace gtsam; +using namespace std; + +NonlinearFactorGraph TranslationRecovery::buildGraph() const { + auto noiseModel = noiseModel::Isotropic::Sigma(3, 0.01); + NonlinearFactorGraph graph; + + // Add all relative translation edges + for (auto edge : relativeTranslations_) { + Key a, b; + tie(a, b) = edge.first; + const Unit3 w_aZb = edge.second; + graph.emplace_shared(a, b, w_aZb, noiseModel); + } + + return graph; +} + +void TranslationRecovery::addPrior(const double scale, + NonlinearFactorGraph* graph) const { + auto noiseModel = noiseModel::Isotropic::Sigma(3, 0.01); + auto edge = relativeTranslations_.begin(); + Key a, b; + tie(a, b) = edge->first; + const Unit3 w_aZb = edge->second; + graph->emplace_shared >(a, Point3(0, 0, 0), noiseModel); + graph->emplace_shared >(b, scale * w_aZb.point3(), + noiseModel); +} + +Values TranslationRecovery::initalizeRandomly() const { + // Create a lambda expression that checks whether value exists and randomly + // initializes if not. + Values initial; + auto insert = [&initial](Key j) { + if (!initial.exists(j)) { + initial.insert(j, Vector3::Random()); + } + }; + + // Loop over measurements and add a random translation + for (auto edge : relativeTranslations_) { + Key a, b; + tie(a, b) = edge.first; + insert(a); + insert(b); + } + return initial; +} + +Values TranslationRecovery::run(const double scale) const { + auto graph = buildGraph(); + addPrior(scale, &graph); + const Values initial = initalizeRandomly(); + LevenbergMarquardtOptimizer lm(graph, initial, params_); + Values result = lm.optimize(); + return result; +} + +TranslationRecovery::TranslationEdges TranslationRecovery::SimulateMeasurements( + const Values& poses, const vector& edges) { + TranslationEdges relativeTranslations; + for (auto edge : edges) { + Key a, b; + tie(a, b) = edge; + const Pose3 wTa = poses.at(a), wTb = poses.at(b); + const Point3 Ta = wTa.translation(), Tb = wTb.translation(); + const Unit3 w_aZb(Tb - Ta); + relativeTranslations[edge] = w_aZb; + } + return relativeTranslations; +} diff --git a/gtsam/sfm/TranslationRecovery.h b/gtsam/sfm/TranslationRecovery.h new file mode 100644 index 000000000..bb3c3cdb1 --- /dev/null +++ b/gtsam/sfm/TranslationRecovery.h @@ -0,0 +1,114 @@ +/* ---------------------------------------------------------------------------- + + * 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 TranslationRecovery.h + * @author Frank Dellaert + * @date March 2020 + * @brief test recovering translations when rotations are given. + */ + +#include +#include +#include + +#include +#include + +namespace gtsam { + +// Set up an optimization problem for the unknown translations Ti in the world +// coordinate frame, given the known camera attitudes wRi with respect to the +// world frame, and a set of (noisy) translation directions of type Unit3, +// w_aZb. The measurement equation is +// w_aZb = Unit3(Tb - Ta) (1) +// i.e., w_aZb is the translation direction from frame A to B, in world +// coordinates. Although Unit3 instances live on a manifold, following +// Wilson14eccv_1DSfM.pdf error we compute the *chordal distance* in the +// ambient world coordinate frame. +// +// It is clear that we cannot recover the scale, nor the absolute position, +// so the gauge freedom in this case is 3 + 1 = 4. We fix these by taking fixing +// the translations Ta and Tb associated with the first measurement w_aZb, +// clamping them to their initial values as given to this method. If no initial +// values are given, we use the origin for Tb and set Tb to make (1) come +// through, i.e., +// Tb = s * wRa * Point3(w_aZb) (2) +// where s is an arbitrary scale that can be supplied, default 1.0. Hence, two +// versions are supplied below corresponding to whether we have initial values +// or not. +class TranslationRecovery { + public: + using KeyPair = std::pair; + using TranslationEdges = std::map; + + private: + TranslationEdges relativeTranslations_; + LevenbergMarquardtParams params_; + + public: + /** + * @brief Construct a new Translation Recovery object + * + * @param relativeTranslations the relative translations, in world coordinate + * frames, indexed in a map by a pair of Pose keys. + * @param lmParams (optional) gtsam::LavenbergMarquardtParams that can be + * used to modify the parameters for the LM optimizer. By default, uses the + * default LM parameters. + */ + TranslationRecovery(const TranslationEdges& relativeTranslations, + const LevenbergMarquardtParams& lmParams = LevenbergMarquardtParams()) + : relativeTranslations_(relativeTranslations), params_(lmParams) { + params_.setVerbosityLM("Summary"); + } + + /** + * @brief Build the factor graph to do the optimization. + * + * @return NonlinearFactorGraph + */ + NonlinearFactorGraph buildGraph() const; + + /** + * @brief Add priors on ednpoints of first measurement edge. + * + * @param scale scale for first relative translation which fixes gauge. + * @param graph factor graph to which prior is added. + */ + void addPrior(const double scale, NonlinearFactorGraph* graph) const; + + /** + * @brief Create random initial translations. + * + * @return Values + */ + Values initalizeRandomly() const; + + /** + * @brief Build and optimize factor graph. + * + * @param scale scale for first relative translation which fixes gauge. + * @return Values + */ + Values run(const double scale = 1.0) const; + + /** + * @brief Simulate translation direction measurements + * + * @param poses SE(3) ground truth poses stored as Values + * @param edges pairs (a,b) for which a measurement w_aZb will be generated. + * @return TranslationEdges map from a KeyPair to the simulated Unit3 + * translation direction measurement between the cameras in KeyPair. + */ + static TranslationEdges SimulateMeasurements( + const Values& poses, const std::vector& edges); +}; +} // namespace gtsam diff --git a/gtsam/sfm/tests/testTranslationFactor.cpp b/gtsam/sfm/tests/testTranslationFactor.cpp new file mode 100644 index 000000000..37e8b6c0f --- /dev/null +++ b/gtsam/sfm/tests/testTranslationFactor.cpp @@ -0,0 +1,108 @@ +/* ---------------------------------------------------------------------------- + + * 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 testTranslationFactor.cpp + * @brief Unit tests for TranslationFactor Class + * @author Frank dellaert + * @date March 2020 + */ + +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +// Create a noise model for the chordal error +static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.05)); + +// Keys are deliberately *not* in sorted order to test that case. +static const Key kKey1(2), kKey2(1); +static const Unit3 kMeasured(1, 0, 0); + +/* ************************************************************************* */ +TEST(TranslationFactor, Constructor) { + TranslationFactor factor(kKey1, kKey2, kMeasured, model); +} + +/* ************************************************************************* */ +TEST(TranslationFactor, ZeroError) { + // Create a factor + TranslationFactor factor(kKey1, kKey2, kMeasured, model); + + // Set the linearization + Point3 T1(1, 0, 0), T2(2, 0, 0); + + // Use the factor to calculate the error + Vector actualError(factor.evaluateError(T1, T2)); + + // Verify we get the expected error + Vector expected = (Vector3() << 0, 0, 0).finished(); + EXPECT(assert_equal(expected, actualError, 1e-9)); + + +} + +/* ************************************************************************* */ +TEST(TranslationFactor, NonZeroError) { + // create a factor + TranslationFactor factor(kKey1, kKey2, kMeasured, model); + + // set the linearization + Point3 T1(0, 1, 1), T2(0, 2, 2); + + // use the factor to calculate the error + Vector actualError(factor.evaluateError(T1, T2)); + + // verify we get the expected error + Vector expected = (Vector3() << -1, 1/sqrt(2), 1/sqrt(2)).finished(); + EXPECT(assert_equal(expected, actualError, 1e-9)); +} + +/* ************************************************************************* */ +Vector factorError(const Point3& T1, const Point3& T2, + const TranslationFactor& factor) { + return factor.evaluateError(T1, T2); +} + +TEST(TranslationFactor, Jacobian) { + // Create a factor + TranslationFactor factor(kKey1, kKey2, kMeasured, model); + + // Set the linearization + Point3 T1(1, 0, 0), T2(2, 0, 0); + + // Use the factor to calculate the Jacobians + Matrix H1Actual, H2Actual; + factor.evaluateError(T1, T2, H1Actual, H2Actual); + + // Use numerical derivatives to calculate the Jacobians + Matrix H1Expected, H2Expected; + H1Expected = numericalDerivative11( + boost::bind(&factorError, _1, T2, factor), T1); + H2Expected = numericalDerivative11( + boost::bind(&factorError, T1, _1, factor), T2); + + // Verify the Jacobians are correct + EXPECT(assert_equal(H1Expected, H1Actual, 1e-9)); + EXPECT(assert_equal(H2Expected, H2Actual, 1e-9)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index 23138b9e6..b1d4904aa 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -126,7 +126,7 @@ namespace gtsam { // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html enum { NeedsToAlign = (sizeof(VALUE) % 16) == 0 }; public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) + GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) }; // \class BetweenFactor /// traits diff --git a/gtsam/slam/EssentialMatrixConstraint.h b/gtsam/slam/EssentialMatrixConstraint.h index 179200fe1..e474ce5b3 100644 --- a/gtsam/slam/EssentialMatrixConstraint.h +++ b/gtsam/slam/EssentialMatrixConstraint.h @@ -105,7 +105,7 @@ private: } public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; // \class EssentialMatrixConstraint diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index 8bd155a14..c214a2f48 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -81,7 +81,7 @@ public: } public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; /** @@ -201,7 +201,7 @@ public: } public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; // EssentialMatrixFactor2 @@ -286,7 +286,7 @@ public: } public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; // EssentialMatrixFactor3 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/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index 856913aae..0bed15fdc 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -189,7 +189,7 @@ namespace gtsam { } public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; /// traits diff --git a/gtsam/slam/RotateFactor.h b/gtsam/slam/RotateFactor.h index b6ccc36a2..948fffe13 100644 --- a/gtsam/slam/RotateFactor.h +++ b/gtsam/slam/RotateFactor.h @@ -113,7 +113,7 @@ public: return error; } - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; } // namespace gtsam diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 34f9b9e9f..1c80b8744 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -81,7 +81,7 @@ protected: mutable FBlocks Fs; public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + GTSAM_MAKE_ALIGNED_OPERATOR_NEW /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; @@ -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 052bb3343..669935994 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -37,6 +37,7 @@ #include #include #include +#include #include #include @@ -441,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) @@ -485,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 @@ -510,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()); @@ -534,17 +539,24 @@ 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); + boost::optional sampler; + if (corruptingNoise) { + sampler = Sampler(corruptingNoise); + } + std::vector::shared_ptr> factors; while (!is.eof()) { char buf[LINESIZE]; @@ -585,8 +597,13 @@ 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 = NormalizedRot3(qw, qx, qy, qz); + if (sampler) { + R12 = R12.retract(sampler->sample()); + } + factors.emplace_back(new BetweenFactor( - id1, id2, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z}), model)); + id1, id2, Pose3(R12, {x, y, z}), model)); } } return factors; diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 3ab199bab..032799429 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -159,7 +159,8 @@ GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph, /// Parse edges in 3D TORO graph file into a set of BetweenFactors. using BetweenFactorPose3s = std::vector::shared_ptr>; -GTSAM_EXPORT BetweenFactorPose3s parse3DFactors(const std::string& filename); +GTSAM_EXPORT BetweenFactorPose3s parse3DFactors(const std::string& filename, + const noiseModel::Diagonal::shared_ptr& corruptingNoise=nullptr); /// Parse vertices in 3D TORO graph file into a map of Pose3s. GTSAM_EXPORT std::map parse3DPoses(const std::string& filename); 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/gtsam_extra.cmake.in b/gtsam_extra.cmake.in index 8a9a13648..01ac00b37 100644 --- a/gtsam_extra.cmake.in +++ b/gtsam_extra.cmake.in @@ -9,5 +9,6 @@ set (GTSAM_USE_TBB @GTSAM_USE_TBB@) set (GTSAM_DEFAULT_ALLOCATOR @GTSAM_DEFAULT_ALLOCATOR@) if("@GTSAM_INSTALL_CYTHON_TOOLBOX@") + list(APPEND GTSAM_CYTHON_INSTALL_PATH "@GTSAM_CYTHON_INSTALL_PATH@") list(APPEND GTSAM_EIGENCY_INSTALL_PATH "@GTSAM_EIGENCY_INSTALL_PATH@") endif() diff --git a/gtsam_unstable/slam/PoseToPointFactor.h b/gtsam_unstable/slam/PoseToPointFactor.h new file mode 100644 index 000000000..ec7da22ef --- /dev/null +++ b/gtsam_unstable/slam/PoseToPointFactor.h @@ -0,0 +1,90 @@ +/** + * @file PoseToPointFactor.hpp + * @brief This factor can be used to track a 3D landmark over time by + *providing local measurements of its location. + * @author David Wisth + **/ +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + +/** + * A class for a measurement between a pose and a point. + * @addtogroup SLAM + */ +class PoseToPointFactor : public NoiseModelFactor2 { + private: + typedef PoseToPointFactor This; + typedef NoiseModelFactor2 Base; + + Point3 measured_; /** the point measurement in local coordinates */ + + public: + // shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /** default constructor - only use for serialization */ + PoseToPointFactor() {} + + /** Constructor */ + PoseToPointFactor(Key key1, Key key2, const Point3& measured, + const SharedNoiseModel& model) + : Base(model, key1, key2), measured_(measured) {} + + virtual ~PoseToPointFactor() {} + + /** implement functions needed for Testable */ + + /** print */ + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const { + std::cout << s << "PoseToPointFactor(" << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << ")\n" + << " measured: " << measured_.transpose() << std::endl; + this->noiseModel_->print(" noise model: "); + } + + /** equals */ + virtual bool equals(const NonlinearFactor& expected, + double tol = 1e-9) const { + const This* e = dynamic_cast(&expected); + return e != nullptr && Base::equals(*e, tol) && + traits::Equals(this->measured_, e->measured_, tol); + } + + /** implement functions needed to derive from Factor */ + + /** vector of errors + * @brief Error = wTwi.inverse()*wPwp - measured_ + * @param wTwi The pose of the sensor in world coordinates + * @param wPwp The estimated point location in world coordinates + * + * Note: measured_ and the error are in local coordiantes. + */ + Vector evaluateError(const Pose3& wTwi, const Point3& wPwp, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const { + return wTwi.transformTo(wPwp, H1, H2) - measured_; + } + + /** return the measured */ + const Point3& measured() const { return measured_; } + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "NoiseModelFactor2", boost::serialization::base_object(*this)); + ar& BOOST_SERIALIZATION_NVP(measured_); + } + +}; // \class PoseToPointFactor + +} // namespace gtsam diff --git a/gtsam_unstable/slam/tests/testPoseToPointFactor.h b/gtsam_unstable/slam/tests/testPoseToPointFactor.h new file mode 100644 index 000000000..8f8563e9d --- /dev/null +++ b/gtsam_unstable/slam/tests/testPoseToPointFactor.h @@ -0,0 +1,86 @@ +/** + * @file testPoseToPointFactor.cpp + * @brief + * @author David Wisth + * @date June 20, 2020 + */ + +#include +#include +#include + +using namespace gtsam; +using namespace gtsam::noiseModel; + +/// Verify zero error when there is no noise +TEST(PoseToPointFactor, errorNoiseless) { + Pose3 pose = Pose3::identity(); + Point3 point(1.0, 2.0, 3.0); + Point3 noise(0.0, 0.0, 0.0); + Point3 measured = t + noise; + + Key pose_key(1); + Key point_key(2); + PoseToPointFactor factor(pose_key, point_key, measured, + Isotropic::Sigma(3, 0.05)); + Vector expectedError = Vector3(0.0, 0.0, 0.0); + Vector actualError = factor.evaluateError(pose, point); + EXPECT(assert_equal(expectedError, actualError, 1E-5)); +} + +/// Verify expected error in test scenario +TEST(PoseToPointFactor, errorNoise) { + Pose3 pose = Pose3::identity(); + Point3 point(1.0, 2.0, 3.0); + Point3 noise(-1.0, 0.5, 0.3); + Point3 measured = t + noise; + + Key pose_key(1); + Key point_key(2); + PoseToPointFactor factor(pose_key, point_key, measured, + Isotropic::Sigma(3, 0.05)); + Vector expectedError = noise; + Vector actualError = factor.evaluateError(pose, point); + EXPECT(assert_equal(expectedError, actualError, 1E-5)); +} + +/// Check Jacobians are correct +TEST(PoseToPointFactor, jacobian) { + // Measurement + gtsam::Point3 l_meas = gtsam::Point3(1, 2, 3); + + // Linearisation point + gtsam::Point3 p_t = gtsam::Point3(-5, 12, 2); + gtsam::Rot3 p_R = gtsam::Rot3::RzRyRx(1.5 * M_PI, -0.3 * M_PI, 0.4 * M_PI); + Pose3 p(p_R, p_t); + + gtsam::Point3 l = gtsam::Point3(3, 0, 5); + + // Factor + Key pose_key(1); + Key point_key(2); + SharedGaussian noise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)); + PoseToPointFactor factor(pose_key, point_key, l_meas, noise); + + // Calculate numerical derivatives + auto f = boost::bind(&PoseToPointFactor::evaluateError, factor, _1, _2, + boost::none, boost::none); + Matrix numerical_H1 = numericalDerivative21(f, p, l); + Matrix numerical_H2 = numericalDerivative22(f, p, l); + + // Use the factor to calculate the derivative + Matrix actual_H1; + Matrix actual_H2; + factor.evaluateError(p, l, actual_H1, actual_H2); + + // Verify we get the expected error + EXPECT_TRUE(assert_equal(numerical_H1, actual_H1, 1e-8)); + EXPECT_TRUE(assert_equal(numerical_H2, actual_H2, 1e-8)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/package_scripts/README.md b/package_scripts/README.md deleted file mode 100644 index e27747717..000000000 --- a/package_scripts/README.md +++ /dev/null @@ -1,44 +0,0 @@ -# How to build Debian and Ubuntu Packages - -## Preparations - -Packages must be signed with a GPG key. First have a look of the keys -you have available: - - gpg --list-secret-keys - -If you don't have one, create one, then list again. - -Pick a secret key you like from the listed keys, for instance -"Your Name ". Then unlock that key by -signing a dummy file. The following line should pop up a window to -enter the passphrase: - - echo | gpg --local-user "Your Name " -s >/dev/null - -Now you can run the below scripts. Without this step they will fail -with "No secret key" or similar messages. - -## How to generate a Debian package - -Run the package script, providing a name/email that matches your PGP key. - - cd [GTSAM_SOURCE_ROOT] - bash package_scripts/prepare_debian.sh -e "Your Name " - - -## How to generate Ubuntu packages for a PPA - -Run the packaging script, passing the name of the gpg key -(see above) with the "-e" option: - - cd [GTSAM_SOURCE_ROOT] - bash package_scripts/prepare_ubuntu_pkgs_for_ppa.sh -e "Your Name " - -Check that you have uploaded this key to the ubuntu key server, and -have added the key to your account. - -Upload the package to your ppa: - - cd ~/gtsam_ubuntu - bash [GTSAM_SOURCE_ROOT]/package_scripts/upload_all_gtsam_ppa.sh -p "ppa:your-name/ppa-name" diff --git a/package_scripts/compile_static_boost.sh b/package_scripts/compile_static_boost.sh deleted file mode 100755 index ca3b99e09..000000000 --- a/package_scripts/compile_static_boost.sh +++ /dev/null @@ -1,8 +0,0 @@ -#!/bin/sh - -# Compile boost statically, with -fPIC to allow linking it into the mex -# module (which is a dynamic library). --disable-icu prevents depending -# on libicu, which is unneeded and would require then linking the mex -# module with it as well. We just stage instead of install, then the -# toolbox_package_unix.sh script uses the staged boost. -./b2 link=static threading=multi cxxflags=-fPIC cflags=-fPIC --disable-icu -a -j4 stage diff --git a/package_scripts/prepare_debian.sh b/package_scripts/prepare_debian.sh deleted file mode 100755 index 5dd191fc6..000000000 --- a/package_scripts/prepare_debian.sh +++ /dev/null @@ -1,187 +0,0 @@ -#!/bin/bash -# Prepare to build a Debian package. -# Jose Luis Blanco Claraco, 2019 (for GTSAM) -# Jose Luis Blanco Claraco, 2008-2018 (for MRPT) - -set -e # end on error -#set -x # for debugging - -APPEND_SNAPSHOT_NUM=0 -IS_FOR_UBUNTU=0 -APPEND_LINUX_DISTRO="" -VALUE_EXTRA_CMAKE_PARAMS="" -while getopts "sud:c:e:" OPTION -do - case $OPTION in - s) - APPEND_SNAPSHOT_NUM=1 - ;; - u) - IS_FOR_UBUNTU=1 - ;; - d) - APPEND_LINUX_DISTRO=$OPTARG - ;; - c) - VALUE_EXTRA_CMAKE_PARAMS=$OPTARG - ;; - e) - PACKAGER_EMAIL=$OPTARG - ;; - ?) - echo "Unknown command line argument!" - exit 1 - ;; - esac -done - -if [ -z ${PACKAGER_EMAIL+x} ]; then - echo "must specify packager email via -e option!" - exit -1 -fi - -if [ -f CMakeLists.txt ]; -then - source package_scripts/prepare_debian_gen_snapshot_version.sh -else - echo "Error: cannot find CMakeList.txt. This script is intended to be run from the root of the source tree." - exit 1 -fi - -# Append snapshot? -if [ $APPEND_SNAPSHOT_NUM == "1" ]; -then - CUR_SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" - source $CUR_SCRIPT_DIR/prepare_debian_gen_snapshot_version.sh # populate GTSAM_SNAPSHOT_VERSION - - GTSAM_VERSION_STR="${GTSAM_VERSION_STR}~snapshot${GTSAM_SNAPSHOT_VERSION}${APPEND_LINUX_DISTRO}" -else - GTSAM_VERSION_STR="${GTSAM_VERSION_STR}${APPEND_LINUX_DISTRO}" -fi - -# Call prepare_release -GTSAMSRC=`pwd` - -if [ -f $HOME/gtsam_release/gtsam*.tar.gz ]; -then - echo "## release file already exists. Reusing it." -else - source package_scripts/prepare_release.sh - echo - echo "## Done prepare_release.sh" -fi - -echo "=========== Generating GTSAM ${GTSAM_VER_MMP} Debian package ==============" -cd $GTSAMSRC - -set -x -if [ -z "$GTSAM_DEB_DIR" ]; then - GTSAM_DEB_DIR="$HOME/gtsam_debian" -fi -GTSAM_EXTERN_DEBIAN_DIR="$GTSAMSRC/debian/" -GTSAM_EXTERN_UBUNTU_PPA_DIR="$GTSAMSRC/debian/" - -if [ -f ${GTSAM_EXTERN_DEBIAN_DIR}/control ]; -then - echo "Using debian dir: ${GTSAM_EXTERN_DEBIAN_DIR}" -else - echo "ERROR: Cannot find ${GTSAM_EXTERN_DEBIAN_DIR}" - exit 1 -fi - -GTSAM_DEBSRC_DIR=$GTSAM_DEB_DIR/gtsam-${GTSAM_VERSION_STR} - -echo "GTSAM_VERSION_STR: ${GTSAM_VERSION_STR}" -echo "GTSAM_DEBSRC_DIR: ${GTSAM_DEBSRC_DIR}" - -# Prepare a directory for building the debian package: -# -rm -fR $GTSAM_DEB_DIR || true -mkdir -p $GTSAM_DEB_DIR || true - -# Orig tarball: -echo "Copying orig tarball: gtsam_${GTSAM_VERSION_STR}.orig.tar.gz" -cp $HOME/gtsam_release/gtsam*.tar.gz $GTSAM_DEB_DIR/gtsam_${GTSAM_VERSION_STR}.orig.tar.gz -cd ${GTSAM_DEB_DIR} -tar -xf gtsam_${GTSAM_VERSION_STR}.orig.tar.gz - -if [ ! -d "${GTSAM_DEBSRC_DIR}" ]; -then - mv gtsam-* ${GTSAM_DEBSRC_DIR} # fix different dir names for Ubuntu PPA packages -fi - -if [ ! -f "${GTSAM_DEBSRC_DIR}/CMakeLists.txt" ]; -then - echo "*ERROR*: Seems there was a problem copying sources to ${GTSAM_DEBSRC_DIR}... aborting script." - exit 1 -fi - -cd ${GTSAM_DEBSRC_DIR} - -# Copy debian directory: -#mkdir debian -cp -r ${GTSAM_EXTERN_DEBIAN_DIR}/* debian - -# Use modified control & rules files for Ubuntu PPA packages: -#if [ $IS_FOR_UBUNTU == "1" ]; -#then - # already done: cp ${GTSAM_EXTERN_UBUNTU_PPA_DIR}/control.in debian/ - # Ubuntu: force use of gcc-7: - #sed -i '9i\export CXX=/usr/bin/g++-7\' debian/rules - #sed -i '9i\export CC=/usr/bin/gcc-7\' debian/rules7 -#fi - -# Export signing pub key: -mkdir debian/upstream/ -gpg --export --export-options export-minimal --armor > debian/upstream/signing-key.asc - -# Parse debian/ control.in --> control -#mv debian/control.in debian/control -#sed -i "s/@GTSAM_VER_MM@/${GTSAM_VER_MM}/g" debian/control - -# Replace the text "REPLACE_HERE_EXTRA_CMAKE_PARAMS" in the "debian/rules" file -# with: ${${VALUE_EXTRA_CMAKE_PARAMS}} -RULES_FILE=debian/rules -sed -i -e "s/REPLACE_HERE_EXTRA_CMAKE_PARAMS/${VALUE_EXTRA_CMAKE_PARAMS}/g" $RULES_FILE -echo "Using these extra parameters for CMake: '${VALUE_EXTRA_CMAKE_PARAMS}'" - -# Strip my custom files... -rm debian/*.new || true - - -# Figure out the next Debian version number: -echo "Detecting next Debian version number..." - -CHANGELOG_UPSTREAM_VER=$( dpkg-parsechangelog | sed -n 's/Version:.*\([0-9]\.[0-9]*\.[0-9]*.*snapshot.*\)-.*/\1/p' ) -CHANGELOG_LAST_DEBIAN_VER=$( dpkg-parsechangelog | sed -n 's/Version:.*\([0-9]\.[0-9]*\.[0-9]*\).*-\([0-9]*\).*/\2/p' ) - -echo " -> PREVIOUS UPSTREAM: $CHANGELOG_UPSTREAM_VER -> New: ${GTSAM_VERSION_STR}" -echo " -> PREVIOUS DEBIAN VERSION: $CHANGELOG_LAST_DEBIAN_VER" - -# If we have the same upstream versions, increase the Debian version, otherwise create a new entry: -if [ "$CHANGELOG_UPSTREAM_VER" = "$GTSAM_VERSION_STR" ]; -then - NEW_DEBIAN_VER=$[$CHANGELOG_LAST_DEBIAN_VER + 1] - echo "Changing to a new Debian version: ${GTSAM_VERSION_STR}-${NEW_DEBIAN_VER}" - DEBCHANGE_CMD="--newversion ${GTSAM_VERSION_STR}-${NEW_DEBIAN_VER}" -else - DEBCHANGE_CMD="--newversion ${GTSAM_VERSION_STR}-1" -fi - -echo "Adding a new entry to debian/changelog..." - -DEBEMAIL=${PACKAGER_EMAIL} debchange $DEBCHANGE_CMD -b --distribution unstable --force-distribution New version of upstream sources. - -echo "Copying back the new changelog to a temporary file in: ${GTSAM_EXTERN_DEBIAN_DIR}changelog.new" -cp debian/changelog ${GTSAM_EXTERN_DEBIAN_DIR}changelog.new - -set +x - -echo "==============================================================" -echo "Now, you can build the source Deb package with 'debuild -S -sa'" -echo "==============================================================" - -cd .. -ls -lh - -exit 0 diff --git a/package_scripts/prepare_debian_gen_snapshot_version.sh b/package_scripts/prepare_debian_gen_snapshot_version.sh deleted file mode 100755 index 589d422fe..000000000 --- a/package_scripts/prepare_debian_gen_snapshot_version.sh +++ /dev/null @@ -1,25 +0,0 @@ -#!/bin/bash - -# See https://reproducible-builds.org/specs/source-date-epoch/ -# get SOURCE_DATE_EPOCH with UNIX time_t -if [ -d ".git" ]; -then - SOURCE_DATE_EPOCH=$(git log -1 --pretty=%ct) -else - echo "Error: intended for use from within a git repository" - exit 1 -fi -GTSAM_SNAPSHOT_VERSION=$(date -d @$SOURCE_DATE_EPOCH +%Y%m%d-%H%M) - -GTSAM_SNAPSHOT_VERSION+="-git-" -GTSAM_SNAPSHOT_VERSION+=`git rev-parse --short=8 HEAD` -GTSAM_SNAPSHOT_VERSION+="-" - -# x.y.z version components: -GTSAM_VERSION_MAJOR=$(grep "(GTSAM_VERSION_MAJOR" CMakeLists.txt | sed -r 's/^.*GTSAM_VERSION_MAJOR\s*([0-9])*.*$/\1/g') -GTSAM_VERSION_MINOR=$(grep "(GTSAM_VERSION_MINOR" CMakeLists.txt | sed -r 's/^.*GTSAM_VERSION_MINOR\s*([0-9])*.*$/\1/g') -GTSAM_VERSION_PATCH=$(grep "(GTSAM_VERSION_PATCH" CMakeLists.txt | sed -r 's/^.*GTSAM_VERSION_PATCH\s*([0-9])*.*$/\1/g') - -GTSAM_VER_MM="${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}" -GTSAM_VER_MMP="${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}" -GTSAM_VERSION_STR=$GTSAM_VER_MMP diff --git a/package_scripts/prepare_release.sh b/package_scripts/prepare_release.sh deleted file mode 100755 index 750fc27b3..000000000 --- a/package_scripts/prepare_release.sh +++ /dev/null @@ -1,71 +0,0 @@ -#!/bin/bash -# Export sources from a git tree and prepare it for a public release. -# Jose Luis Blanco Claraco, 2019 (for GTSAM) -# Jose Luis Blanco Claraco, 2008-2018 (for MRPT) - -set -e # exit on error -#set -x # for debugging - -# Checks -# -------------------------------- -if [ -f version_prefix.txt ]; -then - if [ -z ${GTSAM_VERSION_STR+x} ]; - then - source package_scripts/prepare_debian_gen_snapshot_version.sh - fi - echo "ERROR: Run this script from the GTSAM source tree root directory." - exit 1 -fi - -GTSAM_SRC=`pwd` -OUT_RELEASES_DIR="$HOME/gtsam_release" - -OUT_DIR=$OUT_RELEASES_DIR/gtsam-${GTSAM_VERSION_STR} - -echo "=========== Generating GTSAM release ${GTSAM_VER_MMP} ==================" -echo "GTSAM_VERSION_STR : ${GTSAM_VERSION_STR}" -echo "OUT_DIR : ${OUT_DIR}" -echo "============================================================" -echo - -# Prepare output directory: -rm -fR $OUT_RELEASES_DIR || true -mkdir -p ${OUT_DIR} - -# Export / copy sources to target dir: -if [ -d "$GTSAM_SRC/.git" ]; -then - echo "# Exporting git source tree to ${OUT_DIR}" - git archive --format=tar HEAD | tar -x -C ${OUT_DIR} - - # Remove VCS control files: - find ${OUT_DIR} -name '.gitignore' | xargs rm - - # Generate ./SOURCE_DATE_EPOCH with UNIX time_t - SOURCE_DATE_EPOCH=$(git log -1 --pretty=%ct) -else - echo "# Copying sources to ${OUT_DIR}" - cp -R . ${OUT_DIR} - - # Generate ./SOURCE_DATE_EPOCH with UNIX time_t - SOURCE_DATE_EPOCH=$(date +%s) -fi - -# See https://reproducible-builds.org/specs/source-date-epoch/ -echo $SOURCE_DATE_EPOCH > ${OUT_DIR}/SOURCE_DATE_EPOCH - -cd ${OUT_DIR} - -# Dont include Debian files in releases: -rm -fR package_scripts - -# Orig tarball: -cd .. -echo "# Creating orig tarball: gtsam-${GTSAM_VERSION_STR}.tar.gz" -tar czf gtsam-${GTSAM_VERSION_STR}.tar.gz gtsam-${GTSAM_VERSION_STR} - -rm -fr gtsam-${GTSAM_VERSION_STR} - -# GPG signature: -gpg --armor --detach-sign gtsam-${GTSAM_VERSION_STR}.tar.gz diff --git a/package_scripts/prepare_ubuntu_pkgs_for_ppa.sh b/package_scripts/prepare_ubuntu_pkgs_for_ppa.sh deleted file mode 100755 index 33c016b94..000000000 --- a/package_scripts/prepare_ubuntu_pkgs_for_ppa.sh +++ /dev/null @@ -1,123 +0,0 @@ -#!/bin/bash -# Creates a set of packages for each different Ubuntu distribution, with the -# intention of uploading them to a PPA on launchpad -# -# JLBC, 2010 -# [Addition 2012:] -# -# You can declare a variable (in the caller shell) with extra flags for the -# CMake in the final ./configure like: -# -# GTSAM_PKG_CUSTOM_CMAKE_PARAMS="\"-DDISABLE_SSE3=ON\"" -# - - -function show_help { - echo "USAGE:" - echo "" - echo "- to display this help: " - echo "prepare_ubuntu_packages_for_ppa.sh -h or -?" - echo "" - echo "- to package to your PPA: " - echo "prepare_ubuntu_packages_for_ppa.sh -e email_of_your_gpg_key" - echo "" - echo "to pass custom config for GTSAM, set the following" - echo "environment variable beforehand: " - echo "" - echo "GTSAM_PKG_CUSTOM_CMAKE_PARAMS=\"\"-DDISABLE_SSE3=ON\"\"" - echo "" -} - -while getopts "h?e:" opt; do - case "$opt" in - h|\?) - show_help - exit 0 - ;; - e) PACKAGER_EMAIL=$OPTARG - ;; - esac -done - -if [ -z ${PACKAGER_EMAIL+x} ]; then - show_help - exit -1 -fi - - -set -e - -# List of distributions to create PPA packages for: -LST_DISTROS=(xenial bionic eoan focal) - -# Checks -# -------------------------------- -if [ -f CMakeLists.txt ]; -then - source package_scripts/prepare_debian_gen_snapshot_version.sh - echo "GTSAM version: ${GTSAM_VER_MMP}" -else - echo "ERROR: Run this script from the GTSAM root directory." - exit 1 -fi - -if [ -z "${gtsam_ubuntu_OUT_DIR}" ]; then - export gtsam_ubuntu_OUT_DIR="$HOME/gtsam_ubuntu" -fi -GTSAMSRC=`pwd` -if [ -z "${GTSAM_DEB_DIR}" ]; then - export GTSAM_DEB_DIR="$HOME/gtsam_debian" -fi -GTSAM_EXTERN_DEBIAN_DIR="$GTSAMSRC/debian/" - -# Clean out dirs: -rm -fr $gtsam_ubuntu_OUT_DIR/ - -# ------------------------------------------------------------------- -# And now create the custom packages for each Ubuntu distribution: -# ------------------------------------------------------------------- -count=${#LST_DISTROS[@]} -IDXS=$(seq 0 $(expr $count - 1)) - -cp ${GTSAM_EXTERN_DEBIAN_DIR}/changelog /tmp/my_changelog - -for IDX in ${IDXS}; -do - DEBIAN_DIST=${LST_DISTROS[$IDX]} - - # ------------------------------------------------------------------- - # Call the standard "prepare_debian.sh" script: - # ------------------------------------------------------------------- - cd ${GTSAMSRC} - bash package_scripts/prepare_debian.sh -e "$PACKAGER_EMAIL" -s -u -d ${DEBIAN_DIST} -c "${GTSAM_PKG_CUSTOM_CMAKE_PARAMS}" - - CUR_SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" - source $CUR_SCRIPT_DIR/prepare_debian_gen_snapshot_version.sh # populate GTSAM_SNAPSHOT_VERSION - - echo "===== Distribution: ${DEBIAN_DIST} =========" - cd ${GTSAM_DEB_DIR}/gtsam-${GTSAM_VER_MMP}~snapshot${GTSAM_SNAPSHOT_VERSION}${DEBIAN_DIST}/debian - #cp ${GTSAM_EXTERN_DEBIAN_DIR}/changelog changelog - cp /tmp/my_changelog changelog - DEBCHANGE_CMD="--newversion ${GTSAM_VERSION_STR}~snapshot${GTSAM_SNAPSHOT_VERSION}${DEBIAN_DIST}-1" - echo "Changing to a new Debian version: ${DEBCHANGE_CMD}" - echo "Adding a new entry to debian/changelog for distribution ${DEBIAN_DIST}" - DEBEMAIL="${PACKAGER_EMAIL}" debchange $DEBCHANGE_CMD -b --distribution ${DEBIAN_DIST} --force-distribution New version of upstream sources. - - cp changelog /tmp/my_changelog - - echo "Now, let's build the source Deb package with 'debuild -S -sa':" - cd .. - # -S: source package - # -sa: force inclusion of sources - # -d: don't check dependencies in this system - debuild -S -sa -d - - # Make a copy of all these packages: - cd .. - mkdir -p $gtsam_ubuntu_OUT_DIR/$DEBIAN_DIST - cp gtsam_* $gtsam_ubuntu_OUT_DIR/${DEBIAN_DIST}/ - echo ">>>>>> Saving packages to: $gtsam_ubuntu_OUT_DIR/$DEBIAN_DIST/" -done - - -exit 0 diff --git a/package_scripts/toolbox_package_unix.sh b/package_scripts/toolbox_package_unix.sh deleted file mode 100755 index 28de2572a..000000000 --- a/package_scripts/toolbox_package_unix.sh +++ /dev/null @@ -1,64 +0,0 @@ -#!/bin/sh - -# Script to build a tarball with the matlab toolbox - -# Detect platform -os=`uname -s` -arch=`uname -m` -if [ "$os" = "Linux" -a "$arch" = "x86_64" ]; then - platform=lin64 -elif [ "$os" = "Linux" -a "$arch" = "i686" ]; then - platform=lin32 -elif [ "$os" = "Darwin" -a "$arch" = "x86_64" ]; then - platform=mac64 -else - echo "Unrecognized platform" - exit 1 -fi - -echo "Platform is ${platform}" - -# Check for empty diectory -if [ ! -z "`ls`" ]; then - echo "Please run this script from an empty build directory" - exit 1 -fi - -# Check for boost -if [ -z "$1" ]; then - echo "Usage: $0 BOOSTTREE" - echo "BOOSTTREE should be a boost source tree compiled with toolbox_build_boost." - exit 1 -fi - -# Run cmake -cmake -DCMAKE_BUILD_TYPE=Release \ --DGTSAM_INSTALL_MATLAB_TOOLBOX:BOOL=ON \ --DCMAKE_INSTALL_PREFIX="$PWD/stage" \ --DBoost_NO_SYSTEM_PATHS:BOOL=ON \ --DBoost_USE_STATIC_LIBS:BOOL=ON \ --DBOOST_ROOT="$1" \ --DGTSAM_BUILD_TESTS:BOOL=OFF \ --DGTSAM_BUILD_TIMING:BOOL=OFF \ --DGTSAM_BUILD_EXAMPLES_ALWAYS:BOOL=OFF \ --DGTSAM_WITH_TBB:BOOL=OFF \ --DGTSAM_SUPPORT_NESTED_DISSECTION:BOOL=OFF \ --DGTSAM_INSTALL_GEOGRAPHICLIB:BOOL=OFF \ --DGTSAM_BUILD_UNSTABLE:BOOL=OFF \ --DGTSAM_MEX_BUILD_STATIC_MODULE:BOOL=ON .. - -if [ $? -ne 0 ]; then - echo "CMake failed" - exit 1 -fi - -# Compile -make -j8 install - -if [ $? -ne 0 ]; then - echo "Compile failed" - exit 1 -fi - -# Create package -tar czf gtsam-toolbox-3.2.0-$platform.tgz -C stage/gtsam_toolbox toolbox diff --git a/package_scripts/upload_all_gtsam_ppa.sh b/package_scripts/upload_all_gtsam_ppa.sh deleted file mode 100755 index f06d005fb..000000000 --- a/package_scripts/upload_all_gtsam_ppa.sh +++ /dev/null @@ -1,31 +0,0 @@ -#!/bin/bash - -function show_help { - echo "USAGE:" - echo "" - echo "- to display this help: " - echo "upload_all_gtsam_ppa.sh -h or -?" - echo "" - echo "- to upload to your PPA: " - echo "upload_all_gtsam_ppa.sh -p ppa:your_name/name_of_ppa" - echo "" -} - -while getopts "h?p:" opt; do - case "$opt" in - h|\?) - show_help - exit 0 - ;; - p) ppa_name=$OPTARG - ;; - esac -done - -if [ -z ${ppa_name+x} ]; then - show_help - exit -1 -fi - - -find . -name '*.changes' | xargs -I FIL dput ${ppa_name} FIL 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/tests/testTranslationRecovery.cpp b/tests/testTranslationRecovery.cpp new file mode 100644 index 000000000..5a98c3bf5 --- /dev/null +++ b/tests/testTranslationRecovery.cpp @@ -0,0 +1,87 @@ +/* ---------------------------------------------------------------------------- + + * 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 testTranslationRecovery.cpp + * @author Frank Dellaert + * @date March 2020 + * @brief test recovering translations when rotations are given. + */ + +#include + +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +// We read the BAL file, which has 3 cameras in it, with poses. We then assume +// the rotations are correct, but translations have to be estimated from +// translation directions only. Since we have 3 cameras, A, B, and C, we can at +// most create three relative measurements, let's call them w_aZb, w_aZc, and +// bZc. These will be of type Unit3. We then call `recoverTranslations` which +// sets up an optimization problem for the three unknown translations. +TEST(TranslationRecovery, BAL) { + const string filename = findExampleDataFile("dubrovnik-3-7-pre"); + SfmData db; + bool success = readBAL(filename, db); + if (!success) throw runtime_error("Could not access file!"); + + // Get camera poses, as Values + size_t j = 0; + Values poses; + for (auto camera : db.cameras) { + poses.insert(j++, camera.pose()); + } + + // Simulate measurements + const auto relativeTranslations = TranslationRecovery::SimulateMeasurements( + poses, {{0, 1}, {0, 2}, {1, 2}}); + + // Check + const Pose3 wTa = poses.at(0), wTb = poses.at(1), + wTc = poses.at(2); + const Point3 Ta = wTa.translation(), Tb = wTb.translation(), + Tc = wTc.translation(); + const Rot3 aRw = wTa.rotation().inverse(); + const Unit3 w_aZb = relativeTranslations.at({0, 1}); + EXPECT(assert_equal(Unit3(Tb - Ta), w_aZb)); + const Unit3 w_aZc = relativeTranslations.at({0, 2}); + EXPECT(assert_equal(Unit3(Tc - Ta), w_aZc)); + + TranslationRecovery algorithm(relativeTranslations); + const auto graph = algorithm.buildGraph(); + EXPECT_LONGS_EQUAL(3, graph.size()); + + // Translation recovery, version 1 + const double scale = 2.0; + const auto result = algorithm.run(scale); + + // Check result for first two translations, determined by prior + EXPECT(assert_equal(Point3(0, 0, 0), result.at(0))); + EXPECT(assert_equal(Point3(2 * w_aZb.point3()), result.at(1))); + + // Check that the third translations is correct + Point3 expected = (Tc - Ta) * (scale / (Tb - Ta).norm()); + EXPECT(assert_equal(expected, result.at(2), 1e-4)); + + // TODO(frank): how to get stats back? + // EXPECT_DOUBLES_EQUAL(0.0199833, actualError, 1e-5); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ 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; +} diff --git a/wrap/Module.cpp b/wrap/Module.cpp index a7db9e1f6..ec02dc412 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -350,7 +350,10 @@ void Module::emit_cython_pxd(FileWriter& pxdFile) const { " T* get()\n" " long use_count() const\n" " T& operator*()\n\n" - " cdef shared_ptr[T] dynamic_pointer_cast[T,U](const shared_ptr[U]& r)\n" + " cdef shared_ptr[T] dynamic_pointer_cast[T,U](const shared_ptr[U]& r)\n\n"; + + // gtsam alignment-friendly shared_ptr + pxdFile.oss << "cdef extern from \"gtsam/base/make_shared.h\" namespace \"gtsam\":\n" " cdef shared_ptr[T] make_shared[T](const T& r)\n\n"; for(const TypedefPair& types: typedefs) diff --git a/wrap/tests/expected-cython/geometry.pxd b/wrap/tests/expected-cython/geometry.pxd index 0d9adac5f..3527840a3 100644 --- a/wrap/tests/expected-cython/geometry.pxd +++ b/wrap/tests/expected-cython/geometry.pxd @@ -16,6 +16,8 @@ cdef extern from "boost/shared_ptr.hpp" namespace "boost": T& operator*() cdef shared_ptr[T] dynamic_pointer_cast[T,U](const shared_ptr[U]& r) + +cdef extern from "gtsam/base/make_shared.h" namespace "gtsam": cdef shared_ptr[T] make_shared[T](const T& r) cdef extern from "gtsam/geometry/Point2.h" namespace "gtsam":